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

Fixed row/col major bug in LieMatrix Lie group Logmap

release/4.3a0
Richard Roberts 2013-10-13 22:21:02 +00:00
parent 7573b7e582
commit 9dfd9297f3
2 changed files with 13 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/base/LieMatrix.h
View File

@ -149,10 +149,14 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
throw std::runtime_error("LieMatrix::Expmap(): Don't use this function");
return LieMatrix(v); }
/** Logmap around identity - just returns with default cast back */
/** Logmap around identity */
static inline Vector Logmap(const LieMatrix& p) {
return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
Vector result(p.size());
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
result.data(), p.rows(), p.cols()) = p;
return result;
}
/// @}
private:

8
gtsam/base/tests/testLieMatrix.cpp
View File

@ -57,10 +57,14 @@ TEST(LieMatrix, retract) {
EXPECT(assert_equal(expected, actual));
LieMatrix expectedUpdate = update;
LieMatrix actualUpdate = init.localCoordinates(actual);
Vector expectedUpdate = update;
Vector actualUpdate = init.localCoordinates(actual);
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vec() << 1, 2, 3, 4);
Vector actualLogmap = LieMatrix::Logmap(LieMatrix(2,2, 1.0, 2.0, 3.0, 4.0));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}
/* ************************************************************************* */