diff --git a/.cproject b/.cproject index e9d7d08c9..1e381c478 100644 --- a/.cproject +++ b/.cproject @@ -378,6 +378,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -404,7 +412,6 @@ make - tests/testBayesTree.run true false @@ -412,7 +419,6 @@ make - testBinaryBayesNet.run true false @@ -460,7 +466,6 @@ make - testSymbolicBayesNet.run true false @@ -468,7 +473,6 @@ make - tests/testSymbolicFactor.run true false @@ -476,7 +480,6 @@ make - testSymbolicFactorGraph.run true false @@ -492,20 +495,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -532,6 +526,7 @@ make + testGraph.run true false @@ -603,6 +598,7 @@ make + testInference.run true false @@ -610,6 +606,7 @@ make + testGaussianFactor.run true false @@ -617,6 +614,7 @@ make + testJunctionTree.run true false @@ -624,6 +622,7 @@ make + testSymbolicBayesNet.run true false @@ -631,6 +630,7 @@ make + testSymbolicFactorGraph.run true false @@ -700,22 +700,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -732,6 +716,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -756,15 +756,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -772,14 +764,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -820,7 +804,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -828,6 +820,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1150,7 +1150,6 @@ make - testErrors.run true false @@ -1476,6 +1475,22 @@ true true + + make + -j2 + tests/testPoint3.run + true + true + true + + + make + -j2 + tests/testCalibratedCamera.run + true + true + true + make -j2 @@ -1558,6 +1573,7 @@ make + testSimulated2DOriented.run true false @@ -1597,6 +1613,7 @@ make + testSimulated2D.run true false @@ -1604,6 +1621,7 @@ make + testSimulated3D.run true false @@ -1946,6 +1964,7 @@ make + tests/testGaussianISAM2 true false @@ -1967,46 +1986,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -2103,6 +2082,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + make -j2 @@ -2159,54 +2226,6 @@ false true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index a45da9789..a442e9357 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -30,10 +30,13 @@ CalibratedCamera::CalibratedCamera(const Pose3& pose) : CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} /* ************************************************************************* */ -Point2 CalibratedCamera::project_to_camera(const Point3& P, boost::optional H1) { +Point2 CalibratedCamera::project_to_camera(const Point3& P, + boost::optional H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + *H1 = Matrix_(2, 3, + d, 0.0, -P.x() * d2, + 0.0, d, -P.y() * d2); } return Point2(P.x() / P.z(), P.y() / P.z()); } @@ -55,30 +58,25 @@ CalibratedCamera CalibratedCamera::level(const Pose2& pose2, double height) { /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - boost::optional D_intrinsic_pose, - boost::optional D_intrinsic_point) const { - const Pose3& pose = pose_; - const Rot3& R = pose.rotation(); - const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); - Point3 q = pose.transform_to(point); + boost::optional H1, + boost::optional H2) const { + + Point3 q = pose_.transform_to(point, H1, H2); + + // Check if point is in front of camera if(q.z() <= 0) throw CheiralityException(); - if (D_intrinsic_pose || D_intrinsic_point) { - double X = q.x(), Y = q.y(), Z = q.z(); - double d = 1.0 / Z, d2 = d * d, Xd2 = X*d2, Yd2 = Y*d2; - double dp11 = d*r1.x()-r3.x()*Xd2, dp12 = d*r1.y()-r3.y()*Xd2, dp13 = d*r1.z()-r3.z()*Xd2; - double dp21 = d*r2.x()-r3.x()*Yd2, dp22 = d*r2.y()-r3.y()*Yd2, dp23 = d*r2.z()-r3.z()*Yd2; - if (D_intrinsic_pose) - *D_intrinsic_pose = Matrix_(2,6, - X*Yd2, -Z*d-X*Xd2, d*Y, -dp11, -dp12, -dp13, - d*Z+Y*Yd2, -X*Yd2, -d*X, -dp21, -dp22, -dp23); - if (D_intrinsic_point) - *D_intrinsic_point = Matrix_(2,3, - dp11, dp12, dp13, - dp21, dp22, dp23); + if (H1 || H2) { + Matrix H; + Point2 intrinsic = project_to_camera(q,H); + // just implement chain rule + if (H1) *H1 = H * (*H1); + if (H1) *H2 = H * (*H2); + return intrinsic; } - return project_to_camera(q); + else + return project_to_camera(q); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 528400d6f..9288731d0 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -29,10 +29,7 @@ namespace gtsam { /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); - static const Matrix I3 = eye(3), Z3 = zeros(3, 3); -#ifdef CORRECT_POSE3_EXMAP - static const Matrix _I3=-I3, I6 = eye(6); -#endif + static const Matrix I3 = eye(3), Z3 = zeros(3, 3), _I3=-I3, I6 = eye(6); /* ************************************************************************* */ // Calculate Adjoint map @@ -94,42 +91,60 @@ namespace gtsam { } } -#ifdef CORRECT_POSE3_EXMAP /* ************************************************************************* */ // Changes default to use the full verions of expmap/logmap /* ************************************************************************* */ /* ************************************************************************* */ - Pose3 Pose3::retract(const Vector& d) const { - return compose(Expmap(d)); - } - - /* ************************************************************************* */ - Vector Pose3::localCoordinates(const Pose3& T2) const { - return Logmap(between(T2)); - } - + // Different versions of retract + Pose3 Pose3::retract(const Vector& xi) const { +#ifdef CORRECT_POSE3_EXMAP + // Lie group exponential map, traces out geodesic + return compose(Expmap(xi)); #else + Vector omega(sub(xi, 0, 3)); + Point3 v(sub(xi, 3, 6)); + + // R is always done exactly in all three retract versions below + Rot3 R = R_.retract(omega); + + // Incorrect version + // Retracts R and t independently + // Point3 t = t_.retract(v.vector()); + + // First order t approximation + Point3 t = t_ + R_ * v; + + // Second order t approximation + // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); + + return Pose3(R, t); +#endif + } /* ************************************************************************* */ - /** These are the "old-style" expmap and logmap about the specified - * pose. Increments the offset and rotation independently given a translation and - * canonical rotation coordinates. Created to match ML derivatives, but - * superseded by the correct exponential map story in .cpp */ - Pose3 Pose3::retract(const Vector& d) const { - return Pose3(R_.retract(sub(d, 0, 3)), - t_.retract(sub(d, 3, 6))); - } + // different versions of localCoordinates + Vector Pose3::localCoordinates(const Pose3& T) const { +#ifdef CORRECT_POSE3_EXMAP + // Lie group logarithm map, exact inverse of exponential map + return Logmap(between(T)); +#else + // R is always done exactly in all three retract versions below + Vector omega = R_.localCoordinates(T.rotation()); - /* ************************************************************************* */ - /** Independently computes the logmap of the translation and rotation. */ - Vector Pose3::localCoordinates(const Pose3& pp) const { - const Vector r(R_.localCoordinates(pp.rotation())), - t(t_.localCoordinates(pp.translation())); - return concatVectors(2, &r, &t); - } + // Incorrect version + // Independently computes the logmap of the translation and rotation + // Vector v = t_.localCoordinates(T.translation()); - #endif + // Correct first order t inverse + Point3 d = R_.unrotate(T.translation() - t_); + Vector v = d.vector(); + + // TODO: correct second order t inverse + + return concatVectors(2, &omega, &v); +#endif + } /* ************************************************************************* */ Matrix Pose3::matrix() const { @@ -150,15 +165,9 @@ namespace gtsam { Point3 Pose3::transform_from(const Point3& p, boost::optional H1, boost::optional H2) const { if (H1) { -#ifdef CORRECT_POSE3_EXMAP const Matrix R = R_.matrix(); Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); *H1 = collect(2,&DR,&R); -#else - Matrix DR; - R_.rotate(p, DR, boost::none); - *H1 = collect(2,&DR,&I3); -#endif } if (H2) *H2 = R_.matrix(); return R_ * p + t_; @@ -168,17 +177,11 @@ namespace gtsam { Point3 Pose3::transform_to(const Point3& p, boost::optional H1, boost::optional H2) const { const Point3 result = R_.unrotate(p - t_); - if (H1) { - const Point3& q = result; - Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); -#ifdef CORRECT_POSE3_EXMAP - *H1 = collect(2, &DR, &_I3); -#else - Matrix DT = - R_.transpose(); // negative because of sub - *H1 = collect(2,&DR,&DT); -#endif - } - + if (H1) { + const Point3& q = result; + Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); + *H1 = collect(2, &DR, &_I3); + } if (H2) *H2 = R_.transpose(); return result; } @@ -186,47 +189,14 @@ namespace gtsam { /* ************************************************************************* */ Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, boost::optional H2) const { - if (H1) { -#ifdef CORRECT_POSE3_EXMAP - *H1 = p2.inverse().adjointMap(); -#else - const Rot3& R2 = p2.rotation(); - const Point3& t2 = p2.translation(); - Matrix DR_R1 = R2.transpose(), DR_t1 = Z3; - Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt; - transform_from(t2, Dt, boost::none); - *H1 = gtsam::stack(2, &DR, &Dt); -#endif - } - if (H2) { -#ifdef CORRECT_POSE3_EXMAP - *H2 = I6; -#else - Matrix R1 = rotation().matrix(); - Matrix DR = collect(2, &I3, &Z3); - Matrix Dt = collect(2, &Z3, &R1); - *H2 = gtsam::stack(2, &DR, &Dt); -#endif - } - return (*this) * p2; + if (H1) *H1 = p2.inverse().adjointMap(); + if (H2) *H2 = I6; + return (*this) * p2; } /* ************************************************************************* */ Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) -#ifdef CORRECT_POSE3_EXMAP - { *H1 = - adjointMap(); } -#else - { - Matrix Rt = R_.transpose(); - Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; - Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; - Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt = collect(2, &Dt_R1, &Dt_t1); - *H1 = gtsam::stack(2, &DR, &Dt); - } -#endif + if (H1) *H1 = -adjointMap(); Rot3 Rt = R_.inverse(); return Pose3(Rt, Rt*(-t_)); } @@ -262,11 +232,7 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { double r = range(point.translation(), H1, H2); if (H2) { -#ifdef CORRECT_POSE3_EXMAP Matrix H2_ = *H2 * point.rotation().matrix(); -#else - Matrix H2_ = *H2; -#endif *H2 = zeros(1, 6); insertSub(*H2, H2_, 0, 3); } diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index df491b91f..db48057a7 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -44,12 +44,12 @@ TEST( Pose3, equals) } /* ************************************************************************* */ -TEST( Pose3, expmap_a) +TEST( Pose3, retract) { Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(id.retract(v), Pose3(R, Point3()))); + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v))); #ifdef CORRECT_POSE3_EXMAP v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; #else @@ -195,9 +195,7 @@ TEST( Pose3, compose ) Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); -#ifdef CORRECT_POSE3_EXMAP EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3)); -#endif Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4)); @@ -217,9 +215,7 @@ TEST( Pose3, compose2 ) Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); -#ifdef CORRECT_POSE3_EXMAP EXPECT(assert_equal(T2.inverse().adjointMap(),actualDcompose1,5e-3)); -#endif Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5)); @@ -235,9 +231,7 @@ TEST( Pose3, inverse) Matrix numericalH = numericalDerivative11(testing::inverse, T); EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); -#ifdef CORRECT_POSE3_EXMAP EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3)); -#endif } /* ************************************************************************* */ @@ -251,9 +245,7 @@ TEST( Pose3, inverseDerivatives2) Matrix actualDinverse; T.inverse(actualDinverse); EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); -#ifdef CORRECT_POSE3_EXMAP EXPECT(assert_equal(-T.adjointMap(),actualDinverse,5e-3)); -#endif } /* ************************************************************************* */ @@ -450,10 +442,17 @@ TEST( Pose3, transformPose_to) EXPECT(assert_equal(expected, actual, 0.001)); } +/* ************************************************************************* */ +TEST(Pose3, localCoordinates) +{ + Vector d12 = repeat(6,0.1); + Pose3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} + /* ************************************************************************* */ TEST(Pose3, manifold) { - //cout << "manifold" << endl; Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; @@ -466,13 +465,13 @@ TEST(Pose3, manifold) // todo: richard - commented out because this tests for "compose-style" (new) expmap // EXPECT(assert_equal(t1, retract(origin,d21)*t2)); - // Check that log(t1,t2)=-log(t2,t1) - this holds even for incorrect expmap :-) - EXPECT(assert_equal(d12,-d21)); - -#ifdef CORRECT_POSE3_EXMAP - - - // todo: Frank - Below only works for correct "Agrawal06iros style expmap + // Check that log(t1,t2)=-log(t2,t1) TODO: only holds for exp map + // EXPECT(assert_equal(d12,-d21)); +} +/* ************************************************************************* */ +TEST(Pose3, subgroups) +{ + // Frank - Below only works for correct "Agrawal06iros style expmap // lines in canonical coordinates correspond to Abelian subgroups in SE(3) Vector d = Vector_(6,0.1,0.2,0.3,0.4,0.5,0.6); // exp(-d)=inverse(exp(d)) @@ -483,8 +482,6 @@ TEST(Pose3, manifold) Pose3 T5 = Pose3::Expmap(5*d); EXPECT(assert_equal(T5,T2*T3)); EXPECT(assert_equal(T5,T3*T2)); - -#endif } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index 1eb62ec93..949afea0f 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -197,21 +197,10 @@ TEST( Pose3Values, pose3Circle ) TEST( Pose3Values, expmap ) { Pose3Values expected; -#ifdef CORRECT_POSE3_EXMAP expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); -#else - // expected is circle shifted to East - expected.insert(0, Pose3(R1, Point3( 1.1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0.1, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-0.9, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1, 0))); -#endif - - // Note expmap coordinates are in global coordinates with non-compose expmap - // so shifting to East requires little thought, different from with Pose2 !!! Ordering ordering(*expected.orderingArbitrary()); VectorValues delta(expected.dims(ordering));