Merge remote-tracking branch 'origin/feature/ExpressionsBALExample' into feature/Feature/FixedValues

Conflicts:
	examples/SFMExampleExpressions_bal.cpp

examples/RangeISAMExample_plaza2.cpp

@@ -82,7 +82,8 @@ vector<RangeTriple> readTriples() {
   ifstream is(data_file.c_str());
 
   while (is) {
-    double t, sender, receiver, range;
+    double t, sender, range;
+	size_t receiver;
     is >> t >> sender >> receiver >> range;
     triples.push_back(RangeTriple(t, receiver, range));
   }

examples/SFMExampleExpressions_bal.cpp

@@ -27,12 +27,8 @@
 
 // Header order is close to far
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/dataset.h> // for loading BAL datasets !
-
 #include <vector>
 
 using namespace std;
@@ -45,16 +41,19 @@ using symbol_shorthand::P;
 // and has a total of 9 free parameters
 
 /* ************************************************************************* */
-int main (int argc, char* argv[]) {
+int main(int argc, char* argv[]) {
 
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  if (argc>1) filename = string(argv[1]);
+  if (argc > 1)
+    filename = string(argv[1]);
 
   // Load the SfM data from file
   SfM_data mydata;
-  assert(readBAL(filename, mydata));
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+  readBAL(filename, mydata);
+  cout
+      << boost::format("read %1% tracks on %2% cameras\n")
+          % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   ExpressionFactorGraph graph;
@@ -63,17 +62,19 @@ int main (int argc, char* argv[]) {
   // First, we create an expression to the pose from the first camera
   Expression<SfM_Camera> camera0_(C(0));
   // Then, to get its pose:
-  Pose3_ pose0_(&SfM_Camera::pose,camera0_);
+  Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
   // Finally, we say it should be equal to first guess
-  graph.addExpressionFactor(pose0_, mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
+  graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
+      noiseModel::Isotropic::Sigma(6, 0.1));
 
   // similarly, we create a prior on the first point
   Point3_ point0_(P(0));
-  graph.addExpressionFactor(point0_, mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
+  graph.addExpressionFactor(point0_, mydata.tracks[0].p,
+      noiseModel::Isotropic::Sigma(3, 0.1));
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
+      1.0); // one pixel in u and v
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   size_t j = 0;
@@ -86,7 +87,7 @@ int main (int argc, char* argv[]) {
       // Leaf expression for i^th camera
       Expression<SfM_Camera> camera_(C(i));
       // Below an expression for the prediction of the measurement:
-      Point2_ predict_ = project2<SfM_Camera>(camera_,point_);
+      Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
       // Again, here we use an ExpressionFactor
       graph.addExpressionFactor(predict_, uv, noise);
     }
@@ -95,9 +96,12 @@ int main (int argc, char* argv[]) {
 
   // Create initial estimate
   Values initial;
-  size_t i = 0; j = 0;
-  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
-  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);
+  size_t i = 0;
+  j = 0;
+  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
+    initial.insert(C(i++), camera);
+  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
+    initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;

examples/SFMExample_bal.cpp

@@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
 
   // Load the SfM data from file
   SfM_data mydata;
-  assert(readBAL(filename, mydata));
+  readBAL(filename, mydata);
   cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph

examples/TimeTBB.cpp

@@ -134,7 +134,7 @@ map<int, double> testWithMemoryAllocation()
     tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
     tbb::tick_count t1 = tbb::tick_count::now();
     cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
-    timingResults[grainSize] = (t1 - t0).seconds();
+    timingResults[(int)grainSize] = (t1 - t0).seconds();
   }
 
   return timingResults;
@@ -152,9 +152,9 @@ int main(int argc, char* argv[])
   BOOST_FOREACH(size_t n, numThreads)
   {
     cout << "With " << n << " threads:" << endl;
-    tbb::task_scheduler_init init(n);
-    results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
-    results[n].grainSizesWithAllocation = testWithMemoryAllocation();
+    tbb::task_scheduler_init init((int)n);
+    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
+    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
     cout << endl;
   }
 

gtsam/geometry/Cal3Bundler.h

@@ -174,4 +174,7 @@ private:
 template<>
 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
+template<>
+struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
+
 } // namespace gtsam

gtsam/geometry/Cal3DS2.h

@@ -112,5 +112,8 @@ private:
 template<>
 struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
+template<>
+struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
+
 }
 

gtsam/geometry/Cal3Unified.h

@@ -142,5 +142,8 @@ private:
 template<>
 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
+template<>
+struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
+
 }
 

gtsam/geometry/Cal3_S2.h

@@ -226,4 +226,7 @@ private:
 template<>
 struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
+template<>
+struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
+
 } // \ namespace gtsam

gtsam/geometry/Cal3_S2Stereo.h

@@ -158,4 +158,8 @@ namespace gtsam {
   struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
   };
 
+  template<>
+  struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
+  };
+
 } // \ namespace gtsam

gtsam/geometry/CalibratedCamera.h

@@ -204,5 +204,8 @@ private:
 template<>
 struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
 
+template<>
+struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
+
 }
 

gtsam/geometry/EssentialMatrix.h

@@ -201,5 +201,8 @@ private:
 template<>
 struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
 
+template<>
+struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
+
 } // gtsam
 

gtsam/geometry/PinholeCamera.h

@@ -151,11 +151,20 @@ public:
     return pose_;
   }
 
-  /// return pose
+  /// return pose, constant version
   inline const Pose3& pose() const {
     return pose_;
   }
 
+  /// return pose, with derivative
+  inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
+    if (H) {
+      H->setZero();
+      H->block(0, 0, 6, 6) = I_6x6;
+    }
+    return pose_;
+  }
+
   /// return calibration
   inline Calibration& calibration() {
     return K_;
@@ -499,4 +508,7 @@ private:
 template<typename Calibration>
 struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
 
+template<typename Calibration>
+struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
+
 } // \ gtsam

gtsam/geometry/Point3.h

@@ -197,4 +197,7 @@ namespace gtsam {
 
   template<>
   struct traits<Point3> : public internal::VectorSpace<Point3> {};
+
+ template<>
+  struct traits<const Point3> : public internal::VectorSpace<Point3> {};
 }

gtsam/geometry/Pose2.h

@@ -293,5 +293,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 template<>
 struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
 
+template<>
+struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
+
 } // namespace gtsam
 

gtsam/geometry/Pose3.h

@@ -325,4 +325,8 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 template<>
 struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
 
+template<>
+struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
+
+
 } // namespace gtsam

gtsam/geometry/Rot2.h

@@ -210,4 +210,7 @@ namespace gtsam {
   template<>
   struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
 
+  template<>
+  struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
+
 } // gtsam

gtsam/geometry/Rot3.h

@@ -463,5 +463,8 @@ namespace gtsam {
 
   template<>
   struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
+  
+  template<>
+  struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
 }
 

gtsam/geometry/SO3.h

@@ -85,6 +85,7 @@ public:
 template<>
 struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
 
-
+template<>
+struct traits<const SO3> : public internal::LieGroupTraits<SO3> {};
 } // end namespace gtsam
 

gtsam/geometry/StereoCamera.h

@@ -149,4 +149,6 @@ private:
 template<>
 struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
 
+template<>
+struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
 }

gtsam/geometry/StereoPoint2.h

@@ -176,4 +176,7 @@ namespace gtsam {
 
   template<>
   struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
+
+  template<>
+  struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
 }

gtsam/geometry/Unit3.h

@@ -171,5 +171,7 @@ private:
 // Define GTSAM traits
 template <> struct traits<Unit3> : public internal::Manifold<Unit3> {};
 
+template <> struct traits<const Unit3> : public internal::Manifold<Unit3> {};
+
 } // namespace gtsam
 

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -51,8 +51,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
 /* ************************************************************************* */
 TEST( PinholeCamera, constructor)
 {
-  EXPECT(assert_equal( camera.calibration(), K));
-  EXPECT(assert_equal( camera.pose(), pose));
+  EXPECT(assert_equal( K, camera.calibration()));
+  EXPECT(assert_equal( pose, camera.pose()));
+}
+
+//******************************************************************************
+TEST(PinholeCamera, Pose) {
+
+  Matrix actualH;
+  EXPECT(assert_equal(pose, camera.pose(actualH)));
+
+  // Check derivative
+  boost::function<Pose3(Camera)> f = //
+      boost::bind(&Camera::pose,_1,boost::none);
+  Matrix numericalH = numericalDerivative11<Pose3,Camera>(&Camera::getPose,camera);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
 /* ************************************************************************* */

gtsam/slam/dataset.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/linear/Sampler.h>
 #include <gtsam/inference/Symbol.h>