Skip to content

Commit 656659b

Browse files
pettnipettni
committed
Rename block->element
1 parent 01955f9 commit 656659b

File tree

7 files changed

+138
-131
lines changed

7 files changed

+138
-131
lines changed

‎examples/bundle_sam.cpp‎

Lines changed: 32 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,7 @@ static const int NUM_MEAS = NUM_POSES * DoF + NUM_FACTORS * Dim;
6060
static const int MAX_ITER = 20; // for the solver
6161

6262

63+
// bundle state type to optimize over
6364
using BundleT = manif::Bundle<double,
6465
manif::SE2,
6566
manif::SE2,
@@ -76,35 +77,38 @@ using BundleT = manif::Bundle<double,
7677
template<std::size_t XI, std::size_t XJ>
7778
void add_pose_factor(
7879
const BundleT & X,
79-
const manif::SE2d::Tangent & control,
80+
const SE2Tangentd & control,
8081
const MatrixT & W,
8182
Eigen::Ref<Eigen::Matrix<double, 3, 1>> r,
8283
Eigen::Ref<Eigen::Matrix<double, 3, BundleT::DoF>> J)
8384
{
85+
// index start position and length in the DoF of BundleT
8486
constexpr int BegI = manif::internal::intseq_element<XI, BundleT::BegDoF>::value;
8587
constexpr int LenI = manif::internal::intseq_element<XI, BundleT::LenDoF>::value;
8688
constexpr int BegJ = manif::internal::intseq_element<XJ, BundleT::BegDoF>::value;
8789
constexpr int LenJ = manif::internal::intseq_element<XJ, BundleT::LenDoF>::value;
8890

8991
MatrixT J_d_xi, J_d_xj; // Jacobian of motion wrt poses i and j
9092

91-
auto d = X.block<XJ>().rminus(X.block<XI>(), J_d_xj, J_d_xi);
93+
auto d = X.element<XJ>().rminus(X.element<XI>(), J_d_xj, J_d_xi);
9294
r = W * (d - control).coeffs();
9395
J.setZero();
9496
J.block<3, LenI>(0, BegI) = W * J_d_xi;
9597
J.block<3, LenJ>(0, BegJ) = W * J_d_xj;
9698
}
9799

100+
98101
// Insert a landmark measurement factor of landmark LK
99102
// from pose XI into the residual-jacobian pair (r, J)
100103
template<std::size_t XI, std::size_t LK>
101104
void add_beacon_factor(
102105
const BundleT & X,
103-
const Eigen::Vector2d & measurement,
104-
const Eigen::Matrix2d & S,
106+
const VectorY & measurement,
107+
const MatrixY & S,
105108
Eigen::Ref<Eigen::Matrix<double, 2, 1>> r,
106109
Eigen::Ref<Eigen::Matrix<double, 2, BundleT::DoF>> J)
107110
{
111+
// index start position and length in the DoF of BundleT
108112
constexpr int BegX = manif::internal::intseq_element<XI, BundleT::BegDoF>::value;
109113
constexpr int LenX = manif::internal::intseq_element<XI, BundleT::LenDoF>::value;
110114
constexpr int BegLMK = manif::internal::intseq_element<NUM_POSES + LK, BundleT::BegDoF>::value;
@@ -115,7 +119,7 @@ void add_beacon_factor(
115119
MatrixYT J_e_x; // Jacobian of measurement expectation wrt pose
116120
MatrixYB J_e_b; // Jacobian of measurement expectation wrt lmk
117121

118-
auto e = X.block<XI>().inverse(J_ix_x).act(X.block<NUM_POSES + LK>().coeffs(), J_e_ix, J_e_b);
122+
auto e = X.element<XI>().inverse(J_ix_x).act(X.element<NUM_POSES + LK>().coeffs(), J_e_ix, J_e_b);
119123
J_e_x = J_e_ix * J_ix_x;
120124
r = S * (e - measurement);
121125
J.setZero();
@@ -279,15 +283,15 @@ int main()
279283
manif::R2d(landmarks[4]));
280284

281285
cout << "prior" << std::showpos << endl;
282-
cout << "pose :" << X.block<0>().translation().transpose() << " " << X.block<0>().angle() << endl;
283-
cout << "pose :" << X.block<1>().translation().transpose() << " " << X.block<1>().angle() << endl;
284-
cout << "pose :" << X.block<2>().translation().transpose() << " " << X.block<2>().angle() << endl;
285-
286-
cout << "lmk :" << X.block<3>() << endl;
287-
cout << "lmk :" << X.block<4>() << endl;
288-
cout << "lmk :" << X.block<5>() << endl;
289-
cout << "lmk :" << X.block<6>() << endl;
290-
cout << "lmk :" << X.block<7>() << endl;
286+
cout << "pose :" << X.element<0>().translation().transpose() << " " << X.element<0>().angle() << endl;
287+
cout << "pose :" << X.element<1>().translation().transpose() << " " << X.element<1>().angle() << endl;
288+
cout << "pose :" << X.element<2>().translation().transpose() << " " << X.element<2>().angle() << endl;
289+
290+
cout << "lmk :" << X.element<3>() << endl;
291+
cout << "lmk :" << X.element<4>() << endl;
292+
cout << "lmk :" << X.element<5>() << endl;
293+
cout << "lmk :" << X.element<6>() << endl;
294+
cout << "lmk :" << X.element<7>() << endl;
291295
cout << "-----------------------------------------------" << endl;
292296

293297
// iterate
@@ -301,7 +305,7 @@ int main()
301305
int row = 0; // keep track of row in J and r
302306

303307
// first residual: prior
304-
r.segment<DoF>(row) = X.block<0>().lminus(SE2d::Identity(), J.block<DoF, DoF>(row, 0)).coeffs();
308+
r.segment<DoF>(row) = X.element<0>().lminus(SE2d::Identity(), J.block<DoF, DoF>(row, 0)).coeffs();
305309
row += DoF;
306310

307311
// motion residuals
@@ -338,7 +342,10 @@ int main()
338342
// compute optimal step
339343
// ATTENTION: This is an expensive step!!
340344
// ATTENTION: Use QR factorization and column reordering for larger problems!!
341-
X += BundleT::Tangent(-(J.transpose() * J).inverse() * J.transpose() * r);
345+
const auto dX = (-(J.transpose() * J).inverse() * J.transpose() * r).eval();
346+
347+
// update estimate
348+
X += BundleT::Tangent(dX);
342349

343350
// DEBUG INFO
344351
cout << "residual norm: " << std::scientific << r.norm() << ", step norm: " << dX.norm() << endl;
@@ -355,15 +362,15 @@ int main()
355362

356363
// solved problem
357364
cout << "posterior" << std::showpos << endl;
358-
cout << "pose :" << X.block<0>().translation().transpose() << " " << X.block<0>().angle() << endl;
359-
cout << "pose :" << X.block<1>().translation().transpose() << " " << X.block<1>().angle() << endl;
360-
cout << "pose :" << X.block<2>().translation().transpose() << " " << X.block<2>().angle() << endl;
361-
362-
cout << "lmk :" << X.block<3>() << endl;
363-
cout << "lmk :" << X.block<4>() << endl;
364-
cout << "lmk :" << X.block<5>() << endl;
365-
cout << "lmk :" << X.block<6>() << endl;
366-
cout << "lmk :" << X.block<7>() << endl;
365+
cout << "pose :" << X.element<0>().translation().transpose() << " " << X.element<0>().angle() << endl;
366+
cout << "pose :" << X.element<1>().translation().transpose() << " " << X.element<1>().angle() << endl;
367+
cout << "pose :" << X.element<2>().translation().transpose() << " " << X.element<2>().angle() << endl;
368+
369+
cout << "lmk :" << X.element<3>() << endl;
370+
cout << "lmk :" << X.element<4>() << endl;
371+
cout << "lmk :" << X.element<5>() << endl;
372+
cout << "lmk :" << X.element<6>() << endl;
373+
cout << "lmk :" << X.element<7>() << endl;
367374

368375
cout << "-----------------------------------------------" << endl;
369376

‎examples/se3_localization.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -144,11 +144,11 @@ Vector3d group_act(
144144
)
145145
{
146146
if (!J_vout_m) {
147-
return x.block<0>().act(x.block<1>().act(b));
147+
return x.element<0>().act(x.element<1>().act(b));
148148
} else {
149149
Eigen::Matrix3d J_rot_x1, J_out_x0, J_out_rot;
150-
Vector3d rot = x.block<1>().act(b, J_rot_x1);
151-
Vector3d out = x.block<0>().act(rot, J_out_x0, J_out_rot);
150+
Vector3d rot = x.element<1>().act(b, J_rot_x1);
151+
Vector3d out = x.element<0>().act(rot, J_out_x0, J_out_rot);
152152
J_vout_m.value().block<3, 3>(0, 0) = J_out_x0;
153153
J_vout_m.value().block<3, 3>(0, 3) = J_out_rot * J_rot_x1;
154154
return out;

‎include/manif/impl/bundle/Bundle.h‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ struct traits<Bundle<_Scalar, _T ...>>
3333
using BegRep = intseq_psum_t<LenRep>;
3434

3535
template<std::size_t _Idx>
36-
using BlockType = typename bundle_element<_Idx, _T<_Scalar>...>::type;
36+
using ElementType = typename bundle_element<_Idx, _T<_Scalar>...>::type;
3737

3838
// Regular traits
3939
using Scalar = _Scalar;
@@ -67,7 +67,7 @@ struct traits<Bundle<_Scalar, _T ...>>
6767
* (see also Example 7).
6868
*
6969
* A Bundle <G1, ..., Gn> of Lie groups can be utilized as
70-
* a single group with block-wise operations. This can be
70+
* a single group with element-wise operations. This can be
7171
* convenient when working with aggregate states that consist of
7272
* multiple Lie group sub-states, like the example in Section VIIb
7373
* of the reference paper.
@@ -133,15 +133,15 @@ struct Bundle : BundleBase<Bundle<_Scalar, _T ...>>
133133
// Bundle specific API
134134

135135
/**
136-
* @brief Construct from Bundle blocks
136+
* @brief Construct from Bundle elements
137137
*/
138-
Bundle(const _T<_Scalar> & ... blocks);
138+
Bundle(const _T<_Scalar> & ... elements);
139139

140140
protected:
141141

142-
// Helper for the blocks constructor
142+
// Helper for the elements constructor
143143
template<int ... _BegRep, int ... _LenRep>
144-
Bundle(internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, const _T<_Scalar> & ... blocks);
144+
Bundle(internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, const _T<_Scalar> & ... elements);
145145

146146
protected:
147147

@@ -157,17 +157,17 @@ Bundle<_Scalar, _T...>::Bundle(const LieGroupBase<_DerivedOther> & o)
157157
{}
158158

159159
template<typename _Scalar, template<typename> class ... _T>
160-
Bundle<_Scalar, _T...>::Bundle(const _T<_Scalar> & ... blocks)
161-
: Bundle(BegRep{}, LenRep{}, blocks ...)
160+
Bundle<_Scalar, _T...>::Bundle(const _T<_Scalar> & ... elements)
161+
: Bundle(BegRep{}, LenRep{}, elements ...)
162162
{}
163163

164164
template<typename _Scalar, template<typename> class ... _T>
165165
template<int ... _BegRep, int ... _LenRep>
166166
Bundle<_Scalar, _T...>::Bundle(
167-
internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, const _T<_Scalar> & ... blocks)
167+
internal::intseq<_BegRep...>, internal::intseq<_LenRep...>, const _T<_Scalar> & ... elements)
168168
{
169169
// c++11 "fold expression"
170-
auto l = {((data_.template segment<_LenRep>(_BegRep) = blocks.coeffs()), 0) ...};
170+
auto l = {((data_.template segment<_LenRep>(_BegRep) = elements.coeffs()), 0) ...};
171171
static_cast<void>(l); // compiler warning
172172
}
173173

‎include/manif/impl/bundle/BundleTangent.h‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ struct traits<BundleTangent<_Scalar, _T...>>
3333
using BegAlg = intseq_psum_t<LenAlg>;
3434

3535
template<std::size_t _Idx>
36-
using BlockType = typename bundle_element<_Idx, _T<_Scalar>...>::type::Tangent;
36+
using ElementType = typename bundle_element<_Idx, _T<_Scalar>...>::type::Tangent;
3737

3838
// Regular traits
3939
using Scalar = _Scalar;
@@ -117,17 +117,17 @@ struct BundleTangent : BundleTangentBase<BundleTangent<_Scalar, _T...>>
117117
// BundleTangent specific API
118118

119119
/**
120-
* @brief Construct from BundleTangent blocks
120+
* @brief Construct from BundleTangent elements
121121
*/
122-
BundleTangent(const typename _T<_Scalar>::Tangent & ... blocks);
122+
BundleTangent(const typename _T<_Scalar>::Tangent & ... elements);
123123

124124
protected:
125125

126-
// Helper for the blocks constructor
126+
// Helper for the elements constructor
127127
template<int ... _BegRep, int ... _LenRep>
128128
BundleTangent(
129129
internal::intseq<_BegRep...>, internal::intseq<_LenRep...>,
130-
const typename _T<_Scalar>::Tangent & ... blocks);
130+
const typename _T<_Scalar>::Tangent & ... elements);
131131

132132
protected:
133133

@@ -142,18 +142,18 @@ BundleTangent<_Scalar, _T...>::BundleTangent(const TangentBase<_DerivedOther> &
142142
{}
143143

144144
template<typename _Scalar, template<typename> class ... _T>
145-
BundleTangent<_Scalar, _T...>::BundleTangent(const typename _T<_Scalar>::Tangent & ... blocks)
146-
: BundleTangent(BegRep{}, LenRep{}, blocks ...)
145+
BundleTangent<_Scalar, _T...>::BundleTangent(const typename _T<_Scalar>::Tangent & ... elements)
146+
: BundleTangent(BegRep{}, LenRep{}, elements ...)
147147
{}
148148

149149
template<typename _Scalar, template<typename> class ... _T>
150150
template<int ... _BegRep, int ... _LenRep>
151151
BundleTangent<_Scalar, _T...>::BundleTangent(
152152
internal::intseq<_BegRep...>, internal::intseq<_LenRep...>,
153-
const typename _T<_Scalar>::Tangent & ... blocks)
153+
const typename _T<_Scalar>::Tangent & ... elements)
154154
{
155155
// c++11 "fold expression"
156-
auto l = {((data_.template segment<_LenRep>(_BegRep) = blocks.coeffs()), 0) ...};
156+
auto l = {((data_.template segment<_LenRep>(_BegRep) = elements.coeffs()), 0) ...};
157157
static_cast<void>(l); // compiler warning
158158
}
159159

0 commit comments

Comments
 (0)