diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 2514e4a92..c2fea07e0 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -426,6 +426,15 @@ public: typedef Eigen::Matrix Matrix2K6; + /** project a point from world coordinate to the image + * @param pw is a point in the world coordinate + */ + Point2 project2(const Point3& pw) const { + const Point3 pc = pose_.transform_to(pw); + const Point2 pn = project_to_camera(pc); + return K_.uncalibrate(pn); + } + /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] @@ -433,8 +442,8 @@ public: */ Point2 project2( const Point3& pw, // - boost::optional Dcamera = boost::none, - boost::optional Dpoint = boost::none) const { + boost::optional Dcamera, + boost::optional Dpoint) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); @@ -465,10 +474,8 @@ public: * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dpoint is the Jacobian w.r.t. point3 */ - Point2 project2( - const Point3& pw, // - boost::optional Dcamera = boost::none, - boost::optional Dpoint = boost::none) const { + Point2 project2(const Point3& pw, // + boost::optional Dcamera, boost::optional Dpoint) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); diff --git a/gtsam/geometry/tests/testCal3Bundler.cpp b/gtsam/geometry/tests/testCal3Bundler.cpp index 905605b55..e9eb689e1 100644 --- a/gtsam/geometry/tests/testCal3Bundler.cpp +++ b/gtsam/geometry/tests/testCal3Bundler.cpp @@ -14,7 +14,6 @@ * @brief Unit tests for transform derivatives */ - #include #include #include @@ -32,7 +31,9 @@ static Point2 p(2,3); TEST( Cal3Bundler, vector) { Cal3Bundler K; - CHECK(assert_equal((Vector(3)<<1,0,0),K.vector())); + Vector expected(3); + expected << 1, 0, 0; + CHECK(assert_equal(expected,K.vector())); } /* ************************************************************************* */