Merge remote-tracking branch 'origin/develop' into develop

Conflicts:
	gtsam/geometry/Cal3DS2_Base.h

examples/Pose2SLAMExample_g2o.cpp

@@ -26,30 +26,53 @@
 using namespace std;
 using namespace gtsam;
 
+// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
 int main(const int argc, const char *argv[]) {
 
-  // Read graph from file
-  string g2oFile;
-  if (argc < 2)
-    g2oFile = findExampleDataFile("noisyToyGraph.txt");
-  else
-    g2oFile = argv[1];
+  string kernelType = "none";
+  int maxIterations = 100; // default
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
 
+  // Parse user's inputs
+  if (argc > 1){
+    g2oFile = argv[1]; // input dataset filename
+    // outputFile = g2oFile = argv[2]; // done later
+  }
+  if (argc > 3){
+    maxIterations = atoi(argv[3]); // user can specify either tukey or huber
+  }
+  if (argc > 4){
+    kernelType = argv[4]; // user can specify either tukey or huber
+  }
+
+  // reading file and creating factor graph
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
-  boost::tie(graph, initial) = readG2o(g2oFile);
+  bool is3D = false;
+  if(kernelType.compare("none") == 0){
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D);
+  }
+  if(kernelType.compare("huber") == 0){
+    std::cout << "Using robust kernel: huber " << std::endl;
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
+  }
+  if(kernelType.compare("tukey") == 0){
+    std::cout << "Using robust kernel: tukey " << std::endl;
+    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
+  }
 
   // Add prior on the pose having index (key) = 0
   NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  std::cout << "Adding prior on pose 0 " << std::endl;
 
   GaussNewtonParams params;
   params.setVerbosity("TERMINATION");
-  if (argc == 4) {
-    params.maxIterations = atoi(argv[3]);
-    std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl;
+  if (argc > 3) {
+    params.maxIterations = maxIterations;
+    std::cout << "User required to perform maximum  " << params.maxIterations << " iterations "<< std::endl;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
@@ -65,7 +88,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, result, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

gtsam/geometry/Cal3DS2.h

@@ -11,7 +11,7 @@
 
 /**
  * @file Cal3DS2.h
- * @brief Calibration of a camera with radial distortion
+ * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
  * @date Feb 28, 2010
  * @author ydjian
  */

gtsam/geometry/Cal3Unified.h

@@ -90,7 +90,7 @@ public:
   /**
    * convert intrinsic coordinates xy to image coordinates uv
    * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
+   * @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */

gtsam/geometry/tests/testCal3Unified.cpp

@@ -19,6 +19,9 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unified.h>
 
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Key.h>
+
 using namespace gtsam;
 
 GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
@@ -97,6 +100,19 @@ TEST( Cal3Unified, retract)
   CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
 }
 
+/* ************************************************************************* */
+TEST( Cal3Unified, DerivedValue)
+{
+  Values values;
+  Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
+  Key key = 1;
+  values.insert(key, cal);
+
+  Cal3Unified calafter = values.at<Cal3Unified>(key);
+
+  CHECK(assert_equal(cal,calafter,1e-9));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject)
 }
 
 /* ************************************************************************* */
-//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
-//  Point3 point(point2D.x(), point2D.y(), 1.0);
-//  return Camera(pose,cal).projectPointAtInfinity(point);
-//}
-//
-//TEST( PinholeCamera, Dproject_Infinity)
-//{
-//  Matrix Dpose, Dpoint, Dcal;
-//  Point2 point2D(-0.08,-0.08);
-//  Point3 point3D(point1.x(), point1.y(), 1.0);
-//  Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
-//  Matrix numerical_pose  = numericalDerivative31(projectInfinity3, pose1, point2D, K);
-//  Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K);
-//  Matrix numerical_cal   = numericalDerivative33(projectInfinity3, pose1, point2D, K);
-//  CHECK(assert_equal(numerical_pose,  Dpose,  1e-7));
-//  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
-//  CHECK(assert_equal(numerical_cal,   Dcal,   1e-7));
-//}
-//
+static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
+  return Camera(pose,cal).projectPointAtInfinity(point3D);
+}
+
+TEST( PinholeCamera, Dproject_Infinity)
+{
+  Matrix Dpose, Dpoint, Dcal;
+  Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
+
+  // test Projection
+  Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
+  Point2 expected(-5.0, 5.0);
+  CHECK(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));
+}
+
 /* ************************************************************************* */
 static Point2 project4(const Camera& camera, const Point3& point) {
   return camera.project2(point);

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -25,6 +25,7 @@
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/StereoCamera.h>
 #include <gtsam/geometry/StereoPoint2.h>
 
@@ -46,6 +47,7 @@ static Cal3Bundler cal3(1.0, 2.0, 3.0);
 static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
 static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
 static CalibratedCamera cal5(Pose3(rt3, pt3));
+static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0);
 
 static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
 static StereoCamera cam2(pose3, cal4ptr);
@@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) {
   EXPECT(equalsObj(cal3));
   EXPECT(equalsObj(cal4));
   EXPECT(equalsObj(cal5));
+  EXPECT(equalsObj(cal6));
 
   EXPECT(equalsObj(cam1));
   EXPECT(equalsObj(cam2));

gtsam/nonlinear/NonlinearOptimizer.cpp

@@ -77,10 +77,12 @@ void NonlinearOptimizer::defaultOptimize() {
             params.errorTol, currentError, this->error(), params.verbosity));
 
   // Printing if verbose
-  if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
-      this->iterations() >= params.maxIterations)
+  if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
+    cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl;
+    if (this->iterations() >= params.maxIterations)
       cout << "Terminating because reached maximum iterations" << endl;
   }
+}
 
 /* ************************************************************************* */
 const Values& NonlinearOptimizer::optimizeSafely() {