diff --git a/cpp/Lie.h b/cpp/Lie.h index 106bbd325..6d6f52623 100644 --- a/cpp/Lie.h +++ b/cpp/Lie.h @@ -25,15 +25,15 @@ namespace gtsam { // Compute l1 s.t. l2=l1*l0 template - inline T between(const T& l0, const T& l2) { return l2*inverse(l0); } + inline T between(const T& l1, const T& l2) { return inverse(l1)*l2; } // Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp template inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); } - /* Exponential map centered at l0, s.t. exp(l0,v) = exp(v)*l0 */ + /* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ template - inline T expmap(const T& l0, const Vector& v) { return expmap(v)*l0; } + inline T expmap(const T& t, const Vector& d) { return t * expmap(d); } /** * Base class for Lie group type diff --git a/cpp/Pose2.h b/cpp/Pose2.h index 2f1298f13..b415b71fc 100644 --- a/cpp/Pose2.h +++ b/cpp/Pose2.h @@ -73,7 +73,7 @@ namespace gtsam { pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); } /** compose this transformation onto another (pre-multiply this*p1) */ - inline Pose2 compose(const Pose2& p1, const Pose2& p0) { + inline Pose2 compose(const Pose2& p0, const Pose2& p1) { return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); } /** exponential and log maps around identity */ diff --git a/cpp/Pose3.cpp b/cpp/Pose3.cpp index 69e0e9670..f75598c32 100644 --- a/cpp/Pose3.cpp +++ b/cpp/Pose3.cpp @@ -122,9 +122,8 @@ namespace gtsam { /* ************************************************************************* */ Matrix Dtransform_to1(const Pose3& pose, const Point3& p) { Point3 q = transform_to(pose,p); - Matrix Rt = pose.rotation().transpose(); - Matrix DR = skewSymmetric(q.x(), q.y(), q.z()) * Rt; - Matrix DT = - Rt; // negative because of sub + Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); + Matrix DT = - pose.rotation().transpose(); // negative because of sub return collect(2,&DR,&DT); } @@ -137,7 +136,7 @@ namespace gtsam { // compose = Pose3(compose(R1,R2),transform_from(p1,t2); Matrix Dcompose1(const Pose3& p1, const Pose3& p2) { - static const Matrix DR_R1 = eye(3); + Matrix DR_R1 = p2.rotation().transpose(); Matrix DR_t1 = zeros(3, 3); Matrix DR = collect(2, &DR_R1, &DR_t1); Matrix Dt = Dtransform_from1(p1, p2.translation()); @@ -145,8 +144,10 @@ namespace gtsam { } Matrix Dcompose2(const Pose3& p1, const Pose3& p2) { - Matrix R1 = p1.rotation().matrix(), Z3 = zeros(3, 3); - Matrix DR = collect(2, &R1, &Z3); + Matrix R1 = p1.rotation().matrix(); + const static Matrix I = eye(3,3); + const static Matrix Z3 = zeros(3, 3); + Matrix DR = collect(2, &I, &Z3); Matrix Dt = collect(2, &Z3, &R1); return stack(2, &DR, &Dt); } @@ -155,10 +156,10 @@ namespace gtsam { // inverse = Pose3(inverse(R),-unrotate(R,t)); Matrix Dinverse(const Pose3& p) { Matrix Rt = p.rotation().transpose(); - Matrix DR_R1 = -Rt; + Matrix DR_R1 = -p.rotation().matrix(); Matrix DR_t1 = zeros(3, 3); Matrix DR = collect(2, &DR_R1, &DR_t1); - Matrix Dt_R1 = -(skewSymmetric(Rt * p.translation().vector()) * Rt); + Matrix Dt_R1 = -skewSymmetric(unrotate(p.rotation(),p.translation()).vector()); Matrix Dt_t1 = -Rt; Matrix Dt = collect(2, &Dt_R1, &Dt_t1); return stack(2, &DR, &Dt); @@ -169,9 +170,9 @@ namespace gtsam { Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional H1, boost::optional H2) { Pose3 invp1 = inverse(p1); - Pose3 result = compose(p2, invp1); - if (H1) *H1 = Dcompose2(p2, invp1) * Dinverse(p1); - if (H2) *H2 = Dcompose1(p2, invp1); + Pose3 result = compose(invp1, p2); + if (H1) *H1 = Dcompose1(invp1, p2) * Dinverse(p1); + if (H2) *H2 = Dcompose2(invp1, p2); return result; } diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index d02653e8c..d21fc21ec 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -171,8 +171,7 @@ namespace gtsam { /* ************************************************************************* */ Matrix Drotate1(const Rot3& R, const Point3& p) { - Point3 q = R * p; - return skewSymmetric(-q.x(), -q.y(), -q.z()); + return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); } /* ************************************************************************* */ @@ -194,7 +193,7 @@ namespace gtsam { /* ************************************************************************* */ Matrix Dunrotate1(const Rot3 & R, const Point3 & p) { Point3 q = unrotate(R,p); - return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose(); + return skewSymmetric(q.x(), q.y(), q.z()); } /* ************************************************************************* */ @@ -204,22 +203,22 @@ namespace gtsam { /* ************************************************************************* */ Matrix Dcompose1(const Rot3& R1, const Rot3& R2){ - return eye(3); + return R2.transpose(); } /* ************************************************************************* */ Matrix Dcompose2(const Rot3& R1, const Rot3& R2){ - return R1.matrix(); + return eye(3); } /* ************************************************************************* */ Matrix Dbetween1(const Rot3& R1, const Rot3& R2){ - return -between(R1,R2).matrix(); + return -(R2.transpose()*R1.matrix()); } /* ************************************************************************* */ Matrix Dbetween2(const Rot3& R1, const Rot3& R2){ - return eye(3); + return eye(3); } /* ************************************************************************* */ diff --git a/cpp/Rot3.h b/cpp/Rot3.h index 03181ea1b..b2635277b 100644 --- a/cpp/Rot3.h +++ b/cpp/Rot3.h @@ -171,6 +171,9 @@ namespace gtsam { R.r3().x(), R.r3().y(), R.r3().z()); } + // and its derivative + inline Matrix Dinverse(Rot3 R) { return -R.matrix();} + /** * rotate point from rotated coordinate frame to * world = R*p diff --git a/cpp/dataset.cpp b/cpp/dataset.cpp index 5f4a5da4a..758693204 100644 --- a/cpp/dataset.cpp +++ b/cpp/dataset.cpp @@ -51,8 +51,14 @@ pair > dataset(const string& dataset, c /* ************************************************************************* */ +pair load2D( + pair > dataset, + int maxID, bool addNoise, bool smart) { + return load2D(dataset.first, dataset.second, maxID, addNoise, smart); +} + pair load2D(const string& filename, - int maxID, boost::optional model, bool addNoise, bool smart) { + boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) { @@ -108,7 +114,7 @@ pair load2D(const string& filename, if (maxID && (id1 >= maxID || id2 >= maxID)) continue; - measured = inverse(Pose2(x, y, yaw)); + measured = Pose2(x, y, yaw); // SharedGaussian noise = noiseModel::Gaussian::Covariance(m, smart); // hack use diagonal for now ! @@ -123,7 +129,7 @@ pair load2D(const string& filename, // Insert vertices if pure odometry file if (!poses->exists(id1)) poses->insert(id1, Pose2()); - if (!poses->exists(id2)) poses->insert(id2, measured * poses->at(id1)); + if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * measured); Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2,measured,*model)); graph->push_back(factor); diff --git a/cpp/dataset.h b/cpp/dataset.h index 3dc337d6e..278a155b4 100644 --- a/cpp/dataset.h +++ b/cpp/dataset.h @@ -33,9 +33,12 @@ std::pair > * @param smart: try to reduce complexity of covariance to cheapest model */ std::pair, boost::shared_ptr > load2D( - const std::string& filename, int maxID = 0, + std::pair > dataset, + int maxID = 0, bool addNoise=false, bool smart=true); +std::pair, boost::shared_ptr > load2D( + const std::string& filename, boost::optional model = boost::optional(), - bool addNoise=false, bool smart=true); + int maxID = 0, bool addNoise=false, bool smart=true); /** save 2d graph */ void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Config& config, const gtsam::SharedDiagonal model, diff --git a/cpp/pose3SLAM.cpp b/cpp/pose3SLAM.cpp index ed909d4c8..c58bf65ce 100644 --- a/cpp/pose3SLAM.cpp +++ b/cpp/pose3SLAM.cpp @@ -20,14 +20,22 @@ namespace gtsam { namespace pose3SLAM { /* ************************************************************************* */ - Config circle(size_t n, double R) { + Config circle(size_t n, double radius) { Config x; double theta = 0, dtheta = 2 * M_PI / n; - // Vehicle at p0 is looking towards y axis - Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); - for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta), - sin(theta), 0))); + // We use aerospace/navlab convention, X forward, Y right, Z down + // First pose will be at (R,0,0) + // ^y ^ X + // | | + // z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) + // Vehicle at p0 is looking towards y axis (X-axis points towards world y) + Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1)); + for (size_t i = 0; i < n; i++, theta += dtheta) { + Point3 gti(radius*cos(theta), radius*sin(theta), 0); + Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! + Pose3 gTi(gR0 * _0Ri, gti); + x.insert(i, gTi); + } return x; } diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index fbd399fc7..316c96cc2 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -31,10 +31,10 @@ TEST(Pose2, manifold) { Pose2 origin; Vector d12 = logmap(t1,t2); CHECK(assert_equal(t2, expmap(t1,d12))); - CHECK(assert_equal(t2, expmap(origin,d12)*t1)); + CHECK(assert_equal(t2, t1*expmap(origin,d12))); Vector d21 = logmap(t2,t1); CHECK(assert_equal(t1, expmap(t2,d21))); - CHECK(assert_equal(t1, expmap(origin,d21)*t2)); + CHECK(assert_equal(t1, t2*expmap(origin,d21))); } /* ************************************************************************* */ @@ -51,7 +51,7 @@ TEST(Pose2, expmap0) { //cout << "expmap0" << endl; Pose2 pose(M_PI_2, Point2(1, 2)); Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01)); - Pose2 actual = expmap(Vector_(3, 0.01, -0.015, 0.018)) * pose; + Pose2 actual = pose * expmap(Vector_(3, 0.01, -0.015, 0.018)); CHECK(assert_equal(expected, actual)); } @@ -105,12 +105,12 @@ TEST(Pose2, compose_a) Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0)); Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5))); - Pose2 actual = pose2 * pose1; + Pose2 actual = pose1 * pose2; CHECK(assert_equal(expected, actual)); Point2 point(sqrt(0.5), 3.0*sqrt(0.5)); Point2 expected_point(-1.0, -1.0); - Point2 actual_point1 = transform_to(pose2 * pose1, point); + Point2 actual_point1 = transform_to(pose1 * pose2, point); Point2 actual_point2 = transform_to(pose2, transform_to(pose1, point)); CHECK(assert_equal(expected_point, actual_point1)); CHECK(assert_equal(expected_point, actual_point2)); @@ -125,8 +125,8 @@ TEST(Pose2, compose_b) Pose2 pose_expected(Rot2(M_PI/4.0), Point2(1.0, 2.0)); - Pose2 pose_actual_op = pose2 * pose1; - Pose2 pose_actual_fcn = compose(pose2,pose1); + Pose2 pose_actual_op = pose1 * pose2; + Pose2 pose_actual_fcn = compose(pose1, pose2); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -141,8 +141,8 @@ TEST(Pose2, compose_c) Pose2 pose_expected(Rot2(M_PI/2.0), Point2(1.0, 2.0)); - Pose2 pose_actual_op = pose2 * pose1; - Pose2 pose_actual_fcn = compose(pose2,pose1); + Pose2 pose_actual_op = pose1 * pose2; + Pose2 pose_actual_fcn = compose(pose1,pose2); CHECK(assert_equal(pose_expected, pose_actual_op)); CHECK(assert_equal(pose_expected, pose_actual_fcn)); @@ -215,8 +215,7 @@ TEST( Pose2, compose_matrix ) Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2)); - CHECK(assert_equal(gM1*_1M2,matrix(compose(_1T2,gT1)))); // WRONG CHECKS OUT ! - // CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); RIGHT DOES NOT + CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); // RIGHT DOES NOT } /* ************************************************************************* */ @@ -276,7 +275,7 @@ TEST( Pose2, round_trip ) { Pose2 p1(1.23, 2.30, 0.2); Pose2 odo(0.53, 0.39, 0.15); - Pose2 p2 = compose(odo, p1); + Pose2 p2 = compose(p1, odo); CHECK(assert_equal(odo, between(p1, p2))); } diff --git a/cpp/testPose3.cpp b/cpp/testPose3.cpp index 4924fa5e4..63af28373 100644 --- a/cpp/testPose3.cpp +++ b/cpp/testPose3.cpp @@ -245,29 +245,6 @@ TEST( Pose3, transformPose_to) CHECK(assert_equal(expected, actual, 0.001)); } -/* ************************************************************************* */ -TEST( Pose3, composeTransform ) -{ - // known transform - Rot3 R1 = rodriguez(0, 0, -1.570796); - Pose3 expected(R1, Point3(1, 2, 3)); - - // current - Rot3 R2 = rodriguez(0, 0, 0.698131701); - Pose3 current(R2, Point3(21., 32., 13.)); - - // target - Pose3 target(rodriguez(0, 0, 2.26892803), Point3(-30., 20., 10.)); - - // calculate transform - // todo: which should this be? - //Pose3 actual = compose(current, target); - Pose3 actual = between (target, current); - - //verify - CHECK(assert_equal(expected, actual, 0.001)); -} - /* ************************************************************************* */ TEST(Pose3, manifold) { @@ -311,7 +288,7 @@ TEST( Pose3, between ) Point3 t(3.5,-8.2,4.2); Pose3 T(R,t); - Pose3 expected = pose1 * inverse(T); + Pose3 expected = inverse(T) * pose1; Matrix actualH1,actualH2; Pose3 actual = between(T, pose1, actualH1,actualH2); CHECK(assert_equal(expected,actual)); diff --git a/cpp/testPose3SLAM.cpp b/cpp/testPose3SLAM.cpp index 6d8eeb411..faa8c280a 100644 --- a/cpp/testPose3SLAM.cpp +++ b/cpp/testPose3SLAM.cpp @@ -31,23 +31,26 @@ static Matrix covariance = eye(6); TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses - Pose3Config hexagon = pose3SLAM::circle(6,1.0); - Pose3 p0 = hexagon[0], p1 = hexagon[1]; + double radius = 10; + Pose3Config hexagon = pose3SLAM::circle(6,radius); + Pose3 gT0 = hexagon[0], gT1 = hexagon[1]; // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); - fg->addHardConstraint(0, p0); - Pose3 delta = between(p0,p1); - fg->addConstraint(0,1, delta, covariance); - fg->addConstraint(1,2, delta, covariance); - fg->addConstraint(2,3, delta, covariance); - fg->addConstraint(3,4, delta, covariance); - fg->addConstraint(4,5, delta, covariance); - fg->addConstraint(5,0, delta, covariance); + fg->addHardConstraint(0, gT0); + Pose3 _0T1 = between(gT0,gT1); // inv(gT0)*gT1 + double theta = M_PI/3.0; + CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1)); + fg->addConstraint(0,1, _0T1, covariance); + fg->addConstraint(1,2, _0T1, covariance); + fg->addConstraint(2,3, _0T1, covariance); + fg->addConstraint(3,4, _0T1, covariance); + fg->addConstraint(4,5, _0T1, covariance); + fg->addConstraint(5,0, _0T1, covariance); // Create initial config boost::shared_ptr initial(new Pose3Config()); - initial->insert(0, p0); + initial->insert(0, gT0); initial->insert(1, expmap(hexagon[1],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); initial->insert(2, expmap(hexagon[2],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); initial->insert(3, expmap(hexagon[3],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); @@ -67,10 +70,10 @@ TEST(Pose3Graph, optimizeCircle) { Pose3Config actual = *optimizer.config(); // Check with ground truth - CHECK(assert_equal(hexagon, actual,1e-5)); + CHECK(assert_equal(hexagon, actual,1e-4)); // Check loop closure - CHECK(assert_equal(delta,between(actual[5],actual[0]),1e-5)); + CHECK(assert_equal(_0T1,between(actual[5],actual[0]),1e-5)); } /* ************************************************************************* */ diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 16f32095a..ffb202a90 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -143,17 +143,17 @@ TEST(Rot3, log) /* ************************************************************************* */ TEST(Rot3, manifold) { - Rot3 t1 = rodriguez(0.1, 0.4, 0.2); - Rot3 t2 = rodriguez(0.3, 0.1, 0.7); + Rot3 gR1 = rodriguez(0.1, 0.4, 0.2); + Rot3 gR2 = rodriguez(0.3, 0.1, 0.7); Rot3 origin; // log behaves correctly - Vector d12 = logmap(t1, t2); - CHECK(assert_equal(t2, expmap(t1,d12))); - CHECK(assert_equal(t2, expmap(d12)*t1)); - Vector d21 = logmap(t2, t1); - CHECK(assert_equal(t1, expmap(t2,d21))); - CHECK(assert_equal(t1, expmap(d21)*t2)); + Vector d12 = logmap(gR1, gR2); + CHECK(assert_equal(gR2, expmap(gR1,d12))); + CHECK(assert_equal(gR2, gR1*expmap(d12))); + Vector d21 = logmap(gR2, gR1); + CHECK(assert_equal(gR1, expmap(gR2,d21))); + CHECK(assert_equal(gR1, gR2*expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); @@ -237,6 +237,21 @@ TEST( Rot3, compose ) CHECK(assert_equal(numericalH2,actualH2)); } +/* ************************************************************************* */ + +TEST( Rot3, inverse ) +{ + Rot3 R = rodriguez(0.1, 0.2, 0.3); + + Rot3 I; + CHECK(assert_equal(I,R*inverse(R))); + CHECK(assert_equal(I,inverse(R)*R)); + + Matrix numericalH = numericalDerivative11(inverse, R, 1e-5); + Matrix actualH = Dinverse(R); + CHECK(assert_equal(numericalH,actualH)); +} + /* ************************************************************************* */ TEST( Rot3, between ) { @@ -248,7 +263,7 @@ TEST( Rot3, between ) Rot3 R1 = rodriguez(0.1, 0.2, 0.3); Rot3 R2 = rodriguez(0.2, 0.3, 0.5); - Rot3 expected = R2*inverse(R1); + Rot3 expected = inverse(R1)*R2; Rot3 actual = between(R1, R2); CHECK(assert_equal(expected,actual));