Added tests, everything checks out

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;
+    Matrix16 Dpose_;
-    double result = pose_.range(point, Dcamera ? &Dpose : 0, Dpoint);
+    double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
     if (Dcamera)
-      *Dcamera << Dpose, Eigen::Matrix<double, 1, DimK>::Zero();
+      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::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 {
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
-    Matrix16 Dpose;
+    Matrix16 Dpose_;
-    double result = pose_.range(pose, Dcamera ? &Dpose : 0, Dpose2);
+    double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
     if (Dcamera)
-      *Dcamera << Dpose, Eigen::Matrix<double, 1, DimK>::Zero();
+      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
     return result;
   }
 
@@ -397,14 +397,17 @@ public:
   double range(
       const PinholeCamera<CalibrationB>& camera, //
       OptionalJacobian<1, DimC> Dcamera = boost::none,
-      OptionalJacobian<1, 6 + CalibrationB::Dim()> Dother = boost::none) const {
+      OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
-    Matrix16 Dpose, Dpose2;
+          boost::none) const {
-    double result = pose_.range(camera.pose(), Dcamera ? &Dpose : 0,
+    Matrix16 Dpose_, Dpose2;
+    double result = pose_.range(camera.pose(), Dcamera ? &Dpose_ : 0,
         Dother ? &Dpose2 : 0);
     if (Dcamera)
-      *Dcamera << Dpose, Eigen::Matrix<double, 1, DimK>::Zero();
+      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
-    if (Dother)
+    if (Dother) {
-      *Dother << Dpose2, Eigen::Matrix<double, 1, CalibrationB::DimC()>::Zero();
+      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:

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,
+double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
-    OptionalJacobian<1,3> H2) const {
+    OptionalJacobian<1, 3> H2) const {
   if (!H1 && !H2)
     return transform_to(point).norm();
-  Matrix36 D1;
+  else {
-  Matrix3 D2;
+    Matrix36 D1;
-  Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
+    Matrix3 D2;
-  double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
+    Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
-      d2);
+    const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
-  Matrix13 D_result_d ;
+        n = sqrt(d2);
-  D_result_d << x / n, y / n, z / n;
+    Matrix13 D_result_d;
-  if (H1) *H1 << D_result_d * D1;
+    D_result_d << x / n, y / n, z / n;
-  if (H2) *H2 << D_result_d * D2;
+    if (H1)
-  return n;
+      *H1 = D_result_d * D1;
+    if (H2)
+      *H2 = D_result_d * D2;
+    return n;
+  }
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -15,12 +15,14 @@
  * @brief test CalibratedCamera class
  */
 
-#include <iostream>
+#include <gtsam/geometry/CalibratedCamera.h>
-
+#include <gtsam/geometry/Pose2.h>
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/CalibratedCamera.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -15,29 +15,28 @@
  * @brief test PinholeCamera class
  */
 
-#include <cmath>
-#include <iostream>
-
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+
+#include <cmath>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;
 
+typedef PinholeCamera<Cal3_S2> Camera;
+
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose1((Matrix)(Matrix(3,3) <<
+static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
-              1., 0., 0.,
+static const Camera camera(pose, K);
-              0.,-1., 0.,
-              0., 0.,-1.
-              ).finished(),
-            Point3(0,0,0.5));
 
-typedef PinholeCamera<Cal3_S2> Camera;
+static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
-static const Camera camera(pose1, K);
+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));
+  EXPECT(assert_equal( camera.calibration(), K));
-  CHECK(assert_equal( camera.pose(), pose1));
+  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) ));
+  EXPECT(assert_equal( camera.project(point1), Point2(-100,  100) ));
-  CHECK(assert_equal( camera.project(point2), Point2(-100, -100) ));
+  EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) ));
-  CHECK(assert_equal( camera.project(point3), Point2( 100, -100) ));
+  EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) ));
-  CHECK(assert_equal( camera.project(point4), 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));
+  EXPECT(assert_equal( camera.backproject(Point2(-100,  100), 0.5),  point1));
-  CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
+  EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
-  CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5),  point3));
+  EXPECT(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),  point4));
 }
 
 /* ************************************************************************* */
 TEST( PinholeCamera, backprojectInfinity)
 {
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100,  100)),  point1_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100,  100)),  point1_inf));
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)),  point2_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)),  point2_inf));
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)),  point3_inf));
+  EXPECT(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)),  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<Point2, bool> x = camera.projectSafe(expected);
 
-  CHECK(assert_equal(expected, actual));
+  EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(Point2(), x.first));
+  EXPECT(assert_equal(Point2(), x.first));
-  CHECK(x.second);
+  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));
+  EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(Point2(), x));
+  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));
+  EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(Point2(), x));
+  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_pose  = numericalDerivative31(project3, pose, point1, K);
-  Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
+  Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
-  Matrix numerical_cal   = numericalDerivative33(project3, pose1, point1, K);
+  Matrix numerical_cal   = numericalDerivative33(project3, pose, point1, K);
-  CHECK(assert_equal(Point2(-100,  100), result));
+  EXPECT(assert_equal(Point2(-100,  100), result));
-  CHECK(assert_equal(numerical_pose,  Dpose,  1e-7));
+  EXPECT(assert_equal(numerical_pose,  Dpose,  1e-7));
-  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
-  CHECK(assert_equal(numerical_cal,   Dcal,   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_pose     = numericalDerivative31(projectInfinity3, pose, point3D, K);
-  Matrix numerical_point    = numericalDerivative32(projectInfinity3, pose1, point3D, K);
+  Matrix Hexpected2    = numericalDerivative32(projectInfinity3, pose, point3D, K);
-  Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
+  Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters
-  Matrix numerical_cal      = numericalDerivative33(projectInfinity3, pose1, point3D, K);
+  Matrix numerical_cal      = numericalDerivative33(projectInfinity3, pose, point3D, K);
-  CHECK(assert_equal(numerical_pose,     Dpose,  1e-7));
+  EXPECT(assert_equal(numerical_pose,     Dpose,  1e-7));
-  CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
+  EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7));
-  CHECK(assert_equal(numerical_cal,      Dcal,   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 Hexpected1 = numericalDerivative21(project4, camera, point1);
-  Matrix numerical_point  = numericalDerivative22(project4, camera, point1);
+  Matrix Hexpected2  = numericalDerivative22(project4, camera, point1);
-  CHECK(assert_equal(result, Point2(-100,  100) ));
+  EXPECT(assert_equal(result, Point2(-100,  100) ));
-  CHECK(assert_equal(numerical_camera, Dcamera, 1e-7));
+  EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
-  CHECK(assert_equal(numerical_point,  Dpoint,  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<double,1,11> 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<Cal3Bundler> 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<Cal3Bundler>(camera2);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, range2) {
+  typedef Eigen::Matrix<double,1,9> Matrix19;
+  Matrix1_11 D1; Matrix19 D2;
+  double result = camera.range<Cal3Bundler>(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));
 }
 
 /* ************************************************************************* */