From 5cc4513ddb72ed7b0dab51d885f6086d26ebce66 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 5 Mar 2015 22:14:03 -0800 Subject: [PATCH] PinholeBaseK::project and PinholePose::project2 of Unit3 --- gtsam/geometry/PinholePose.h | 90 ++++++++++++++++-------- gtsam/geometry/tests/testPinholePose.cpp | 79 +++++++++++++-------- 2 files changed, 110 insertions(+), 59 deletions(-) diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 2ef008a94..c23cbd4f5 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -95,45 +95,66 @@ public: return calibration().uncalibrate(pn); } + /** 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_to_camera(pc); + return calibration().uncalibrate(pn); + } + + /** 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 + */ + Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose, + 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; + + return pi; + } + /** project a point at infinity from world coordinate to the image * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) * @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 */ - Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose = - boost::none, OptionalJacobian<2, 2> Dpoint = boost::none, + Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose, + OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, DimK> Dcal = boost::none) const { - if (!Dpose && !Dpoint && !Dcal) { - const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter) - const Point2 pn = PinholeBase::project_to_camera(pc);// project the point to the camera - return calibration().uncalibrate(pn); - } + // project to normalized coordinates + const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint); - // world to camera coordinate - Matrix3 Dpc_rot, Dpc_point; - const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point); + // uncalibrate to pixel coordinates + Matrix2 Dpi_pn; + const Point2 pi = calibration().uncalibrate(pn, Dcal, + Dpose || Dpoint ? &Dpi_pn : 0); - // only rotation is important - Matrix36 Dpc_pose; - Dpc_pose.setZero(); - Dpc_pose.leftCols<3>() = Dpc_rot; - - // camera to normalized image coordinate - Matrix23 Dpn_pc;// 2*3 - const Point2 pn = PinholeBase::project_to_camera(pc, Dpn_pc); - - // uncalibration - Matrix2 Dpi_pn;// 2*2 - const Point2 pi = calibration().uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; + // If needed, apply chain rule if (Dpose) - *Dpose = Dpi_pc * Dpc_pose; + *Dpose = Dpi_pn * *Dpose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only) + *Dpoint = Dpi_pn * *Dpoint; + return pi; } @@ -144,9 +165,9 @@ public: } /// backproject a 2-dimensional point to a 3-dimensional point at infinity - Point3 backprojectPointAtInfinity(const Point2& p) const { + Unit3 backprojectPointAtInfinity(const Point2& p) const { const Point2 pn = calibration().calibrate(p); - const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 + const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 return pose().rotation().rotate(pc); } @@ -344,6 +365,17 @@ public: return pi; } + /** project a point at infinity from world coordinate to the image, 2 derivatives only + * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) + * @param Dpose is the Jacobian w.r.t. the whole camera (realy only the pose) + * @param Dpoint is the Jacobian w.r.t. point3 + * TODO should use Unit3 + */ + Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none) const { + return Base::project(pw, Dpose, Dpoint); + } + /// @} /// @name Manifold /// @{ diff --git a/gtsam/geometry/tests/testPinholePose.cpp b/gtsam/geometry/tests/testPinholePose.cpp index 411273c1f..dc294899f 100644 --- a/gtsam/geometry/tests/testPinholePose.cpp +++ b/gtsam/geometry/tests/testPinholePose.cpp @@ -46,10 +46,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( PinholePose, constructor) @@ -144,11 +144,11 @@ TEST( PinholePose, Dproject) { Matrix Dpose, Dpoint; Point2 result = camera.project2(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); - Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix expectedDcamera = numericalDerivative31(project3, pose, point1, K); + Matrix expectedDpoint = numericalDerivative32(project3, pose, point1, K); EXPECT(assert_equal(Point2(-100, 100), result)); - EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); - EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); } /* ************************************************************************* */ @@ -161,11 +161,11 @@ TEST( PinholePose, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); - Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + Matrix expectedDcamera = numericalDerivative21(project4, camera, point1); + Matrix expectedDpoint = numericalDerivative22(project4, camera, point1); EXPECT(assert_equal(result, Point2(-100, 100) )); - EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); - EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(expectedDcamera, Dcamera, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); } /* ************************************************************************* */ @@ -176,12 +176,31 @@ TEST( CalibratedCamera, Dproject3) static const Camera camera(pose1); Matrix Dpose, Dpoint; camera.project2(point1, Dpose, Dpoint); - Matrix numerical_pose = numericalDerivative21(project4, camera, point1); + Matrix expectedDcamera = numericalDerivative21(project4, camera, point1); Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(expectedDcamera, Dpose, 1e-7)); CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); } +/* ************************************************************************* */ +static Point2 project(const Pose3& pose, const Unit3& pointAtInfinity, + const Cal3_S2::shared_ptr& cal) { + return Camera(pose, cal).project(pointAtInfinity); +} + +/* ************************************************************************* */ +TEST( PinholePose, DprojectAtInfinity2) +{ + Unit3 pointAtInfinity(0,0,1000); + Matrix Dpose, Dpoint; + Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint); + Matrix expectedDcamera = numericalDerivative31(project, pose, pointAtInfinity, K); + Matrix expectedDpoint = numericalDerivative32(project, pose, pointAtInfinity, K); + EXPECT(assert_equal(Point2(0,0), result)); + EXPECT(assert_equal(expectedDcamera, Dpose, 1e-7)); + EXPECT(assert_equal(expectedDpoint, Dpoint, 1e-7)); +} + /* ************************************************************************* */ static double range0(const Camera& camera, const Point3& point) { return camera.range(point); @@ -191,12 +210,12 @@ static double range0(const Camera& camera, const Point3& point) { TEST( PinholePose, range0) { Matrix D1; Matrix D2; double result = camera.range(point1, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); - Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + Matrix expectedDcamera = numericalDerivative21(range0, camera, point1); + Matrix expectedDpoint = numericalDerivative22(range0, camera, point1); EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ @@ -208,11 +227,11 @@ static double range1(const Camera& camera, const Pose3& pose) { TEST( PinholePose, range1) { Matrix D1; Matrix D2; double result = camera.range(pose1, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); - Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + Matrix expectedDcamera = numericalDerivative21(range1, camera, pose1); + Matrix expectedDpoint = numericalDerivative22(range1, camera, pose1); EXPECT_DOUBLES_EQUAL(1, result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ @@ -228,11 +247,11 @@ static double range2(const Camera& camera, const Camera2& camera2) { TEST( PinholePose, range2) { Matrix D1; Matrix D2; double result = camera.range(camera2, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); - Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + Matrix expectedDcamera = numericalDerivative21(range2, camera, camera2); + Matrix expectedDpoint = numericalDerivative22(range2, camera, camera2); EXPECT_DOUBLES_EQUAL(1, result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */ @@ -245,11 +264,11 @@ static double range3(const Camera& camera, const CalibratedCamera& camera3) { TEST( PinholePose, range3) { Matrix D1; Matrix D2; double result = camera.range(camera3, D1, D2); - Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); - Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + Matrix expectedDcamera = numericalDerivative21(range3, camera, camera3); + Matrix expectedDpoint = numericalDerivative22(range3, camera, camera3); EXPECT_DOUBLES_EQUAL(1, result, 1e-9); - EXPECT(assert_equal(Hexpected1, D1, 1e-7)); - EXPECT(assert_equal(Hexpected2, D2, 1e-7)); + EXPECT(assert_equal(expectedDcamera, D1, 1e-7)); + EXPECT(assert_equal(expectedDpoint, D2, 1e-7)); } /* ************************************************************************* */