diff --git a/.cproject b/.cproject index 6b282026b..dafdeed71 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -1130,6 +1136,7 @@ make + testErrors.run true false @@ -1359,6 +1366,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1441,7 +1488,6 @@ make - testSimulated2DOriented.run true false @@ -1481,7 +1527,6 @@ make - testSimulated2D.run true false @@ -1489,7 +1534,6 @@ make - testSimulated3D.run true false @@ -1503,46 +1547,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1800,6 +1804,7 @@ cpack + -G DEB true false @@ -1807,6 +1812,7 @@ cpack + -G RPM true false @@ -1814,6 +1820,7 @@ cpack + -G TGZ true false @@ -1821,6 +1828,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2185,10 +2193,10 @@ true true - + make -j4 - testQuaternion.run + testSO3.run true true true @@ -2305,14 +2313,6 @@ true true - - make - -j4 - testSimpleHelicopter.run - true - true - true - make -j1 @@ -2699,6 +2699,7 @@ make + testGraph.run true false @@ -2706,6 +2707,7 @@ make + testJunctionTree.run true false @@ -2713,6 +2715,7 @@ make + testSymbolicBayesNetB.run true false @@ -2806,6 +2809,14 @@ true true + + make + -j5 + testAdaptAutoDiff.run + true + true + true + make -j5 @@ -3190,14 +3201,6 @@ true true - - make - -j4 - testAdaptAutoDiff.run - true - true - true - make -j4 @@ -3256,7 +3259,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index d67232fe2..08ae31945 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -17,6 +17,7 @@ */ #include +#include #include namespace gtsam { @@ -44,54 +45,60 @@ SO3 Rodrigues(const double& theta, const Vector3& axis) { return R; } -// -//namespace lie_group { -///// simply convert omega to axis/angle representation -//template <> -//SO3 expmap(const Eigen::Ref& omega) { -// if (omega.isZero()) -// return SO3::Identity(); -// else { -// double angle = omega.norm(); -// return Rodrigues(angle, omega / angle); -// } -//} -// -//template <> -//Vector3 logmap(const SO3& R) { -// -// // note switch to base 1 -// const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); -// const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); -// const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); -// -// // Get trace(R) -// double tr = R.trace(); -// -// // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. -// // we do something special -// if (std::abs(tr + 1.0) < 1e-10) { -// if (std::abs(R33 + 1.0) > 1e-10) -// return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); -// else if (std::abs(R22 + 1.0) > 1e-10) -// return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); -// else -// // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit -// return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); -// } else { -// double magnitude; -// double tr_3 = tr - 3.0; // always negative -// if (tr_3 < -1e-7) { -// double theta = acos((tr - 1.0) / 2.0); -// magnitude = theta / (2.0 * sin(theta)); -// } else { -// // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) -// // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) -// magnitude = 0.5 - tr_3 * tr_3 / 12.0; -// } -// return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); -// } -//} -//} // end namespace lie_group +/// simply convert omega to axis/angle representation +SO3 SO3::Expmap(const Eigen::Ref& omega, + ChartJacobian H) { + + if (H) + CONCEPT_NOT_IMPLEMENTED; + + if (omega.isZero()) + return SO3::Identity(); + else { + double angle = omega.norm(); + return Rodrigues(angle, omega / angle); + } +} + +Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { + using std::sqrt; + using std::sin; + + if (H) + CONCEPT_NOT_IMPLEMENTED; + + // note switch to base 1 + const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); + const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); + const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); + + // Get trace(R) + double tr = R.trace(); + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr + 1.0) < 1e-10) { + if (std::abs(R33 + 1.0) > 1e-10) + return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); + else if (std::abs(R22 + 1.0) > 1e-10) + return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); + else + // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + } else { + double magnitude; + double tr_3 = tr - 3.0; // always negative + if (tr_3 < -1e-7) { + double theta = acos((tr - 1.0) / 2.0); + magnitude = theta / (2.0 * sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3 * tr_3 / 12.0; + } + return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); + } +} + } // end namespace gtsam diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index cafdc6bf7..bed2f1212 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -30,13 +30,17 @@ namespace gtsam { * We guarantee (all but first) constructors only generate from sub-manifold. * However, round-off errors in repeated composition could move off it... */ -class SO3: public Matrix3 { +class SO3: public Matrix3, public LieGroup { protected: public: enum { dimension=3 }; + /// Constructor from AngleAxisd + SO3() : Matrix3(I_3x3) { + } + /// Constructor from Eigen Matrix template SO3(const MatrixBase& R) : @@ -48,10 +52,6 @@ public: Matrix3(angleAxis) { } - static SO3 identity() { - return I_3x3; - } - void print(const std::string& s) const { std::cout << s << *this << std::endl; } @@ -60,6 +60,26 @@ public: return equal_with_abs_tol(*this, R, tol); } + static SO3 identity() { return I_3x3; } + SO3 inverse() const { return this->Matrix3::inverse(); } + + static SO3 Expmap(const Eigen::Ref& omega, ChartJacobian H = boost::none); + static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); + + Matrix3 AdjointMap() const { return *this; } + + // Chart at origin + struct ChartAtOrigin { + static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) { + return Expmap(v,H); + } + static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { + return Logmap(R,H); + } + }; + + using LieGroup::inverse; + }; template<> diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index a36d30194..3d8e41af5 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -16,19 +16,18 @@ **/ #include -#include +#include +#include #include using namespace std; using namespace gtsam; -typedef OptionalJacobian<3,3> SO3Jacobian; - //****************************************************************************** TEST(SO3 , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); -// BOOST_CONCEPT_ASSERT((IsManifold)); -// BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** @@ -37,84 +36,94 @@ TEST(SO3 , Constructor) { } //****************************************************************************** -TEST(SO3 , Invariants) { - SO3 q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); - SO3 q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - // group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!) -} +SO3 id; +Vector3 z_axis(0, 0, 1); +SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); +SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); -#if 0 //****************************************************************************** TEST(SO3 , Local) { - Vector3 z_axis(0, 0, 1); - SO3 q1(Eigen::AngleAxisd(0, z_axis)); - SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); - SO3Jacobian H1,H2; Vector3 expected(0, 0, 0.1); - Vector3 actual = traits::Local(q1, q2, H1, H2); + Vector3 actual = traits::Local(R1, R2); EXPECT(assert_equal((Vector)expected,actual)); } //****************************************************************************** TEST(SO3 , Retract) { - Vector3 z_axis(0, 0, 1); - SO3 q(Eigen::AngleAxisd(0, z_axis)); - SO3 expected(Eigen::AngleAxisd(0.1, z_axis)); Vector3 v(0, 0, 0.1); - SO3 actual = traits::Retract(q, v); - EXPECT(actual.isApprox(expected)); + SO3 actual = traits::Retract(R1, v); + EXPECT(actual.isApprox(R2)); } //****************************************************************************** TEST(SO3 , Compose) { - Vector3 z_axis(0, 0, 1); - SO3 q1(Eigen::AngleAxisd(0.2, z_axis)); - SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); - - SO3 expected = q1 * q2; + SO3 expected = R1 * R2; Matrix actualH1, actualH2; - SO3 actual = traits::Compose(q1, q2, actualH1, actualH2); + SO3 actual = traits::Compose(R1, R2, actualH1, actualH2); EXPECT(traits::Equals(expected,actual)); - Matrix numericalH1 = numericalDerivative21(traits::Compose, q1, q2); + Matrix numericalH1 = numericalDerivative21(traits::Compose, R1, R2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(traits::Compose, q1, q2); + Matrix numericalH2 = numericalDerivative22(traits::Compose, R1, R2); EXPECT(assert_equal(numericalH2,actualH2)); } //****************************************************************************** TEST(SO3 , Between) { - Vector3 z_axis(0, 0, 1); - SO3 q1(Eigen::AngleAxisd(0.2, z_axis)); - SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); - - SO3 expected = q1.inverse() * q2; + SO3 expected = R1.inverse() * R2; Matrix actualH1, actualH2; - SO3 actual = traits::Between(q1, q2, actualH1, actualH2); + SO3 actual = traits::Between(R1, R2, actualH1, actualH2); EXPECT(traits::Equals(expected,actual)); - Matrix numericalH1 = numericalDerivative21(traits::Between, q1, q2); + Matrix numericalH1 = numericalDerivative21(traits::Between, R1, R2); EXPECT(assert_equal(numericalH1,actualH1)); - Matrix numericalH2 = numericalDerivative22(traits::Between, q1, q2); + Matrix numericalH2 = numericalDerivative22(traits::Between, R1, R2); EXPECT(assert_equal(numericalH2,actualH2)); } //****************************************************************************** TEST(SO3 , Inverse) { - Vector3 z_axis(0, 0, 1); - SO3 q1(Eigen::AngleAxisd(0.1, z_axis)); SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); Matrix actualH; - SO3 actual = traits::Inverse(q1, actualH); + SO3 actual = traits::Inverse(R1, actualH); EXPECT(traits::Equals(expected,actual)); - Matrix numericalH = numericalDerivative11(traits::Inverse, q1); + Matrix numericalH = numericalDerivative11(traits::Inverse, R1); EXPECT(assert_equal(numericalH,actualH)); } -#endif + +//****************************************************************************** +TEST(SO3 , Invariants) { + check_group_invariants(id,id); + check_group_invariants(id,R1); + check_group_invariants(R2,id); + check_group_invariants(R2,R1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,R1); + check_manifold_invariants(R2,id); + check_manifold_invariants(R2,R1); + +} + +//****************************************************************************** +//TEST(SO3 , LieGroupDerivatives) { +// CHECK_LIE_GROUP_DERIVATIVES(id,id); +// CHECK_LIE_GROUP_DERIVATIVES(id,R2); +// CHECK_LIE_GROUP_DERIVATIVES(R2,id); +// CHECK_LIE_GROUP_DERIVATIVES(R2,R1); +//} + +//****************************************************************************** +TEST(SO3 , ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,R2); + CHECK_CHART_DERIVATIVES(R2,id); + CHECK_CHART_DERIVATIVES(R2,R1); +} //****************************************************************************** int main() {