diff --git a/gtsam/geometry/SphericalCamera.cpp b/gtsam/geometry/SphericalCamera.cpp index bc4fb2015..58a29dc09 100644 --- a/gtsam/geometry/SphericalCamera.cpp +++ b/gtsam/geometry/SphericalCamera.cpp @@ -23,14 +23,12 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -bool SphericalCamera::equals(const SphericalCamera &camera, double tol) const { +bool SphericalCamera::equals(const SphericalCamera& camera, double tol) const { return pose_.equals(camera.pose(), tol); } /* ************************************************************************* */ -void SphericalCamera::print(const string& s) const { - pose_.print(s + ".pose"); -} +void SphericalCamera::print(const string& s) const { pose_.print(s + ".pose"); } /* ************************************************************************* */ pair SphericalCamera::projectSafe(const Point3& pw) const { @@ -41,37 +39,33 @@ pair SphericalCamera::projectSafe(const Point3& pw) const { /* ************************************************************************* */ Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose, - OptionalJacobian<2, 3> Dpoint) const { - + OptionalJacobian<2, 3> Dpoint) const { Matrix36 Dtf_pose; - Matrix3 Dtf_point; // calculated by transformTo if needed - const Point3 pc = pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0); + Matrix3 Dtf_point; // calculated by transformTo if needed + const Point3 pc = + pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0); - if (pc.norm() <= 1e-8) - throw("point cannot be at the center of the camera"); + if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera"); - Matrix23 Dunit; // calculated by FromPoint3 if needed + Matrix23 Dunit; // calculated by FromPoint3 if needed Unit3 pu = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0); - if (Dpose) - *Dpose = Dunit * Dtf_pose; //2x3 * 3x6 = 2x6 - if (Dpoint) - *Dpoint = Dunit * Dtf_point; //2x3 * 3x3 = 2x3 + if (Dpose) *Dpose = Dunit * Dtf_pose; // 2x3 * 3x6 = 2x6 + if (Dpoint) *Dpoint = Dunit * Dtf_point; // 2x3 * 3x3 = 2x3 return pu; } /* ************************************************************************* */ Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose, - OptionalJacobian<2, 2> Dpoint) const { - + OptionalJacobian<2, 2> Dpoint) const { Matrix23 Dtf_rot; - Matrix2 Dtf_point; // calculated by transformTo if needed - const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0, Dpoint ? &Dtf_point : 0); + Matrix2 Dtf_point; // calculated by transformTo if needed + const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0, + Dpoint ? &Dtf_point : 0); if (Dpose) - *Dpose << Dtf_rot, Matrix::Zero(2,3); //2x6 (translation part is zero) - if (Dpoint) - *Dpoint = Dtf_point; //2x2 + *Dpose << Dtf_rot, Matrix::Zero(2, 3); // 2x6 (translation part is zero) + if (Dpoint) *Dpoint = Dtf_point; // 2x2 return pu; } @@ -87,7 +81,8 @@ Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const { /* ************************************************************************* */ Unit3 SphericalCamera::project(const Point3& point, - OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const { + OptionalJacobian<2, 6> Dcamera, + OptionalJacobian<2, 3> Dpoint) const { return project2(point, Dcamera, Dpoint); } @@ -102,10 +97,8 @@ Vector2 SphericalCamera::reprojectionError( Matrix22 H_error; Unit3 projected = project2(point, H_project_pose, H_project_point); Vector2 error = measured.errorVector(projected, boost::none, H_error); - if (Dpose) - *Dpose = H_error * H_project_pose; - if (Dpoint) - *Dpoint = H_error * H_project_point; + if (Dpose) *Dpose = H_error * H_project_pose; + if (Dpoint) *Dpoint = H_error * H_project_point; return error; } else { return measured.errorVector(project2(point, Dpose, Dpoint)); @@ -113,4 +106,4 @@ Vector2 SphericalCamera::reprojectionError( } /* ************************************************************************* */ -} +} // namespace gtsam diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index 15d5128bc..4e4e9db61 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -18,13 +18,14 @@ #pragma once -#include -#include -#include -#include #include #include +#include #include +#include +#include +#include + #include namespace gtsam { @@ -32,11 +33,11 @@ namespace gtsam { class GTSAM_EXPORT EmptyCal { public: enum { dimension = 0 }; - EmptyCal(){} + EmptyCal() {} virtual ~EmptyCal() = default; using shared_ptr = boost::shared_ptr; void print(const std::string& s) const { - std::cout << "empty calibration: " << s << std::endl; + std::cout << "empty calibration: " << s << std::endl; } }; @@ -46,56 +47,41 @@ class GTSAM_EXPORT EmptyCal { * \nosubgrouping */ class GTSAM_EXPORT SphericalCamera { - public: - - enum { - dimension = 6 - }; + enum { dimension = 6 }; typedef Unit3 Measurement; typedef std::vector MeasurementVector; typedef EmptyCal CalibrationType; private: - Pose3 pose_; ///< 3D pose of camera protected: - EmptyCal::shared_ptr emptyCal_; public: - /// @} /// @name Standard Constructors /// @{ /// Default constructor SphericalCamera() - : pose_(Pose3::identity()), - emptyCal_(boost::make_shared()) { - } + : pose_(Pose3::identity()), emptyCal_(boost::make_shared()) {} /// Constructor with pose explicit SphericalCamera(const Pose3& pose) - : pose_(pose), - emptyCal_(boost::make_shared()) { - } + : pose_(pose), emptyCal_(boost::make_shared()) {} /// Constructor with empty intrinsics (needed for smart factors) explicit SphericalCamera(const Pose3& pose, const boost::shared_ptr& cal) - : pose_(pose), - emptyCal_(boost::make_shared()) { - } + : pose_(pose), emptyCal_(boost::make_shared()) {} /// @} /// @name Advanced Constructors /// @{ - explicit SphericalCamera(const Vector& v) - : pose_(Pose3::Expmap(v)) { - } + explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {} /// Default destructor virtual ~SphericalCamera() = default; @@ -106,16 +92,14 @@ class GTSAM_EXPORT SphericalCamera { } /// return calibration - const EmptyCal& calibration() const { - return *emptyCal_; - } + const EmptyCal& calibration() const { return *emptyCal_; } /// @} /// @name Testable /// @{ /// assert equality up to a tolerance - bool equals(const SphericalCamera &camera, double tol = 1e-9) const; + bool equals(const SphericalCamera& camera, double tol = 1e-9) const; /// print virtual void print(const std::string& s = "SphericalCamera") const; @@ -125,19 +109,13 @@ class GTSAM_EXPORT SphericalCamera { /// @{ /// return pose, constant version - const Pose3& pose() const { - return pose_; - } + const Pose3& pose() const { return pose_; } /// get rotation - const Rot3& rotation() const { - return pose_.rotation(); - } + const Rot3& rotation() const { return pose_.rotation(); } /// get translation - const Point3& translation() const { - return pose_.translation(); - } + const Point3& translation() const { return pose_.translation(); } // /// return pose, with derivative // const Pose3& getPose(OptionalJacobian<6, 6> H) const; @@ -200,7 +178,8 @@ class GTSAM_EXPORT SphericalCamera { /// for Canonical static SphericalCamera identity() { - return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid + return SphericalCamera( + Pose3::identity()); // assumes that the default constructor is valid } /// for Linear Triangulation @@ -210,36 +189,29 @@ class GTSAM_EXPORT SphericalCamera { /// for Nonlinear Triangulation Vector defaultErrorWhenTriangulatingBehindCamera() const { - return Eigen::Matrix::dimension,1>::Constant(0.0); + return Eigen::Matrix::dimension, 1>::Constant(0.0); } /// @deprecated - size_t dim() const { - return 6; - } + size_t dim() const { return 6; } - /// @deprecated - static size_t Dim() { - return 6; - } + /// @deprecated + static size_t Dim() { return 6; } private: - /** Serialization function */ friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_NVP(pose_); + template + void serialize(Archive& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_NVP(pose_); } }; // end of class SphericalCamera -template<> -struct traits : public internal::LieGroup { -}; +template <> +struct traits : public internal::LieGroup {}; -template<> -struct traits : public internal::LieGroup { -}; +template <> +struct traits : public internal::LieGroup {}; -} +} // namespace gtsam diff --git a/gtsam/geometry/tests/testSphericalCamera.cpp b/gtsam/geometry/tests/testSphericalCamera.cpp index 0e5e3d9bf..4bc851f35 100644 --- a/gtsam/geometry/tests/testSphericalCamera.cpp +++ b/gtsam/geometry/tests/testSphericalCamera.cpp @@ -16,11 +16,10 @@ * @author Luca Carlone */ -#include +#include #include #include - -#include +#include #include #include @@ -31,64 +30,65 @@ using namespace gtsam; typedef SphericalCamera Camera; -//static const Cal3_S2 K(625, 625, 0, 0, 0); +// static const Cal3_S2 K(625, 625, 0, 0, 0); // -static const Pose3 pose(Rot3(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); // static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); static const Camera camera1(pose1); -static const Point3 point1(-0.08,-0.08, 0.0); +static const Point3 point1(-0.08, -0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); -static const Point3 point3( 0.08, 0.08, 0.0); -static const Point3 point4( 0.08,-0.08, 0.0); +static const Point3 point3(0.08, 0.08, 0.0); +static const Point3 point4(0.08, -0.08, 0.0); // manually computed in matlab -static const Unit3 bearing1(-0.156054862928174, 0.156054862928174, 0.975342893301088); -static const Unit3 bearing2(-0.156054862928174,-0.156054862928174,0.975342893301088); -static const Unit3 bearing3(0.156054862928174,-0.156054862928174,0.975342893301088); -static const Unit3 bearing4(0.156054862928174, 0.156054862928174, 0.975342893301088); +static const Unit3 bearing1(-0.156054862928174, 0.156054862928174, + 0.975342893301088); +static const Unit3 bearing2(-0.156054862928174, -0.156054862928174, + 0.975342893301088); +static const Unit3 bearing3(0.156054862928174, -0.156054862928174, + 0.975342893301088); +static const Unit3 bearing4(0.156054862928174, 0.156054862928174, + 0.975342893301088); static double depth = 0.512640224719052; /* ************************************************************************* */ -TEST( SphericalCamera, constructor) -{ - EXPECT(assert_equal( pose, camera.pose())); +TEST(SphericalCamera, constructor) { + EXPECT(assert_equal(pose, camera.pose())); } /* ************************************************************************* */ -TEST( SphericalCamera, project) -{ +TEST(SphericalCamera, project) { // expected from manual calculation in Matlab - EXPECT(assert_equal( camera.project(point1), bearing1 )); - EXPECT(assert_equal( camera.project(point2), bearing2 )); - EXPECT(assert_equal( camera.project(point3), bearing3 )); - EXPECT(assert_equal( camera.project(point4), bearing4 )); + EXPECT(assert_equal(camera.project(point1), bearing1)); + EXPECT(assert_equal(camera.project(point2), bearing2)); + EXPECT(assert_equal(camera.project(point3), bearing3)); + EXPECT(assert_equal(camera.project(point4), bearing4)); } /* ************************************************************************* */ -TEST( SphericalCamera, backproject) -{ - EXPECT(assert_equal( camera.backproject(bearing1, depth), point1)); - EXPECT(assert_equal( camera.backproject(bearing2, depth), point2)); - EXPECT(assert_equal( camera.backproject(bearing3, depth), point3)); - EXPECT(assert_equal( camera.backproject(bearing4, depth), point4)); +TEST(SphericalCamera, backproject) { + EXPECT(assert_equal(camera.backproject(bearing1, depth), point1)); + EXPECT(assert_equal(camera.backproject(bearing2, depth), point2)); + EXPECT(assert_equal(camera.backproject(bearing3, depth), point3)); + EXPECT(assert_equal(camera.backproject(bearing4, depth), point4)); } /* ************************************************************************* */ -TEST( SphericalCamera, backproject2) -{ - Point3 origin(0,0,0); - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down +TEST(SphericalCamera, backproject2) { + Point3 origin(0, 0, 0); + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin)); - Point3 actual = camera.backproject(Unit3(0,0,1), 1.); + Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); EXPECT(assert_equal(expected, actual)); - EXPECT(assert_equal(Unit3(0,0,1), x.first)); + EXPECT(assert_equal(Unit3(0, 0, 1), x.first)); EXPECT(x.second); } @@ -98,45 +98,45 @@ static Unit3 project3(const Pose3& pose, const Point3& point) { } /* ************************************************************************* */ -TEST( SphericalCamera, Dproject) -{ +TEST(SphericalCamera, Dproject) { Matrix Dpose, Dpoint; Unit3 result = camera.project(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project3, pose, point1); + Matrix numerical_pose = numericalDerivative21(project3, pose, point1); Matrix numerical_point = numericalDerivative22(project3, pose, point1); EXPECT(assert_equal(bearing1, result)); - EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); } /* ************************************************************************* */ -static Vector2 reprojectionError2(const Pose3& pose, const Point3& point, const Unit3& measured) { - return Camera(pose).reprojectionError(point,measured); +static Vector2 reprojectionError2(const Pose3& pose, const Point3& point, + const Unit3& measured) { + return Camera(pose).reprojectionError(point, measured); } /* ************************************************************************* */ -TEST( SphericalCamera, reprojectionError) { +TEST(SphericalCamera, reprojectionError) { Matrix Dpose, Dpoint; Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose, - point1, bearing1); - Matrix numerical_point = numericalDerivative32(reprojectionError2, pose, - point1, bearing1); + Matrix numerical_pose = + numericalDerivative31(reprojectionError2, pose, point1, bearing1); + Matrix numerical_point = + numericalDerivative32(reprojectionError2, pose, point1, bearing1); EXPECT(assert_equal(Vector2(0.0, 0.0), result)); EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); } /* ************************************************************************* */ -TEST( SphericalCamera, reprojectionError_noisy) { +TEST(SphericalCamera, reprojectionError_noisy) { Matrix Dpose, Dpoint; Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05)); - Vector2 result = camera.reprojectionError(point1, bearing_noisy, Dpose, - Dpoint); - Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose, - point1, bearing_noisy); - Matrix numerical_point = numericalDerivative32(reprojectionError2, pose, - point1, bearing_noisy); + Vector2 result = + camera.reprojectionError(point1, bearing_noisy, Dpose, Dpoint); + Matrix numerical_pose = + numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy); + Matrix numerical_point = + numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy); EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5)); EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); EXPECT(assert_equal(numerical_point, Dpoint, 1e-7)); @@ -144,20 +144,20 @@ TEST( SphericalCamera, reprojectionError_noisy) { /* ************************************************************************* */ // Add a test with more arbitrary rotation -TEST( SphericalCamera, Dproject2) -{ +TEST(SphericalCamera, Dproject2) { static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project3, pose1, point1); + Matrix numerical_pose = numericalDerivative21(project3, pose1, point1); Matrix numerical_point = numericalDerivative22(project3, pose1, point1); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - - diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 327da64de..78619a90e 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -17,17 +17,16 @@ * @author Luca Carlone */ -#include +#include +#include +#include #include #include #include -#include -#include -#include -#include +#include #include -#include - +#include +#include #include #include @@ -38,7 +37,7 @@ using namespace boost::assign; // Some common constants -static const boost::shared_ptr sharedCal = // +static const boost::shared_ptr sharedCal = // boost::make_shared(1500, 1200, 0, 640, 480); // Looking along X-axis, 1 meter above ground plane (x-y) @@ -59,8 +58,7 @@ Point2 z2 = camera2.project(landmark); //****************************************************************************** // Simple test with a well-behaved two camera situation -TEST( triangulation, twoPoses) { - +TEST(triangulation, twoPoses) { vector poses; Point2Vector measurements; @@ -71,36 +69,36 @@ TEST( triangulation, twoPoses) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + boost::optional actual1 = // triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + boost::optional actual2 = // triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *actual2, 1e-7)); - // 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // 3. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + boost::optional actual3 = // triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + boost::optional actual4 = // triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } //****************************************************************************** // Similar, but now with Bundler calibration -TEST( triangulation, twoPosesBundler) { - - boost::shared_ptr bundlerCal = // +TEST(triangulation, twoPosesBundler) { + boost::shared_ptr bundlerCal = // boost::make_shared(1500, 0, 0, 640, 480); PinholeCamera camera1(pose1, *bundlerCal); PinholeCamera camera2(pose2, *bundlerCal); @@ -118,7 +116,7 @@ TEST( triangulation, twoPosesBundler) { bool optimize = true; double rank_tol = 1e-9; - boost::optional actual = // + boost::optional actual = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(landmark, *actual, 1e-7)); @@ -126,28 +124,29 @@ TEST( triangulation, twoPosesBundler) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + boost::optional actual2 = // triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); } //****************************************************************************** -TEST( triangulation, fourPoses) { +TEST(triangulation, fourPoses) { vector poses; Point2Vector measurements; poses += pose1, pose2; measurements += z1, z2; - boost::optional actual = triangulatePoint3(poses, sharedCal, - measurements); + boost::optional actual = + triangulatePoint3(poses, sharedCal, measurements); EXPECT(assert_equal(landmark, *actual, 1e-2)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // 2. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + boost::optional actual2 = // triangulatePoint3(poses, sharedCal, measurements); EXPECT(assert_equal(landmark, *actual2, 1e-2)); @@ -159,13 +158,13 @@ TEST( triangulation, fourPoses) { poses += pose3; measurements += z3 + Point2(0.1, -0.1); - boost::optional triangulated_3cameras = // + boost::optional triangulated_3cameras = // triangulatePoint3(poses, sharedCal, measurements); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = triangulatePoint3(poses, - sharedCal, measurements, 1e-9, true); + boost::optional triangulated_3cameras_opt = + triangulatePoint3(poses, sharedCal, measurements, 1e-9, true); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way @@ -179,12 +178,12 @@ TEST( triangulation, fourPoses) { measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), - TriangulationCheiralityException); + TriangulationCheiralityException); #endif } //****************************************************************************** -TEST( triangulation, fourPoses_distinct_Ks) { +TEST(triangulation, fourPoses_distinct_Ks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) PinholeCamera camera1(pose1, K1); @@ -197,21 +196,22 @@ TEST( triangulation, fourPoses_distinct_Ks) { Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet > cameras; + CameraSet> cameras; Point2Vector measurements; cameras += camera1, camera2; measurements += z1, z2; - boost::optional actual = // + boost::optional actual = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(landmark, *actual, 1e-2)); - // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) + // 2. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + boost::optional actual2 = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(landmark, *actual2, 1e-2)); @@ -224,13 +224,13 @@ TEST( triangulation, fourPoses_distinct_Ks) { cameras += camera3; measurements += z3 + Point2(0.1, -0.1); - boost::optional triangulated_3cameras = // + boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = triangulatePoint3(cameras, - measurements, 1e-9, true); + boost::optional triangulated_3cameras_opt = + triangulatePoint3(cameras, measurements, 1e-9, true); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); // 4. Test failure: Add a 4th camera facing the wrong way @@ -244,12 +244,12 @@ TEST( triangulation, fourPoses_distinct_Ks) { cameras += camera4; measurements += Point2(400, 400); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), - TriangulationCheiralityException); + TriangulationCheiralityException); #endif } //****************************************************************************** -TEST( triangulation, outliersAndFarLandmarks) { +TEST(triangulation, outliersAndFarLandmarks) { Cal3_S2 K1(1500, 1200, 0, 640, 480); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) PinholeCamera camera1(pose1, K1); @@ -262,24 +262,29 @@ TEST( triangulation, outliersAndFarLandmarks) { Point2 z1 = camera1.project(landmark); Point2 z2 = camera2.project(landmark); - CameraSet > cameras; + CameraSet> cameras; Point2Vector measurements; cameras += camera1, camera2; measurements += z1, z2; - double landmarkDistanceThreshold = 10; // landmark is closer than that - TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold - TriangulationResult actual = triangulateSafe(cameras,measurements,params); + double landmarkDistanceThreshold = 10; // landmark is closer than that + TriangulationParameters params( + 1.0, false, landmarkDistanceThreshold); // all default except + // landmarkDistanceThreshold + TriangulationResult actual = triangulateSafe(cameras, measurements, params); EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(actual.valid()); - landmarkDistanceThreshold = 4; // landmark is farther than that - TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold - actual = triangulateSafe(cameras,measurements,params2); + landmarkDistanceThreshold = 4; // landmark is farther than that + TriangulationParameters params2( + 1.0, false, landmarkDistanceThreshold); // all default except + // landmarkDistanceThreshold + actual = triangulateSafe(cameras, measurements, params2); EXPECT(actual.farPoint()); - // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) + // 3. Add a slightly rotated third camera above with a wrong measurement + // (OUTLIER) Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Cal3_S2 K3(700, 500, 0, 640, 480); PinholeCamera camera3(pose3, K3); @@ -288,21 +293,23 @@ TEST( triangulation, outliersAndFarLandmarks) { cameras += camera3; measurements += z3 + Point2(10, -10); - landmarkDistanceThreshold = 10; // landmark is closer than that - double outlierThreshold = 100; // loose, the outlier is going to pass - TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold); - actual = triangulateSafe(cameras,measurements,params3); + landmarkDistanceThreshold = 10; // landmark is closer than that + double outlierThreshold = 100; // loose, the outlier is going to pass + TriangulationParameters params3(1.0, false, landmarkDistanceThreshold, + outlierThreshold); + actual = triangulateSafe(cameras, measurements, params3); EXPECT(actual.valid()); // now set stricter threshold for outlier rejection - outlierThreshold = 5; // tighter, the outlier is not going to pass - TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold); - actual = triangulateSafe(cameras,measurements,params4); + outlierThreshold = 5; // tighter, the outlier is not going to pass + TriangulationParameters params4(1.0, false, landmarkDistanceThreshold, + outlierThreshold); + actual = triangulateSafe(cameras, measurements, params4); EXPECT(actual.outlier()); } //****************************************************************************** -TEST( triangulation, twoIdenticalPoses) { +TEST(triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) PinholeCamera camera1(pose1, *sharedCal); @@ -316,11 +323,11 @@ TEST( triangulation, twoIdenticalPoses) { measurements += z1, z1; CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), - TriangulationUnderconstrainedException); + TriangulationUnderconstrainedException); } //****************************************************************************** -TEST( triangulation, onePose) { +TEST(triangulation, onePose) { // we expect this test to fail with a TriangulationUnderconstrainedException // because there's only one camera observation @@ -328,28 +335,26 @@ TEST( triangulation, onePose) { Point2Vector measurements; poses += Pose3(); - measurements += Point2(0,0); + measurements += Point2(0, 0); CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), - TriangulationUnderconstrainedException); + TriangulationUnderconstrainedException); } //****************************************************************************** -TEST( triangulation, StereotriangulateNonlinear ) { - - auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); +TEST(triangulation, StereotriangulateNonlinear) { + auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, + 508.835, 0.0699612); // two camera poses m1, m2 Matrix4 m1, m2; - m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, - 0.592783835, -0.77156583, 0.230856632, 66.2186159, - 0.116517574, -0.201470143, -0.9725393, -4.28382528, - 0, 0, 0, 1; + m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835, + -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143, + -0.9725393, -4.28382528, 0, 0, 0, 1; - m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, - -0.29277519, 0.947083213, 0.131587097, 65.843136, - -0.0206094928, 0.131334858, -0.991123524, -4.3525033, - 0, 0, 0, 1; + m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, -0.29277519, + 0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858, + -0.991123524, -4.3525033, 0, 0, 0, 1; typedef CameraSet Cameras; Cameras cameras; @@ -360,18 +365,19 @@ TEST( triangulation, StereotriangulateNonlinear ) { measurements += StereoPoint2(226.936, 175.212, 424.469); measurements += StereoPoint2(339.571, 285.547, 669.973); - Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 + Point3 initial = + Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 Point3 actual = triangulateNonlinear(cameras, measurements, initial); - Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187 + Point3 expected(46.0484569, 66.4710686, + -6.55046613); // error: 0.763510644187 EXPECT(assert_equal(expected, actual, 1e-4)); - // regular stereo factor comparison - expect very similar result as above { - typedef GenericStereoFactor StereoFactor; + typedef GenericStereoFactor StereoFactor; Values values; values.insert(Symbol('x', 1), Pose3(m1)); @@ -380,17 +386,19 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.emplace_shared(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); - graph.emplace_shared(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); + graph.emplace_shared(measurements[0], unit, Symbol('x', 1), + Symbol('l', 1), stereoK); + graph.emplace_shared(measurements[1], unit, Symbol('x', 2), + Symbol('l', 1), stereoK); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); - graph.addPrior(Symbol('x',1), Pose3(m1), posePrior); - graph.addPrior(Symbol('x',2), Pose3(m2), posePrior); + graph.addPrior(Symbol('x', 1), Pose3(m1), posePrior); + graph.addPrior(Symbol('x', 2), Pose3(m2), posePrior); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); - EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + EXPECT(assert_equal(expected, result.at(Symbol('l', 1)), 1e-4)); } // use Triangulation Factor directly - expect same result as above @@ -401,13 +409,15 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - graph.emplace_shared >(cameras[0], measurements[0], unit, Symbol('l',1)); - graph.emplace_shared >(cameras[1], measurements[1], unit, Symbol('l',1)); + graph.emplace_shared>( + cameras[0], measurements[0], unit, Symbol('l', 1)); + graph.emplace_shared>( + cameras[1], measurements[1], unit, Symbol('l', 1)); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); - EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + EXPECT(assert_equal(expected, result.at(Symbol('l', 1)), 1e-4)); } // use ExpressionFactor - expect same result as above @@ -418,11 +428,13 @@ TEST( triangulation, StereotriangulateNonlinear ) { NonlinearFactorGraph graph; static SharedNoiseModel unit(noiseModel::Unit::Create(3)); - Expression point_(Symbol('l',1)); + Expression point_(Symbol('l', 1)); Expression camera0_(cameras[0]); Expression camera1_(cameras[1]); - Expression project0_(camera0_, &StereoCamera::project2, point_); - Expression project1_(camera1_, &StereoCamera::project2, point_); + Expression project0_(camera0_, &StereoCamera::project2, + point_); + Expression project1_(camera1_, &StereoCamera::project2, + point_); graph.addExpressionFactor(unit, measurements[0], project0_); graph.addExpressionFactor(unit, measurements[1], project1_); @@ -430,14 +442,13 @@ TEST( triangulation, StereotriangulateNonlinear ) { LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); - EXPECT(assert_equal(expected, result.at(Symbol('l',1)), 1e-4)); + EXPECT(assert_equal(expected, result.at(Symbol('l', 1)), 1e-4)); } } //****************************************************************************** // Simple test with a well-behaved two camera situation -TEST( triangulation, twoPoses_sphericalCamera) { - +TEST(triangulation, twoPoses_sphericalCamera) { vector poses; std::vector measurements; @@ -457,8 +468,9 @@ TEST( triangulation, twoPoses_sphericalCamera) { double rank_tol = 1e-9; // 1. Test linear triangulation via DLT - std::vector> projection_matrices = - getCameraProjectionMatrices(cameras); + std::vector> + projection_matrices = + getCameraProjectionMatrices(cameras); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); EXPECT(assert_equal(landmark, point, 1e-7)); @@ -468,48 +480,60 @@ TEST( triangulation, twoPoses_sphericalCamera) { // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; - boost::optional actual1 = // - triangulatePoint3(cameras, measurements, rank_tol, optimize); + boost::optional actual1 = // + triangulatePoint3(cameras, measurements, rank_tol, + optimize); EXPECT(assert_equal(landmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; - boost::optional actual2 = // - triangulatePoint3(cameras, measurements, rank_tol, optimize); + boost::optional actual2 = // + triangulatePoint3(cameras, measurements, rank_tol, + optimize); EXPECT(assert_equal(landmark, *actual2, 1e-7)); - // 5. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) - measurements.at(0) = u1.retract(Vector2(0.01, 0.05)); //note: perturbation smaller for Unit3 + // 5. Add some noise and try again: result should be ~ (4.995, + // 0.499167, 1.19814) + measurements.at(0) = + u1.retract(Vector2(0.01, 0.05)); // note: perturbation smaller for Unit3 measurements.at(1) = u2.retract(Vector2(-0.02, 0.03)); optimize = false; - boost::optional actual3 = // - triangulatePoint3(cameras, measurements, rank_tol, optimize); + boost::optional actual3 = // + triangulatePoint3(cameras, measurements, rank_tol, + optimize); EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3)); // 6. Now with optimization on optimize = true; - boost::optional actual4 = // - triangulatePoint3(cameras, measurements, rank_tol, optimize); + boost::optional actual4 = // + triangulatePoint3(cameras, measurements, rank_tol, + optimize); EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3)); } //****************************************************************************** -TEST( triangulation, twoPoses_sphericalCamera_extremeFOV) { - +TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { vector poses; std::vector measurements; // Project landmark into two cameras and triangulate - Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame - Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(2.0,0.0,0.0)); // 2m in front of poseA - Point3 landmarkL(1.0,-1.0,0.0);// 1m to the right of both cameras, in front of poseA, behind poseB + Pose3 poseA = Pose3( + Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame + Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(2.0, 0.0, 0.0)); // 2m in front of poseA + Point3 landmarkL( + 1.0, -1.0, + 0.0); // 1m to the right of both cameras, in front of poseA, behind poseB SphericalCamera cam1(poseA); SphericalCamera cam2(poseB); Unit3 u1 = cam1.project(landmarkL); Unit3 u2 = cam2.project(landmarkL); - EXPECT(assert_equal(Unit3(Point3(1.0,0.0,1.0)), u1, 1e-7)); // in front and to the right of PoseA - EXPECT(assert_equal(Unit3(Point3(1.0,0.0,-1.0)), u2, 1e-7));// behind and to the right of PoseB + EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, 1.0)), u1, + 1e-7)); // in front and to the right of PoseA + EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, + 1e-7)); // behind and to the right of PoseB poses += pose1, pose2; measurements += u1, u2; @@ -521,57 +545,65 @@ TEST( triangulation, twoPoses_sphericalCamera_extremeFOV) { double rank_tol = 1e-9; { - // 1. Test simple DLT, when 1 point is behind spherical camera - bool optimize = false; + // 1. Test simple DLT, when 1 point is behind spherical camera + bool optimize = false; #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION( - triangulatePoint3(cameras, measurements, rank_tol, - optimize), TriangulationCheiralityException); -#else // otherwise project should not throw the exception - boost::optional actual1 = // - triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); + CHECK_EXCEPTION(triangulatePoint3(cameras, measurements, + rank_tol, optimize), + TriangulationCheiralityException); +#else // otherwise project should not throw the exception + boost::optional actual1 = // + triangulatePoint3(cameras, measurements, rank_tol, + optimize); + EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); #endif } { - // 2. test with optimization on, same answer - bool optimize = true; + // 2. test with optimization on, same answer + bool optimize = true; #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - CHECK_EXCEPTION( - triangulatePoint3(cameras, measurements, rank_tol, - optimize), TriangulationCheiralityException); -#else // otherwise project should not throw the exception - boost::optional actual1 = // - triangulatePoint3(cameras, measurements, rank_tol, optimize); - EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); + CHECK_EXCEPTION(triangulatePoint3(cameras, measurements, + rank_tol, optimize), + TriangulationCheiralityException); +#else // otherwise project should not throw the exception + boost::optional actual1 = // + triangulatePoint3(cameras, measurements, rank_tol, + optimize); + EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); #endif } } //****************************************************************************** -TEST( triangulation, reprojectionError_cameraComparison) { - - Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame - Point3 landmarkL(5.0,0.0,0.0);// 1m in front of poseA +TEST(triangulation, reprojectionError_cameraComparison) { + Pose3 poseA = Pose3( + Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame + Point3 landmarkL(5.0, 0.0, 0.0); // 1m in front of poseA SphericalCamera sphericalCamera(poseA); Unit3 u = sphericalCamera.project(landmarkL); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(60, 640, 480)); - PinholePose pinholeCamera(poseA,sharedK); + PinholePose pinholeCamera(poseA, sharedK); Vector2 px = pinholeCamera.project(landmarkL); // add perturbation and compare error in both cameras - Vector2 px_noise(1.0,1.0); // 1px perturbation vertically and horizontally + Vector2 px_noise(1.0, 1.0); // 1px perturbation vertically and horizontally Vector2 measured_px = px + px_noise; Vector2 measured_px_calibrated = sharedK->calibrate(measured_px); - Unit3 measured_u = Unit3(measured_px_calibrated[0],measured_px_calibrated[1],1.0); + Unit3 measured_u = + Unit3(measured_px_calibrated[0], measured_px_calibrated[1], 1.0); - Vector2 actualErrorPinhole = pinholeCamera.reprojectionError(landmarkL,measured_px); - Vector2 expectedErrorPinhole = Vector2(-px_noise[0],-px_noise[1]); - EXPECT(assert_equal(expectedErrorPinhole, actualErrorPinhole, 1e-7)); //- sign due to definition of error + Vector2 actualErrorPinhole = + pinholeCamera.reprojectionError(landmarkL, measured_px); + Vector2 expectedErrorPinhole = Vector2(-px_noise[0], -px_noise[1]); + EXPECT(assert_equal(expectedErrorPinhole, actualErrorPinhole, + 1e-7)); //- sign due to definition of error - Vector2 actualErrorSpherical = sphericalCamera.reprojectionError(landmarkL,measured_u); - Vector2 expectedErrorSpherical = Vector2(px_noise[0]/sharedK->fx(),px_noise[1]/sharedK->fy()); + Vector2 actualErrorSpherical = + sphericalCamera.reprojectionError(landmarkL, measured_u); + Vector2 expectedErrorSpherical = + Vector2(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy()); EXPECT(assert_equal(expectedErrorSpherical, actualErrorSpherical, 1e-7)); } diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 1875c911e..310dbe79e 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -17,12 +17,13 @@ */ #pragma once -#include -#include -#include -#include #include +#include #include +#include +#include +#include + #include "../SmartProjectionRigFactor.h" using namespace std; @@ -45,7 +46,7 @@ Pose3 pose_above = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); // Create a noise unit2 for the pixel error static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); -static double fov = 60; // degrees +static double fov = 60; // degrees static size_t w = 640, h = 480; /* ************************************************************************* */ @@ -64,7 +65,7 @@ Camera cam2(pose_right, K2); Camera cam3(pose_above, K2); typedef GeneralSFMFactor SFMFactor; SmartProjectionParams params; -} +} // namespace vanilla /* ************************************************************************* */ // default Cal3_S2 poses @@ -79,7 +80,7 @@ Camera level_camera_right(pose_right, sharedK); Camera cam1(level_pose, sharedK); Camera cam2(pose_right, sharedK); Camera cam3(pose_above, sharedK); -} +} // namespace vanillaPose /* ************************************************************************* */ // default Cal3_S2 poses @@ -94,7 +95,7 @@ Camera level_camera_right(pose_right, sharedK2); Camera cam1(level_pose, sharedK2); Camera cam2(pose_right, sharedK2); Camera cam3(pose_above, sharedK2); -} +} // namespace vanillaPose2 /* *************************************************************************/ // Cal3Bundler cameras @@ -112,7 +113,7 @@ Camera cam1(level_pose, K); Camera cam2(pose_right, K); Camera cam3(pose_above, K); typedef GeneralSFMFactor SFMFactor; -} +} // namespace bundler /* *************************************************************************/ // Cal3Bundler poses @@ -121,14 +122,15 @@ typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartRigFactor; -static boost::shared_ptr sharedBundlerK( - new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); +static boost::shared_ptr sharedBundlerK(new Cal3Bundler(500, 1e-3, + 1e-3, 1000, + 2000)); Camera level_camera(level_pose, sharedBundlerK); Camera level_camera_right(pose_right, sharedBundlerK); Camera cam1(level_pose, sharedBundlerK); Camera cam2(pose_right, sharedBundlerK); Camera cam3(pose_above, sharedBundlerK); -} +} // namespace bundlerPose /* ************************************************************************* */ // sphericalCamera @@ -142,21 +144,22 @@ Camera level_camera_right(pose_right); Camera cam1(level_pose); Camera cam2(pose_right); Camera cam3(pose_above); -} +} // namespace sphericalCamera /* *************************************************************************/ -template +template CAMERA perturbCameraPose(const CAMERA& camera) { - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); return CAMERA(perturbedCameraPose, camera.calibration()); } -template -void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, - const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) { +template +void projectToMultipleCameras( + const CAMERA& cam1, const CAMERA& cam2, const CAMERA& cam3, Point3 landmark, + typename CAMERA::MeasurementVector& measurements_cam) { typename CAMERA::Measurement cam1_uv1 = cam1.project(landmark); typename CAMERA::Measurement cam2_uv1 = cam2.project(landmark); typename CAMERA::Measurement cam3_uv1 = cam3.project(landmark); @@ -166,4 +169,3 @@ void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, } /* ************************************************************************* */ - diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 7e80eb009..5bee8e4ef 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -1195,8 +1195,9 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // this factor is slightly slower (but comparable) to original // SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.05 CPU (10000 times, 0.057952 wall, 0.05 children, min: 0 max: 0) -//| -SmartPoseFactor LINEARIZE: 0.05 CPU (10000 times, 0.069647 wall, 0.05 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.05 CPU (10000 times, 0.057952 wall, 0.05 +// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.05 CPU (10000 +// times, 0.069647 wall, 0.05 children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionRigFactor, timing) { using namespace vanillaRig; @@ -1256,15 +1257,18 @@ TEST(SmartProjectionRigFactor, timing) { #endif /* *************************************************************************/ -TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { - +TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) { using namespace sphericalCamera; - Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3; + Camera::MeasurementVector measurements_lmk1, measurements_lmk2, + measurements_lmk3; // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, + measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, + measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, + measurements_lmk3); // create inputs KeyVector keys; @@ -1280,13 +1284,16 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors params.setRankTolerance(0.1); - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, keys); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, keys); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, keys); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1305,12 +1312,13 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 100), - Point3(0.2, 0.2, 0.2)); // note: larger noise! + Point3(0.2, 0.2, 0.2)); // note: larger noise! Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); DOUBLES_EQUAL(0.94148963675515274, graph.error(values), 1e-9); @@ -1324,12 +1332,13 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { #ifndef DISABLE_TIMING #include -// using spherical camera is slightly slower (but comparable) to PinholePose -//| -SmartFactorP spherical LINEARIZE: 0.01 CPU (1000 times, 0.008178 wall, 0.01 children, min: 0 max: 0) -//| -SmartFactorP pinhole LINEARIZE: 0.01 CPU (1000 times, 0.005717 wall, 0.01 children, min: 0 max: 0) +// using spherical camera is slightly slower (but comparable) to +// PinholePose +//| -SmartFactorP spherical LINEARIZE: 0.01 CPU (1000 times, 0.008178 wall, +// 0.01 children, min: 0 max: 0) | -SmartFactorP pinhole LINEARIZE: 0.01 CPU +//(1000 times, 0.005717 wall, 0.01 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionFactorP, timing_sphericalCamera ) { - +TEST(SmartProjectionFactorP, timing_sphericalCamera) { // create common data Rot3 R = Rot3::identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); @@ -1359,7 +1368,7 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) { size_t nrTests = 1000; - for(size_t i = 0; i cameraRig; // single camera in the rig cameraRig.push_back(SphericalCamera(body_P_sensorId, emptyK)); @@ -1377,13 +1386,13 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) { gttoc_(SmartFactorP_spherical_LINEARIZE); } - for(size_t i = 0; i> cameraRig; // single camera in the rig cameraRig.push_back(PinholePose(body_P_sensorId, sharedKSimple)); SmartProjectionRigFactor>::shared_ptr smartFactorP2( new SmartProjectionRigFactor>(model, cameraRig, - params)); + params)); smartFactorP2->add(measurements_lmk1[0], x1); smartFactorP2->add(measurements_lmk1[1], x1); @@ -1399,17 +1408,21 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) { #endif /* *************************************************************************/ -TEST( SmartProjectionFactorP, 2poses_rankTol ) { - Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame - Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,-0.1,0.0)); // 10cm to the right of poseA - Point3 landmarkL = Point3(5.0,0.0,0.0); // 5m in front of poseA +TEST(SmartProjectionFactorP, 2poses_rankTol) { + Pose3 poseA = Pose3( + Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame + Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0.0, -0.1, 0.0)); // 10cm to the right of poseA + Point3 landmarkL = Point3(5.0, 0.0, 0.0); // 5m in front of poseA // triangulate from a stereo with 10cm baseline, assuming standard calibration - {// default rankTol = 1 gives a valid point (compare with calibrated and spherical cameras below) - using namespace vanillaPose; // pinhole with Cal3_S2 calibration + { // default rankTol = 1 gives a valid point (compare with calibrated and + // spherical cameras below) + using namespace vanillaPose; // pinhole with Cal3_S2 calibration - Camera cam1(poseA,sharedK); - Camera cam2(poseB,sharedK); + Camera cam1(poseA, sharedK); + Camera cam2(poseB, sharedK); SmartProjectionParams params( gtsam::HESSIAN, @@ -1419,7 +1432,8 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) { CameraSet> cameraRig; // single camera in the rig cameraRig.push_back(PinholePose(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(cam1.project(landmarkL), x1); smartFactor1->add(cam2.project(landmarkL), x2); @@ -1433,16 +1447,19 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) { // get point TriangulationResult point = smartFactor1->point(); - EXPECT(point.valid()); // valid triangulation + EXPECT(point.valid()); // valid triangulation EXPECT(assert_equal(landmarkL, *point, 1e-7)); } - // triangulate from a stereo with 10cm baseline, assuming canonical calibration - {// default rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a point 5m away and 10cm baseline - using namespace vanillaPose; // pinhole with Cal3_S2 calibration - static Cal3_S2::shared_ptr canonicalK(new Cal3_S2(1.0,1.0,0.0,0.0,0.0)); //canonical camera + // triangulate from a stereo with 10cm baseline, assuming canonical + // calibration + { // default rankTol = 1 or 0.1 gives a degenerate point, which is + // undesirable for a point 5m away and 10cm baseline + using namespace vanillaPose; // pinhole with Cal3_S2 calibration + static Cal3_S2::shared_ptr canonicalK( + new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); // canonical camera - Camera cam1(poseA,canonicalK); - Camera cam2(poseB,canonicalK); + Camera cam1(poseA, canonicalK); + Camera cam2(poseB, canonicalK); SmartProjectionParams params( gtsam::HESSIAN, @@ -1452,7 +1469,8 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) { CameraSet> cameraRig; // single camera in the rig cameraRig.push_back(PinholePose(Pose3::identity(), canonicalK)); - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(cam1.project(landmarkL), x1); smartFactor1->add(cam2.project(landmarkL), x2); @@ -1466,15 +1484,18 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) { // get point TriangulationResult point = smartFactor1->point(); - EXPECT(point.degenerate()); // valid triangulation + EXPECT(point.degenerate()); // valid triangulation } - // triangulate from a stereo with 10cm baseline, assuming canonical calibration - {// smaller rankTol = 0.01 gives a valid point (compare with calibrated and spherical cameras below) - using namespace vanillaPose; // pinhole with Cal3_S2 calibration - static Cal3_S2::shared_ptr canonicalK(new Cal3_S2(1.0,1.0,0.0,0.0,0.0)); //canonical camera + // triangulate from a stereo with 10cm baseline, assuming canonical + // calibration + { // smaller rankTol = 0.01 gives a valid point (compare with calibrated and + // spherical cameras below) + using namespace vanillaPose; // pinhole with Cal3_S2 calibration + static Cal3_S2::shared_ptr canonicalK( + new Cal3_S2(1.0, 1.0, 0.0, 0.0, 0.0)); // canonical camera - Camera cam1(poseA,canonicalK); - Camera cam2(poseB,canonicalK); + Camera cam1(poseA, canonicalK); + Camera cam2(poseB, canonicalK); SmartProjectionParams params( gtsam::HESSIAN, @@ -1484,7 +1505,8 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) { CameraSet> cameraRig; // single camera in the rig cameraRig.push_back(PinholePose(Pose3::identity(), canonicalK)); - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(cam1.project(landmarkL), x1); smartFactor1->add(cam2.project(landmarkL), x2); @@ -1498,19 +1520,22 @@ TEST( SmartProjectionFactorP, 2poses_rankTol ) { // get point TriangulationResult point = smartFactor1->point(); - EXPECT(point.valid()); // valid triangulation + EXPECT(point.valid()); // valid triangulation EXPECT(assert_equal(landmarkL, *point, 1e-7)); } } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) { +TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) { typedef SphericalCamera Camera; typedef SmartProjectionRigFactor SmartRigFactor; static EmptyCal::shared_ptr emptyK; - Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame - Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,-0.1,0.0)); // 10cm to the right of poseA - Point3 landmarkL = Point3(5.0,0.0,0.0); // 5m in front of poseA + Pose3 poseA = Pose3( + Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame + Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(0.0, -0.1, 0.0)); // 10cm to the right of poseA + Point3 landmarkL = Point3(5.0, 0.0, 0.0); // 5m in front of poseA Camera cam1(poseA); Camera cam2(poseB); @@ -1519,73 +1544,82 @@ TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) { cameraRig.push_back(SphericalCamera(Pose3::identity(), emptyK)); // TRIANGULATION TEST WITH DEFAULT RANK TOL - {// rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a point 5m away and 10cm baseline + { // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a + // point 5m away and 10cm baseline SmartProjectionParams params( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors params.setRankTolerance(0.1); - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params)); - smartFactor1->add(cam1.project(landmarkL), x1); - smartFactor1->add(cam2.project(landmarkL), x2); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); + smartFactor1->add(cam1.project(landmarkL), x1); + smartFactor1->add(cam2.project(landmarkL), x2); - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); - Values groundTruth; - groundTruth.insert(x1, poseA); - groundTruth.insert(x2, poseB); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // get point - TriangulationResult point = smartFactor1->point(); - EXPECT(point.degenerate()); // not enough parallax + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.degenerate()); // not enough parallax } // SAME TEST WITH SMALLER RANK TOL - {// rankTol = 0.01 gives a valid point - // By playing with this test, we can show we can triangulate also with a baseline of 5cm (even for points - // far away, >100m), but the test fails when the baseline becomes 1cm. This suggests using - // rankTol = 0.01 and setting a reasonable max landmark distance to obtain best results. - SmartProjectionParams params( + { // rankTol = 0.01 gives a valid point + // By playing with this test, we can show we can triangulate also with a + // baseline of 5cm (even for points far away, >100m), but the test fails + // when the baseline becomes 1cm. This suggests using rankTol = 0.01 and + // setting a reasonable max landmark distance to obtain best results. + SmartProjectionParams params( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors - params.setRankTolerance(0.01); + params.setRankTolerance(0.01); - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig,params)); - smartFactor1->add(cam1.project(landmarkL), x1); - smartFactor1->add(cam2.project(landmarkL), x2); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); + smartFactor1->add(cam1.project(landmarkL), x1); + smartFactor1->add(cam2.project(landmarkL), x2); - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); - Values groundTruth; - groundTruth.insert(x1, poseA); - groundTruth.insert(x2, poseB); - DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // get point - TriangulationResult point = smartFactor1->point(); - EXPECT(point.valid()); // valid triangulation - EXPECT(assert_equal(landmarkL, *point, 1e-7)); + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // valid triangulation + EXPECT(assert_equal(landmarkL, *point, 1e-7)); } } /* ************************************************************************* */ -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, +// "gtsam_noiseModel_Constrained"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, +// "gtsam_noiseModel_Diagonal"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, +// "gtsam_noiseModel_Gaussian"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, +// "gtsam_noiseModel_Isotropic"); +// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // // SERIALIZATION TEST FAILS: to be fixed -//TEST(SmartProjectionFactorP, serialize) { +// TEST(SmartProjectionFactorP, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params( // gtsam::HESSIAN, -// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors +// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig +// factors // params.setRankTolerance(rankTol); // // CameraSet> cameraRig; // single camera in the rig @@ -1598,7 +1632,7 @@ TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) { // EXPECT(equalsBinary(factor)); //} // -//TEST(SmartProjectionFactorP, serialize2) { +// TEST(SmartProjectionFactorP, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params( diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index b88f475cd..da2e6e389 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -1352,31 +1352,34 @@ namespace sphericalCameraRS { typedef SphericalCamera Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS_spherical; -Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); -Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); -Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); static EmptyCal::shared_ptr emptyK; Camera cam1(interp_pose1, emptyK); Camera cam2(interp_pose2, emptyK); Camera cam3(interp_pose3, emptyK); -} +} // namespace sphericalCameraRS /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCameras ) { - +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_sphericalCameras) { using namespace sphericalCameraRS; std::vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, + measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, + measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, + measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); @@ -1392,15 +1395,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCame cameraRig.push_back(Camera(Pose3::identity(), emptyK)); SmartFactorRS_spherical::shared_ptr smartFactor1( - new SmartFactorRS_spherical(model,cameraRig,params)); + new SmartFactorRS_spherical(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); SmartFactorRS_spherical::shared_ptr smartFactor2( - new SmartFactorRS_spherical(model,cameraRig,params)); + new SmartFactorRS_spherical(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS_spherical::shared_ptr smartFactor3( - new SmartFactorRS_spherical(model,cameraRig,params)); + new SmartFactorRS_spherical(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1418,20 +1421,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCame groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);