12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

replaces insertsub in Pose2. make check works. Are all functions in Matrix.h required ?

release/4.3a0
nsrinivasan7 2014-11-30 08:33:35 -05:00
parent bbe657d19d
commit 2c4adcc6af
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose2.cpp
View File

@ -213,9 +213,9 @@ Rot2 Pose2::bearing(const Pose2& point,
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
Rot2 result = bearing(point.t(), H1, H2);
if (H2) {
Matrix H2_ = *H2 * point.r().matrix();
Matrix2 H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
(*H2).block(0, 0, H2_.rows(), H2_.cols()) = H2_;
}
return result;
}