diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 41176d0d8..1700b4960 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -361,10 +361,10 @@ public: const Point3& point, // OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { - Matrix16 Dpose; - double result = pose_.range(point, Dcamera ? &Dpose : 0, Dpoint); + Matrix16 Dpose_; + double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); if (Dcamera) - *Dcamera << Dpose, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } @@ -378,11 +378,11 @@ public: double range( const Pose3& pose, // OptionalJacobian<1, DimC> Dcamera = boost::none, - OptionalJacobian<1, 6> Dpose2 = boost::none) const { - Matrix16 Dpose; - double result = pose_.range(pose, Dcamera ? &Dpose : 0, Dpose2); + OptionalJacobian<1, 6> Dpose = boost::none) const { + Matrix16 Dpose_; + double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); if (Dcamera) - *Dcamera << Dpose, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } @@ -397,14 +397,17 @@ public: double range( const PinholeCamera& camera, // OptionalJacobian<1, DimC> Dcamera = boost::none, - OptionalJacobian<1, 6 + CalibrationB::Dim()> Dother = boost::none) const { - Matrix16 Dpose, Dpose2; - double result = pose_.range(camera.pose(), Dcamera ? &Dpose : 0, + OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::none) const { + Matrix16 Dpose_, Dpose2; + double result = pose_.range(camera.pose(), Dcamera ? &Dpose_ : 0, Dother ? &Dpose2 : 0); if (Dcamera) - *Dcamera << Dpose, Eigen::Matrix::Zero(); - if (Dother) - *Dother << Dpose2, Eigen::Matrix::Zero(); + *Dcamera << Dpose_, Eigen::Matrix::Zero(); + if (Dother) { + Dother->setZero(); + Dother->block(0, 0, 1, 6) = Dpose2; + } return result; } @@ -419,7 +422,7 @@ public: const CalibratedCamera& camera, // OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { - return range(camera.pose_, Dcamera, Dother); + return range(camera.pose(), Dcamera, Dother); } private: diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f85ad9641..3158c95e8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -297,20 +297,24 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, } /* ************************************************************************* */ -double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1, - OptionalJacobian<1,3> H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, + OptionalJacobian<1, 3> H2) const { if (!H1 && !H2) return transform_to(point).norm(); - Matrix36 D1; - Matrix3 D2; - Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); - double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( - d2); - Matrix13 D_result_d ; - D_result_d << x / n, y / n, z / n; - if (H1) *H1 << D_result_d * D1; - if (H2) *H2 << D_result_d * D2; - return n; + else { + Matrix36 D1; + Matrix3 D2; + Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); + const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, + n = sqrt(d2); + Matrix13 D_result_d; + D_result_d << x / n, y / n, z / n; + if (H1) + *H1 = D_result_d * D1; + if (H2) + *H2 = D_result_d * D2; + return n; + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5aeee03d4..6a990e08e 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -15,12 +15,14 @@ * @brief test CalibratedCamera class */ -#include - -#include +#include +#include #include #include -#include + +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ddeae2b7d..d11fbe3bf 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,29 +15,28 @@ * @brief test PinholeCamera class */ -#include -#include - #include #include #include #include #include +#include + +#include +#include using namespace std; using namespace gtsam; +typedef PinholeCamera Camera; + static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); -typedef PinholeCamera Camera; -static const Camera camera(pose1, K); +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); @@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); + EXPECT(assert_equal( camera.calibration(), K)); + EXPECT(assert_equal( camera.pose(), pose)); } /* ************************************************************************* */ @@ -67,7 +66,7 @@ TEST( PinholeCamera, level2) Point3 x(1,0,0),y(0,0,-1),z(0,1,0); Rot3 wRc(x,y,z); Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); } /* ************************************************************************* */ @@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat) // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); Point3 C2(30.0,0.0,10.0); Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, eye(3))); } /* ************************************************************************* */ TEST( PinholeCamera, project) { - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); + EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) )); } /* ************************************************************************* */ TEST( PinholeCamera, backproject) { - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity) { - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); } /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backproject(Point2(), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); - CHECK(x.second); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + 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); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3) Point3 expected(0., 0., 1.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); - Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); - Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); - Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(Point2(-100, 100), result)); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix numerical_cal = numericalDerivative33(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(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + Point3 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 expected(-5.0, 5.0); - CHECK(assert_equal(actual, expected, 1e-7)); + EXPECT(assert_equal(actual, expected, 1e-7)); // test Jacobians - Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); - Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); - Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters - Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K); + Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K); + Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -217,11 +216,82 @@ TEST( PinholeCamera, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix numerical_camera = numericalDerivative21(project4, camera, point1); - Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = 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)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +typedef Eigen::Matrix Matrix1_11; +TEST( PinholeCamera, range0) { + Matrix1_11 D1; Matrix13 D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = 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)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range1) { + Matrix1_11 D1; Matrix16 D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = 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)); +} + +/* ************************************************************************* */ +typedef PinholeCamera Camera2; +static const Cal3Bundler K2(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range2) { + typedef Eigen::Matrix Matrix19; + Matrix1_11 D1; Matrix19 D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = 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)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range3) { + Matrix1_11 D1; Matrix16 D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = 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)); } /* ************************************************************************* */