From a19aa793d7b8f9a80c853da668c5cca5e0dcf92a Mon Sep 17 00:00:00 2001 From: Frank Date: Mon, 8 Feb 2016 17:30:44 -0800 Subject: [PATCH] Cleaned up a number of Point3/Rot3 related uses --- gtsam/geometry/tests/testCalibratedCamera.cpp | 12 +++----- gtsam/geometry/tests/testPinholeCamera.cpp | 4 +-- gtsam/geometry/tests/testPinholePose.cpp | 4 +-- gtsam/geometry/tests/testPoint3.cpp | 12 ++++---- gtsam/geometry/tests/testRot3.cpp | 29 +++++++++---------- gtsam/geometry/tests/testSimpleCamera.cpp | 12 +++----- gtsam/geometry/tests/testStereoCamera.cpp | 6 ++-- gtsam/navigation/GPSFactor.cpp | 4 +-- gtsam/navigation/ScenarioRunner.h | 2 +- gtsam/navigation/tests/testNavState.cpp | 2 +- gtsam/slam/tests/testGeneralSFMFactor.cpp | 8 ++--- gtsam/slam/tests/testStereoFactor.cpp | 6 +--- gtsam_unstable/dynamics/VelocityConstraint.h | 2 +- tests/simulated3D.h | 6 ++-- 14 files changed, 48 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5bc645a58..cc7a0c0f8 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -30,13 +30,9 @@ using namespace gtsam; GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera) // Camera situated at 0.5 meters high, looking down -static const Pose3 pose1((Matrix3() << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); - +static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), + Point3(0, 0, 0.5)); + static const CalibratedCamera camera(pose1); static const Point3 point1(-0.08,-0.08, 0.0); diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index b2579c0d9..ce9932879 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -34,7 +34,7 @@ typedef PinholeCamera Camera; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 8f3eadc51..3c79a45e3 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -35,7 +35,7 @@ typedef PinholePose Camera; static const Cal3_S2::shared_ptr K = boost::make_shared(625, 625, 0, 0, 0); -static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); static const Camera camera(pose, K); static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a695b95dc..d0123af83 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -61,12 +61,12 @@ TEST(Point3, Lie) { /* ************************************************************************* */ TEST( Point3, arithmetic) { CHECK(P * 3 == 3 * P); - CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); - CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); - CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); - CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); - CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); - CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); + CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6))); + CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1))); + CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1))); + CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2)); + CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3))); + CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 4b3c84e01..ce5d3268c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -97,10 +97,10 @@ TEST( Rot3, equals) // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... Rot3 slow_but_correct_Rodrigues(const Vector& w) { double t = norm_2(w); - Matrix J = skewSymmetric(w / t); + Matrix3 J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); - return R; + Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + return Rot3(R); } /* ************************************************************************* */ @@ -201,7 +201,7 @@ TEST(Rot3, log) // Windows and Linux have flipped sign in quaternion mode #if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) w = (Vector(3) << x*PI, y*PI, z*PI).finished(); - R = Rot3::Rodrigues(w); + R = Rot3::Rodrigues(w); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); #else CHECK_OMEGA(x*PI,y*PI,z*PI) @@ -274,14 +274,13 @@ TEST(Rot3, manifold_expmap) } /* ************************************************************************* */ -class AngularVelocity: public Point3 { -public: - AngularVelocity(const Point3& p) : - Point3(p) { - } - AngularVelocity(double wx, double wy, double wz) : - Point3(wx, wy, wz) { - } +class AngularVelocity : public Vector3 { + public: + template + inline AngularVelocity(const Eigen::MatrixBase& v) + : Vector3(v) {} + + AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {} }; AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { @@ -294,10 +293,10 @@ TEST(Rot3, BCH) // Approximate exmap by BCH formula AngularVelocity w1(0.2, -0.1, 0.1); AngularVelocity w2(0.01, 0.02, -0.03); - Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); + Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2); Rot3 R3 = R1 * R2; Vector expected = Rot3::Logmap(R3); - Vector actual = BCH(w1, w2).vector(); + Vector actual = BCH(w1, w2); CHECK(assert_equal(expected, actual,1e-5)); } diff --git a/gtsam/geometry/tests/testSimpleCamera.cpp b/gtsam/geometry/tests/testSimpleCamera.cpp index 515542298..8db8113a1 100644 --- a/gtsam/geometry/tests/testSimpleCamera.cpp +++ b/gtsam/geometry/tests/testSimpleCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -28,13 +28,9 @@ using namespace gtsam; static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix3() << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); - +static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()), + Point3(0, 0, 0.5)); + static const SimpleCamera camera(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 3adf2257d..6508965ec 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -74,11 +74,11 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camPose((Matrix)(Matrix(3,3) << +static Pose3 camPose(Rot3((Matrix3() << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ).finished(), + ).finished()), Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index 87913cda6..2f4cb9bc3 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -43,7 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - return (p.translation() -nT_).vector(); + return p.translation().vector() -nT_.vector(); } //*************************************************************************** diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h index b038a831b..537785e06 100644 --- a/gtsam/navigation/ScenarioRunner.h +++ b/gtsam/navigation/ScenarioRunner.h @@ -73,7 +73,7 @@ class ScenarioRunner { // An accelerometer measures acceleration in body, but not gravity Vector3 actualSpecificForce(double t) const { - Rot3 bRn = scenario_->rotation(t).transpose(); + Rot3 bRn(scenario_->rotation(t).transpose()); return scenario_->acceleration_b(t) - bRn * gravity_n(); } diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index a62ca06a8..2fbb9fee3 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -81,7 +81,7 @@ TEST( NavState, Velocity) { TEST( NavState, BodyVelocity) { Matrix39 aH, eH; Velocity3 actual = kState1.bodyVelocity(aH); - EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); + EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity))); eH = numericalDerivative11( boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1); EXPECT(assert_equal((Matrix )eH, aH)); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index a634928dc..07abb557f 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -105,16 +105,16 @@ static vector genPoint3() { static vector genCameraDefaultCalibration() { vector X; - X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)))); - X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)))); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)))); return X; } static vector genCameraVariableCalibration() { const Cal3_S2 K(640, 480, 0.1, 320, 240); vector X; - X.push_back(GeneralCamera(Pose3(eye(3), Point3(-baseline / 2., 0., 0.)), K)); - X.push_back(GeneralCamera(Pose3(eye(3), Point3(baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(-baseline / 2., 0., 0.)), K)); + X.push_back(GeneralCamera(Pose3(Rot3(), Point3(baseline / 2., 0., 0.)), K)); return X; } diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index b6d2e2ef8..45b978c27 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -31,11 +31,7 @@ using namespace std; using namespace gtsam; -static Pose3 camera1((Matrix) (Matrix(3, 3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), +static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0,0,6.25)); static boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 4d1518f4b..4ce4d5758 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -115,7 +115,7 @@ private: case dynamics::EULER_END : hx = p1 + Point3(v2 * dt); break; default: assert(false); break; } - return (p2 - hx).vector(); + return p2.vector() - hx.vector(); } }; diff --git a/tests/simulated3D.h b/tests/simulated3D.h index 46945558a..cf69a8fa3 100644 --- a/tests/simulated3D.h +++ b/tests/simulated3D.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -91,7 +91,7 @@ struct PointPrior3D: public NoiseModelFactor1 { */ Vector evaluateError(const Point3& x, boost::optional H = boost::none) const { - return (prior(x, H) - measured_).vector(); + return prior(x, H).vector() - measured_.vector(); } }; @@ -122,7 +122,7 @@ struct Simulated3DMeasurement: public NoiseModelFactor2 { */ Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - measured_).vector(); + return mea(x1, x2, H1, H2).vector() - measured_.vector(); } };