commit
						c7781961e6
					
				|  | @ -312,6 +312,16 @@ public: | |||
|     return range(camera.pose(), Dcamera, Dother); | ||||
|   } | ||||
| 
 | ||||
|   /// for Linear Triangulation
 | ||||
|   Matrix34 cameraProjectionMatrix() const { | ||||
|     return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4); | ||||
|   } | ||||
| 
 | ||||
|   /// for Nonlinear Triangulation
 | ||||
|   Vector defaultErrorWhenTriangulatingBehindCamera() const { | ||||
|     return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());; | ||||
|   } | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|   /** Serialization function */ | ||||
|  |  | |||
|  | @ -121,6 +121,13 @@ public: | |||
|     return _project(pw, Dpose, Dpoint, Dcal); | ||||
|   } | ||||
| 
 | ||||
|   /// project a 3D point from world coordinates into the image
 | ||||
|   Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|       OptionalJacobian<2, 3> Dpoint = boost::none, | ||||
|       OptionalJacobian<2, DimK> Dcal = boost::none) const { | ||||
|     return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured); | ||||
|   } | ||||
| 
 | ||||
|   /// project a point at infinity from world coordinates into the image
 | ||||
|   Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|       OptionalJacobian<2, 2> Dpoint = boost::none, | ||||
|  | @ -159,7 +166,6 @@ public: | |||
|     return result; | ||||
|   } | ||||
| 
 | ||||
| 
 | ||||
|   /// backproject a 2-dimensional point to a 3-dimensional point at infinity
 | ||||
|   Unit3 backprojectPointAtInfinity(const Point2& p) const { | ||||
|     const Point2 pn = calibration().calibrate(p); | ||||
|  | @ -410,6 +416,16 @@ public: | |||
|     return PinholePose(); // assumes that the default constructor is valid
 | ||||
|   } | ||||
| 
 | ||||
|   /// for Linear Triangulation
 | ||||
|   Matrix34 cameraProjectionMatrix() const { | ||||
|     Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4)); | ||||
|     return K_->K() * P; | ||||
|   } | ||||
| 
 | ||||
|   /// for Nonlinear Triangulation
 | ||||
|   Vector defaultErrorWhenTriangulatingBehindCamera() const { | ||||
|     return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());; | ||||
|   } | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
|  |  | |||
|  | @ -0,0 +1,109 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file SphericalCamera.h | ||||
|  * @brief Calibrated camera with spherical projection | ||||
|  * @date Aug 26, 2021 | ||||
|  * @author Luca Carlone | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/geometry/SphericalCamera.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 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"); } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const { | ||||
|   const Point3 pc = pose().transformTo(pw); | ||||
|   Unit3 pu = Unit3::FromPoint3(pc); | ||||
|   return make_pair(pu, pc.norm() > 1e-8); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose, | ||||
|                                 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); | ||||
| 
 | ||||
|   if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera"); | ||||
| 
 | ||||
|   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
 | ||||
|   return pu; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose, | ||||
|                                 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); | ||||
| 
 | ||||
|   if (Dpose) | ||||
|     *Dpose << Dtf_rot, Matrix::Zero(2, 3);  // 2x6 (translation part is zero)
 | ||||
|   if (Dpoint) *Dpoint = Dtf_point;          // 2x2
 | ||||
|   return pu; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const { | ||||
|   return pose().transformFrom(depth * pu); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const { | ||||
|   return pose().rotation().rotate(p); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Unit3 SphericalCamera::project(const Point3& point, | ||||
|                                OptionalJacobian<2, 6> Dcamera, | ||||
|                                OptionalJacobian<2, 3> Dpoint) const { | ||||
|   return project2(point, Dcamera, Dpoint); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector2 SphericalCamera::reprojectionError( | ||||
|     const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose, | ||||
|     OptionalJacobian<2, 3> Dpoint) const { | ||||
|   // project point
 | ||||
|   if (Dpose || Dpoint) { | ||||
|     Matrix26 H_project_pose; | ||||
|     Matrix23 H_project_point; | ||||
|     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; | ||||
|     return error; | ||||
|   } else { | ||||
|     return measured.errorVector(project2(point, Dpose, Dpoint)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| }  // namespace gtsam
 | ||||
|  | @ -0,0 +1,225 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file SphericalCamera.h | ||||
|  * @brief Calibrated camera with spherical projection | ||||
|  * @date Aug 26, 2021 | ||||
|  * @author Luca Carlone | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/Manifold.h> | ||||
| #include <gtsam/base/ThreadsafeException.h> | ||||
| #include <gtsam/base/concepts.h> | ||||
| #include <gtsam/dllexport.h> | ||||
| #include <gtsam/geometry/BearingRange.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/geometry/Unit3.h> | ||||
| 
 | ||||
| #include <boost/serialization/nvp.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /**
 | ||||
|  * Empty calibration. Only needed to play well with other cameras | ||||
|  * (e.g., when templating functions wrt cameras), since other cameras | ||||
|  * have constuctors in the form ‘camera(pose,calibration)’ | ||||
|  * @addtogroup geometry | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| class GTSAM_EXPORT EmptyCal { | ||||
|  public: | ||||
|   enum { dimension = 0 }; | ||||
|   EmptyCal() {} | ||||
|   virtual ~EmptyCal() = default; | ||||
|   using shared_ptr = boost::shared_ptr<EmptyCal>; | ||||
|   void print(const std::string& s) const { | ||||
|     std::cout << "empty calibration: " << s << std::endl; | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| /**
 | ||||
|  * A spherical camera class that has a Pose3 and measures bearing vectors. | ||||
|  * The camera has an ‘Empty’ calibration and the only 6 dof are the pose | ||||
|  * @addtogroup geometry | ||||
|  * \nosubgrouping | ||||
|  */ | ||||
| class GTSAM_EXPORT SphericalCamera { | ||||
|  public: | ||||
|   enum { dimension = 6 }; | ||||
| 
 | ||||
|   typedef Unit3 Measurement; | ||||
|   typedef std::vector<Unit3> 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<EmptyCal>()) {} | ||||
| 
 | ||||
|   /// Constructor with pose
 | ||||
|   explicit SphericalCamera(const Pose3& pose) | ||||
|       : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {} | ||||
| 
 | ||||
|   /// Constructor with empty intrinsics (needed for smart factors)
 | ||||
|   explicit SphericalCamera(const Pose3& pose, | ||||
|                            const boost::shared_ptr<EmptyCal>& cal) | ||||
|       : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {} | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Advanced Constructors
 | ||||
|   /// @{
 | ||||
|   explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {} | ||||
| 
 | ||||
|   /// Default destructor
 | ||||
|   virtual ~SphericalCamera() = default; | ||||
| 
 | ||||
|   /// return shared pointer to calibration
 | ||||
|   const boost::shared_ptr<EmptyCal>& sharedCalibration() const { | ||||
|     return emptyCal_; | ||||
|   } | ||||
| 
 | ||||
|   /// return calibration
 | ||||
|   const EmptyCal& calibration() const { return *emptyCal_; } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Testable
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// assert equality up to a tolerance
 | ||||
|   bool equals(const SphericalCamera& camera, double tol = 1e-9) const; | ||||
| 
 | ||||
|   /// print
 | ||||
|   virtual void print(const std::string& s = "SphericalCamera") const; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Standard Interface
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// return pose, constant version
 | ||||
|   const Pose3& pose() const { return pose_; } | ||||
| 
 | ||||
|   /// get rotation
 | ||||
|   const Rot3& rotation() const { return pose_.rotation(); } | ||||
| 
 | ||||
|   /// get translation
 | ||||
|   const Point3& translation() const { return pose_.translation(); } | ||||
| 
 | ||||
|   //  /// return pose, with derivative
 | ||||
|   //  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
 | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Transformations and measurement functions
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// Project a point into the image and check depth
 | ||||
|   std::pair<Unit3, bool> projectSafe(const Point3& pw) const; | ||||
| 
 | ||||
|   /** Project point into the image
 | ||||
|    * (note: there is no CheiralityException for a spherical camera) | ||||
|    * @param point 3D point in world coordinates | ||||
|    * @return the intrinsic coordinates of the projected point | ||||
|    */ | ||||
|   Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|                  OptionalJacobian<2, 3> Dpoint = boost::none) const; | ||||
| 
 | ||||
|   /** Project point into the image
 | ||||
|    * (note: there is no CheiralityException for a spherical camera) | ||||
|    * @param point 3D direction in world coordinates | ||||
|    * @return the intrinsic coordinates of the projected point | ||||
|    */ | ||||
|   Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|                  OptionalJacobian<2, 2> Dpoint = boost::none) const; | ||||
| 
 | ||||
|   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
 | ||||
|   Point3 backproject(const Unit3& p, const double depth) const; | ||||
| 
 | ||||
|   /// backproject point at infinity
 | ||||
|   Unit3 backprojectPointAtInfinity(const Unit3& p) const; | ||||
| 
 | ||||
|   /** Project point into the image
 | ||||
|    * (note: there is no CheiralityException for a spherical camera) | ||||
|    * @param point 3D point in world coordinates | ||||
|    * @return the intrinsic coordinates of the projected point | ||||
|    */ | ||||
|   Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|                 OptionalJacobian<2, 3> Dpoint = boost::none) const; | ||||
| 
 | ||||
|   /** Compute reprojection error for a given 3D point in world coordinates
 | ||||
|    * @param point 3D point in world coordinates | ||||
|    * @return the tangent space error between the projection and the measurement | ||||
|    */ | ||||
|   Vector2 reprojectionError(const Point3& point, const Unit3& measured, | ||||
|                             OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|                             OptionalJacobian<2, 3> Dpoint = boost::none) const; | ||||
|   /// @}
 | ||||
| 
 | ||||
|   /// move a cameras according to d
 | ||||
|   SphericalCamera retract(const Vector6& d) const { | ||||
|     return SphericalCamera(pose().retract(d)); | ||||
|   } | ||||
| 
 | ||||
|   /// return canonical coordinate
 | ||||
|   Vector6 localCoordinates(const SphericalCamera& p) const { | ||||
|     return pose().localCoordinates(p.pose()); | ||||
|   } | ||||
| 
 | ||||
|   /// for Canonical
 | ||||
|   static SphericalCamera identity() { | ||||
|     return SphericalCamera( | ||||
|         Pose3::identity());  // assumes that the default constructor is valid
 | ||||
|   } | ||||
| 
 | ||||
|   /// for Linear Triangulation
 | ||||
|   Matrix34 cameraProjectionMatrix() const { | ||||
|     return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4)); | ||||
|   } | ||||
| 
 | ||||
|   /// for Nonlinear Triangulation
 | ||||
|   Vector defaultErrorWhenTriangulatingBehindCamera() const { | ||||
|     return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0); | ||||
|   } | ||||
| 
 | ||||
|   /// @deprecated
 | ||||
|   size_t dim() const { return 6; } | ||||
| 
 | ||||
|   /// @deprecated
 | ||||
|   static size_t Dim() { return 6; } | ||||
| 
 | ||||
|  private: | ||||
|   /** Serialization function */ | ||||
|   friend class boost::serialization::access; | ||||
|   template <class Archive> | ||||
|   void serialize(Archive& ar, const unsigned int /*version*/) { | ||||
|     ar& BOOST_SERIALIZATION_NVP(pose_); | ||||
|   } | ||||
| }; | ||||
| // end of class SphericalCamera
 | ||||
| 
 | ||||
| template <> | ||||
| struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {}; | ||||
| 
 | ||||
| template <> | ||||
| struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
|  | @ -170,6 +170,11 @@ public: | |||
|       OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = | ||||
|           boost::none) const; | ||||
| 
 | ||||
|   /// for Nonlinear Triangulation
 | ||||
|   Vector defaultErrorWhenTriangulatingBehindCamera() const { | ||||
|     return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());; | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
| 
 | ||||
| private: | ||||
|  |  | |||
|  | @ -0,0 +1,163 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file SphericalCamera.h | ||||
|  * @brief Calibrated camera with spherical projection | ||||
|  * @date Aug 26, 2021 | ||||
|  * @author Luca Carlone | ||||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/geometry/SphericalCamera.h> | ||||
| 
 | ||||
| #include <cmath> | ||||
| #include <iostream> | ||||
| 
 | ||||
| using namespace std::placeholders; | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| typedef SphericalCamera Camera; | ||||
| 
 | ||||
| // 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 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 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); | ||||
| 
 | ||||
| // 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 double depth = 0.512640224719052; | ||||
| /* ************************************************************************* */ | ||||
| TEST(SphericalCamera, constructor) { | ||||
|   EXPECT(assert_equal(pose, camera.pose())); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 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)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 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
 | ||||
|   Camera camera(Pose3(rot, origin)); | ||||
| 
 | ||||
|   Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.); | ||||
|   Point3 expected(0., 1., 0.); | ||||
|   pair<Unit3, bool> x = camera.projectSafe(expected); | ||||
| 
 | ||||
|   EXPECT(assert_equal(expected, actual)); | ||||
|   EXPECT(assert_equal(Unit3(0, 0, 1), x.first)); | ||||
|   EXPECT(x.second); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| static Unit3 project3(const Pose3& pose, const Point3& point) { | ||||
|   return Camera(pose).project(point); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(SphericalCamera, Dproject) { | ||||
|   Matrix Dpose, Dpoint; | ||||
|   Unit3 result = camera.project(point1, Dpose, Dpoint); | ||||
|   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_point, Dpoint, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| static Vector2 reprojectionError2(const Pose3& pose, const Point3& point, | ||||
|                                   const Unit3& measured) { | ||||
|   return Camera(pose).reprojectionError(point, measured); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 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); | ||||
|   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) { | ||||
|   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); | ||||
|   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)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Add a test with more arbitrary rotation
 | ||||
| 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_point = numericalDerivative22(project3, pose1, point1); | ||||
|   CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); | ||||
|   CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -10,22 +10,23 @@ | |||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * testTriangulation.cpp | ||||
|  * | ||||
|  *  Created on: July 30th, 2013 | ||||
|  *      Author: cbeall3 | ||||
|  * @file testTriangulation.cpp | ||||
|  * @brief triangulation utilities | ||||
|  * @date July 30th, 2013 | ||||
|  * @author Chris Beall (cbeall3) | ||||
|  * @author Luca Carlone | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/geometry/triangulation.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/StereoCamera.h> | ||||
| #include <gtsam/geometry/CameraSet.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/slam/StereoFactor.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/ExpressionFactor.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/CameraSet.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/SphericalCamera.h> | ||||
| #include <gtsam/geometry/StereoCamera.h> | ||||
| #include <gtsam/geometry/triangulation.h> | ||||
| #include <gtsam/nonlinear/ExpressionFactor.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/slam/StereoFactor.h> | ||||
| 
 | ||||
| #include <boost/assign.hpp> | ||||
| #include <boost/assign/std/vector.hpp> | ||||
|  | @ -36,7 +37,7 @@ using namespace boost::assign; | |||
| 
 | ||||
| // Some common constants
 | ||||
| 
 | ||||
| static const boost::shared_ptr<Cal3_S2> sharedCal = //
 | ||||
| static const boost::shared_ptr<Cal3_S2> sharedCal =  //
 | ||||
|     boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480); | ||||
| 
 | ||||
| // Looking along X-axis, 1 meter above ground plane (x-y)
 | ||||
|  | @ -57,8 +58,7 @@ Point2 z2 = camera2.project(landmark); | |||
| 
 | ||||
| //******************************************************************************
 | ||||
| // Simple test with a well-behaved two camera situation
 | ||||
| TEST( triangulation, twoPoses) { | ||||
| 
 | ||||
| TEST(triangulation, twoPoses) { | ||||
|   vector<Pose3> poses; | ||||
|   Point2Vector measurements; | ||||
| 
 | ||||
|  | @ -69,36 +69,36 @@ TEST( triangulation, twoPoses) { | |||
| 
 | ||||
|   // 1. Test simple DLT, perfect in no noise situation
 | ||||
|   bool optimize = false; | ||||
|   boost::optional<Point3> actual1 = //
 | ||||
|       triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); | ||||
|   boost::optional<Point3> actual1 =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize); | ||||
|   EXPECT(assert_equal(landmark, *actual1, 1e-7)); | ||||
| 
 | ||||
|   // 2. test with optimization on, same answer
 | ||||
|   optimize = true; | ||||
|   boost::optional<Point3> actual2 = //
 | ||||
|       triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); | ||||
|   boost::optional<Point3> actual2 =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(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<Point3> actual3 = //
 | ||||
|       triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); | ||||
|   boost::optional<Point3> actual3 =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(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<Point3> actual4 = //
 | ||||
|       triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); | ||||
|   boost::optional<Point3> actual4 =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(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<Cal3Bundler> bundlerCal = //
 | ||||
| TEST(triangulation, twoPosesBundler) { | ||||
|   boost::shared_ptr<Cal3Bundler> bundlerCal =  //
 | ||||
|       boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480); | ||||
|   PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal); | ||||
|   PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal); | ||||
|  | @ -116,37 +116,38 @@ TEST( triangulation, twoPosesBundler) { | |||
|   bool optimize = true; | ||||
|   double rank_tol = 1e-9; | ||||
| 
 | ||||
|   boost::optional<Point3> actual = //
 | ||||
|       triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); | ||||
|   boost::optional<Point3> actual =  //
 | ||||
|       triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize); | ||||
|   EXPECT(assert_equal(landmark, *actual, 1e-7)); | ||||
| 
 | ||||
|   // Add some noise and try again
 | ||||
|   measurements.at(0) += Point2(0.1, 0.5); | ||||
|   measurements.at(1) += Point2(-0.2, 0.3); | ||||
| 
 | ||||
|   boost::optional<Point3> actual2 = //
 | ||||
|       triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); | ||||
|   boost::optional<Point3> actual2 =  //
 | ||||
|       triangulatePoint3<Cal3Bundler>(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<Pose3> poses; | ||||
|   Point2Vector measurements; | ||||
| 
 | ||||
|   poses += pose1, pose2; | ||||
|   measurements += z1, z2; | ||||
| 
 | ||||
|   boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal, | ||||
|       measurements); | ||||
|   boost::optional<Point3> actual = | ||||
|       triangulatePoint3<Cal3_S2>(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<Point3> actual2 = //
 | ||||
|       triangulatePoint3(poses, sharedCal, measurements); | ||||
|   boost::optional<Point3> actual2 =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); | ||||
|   EXPECT(assert_equal(landmark, *actual2, 1e-2)); | ||||
| 
 | ||||
|   // 3. Add a slightly rotated third camera above, again with measurement noise
 | ||||
|  | @ -157,13 +158,13 @@ TEST( triangulation, fourPoses) { | |||
|   poses += pose3; | ||||
|   measurements += z3 + Point2(0.1, -0.1); | ||||
| 
 | ||||
|   boost::optional<Point3> triangulated_3cameras = //
 | ||||
|       triangulatePoint3(poses, sharedCal, measurements); | ||||
|   boost::optional<Point3> triangulated_3cameras =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements); | ||||
|   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); | ||||
| 
 | ||||
|   // Again with nonlinear optimization
 | ||||
|   boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, | ||||
|       sharedCal, measurements, 1e-9, true); | ||||
|   boost::optional<Point3> triangulated_3cameras_opt = | ||||
|       triangulatePoint3<Cal3_S2>(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
 | ||||
|  | @ -176,13 +177,13 @@ TEST( triangulation, fourPoses) { | |||
|   poses += pose4; | ||||
|   measurements += Point2(400, 400); | ||||
| 
 | ||||
|   CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), | ||||
|       TriangulationCheiralityException); | ||||
|   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements), | ||||
|                   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<Cal3_S2> camera1(pose1, K1); | ||||
|  | @ -195,22 +196,23 @@ TEST( triangulation, fourPoses_distinct_Ks) { | |||
|   Point2 z1 = camera1.project(landmark); | ||||
|   Point2 z2 = camera2.project(landmark); | ||||
| 
 | ||||
|   CameraSet<PinholeCamera<Cal3_S2> > cameras; | ||||
|   CameraSet<PinholeCamera<Cal3_S2>> cameras; | ||||
|   Point2Vector measurements; | ||||
| 
 | ||||
|   cameras += camera1, camera2; | ||||
|   measurements += z1, z2; | ||||
| 
 | ||||
|   boost::optional<Point3> actual = //
 | ||||
|       triangulatePoint3(cameras, measurements); | ||||
|   boost::optional<Point3> actual =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(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<Point3> actual2 = //
 | ||||
|       triangulatePoint3(cameras, measurements); | ||||
|   boost::optional<Point3> actual2 =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(cameras, measurements); | ||||
|   EXPECT(assert_equal(landmark, *actual2, 1e-2)); | ||||
| 
 | ||||
|   // 3. Add a slightly rotated third camera above, again with measurement noise
 | ||||
|  | @ -222,13 +224,13 @@ TEST( triangulation, fourPoses_distinct_Ks) { | |||
|   cameras += camera3; | ||||
|   measurements += z3 + Point2(0.1, -0.1); | ||||
| 
 | ||||
|   boost::optional<Point3> triangulated_3cameras = //
 | ||||
|       triangulatePoint3(cameras, measurements); | ||||
|   boost::optional<Point3> triangulated_3cameras =  //
 | ||||
|       triangulatePoint3<Cal3_S2>(cameras, measurements); | ||||
|   EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); | ||||
| 
 | ||||
|   // Again with nonlinear optimization
 | ||||
|   boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras, | ||||
|       measurements, 1e-9, true); | ||||
|   boost::optional<Point3> triangulated_3cameras_opt = | ||||
|       triangulatePoint3<Cal3_S2>(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
 | ||||
|  | @ -241,13 +243,13 @@ TEST( triangulation, fourPoses_distinct_Ks) { | |||
| 
 | ||||
|   cameras += camera4; | ||||
|   measurements += Point2(400, 400); | ||||
|   CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), | ||||
|       TriangulationCheiralityException); | ||||
|   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(cameras, measurements), | ||||
|                   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<Cal3_S2> camera1(pose1, K1); | ||||
|  | @ -260,24 +262,29 @@ TEST( triangulation, outliersAndFarLandmarks) { | |||
|   Point2 z1 = camera1.project(landmark); | ||||
|   Point2 z2 = camera2.project(landmark); | ||||
| 
 | ||||
|   CameraSet<PinholeCamera<Cal3_S2> > cameras; | ||||
|   CameraSet<PinholeCamera<Cal3_S2>> 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<Cal3_S2> camera3(pose3, K3); | ||||
|  | @ -286,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<Cal3_S2> camera1(pose1, *sharedCal); | ||||
| 
 | ||||
|  | @ -313,12 +322,12 @@ TEST( triangulation, twoIdenticalPoses) { | |||
|   poses += pose1, pose1; | ||||
|   measurements += z1, z1; | ||||
| 
 | ||||
|   CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), | ||||
|       TriangulationUnderconstrainedException); | ||||
|   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements), | ||||
|                   TriangulationUnderconstrainedException); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST( triangulation, onePose) { | ||||
| TEST(triangulation, onePose) { | ||||
|   // we expect this test to fail with a TriangulationUnderconstrainedException
 | ||||
|   // because there's only one camera observation
 | ||||
| 
 | ||||
|  | @ -326,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); | ||||
|   CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements), | ||||
|                   TriangulationUnderconstrainedException); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST( triangulation, StereotriangulateNonlinear ) { | ||||
| 
 | ||||
|   auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); | ||||
| TEST(triangulation, StereotriangulateNonlinear) { | ||||
|   auto stereoK = boost::make_shared<Cal3_S2Stereo>(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<StereoCamera> Cameras; | ||||
|   Cameras cameras; | ||||
|  | @ -358,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 actual = triangulateNonlinear<StereoCamera>(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<Pose3,Point3> StereoFactor; | ||||
|     typedef GenericStereoFactor<Pose3, Point3> StereoFactor; | ||||
| 
 | ||||
|     Values values; | ||||
|     values.insert(Symbol('x', 1), Pose3(m1)); | ||||
|  | @ -378,17 +386,19 @@ TEST( triangulation, StereotriangulateNonlinear ) { | |||
| 
 | ||||
|     NonlinearFactorGraph graph; | ||||
|     static SharedNoiseModel unit(noiseModel::Unit::Create(3)); | ||||
|     graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); | ||||
|     graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); | ||||
|     graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x', 1), | ||||
|                                        Symbol('l', 1), stereoK); | ||||
|     graph.emplace_shared<StereoFactor>(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<Point3>(Symbol('l',1)), 1e-4)); | ||||
|     EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4)); | ||||
|   } | ||||
| 
 | ||||
|   // use Triangulation Factor directly - expect same result as above
 | ||||
|  | @ -399,13 +409,15 @@ TEST( triangulation, StereotriangulateNonlinear ) { | |||
|     NonlinearFactorGraph graph; | ||||
|     static SharedNoiseModel unit(noiseModel::Unit::Create(3)); | ||||
| 
 | ||||
|     graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1)); | ||||
|     graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1)); | ||||
|     graph.emplace_shared<TriangulationFactor<StereoCamera>>( | ||||
|         cameras[0], measurements[0], unit, Symbol('l', 1)); | ||||
|     graph.emplace_shared<TriangulationFactor<StereoCamera>>( | ||||
|         cameras[1], measurements[1], unit, Symbol('l', 1)); | ||||
| 
 | ||||
|     LevenbergMarquardtOptimizer optimizer(graph, values); | ||||
|     Values result = optimizer.optimize(); | ||||
| 
 | ||||
|     EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4)); | ||||
|     EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4)); | ||||
|   } | ||||
| 
 | ||||
|   // use ExpressionFactor - expect same result as above
 | ||||
|  | @ -416,11 +428,13 @@ TEST( triangulation, StereotriangulateNonlinear ) { | |||
|     NonlinearFactorGraph graph; | ||||
|     static SharedNoiseModel unit(noiseModel::Unit::Create(3)); | ||||
| 
 | ||||
|     Expression<Point3> point_(Symbol('l',1)); | ||||
|     Expression<Point3> point_(Symbol('l', 1)); | ||||
|     Expression<StereoCamera> camera0_(cameras[0]); | ||||
|     Expression<StereoCamera> camera1_(cameras[1]); | ||||
|     Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_); | ||||
|     Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_); | ||||
|     Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, | ||||
|                                        point_); | ||||
|     Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, | ||||
|                                        point_); | ||||
| 
 | ||||
|     graph.addExpressionFactor(unit, measurements[0], project0_); | ||||
|     graph.addExpressionFactor(unit, measurements[1], project1_); | ||||
|  | @ -428,10 +442,172 @@ TEST( triangulation, StereotriangulateNonlinear ) { | |||
|     LevenbergMarquardtOptimizer optimizer(graph, values); | ||||
|     Values result = optimizer.optimize(); | ||||
| 
 | ||||
|     EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4)); | ||||
|     EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| // Simple test with a well-behaved two camera situation
 | ||||
| TEST(triangulation, twoPoses_sphericalCamera) { | ||||
|   vector<Pose3> poses; | ||||
|   std::vector<Unit3> measurements; | ||||
| 
 | ||||
|   // Project landmark into two cameras and triangulate
 | ||||
|   SphericalCamera cam1(pose1); | ||||
|   SphericalCamera cam2(pose2); | ||||
|   Unit3 u1 = cam1.project(landmark); | ||||
|   Unit3 u2 = cam2.project(landmark); | ||||
| 
 | ||||
|   poses += pose1, pose2; | ||||
|   measurements += u1, u2; | ||||
| 
 | ||||
|   CameraSet<SphericalCamera> cameras; | ||||
|   cameras.push_back(cam1); | ||||
|   cameras.push_back(cam2); | ||||
| 
 | ||||
|   double rank_tol = 1e-9; | ||||
| 
 | ||||
|   // 1. Test linear triangulation via DLT
 | ||||
|   auto projection_matrices = projectionMatricesFromCameras(cameras); | ||||
|   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); | ||||
|   EXPECT(assert_equal(landmark, point, 1e-7)); | ||||
| 
 | ||||
|   // 2. Test nonlinear triangulation
 | ||||
|   point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point); | ||||
|   EXPECT(assert_equal(landmark, point, 1e-7)); | ||||
| 
 | ||||
|   // 3. Test simple DLT, now within triangulatePoint3
 | ||||
|   bool optimize = false; | ||||
|   boost::optional<Point3> actual1 =  //
 | ||||
|       triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, | ||||
|                                          optimize); | ||||
|   EXPECT(assert_equal(landmark, *actual1, 1e-7)); | ||||
| 
 | ||||
|   // 4. test with optimization on, same answer
 | ||||
|   optimize = true; | ||||
|   boost::optional<Point3> actual2 =  //
 | ||||
|       triangulatePoint3<SphericalCamera>(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
 | ||||
|   measurements.at(1) = u2.retract(Vector2(-0.02, 0.03)); | ||||
|   optimize = false; | ||||
|   boost::optional<Point3> actual3 =  //
 | ||||
|       triangulatePoint3<SphericalCamera>(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<Point3> actual4 =  //
 | ||||
|       triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, | ||||
|                                          optimize); | ||||
|   EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { | ||||
|   vector<Pose3> poses; | ||||
|   std::vector<Unit3> 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
 | ||||
|   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
 | ||||
| 
 | ||||
|   poses += pose1, pose2; | ||||
|   measurements += u1, u2; | ||||
| 
 | ||||
|   CameraSet<SphericalCamera> cameras; | ||||
|   cameras.push_back(cam1); | ||||
|   cameras.push_back(cam2); | ||||
| 
 | ||||
|   double rank_tol = 1e-9; | ||||
| 
 | ||||
|   { | ||||
|     // 1. Test simple DLT, when 1 point is behind spherical camera
 | ||||
|     bool optimize = false; | ||||
| #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|     CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements, | ||||
|                                                        rank_tol, optimize), | ||||
|                     TriangulationCheiralityException); | ||||
| #else  // otherwise project should not throw the exception
 | ||||
|     boost::optional<Point3> actual1 =  //
 | ||||
|         triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol, | ||||
|                                            optimize); | ||||
|     EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); | ||||
| #endif | ||||
|   } | ||||
|   { | ||||
|     // 2. test with optimization on, same answer
 | ||||
|     bool optimize = true; | ||||
| #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|     CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements, | ||||
|                                                        rank_tol, optimize), | ||||
|                     TriangulationCheiralityException); | ||||
| #else  // otherwise project should not throw the exception
 | ||||
|     boost::optional<Point3> actual1 =  //
 | ||||
|         triangulatePoint3<SphericalCamera>(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
 | ||||
|   SphericalCamera sphericalCamera(poseA); | ||||
|   Unit3 u = sphericalCamera.project(landmarkL); | ||||
| 
 | ||||
|   static Cal3_S2::shared_ptr sharedK(new Cal3_S2(60, 640, 480)); | ||||
|   PinholePose<Cal3_S2> pinholeCamera(poseA, sharedK); | ||||
|   Vector2 px = pinholeCamera.project(landmarkL); | ||||
| 
 | ||||
|   // add perturbation and compare error in both cameras
 | ||||
|   Vector2 px_noise(1.0, 2.0);  // px 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 expected_measured_u = | ||||
|       Unit3(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy(), 1.0); | ||||
|   EXPECT(assert_equal(expected_measured_u, measured_u, 1e-7)); | ||||
| 
 | ||||
|   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); | ||||
|   // expectedError: not easy to calculate, since it involves the unit3 basis
 | ||||
|   Vector2 expectedErrorSpherical(-0.00360842, 0.00180419); | ||||
|   EXPECT(assert_equal(expectedErrorSpherical, actualErrorSpherical, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
|  | @ -53,15 +53,57 @@ Vector4 triangulateHomogeneousDLT( | |||
|   return v; | ||||
| } | ||||
| 
 | ||||
| Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
| Vector4 triangulateHomogeneousDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
|     const std::vector<Unit3>& measurements, double rank_tol) { | ||||
| 
 | ||||
|   // number of cameras
 | ||||
|   size_t m = projection_matrices.size(); | ||||
| 
 | ||||
|   // Allocate DLT matrix
 | ||||
|   Matrix A = Matrix::Zero(m * 2, 4); | ||||
| 
 | ||||
|   for (size_t i = 0; i < m; i++) { | ||||
|     size_t row = i * 2; | ||||
|     const Matrix34& projection = projection_matrices.at(i); | ||||
|     const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
 | ||||
| 
 | ||||
|     // build system of equations
 | ||||
|     A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); | ||||
|     A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1); | ||||
|   } | ||||
|   int rank; | ||||
|   double error; | ||||
|   Vector v; | ||||
|   boost::tie(rank, error, v) = DLT(A, rank_tol); | ||||
| 
 | ||||
|   if (rank < 3) | ||||
|     throw(TriangulationUnderconstrainedException()); | ||||
| 
 | ||||
|   return v; | ||||
| } | ||||
| 
 | ||||
| Point3 triangulateDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
|     const Point2Vector& measurements, double rank_tol) { | ||||
| 
 | ||||
|   Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); | ||||
| 
 | ||||
|   Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, | ||||
|                                         rank_tol); | ||||
|   // Create 3D point from homogeneous coordinates
 | ||||
|   return Point3(v.head<3>() / v[3]); | ||||
| } | ||||
| 
 | ||||
| Point3 triangulateDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
|     const std::vector<Unit3>& measurements, double rank_tol) { | ||||
| 
 | ||||
|   // contrary to previous triangulateDLT, this is now taking Unit3 inputs
 | ||||
|   Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, | ||||
|                                          rank_tol); | ||||
|    // Create 3D point from homogeneous coordinates
 | ||||
|    return Point3(v.head<3>() / v[3]); | ||||
| } | ||||
| 
 | ||||
| ///
 | ||||
| /**
 | ||||
|  * Optimize for triangulation | ||||
|  | @ -71,7 +113,7 @@ Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matri | |||
|  * @return refined Point3 | ||||
|  */ | ||||
| Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, | ||||
|     Key landmarkKey) { | ||||
|                 Key landmarkKey) { | ||||
|   // Maybe we should consider Gauss-Newton?
 | ||||
|   LevenbergMarquardtParams params; | ||||
|   params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; | ||||
|  |  | |||
|  | @ -24,6 +24,7 @@ | |||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/geometry/CameraSet.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <gtsam/geometry/SphericalCamera.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
|  | @ -59,6 +60,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( | |||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
|     const Point2Vector& measurements, double rank_tol = 1e-9); | ||||
| 
 | ||||
| /**
 | ||||
|  * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors | ||||
|  * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) | ||||
|  * @param projection_matrices Projection matrices (K*P^-1) | ||||
|  * @param measurements Unit3 bearing measurements | ||||
|  * @param rank_tol SVD rank tolerance | ||||
|  * @return Triangulated point, in homogeneous coordinates | ||||
|  */ | ||||
| GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
|     const std::vector<Unit3>& measurements, double rank_tol = 1e-9); | ||||
| 
 | ||||
| /**
 | ||||
|  * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 | ||||
|  * @param projection_matrices Projection matrices (K*P^-1) | ||||
|  | @ -71,6 +84,14 @@ GTSAM_EXPORT Point3 triangulateDLT( | |||
|     const Point2Vector& measurements, | ||||
|     double rank_tol = 1e-9); | ||||
| 
 | ||||
| /**
 | ||||
|  * overload of previous function to work with Unit3 (projected to canonical camera) | ||||
|  */ | ||||
| GTSAM_EXPORT Point3 triangulateDLT( | ||||
|     const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, | ||||
|     const std::vector<Unit3>& measurements, | ||||
|     double rank_tol = 1e-9); | ||||
| 
 | ||||
| /**
 | ||||
|  * Create a factor graph with projection factors from poses and one calibration | ||||
|  * @param poses Camera poses | ||||
|  | @ -180,26 +201,27 @@ Point3 triangulateNonlinear( | |||
|   return optimize(graph, values, Symbol('p', 0)); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Create a 3*4 camera projection matrix from calibration and pose. | ||||
|  * Functor for partial application on calibration | ||||
|  * @param pose The camera pose | ||||
|  * @param cal  The calibration | ||||
|  * @return Returns a Matrix34 | ||||
|  */ | ||||
| template<class CAMERA> | ||||
| std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> | ||||
| projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) { | ||||
|   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; | ||||
|   for (const CAMERA &camera: cameras) { | ||||
|     projection_matrices.push_back(camera.cameraProjectionMatrix()); | ||||
|   } | ||||
|   return projection_matrices; | ||||
| } | ||||
| 
 | ||||
| // overload, assuming pinholePose
 | ||||
| template<class CALIBRATION> | ||||
| struct CameraProjectionMatrix { | ||||
|   CameraProjectionMatrix(const CALIBRATION& calibration) : | ||||
|       K_(calibration.K()) { | ||||
| std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses( | ||||
|         const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) { | ||||
|   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; | ||||
|   for (size_t i = 0; i < poses.size(); i++) { | ||||
|     PinholePose<CALIBRATION> camera(poses.at(i), sharedCal); | ||||
|     projection_matrices.push_back(camera.cameraProjectionMatrix()); | ||||
|   } | ||||
|   Matrix34 operator()(const Pose3& pose) const { | ||||
|     return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); | ||||
|   } | ||||
| private: | ||||
|   const Matrix3 K_; | ||||
| public: | ||||
|   GTSAM_MAKE_ALIGNED_OPERATOR_NEW | ||||
| }; | ||||
|   return projection_matrices; | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Function to triangulate 3D landmark point from an arbitrary number | ||||
|  | @ -224,10 +246,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses, | |||
|     throw(TriangulationUnderconstrainedException()); | ||||
| 
 | ||||
|   // construct projection matrices from poses & calibration
 | ||||
|   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; | ||||
|   CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
 | ||||
|   for(const Pose3& pose: poses) | ||||
|     projection_matrices.push_back(createP(pose)); | ||||
|   auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); | ||||
| 
 | ||||
|   // Triangulate linearly
 | ||||
|   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); | ||||
|  | @ -274,11 +293,7 @@ Point3 triangulatePoint3( | |||
|     throw(TriangulationUnderconstrainedException()); | ||||
| 
 | ||||
|   // construct projection matrices from poses & calibration
 | ||||
|   std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; | ||||
|   for(const CAMERA& camera: cameras) | ||||
|     projection_matrices.push_back( | ||||
|         CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())( | ||||
|             camera.pose())); | ||||
|   auto projection_matrices = projectionMatricesFromCameras(cameras); | ||||
|   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); | ||||
| 
 | ||||
|   // The n refine using non-linear optimization
 | ||||
|  | @ -474,8 +489,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras, | |||
| #endif | ||||
|         // Check reprojection error
 | ||||
|         if (params.dynamicOutlierRejectionThreshold > 0) { | ||||
|           const Point2& zi = measured.at(i); | ||||
|           Point2 reprojectionError(camera.project(point) - zi); | ||||
|           const typename CAMERA::Measurement& zi = measured.at(i); | ||||
|           Point2 reprojectionError = camera.reprojectionError(point, zi); | ||||
|           maxReprojError = std::max(maxReprojError, reprojectionError.norm()); | ||||
|         } | ||||
|         i += 1; | ||||
|  | @ -503,6 +518,6 @@ using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>; | |||
| using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>; | ||||
| using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>; | ||||
| using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>; | ||||
| 
 | ||||
| using CameraSetSpherical = CameraSet<SphericalCamera>; | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -54,6 +54,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> { | |||
|   typedef SmartProjectionFactor<CAMERA> Base; | ||||
|   typedef SmartProjectionRigFactor<CAMERA> This; | ||||
|   typedef typename CAMERA::CalibrationType CALIBRATION; | ||||
|   typedef typename CAMERA::Measurement MEASUREMENT; | ||||
|   typedef typename CAMERA::MeasurementVector MEASUREMENTS; | ||||
| 
 | ||||
|   static const int DimPose = 6;  ///< Pose3 dimension
 | ||||
|   static const int ZDim = 2;     ///< Measurement dimension
 | ||||
|  | @ -118,7 +120,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> { | |||
|    * @param cameraId ID of the camera in the rig taking the measurement (default | ||||
|    * 0) | ||||
|    */ | ||||
|   void add(const Point2& measured, const Key& poseKey, | ||||
|   void add(const MEASUREMENT& measured, const Key& poseKey, | ||||
|            const size_t& cameraId = 0) { | ||||
|     // store measurement and key
 | ||||
|     this->measured_.push_back(measured); | ||||
|  | @ -144,7 +146,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> { | |||
|    * @param cameraIds IDs of the cameras in the rig taking each measurement | ||||
|    * (same order as the measurements) | ||||
|    */ | ||||
|   void add(const Point2Vector& measurements, const KeyVector& poseKeys, | ||||
|   void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys, | ||||
|            const FastVector<size_t>& cameraIds = FastVector<size_t>()) { | ||||
|     if (poseKeys.size() != measurements.size() || | ||||
|         (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { | ||||
|  |  | |||
|  | @ -129,7 +129,7 @@ public: | |||
|             << std::endl; | ||||
|       if (throwCheirality_) | ||||
|         throw e; | ||||
|       return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * camera_.calibration().fx()); | ||||
|       return camera_.defaultErrorWhenTriangulatingBehindCamera(); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -17,11 +17,13 @@ | |||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| #include <gtsam/slam/SmartProjectionPoseFactor.h> | ||||
| #include <gtsam/slam/SmartProjectionFactor.h> | ||||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/geometry/Cal3Bundler.h> | ||||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/geometry/SphericalCamera.h> | ||||
| #include <gtsam/slam/GeneralSFMFactor.h> | ||||
| #include <gtsam/slam/SmartProjectionFactor.h> | ||||
| #include <gtsam/slam/SmartProjectionPoseFactor.h> | ||||
| 
 | ||||
| #include "../SmartProjectionRigFactor.h" | ||||
| 
 | ||||
| using namespace std; | ||||
|  | @ -44,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; | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -63,7 +65,7 @@ Camera cam2(pose_right, K2); | |||
| Camera cam3(pose_above, K2); | ||||
| typedef GeneralSFMFactor<Camera, Point3> SFMFactor; | ||||
| SmartProjectionParams params; | ||||
| } | ||||
| }  // namespace vanilla
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // default Cal3_S2 poses
 | ||||
|  | @ -78,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
 | ||||
|  | @ -93,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
 | ||||
|  | @ -111,7 +113,8 @@ Camera cam1(level_pose, K); | |||
| Camera cam2(pose_right, K); | ||||
| Camera cam3(pose_above, K); | ||||
| typedef GeneralSFMFactor<Camera, Point3> SFMFactor; | ||||
| } | ||||
| }  // namespace bundler
 | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| // Cal3Bundler poses
 | ||||
| namespace bundlerPose { | ||||
|  | @ -119,35 +122,50 @@ typedef PinholePose<Cal3Bundler> Camera; | |||
| typedef CameraSet<Camera> Cameras; | ||||
| typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor; | ||||
| typedef SmartProjectionRigFactor<Camera> SmartRigFactor; | ||||
| static boost::shared_ptr<Cal3Bundler> sharedBundlerK( | ||||
|     new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); | ||||
| static boost::shared_ptr<Cal3Bundler> 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
 | ||||
| namespace sphericalCamera { | ||||
| typedef SphericalCamera Camera; | ||||
| typedef CameraSet<Camera> Cameras; | ||||
| typedef SmartProjectionRigFactor<Camera> SmartFactorP; | ||||
| static EmptyCal::shared_ptr emptyK; | ||||
| Camera level_camera(level_pose); | ||||
| Camera level_camera_right(pose_right); | ||||
| Camera cam1(level_pose); | ||||
| Camera cam2(pose_right); | ||||
| Camera cam3(pose_above); | ||||
| }  // namespace sphericalCamera
 | ||||
| /* *************************************************************************/ | ||||
| 
 | ||||
| template<class CAMERA> | ||||
| template <class CAMERA> | ||||
| 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<class CAMERA> | ||||
| void projectToMultipleCameras(const CAMERA& cam1, const CAMERA& cam2, | ||||
|     const CAMERA& cam3, Point3 landmark, typename CAMERA::MeasurementVector& measurements_cam) { | ||||
|   Point2 cam1_uv1 = cam1.project(landmark); | ||||
|   Point2 cam2_uv1 = cam2.project(landmark); | ||||
|   Point2 cam3_uv1 = cam3.project(landmark); | ||||
| template <class CAMERA> | ||||
| 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); | ||||
|   measurements_cam.push_back(cam1_uv1); | ||||
|   measurements_cam.push_back(cam2_uv1); | ||||
|   measurements_cam.push_back(cam3_uv1); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -55,8 +55,6 @@ Key cameraId3 = 2; | |||
| static Point2 measurement1(323.0, 240.0); | ||||
| 
 | ||||
| LevenbergMarquardtParams lmParams; | ||||
| // Make more verbose like so (in tests):
 | ||||
| // params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // default Cal3_S2 poses with rolling shutter effect
 | ||||
|  | @ -1187,10 +1185,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.06 CPU
 | ||||
| //(10000 times, 0.061226 wall, 0.06 children, min: 0 max: 0)
 | ||||
| //|   -SmartPoseFactor LINEARIZE: 0.06 CPU
 | ||||
| //(10000 times, 0.073037 wall, 0.06 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; | ||||
|  | @ -1249,6 +1246,355 @@ TEST(SmartProjectionRigFactor, timing) { | |||
| } | ||||
| #endif | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) { | ||||
|   using namespace sphericalCamera; | ||||
|   Camera::MeasurementVector measurements_lmk1, measurements_lmk2, | ||||
|       measurements_lmk3; | ||||
| 
 | ||||
|   // Project three landmarks into three cameras
 | ||||
|   projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, | ||||
|                                    measurements_lmk1); | ||||
|   projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, | ||||
|                                    measurements_lmk2); | ||||
|   projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, | ||||
|                                    measurements_lmk3); | ||||
| 
 | ||||
|   // create inputs
 | ||||
|   KeyVector keys; | ||||
|   keys.push_back(x1); | ||||
|   keys.push_back(x2); | ||||
|   keys.push_back(x3); | ||||
| 
 | ||||
|   boost::shared_ptr<Cameras> cameraRig(new Cameras()); | ||||
|   cameraRig->push_back(Camera(Pose3::identity(), emptyK)); | ||||
| 
 | ||||
|   SmartProjectionParams params( | ||||
|       gtsam::HESSIAN, | ||||
|       gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
 | ||||
|   params.setRankTolerance(0.1); | ||||
| 
 | ||||
|   SmartFactorP::shared_ptr smartFactor1( | ||||
|       new SmartFactorP(model, cameraRig, params)); | ||||
|   smartFactor1->add(measurements_lmk1, keys); | ||||
| 
 | ||||
|   SmartFactorP::shared_ptr smartFactor2( | ||||
|       new SmartFactorP(model, cameraRig, params)); | ||||
|   smartFactor2->add(measurements_lmk2, keys); | ||||
| 
 | ||||
|   SmartFactorP::shared_ptr smartFactor3( | ||||
|       new SmartFactorP(model, cameraRig, params)); | ||||
|   smartFactor3->add(measurements_lmk3, keys); | ||||
| 
 | ||||
|   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.push_back(smartFactor1); | ||||
|   graph.push_back(smartFactor2); | ||||
|   graph.push_back(smartFactor3); | ||||
|   graph.addPrior(x1, level_pose, noisePrior); | ||||
|   graph.addPrior(x2, pose_right, noisePrior); | ||||
| 
 | ||||
|   Values groundTruth; | ||||
|   groundTruth.insert(x1, level_pose); | ||||
|   groundTruth.insert(x2, pose_right); | ||||
|   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 / 100), | ||||
|                            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
 | ||||
|   values.insert(x3, pose_above * noise_pose); | ||||
| 
 | ||||
|   DOUBLES_EQUAL(0.94148963675515274, graph.error(values), 1e-9); | ||||
| 
 | ||||
|   Values result; | ||||
|   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); | ||||
|   result = optimizer.optimize(); | ||||
| 
 | ||||
|   EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-5)); | ||||
| } | ||||
| 
 | ||||
| #ifndef DISABLE_TIMING | ||||
| #include <gtsam/base/timing.h> | ||||
| // using spherical camera is slightly slower (but comparable) to
 | ||||
| // PinholePose<Cal3_S2>
 | ||||
| //|   -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) { | ||||
|   // create common data
 | ||||
|   Rot3 R = Rot3::identity(); | ||||
|   Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); | ||||
|   Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); | ||||
|   Pose3 body_P_sensorId = Pose3::identity(); | ||||
|   Point3 landmark1(0, 0, 10); | ||||
| 
 | ||||
|   // create spherical data
 | ||||
|   EmptyCal::shared_ptr emptyK; | ||||
|   SphericalCamera cam1_sphere(pose1, emptyK), cam2_sphere(pose2, emptyK); | ||||
|   // Project 2 landmarks into 2 cameras
 | ||||
|   std::vector<Unit3> measurements_lmk1_sphere; | ||||
|   measurements_lmk1_sphere.push_back(cam1_sphere.project(landmark1)); | ||||
|   measurements_lmk1_sphere.push_back(cam2_sphere.project(landmark1)); | ||||
| 
 | ||||
|   // create Cal3_S2 data
 | ||||
|   static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); | ||||
|   PinholePose<Cal3_S2> cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); | ||||
|   // Project 2 landmarks into 2 cameras
 | ||||
|   std::vector<Point2> measurements_lmk1; | ||||
|   measurements_lmk1.push_back(cam1.project(landmark1)); | ||||
|   measurements_lmk1.push_back(cam2.project(landmark1)); | ||||
| 
 | ||||
|   SmartProjectionParams params( | ||||
|       gtsam::HESSIAN, | ||||
|       gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
 | ||||
| 
 | ||||
|   size_t nrTests = 1000; | ||||
| 
 | ||||
|   for (size_t i = 0; i < nrTests; i++) { | ||||
|     boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig( | ||||
|         new CameraSet<SphericalCamera>());  // single camera in the rig
 | ||||
|     cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK)); | ||||
| 
 | ||||
|     SmartProjectionRigFactor<SphericalCamera>::shared_ptr smartFactorP( | ||||
|         new SmartProjectionRigFactor<SphericalCamera>(model, cameraRig, | ||||
|                                                       params)); | ||||
|     smartFactorP->add(measurements_lmk1_sphere[0], x1); | ||||
|     smartFactorP->add(measurements_lmk1_sphere[1], x1); | ||||
| 
 | ||||
|     Values values; | ||||
|     values.insert(x1, pose1); | ||||
|     values.insert(x2, pose2); | ||||
|     gttic_(SmartFactorP_spherical_LINEARIZE); | ||||
|     smartFactorP->linearize(values); | ||||
|     gttoc_(SmartFactorP_spherical_LINEARIZE); | ||||
|   } | ||||
| 
 | ||||
|   for (size_t i = 0; i < nrTests; i++) { | ||||
|     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( | ||||
|         new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
 | ||||
|     cameraRig->push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple)); | ||||
| 
 | ||||
|     SmartProjectionRigFactor<PinholePose<Cal3_S2>>::shared_ptr smartFactorP2( | ||||
|         new SmartProjectionRigFactor<PinholePose<Cal3_S2>>(model, cameraRig, | ||||
|                                                            params)); | ||||
|     smartFactorP2->add(measurements_lmk1[0], x1); | ||||
|     smartFactorP2->add(measurements_lmk1[1], x1); | ||||
| 
 | ||||
|     Values values; | ||||
|     values.insert(x1, pose1); | ||||
|     values.insert(x2, pose2); | ||||
|     gttic_(SmartFactorP_pinhole_LINEARIZE); | ||||
|     smartFactorP2->linearize(values); | ||||
|     gttoc_(SmartFactorP_pinhole_LINEARIZE); | ||||
|   } | ||||
|   tictoc_print_(); | ||||
| } | ||||
| #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
 | ||||
| 
 | ||||
|   // 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
 | ||||
| 
 | ||||
|     Camera cam1(poseA, sharedK); | ||||
|     Camera cam2(poseB, sharedK); | ||||
| 
 | ||||
|     SmartProjectionParams params( | ||||
|         gtsam::HESSIAN, | ||||
|         gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
 | ||||
|     params.setRankTolerance(1); | ||||
| 
 | ||||
|     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( | ||||
|         new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
 | ||||
|     cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK)); | ||||
| 
 | ||||
|     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); | ||||
| 
 | ||||
|     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)); | ||||
|   } | ||||
|   // 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); | ||||
| 
 | ||||
|     SmartProjectionParams params( | ||||
|         gtsam::HESSIAN, | ||||
|         gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
 | ||||
|     params.setRankTolerance(0.1); | ||||
| 
 | ||||
|     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( | ||||
|             new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
 | ||||
|     cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK)); | ||||
| 
 | ||||
|     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); | ||||
| 
 | ||||
|     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());  // 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
 | ||||
| 
 | ||||
|     Camera cam1(poseA, canonicalK); | ||||
|     Camera cam2(poseB, canonicalK); | ||||
| 
 | ||||
|     SmartProjectionParams params( | ||||
|         gtsam::HESSIAN, | ||||
|         gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
 | ||||
|     params.setRankTolerance(0.01); | ||||
| 
 | ||||
|     boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig( | ||||
|         new CameraSet<PinholePose<Cal3_S2>>());  // single camera in the rig
 | ||||
|     cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK)); | ||||
| 
 | ||||
|     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); | ||||
| 
 | ||||
|     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)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) { | ||||
|   typedef SphericalCamera Camera; | ||||
|   typedef SmartProjectionRigFactor<Camera> 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
 | ||||
| 
 | ||||
|   Camera cam1(poseA); | ||||
|   Camera cam2(poseB); | ||||
| 
 | ||||
|   boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig( | ||||
|           new CameraSet<SphericalCamera>());  // single camera in the rig
 | ||||
|   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
 | ||||
|     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); | ||||
| 
 | ||||
|     NonlinearFactorGraph graph; | ||||
|     graph.push_back(smartFactor1); | ||||
| 
 | ||||
|     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
 | ||||
|   } | ||||
|   // 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( | ||||
|         gtsam::HESSIAN, | ||||
|         gtsam::ZERO_ON_DEGENERACY);  // only config that works with rig factors
 | ||||
|     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); | ||||
| 
 | ||||
|     NonlinearFactorGraph graph; | ||||
|     graph.push_back(smartFactor1); | ||||
| 
 | ||||
|     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)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
|  | @ -47,6 +47,8 @@ class SmartProjectionPoseFactorRollingShutter | |||
|   typedef SmartProjectionFactor<CAMERA> Base; | ||||
|   typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This; | ||||
|   typedef typename CAMERA::CalibrationType CALIBRATION; | ||||
|   typedef typename CAMERA::Measurement MEASUREMENT; | ||||
|   typedef typename CAMERA::MeasurementVector MEASUREMENTS; | ||||
| 
 | ||||
|  protected: | ||||
|   /// The keys of the pose of the body (with respect to an external world
 | ||||
|  | @ -68,12 +70,6 @@ class SmartProjectionPoseFactorRollingShutter | |||
|  public: | ||||
|   EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||||
| 
 | ||||
|   typedef CAMERA Camera; | ||||
|   typedef CameraSet<CAMERA> Cameras; | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<This> shared_ptr; | ||||
| 
 | ||||
|   static const int DimBlock = | ||||
|       12;  ///< size of the variable stacking 2 poses from which the observation
 | ||||
|            ///< pose is interpolated
 | ||||
|  | @ -84,6 +80,12 @@ class SmartProjectionPoseFactorRollingShutter | |||
|   typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>> | ||||
|       FBlocks;  // vector of F blocks
 | ||||
| 
 | ||||
|   typedef CAMERA Camera; | ||||
|   typedef CameraSet<CAMERA> Cameras; | ||||
| 
 | ||||
|   /// shorthand for a smart pointer to a factor
 | ||||
|   typedef boost::shared_ptr<This> shared_ptr; | ||||
| 
 | ||||
|   /// Default constructor, only for serialization
 | ||||
|   SmartProjectionPoseFactorRollingShutter() {} | ||||
| 
 | ||||
|  | @ -125,7 +127,7 @@ class SmartProjectionPoseFactorRollingShutter | |||
|    *  interpolated pose is the same as world_P_body_key1 | ||||
|    * @param cameraId ID of the camera taking the measurement (default 0) | ||||
|    */ | ||||
|   void add(const Point2& measured, const Key& world_P_body_key1, | ||||
|   void add(const MEASUREMENT& measured, const Key& world_P_body_key1, | ||||
|            const Key& world_P_body_key2, const double& alpha, | ||||
|            const size_t& cameraId = 0) { | ||||
|     // store measurements in base class
 | ||||
|  | @ -164,7 +166,7 @@ class SmartProjectionPoseFactorRollingShutter | |||
|    * @param cameraIds IDs of the cameras taking each measurement (same order as | ||||
|    * the measurements) | ||||
|    */ | ||||
|   void add(const Point2Vector& measurements, | ||||
|   void add(const MEASUREMENTS& measurements, | ||||
|            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, | ||||
|            const std::vector<double>& alphas, | ||||
|            const FastVector<size_t>& cameraIds = FastVector<size_t>()) { | ||||
|  | @ -330,12 +332,13 @@ class SmartProjectionPoseFactorRollingShutter | |||
|         const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; | ||||
|         auto body_P_cam = camera_i.pose(); | ||||
|         auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); | ||||
|         PinholeCamera<CALIBRATION> camera(w_P_cam, camera_i.calibration()); | ||||
|         typename Base::Camera camera( | ||||
|             w_P_cam, make_shared<typename CAMERA::CalibrationType>( | ||||
|                          camera_i.calibration())); | ||||
| 
 | ||||
|         // get jacobians and error vector for current measurement
 | ||||
|         Point2 reprojectionError_i = | ||||
|             Point2(camera.project(*this->result_, dProject_dPoseCam, Ei) - | ||||
|                    this->measured_.at(i)); | ||||
|         Point2 reprojectionError_i = camera.reprojectionError( | ||||
|             *this->result_, this->measured_.at(i), dProject_dPoseCam, Ei); | ||||
|         Eigen::Matrix<double, ZDim, DimBlock> J;  // 2 x 12
 | ||||
|         J.block(0, 0, ZDim, 6) = | ||||
|             dProject_dPoseCam * dPoseCam_dInterpPose * | ||||
|  | @ -403,7 +406,7 @@ class SmartProjectionPoseFactorRollingShutter | |||
|     for (size_t i = 0; i < Fs.size(); i++) | ||||
|       Fs[i] = this->noiseModel_->Whiten(Fs[i]); | ||||
| 
 | ||||
|     Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); | ||||
|     Matrix3 P = Cameras::PointCov(E, lambda, diagonalDamping); | ||||
| 
 | ||||
|     // Collect all the key pairs: these are the keys that correspond to the
 | ||||
|     // blocks in Fs (on which we apply the Schur Complement)
 | ||||
|  |  | |||
|  | @ -1317,10 +1317,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, | |||
| #ifndef DISABLE_TIMING | ||||
| #include <gtsam/base/timing.h> | ||||
| //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
 | ||||
| //|   -SF RS LINEARIZE: 0.09 CPU
 | ||||
| // (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0)
 | ||||
| //|   -RS LINEARIZE: 0.09 CPU
 | ||||
| // (10000 times, 0.068719 wall, 0.09 children, min: 0 max: 0)
 | ||||
| //|   -SF RS LINEARIZE: 0.14 CPU
 | ||||
| //(10000 times, 0.131202 wall, 0.14 children, min: 0 max: 0)
 | ||||
| //|   -RS LINEARIZE: 0.06 CPU
 | ||||
| //(10000 times, 0.066951 wall, 0.06 children, min: 0 max: 0)
 | ||||
| /* *************************************************************************/ | ||||
| TEST(SmartProjectionPoseFactorRollingShutter, timing) { | ||||
|   using namespace vanillaPose; | ||||
|  | @ -1384,6 +1384,105 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { | |||
| } | ||||
| #endif | ||||
| 
 | ||||
| #include <gtsam/geometry/SphericalCamera.h> | ||||
| /* ************************************************************************* */ | ||||
| // spherical Camera with rolling shutter effect
 | ||||
| namespace sphericalCameraRS { | ||||
| typedef SphericalCamera Camera; | ||||
| typedef CameraSet<Camera> Cameras; | ||||
| typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical; | ||||
| Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1); | ||||
| Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2); | ||||
| Pose3 interp_pose3 = interpolate<Pose3>(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) { | ||||
|   using namespace sphericalCameraRS; | ||||
|   std::vector<Unit3> measurements_lmk1, measurements_lmk2, measurements_lmk3; | ||||
| 
 | ||||
|   // Project three landmarks into three cameras
 | ||||
|   projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, | ||||
|                                    measurements_lmk1); | ||||
|   projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, | ||||
|                                    measurements_lmk2); | ||||
|   projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, | ||||
|                                    measurements_lmk3); | ||||
| 
 | ||||
|   // create inputs
 | ||||
|   std::vector<std::pair<Key, Key>> 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<double> interp_factors; | ||||
|   interp_factors.push_back(interp_factor1); | ||||
|   interp_factors.push_back(interp_factor2); | ||||
|   interp_factors.push_back(interp_factor3); | ||||
| 
 | ||||
|   SmartProjectionParams params( | ||||
|       gtsam::HESSIAN, | ||||
|       gtsam::ZERO_ON_DEGENERACY);  // only config that works with RS factors
 | ||||
|   params.setRankTolerance(0.1); | ||||
| 
 | ||||
|   boost::shared_ptr<Cameras> cameraRig(new Cameras()); | ||||
|   cameraRig->push_back(Camera(Pose3::identity(), emptyK)); | ||||
| 
 | ||||
|   SmartFactorRS_spherical::shared_ptr smartFactor1( | ||||
|       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)); | ||||
|   smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); | ||||
| 
 | ||||
|   SmartFactorRS_spherical::shared_ptr smartFactor3( | ||||
|       new SmartFactorRS_spherical(model, cameraRig, params)); | ||||
|   smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); | ||||
| 
 | ||||
|   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.push_back(smartFactor1); | ||||
|   graph.push_back(smartFactor2); | ||||
|   graph.push_back(smartFactor3); | ||||
|   graph.addPrior(x1, level_pose, noisePrior); | ||||
|   graph.addPrior(x2, pose_right, noisePrior); | ||||
| 
 | ||||
|   Values groundTruth; | ||||
|   groundTruth.insert(x1, level_pose); | ||||
|   groundTruth.insert(x2, pose_right); | ||||
|   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 / 100, 0., -M_PI / 100), | ||||
|                            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
 | ||||
|   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<Pose3>(x3))); | ||||
| 
 | ||||
|   Values result; | ||||
|   LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); | ||||
|   result = optimizer.optimize(); | ||||
|   EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue