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

added const to matrix() function for Rot3 to be consistent with SOn and added note about using Eigen transpose expression

release/4.3a0
Varun Agrawal 2020-03-19 12:04:32 -04:00
parent dec918c3d5
commit c96300a7c6
4 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Rot3.h
View File

@ -406,12 +406,12 @@ namespace gtsam {
/// @{ /// @{
/** return 3*3 rotation matrix */ /** return 3*3 rotation matrix */
Matrix3 matrix() const; const Matrix3 matrix() const;
/** /**
* Return 3*3 transpose (inverse) rotation matrix * Return 3*3 transpose (inverse) rotation matrix
*/ */
Matrix3 transpose() const; const Matrix3 transpose() const;
/// @deprecated, this is base 1, and was just confusing /// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const; Point3 column(int index) const;

4
gtsam/geometry/Rot3M.cpp
View File

@ -110,7 +110,7 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::transpose() const { const Matrix3 Rot3::transpose() const {
return rot_.matrix().transpose(); return rot_.matrix().transpose();
} }
@ -175,7 +175,7 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const { const Matrix3 Rot3::matrix() const {
return rot_.matrix(); return rot_.matrix();
} }

15
gtsam/geometry/Rot3Q.cpp
View File

@ -79,10 +79,15 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::transpose() const { // TODO: Maybe use return type `const Eigen::Transpose<const Matrix3>`?
// `eval` for immediate evaluation (allows compilation). // It works in Rot3M but not here, because of some weird behavior
// return Rot3(matrix()).matrix().eval().transpose(); // due to Eigen's expression templates which needs more investigation.
return matrix().eval().transpose(); // For example, if we use matrix(), the value returned has a 1e-10,
// and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary.
// Using eval() here doesn't help, it only helps if we use it in
// the downstream code.
const Matrix3 Rot3::transpose() const {
return matrix().transpose();
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -115,7 +120,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} const Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -380,7 +380,7 @@ TEST( Rot3, inverse )
Rot3 actual = R.inverse(actualH); Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R)); CHECK(assert_equal(I,actual*R));
CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); CHECK(assert_equal(actual.matrix(), R.transpose()));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R); Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH)); CHECK(assert_equal(numericalH,actualH));