diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 632f6de86..e900d5a85 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -200,57 +200,33 @@ public: typedef Eigen::Matrix 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 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration + /** project a point from world coordinate to the image, fixed Jacobians + * @param pw is a point in the world coordinate */ - Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none, - OptionalJacobian<2, DimK> Dcal = boost::none) const { - - // project to normalized coordinates - const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - - // uncalibrate to pixel coordinates - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, Dcal, - Dpose || Dpoint ? &Dpi_pn : 0); - - // If needed, apply chain rule - if (Dpose) - *Dpose = Dpi_pn * *Dpose; - if (Dpoint) - *Dpoint = Dpi_pn * *Dpoint; - + Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + // We just call 3-derivative version in Base + Matrix26 Dpose; + Eigen::Matrix Dcal; + Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, + Dcamera ? &Dcal : 0); + if (Dcamera) + *Dcamera << Dpose, Dcal; return pi; } /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate */ - Point2 project2( - const Point3& pw, // - OptionalJacobian<2, dimension> Dcamera = boost::none, - OptionalJacobian<2, 3> Dpoint = boost::none) const { - - // project to normalized coordinates + Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { + // We just call 3-derivative version in Base Matrix26 Dpose; - const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - - // uncalibrate to pixel coordinates - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0, - Dcamera || Dpoint ? &Dpi_pn : 0); - - // If needed, calculate derivatives + Eigen::Matrix Dcal; + Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint, + Dcamera ? &Dcal : 0); if (Dcamera) - *Dcamera << Dpi_pn * Dpose, Dcal; - if (Dpoint) - *Dpoint = Dpi_pn * (*Dpoint); - + *Dcamera << Dpose, Dcal; return pi; } diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 0e610d8d6..74bc4ca2a 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -45,10 +45,10 @@ static const Point3 point2(-0.08, 0.08, 0.0); static const Point3 point3( 0.08, 0.08, 0.0); static const Point3 point4( 0.08,-0.08, 0.0); -static const Point3 point1_inf(-0.16,-0.16, -1.0); -static const Point3 point2_inf(-0.16, 0.16, -1.0); -static const Point3 point3_inf( 0.16, 0.16, -1.0); -static const Point3 point4_inf( 0.16,-0.16, -1.0); +static const Unit3 point1_inf(-0.16,-0.16, -1.0); +static const Unit3 point2_inf(-0.16, 0.16, -1.0); +static const Unit3 point3_inf( 0.16, 0.16, -1.0); +static const Unit3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) @@ -154,9 +154,9 @@ TEST( PinholeCamera, backprojectInfinity2) Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 1., 0.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 1., 0.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -169,9 +169,9 @@ TEST( PinholeCamera, backprojectInfinity3) Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity Camera camera(Pose3(rot, origin), K); - Point3 actual = camera.backprojectPointAtInfinity(Point2()); - Point3 expected(0., 0., 1.); - Point2 x = camera.projectPointAtInfinity(expected); + Unit3 actual = camera.backprojectPointAtInfinity(Point2()); + Unit3 expected(0., 0., 1.); + Point2 x = camera.project(expected); EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(Point2(), x)); @@ -197,17 +197,17 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { - return Camera(pose,cal).projectPointAtInfinity(point3D); +static Point2 projectInfinity3(const Pose3& pose, const Unit3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).project(point3D); } TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 + Unit3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection - Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Point2 actual = camera.project(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); EXPECT(assert_equal(actual, expected, 1e-7));