Merge branch 'origin/release/2.4.0'

examples/CreateSFMExampleData.cpp

@@ -17,28 +17,29 @@
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+
+#include <boost/foreach.hpp>
 #include <boost/assign/std/vector.hpp>
 
 using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
-void create5PointExample1() {
+/* ************************************************************************* */
+
+void createExampleBALFile(const string& filename, const vector<Point3>& P,
+    const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
+        Cal3Bundler()) {
 
   // Class that will gather all data
   SfM_data data;
 
-  // Create two cameras and corresponding essential matrix E
+  // Create two cameras
   Rot3 aRb = Rot3::yaw(M_PI_2);
   Point3 aTb(0.1, 0, 0);
   Pose3 identity, aPb(aRb, aTb);
-  data.cameras.push_back(SfM_Camera(identity));
+  data.cameras.push_back(SfM_Camera(pose1, K));
-  data.cameras.push_back(SfM_Camera(aPb));
+  data.cameras.push_back(SfM_Camera(pose2, K));
-
-  // Create test data, we need at least 5 points
-  vector<Point3> P;
-  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
-  Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
 
   BOOST_FOREACH(const Point3& p, P) {
 
@@ -57,13 +58,53 @@ void create5PointExample1() {
     data.tracks.push_back(track);
   }
 
-  // Assumes example is run in ${GTSAM_TOP}/build/examples
-  const string filename = "../../examples/data/5pointExample1.txt";
   writeBAL(filename, data);
 }
 
+/* ************************************************************************* */
+
+void create5PointExample1() {
+
+  // Create two cameras poses
+  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Point3 aTb(0.1, 0, 0);
+  Pose3 pose1, pose2(aRb, aTb);
+
+  // Create test data, we need at least 5 points
+  vector<Point3> P;
+  P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
+  Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
+
+  // Assumes example is run in ${GTSAM_TOP}/build/examples
+  const string filename = "../../examples/data/5pointExample1.txt";
+  createExampleBALFile(filename, P, pose1, pose2);
+}
+
+/* ************************************************************************* */
+
+void create5PointExample2() {
+
+  // Create two cameras poses
+  Rot3 aRb = Rot3::yaw(M_PI_2);
+  Point3 aTb(10, 0, 0);
+  Pose3 pose1, pose2(aRb, aTb);
+
+  // Create test data, we need at least 5 points
+  vector<Point3> P;
+  P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
+  Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
+
+  // Assumes example is run in ${GTSAM_TOP}/build/examples
+  const string filename = "../../examples/data/5pointExample2.txt";
+  Cal3Bundler K(500, 0, 0);
+  createExampleBALFile(filename, P, pose1, pose2,K);
+}
+
+/* ************************************************************************* */
+
 int main(int argc, char* argv[]) {
   create5PointExample1();
+  create5PointExample2();
   return 0;
 }
 

examples/Data/5pointExample2.txt

@@ -0,0 +1,65 @@
+2 7 14
+
+0 0 0 -0
+1 0 -3.0616169978683830426e-15 -50
+0 1 -50 -0
+1 1 -6.1232339957367660852e-15 -100
+0 2 50 -0
+1 2 0 -0
+0 3 0 -500
+1 3 500 -100.00000000000002842
+0 4 0 500
+1 4 -500 -99.999999999999957367
+0 5 -125 -0
+1 5 -1.1481063742006437494e-14 -187.5
+0 6 125 312.5
+1 6 -312.5 62.500000000000028422
+
+3.141592653589793116
+0
+0
+-0
+0
+0
+500
+0
+0
+
+2.2214414690791830509
+2.2214414690791826068
+0
+-6.1232339957367662824e-16
+-10
+0
+500
+0
+0
+
+0
+0
+100
+
+-10
+0
+100
+
+10
+0
+100
+
+0
+50
+50
+
+0
+-50
+50
+
+-20
+0
+80
+
+20
+-50
+80
+

gtsam/slam/EssentialMatrixFactor.h

@@ -66,7 +66,8 @@ public:
 class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
     LieScalar> {
 
-  Point2 pA_, pB_; ///< Measurements in image A and B
+  Point3 dP1_; ///< 3D point corresponding to measurement in image 1
+  Point2 p1_, p2_; ///< Measurements in image 1 and image 2
   Cal3_S2 K_; ///< Calibration
 
   typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
@@ -77,7 +78,9 @@ public:
   /// Constructor
   EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
       const Cal3_S2& K, const SharedNoiseModel& model) :
-      Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) {
+      Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) {
+    Point2 xy = K_.calibrate(p1_);
+    dP1_ = Point3(xy.x(), xy.y(), 1);
   }
 
   /// @return a deep copy of this factor
@@ -91,7 +94,7 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
-        << pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
+        << p1_.vector().transpose() << ")' and (" << p2_.vector().transpose()
         << ")'" << std::endl;
   }
 
@@ -102,11 +105,11 @@ public:
 
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
-    // We then convert to first camera by 2P = 1R2Õ*(P1-1T2)
+    // We then convert to first camera by 2P = 1R2<EFBFBD>*(P1-1T2)
     // The homogeneous coordinates of can be written as
     //   2R1*(P1-1T2) ==  2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
     // Note that this is just a homography for d==0
-    Point3 dP1(pA_.x(), pA_.y(), 1);
+    // The point d*P1 = (x,y,1) is computed in constructor as dP1_
 
     // Project to normalized image coordinates, then uncalibrate
     Point2 pi;
@@ -114,7 +117,7 @@ public:
 
       Point3 _1T2 = E.direction().point3();
       Point3 d1T2 = d * _1T2;
-      Point3 dP2 = E.rotation().unrotate(dP1 - d1T2);
+      Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);
       Point2 pn = SimpleCamera::project_to_camera(dP2);
       pi = K_.uncalibrate(pn);
 
@@ -126,7 +129,7 @@ public:
 
       Point3 _1T2 = E.direction().point3(D_1T2_dir);
       Point3 d1T2 = d * _1T2;
-      Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point);
+      Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
       Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
       pi = K_.uncalibrate(pn, boost::none, Dpi_pn);
 
@@ -141,7 +144,7 @@ public:
         *Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
 
     }
-    Point2 reprojectionError = pi - pB_;
+    Point2 reprojectionError = pi - p2_;
     return reprojectionError.vector();
   }
 

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -20,6 +20,8 @@ using namespace gtsam;
 
 typedef noiseModel::Isotropic::shared_ptr Model;
 
+namespace example1 {
+
 const string filename = findExampleDataFile("5pointExample1.txt");
 SfM_data data;
 bool readOK = readBAL(filename, data);
@@ -207,12 +209,74 @@ TEST (EssentialMatrixFactor2, minimization) {
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
   EXPECT(assert_equal(trueE, actual,1e-1));
   for (size_t i = 0; i < 5; i++)
-    EXPECT(assert_equal(result.at<LieScalar>(i), truth.at<LieScalar>(i),1e-1));
+    EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
 }
 
+} // namespace example1
+
+//*************************************************************************
+
+namespace example2 {
+
+const string filename = findExampleDataFile("5pointExample2.txt");
+SfM_data data;
+bool readOK = readBAL(filename, data);
+Rot3 aRb = data.cameras[1].pose().rotation();
+Point3 aTb = data.cameras[1].pose().translation();
+double baseline = 10; // actual baseline of the camera
+
+Point2 pA(size_t i) {
+  return data.tracks[i].measurements[0].second;
+}
+Point2 pB(size_t i) {
+  return data.tracks[i].measurements[1].second;
+}
+
+// Matches Cal3Bundler K(500, 0, 0);
+Cal3_S2 K(500, 500, 0, 0, 0);
+
+//*************************************************************************
+TEST (EssentialMatrixFactor2, extraTest) {
+  // Additional test with camera moving in positive X direction
+
+  // We start with a factor graph and add constraints to it
+  // Noise sigma is 1, assuming pixel measurements
+  NonlinearFactorGraph graph;
+  Model model = noiseModel::Isotropic::Sigma(2, 1);
+  for (size_t i = 0; i < data.number_tracks(); i++)
+    graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
+
+  // Check error at ground truth
+  Values truth;
+  EssentialMatrix trueE(aRb, aTb);
+  truth.insert(100, trueE);
+  for (size_t i = 0; i < data.number_tracks(); i++) {
+    Point3 P1 = data.tracks[i].p;
+    truth.insert(i, LieScalar(baseline / P1.z()));
+  }
+  EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
+
+  // Optimize
+  LevenbergMarquardtParams parameters;
+  // parameters.setVerbosity("ERROR");
+  LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
+  Values result = optimizer.optimize();
+
+  // Check result
+  EssentialMatrix actual = result.at<EssentialMatrix>(100);
+  EXPECT(assert_equal(trueE, actual,1e-1));
+  for (size_t i = 0; i < data.number_tracks(); i++)
+    EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
+
+  // Check error at result
+  EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
+}
+
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;