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

Added transpose_ in Quaternion mode

release/4.3a0
dellaert 2014-10-22 21:55:11 +02:00
parent be676b22cf
commit 0f26842073
2 changed files with 32 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
gtsam/geometry/Rot3Q.cpp
View File

@ -29,47 +29,52 @@ namespace gtsam {
static const Matrix I3 = eye(3); static const Matrix I3 = eye(3);
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3() : quaternion_(Quaternion::Identity()) {} Rot3::Rot3() :
quaternion_(Quaternion::Identity()), transpose_(Matrix3::Identity()) {
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) : Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
quaternion_((Eigen::Matrix3d() << transpose_ << col1.x(), col1.y(), col1.z(), //
col1.x(), col2.x(), col3.x(), col2.x(), col2.y(), col2.z(), //
col1.y(), col2.y(), col3.y(), col3.x(), col3.y(), col3.z();
col1.z(), col2.z(), col3.z()).finished()) {} quaternion_ = Quaternion(transpose_.transpose());
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(double R11, double R12, double R13, Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
double R21, double R22, double R23, double R23, double R31, double R32, double R33) {
double R31, double R32, double R33) : transpose_ << R11, R21, R31, R12, R22, R32, R13, R23, R33;
quaternion_((Eigen::Matrix3d() << quaternion_ = Quaternion(transpose_.transpose());
R11, R12, R13, }
R21, R22, R23,
R31, R32, R33).finished()) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) : Rot3::Rot3(const Matrix3& R) :
quaternion_(R) {} quaternion_(R), transpose_(R.transpose()) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Matrix& R) : Rot3::Rot3(const Matrix& R) :
quaternion_(Matrix3(R)) {} quaternion_(Matrix3(R)), transpose_(R.transpose()) {}
// /* ************************************************************************* */
// Rot3::Rot3(const Matrix3& R) :
// quaternion_(R) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} Rot3::Rot3(const Quaternion& q) :
quaternion_(q), transpose_(q.toRotationMatrix().transpose()) {
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } Rot3 Rot3::Rx(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } Rot3 Rot3::Ry(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } Rot3 Rot3::Rz(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(

6
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -285,8 +285,10 @@ TEST( Rot3, inverse )
Rot3 I; Rot3 I;
Matrix actualH; Matrix actualH;
CHECK(assert_equal(I,R*R.inverse(actualH))); Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R.inverse()*R)); CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R));
CHECK(assert_equal((Matrix)actual.matrix(), R.transpose()));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R); Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH, 1e-4)); CHECK(assert_equal(numericalH,actualH, 1e-4));