another unit test for distinct calibrations, more documentation

gtsam_unstable/geometry/tests/testTriangulation.cpp

@@ -32,7 +32,7 @@ using namespace gtsam;
 using namespace boost::assign;
 
 /* ************************************************************************* */
-TEST( triangulation, twoPoses) {
+TEST( triangulation, fourPoses) {
   Cal3_S2 K(1500, 1200, 0, 640, 480);
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
   Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
@@ -95,6 +95,79 @@ TEST( triangulation, twoPoses) {
   CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), TriangulationCheiralityException);
 }
 
+/* ************************************************************************* */
+TEST( triangulation, fourPoses_distinct_Ks) {
+  Cal3_S2 K1(1500, 1200, 0, 640, 480);
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  SimpleCamera level_camera(level_pose, K1);
+
+  // create second camera 1 meter to the right of first camera
+  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+  Cal3_S2 K2(1600, 1300, 0, 650, 440);
+  SimpleCamera level_camera_right(level_pose_right, K2);
+
+  // landmark ~5 meters infront of camera
+  Point3 landmark(5, 0.5, 1.2);
+
+
+  // 1. Project two landmarks into two cameras and triangulate
+  Point2 level_uv = level_camera.project(landmark);
+  Point2 level_uv_right = level_camera_right.project(landmark);
+
+  vector<Pose3> poses;
+  vector<Point2> measurements;
+
+  poses += level_pose, level_pose_right;
+  measurements += level_uv, level_uv_right;
+
+  std::vector<boost::shared_ptr<Cal3_S2> > Ks;
+  Ks.push_back(boost::make_shared<Cal3_S2>(K1));
+  Ks.push_back(boost::make_shared<Cal3_S2>(K2));
+
+  boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, measurements, Ks);
+  EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
+
+
+  // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
+  measurements.at(0) += Point2(0.1,0.5);
+  measurements.at(1) += Point2(-0.2, 0.3);
+
+  boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, measurements, Ks);
+  EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
+
+
+  // 3. Add a slightly rotated third camera above, again with measurement noise
+  Pose3 pose_top = level_pose * Pose3(Rot3::ypr(0.1,0.2,0.1), Point3(0.1,-2,-.1));
+  Cal3_S2 K3(700, 500, 0, 640, 480);
+  SimpleCamera camera_top(pose_top, K3);
+  Point2 top_uv = camera_top.project(landmark);
+
+  poses += pose_top;
+  measurements += top_uv + Point2(0.1, -0.1);
+  Ks.push_back(boost::make_shared<Cal3_S2>(K3));
+
+  boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, Ks);
+  EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
+
+  // Again with nonlinear optimization
+  boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, measurements, Ks, 1e-9, true);
+  EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
+
+  // 4. Test failure: Add a 4th camera facing the wrong way
+  Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI/2, 0., -M_PI/2), Point3(0,0,1));
+  Cal3_S2 K4(700, 500, 0, 640, 480);
+  SimpleCamera camera_180(level_pose180, K4);
+
+  CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException);
+
+  poses += level_pose180;
+  measurements += Point2(400,400);
+  Ks.push_back(boost::make_shared<Cal3_S2>(K4));
+
+  CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks), TriangulationCheiralityException);
+}
+
 /* ************************************************************************* */
 TEST( triangulation, twoIdenticalPoses) {
   Cal3_S2 K(1500, 1200, 0, 640, 480);

gtsam_unstable/geometry/triangulation.h

@@ -56,9 +56,22 @@ public:
 
 /* ************************************************************************* */
 // See Hartley and Zisserman, 2nd Ed., page 312
+/**
+ *
+ * @param poses Camera poses
+ * @param projection_matrices Projection matrices (K*P^-1)
+ * @param measurements 2D measurements
+ * @param Ks vector of calibrations
+ * @param rank_tol SVD rank tolerance
+ * @param Flag to turn on nonlinear refinement of triangulation
+ * @return Triangulated Point3
+ */
 template<class CALIBRATION>
-Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Matrix>& projection_matrices,
+Point3 triangulateDLT(const std::vector<Pose3>& poses,
-    const std::vector<Point2>& measurements, const  std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol, bool optimize) {
+    const std::vector<Matrix>& projection_matrices,
+    const std::vector<Point2>& measurements,
+    const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
+    bool optimize) {
 
   Matrix A = zeros(projection_matrices.size() *2, 4);