Merge remote-tracking branch 'origin/feature/BAD_FixedJacobians' into feature/BAD
Conflicts: gtsam_unstable/nonlinear/tests/testBADFactor.cpprelease/4.3a0
						commit
						d38bcf1805
					
				|  | @ -48,7 +48,7 @@ public: | |||
|   virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = | ||||
|       boost::none) const { | ||||
|     SimpleCamera camera(pose, *K_); | ||||
|     Point2 reprojectionError(camera.project(P_, H) - p_); | ||||
|     Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_); | ||||
|     return reprojectionError.vector(); | ||||
|   } | ||||
| }; | ||||
|  |  | |||
|  | @ -37,10 +37,28 @@ namespace gtsam { | |||
| typedef Eigen::MatrixXd Matrix; | ||||
| typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor; | ||||
| 
 | ||||
| typedef Eigen::Matrix2d Matrix2; | ||||
| typedef Eigen::Matrix3d Matrix3; | ||||
| typedef Eigen::Matrix4d Matrix4; | ||||
| typedef Eigen::Matrix<double,5,5> Matrix5; | ||||
| typedef Eigen::Matrix<double,6,6> Matrix6; | ||||
| 
 | ||||
| typedef Eigen::Matrix<double,2,3> Matrix23; | ||||
| typedef Eigen::Matrix<double,2,4> Matrix24; | ||||
| typedef Eigen::Matrix<double,2,5> Matrix25; | ||||
| typedef Eigen::Matrix<double,2,6> Matrix26; | ||||
| typedef Eigen::Matrix<double,2,7> Matrix27; | ||||
| typedef Eigen::Matrix<double,2,8> Matrix28; | ||||
| typedef Eigen::Matrix<double,2,9> Matrix29; | ||||
| 
 | ||||
| typedef Eigen::Matrix<double,3,2> Matrix32; | ||||
| typedef Eigen::Matrix<double,3,4> Matrix34; | ||||
| typedef Eigen::Matrix<double,3,5> Matrix35; | ||||
| typedef Eigen::Matrix<double,3,6> Matrix36; | ||||
| typedef Eigen::Matrix<double,3,7> Matrix37; | ||||
| typedef Eigen::Matrix<double,3,8> Matrix38; | ||||
| typedef Eigen::Matrix<double,3,9> Matrix39; | ||||
| 
 | ||||
| // Matrix expressions for accessing parts of matrices
 | ||||
| typedef Eigen::Block<Matrix> SubMatrix; | ||||
| typedef Eigen::Block<const Matrix> ConstSubMatrix; | ||||
|  |  | |||
|  | @ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector; | |||
| // Commonly used fixed size vectors
 | ||||
| typedef Eigen::Vector2d Vector2; | ||||
| typedef Eigen::Vector3d Vector3; | ||||
| typedef Eigen::Matrix<double, 4, 1> Vector4; | ||||
| typedef Eigen::Matrix<double, 5, 1> Vector5; | ||||
| typedef Eigen::Matrix<double, 6, 1> Vector6; | ||||
| typedef Eigen::Matrix<double, 7, 1> Vector7; | ||||
| typedef Eigen::Matrix<double, 8, 1> Vector8; | ||||
| typedef Eigen::Matrix<double, 9, 1> Vector9; | ||||
| 
 | ||||
| typedef Eigen::VectorBlock<Vector> SubVector; | ||||
| typedef Eigen::VectorBlock<const Vector> ConstSubVector; | ||||
|  |  | |||
|  | @ -36,6 +36,8 @@ private: | |||
|   double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
 | ||||
| 
 | ||||
| public: | ||||
|   /// dimension of the variable - used to autodetect sizes
 | ||||
|   static const size_t dimension = 3; | ||||
| 
 | ||||
|   /// @name Standard Constructors
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -53,23 +53,23 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx, | ||||
| static Matrix29 D2dcalibration(double x, double y, double xx, | ||||
|     double yy, double xy, double rr, double r4, double pnx, double pny, | ||||
|     const Eigen::Matrix<double, 2, 2>& DK) { | ||||
|   Eigen::Matrix<double, 2, 5> DR1; | ||||
|     const Matrix2& DK) { | ||||
|   Matrix25 DR1; | ||||
|   DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; | ||||
|   Eigen::Matrix<double, 2, 4> DR2; | ||||
|   Matrix24 DR2; | ||||
|   DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
 | ||||
|          y * rr, y * r4, rr + 2 * yy, 2 * xy; | ||||
|   Eigen::Matrix<double, 2, 9> D; | ||||
|   Matrix29 D; | ||||
|   D << DR1, DK * DR2; | ||||
|   return D; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr, | ||||
| static Matrix2 D2dintrinsic(double x, double y, double rr, | ||||
|     double g, double k1, double k2, double p1, double p2, | ||||
|     const Eigen::Matrix<double, 2, 2>& DK) { | ||||
|     const Matrix2& DK) { | ||||
|   const double drdx = 2. * x; | ||||
|   const double drdy = 2. * y; | ||||
|   const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; | ||||
|  | @ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr, | |||
|   const double dDydx = 2. * p2 * y + p1 * drdx; | ||||
|   const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); | ||||
| 
 | ||||
|   Eigen::Matrix<double, 2, 2> DR; | ||||
|   Matrix2 DR; | ||||
|   DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
 | ||||
|         y * dgdx + dDydx, g + y * dgdy + dDydy; | ||||
| 
 | ||||
|  | @ -110,7 +110,7 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1, | |||
|   const double pnx = g * x + dx; | ||||
|   const double pny = g * y + dy; | ||||
| 
 | ||||
|   Eigen::Matrix<double, 2, 2> DK; | ||||
|   Matrix2 DK; | ||||
|   if (H1 || H2) DK << fx_, s_, 0.0, fy_; | ||||
| 
 | ||||
|   // Derivative for calibration
 | ||||
|  | @ -161,7 +161,7 @@ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { | |||
|   const double rr = xx + yy; | ||||
|   const double r4 = rr * rr; | ||||
|   const double g = (1 + k1_ * rr + k2_ * r4); | ||||
|   Eigen::Matrix<double, 2, 2> DK; | ||||
|   Matrix2 DK; | ||||
|   DK << fx_, s_, 0.0, fy_; | ||||
|   return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); | ||||
| } | ||||
|  | @ -176,7 +176,7 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { | |||
|   const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); | ||||
|   const double pnx = g * x + dx; | ||||
|   const double pny = g * y + dy; | ||||
|   Eigen::Matrix<double, 2, 2> DK; | ||||
|   Matrix2 DK; | ||||
|   DK << fx_, s_, 0.0, fy_; | ||||
|   return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); | ||||
| } | ||||
|  |  | |||
|  | @ -46,6 +46,9 @@ protected: | |||
|   double p1_, p2_ ; // tangential distortion
 | ||||
| 
 | ||||
| public: | ||||
|   /// dimension of the variable - used to autodetect sizes
 | ||||
|   static const size_t dimension = 9; | ||||
| 
 | ||||
|   Matrix K() const ; | ||||
|   Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } | ||||
|   Vector vector() const ; | ||||
|  |  | |||
|  | @ -50,8 +50,9 @@ private: | |||
|   double xi_;  // mirror parameter
 | ||||
| 
 | ||||
| public: | ||||
|   //Matrix K() const ;
 | ||||
|   //Eigen::Vector4d k() const { return Base::k(); }
 | ||||
|   /// dimension of the variable - used to autodetect sizes
 | ||||
|   static const size_t dimension = 10; | ||||
| 
 | ||||
|   Vector vector() const ; | ||||
| 
 | ||||
|   /// @name Standard Constructors
 | ||||
|  |  | |||
|  | @ -86,6 +86,21 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal, | |||
|   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal, | ||||
|     boost::optional<Matrix2&> Dp) const { | ||||
|   const double x = p.x(), y = p.y(); | ||||
|   if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; | ||||
|   if (Dp) *Dp << fx_, s_, 0.0, fy_; | ||||
|   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point2 Cal3_S2::uncalibrate(const Point2& p) const { | ||||
|   const double x = p.x(), y = p.y(); | ||||
|   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point2 Cal3_S2::calibrate(const Point2& p) const { | ||||
|   const double u = p.x(), v = p.y(); | ||||
|  |  | |||
|  | @ -36,6 +36,8 @@ private: | |||
|   double fx_, fy_, s_, u0_, v0_; | ||||
| 
 | ||||
| public: | ||||
|   /// dimension of the variable - used to autodetect sizes
 | ||||
|   static const size_t dimension = 5; | ||||
| 
 | ||||
|   typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
 | ||||
| 
 | ||||
|  | @ -144,12 +146,29 @@ public: | |||
|   /**
 | ||||
|    * convert intrinsic coordinates xy to image coordinates uv | ||||
|    * @param p point in intrinsic coordinates | ||||
|    * @return point in image coordinates | ||||
|    */ | ||||
|   Point2 uncalibrate(const Point2& p) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves | ||||
|    * @param p point in intrinsic coordinates | ||||
|    * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters | ||||
|    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates | ||||
|    * @return point in image coordinates | ||||
|    */ | ||||
|   Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal = | ||||
|       boost::none, boost::optional<Matrix&> Dp = boost::none) const; | ||||
|   Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal, | ||||
|       boost::optional<Matrix2&> Dp) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves | ||||
|    * @param p point in intrinsic coordinates | ||||
|    * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters | ||||
|    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates | ||||
|    * @return point in image coordinates | ||||
|    */ | ||||
|   Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal, | ||||
|       boost::optional<Matrix&> Dp) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * convert image coordinates uv to intrinsic coordinates xy | ||||
|  | @ -181,12 +200,12 @@ public: | |||
| 
 | ||||
|   /// return DOF, dimensionality of tangent space
 | ||||
|   inline size_t dim() const { | ||||
|     return 5; | ||||
|     return dimension; | ||||
|   } | ||||
| 
 | ||||
|   /// return DOF, dimensionality of tangent space
 | ||||
|   static size_t Dim() { | ||||
|     return 5; | ||||
|     return dimension; | ||||
|   } | ||||
| 
 | ||||
|   /// Given 5-dim tangent vector, create new calibration
 | ||||
|  |  | |||
|  | @ -270,17 +270,15 @@ public: | |||
|    * @param Dpoint is the 2*3 Jacobian w.r.t. P | ||||
|    */ | ||||
|   inline static Point2 project_to_camera(const Point3& P, | ||||
|       boost::optional<Matrix&> Dpoint = boost::none) { | ||||
|       boost::optional<Matrix23&> Dpoint = boost::none) { | ||||
| #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|     if (P.z() <= 0) | ||||
|       throw CheiralityException(); | ||||
| #endif | ||||
|     double d = 1.0 / P.z(); | ||||
|     const double u = P.x() * d, v = P.y() * d; | ||||
|     if (Dpoint) { | ||||
|       Dpoint->resize(2,3); | ||||
|     if (Dpoint) | ||||
|       *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; | ||||
|     } | ||||
|     return Point2(u, v); | ||||
|   } | ||||
| 
 | ||||
|  | @ -291,6 +289,22 @@ public: | |||
|     return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); | ||||
|   } | ||||
| 
 | ||||
|   /** project a point from world coordinate to the image
 | ||||
|    *  @param pw is a point in world coordinates | ||||
|    */ | ||||
|   inline Point2 project(const Point3& pw) const { | ||||
| 
 | ||||
|     // Transform to camera coordinates and check cheirality
 | ||||
|     const Point3 pc = pose_.transform_to(pw); | ||||
| 
 | ||||
|     // Project to normalized image coordinates
 | ||||
|     const Point2 pn = project_to_camera(pc); | ||||
| 
 | ||||
|     return K_.uncalibrate(pn); | ||||
|   } | ||||
| 
 | ||||
|   typedef Eigen::Matrix<double,2,Calibration::dimension> Matrix2K; | ||||
| 
 | ||||
|   /** project a point from world coordinate to the image
 | ||||
|    *  @param pw is a point in world coordinates | ||||
|    *  @param Dpose is the Jacobian w.r.t. pose3 | ||||
|  | @ -299,9 +313,44 @@ public: | |||
|    */ | ||||
|   inline Point2 project( | ||||
|       const Point3& pw, //
 | ||||
|       boost::optional<Matrix&> Dpose = boost::none, | ||||
|       boost::optional<Matrix&> Dpoint = boost::none, | ||||
|       boost::optional<Matrix&> Dcal = boost::none) const { | ||||
|       boost::optional<Matrix26&> Dpose, | ||||
|       boost::optional<Matrix23&> Dpoint, | ||||
|       boost::optional<Matrix2K&> Dcal) const { | ||||
| 
 | ||||
|     // Transform to camera coordinates and check cheirality
 | ||||
|     const Point3 pc = pose_.transform_to(pw); | ||||
| 
 | ||||
|     // Project to normalized image coordinates
 | ||||
|     const Point2 pn = project_to_camera(pc); | ||||
| 
 | ||||
|     if (Dpose || Dpoint) { | ||||
|       const double z = pc.z(), d = 1.0 / z; | ||||
| 
 | ||||
|       // uncalibration
 | ||||
|       Matrix2 Dpi_pn; | ||||
|       const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); | ||||
| 
 | ||||
|       // chain the Jacobian matrices
 | ||||
|       if (Dpose) | ||||
|         calculateDpose(pn, d, Dpi_pn, *Dpose); | ||||
|       if (Dpoint) | ||||
|         calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); | ||||
|       return pi; | ||||
|     } else | ||||
|       return K_.uncalibrate(pn, Dcal, boost::none); | ||||
|   } | ||||
| 
 | ||||
|   /** project a point from world coordinate to the image
 | ||||
|    *  @param pw is a point in world coordinates | ||||
|    *  @param Dpose is the Jacobian w.r.t. pose3 | ||||
|    *  @param Dpoint is the Jacobian w.r.t. point3 | ||||
|    *  @param Dcal is the Jacobian w.r.t. calibration | ||||
|    */ | ||||
|   inline Point2 project( | ||||
|       const Point3& pw, //
 | ||||
|       boost::optional<Matrix&> Dpose, | ||||
|       boost::optional<Matrix&> Dpoint, | ||||
|       boost::optional<Matrix&> Dcal) const { | ||||
| 
 | ||||
|     // Transform to camera coordinates and check cheirality
 | ||||
|     const Point3 pc = pose_.transform_to(pw); | ||||
|  | @ -327,7 +376,7 @@ public: | |||
|       } | ||||
|       return pi; | ||||
|     } else | ||||
|       return K_.uncalibrate(pn, Dcal); | ||||
|       return K_.uncalibrate(pn, Dcal, boost::none); | ||||
|   } | ||||
| 
 | ||||
|   /** project a point at infinity from world coordinate to the image
 | ||||
|  | @ -356,7 +405,7 @@ public: | |||
|     Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; | ||||
| 
 | ||||
|     // camera to normalized image coordinate
 | ||||
|     Matrix Dpn_pc; // 2*3
 | ||||
|     Matrix23 Dpn_pc; // 2*3
 | ||||
|     const Point2 pn = project_to_camera(pc, Dpn_pc); | ||||
| 
 | ||||
|     // uncalibration
 | ||||
|  | @ -391,12 +440,12 @@ public: | |||
|       const double z = pc.z(), d = 1.0 / z; | ||||
| 
 | ||||
|       // uncalibration
 | ||||
|       Matrix Dcal, Dpi_pn(2, 2); | ||||
|       Matrix Dcal, Dpi_pn(2,2); | ||||
|       const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); | ||||
| 
 | ||||
|       if (Dcamera) { | ||||
|         Dcamera->resize(2, this->dim()); | ||||
|         calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); | ||||
|         calculateDpose(pn, d, Dpi_pn.block<2,2>(0,0), Dcamera->leftCols<6>()); | ||||
|         Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
 | ||||
|       } | ||||
|       if (Dpoint) { | ||||
|  | @ -518,16 +567,16 @@ private: | |||
|    * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
 | ||||
|    */ | ||||
|   template<typename Derived> | ||||
|   static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, | ||||
|   static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn, | ||||
|       Eigen::MatrixBase<Derived> const & Dpose) { | ||||
|     // optimized version of derivatives, see CalibratedCamera.nb
 | ||||
|     const double u = pn.x(), v = pn.y(); | ||||
|     double uv = u * v, uu = u * u, vv = v * v; | ||||
|     Eigen::Matrix<double, 2, 6> Dpn_pose; | ||||
|     Matrix26 Dpn_pose; | ||||
|     Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; | ||||
|     assert(Dpose.rows()==2 && Dpose.cols()==6); | ||||
|     const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
 | ||||
|         Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; | ||||
|         Dpi_pn * Dpn_pose; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -539,18 +588,18 @@ private: | |||
|    * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
 | ||||
|    */ | ||||
|   template<typename Derived> | ||||
|   static void calculateDpoint(const Point2& pn, double d, const Matrix& R, | ||||
|       const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) { | ||||
|   static void calculateDpoint(const Point2& pn, double d, const Matrix3& R, | ||||
|       const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) { | ||||
|     // optimized version of derivatives, see CalibratedCamera.nb
 | ||||
|     const double u = pn.x(), v = pn.y(); | ||||
|     Eigen::Matrix<double, 2, 3> Dpn_point; | ||||
|     Matrix23 Dpn_point; | ||||
|     Dpn_point << //
 | ||||
|         R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
 | ||||
|     /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); | ||||
|     Dpn_point *= d; | ||||
|     assert(Dpoint.rows()==2 && Dpoint.cols()==3); | ||||
|     const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
 | ||||
|         Dpi_pn.block<2, 2>(0, 0) * Dpn_point; | ||||
|         Dpi_pn * Dpn_point; | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|  |  | |||
|  | @ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const { | |||
|   return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // see doc/math.lyx, SE(2) section
 | ||||
| Point2 Pose2::transform_to(const Point2& point) const { | ||||
|   Point2 d = point - t_; | ||||
|   return r_.unrotate(d); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // see doc/math.lyx, SE(2) section
 | ||||
| Point2 Pose2::transform_to(const Point2& point, | ||||
|     boost::optional<Matrix23&> H1, boost::optional<Matrix2&> H2) const { | ||||
|   Point2 d = point - t_; | ||||
|   Point2 q = r_.unrotate(d); | ||||
|   if (!H1 && !H2) return q; | ||||
|   if (H1) *H1 << | ||||
|       -1.0, 0.0,  q.y(), | ||||
|       0.0, -1.0, -q.x(); | ||||
|   if (H2) *H2 = r_.transpose(); | ||||
|   return q; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // see doc/math.lyx, SE(2) section
 | ||||
| Point2 Pose2::transform_to(const Point2& point, | ||||
|  | @ -161,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p, | |||
|   return q + t_; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose2 Pose2::between(const Pose2& p2) const { | ||||
|   // get cosines and sines from rotation matrices
 | ||||
|   const Rot2& R1 = r_, R2 = p2.r(); | ||||
|   double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); | ||||
| 
 | ||||
|   // Assert that R1 and R2 are normalized
 | ||||
|   assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); | ||||
| 
 | ||||
|   // Calculate delta rotation = between(R1,R2)
 | ||||
|   double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; | ||||
|   Rot2 R(Rot2::atan2(s,c)); // normalizes
 | ||||
| 
 | ||||
|   // Calculate delta translation = unrotate(R1, dt);
 | ||||
|   Point2 dt = p2.t() - t_; | ||||
|   double x = dt.x(), y = dt.y(); | ||||
|   // t = R1' * (t2-t1)
 | ||||
|   Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); | ||||
| 
 | ||||
|   return Pose2(R,t); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1, | ||||
|     boost::optional<Matrix3&> H2) const { | ||||
|   // get cosines and sines from rotation matrices
 | ||||
|   const Rot2& R1 = r_, R2 = p2.r(); | ||||
|   double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); | ||||
| 
 | ||||
|   // Assert that R1 and R2 are normalized
 | ||||
|   assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); | ||||
| 
 | ||||
|   // Calculate delta rotation = between(R1,R2)
 | ||||
|   double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; | ||||
|   Rot2 R(Rot2::atan2(s,c)); // normalizes
 | ||||
| 
 | ||||
|   // Calculate delta translation = unrotate(R1, dt);
 | ||||
|   Point2 dt = p2.t() - t_; | ||||
|   double x = dt.x(), y = dt.y(); | ||||
|   // t = R1' * (t2-t1)
 | ||||
|   Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); | ||||
| 
 | ||||
|   // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
 | ||||
|   if (H1) { | ||||
|     double dt1 = -s2 * x + c2 * y; | ||||
|     double dt2 = -c2 * x - s2 * y; | ||||
|     *H1 << | ||||
|         -c,  -s,  dt1, | ||||
|         s,  -c,  dt2, | ||||
|         0.0, 0.0,-1.0; | ||||
|   } | ||||
|   if (H2) *H2 = I3; | ||||
| 
 | ||||
|   return Pose2(R,t); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1, | ||||
|     boost::optional<Matrix&> H2) const { | ||||
|  |  | |||
|  | @ -123,10 +123,19 @@ public: | |||
|   /**
 | ||||
|    * Return relative pose between p1 and p2, in p1 coordinate frame | ||||
|    */ | ||||
|   Pose2 between(const Pose2& p2, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const; | ||||
|   Pose2 between(const Pose2& p2) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Return relative pose between p1 and p2, in p1 coordinate frame | ||||
|    */ | ||||
|   Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1, | ||||
|       boost::optional<Matrix3&> H2) const; | ||||
| 
 | ||||
|   /**
 | ||||
|    * Return relative pose between p1 and p2, in p1 coordinate frame | ||||
|    */ | ||||
|   Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1, | ||||
|       boost::optional<Matrix&> H2) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Manifold
 | ||||
|  | @ -182,10 +191,18 @@ public: | |||
|   /// @name Group Action on Point2
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /** Return point coordinates in pose coordinate frame */ | ||||
|   Point2 transform_to(const Point2& point) const; | ||||
| 
 | ||||
|   /** Return point coordinates in pose coordinate frame */ | ||||
|   Point2 transform_to(const Point2& point, | ||||
|       boost::optional<Matrix&> H1=boost::none, | ||||
|       boost::optional<Matrix&> H2=boost::none) const; | ||||
|       boost::optional<Matrix23&> H1, | ||||
|       boost::optional<Matrix2&> H2) const; | ||||
| 
 | ||||
|   /** Return point coordinates in pose coordinate frame */ | ||||
|   Point2 transform_to(const Point2& point, | ||||
|       boost::optional<Matrix&> H1, | ||||
|       boost::optional<Matrix&> H2) const; | ||||
| 
 | ||||
|   /** Return point coordinates in global frame */ | ||||
|   Point2 transform_from(const Point2& point, | ||||
|  |  | |||
|  | @ -254,16 +254,36 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose, | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose, | ||||
|     boost::optional<Matrix&> Dpoint) const { | ||||
| Point3 Pose3::transform_to(const Point3& p) const { | ||||
|   return R_.unrotate(p - t_); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose, | ||||
|     boost::optional<Matrix3&> Dpoint) const { | ||||
|   // Only get transpose once, to avoid multiple allocations,
 | ||||
|   // as well as multiple conversions in the Quaternion case
 | ||||
|   Matrix3 Rt(R_.transpose()); | ||||
|   const Point3 q(Rt*(p - t_).vector()); | ||||
|   if (Dpose) { | ||||
|     double wx = q.x(); | ||||
|     double wy = q.y(); | ||||
|     double wz = q.z(); | ||||
|     const double wx = q.x(), wy = q.y(), wz = q.z(); | ||||
|     (*Dpose) << | ||||
|         0.0, -wz, +wy,-1.0, 0.0, 0.0, | ||||
|         +wz, 0.0, -wx, 0.0,-1.0, 0.0, | ||||
|         -wy, +wx, 0.0, 0.0, 0.0,-1.0; | ||||
|   } | ||||
|   if (Dpoint) | ||||
|     *Dpoint = Rt; | ||||
|   return q; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose, | ||||
|     boost::optional<Matrix&> Dpoint) const { | ||||
|   Matrix3 Rt(R_.transpose()); | ||||
|   const Point3 q(Rt*(p - t_).vector()); | ||||
|   if (Dpose) { | ||||
|     const double wx = q.x(), wy = q.y(), wz = q.z(); | ||||
|     Dpose->resize(3, 6); | ||||
|     (*Dpose) << | ||||
|         0.0, -wz, +wy,-1.0, 0.0, 0.0, | ||||
|  |  | |||
|  | @ -250,8 +250,13 @@ public: | |||
|      * @param Dpoint optional 3*3 Jacobian wrpt point | ||||
|      * @return point in Pose coordinates | ||||
|      */ | ||||
|     Point3 transform_to(const Point3& p) const; | ||||
| 
 | ||||
|     Point3 transform_to(const Point3& p, | ||||
|         boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const; | ||||
|         boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const; | ||||
| 
 | ||||
|     Point3 transform_to(const Point3& p, | ||||
|         boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const; | ||||
| 
 | ||||
|     /// @}
 | ||||
|     /// @name Standard Interface
 | ||||
|  |  | |||
|  | @ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const { | |||
|   return rotate(p); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // see doc/math.lyx, SO(3) section
 | ||||
| Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1, | ||||
|     boost::optional<Matrix3&> H2) const { | ||||
|   Matrix3 Rt(transpose()); | ||||
|   Point3 q(Rt * p.vector()); // q = Rt*p
 | ||||
|   const double wx = q.x(), wy = q.y(), wz = q.z(); | ||||
|   if (H1) | ||||
|     *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; | ||||
|   if (H2) | ||||
|     *H2 = Rt; | ||||
|   return q; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // see doc/math.lyx, SO(3) section
 | ||||
| Point3 Rot3::unrotate(const Point3& p, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { | ||||
|   Point3 q(transpose()*p.vector()); // q = Rt*p
 | ||||
|   if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); | ||||
|   if (H2) *H2 = transpose(); | ||||
|   Matrix3 Rt(transpose()); | ||||
|   Point3 q(Rt * p.vector()); // q = Rt*p
 | ||||
|   const double wx = q.x(), wy = q.y(), wz = q.z(); | ||||
|   if (H1) { | ||||
|     H1->resize(3,3); | ||||
|     *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; | ||||
|   } | ||||
|   if (H2) | ||||
|     *H2 = Rt; | ||||
|   return q; | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -219,8 +219,15 @@ namespace gtsam { | |||
|     Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const; | ||||
| 
 | ||||
|     /// Compose two rotations i.e., R= (*this) * R2
 | ||||
|     Rot3 compose(const Rot3& R2, | ||||
|     boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const; | ||||
|     Rot3 compose(const Rot3& R2) const; | ||||
| 
 | ||||
|     /// Compose two rotations i.e., R= (*this) * R2
 | ||||
|     Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1, | ||||
|         boost::optional<Matrix3&> H2) const; | ||||
| 
 | ||||
|     /// Compose two rotations i.e., R= (*this) * R2
 | ||||
|     Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1, | ||||
|         boost::optional<Matrix&> H2) const; | ||||
| 
 | ||||
|     /** compose two rotations */ | ||||
|     Rot3 operator*(const Rot3& R2) const; | ||||
|  | @ -328,11 +335,16 @@ namespace gtsam { | |||
|     /// rotate point from rotated coordinate frame to world = R*p
 | ||||
|     Point3 operator*(const Point3& p) const; | ||||
| 
 | ||||
|     /**
 | ||||
|      * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ | ||||
|      */ | ||||
|     Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none, | ||||
|         boost::optional<Matrix&> H2 = boost::none) const; | ||||
|     /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
 | ||||
|     Point3 unrotate(const Point3& p) const; | ||||
| 
 | ||||
|     /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
 | ||||
|     Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1, | ||||
|         boost::optional<Matrix3&> H2) const; | ||||
| 
 | ||||
|     /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
 | ||||
|     Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1, | ||||
|         boost::optional<Matrix&> H2) const; | ||||
| 
 | ||||
|     /// @}
 | ||||
|     /// @name Group Action on Unit3
 | ||||
|  |  | |||
|  | @ -143,6 +143,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { | |||
|      -swy + C02,  swx + C12,    c + C22); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Rot3 Rot3::compose (const Rot3& R2) const { | ||||
|   return *this * R2; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Rot3 Rot3::compose (const Rot3& R2, | ||||
|     boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const { | ||||
|   if (H1) *H1 = R2.transpose(); | ||||
|   if (H2) *H2 = I3; | ||||
|   return *this * R2; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Rot3 Rot3::compose (const Rot3& R2, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { | ||||
|  | @ -301,6 +314,12 @@ Quaternion Rot3::toQuaternion() const { | |||
|   return Quaternion(rot_); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point3 Rot3::unrotate(const Point3& p) const { | ||||
|   // Eigen expression
 | ||||
|   return Point3(rot_.transpose()*p.vector()); // q = Rt*p
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -82,6 +82,19 @@ namespace gtsam { | |||
|   Rot3 Rot3::rodriguez(const Vector& w, double theta) { | ||||
|     return Quaternion(Eigen::AngleAxisd(theta, w)); } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Rot3 Rot3::compose(const Rot3& R2) const { | ||||
|     return Rot3(quaternion_ * R2.quaternion_); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Rot3 Rot3::compose(const Rot3& R2, | ||||
|   boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const { | ||||
|     if (H1) *H1 = R2.transpose(); | ||||
|     if (H2) *H2 = I3; | ||||
|     return Rot3(quaternion_ * R2.quaternion_); | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Rot3 Rot3::compose(const Rot3& R2, | ||||
|   boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { | ||||
|  | @ -158,6 +171,11 @@ namespace gtsam { | |||
|   /* ************************************************************************* */ | ||||
|   Quaternion Rot3::toQuaternion() const { return quaternion_; } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   Point3 Rot3::unrotate(const Point3& p) const { | ||||
|     return Point3(transpose()*p.vector()); // q = Rt*p
 | ||||
|   } | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -115,7 +115,7 @@ public: | |||
|   Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 = | ||||
|       boost::none) const { | ||||
|     try { | ||||
|       Point2 error(camera_.project(point, boost::none, H2) - measured_); | ||||
|       Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); | ||||
|       return error.vector(); | ||||
|     } catch (CheiralityException& e) { | ||||
|       if (H2) | ||||
|  | @ -155,7 +155,7 @@ public: | |||
| 
 | ||||
|     // Would be even better if we could pass blocks to project
 | ||||
|     const Point3& point = x.at<Point3>(key()); | ||||
|     b = -(camera_.project(point, boost::none, A) - measured_).vector(); | ||||
|     b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector(); | ||||
|     if (noiseModel_) | ||||
|       this->noiseModel_->WhitenSystem(A, b); | ||||
| 
 | ||||
|  |  | |||
|  | @ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) { | |||
| TEST( SimpleCamera, Dproject_point_pose) | ||||
| { | ||||
|   Matrix Dpose, Dpoint; | ||||
|   Point2 result = camera.project(point1, Dpose, Dpoint); | ||||
|   Point2 result = camera.project(point1, Dpose, Dpoint, boost::none); | ||||
|   Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1); | ||||
|   Matrix numerical_point = numericalDerivative22(project2, pose1, point1); | ||||
|   CHECK(assert_equal(result, Point2(-100,  100) )); | ||||
|  |  | |||
|  | @ -53,7 +53,7 @@ public: | |||
| 
 | ||||
|   static Point3 unrotate(const Rot2& R, const Point3& p, | ||||
|       boost::optional<Matrix&> HR = boost::none) { | ||||
|     Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); | ||||
|     Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none); | ||||
|     if (HR) { | ||||
|       // assign to temporary first to avoid error in Win-Debug mode
 | ||||
|       Matrix H = HR->col(2); | ||||
|  | @ -106,7 +106,7 @@ public: | |||
|   Vector evaluateError(const Rot3& nRb, | ||||
|       boost::optional<Matrix&> H = boost::none) const { | ||||
|     // measured bM = nRbÕ * nM + b
 | ||||
|     Point3 hx = nRb.unrotate(nM_, H) + bias_; | ||||
|     Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_; | ||||
|     return (hx - measured_).vector(); | ||||
|   } | ||||
| }; | ||||
|  |  | |||
|  | @ -174,12 +174,14 @@ public: | |||
|     } else { | ||||
| 
 | ||||
|       // Calculate derivatives. TODO if slow: optimize with Mathematica
 | ||||
|       //     3*2        3*3       3*3        2*3
 | ||||
|       Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; | ||||
|       //     3*2        3*3       3*3
 | ||||
|       Matrix D_1T2_dir, DdP2_rot, DP2_point; | ||||
| 
 | ||||
|       Point3 _1T2 = E.direction().point3(D_1T2_dir); | ||||
|       Point3 d1T2 = d * _1T2; | ||||
|       Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); | ||||
| 
 | ||||
|       Matrix23 Dpn_dP2; | ||||
|       pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); | ||||
| 
 | ||||
|       if (DE) { | ||||
|  |  | |||
|  | @ -29,7 +29,6 @@ public: | |||
| protected: | ||||
| 
 | ||||
|   typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
 | ||||
|   typedef Eigen::Matrix<double, 2, 3> Matrix23; | ||||
|   typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
 | ||||
|   typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
 | ||||
| 
 | ||||
|  | @ -203,7 +202,7 @@ public: | |||
| 
 | ||||
|       // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
 | ||||
|       //      static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
 | ||||
|       //      Eigen::Matrix<double, 2, 2> Q = //
 | ||||
|       //      Matrix2 Q = //
 | ||||
|       //          I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
 | ||||
|       //      blocks[j] = Fj.transpose() * Q * Fj;
 | ||||
|     } | ||||
|  |  | |||
|  | @ -132,17 +132,17 @@ namespace gtsam { | |||
|           if(H1) { | ||||
|             gtsam::Matrix H0; | ||||
|             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H2) - measured_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); | ||||
|             *H1 = *H1 * H0; | ||||
|             return reprojectionError.vector(); | ||||
|           } else { | ||||
|             PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H2) - measured_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); | ||||
|             return reprojectionError.vector(); | ||||
|           } | ||||
|         } else { | ||||
|           PinholeCamera<CALIBRATION> camera(pose, *K_); | ||||
|           Point2 reprojectionError(camera.project(point, H1, H2) - measured_); | ||||
|           Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_); | ||||
|           return reprojectionError.vector(); | ||||
|         } | ||||
|       } catch( CheiralityException& e) { | ||||
|  |  | |||
|  | @ -125,7 +125,7 @@ public: | |||
|     Vector pk_1 = muk_1 - 0.5*Pose3::adjointTranspose(-h_*xik_1, muk_1, D_adjThxik1_muk1); | ||||
| 
 | ||||
|     Matrix D_gravityBody_gk; | ||||
|     Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk); | ||||
|     Point3 gravityBody = gk.rotation().unrotate(Point3(0.0, 0.0, -9.81*m_), D_gravityBody_gk, boost::none); | ||||
|     Vector f_ext = (Vector(6) << 0.0, 0.0, 0.0, gravityBody.x(), gravityBody.y(), gravityBody.z()); | ||||
| 
 | ||||
|     Vector hx = pk - pk_1 - h_*Fu_ - h_*f_ext; | ||||
|  |  | |||
|  | @ -104,7 +104,7 @@ public: | |||
|     } | ||||
|     else { | ||||
|       gtsam::Matrix J2; | ||||
|       gtsam::Point2 uv= camera.project(landmark,H1, J2); | ||||
|       gtsam::Point2 uv= camera.project(landmark,H1, J2, boost::none); | ||||
|       if (H1) { | ||||
|         *H1 = (*H1) * gtsam::eye(6); | ||||
|       } | ||||
|  |  | |||
|  | @ -22,6 +22,7 @@ | |||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/base/Matrix.h> | ||||
| #include <boost/foreach.hpp> | ||||
| #include <boost/tuple/tuple.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -134,16 +135,25 @@ public: | |||
| }; | ||||
| 
 | ||||
| //-----------------------------------------------------------------------------
 | ||||
| template<class T> | ||||
| struct JacobianTrace { | ||||
|   T t; | ||||
|   T value() const { | ||||
|     return t; | ||||
|   virtual ~JacobianTrace() { | ||||
|   } | ||||
|   virtual void reverseAD(JacobianMap& jacobians) const = 0; | ||||
|   virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; | ||||
| //  template<class JacobianFT>
 | ||||
| //  void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
 | ||||
| }; | ||||
| 
 | ||||
| typedef JacobianTrace* TracePtr; | ||||
| 
 | ||||
| //template <class Derived>
 | ||||
| //struct TypedTrace {
 | ||||
| //  virtual void reverseAD(JacobianMap& jacobians) const = 0;
 | ||||
| //  virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
 | ||||
| ////  template<class JacobianFT>
 | ||||
| ////  void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
 | ||||
| //};
 | ||||
| 
 | ||||
| //-----------------------------------------------------------------------------
 | ||||
| /**
 | ||||
|  * Expression node. The superclass for objects that do the heavy lifting | ||||
|  | @ -175,8 +185,7 @@ public: | |||
|   virtual Augmented<T> forward(const Values& values) const = 0; | ||||
| 
 | ||||
|   /// Construct an execution trace for reverse AD
 | ||||
|   virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( | ||||
|       const Values& values) const = 0; | ||||
|   virtual T traceExecution(const Values& values, TracePtr& p) const = 0; | ||||
| }; | ||||
| 
 | ||||
| //-----------------------------------------------------------------------------
 | ||||
|  | @ -217,7 +226,7 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// Trace structure for reverse AD
 | ||||
|   struct Trace: public JacobianTrace<T> { | ||||
|   struct Trace: public JacobianTrace { | ||||
|     /// If the expression is just a constant, we do nothing
 | ||||
|     virtual void reverseAD(JacobianMap& jacobians) const { | ||||
|     } | ||||
|  | @ -227,11 +236,10 @@ public: | |||
|   }; | ||||
| 
 | ||||
|   /// Construct an execution trace for reverse AD
 | ||||
|   virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( | ||||
|       const Values& values) const { | ||||
|     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); | ||||
|     trace->t = constant_; | ||||
|     return trace; | ||||
|   virtual T traceExecution(const Values& values, TracePtr& p) const { | ||||
|     Trace* trace = new Trace(); | ||||
|     p = static_cast<TracePtr>(trace); | ||||
|     return constant_; | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  | @ -270,12 +278,11 @@ public: | |||
| 
 | ||||
|   /// Return value and derivatives
 | ||||
|   virtual Augmented<T> forward(const Values& values) const { | ||||
|     T t = value(values); | ||||
|     return Augmented<T>(t, key_); | ||||
|     return Augmented<T>(values.at<T>(key_), key_); | ||||
|   } | ||||
| 
 | ||||
|   /// Trace structure for reverse AD
 | ||||
|   struct Trace: public JacobianTrace<T> { | ||||
|   struct Trace: public JacobianTrace { | ||||
|     Key key; | ||||
|     /// If the expression is just a leaf, we just insert an identity matrix
 | ||||
|     virtual void reverseAD(JacobianMap& jacobians) const { | ||||
|  | @ -293,12 +300,11 @@ public: | |||
|   }; | ||||
| 
 | ||||
|   /// Construct an execution trace for reverse AD
 | ||||
|   virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( | ||||
|       const Values& values) const { | ||||
|     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); | ||||
|     trace->t = value(values); | ||||
|   virtual T traceExecution(const Values& values, TracePtr& p) const { | ||||
|     Trace* trace = new Trace(); | ||||
|     p = static_cast<TracePtr>(trace); | ||||
|     trace->key = key_; | ||||
|     return trace; | ||||
|     return values.at<T>(key_); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
|  | @ -310,7 +316,8 @@ class UnaryExpression: public ExpressionNode<T> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   typedef boost::function<T(const A&, boost::optional<Matrix&>)> Function; | ||||
|   typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA; | ||||
|   typedef boost::function<T(const A&, boost::optional<JacobianTA&>)> Function; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|  | @ -344,33 +351,35 @@ public: | |||
|   virtual Augmented<T> forward(const Values& values) const { | ||||
|     using boost::none; | ||||
|     Augmented<A> argument = this->expressionA_->forward(values); | ||||
|     Matrix dTdA; | ||||
|     JacobianTA dTdA; | ||||
|     T t = function_(argument.value(), | ||||
|         argument.constant() ? none : boost::optional<Matrix&>(dTdA)); | ||||
|         argument.constant() ? none : boost::optional<JacobianTA&>(dTdA)); | ||||
|     return Augmented<T>(t, dTdA, argument.jacobians()); | ||||
|   } | ||||
| 
 | ||||
|   /// Trace structure for reverse AD
 | ||||
|   struct Trace: public JacobianTrace<T> { | ||||
|     boost::shared_ptr<JacobianTrace<A> > trace1; | ||||
|     Matrix dTdA; | ||||
|   struct Trace: public JacobianTrace { | ||||
|     TracePtr trace; | ||||
|     JacobianTA dTdA; | ||||
|     virtual ~Trace() { | ||||
|       delete trace; | ||||
|     } | ||||
|     /// Start the reverse AD process
 | ||||
|     virtual void reverseAD(JacobianMap& jacobians) const { | ||||
|       trace1->reverseAD(dTdA, jacobians); | ||||
|       trace->reverseAD(dTdA, jacobians); | ||||
|     } | ||||
|     /// Given df/dT, multiply in dT/dA and continue reverse AD process
 | ||||
|     virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { | ||||
|       trace1->reverseAD(dFdT * dTdA, jacobians); | ||||
|       trace->reverseAD(dFdT * dTdA, jacobians); | ||||
|     } | ||||
|   }; | ||||
| 
 | ||||
|   /// Construct an execution trace for reverse AD
 | ||||
|   virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( | ||||
|       const Values& values) const { | ||||
|     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); | ||||
|     trace->trace1 = this->expressionA_->traceExecution(values); | ||||
|     trace->t = function_(trace->trace1->value(), trace->dTdA); | ||||
|     return trace; | ||||
|   virtual T traceExecution(const Values& values, TracePtr& p) const { | ||||
|     Trace* trace = new Trace(); | ||||
|     p = static_cast<TracePtr>(trace); | ||||
|     A a = this->expressionA_->traceExecution(values,trace->trace); | ||||
|     return function_(a, trace->dTdA); | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  | @ -382,9 +391,11 @@ class BinaryExpression: public ExpressionNode<T> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1; | ||||
|   typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2; | ||||
|   typedef boost::function< | ||||
|       T(const A1&, const A2&, boost::optional<Matrix&>, | ||||
|           boost::optional<Matrix&>)> Function; | ||||
|       T(const A1&, const A2&, boost::optional<JacobianTA1&>, | ||||
|           boost::optional<JacobianTA2&>)> Function; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|  | @ -426,18 +437,23 @@ public: | |||
|     using boost::none; | ||||
|     Augmented<A1> a1 = this->expressionA1_->forward(values); | ||||
|     Augmented<A2> a2 = this->expressionA2_->forward(values); | ||||
|     Matrix dTdA1, dTdA2; | ||||
|     JacobianTA1 dTdA1; | ||||
|     JacobianTA2 dTdA2; | ||||
|     T t = function_(a1.value(), a2.value(), | ||||
|         a1.constant() ? none : boost::optional<Matrix&>(dTdA1), | ||||
|         a2.constant() ? none : boost::optional<Matrix&>(dTdA2)); | ||||
|         a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1), | ||||
|         a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2)); | ||||
|     return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); | ||||
|   } | ||||
| 
 | ||||
|   /// Trace structure for reverse AD
 | ||||
|   struct Trace: public JacobianTrace<T> { | ||||
|     boost::shared_ptr<JacobianTrace<A1> > trace1; | ||||
|     boost::shared_ptr<JacobianTrace<A2> > trace2; | ||||
|     Matrix dTdA1, dTdA2; | ||||
|   struct Trace: public JacobianTrace { | ||||
|     TracePtr trace1, trace2; | ||||
|     JacobianTA1 dTdA1; | ||||
|     JacobianTA2 dTdA2; | ||||
|     virtual ~Trace() { | ||||
|       delete trace1; | ||||
|       delete trace2; | ||||
|     } | ||||
|     /// Start the reverse AD process
 | ||||
|     virtual void reverseAD(JacobianMap& jacobians) const { | ||||
|       trace1->reverseAD(dTdA1, jacobians); | ||||
|  | @ -451,14 +467,12 @@ public: | |||
|   }; | ||||
| 
 | ||||
|   /// Construct an execution trace for reverse AD
 | ||||
|   virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( | ||||
|       const Values& values) const { | ||||
|     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); | ||||
|     trace->trace1 = this->expressionA1_->traceExecution(values); | ||||
|     trace->trace2 = this->expressionA2_->traceExecution(values); | ||||
|     trace->t = function_(trace->trace1->value(), trace->trace2->value(), | ||||
|         trace->dTdA1, trace->dTdA2); | ||||
|     return trace; | ||||
|   virtual T traceExecution(const Values& values, TracePtr& p) const { | ||||
|     Trace* trace = new Trace(); | ||||
|     p = static_cast<TracePtr>(trace); | ||||
|     A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1); | ||||
|     A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2); | ||||
|     return function_(a1, a2, trace->dTdA1, trace->dTdA2); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
|  | @ -471,9 +485,12 @@ class TernaryExpression: public ExpressionNode<T> { | |||
| 
 | ||||
| public: | ||||
| 
 | ||||
|   typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1; | ||||
|   typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2; | ||||
|   typedef Eigen::Matrix<double, T::dimension, A3::dimension> JacobianTA3; | ||||
|   typedef boost::function< | ||||
|       T(const A1&, const A2&, const A3&, boost::optional<Matrix&>, | ||||
|           boost::optional<Matrix&>, boost::optional<Matrix&>)> Function; | ||||
|       T(const A1&, const A2&, const A3&, boost::optional<JacobianTA1&>, | ||||
|           boost::optional<JacobianTA2&>, boost::optional<JacobianTA3&>)> Function; | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
|  | @ -523,21 +540,28 @@ public: | |||
|     Augmented<A1> a1 = this->expressionA1_->forward(values); | ||||
|     Augmented<A2> a2 = this->expressionA2_->forward(values); | ||||
|     Augmented<A3> a3 = this->expressionA3_->forward(values); | ||||
|     Matrix dTdA1, dTdA2, dTdA3; | ||||
|     JacobianTA1 dTdA1; | ||||
|     JacobianTA2 dTdA2; | ||||
|     JacobianTA3 dTdA3; | ||||
|     T t = function_(a1.value(), a2.value(), a3.value(), | ||||
|         a1.constant() ? none : boost::optional<Matrix&>(dTdA1), | ||||
|         a2.constant() ? none : boost::optional<Matrix&>(dTdA2), | ||||
|         a3.constant() ? none : boost::optional<Matrix&>(dTdA3)); | ||||
|         a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1), | ||||
|         a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2), | ||||
|         a3.constant() ? none : boost::optional<JacobianTA3&>(dTdA3)); | ||||
|     return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3, | ||||
|         a3.jacobians()); | ||||
|   } | ||||
| 
 | ||||
|   /// Trace structure for reverse AD
 | ||||
|   struct Trace: public JacobianTrace<T> { | ||||
|     boost::shared_ptr<JacobianTrace<A1> > trace1; | ||||
|     boost::shared_ptr<JacobianTrace<A2> > trace2; | ||||
|     boost::shared_ptr<JacobianTrace<A3> > trace3; | ||||
|     Matrix dTdA1, dTdA2, dTdA3; | ||||
|   struct Trace: public JacobianTrace { | ||||
|     TracePtr trace1, trace2, trace3; | ||||
|     JacobianTA1 dTdA1; | ||||
|     JacobianTA2 dTdA2; | ||||
|     JacobianTA3 dTdA3; | ||||
|     virtual ~Trace() { | ||||
|       delete trace1; | ||||
|       delete trace2; | ||||
|       delete trace3; | ||||
|     } | ||||
|     /// Start the reverse AD process
 | ||||
|     virtual void reverseAD(JacobianMap& jacobians) const { | ||||
|       trace1->reverseAD(dTdA1, jacobians); | ||||
|  | @ -553,15 +577,13 @@ public: | |||
|   }; | ||||
| 
 | ||||
|   /// Construct an execution trace for reverse AD
 | ||||
|   virtual boost::shared_ptr<JacobianTrace<T> > traceExecution( | ||||
|       const Values& values) const { | ||||
|     boost::shared_ptr<Trace> trace = boost::make_shared<Trace>(); | ||||
|     trace->trace1 = this->expressionA1_->traceExecution(values); | ||||
|     trace->trace2 = this->expressionA2_->traceExecution(values); | ||||
|     trace->trace3 = this->expressionA3_->traceExecution(values); | ||||
|     trace->t = function_(trace->trace1->value(), trace->trace2->value(), | ||||
|         trace->trace3->value(), trace->dTdA1, trace->dTdA2, trace->dTdA3); | ||||
|     return trace; | ||||
|   virtual T traceExecution(const Values& values, TracePtr& p) const { | ||||
|     Trace* trace = new Trace(); | ||||
|     p = static_cast<TracePtr>(trace); | ||||
|     A1 a1 = this->expressionA1_->traceExecution(values,trace->trace1); | ||||
|     A2 a2 = this->expressionA2_->traceExecution(values,trace->trace2); | ||||
|     A3 a3 = this->expressionA3_->traceExecution(values,trace->trace3); | ||||
|     return function_(a1, a2, a3, trace->dTdA1, trace->dTdA2, trace->dTdA3); | ||||
|   } | ||||
| 
 | ||||
| }; | ||||
|  |  | |||
|  | @ -76,8 +76,10 @@ public: | |||
|   /// Construct a unary method expression
 | ||||
|   template<typename A1, typename A2> | ||||
|   Expression(const Expression<A1>& expression1, | ||||
|       T (A1::*method)(const A2&, boost::optional<Matrix&>, | ||||
|           boost::optional<Matrix&>) const, const Expression<A2>& expression2) { | ||||
|       T (A1::*method)(const A2&, | ||||
|           boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>, | ||||
|           boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const, | ||||
|       const Expression<A2>& expression2) { | ||||
|     root_.reset( | ||||
|         new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4), | ||||
|             expression1, expression2)); | ||||
|  | @ -94,9 +96,11 @@ public: | |||
|   /// Construct a ternary function expression
 | ||||
|   template<typename A1, typename A2, typename A3> | ||||
|   Expression(typename TernaryExpression<T, A1, A2, A3>::Function function, | ||||
|       const Expression<A1>& expression1, const Expression<A2>& expression2,  const Expression<A3>& expression3) { | ||||
|       const Expression<A1>& expression1, const Expression<A2>& expression2, | ||||
|       const Expression<A3>& expression3) { | ||||
|     root_.reset( | ||||
|         new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2, expression3)); | ||||
|         new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2, | ||||
|             expression3)); | ||||
|   } | ||||
| 
 | ||||
|   /// Return keys that play in this expression
 | ||||
|  | @ -113,9 +117,11 @@ public: | |||
|   Augmented<T> augmented(const Values& values) const { | ||||
| #define REVERSE_AD | ||||
| #ifdef REVERSE_AD | ||||
|     boost::shared_ptr<JacobianTrace<T> > trace(root_->traceExecution(values)); | ||||
|     Augmented<T> augmented(trace->value()); | ||||
|     TracePtr trace; | ||||
|     T value = root_->traceExecution(values,trace); | ||||
|     Augmented<T> augmented(value); | ||||
|     trace->reverseAD(augmented.jacobians()); | ||||
|     delete trace; | ||||
|     return augmented; | ||||
| #else | ||||
|     return root_->forward(values); | ||||
|  | @ -132,8 +138,9 @@ public: | |||
| template<class T> | ||||
| struct apply_compose { | ||||
|   typedef T result_type; | ||||
|   T operator()(const T& x, const T& y, boost::optional<Matrix&> H1, | ||||
|       boost::optional<Matrix&> H2) const { | ||||
|   typedef Eigen::Matrix<double, T::dimension, T::dimension> Jacobian; | ||||
|   T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1, | ||||
|       boost::optional<Jacobian&> H2) const { | ||||
|     return x.compose(y, H1, H2); | ||||
|   } | ||||
| }; | ||||
|  |  | |||
|  | @ -58,208 +58,217 @@ TEST(BADFactor, test) { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  // Unary(Binary(Leaf,Leaf))
 | ||||
|  TEST(BADFactor, test1) { | ||||
| // Unary(Binary(Leaf,Leaf))
 | ||||
| TEST(BADFactor, test1) { | ||||
| 
 | ||||
|  // Create some values
 | ||||
|  Values values; | ||||
|  values.insert(1, Pose3()); | ||||
|  values.insert(2, Point3(0, 0, 1)); | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Pose3()); | ||||
|   values.insert(2, Point3(0, 0, 1)); | ||||
| 
 | ||||
|  // Create old-style factor to create expected value and derivatives
 | ||||
|  GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2, | ||||
|  boost::make_shared<Cal3_S2>()); | ||||
|  double expected_error = old.error(values); | ||||
|  GaussianFactor::shared_ptr expected = old.linearize(values); | ||||
|   // Create old-style factor to create expected value and derivatives
 | ||||
|   GenericProjectionFactor<Pose3, Point3> old(measured, model, 1, 2, | ||||
|       boost::make_shared<Cal3_S2>()); | ||||
|   double expected_error = old.error(values); | ||||
|   GaussianFactor::shared_ptr expected = old.linearize(values); | ||||
| 
 | ||||
|  // Create leaves
 | ||||
|  Pose3_ x(1); | ||||
|  Point3_ p(2); | ||||
|   // Create leaves
 | ||||
|   Pose3_ x(1); | ||||
|   Point3_ p(2); | ||||
| 
 | ||||
|  // Try concise version
 | ||||
|  BADFactor<Point2> f2(model, measured, project(transform_to(x, p))); | ||||
|  EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); | ||||
|  EXPECT_LONGS_EQUAL(2, f2.dim()); | ||||
|  boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); | ||||
|  EXPECT( assert_equal(*expected, *gf2, 1e-9)); | ||||
|  } | ||||
|   // Try concise version
 | ||||
|   BADFactor<Point2> f2(model, measured, project(transform_to(x, p))); | ||||
|   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); | ||||
|   EXPECT_LONGS_EQUAL(2, f2.dim()); | ||||
|   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); | ||||
|   EXPECT( assert_equal(*expected, *gf2, 1e-9)); | ||||
| } | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
|  // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
 | ||||
|  TEST(BADFactor, test2) { | ||||
| /* ************************************************************************* */ | ||||
| // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
 | ||||
| TEST(BADFactor, test2) { | ||||
| 
 | ||||
|  // Create some values
 | ||||
|  Values values; | ||||
|  values.insert(1, Pose3()); | ||||
|  values.insert(2, Point3(0, 0, 1)); | ||||
|  values.insert(3, Cal3_S2()); | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Pose3()); | ||||
|   values.insert(2, Point3(0, 0, 1)); | ||||
|   values.insert(3, Cal3_S2()); | ||||
| 
 | ||||
|  // Create old-style factor to create expected value and derivatives
 | ||||
|  GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3); | ||||
|  double expected_error = old.error(values); | ||||
|  GaussianFactor::shared_ptr expected = old.linearize(values); | ||||
|   // Create old-style factor to create expected value and derivatives
 | ||||
|   GeneralSFMFactor2<Cal3_S2> old(measured, model, 1, 2, 3); | ||||
|   double expected_error = old.error(values); | ||||
|   GaussianFactor::shared_ptr expected = old.linearize(values); | ||||
| 
 | ||||
|  // Create leaves
 | ||||
|  Pose3_ x(1); | ||||
|  Point3_ p(2); | ||||
|  Cal3_S2_ K(3); | ||||
|   // Create leaves
 | ||||
|   Pose3_ x(1); | ||||
|   Point3_ p(2); | ||||
|   Cal3_S2_ K(3); | ||||
| 
 | ||||
|  // Create expression tree
 | ||||
|  Point3_ p_cam(x, &Pose3::transform_to, p); | ||||
|  Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam); | ||||
|  Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); | ||||
|   // Create expression tree
 | ||||
|   Point3_ p_cam(x, &Pose3::transform_to, p); | ||||
|   Point2_ xy_hat(PinholeCamera<Cal3_S2>::project_to_camera, p_cam); | ||||
|   Point2_ uv_hat(K, &Cal3_S2::uncalibrate, xy_hat); | ||||
| 
 | ||||
|  // Create factor and check value, dimension, linearization
 | ||||
|  BADFactor<Point2> f(model, measured, uv_hat); | ||||
|  EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); | ||||
|  EXPECT_LONGS_EQUAL(2, f.dim()); | ||||
|  boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|  EXPECT( assert_equal(*expected, *gf, 1e-9)); | ||||
|   // Create factor and check value, dimension, linearization
 | ||||
|   BADFactor<Point2> f(model, measured, uv_hat); | ||||
|   EXPECT_DOUBLES_EQUAL(expected_error, f.error(values), 1e-9); | ||||
|   EXPECT_LONGS_EQUAL(2, f.dim()); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   EXPECT( assert_equal(*expected, *gf, 1e-9)); | ||||
| 
 | ||||
|  // Try concise version
 | ||||
|  BADFactor<Point2> f2(model, measured, | ||||
|  uncalibrate(K, project(transform_to(x, p)))); | ||||
|  EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); | ||||
|  EXPECT_LONGS_EQUAL(2, f2.dim()); | ||||
|  boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); | ||||
|  EXPECT( assert_equal(*expected, *gf2, 1e-9)); | ||||
|  } | ||||
|   // Try concise version
 | ||||
|   BADFactor<Point2> f2(model, measured, | ||||
|       uncalibrate(K, project(transform_to(x, p)))); | ||||
|   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); | ||||
|   EXPECT_LONGS_EQUAL(2, f2.dim()); | ||||
|   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values); | ||||
|   EXPECT( assert_equal(*expected, *gf2, 1e-9)); | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
|   TernaryExpression<Point2, Pose3, Point3, Cal3_S2>::Function fff = project6; | ||||
| 
 | ||||
|  TEST(BADFactor, compose1) { | ||||
|   // Try ternary version
 | ||||
|   BADFactor<Point2> f3(model, measured, project3(x, p, K)); | ||||
|   EXPECT_DOUBLES_EQUAL(expected_error, f3.error(values), 1e-9); | ||||
|   EXPECT_LONGS_EQUAL(2, f3.dim()); | ||||
|   boost::shared_ptr<GaussianFactor> gf3 = f3.linearize(values); | ||||
|   EXPECT( assert_equal(*expected, *gf3, 1e-9)); | ||||
| } | ||||
| 
 | ||||
|  // Create expression
 | ||||
|  Rot3_ R1(1), R2(2); | ||||
|  Rot3_ R3 = R1 * R2; | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  // Create factor
 | ||||
|  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| TEST(BADFactor, compose1) { | ||||
| 
 | ||||
|  // Create some values
 | ||||
|  Values values; | ||||
|  values.insert(1, Rot3()); | ||||
|  values.insert(2, Rot3()); | ||||
|   // Create expression
 | ||||
|   Rot3_ R1(1), R2(2); | ||||
|   Rot3_ R3 = R1 * R2; | ||||
| 
 | ||||
|  // Check unwhitenedError
 | ||||
|  std::vector<Matrix> H(2); | ||||
|  Vector actual = f.unwhitenedError(values, H); | ||||
|  EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|  EXPECT( assert_equal(eye(3), H[1],1e-9)); | ||||
|   // Create factor
 | ||||
|   BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| 
 | ||||
|  // Check linearization
 | ||||
|  JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); | ||||
|  boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|  boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|  EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
|  } | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Rot3()); | ||||
|   values.insert(2, Rot3()); | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
|  // Test compose with arguments referring to the same rotation
 | ||||
|  TEST(BADFactor, compose2) { | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(2); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|   EXPECT( assert_equal(eye(3), H[1],1e-9)); | ||||
| 
 | ||||
|  // Create expression
 | ||||
|  Rot3_ R1(1), R2(1); | ||||
|  Rot3_ R3 = R1 * R2; | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
|  // Create factor
 | ||||
|  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| /* ************************************************************************* */ | ||||
| // Test compose with arguments referring to the same rotation
 | ||||
| TEST(BADFactor, compose2) { | ||||
| 
 | ||||
|  // Create some values
 | ||||
|  Values values; | ||||
|  values.insert(1, Rot3()); | ||||
|   // Create expression
 | ||||
|   Rot3_ R1(1), R2(1); | ||||
|   Rot3_ R3 = R1 * R2; | ||||
| 
 | ||||
|  // Check unwhitenedError
 | ||||
|  std::vector<Matrix> H(1); | ||||
|  Vector actual = f.unwhitenedError(values, H); | ||||
|  EXPECT_LONGS_EQUAL(1, H.size()); | ||||
|  EXPECT( assert_equal(2*eye(3), H[0],1e-9)); | ||||
|   // Create factor
 | ||||
|   BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| 
 | ||||
|  // Check linearization
 | ||||
|  JacobianFactor expected(1, 2 * eye(3), zero(3)); | ||||
|  boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|  boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|  EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
|  } | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Rot3()); | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
|  // Test compose with one arguments referring to a constant same rotation
 | ||||
|  TEST(BADFactor, compose3) { | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(1); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT_LONGS_EQUAL(1, H.size()); | ||||
|   EXPECT( assert_equal(2*eye(3), H[0],1e-9)); | ||||
| 
 | ||||
|  // Create expression
 | ||||
|  Rot3_ R1(Rot3::identity()), R2(3); | ||||
|  Rot3_ R3 = R1 * R2; | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(1, 2 * eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
|  // Create factor
 | ||||
|  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| /* ************************************************************************* */ | ||||
| // Test compose with one arguments referring to a constant same rotation
 | ||||
| TEST(BADFactor, compose3) { | ||||
| 
 | ||||
|  // Create some values
 | ||||
|  Values values; | ||||
|  values.insert(3, Rot3()); | ||||
|   // Create expression
 | ||||
|   Rot3_ R1(Rot3::identity()), R2(3); | ||||
|   Rot3_ R3 = R1 * R2; | ||||
| 
 | ||||
|  // Check unwhitenedError
 | ||||
|  std::vector<Matrix> H(1); | ||||
|  Vector actual = f.unwhitenedError(values, H); | ||||
|  EXPECT_LONGS_EQUAL(1, H.size()); | ||||
|  EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|   // Create factor
 | ||||
|   BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| 
 | ||||
|  // Check linearization
 | ||||
|  JacobianFactor expected(3, eye(3), zero(3)); | ||||
|  boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|  boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|  EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
|  } | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(3, Rot3()); | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
|  // Test compose with three arguments
 | ||||
|  Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, | ||||
|      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|      boost::optional<Matrix&> H3) { | ||||
|    // return dummy derivatives (not correct, but that's ok for testing here)
 | ||||
|    if (H1) | ||||
|      *H1 = eye(3); | ||||
|    if (H2) | ||||
|      *H2 = eye(3); | ||||
|    if (H3) | ||||
|      *H3 = eye(3); | ||||
|    return R1 * (R2 * R3); | ||||
|  } | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(1); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT_LONGS_EQUAL(1, H.size()); | ||||
|   EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
| 
 | ||||
|  TEST(BADFactor, composeTernary) { | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(3, eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
|  // Create expression
 | ||||
|  Rot3_ A(1), B(2), C(3); | ||||
|  Rot3_ ABC(composeThree, A, B, C); | ||||
| /* ************************************************************************* */ | ||||
| // Test compose with three arguments
 | ||||
| Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, | ||||
|     boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2, | ||||
|     boost::optional<Matrix3&> H3) { | ||||
|   // return dummy derivatives (not correct, but that's ok for testing here)
 | ||||
|   if (H1) | ||||
|     *H1 = eye(3); | ||||
|   if (H2) | ||||
|     *H2 = eye(3); | ||||
|   if (H3) | ||||
|     *H3 = eye(3); | ||||
|   return R1 * (R2 * R3); | ||||
| } | ||||
| 
 | ||||
|  // Create factor
 | ||||
|  BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC); | ||||
| TEST(BADFactor, composeTernary) { | ||||
| 
 | ||||
|  // Create some values
 | ||||
|  Values values; | ||||
|  values.insert(1, Rot3()); | ||||
|  values.insert(2, Rot3()); | ||||
|  values.insert(3, Rot3()); | ||||
|   // Create expression
 | ||||
|   Rot3_ A(1), B(2), C(3); | ||||
|   Rot3_ ABC(composeThree, A, B, C); | ||||
| 
 | ||||
|  // Check unwhitenedError
 | ||||
|  std::vector<Matrix> H(3); | ||||
|  Vector actual = f.unwhitenedError(values, H); | ||||
|  EXPECT_LONGS_EQUAL(3, H.size()); | ||||
|  EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|  EXPECT( assert_equal(eye(3), H[1],1e-9)); | ||||
|  EXPECT( assert_equal(eye(3), H[2],1e-9)); | ||||
|   // Create factor
 | ||||
|   BADFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC); | ||||
| 
 | ||||
|  // Check linearization
 | ||||
|  JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); | ||||
|  boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|  boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|  EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
|  } | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Rot3()); | ||||
|   values.insert(2, Rot3()); | ||||
|   values.insert(3, Rot3()); | ||||
| 
 | ||||
|  /* ************************************************************************* */ | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(3); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT_LONGS_EQUAL(3, H.size()); | ||||
|   EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|   EXPECT( assert_equal(eye(3), H[1],1e-9)); | ||||
|   EXPECT( assert_equal(eye(3), H[2],1e-9)); | ||||
| 
 | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
|   TestResult tr; | ||||
|   return TestRegistry::runAllTests(tr); | ||||
|  |  | |||
|  | @ -32,12 +32,12 @@ using namespace gtsam; | |||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| template<class CAL> | ||||
| Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal, | ||||
|     boost::optional<Matrix&> Dp) { | ||||
| Point2 uncalibrate(const CAL& K, const Point2& p, | ||||
|     boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) { | ||||
|   return K.uncalibrate(p, Dcal, Dp); | ||||
| } | ||||
| 
 | ||||
| static const Rot3 someR = Rot3::RzRyRx(1,2,3); | ||||
| static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  | @ -55,7 +55,7 @@ TEST(Expression, constant) { | |||
| TEST(Expression, leaf) { | ||||
|   Expression<Rot3> R(100); | ||||
|   Values values; | ||||
|   values.insert(100,someR); | ||||
|   values.insert(100, someR); | ||||
|   Augmented<Rot3> a = R.augmented(values); | ||||
|   EXPECT(assert_equal(someR, a.value())); | ||||
|   JacobianMap expected; | ||||
|  | @ -76,7 +76,6 @@ TEST(Expression, leaf) { | |||
| //  expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50));
 | ||||
| //  EXPECT(assert_equal(expected.at(67),a.jacobians().at(67)));
 | ||||
| //}
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| TEST(Expression, test) { | ||||
|  | @ -149,8 +148,8 @@ TEST(Expression, compose3) { | |||
| /* ************************************************************************* */ | ||||
| // Test with ternary function
 | ||||
| Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3) { | ||||
|     boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2, | ||||
|     boost::optional<Matrix3&> H3) { | ||||
|   // return dummy derivatives (not correct, but that's ok for testing here)
 | ||||
|   if (H1) | ||||
|     *H1 = eye(3); | ||||
|  |  | |||
|  | @ -127,13 +127,13 @@ namespace gtsam { | |||
|           if(H1 || H2 || H3) { | ||||
|             gtsam::Matrix H0, H02; | ||||
|             PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H3) - measured_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); | ||||
|             *H2 = *H1 * H02; | ||||
|             *H1 = *H1 * H0; | ||||
|             return reprojectionError.vector(); | ||||
|           } else { | ||||
|             PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H3) - measured_); | ||||
|             Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_); | ||||
|             return reprojectionError.vector(); | ||||
|           } | ||||
|       } catch( CheiralityException& e) { | ||||
|  |  | |||
|  | @ -10,6 +10,7 @@ | |||
| #include <gtsam_unstable/nonlinear/Expression.h> | ||||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/geometry/PinholeCamera.h> | ||||
| #include <boost/bind.hpp> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|  | @ -48,6 +49,16 @@ Point2_ project(const Point3_& p_cam) { | |||
|   return Point2_(PinholeCamera<Cal3_S2>::project_to_camera, p_cam); | ||||
| } | ||||
| 
 | ||||
| Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, | ||||
|     boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint, | ||||
|     boost::optional<Matrix25&> Dcal) { | ||||
|   return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal); | ||||
| } | ||||
| 
 | ||||
| Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { | ||||
|   return Point2_(project6, x, p, K); | ||||
| } | ||||
| 
 | ||||
| template<class CAL> | ||||
| Point2_ uncalibrate(const Expression<CAL>& K, const Point2_& xy_hat) { | ||||
|   return Point2_(K, &CAL::uncalibrate, xy_hat); | ||||
|  |  | |||
|  | @ -26,7 +26,6 @@ | |||
| #include <time.h> | ||||
| #include <iostream> | ||||
| #include <iomanip>      // std::setprecision | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
|  | @ -47,9 +46,9 @@ void time(const NonlinearFactor& f, const Values& values) { | |||
| boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2()); | ||||
| 
 | ||||
| Point2 myProject(const Pose3& pose, const Point3& point, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { | ||||
|     boost::optional<Matrix26&> H1, boost::optional<Matrix23&> H2) { | ||||
|   PinholeCamera<Cal3_S2> camera(pose, *fixedK); | ||||
|   return camera.project(point, H1, H2); | ||||
|   return camera.project(point, H1, H2, boost::none); | ||||
| } | ||||
| 
 | ||||
| int main() { | ||||
|  | @ -74,37 +73,43 @@ int main() { | |||
|   // Dedicated factor
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 4.2 musecs/call
 | ||||
|   GeneralSFMFactor2<Cal3_S2> oldFactor2(z, model, 1, 2, 3); | ||||
|   time(oldFactor2, values); | ||||
|   GeneralSFMFactor2<Cal3_S2> f1(z, model, 1, 2, 3); | ||||
|   time(f1, values); | ||||
| 
 | ||||
|   // BADFactor
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 20.3 musecs/call
 | ||||
|   BADFactor<Point2> newFactor2(model, z, | ||||
|   BADFactor<Point2> f2(model, z, | ||||
|       uncalibrate(K, project(transform_to(x, p)))); | ||||
|   time(newFactor2, values); | ||||
|   time(f2, values); | ||||
| 
 | ||||
|   // BADFactor ternary
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 20.3 musecs/call
 | ||||
|   BADFactor<Point2> f3(model, z, project3(x, p, K)); | ||||
|   time(f3, values); | ||||
| 
 | ||||
|   // CALIBRATED
 | ||||
| 
 | ||||
|   // Dedicated factor
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 3.4 musecs/call
 | ||||
|   GenericProjectionFactor<Pose3, Point3> oldFactor1(z, model, 1, 2, fixedK); | ||||
|   time(oldFactor1, values); | ||||
|   GenericProjectionFactor<Pose3, Point3> g1(z, model, 1, 2, fixedK); | ||||
|   time(g1, values); | ||||
| 
 | ||||
|   // BADFactor
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 16.0 musecs/call
 | ||||
|   BADFactor<Point2> newFactor1(model, z, | ||||
|   BADFactor<Point2> g2(model, z, | ||||
|       uncalibrate(Cal3_S2_(*fixedK), project(transform_to(x, p)))); | ||||
|   time(newFactor1, values); | ||||
|   time(g2, values); | ||||
| 
 | ||||
|   // BADFactor, optimized
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 9.0 musecs/call
 | ||||
|   typedef PinholeCamera<Cal3_S2> Camera; | ||||
|   typedef Expression<Camera> Camera_; | ||||
|   BADFactor<Point2> newFactor3(model, z, Point2_(myProject, x, p)); | ||||
|   time(newFactor3, values); | ||||
|   BADFactor<Point2> g3(model, z, Point2_(myProject, x, p)); | ||||
|   time(g3, values); | ||||
|   return 0; | ||||
| } | ||||
|  |  | |||
|  | @ -25,7 +25,7 @@ | |||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| static const int n = 500000; | ||||
| static const int n = 1000000; | ||||
| 
 | ||||
| void time(const NonlinearFactor& f, const Values& values) { | ||||
|   long timeLog = clock(); | ||||
|  | @ -58,9 +58,13 @@ int main() { | |||
|   // BADFactor
 | ||||
|   // Oct 3, 2014, Macbook Air
 | ||||
|   // 20.3 musecs/call
 | ||||
|   BADFactor<Point2> newFactor2(model, z, | ||||
|       uncalibrate(K, project(transform_to(x, p)))); | ||||
|   time(newFactor2, values); | ||||
| #define TERNARY | ||||
| #ifdef TERNARY | ||||
|   BADFactor<Point2> f(model, z, project3(x, p, K)); | ||||
| #else | ||||
|   BADFactor<Point2> f(model, z, uncalibrate(K, project(transform_to(x, p)))); | ||||
| #endif | ||||
|   time(f, values); | ||||
| 
 | ||||
|   return 0; | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue