BackprojectFromCamera and some small fixes from Skydio
							parent
							
								
									cc25ece055
								
							
						
					
					
						commit
						ae86bf0271
					
				|  | @ -144,8 +144,9 @@ bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft) { | |||
|   // Check last diagonal element - Eigen does not check it
 | ||||
|   if (nFrontal >= 2) { | ||||
|     int exp2, exp1; | ||||
|     (void)frexp(R(topleft + nFrontal - 2, topleft + nFrontal - 2), &exp2); | ||||
|     (void)frexp(R(topleft + nFrontal - 1, topleft + nFrontal - 1), &exp1); | ||||
|     // NOTE(gareth): R is already the size of A, so we don't need to add topleft here.
 | ||||
|     (void)frexp(R(nFrontal - 2, nFrontal - 2), &exp2); | ||||
|     (void)frexp(R(nFrontal - 1, nFrontal - 1), &exp1); | ||||
|     return (exp2 - exp1 < underconstrainedExponentDifference); | ||||
|   } else if (nFrontal == 1) { | ||||
|     int exp1; | ||||
|  |  | |||
|  | @ -298,7 +298,7 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (* | |||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 1 of 4-argument function | ||||
|  * @param h ternary function yielding m-vector | ||||
|  * @param h quartic function yielding m-vector | ||||
|  * @param x1 n-dimensional first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  | @ -317,9 +317,15 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41( | |||
|   return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4), x1, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4> | ||||
| inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { | ||||
|   return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 2 of 4-argument function | ||||
|  * @param h ternary function yielding m-vector | ||||
|  * @param h quartic function yielding m-vector | ||||
|  * @param x1 first argument value | ||||
|  * @param x2 n-dimensional second argument value | ||||
|  * @param x3 third argument value | ||||
|  | @ -338,9 +344,15 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42( | |||
|   return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4> | ||||
| inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&), | ||||
|   const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { | ||||
|   return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 3 of 4-argument function | ||||
|  * @param h ternary function yielding m-vector | ||||
|  * @param h quartic function yielding m-vector | ||||
|  * @param x1 first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 n-dimensional third argument value | ||||
|  | @ -359,9 +371,15 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43( | |||
|   return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4), x3, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4> | ||||
| inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { | ||||
|   return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 4 of 4-argument function | ||||
|  * @param h ternary function yielding m-vector | ||||
|  * @param h quartic function yielding m-vector | ||||
|  * @param x1 first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  | @ -380,6 +398,152 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44( | |||
|   return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1), x4, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4> | ||||
| inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&), | ||||
|   const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { | ||||
|   return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, _1, _2, _3, _4), x1, x2, x3, x4); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 1 of 5-argument function | ||||
|  * @param h quintic function yielding m-vector | ||||
|  * @param x1 n-dimensional first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  * @param x4 fourth argument value | ||||
|  * @param x5 fifth argument value | ||||
|  * @param delta increment for numerical derivative | ||||
|  * @return m*n Jacobian computed via central differencing | ||||
|  */ | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51( | ||||
|     boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, | ||||
|     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value), | ||||
|       "Template argument Y must be a manifold type."); | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value), | ||||
|       "Template argument X1 must be a manifold type."); | ||||
|   return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3, x4, x5), x1, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 2 of 5-argument function | ||||
|  * @param h quintic function yielding m-vector | ||||
|  * @param x1 n-dimensional first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  * @param x4 fourth argument value | ||||
|  * @param x5 fifth argument value | ||||
|  * @param delta increment for numerical derivative | ||||
|  * @return m*n Jacobian computed via central differencing | ||||
|  */ | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52( | ||||
|     boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, | ||||
|     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value), | ||||
|       "Template argument Y must be a manifold type."); | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value), | ||||
|       "Template argument X1 must be a manifold type."); | ||||
|   return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4, x5), x2, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 3 of 5-argument function | ||||
|  * @param h quintic function yielding m-vector | ||||
|  * @param x1 n-dimensional first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  * @param x4 fourth argument value | ||||
|  * @param x5 fifth argument value | ||||
|  * @param delta increment for numerical derivative | ||||
|  * @return m*n Jacobian computed via central differencing | ||||
|  */ | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53( | ||||
|     boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, | ||||
|     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value), | ||||
|       "Template argument Y must be a manifold type."); | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value), | ||||
|       "Template argument X1 must be a manifold type."); | ||||
|   return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1, x4, x5), x3, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 4 of 5-argument function | ||||
|  * @param h quintic function yielding m-vector | ||||
|  * @param x1 n-dimensional first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  * @param x4 fourth argument value | ||||
|  * @param x5 fifth argument value | ||||
|  * @param delta increment for numerical derivative | ||||
|  * @return m*n Jacobian computed via central differencing | ||||
|  */ | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54( | ||||
|     boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, | ||||
|     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value), | ||||
|       "Template argument Y must be a manifold type."); | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value), | ||||
|       "Template argument X1 must be a manifold type."); | ||||
|   return numericalDerivative11<Y, X4>(boost::bind(h, x1, x2, x3, _1, x5), x4, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical derivative in argument 5 of 5-argument function | ||||
|  * @param h quintic function yielding m-vector | ||||
|  * @param x1 n-dimensional first argument value | ||||
|  * @param x2 second argument value | ||||
|  * @param x3 third argument value | ||||
|  * @param x4 fourth argument value | ||||
|  * @param x5 fifth argument value | ||||
|  * @param delta increment for numerical derivative | ||||
|  * @return m*n Jacobian computed via central differencing | ||||
|  */ | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55( | ||||
|     boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, | ||||
|     const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value), | ||||
|       "Template argument Y must be a manifold type."); | ||||
|   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value), | ||||
|       "Template argument X1 must be a manifold type."); | ||||
|   return numericalDerivative11<Y, X5>(boost::bind(h, x1, x2, x3, x4, _1), x5, delta); | ||||
| } | ||||
| 
 | ||||
| template<class Y, class X1, class X2, class X3, class X4, class X5> | ||||
| inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&), | ||||
|     const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) { | ||||
|   return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, _1, _2, _3, _4, _5), x1, x2, x3, x4, x5); | ||||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Compute numerical Hessian matrix.  Requires a single-argument Lie->scalar | ||||
|  * function.  This is implemented simply as the derivative of the gradient. | ||||
|  |  | |||
|  | @ -24,6 +24,9 @@ | |||
| 
 | ||||
| #include <gtsam/base/serialization.h> | ||||
| 
 | ||||
| #include <boost/serialization/serialization.hpp> | ||||
| 
 | ||||
| 
 | ||||
| // whether to print the serialized text to stdout
 | ||||
| const bool verbose = false; | ||||
| 
 | ||||
|  | @ -142,4 +145,3 @@ bool equalsDereferencedBinary(const T& input = T()) { | |||
| 
 | ||||
| } // \namespace serializationTestHelpers
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -1,6 +1,6 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation,  | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
|  | @ -11,7 +11,7 @@ | |||
| 
 | ||||
| /**
 | ||||
|  * @file testSerializationBase.cpp | ||||
|  * @brief  | ||||
|  * @brief | ||||
|  * @author Richard Roberts | ||||
|  * @date Feb 7, 2012 | ||||
|  */ | ||||
|  |  | |||
|  | @ -142,8 +142,11 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, | |||
|   Matrix23 Dpc_rot; | ||||
|   Matrix2 Dpc_point; | ||||
|   const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0, | ||||
|       Dpose ? &Dpc_point : 0); | ||||
| 
 | ||||
|       Dpoint ? &Dpc_point : 0); | ||||
| #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|   if (pc.unitVector().z() <= 0) | ||||
|     throw CheiralityException(); | ||||
| #endif | ||||
|   // camera to normalized image coordinate
 | ||||
|   Matrix2 Dpn_pc; | ||||
|   const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0); | ||||
|  | @ -161,8 +164,12 @@ Point2 PinholeBase::project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose, | |||
|   return pn; | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| Point3 PinholeBase::backproject_from_camera(const Point2& p, | ||||
|     const double depth) { | ||||
| Point3 PinholeBase::BackprojectFromCamera(const Point2& p, | ||||
|     const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) { | ||||
|   if (Dpoint) | ||||
|     *Dpoint << depth, 0, 0, depth, 0, 0; | ||||
|   if (Ddepth) | ||||
|     *Ddepth << p.x(), p.y(), 1; | ||||
|   return Point3(p.x() * depth, p.y() * depth, depth); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -201,7 +201,9 @@ public: | |||
|       OptionalJacobian<2, 2> Dpoint = boost::none) const; | ||||
| 
 | ||||
|   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
 | ||||
|   static Point3 backproject_from_camera(const Point2& p, const double depth); | ||||
|   static Point3 BackprojectFromCamera(const Point2& p, const double depth, | ||||
|                                       OptionalJacobian<3, 2> Dpoint = boost::none, | ||||
|                                       OptionalJacobian<3, 1> Ddepth = boost::none); | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Advanced interface
 | ||||
|  | @ -337,8 +339,28 @@ public: | |||
|       boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const; | ||||
| 
 | ||||
|   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
 | ||||
|   Point3 backproject(const Point2& pn, double depth) const { | ||||
|     return pose().transform_from(backproject_from_camera(pn, depth)); | ||||
|   Point3 backproject(const Point2& pn, double depth, | ||||
|                      OptionalJacobian<3, 6> Dresult_dpose = boost::none, | ||||
|                      OptionalJacobian<3, 2> Dresult_dp = boost::none, | ||||
|                      OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const { | ||||
| 
 | ||||
|     Matrix32 Dpoint_dpn; | ||||
|     Matrix31 Dpoint_ddepth; | ||||
|     const Point3 point = BackprojectFromCamera(pn, depth, | ||||
|                                                  Dresult_dp ? &Dpoint_dpn : 0, | ||||
|                                                  Dresult_ddepth ? &Dpoint_ddepth : 0); | ||||
| 
 | ||||
|     Matrix33 Dresult_dpoint; | ||||
|     const Point3 result = pose().transform_from(point, Dresult_dpose, | ||||
|                                                        (Dresult_ddepth || | ||||
|                                                         Dresult_dp) ? &Dresult_dpoint : 0); | ||||
| 
 | ||||
|     if (Dresult_dp) | ||||
|       *Dresult_dp = Dresult_dpoint * Dpoint_dpn; | ||||
|     if (Dresult_ddepth) | ||||
|       *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; | ||||
| 
 | ||||
|     return result; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -404,4 +426,3 @@ template <typename T> | |||
| struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {}; | ||||
| 
 | ||||
| }  // namespace gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -85,25 +85,9 @@ public: | |||
|     return pn; | ||||
|   } | ||||
| 
 | ||||
|   /** project a point from world coordinate to the image
 | ||||
|    *  @param pw is a point in the world coordinates | ||||
|    */ | ||||
|   Point2 project(const Point3& pw) const { | ||||
|     const Point2 pn = PinholeBase::project2(pw); // project to normalized coordinates
 | ||||
|     return calibration().uncalibrate(pn); // uncalibrate to pixel coordinates
 | ||||
|   } | ||||
| 
 | ||||
|   /** project a point from world coordinate to the image
 | ||||
|    *  @param pw is a point at infinity in the world coordinates | ||||
|    */ | ||||
|   Point2 project(const Unit3& pw) const { | ||||
|     const Unit3 pc = pose().rotation().unrotate(pw); // convert to camera frame
 | ||||
|     const Point2 pn = PinholeBase::Project(pc); // project to normalized coordinates
 | ||||
|     return calibration().uncalibrate(pn);  // uncalibrate to pixel coordinates
 | ||||
|   } | ||||
| 
 | ||||
|   /** Templated projection of a point (possibly at infinity) from world coordinate to the image
 | ||||
|    *  @param pw is a 3D point or aUnit3 (point at infinity) in world coordinates | ||||
|    *  @param pw is a 3D point or a Unit3 (point at infinity) 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 | ||||
|  | @ -131,25 +115,51 @@ public: | |||
|   } | ||||
| 
 | ||||
|   /// project a 3D point from world coordinates into the image
 | ||||
|   Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, | ||||
|   Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|       OptionalJacobian<2, 3> Dpoint = boost::none, | ||||
|       OptionalJacobian<2, DimK> Dcal = boost::none) const { | ||||
|     return _project(pw, Dpose, Dpoint, Dcal); | ||||
|   } | ||||
| 
 | ||||
|   /// project a point at infinity from world coordinates into the image
 | ||||
|   Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, | ||||
|   Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, | ||||
|       OptionalJacobian<2, 2> Dpoint = boost::none, | ||||
|       OptionalJacobian<2, DimK> Dcal = boost::none) const { | ||||
|     return _project(pw, Dpose, Dpoint, Dcal); | ||||
|   } | ||||
| 
 | ||||
|   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
 | ||||
|   Point3 backproject(const Point2& p, double depth) const { | ||||
|     const Point2 pn = calibration().calibrate(p); | ||||
|     return pose().transform_from(backproject_from_camera(pn, depth)); | ||||
|   Point3 backproject(const Point2& p, double depth, | ||||
|                      OptionalJacobian<3, 6> Dresult_dpose = boost::none, | ||||
|                      OptionalJacobian<3, 2> Dresult_dp = boost::none, | ||||
|                      OptionalJacobian<3, 1> Dresult_ddepth = boost::none, | ||||
|                      OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const { | ||||
|     typedef Eigen::Matrix<double, 2, DimK> Matrix2K; | ||||
|     Matrix2K Dpn_dcal; | ||||
|     Matrix22 Dpn_dp; | ||||
|     const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0, | ||||
|                                                  Dresult_dp ? &Dpn_dp : 0); | ||||
|     Matrix32 Dpoint_dpn; | ||||
|     Matrix31 Dpoint_ddepth; | ||||
|     const Point3 point = BackprojectFromCamera(pn, depth, | ||||
|                                                (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, | ||||
|                                                Dresult_ddepth ? &Dpoint_ddepth : 0); | ||||
|     Matrix33 Dresult_dpoint; | ||||
|     const Point3 result = pose().transform_from(point, Dresult_dpose, | ||||
|                                                     (Dresult_ddepth || | ||||
|                                                      Dresult_dp     || | ||||
|                                                      Dresult_dcal) ? &Dresult_dpoint : 0); | ||||
|     if (Dresult_dcal) | ||||
|        *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal;  // (3x3)*(3x2)*(2xDimK)
 | ||||
|     if (Dresult_dp) | ||||
|        *Dresult_dp =   Dresult_dpoint * Dpoint_dpn * Dpn_dp;    // (3x3)*(3x2)*(2x2)
 | ||||
|     if (Dresult_ddepth) | ||||
|        *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;        // (3x3)*(3x1)
 | ||||
| 
 | ||||
|     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); | ||||
|  | @ -302,6 +312,11 @@ public: | |||
|       Base(v), K_(new CALIBRATION(K)) { | ||||
|   } | ||||
| 
 | ||||
|   // Init from Pose3 and calibration
 | ||||
|   PinholePose(const Pose3 &pose, const Vector &K) : | ||||
|       Base(pose), K_(new CALIBRATION(K)) { | ||||
|   } | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Testable
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -113,7 +113,7 @@ static Point2 Project2(const Unit3& point) { | |||
|   return PinholeBase::Project(point); | ||||
| } | ||||
| 
 | ||||
| Unit3 pointAtInfinity(0, 0, 1000); | ||||
| Unit3 pointAtInfinity(0, 0, -1000); | ||||
| TEST( CalibratedCamera, DProjectInfinity) { | ||||
|   Matrix test1; | ||||
|   CalibratedCamera::Project(pointAtInfinity, test1); | ||||
|  | @ -127,6 +127,7 @@ static Point2 project2(const CalibratedCamera& camera, const Point3& point) { | |||
|   return camera.project(point); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| TEST( CalibratedCamera, Dproject_point_pose) | ||||
| { | ||||
|   Matrix Dpose, Dpoint; | ||||
|  | @ -173,7 +174,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity) | |||
| // Add a test with more arbitrary rotation
 | ||||
| TEST( CalibratedCamera, Dproject_point_pose2_infinity) | ||||
| { | ||||
|   static const Pose3 pose(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10)); | ||||
|   static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); | ||||
|   static const CalibratedCamera camera(pose); | ||||
|   Matrix Dpose, Dpoint; | ||||
|   camera.project2(pointAtInfinity, Dpose, Dpoint); | ||||
|  | @ -183,7 +184,48 @@ TEST( CalibratedCamera, Dproject_point_pose2_infinity) | |||
|   CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| static Point3 BackprojectFromCamera(const CalibratedCamera& camera, const Point2& point, | ||||
|                           const double& depth) { | ||||
|   return camera.BackprojectFromCamera(point, depth); | ||||
| } | ||||
| TEST( CalibratedCamera, DBackprojectFromCamera) | ||||
| { | ||||
|   static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); | ||||
|   static const CalibratedCamera camera(pose); | ||||
|   static const double depth = 5.4; | ||||
|   static const Point2 point(10.1, 50.3); | ||||
|   Matrix Dpoint, Ddepth; | ||||
|   camera.BackprojectFromCamera(point, depth, Dpoint, Ddepth); | ||||
|   Matrix numerical_point = numericalDerivative32(BackprojectFromCamera, camera, point, depth); | ||||
|   Matrix numerical_depth  = numericalDerivative33(BackprojectFromCamera, camera, point, depth); | ||||
|   CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); | ||||
|   CHECK(assert_equal(numerical_depth,  Ddepth, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| static Point3 backproject(const Pose3& pose, const Point2& point, const double& depth) { | ||||
|   return CalibratedCamera(pose).backproject(point, depth); | ||||
| } | ||||
| TEST( PinholePose, Dbackproject) | ||||
| { | ||||
|   Matrix36 Dpose; | ||||
|   Matrix31 Ddepth; | ||||
|   Matrix32 Dpoint; | ||||
|   const Point2 point(-100, 100); | ||||
|   const double depth(10); | ||||
|   static const Pose3 pose(Rot3::Ypr(-M_PI/4.0, M_PI + M_PI/9.5, M_PI/8.0), Point3(0, 0, -10)); | ||||
|   static const CalibratedCamera camera(pose); | ||||
|   camera.backproject(point, depth, Dpose, Dpoint, Ddepth); | ||||
|   Matrix expectedDpose = numericalDerivative31(backproject, pose, point, depth); | ||||
|   Matrix expectedDpoint = numericalDerivative32(backproject, pose, point, depth); | ||||
|   Matrix expectedDdepth = numericalDerivative33(backproject, pose,point, depth); | ||||
| 
 | ||||
|   EXPECT(assert_equal(expectedDpose,   Dpose,   1e-7)); | ||||
|   EXPECT(assert_equal(expectedDpoint,  Dpoint,  1e-7)); | ||||
|   EXPECT(assert_equal(expectedDdepth,  Ddepth,  1e-7)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { TestResult tr; return TestRegistry::runAllTests(tr); } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -191,7 +191,7 @@ static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, | |||
| /* ************************************************************************* */ | ||||
| TEST( PinholePose, DprojectAtInfinity2) | ||||
| { | ||||
|   Unit3 pointAtInfinity(0,0,1000); | ||||
|   Unit3 pointAtInfinity(0,0,-1000); | ||||
|   Matrix Dpose, Dpoint; | ||||
|   Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); | ||||
|   Matrix expectedDcamera  = numericalDerivative31(project, pose, pointAtInfinity, K); | ||||
|  | @ -201,6 +201,32 @@ TEST( PinholePose, DprojectAtInfinity2) | |||
|   EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| static Point3 backproject(const Pose3& pose, const Cal3_S2& cal, | ||||
|                           const Point2& p, const double& depth) { | ||||
|   return Camera(pose, cal.vector()).backproject(p, depth); | ||||
| } | ||||
| 
 | ||||
| TEST( PinholePose, Dbackproject) | ||||
| { | ||||
|   Matrix36 Dpose; | ||||
|   Matrix31 Ddepth; | ||||
|   Matrix32 Dpoint; | ||||
|   Matrix Dcal; | ||||
|   const Point2 point(-100, 100); | ||||
|   const double depth(10); | ||||
|   camera.backproject(point, depth, Dpose, Dpoint, Ddepth, Dcal); | ||||
|   Matrix expectedDpose = numericalDerivative41(backproject, pose, *K,  point, depth); | ||||
|   Matrix expectedDcal = numericalDerivative42(backproject, pose, *K,  point, depth); | ||||
|   Matrix expectedDpoint = numericalDerivative43(backproject, pose, *K, point, depth); | ||||
|   Matrix expectedDdepth = numericalDerivative44(backproject, pose, *K,  point, depth); | ||||
| 
 | ||||
|   EXPECT(assert_equal(expectedDpose,   Dpose,   1e-7)); | ||||
|   EXPECT(assert_equal(expectedDcal,    Dcal,    1e-7)); | ||||
|   EXPECT(assert_equal(expectedDpoint,  Dpoint,  1e-7)); | ||||
|   EXPECT(assert_equal(expectedDdepth,  Ddepth,  1e-7)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| static double range0(const Camera& camera, const Point3& point) { | ||||
|   return camera.range(point); | ||||
|  |  | |||
|  | @ -1,6 +1,6 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation,  | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
|  | @ -36,31 +36,38 @@ struct PinholeBaseKCal3_S2Callback : PinholeBaseKCal3_S2, wrapper<PinholeBaseKCa | |||
|   } | ||||
| }; | ||||
| 
 | ||||
| BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 2, 4) | ||||
| BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(project_overloads, PinholeBaseKCal3_S2::project, 1, 4) | ||||
| BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(range_overloads, PinholeBaseKCal3_S2::range, 1, 3) | ||||
| 
 | ||||
| // Function pointers to desambiguate project() calls
 | ||||
| Point2  (PinholeBaseKCal3_S2::*project1) (const Point3 &pw) const = &PinholeBaseKCal3_S2::project; | ||||
| Point2  (PinholeBaseKCal3_S2::*project2) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project; | ||||
| Point2  (PinholeBaseKCal3_S2::*project3) (const Unit3 &pw,  OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project; | ||||
| // Function pointers to disambiguate project() calls
 | ||||
| Point2  (PinholeBaseKCal3_S2::*project1) (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project; | ||||
| Point2  (PinholeBaseKCal3_S2::*project2) (const Unit3 &pw,  OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 2 > Dpoint, OptionalJacobian< 2, FixedDimension<Cal3_S2>::value > Dcal) const = &PinholeBaseKCal3_S2::project; | ||||
| 
 | ||||
| // function pointers to desambiguate range() calls
 | ||||
| // function pointers to disambiguate range() calls
 | ||||
| double (PinholeBaseKCal3_S2::*range1) (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 3 > Dpoint) const = &PinholeBaseKCal3_S2::range; | ||||
| double (PinholeBaseKCal3_S2::*range2) (const Pose3 &pose,   OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dpose)  const = &PinholeBaseKCal3_S2::range; | ||||
| double (PinholeBaseKCal3_S2::*range3) (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera, OptionalJacobian< 1, 6 > Dother) const = &PinholeBaseKCal3_S2::range; | ||||
| 
 | ||||
| void exportPinholeBaseK(){ | ||||
| // wrap projectSafe in a function that returns None or a tuple
 | ||||
| // TODO(frank): find out how to return an ndarray instead
 | ||||
| object project_safe(const PinholeBaseKCal3_S2& camera, const gtsam::Point3& p) { | ||||
|   auto result = camera.projectSafe(p); | ||||
|   if (result.second) | ||||
|     return make_tuple(result.first.x(), result.first.y()); | ||||
|   else | ||||
|     return object(); | ||||
| } | ||||
| 
 | ||||
| void exportPinholeBaseK() { | ||||
|   class_<PinholeBaseKCal3_S2Callback, boost::noncopyable>("PinholeBaseKCal3_S2", no_init) | ||||
|     .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), return_value_policy<copy_const_reference>()) | ||||
|     .def("project", project1) | ||||
|     .def("project", project2, project_overloads()) | ||||
|     .def("project", project3, project_overloads()) | ||||
|     .def("backproject", &PinholeBaseKCal3_S2::backproject) | ||||
|     .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) | ||||
|     .def("range", range1, range_overloads()) | ||||
|     .def("range", range2, range_overloads()) | ||||
|     .def("range", range3, range_overloads()) | ||||
|   ; | ||||
| 
 | ||||
| } | ||||
|       .def("calibration", pure_virtual(&PinholeBaseKCal3_S2::calibration), | ||||
|            return_value_policy<copy_const_reference>()) | ||||
|       .def("project_safe", make_function(project_safe)) | ||||
|       .def("project", project1, project_overloads()) | ||||
|       .def("project", project2, project_overloads()) | ||||
|       .def("backproject", &PinholeBaseKCal3_S2::backproject) | ||||
|       .def("backproject_point_at_infinity", &PinholeBaseKCal3_S2::backprojectPointAtInfinity) | ||||
|       .def("range", range1, range_overloads()) | ||||
|       .def("range", range2, range_overloads()) | ||||
|       .def("range", range3, range_overloads()); | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue