Merge branch 'develop'

.cproject

@@ -1551,6 +1551,14 @@
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testLabeledSymbol.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testLabeledSymbol.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2521,6 +2529,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testOptionalJacobian.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testOptionalJacobian.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>

cmake/GtsamBuildTypes.cmake

@@ -54,7 +54,7 @@ if(NOT FIRST_PASS_DONE)
 endif()
 
 # Clang on Mac uses a template depth that is less than standard and is too small
-if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
 	if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
 		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
 	endif()

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/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);
+    tbb::task_scheduler_init init((int)n);
-    results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
+    results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
-    results[n].grainSizesWithAllocation = testWithMemoryAllocation();
+    results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
     cout << endl;
   }
 

gtsam.h

@@ -629,28 +629,13 @@ class Cal3_S2 {
   void serialize() const;
 };
 
-#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_Base.h>
-class Cal3DS2 {
+virtual class Cal3DS2_Base {
   // Standard Constructors
-  Cal3DS2();
+  Cal3DS2_Base();
-  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
-  Cal3DS2(Vector v);
 
   // Testable
   void print(string s) const;
-  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
-
-  // Manifold
-  static size_t Dim();
-  size_t dim() const;
-  gtsam::Cal3DS2 retract(Vector v) const;
-  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
-
-  // Action on Point2
-  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
-  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
-  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
-  // TODO: D2d functions that start with an uppercase letter
 
   // Standard Interface
   double fx() const;
@@ -658,14 +643,66 @@ class Cal3DS2 {
   double skew() const;
   double px() const;
   double py() const;
-  Vector vector() const;
+  double k1() const;
-  Vector k() const;
+  double k2() const;
-  //Matrix K() const; //FIXME: Uppercase
+
+  // Action on Point2
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p) const;
 
   // enabling serialization functionality
   void serialize() const;
 };
 
+#include <gtsam/geometry/Cal3DS2.h>
+virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3DS2();
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
+  Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
+  Cal3DS2(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3DS2 retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3DS2& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3Unified.h>
+virtual class Cal3Unified : gtsam::Cal3DS2_Base {
+  // Standard Constructors
+  Cal3Unified();
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
+  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
+  Cal3Unified(Vector v);
+
+  // Testable
+  bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
+
+  // Standard Interface
+  double xi() const;
+  gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
+  gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
+
+  // Manifold
+  size_t dim() const;
+  static size_t Dim();
+  gtsam::Cal3Unified retract(Vector v) const;
+  Vector localCoordinates(const gtsam::Cal3Unified& c) const;
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+#include <gtsam/geometry/Cal3_S2Stereo.h>
 class Cal3_S2Stereo {
   // Standard Constructors
   Cal3_S2Stereo();

gtsam/3rdparty/metis/CMakeLists.txt

@@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8)
 project(METIS)
 
 # Add flags for currect directory and below
-if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
   add_definitions(-Wno-unused-variable)
   if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0)
     add_definitions(-Wno-sometimes-uninitialized)
   endif()
 endif()
 
-if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
+if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC"))
   #add_definitions(-Wno-unknown-pragmas)
 endif()
 

gtsam/base/Manifold.h

@@ -71,7 +71,7 @@ struct HasManifoldPrereqs {
 };
 
 /// Extra manifold traits for fixed-dimension types
-template<class Class, size_t N>
+template<class Class, int N>
 struct ManifoldImpl {
   // Compile-time dimensionality
   static int GetDimension(const Class&) {

gtsam/base/Matrix.h

@@ -238,7 +238,7 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
  * @param j is the column of the upper left corner insert location
  */
 template <typename Derived1, typename Derived2>
-GTSAM_EXPORT void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
+void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
   fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
 }
 

gtsam/base/OptionalJacobian.h

@@ -100,7 +100,7 @@ public:
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return map_.data();
+    return map_.data() != NULL;
   }
 
   /// De-reference, like boost optional
@@ -156,7 +156,7 @@ public:
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return pointer_;
+    return pointer_!=NULL;
   }
 
   /// De-reference, like boost optional

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

@@ -68,7 +68,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const ;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@@ -89,10 +89,20 @@ public:
   /// Return dimensions of calibration manifold object
   static size_t Dim() { return 9; }  //TODO: make a final dimension variable
 
+  /// @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  virtual boost::shared_ptr<Base> clone() const {
+    return boost::shared_ptr<Base>(new Cal3DS2(*this));
+  }
+
+  /// @}
+
 
 private:
 
-  /// @}
   /// @name Advanced Interface
   /// @{
 
@@ -112,5 +122,8 @@ private:
 template<>
 struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
+template<>
+struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
+
 }
 

gtsam/geometry/Cal3DS2_Base.h

@@ -45,9 +45,6 @@ protected:
   double p1_, p2_ ; // tangential distortion
 
 public:
-  Matrix3 K() const ;
-  Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
-  Vector9 vector() const ;
 
   /// @name Standard Constructors
   /// @{
@@ -59,6 +56,8 @@ public:
       double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
   fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
 
+  virtual ~Cal3DS2_Base() {}
+
   /// @}
   /// @name Advanced Constructors
   /// @{
@@ -70,7 +69,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const ;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
@@ -106,6 +105,15 @@ public:
   /// Second tangential distortion coefficient
   inline double p2() const { return p2_;}
 
+  /// return calibration matrix -- not really applicable
+  Matrix3 K() const;
+
+  /// return distortion parameter vector
+  Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
+
+  /// Return all parameters as a vector
+  Vector9 vector() const;
+
   /**
    * convert intrinsic coordinates xy to (distorted) image coordinates uv
    * @param p point in intrinsic coordinates
@@ -126,9 +134,19 @@ public:
   /// Derivative of uncalibrate wrpt the calibration parameters
   Matrix29 D2d_calibration(const Point2& p) const ;
 
-private:
+  /// @}
+  /// @name Clone
+  /// @{
+
+  /// @return a deep copy of this object
+  virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
+    return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
+  }
 
   /// @}
+
+private:
+
   /// @name Advanced Interface
   /// @{
 

gtsam/geometry/Cal3Unified.h

@@ -50,9 +50,8 @@ private:
   double xi_;  // mirror parameter
 
 public:
-  enum { dimension = 10 };
 
-    Vector10 vector() const ;
+  enum { dimension = 10 };
 
   /// @name Standard Constructors
   /// @{
@@ -77,7 +76,7 @@ public:
   /// @{
 
   /// print with optional string
-  void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const ;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Unified& K, double tol = 10e-9) const;
@@ -125,6 +124,11 @@ public:
   /// Return dimensions of calibration manifold object
   static size_t Dim() { return 10; }  //TODO: make a final dimension variable
 
+  /// Return all parameters as a vector
+  Vector10 vector() const ;
+
+  /// @}
+
 private:
 
   /** Serialization function */
@@ -142,5 +146,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

@@ -499,4 +499,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

@@ -323,7 +323,10 @@ typedef std::pair<Point3, Point3> Point3Pair;
 GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 
 template<>
-struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {
+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

@@ -155,7 +155,13 @@ private:
   template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_NVP(p_);
-      ar & BOOST_SERIALIZATION_NVP(B_);
+      // homebrew serialize Eigen Matrix
+      ar & boost::serialization::make_nvp("B11", (*B_)(0,0));
+      ar & boost::serialization::make_nvp("B12", (*B_)(0,1));
+      ar & boost::serialization::make_nvp("B21", (*B_)(1,0));
+      ar & boost::serialization::make_nvp("B22", (*B_)(1,1));
+      ar & boost::serialization::make_nvp("B31", (*B_)(2,0));
+      ar & boost::serialization::make_nvp("B32", (*B_)(2,1));
     }
 
   /// @}
@@ -165,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/testSerializationGeometry.cpp

@@ -19,6 +19,8 @@
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Unit3.h>
+#include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/CalibratedCamera.h>
@@ -41,6 +43,9 @@ static Point3 pt3(1.0, 2.0, 3.0);
 static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
 static Pose3 pose3(rt3, pt3);
 
+static Unit3 unit3(1.0, 2.1, 3.4);
+static EssentialMatrix ematrix(rt3, unit3);
+
 static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
 static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
 static Cal3Bundler cal3(1.0, 2.0, 3.0);
@@ -59,6 +64,9 @@ TEST (Serialization, text_geometry) {
   EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
   EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
 
+  EXPECT(equalsObj<gtsam::Unit3>(Unit3(1.0, 2.1, 3.4)));
+  EXPECT(equalsObj<gtsam::EssentialMatrix>(EssentialMatrix(rt3, unit3)));
+
   EXPECT(equalsObj(pt3));
   EXPECT(equalsObj<gtsam::Rot3>(rt3));
   EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
@@ -81,6 +89,9 @@ TEST (Serialization, xml_geometry) {
   EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
   EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
 
+  EXPECT(equalsXML<gtsam::Unit3>(Unit3(1.0, 2.1, 3.4)));
+  EXPECT(equalsXML<gtsam::EssentialMatrix>(EssentialMatrix(rt3, unit3)));
+
   EXPECT(equalsXML<gtsam::Point3>(pt3));
   EXPECT(equalsXML<gtsam::Rot3>(rt3));
   EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
@@ -102,6 +113,9 @@ TEST (Serialization, binary_geometry) {
   EXPECT(equalsBinary<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
   EXPECT(equalsBinary<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
 
+  EXPECT(equalsBinary<gtsam::Unit3>(Unit3(1.0, 2.1, 3.4)));
+  EXPECT(equalsBinary<gtsam::EssentialMatrix>(EssentialMatrix(rt3, unit3)));
+
   EXPECT(equalsBinary<gtsam::Point3>(pt3));
   EXPECT(equalsBinary<gtsam::Rot3>(rt3));
   EXPECT(equalsBinary<gtsam::Pose3>(Pose3(rt3, pt3)));

gtsam/inference/LabeledSymbol.cpp

@@ -17,12 +17,8 @@
 
 #include <iostream>
 
-#include <boost/mpl/char.hpp>
 #include <boost/format.hpp>
-#include <boost/function.hpp>
+#include <boost/bind.hpp>
-#include <boost/lambda/bind.hpp>
-#include <boost/lambda/construct.hpp>
-#include <boost/lambda/lambda.hpp>
 
 #include <boost/lexical_cast.hpp>
 
@@ -111,23 +107,21 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const {
 }
 
 /* ************************************************************************* */
+static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);}
+
 boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
-  namespace bl = boost::lambda;
+  return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c;
-  return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == c;
 }
 
-/* ************************************************************************* */
 boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
-  namespace bl = boost::lambda;
+  return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
-  return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == label;
 }
 
-/* ************************************************************************* */
 boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
-  namespace bl = boost::lambda;
+  return boost::bind(&LabeledSymbol::chr,   boost::bind(make, _1)) == c &&
-  return bl::bind(&LabeledSymbol::chr,   bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == c &&
+      boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label;
-      bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == label;
 }
+/* ************************************************************************* */
 
 } // \namespace gtsam
 

gtsam/inference/Symbol.cpp

@@ -18,19 +18,8 @@
 
 #include <gtsam/inference/Symbol.h>
 
-#include <boost/mpl/char.hpp>
 #include <boost/format.hpp>
-#include <boost/function.hpp>
+#include <boost/bind.hpp>
-#include <boost/lambda/construct.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wunused-variable"
-#endif
-#include <boost/lambda/bind.hpp>
-#include <boost/lambda/lambda.hpp>
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
 
 #include <limits.h>
 #include <list>
@@ -71,10 +60,10 @@ Symbol::operator std::string() const {
   return str(boost::format("%c%d") % c_ % j_);
 }
 
+static Symbol make(gtsam::Key key) { return Symbol(key);}
+
 boost::function<bool(Key)> Symbol::ChrTest(unsigned char c) {
-  namespace bl = boost::lambda;
+  return bind(&Symbol::chr, bind(make, _1)) == c;
-  return bl::bind(&Symbol::chr, bl::bind(bl::constructor<Symbol>(), bl::_1))
-  == c;
 }
 
 } // namespace gtsam

gtsam/inference/tests/testKey.cpp

@@ -14,14 +14,14 @@
  * @author Alex Cunningham
  */
 
-#include <boost/assign/std/list.hpp> // for operator +=
+#include <gtsam/inference/Symbol.h>
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
-#include <gtsam/inference/Symbol.h>
 
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
@@ -65,6 +65,13 @@ TEST(Key, KeySymbolEncoding) {
   EXPECT(assert_equal(symbol, Symbol(key)));
 }
 
+/* ************************************************************************* */
+TEST(Key, ChrTest) {
+  Key key = Symbol('c',3);
+  EXPECT(Symbol::ChrTest('c')(key));
+  EXPECT(!Symbol::ChrTest('d')(key));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/inference/tests/testLabeledSymbol.cpp

@@ -14,20 +14,19 @@
  * @author Alex Cunningham
  */
 
-#include <boost/assign/std/list.hpp> // for operator +=
+#include <gtsam/inference/LabeledSymbol.h>
-using namespace boost::assign;
-
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
 
-#include <gtsam/inference/LabeledSymbol.h>
+#include <CppUnitLite/TestHarness.h>
-  
+#include <boost/assign/std/list.hpp> // for operator +=
+
+using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) {
+TEST(LabeledSymbol, KeyLabeledSymbolConversion ) {
   LabeledSymbol expected('x', 'A', 4);
   Key key(expected);
   LabeledSymbol actual(key);
@@ -36,7 +35,7 @@ TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) {
 }
 
 /* ************************************************************************* */
-TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) {
+TEST(LabeledSymbol, KeyLabeledSymbolEncoding ) {
 
   // Test encoding of LabeledSymbol <-> size_t <-> string
   // Encoding scheme:
@@ -69,6 +68,17 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) {
   }
 }
 
+/* ************************************************************************* */
+TEST(LabeledSymbol, ChrTest) {
+  Key key = LabeledSymbol('c','A',3);
+  EXPECT(LabeledSymbol::TypeTest('c')(key));
+  EXPECT(!LabeledSymbol::TypeTest('d')(key));
+  EXPECT(LabeledSymbol::LabelTest('A')(key));
+  EXPECT(!LabeledSymbol::LabelTest('D')(key));
+  EXPECT(LabeledSymbol::TypeLabelTest('c','A')(key));
+  EXPECT(!LabeledSymbol::TypeLabelTest('c','D')(key));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/linear/GaussianFactor.h

@@ -100,7 +100,7 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const = 0;
 
-    /// Return the diagonal of the Hessian for this factor (raw memory version)
+    /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const = 0;
 
     /// Return the block diagonal of the Hessian for this factor
@@ -122,16 +122,10 @@ namespace gtsam {
     /// y += alpha * A'*A*x
     virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;
 
-    /// y += alpha * A'*A*x
-    virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const = 0;
-
-    /// y += alpha * A'*A*x
-    virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0;
-
     /// A'*b for Jacobian, eta for Hessian
     virtual VectorValues gradientAtZero() const = 0;
 
-    /// A'*b for Jacobian, eta for Hessian (raw memory version)
+    /// Raw memory access version of gradientAtZero
     virtual void gradientAtZero(double* d) const = 0;
 
     /// Gradient wrt a key at any values

gtsam/linear/GaussianFactorGraph.cpp

@@ -357,15 +357,6 @@ namespace gtsam {
      f->multiplyHessianAdd(alpha, x, y);
   }
 
-  /* ************************************************************************* */
-  void GaussianFactorGraph::multiplyHessianAdd(double alpha,
-      const double* x, double* y) const {
-  vector<size_t> FactorKeys = getkeydim();
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
-      f->multiplyHessianAdd(alpha, x, y, FactorKeys);
-
-  }
-
   /* ************************************************************************* */
   void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
     multiplyInPlace(x, e.begin());

gtsam/linear/GaussianFactorGraph.h

@@ -310,10 +310,6 @@ namespace gtsam {
     void multiplyHessianAdd(double alpha, const VectorValues& x,
         VectorValues& y) const;
 
-    ///** y += alpha*A'A*x */
-    void multiplyHessianAdd(double alpha, const double* x,
-        double* y) const;
-
     ///** In-place version e <- A*x that overwrites e. */
     void multiplyInPlace(const VectorValues& x, Errors& e) const;
 

gtsam/linear/HessianFactor.cpp

@@ -359,20 +359,10 @@ VectorValues HessianFactor::hessianDiagonal() const {
 }
 
 /* ************************************************************************* */
-// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+// Raw memory access version should be called in Regular Factors only currently
 void HessianFactor::hessianDiagonal(double* d) const {
-
+  throw std::runtime_error(
-  // Use eigen magic to access raw memory
+      "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
-  typedef Eigen::Matrix<double, 9, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-
-  // Loop over all variables in the factor
-  for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
-    Key j = keys_[pos];
-    // Get the diagonal block, and insert its diagonal
-    const Matrix& B = info_(pos, pos).selfadjointView();
-    DMap(d + 9 * j) += B.diagonal();
-  }
 }
 
 /* ************************************************************************* */
@@ -548,48 +538,6 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
   }
 }
 
-/* ************************************************************************* */
-void HessianFactor::multiplyHessianAdd(double alpha, const double* x,
-    double* yvalues, vector<size_t> offsets) const {
-
-  // Use eigen magic to access raw memory
-  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-  typedef Eigen::Map<const DVector> ConstDMap;
-
-  // Create a vector of temporary y values, corresponding to rows i
-  vector<Vector> y;
-  y.reserve(size());
-  for (const_iterator it = begin(); it != end(); it++)
-    y.push_back(zero(getDim(it)));
-
-  // Accessing the VectorValues one by one is expensive
-  // So we will loop over columns to access x only once per column
-  // And fill the above temporary y values, to be added into yvalues after
-  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
-    DenseIndex i = 0;
-    for (; i < j; ++i)
-      y[i] += info_(i, j).knownOffDiagonal()
-          * ConstDMap(x + offsets[keys_[j]],
-              offsets[keys_[j] + 1] - offsets[keys_[j]]);
-    // blocks on the diagonal are only half
-    y[i] += info_(j, j).selfadjointView()
-        * ConstDMap(x + offsets[keys_[j]],
-            offsets[keys_[j] + 1] - offsets[keys_[j]]);
-    // for below diagonal, we take transpose block from upper triangular part
-    for (i = j + 1; i < (DenseIndex) size(); ++i)
-      y[i] += info_(i, j).knownOffDiagonal()
-          * ConstDMap(x + offsets[keys_[j]],
-              offsets[keys_[j] + 1] - offsets[keys_[j]]);
-  }
-
-  // copy to yvalues
-  for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
-    DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
-        alpha * y[i];
-}
-
-
 /* ************************************************************************* */
 VectorValues HessianFactor::gradientAtZero() const {
   VectorValues g;
@@ -600,20 +548,10 @@ VectorValues HessianFactor::gradientAtZero() const {
 }
 
 /* ************************************************************************* */
-// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+// Raw memory access version should be called in Regular Factors only currently
 void HessianFactor::gradientAtZero(double* d) const {
-
+  throw std::runtime_error(
-  // Use eigen magic to access raw memory
+      "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
-  typedef Eigen::Matrix<double, 9, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-
-  // Loop over all variables in the factor
-    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
-      Key j = keys_[pos];
-      // Get the diagonal block, and insert its diagonal
-      DVector dj =  -info_(pos,size()).knownOffDiagonal();
-      DMap(d + 9 * j) += dj;
-    }
 }
 
 /* ************************************************************************* */

gtsam/linear/HessianFactor.h

@@ -340,7 +340,7 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const;
 
-    /* ************************************************************************* */
+    /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const;
 
     /// Return the block diagonal of the Hessian for this factor
@@ -380,13 +380,10 @@ namespace gtsam {
     /** y += alpha * A'*A*x */
     void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
 
-    void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const;
-
-    void multiplyHessianAdd(double alpha, const double* x, double* y) const {};
-
     /// eta for Hessian
     VectorValues gradientAtZero() const;
 
+    /// Raw memory access version of gradientAtZero
     virtual void gradientAtZero(double* d) const;
 
     /**

gtsam/linear/JacobianFactor.cpp

@@ -461,22 +461,9 @@ VectorValues JacobianFactor::hessianDiagonal() const {
 }
 
 /* ************************************************************************* */
-// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+// Raw memory access version should be called in Regular Factors only currently
 void JacobianFactor::hessianDiagonal(double* d) const {
-
+  throw std::runtime_error("JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only");
-  // Use eigen magic to access raw memory
-  typedef Eigen::Matrix<double, 9, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-
-  // Loop over all variables in the factor
-  for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
-    // Get the diagonal block, and insert its diagonal
-    DVector dj;
-    for (size_t k = 0; k < 9; ++k)
-      dj(k) = Ab_(j).col(k).squaredNorm();
-
-    DMap(d + 9 * j) += dj;
-  }
 }
 
 /* ************************************************************************* */
@@ -528,40 +515,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
   transposeMultiplyAdd(alpha, Ax, y);
 }
 
-void JacobianFactor::multiplyHessianAdd(double alpha, const double* x,
-    double* y, std::vector<size_t> keys) const {
-
-  // Use eigen magic to access raw memory
-  typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-  typedef Eigen::Map<const DVector> ConstDMap;
-
-  if (empty())
-    return;
-  Vector Ax = zero(Ab_.rows());
-
-  // Just iterate over all A matrices and multiply in correct config part
-  for (size_t pos = 0; pos < size(); ++pos)
-    Ax += Ab_(pos)
-        * ConstDMap(x + keys[keys_[pos]],
-            keys[keys_[pos] + 1] - keys[keys_[pos]]);
-
-  // Deal with noise properly, need to Double* whiten as we are dividing by variance
-  if (model_) {
-    model_->whitenInPlace(Ax);
-    model_->whitenInPlace(Ax);
-  }
-
-  // multiply with alpha
-  Ax *= alpha;
-
-  // Again iterate over all A matrices and insert Ai^e into y
-  for (size_t pos = 0; pos < size(); ++pos)
-    DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_(
-        pos).transpose() * Ax;
-
-}
-
 /* ************************************************************************* */
 VectorValues JacobianFactor::gradientAtZero() const {
   VectorValues g;
@@ -574,8 +527,9 @@ VectorValues JacobianFactor::gradientAtZero() const {
 }
 
 /* ************************************************************************* */
+// Raw memory access version should be called in Regular Factors only currently
 void JacobianFactor::gradientAtZero(double* d) const {
-  throw std::runtime_error("Raw memory version of gradientAtZero not implemented for Jacobian factor");
+  throw std::runtime_error("JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only");
 }
 
 /* ************************************************************************* */

gtsam/linear/JacobianFactor.h

@@ -283,10 +283,6 @@ namespace gtsam {
     /** y += alpha * A'*A*x */
     void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const;
 
-    void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys) const;
-
-    void multiplyHessianAdd(double alpha, const double* x, double* y) const {};
-
     /// A'*b for Jacobian
     VectorValues gradientAtZero() const;
 

gtsam/linear/Preconditioner.cpp

@@ -126,7 +126,9 @@ void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const
 void BlockJacobiPreconditioner::build(
   const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
 {
+  // n is the number of keys
   const size_t n = keyInfo.size();
+  // dims_ is a vector that contains the dimension of keys
   dims_ = keyInfo.colSpec();
 
   /* prepare the buffer of block diagonals */

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -315,30 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
   EXPECT(assert_equal(2*expected, actual));
 }
 
-/* ************************************************************************* */
-TEST( GaussianFactorGraph, multiplyHessianAdd3 )
-{
-  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
-
-  // brute force
-  Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
-  Vector X(6); X<<1,2,3,4,5,6;
-  Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450;
-  EXPECT(assert_equal(Y,AtA*X));
-
-    double* x = &X[0];
-
-    Vector fast_y = gtsam::zero(6);
-    gfg.multiplyHessianAdd(1.0, x, fast_y.data());
-    EXPECT(assert_equal(Y, fast_y));
-
-    // now, do it with non-zero y
-    gfg.multiplyHessianAdd(1.0, x, fast_y.data());
-    EXPECT(assert_equal(2*Y, fast_y));
-
-}
-
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, matricesMixed )
 {
@@ -351,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed )
   EXPECT(assert_equal(A.transpose()*b, eta));
 }
 
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, gradientAtZero )
 {

gtsam/nonlinear/Values-inl.h

@@ -289,13 +289,13 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  // insert a plain value using the default chart
+  // insert a templated value
   template<typename ValueType>
    void Values::insert(Key j, const ValueType& val) {
      insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
    }
 
-  // update with default chart
+  // update with templated value
   template <typename ValueType>
   void Values::update(Key j, const ValueType& val) {
     update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));

gtsam/nonlinear/Values.h

@@ -260,7 +260,6 @@ namespace gtsam {
 
     /** Templated version to add a variable with the given j,
      * throws KeyAlreadyExists<J> if j is already present
-     * if no chart is specified, the DefaultChart<ValueType> is used
      */
     template <typename ValueType>
     void insert(Key j, const ValueType& val);
@@ -272,15 +271,6 @@ namespace gtsam {
     /// version for double
     void insertDouble(Key j, double c) { insert<double>(j,c); }
 
-    /// overloaded insert version that also specifies a chart
-    template <typename ValueType, typename Chart>
-    void insert(Key j, const ValueType& val);
-
-    /// overloaded insert version that also specifies a chart initializer
-    template <typename ValueType, typename Chart>
-    void insert(Key j, const ValueType& val, Chart chart);
-
-
     /** insert that mimics the STL map insert - if the value already exists, the map is not modified
      *  and an iterator to the existing value is returned, along with 'false'.  If the value did not
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
@@ -297,14 +287,6 @@ namespace gtsam {
     template <typename T>
     void update(Key j, const T& val);
 
-    /// overloaded insert version that also specifies a chart
-    template <typename T, typename Chart>
-    void update(Key j, const T& val);
-
-    /// overloaded insert version that also specifies a chart initializer
-    template <typename T, typename Chart>
-    void update(Key j, const T& val, Chart chart);
-
     /** update the current available values without adding new ones */
     void update(const Values& values);
 

gtsam/slam/RegularHessianFactor.h

@@ -51,18 +51,12 @@ public:
       HessianFactor(keys, augmentedInformation) {
   }
 
-  /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
-    HessianFactor::multiplyHessianAdd(alpha, x, y);
-  }
-
   // Scratch space for multiplyHessianAdd
   typedef Eigen::Matrix<double, D, 1> DVector;
   mutable std::vector<DVector> y;
 
-  void multiplyHessianAdd(double alpha, const double* x,
+  /** y += alpha * A'*A*x */
-      double* yvalues) const {
+  void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
     // Create a vector of temporary y values, corresponding to rows i
     y.resize(size());
     BOOST_FOREACH(DVector & yi, y)
@@ -95,6 +89,83 @@ public:
     }
   }
 
+  void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
+      std::vector<size_t> offsets) const {
+
+    // Use eigen magic to access raw memory
+    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+    typedef Eigen::Map<const DVector> ConstDMap;
+
+    // Create a vector of temporary y values, corresponding to rows i
+    std::vector<Vector> y;
+    y.reserve(size());
+    for (const_iterator it = begin(); it != end(); it++)
+      y.push_back(zero(getDim(it)));
+
+    // Accessing the VectorValues one by one is expensive
+    // So we will loop over columns to access x only once per column
+    // And fill the above temporary y values, to be added into yvalues after
+    for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
+      DenseIndex i = 0;
+      for (; i < j; ++i)
+        y[i] += info_(i, j).knownOffDiagonal()
+            * ConstDMap(x + offsets[keys_[j]],
+                offsets[keys_[j] + 1] - offsets[keys_[j]]);
+      // blocks on the diagonal are only half
+      y[i] += info_(j, j).selfadjointView()
+          * ConstDMap(x + offsets[keys_[j]],
+              offsets[keys_[j] + 1] - offsets[keys_[j]]);
+      // for below diagonal, we take transpose block from upper triangular part
+      for (i = j + 1; i < (DenseIndex) size(); ++i)
+        y[i] += info_(i, j).knownOffDiagonal()
+            * ConstDMap(x + offsets[keys_[j]],
+                offsets[keys_[j] + 1] - offsets[keys_[j]]);
+    }
+
+    // copy to yvalues
+    for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
+      DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
+          alpha * y[i];
+  }
+
+  /** Return the diagonal of the Hessian for this factor (raw memory version) */
+  virtual void hessianDiagonal(double* d) const {
+
+    // Use eigen magic to access raw memory
+    //typedef Eigen::Matrix<double, 9, 1> DVector;
+    typedef Eigen::Matrix<double, D, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+
+    // Loop over all variables in the factor
+    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+      Key j = keys_[pos];
+      // Get the diagonal block, and insert its diagonal
+      const Matrix& B = info_(pos, pos).selfadjointView();
+      //DMap(d + 9 * j) += B.diagonal();
+      DMap(d + D * j) += B.diagonal();
+    }
+  }
+
+  /* ************************************************************************* */
+  // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
+  virtual void gradientAtZero(double* d) const {
+
+    // Use eigen magic to access raw memory
+    //typedef Eigen::Matrix<double, 9, 1> DVector;
+    typedef Eigen::Matrix<double, D, 1> DVector;
+    typedef Eigen::Map<DVector> DMap;
+
+    // Loop over all variables in the factor
+    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+      Key j = keys_[pos];
+      // Get the diagonal block, and insert its diagonal
+      VectorD dj =  -info_(pos,size()).knownOffDiagonal();
+      //DMap(d + 9 * j) += dj;
+      DMap(d + D * j) += dj;
+    }
+  }
+
 };
 
 }

gtsam/slam/RegularImplicitSchurFactor.h

@@ -159,7 +159,7 @@ public:
    * @brief add the contribution of this factor to the diagonal of the hessian
    * d(output) = d(input) + deltaHessianFactor
    */
-  void hessianDiagonal(double* d) const {
+  virtual void hessianDiagonal(double* d) const {
     // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, D, 1> DVector;
@@ -434,7 +434,7 @@ public:
   /**
    * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
    */
-  void gradientAtZero(double* d) const {
+  virtual void gradientAtZero(double* d) const {
 
     // Use eigen magic to access raw memory
     typedef Eigen::Matrix<double, D, 1> DVector;

gtsam/slam/RegularJacobianFactor.h

@@ -0,0 +1,174 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    RegularJacobianFactor.h
+ * @brief   JacobianFactor class with fixed sized blcoks
+ * @author  Sungtae An
+ * @date    Nov 11, 2014
+ */
+
+#pragma once
+
+#include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+#include <boost/foreach.hpp>
+#include <vector>
+
+namespace gtsam {
+
+template<size_t D>
+class RegularJacobianFactor: public JacobianFactor {
+
+private:
+
+  /** Use eigen magic to access raw memory */
+  typedef Eigen::Matrix<double, D, 1> DVector;
+  typedef Eigen::Map<DVector> DMap;
+  typedef Eigen::Map<const DVector> ConstDMap;
+
+public:
+
+  /** Construct an n-ary factor
+   * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
+   *         collection of keys and matrices making up the factor. */
+  template<typename TERMS>
+  RegularJacobianFactor(const TERMS& terms, const Vector& b,
+      const SharedDiagonal& model = SharedDiagonal()) :
+      JacobianFactor(terms, b, model) {
+  }
+
+  /** Constructor with arbitrary number keys, and where the augmented matrix is given all together
+   *  instead of in block terms.  Note that only the active view of the provided augmented matrix
+   *  is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
+   *  factor. */
+  template<typename KEYS>
+  RegularJacobianFactor(const KEYS& keys,
+      const VerticalBlockMatrix& augmentedMatrix,
+      const SharedDiagonal& sigmas = SharedDiagonal()) :
+      JacobianFactor(keys, augmentedMatrix, sigmas) {
+  }
+
+  /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
+   * Note: this is not assuming a fixed dimension for the variables,
+   * but requires the vector accumulatedDims to tell the dimension of
+   * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
+   * then accumulatedDims is [0 3 9 11 13]
+   * NOTE: size of accumulatedDims is size of keys + 1!! */
+  void multiplyHessianAdd(double alpha, const double* x, double* y,
+      const std::vector<size_t>& accumulatedDims) const {
+
+    /// Use eigen magic to access raw memory
+    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorD;
+    typedef Eigen::Map<VectorD> MapD;
+    typedef Eigen::Map<const VectorD> ConstMapD;
+
+    if (empty())
+      return;
+    Vector Ax = zero(Ab_.rows());
+
+    /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
+    /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
+    /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
+    for (size_t pos = 0; pos < size(); ++pos)
+    {
+      Ax += Ab_(pos)
+          * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
+    }
+    /// Deal with noise properly, need to Double* whiten as we are dividing by variance
+    if (model_) {
+      model_->whitenInPlace(Ax);
+      model_->whitenInPlace(Ax);
+    }
+
+    /// multiply with alpha
+    Ax *= alpha;
+
+    /// Again iterate over all A matrices and insert Ai^T into y
+    for (size_t pos = 0; pos < size(); ++pos){
+      MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
+          pos).transpose() * Ax;
+    }
+  }
+
+  /** Raw memory access version of multiplyHessianAdd */
+  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
+
+    if (empty()) return;
+    Vector Ax = zero(Ab_.rows());
+
+    // Just iterate over all A matrices and multiply in correct config part
+    for(size_t pos=0; pos<size(); ++pos)
+      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
+
+    // Deal with noise properly, need to Double* whiten as we are dividing by variance
+    if  (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); }
+
+    // multiply with alpha
+    Ax *= alpha;
+
+    // Again iterate over all A matrices and insert Ai^e into y
+    for(size_t pos=0; pos<size(); ++pos)
+      DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
+  }
+
+  /** Raw memory access version of hessianDiagonal
+   * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
+   */
+  virtual void hessianDiagonal(double* d) const {
+    // Loop over all variables in the factor
+    for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
+      // Get the diagonal block, and insert its diagonal
+      DVector dj;
+      for (size_t k = 0; k < D; ++k){
+        if (model_){
+          Vector column_k = Ab_(j).col(k);
+          column_k = model_->whiten(column_k);
+          dj(k) = dot(column_k, column_k);
+        }else{
+          dj(k) = Ab_(j).col(k).squaredNorm();
+        }
+      }
+      DMap(d + D * j) += dj;
+    }
+  }
+
+  /** Raw memory access version of gradientAtZero
+   * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
+   */
+  virtual void gradientAtZero(double* d) const {
+
+    // Get vector b not weighted
+    Vector b = getb();
+
+    // Whitening b
+    if (model_) {
+      b = model_->whiten(b);
+      b = model_->whiten(b);
+    }
+
+    // Just iterate over all A matrices
+    for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+      DVector dj;
+      // gradient -= A'*b/sigma^2
+      // Computing with each column
+      for (size_t k = 0; k < D; ++k){
+        Vector column_k = Ab_(j).col(k);
+        dj(k) = -1.0*dot(b, column_k);
+      }
+      DMap(d + D * j) += dj;
+    }
+  }
+
+};
+
+}
+

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>
 

gtsam/slam/tests/testRegularHessianFactor.cpp

@@ -0,0 +1,100 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testRegularHessianFactor.cpp
+ * @author  Frank Dellaert
+ * @date    March 4, 2014
+ */
+
+#include <gtsam/linear/VectorValues.h>
+#include <CppUnitLite/TestHarness.h>
+
+//#include <gtsam_unstable/slam/RegularHessianFactor.h>
+#include <gtsam/slam/RegularHessianFactor.h>
+
+#include <boost/assign/std/vector.hpp>
+#include <boost/assign/std/map.hpp>
+#include <boost/assign/list_of.hpp>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::assign;
+
+const double tol = 1e-5;
+
+/* ************************************************************************* */
+TEST(RegularHessianFactor, ConstructorNWay)
+{
+  Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished();
+  Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished();
+  Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished();
+
+  Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished();
+  Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished();
+
+  Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished();
+
+  Vector g1 = (Vector(2) << -7, -9).finished();
+  Vector g2 = (Vector(2) << -9,  1).finished();
+  Vector g3 = (Vector(2) <<  2,  3).finished();
+
+  double f = 10;
+
+  std::vector<Key> js;
+  js.push_back(0); js.push_back(1); js.push_back(3);
+  std::vector<Matrix> Gs;
+  Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
+  std::vector<Vector> gs;
+  gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
+  RegularHessianFactor<2> factor(js, Gs, gs, f);
+
+  // multiplyHessianAdd:
+  {
+  // brute force
+  Matrix AtA = factor.information();
+  HessianFactor::const_iterator i1 = factor.begin();
+  HessianFactor::const_iterator i2 = i1 + 1;
+  Vector X(6); X << 1,2,3,4,5,6;
+  Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696;
+  EXPECT(assert_equal(Y,AtA*X));
+
+  VectorValues x = map_list_of<Key, Vector>
+    (0, (Vector(2) << 1,2).finished())
+    (1, (Vector(2) << 3,4).finished())
+    (3, (Vector(2) << 5,6).finished());
+
+  VectorValues expected;
+  expected.insert(0, Y.segment<2>(0));
+  expected.insert(1, Y.segment<2>(2));
+  expected.insert(3, Y.segment<2>(4));
+
+  // RAW ACCESS
+  Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
+  Vector fast_y = gtsam::zero(8);
+  double xvalues[8] = {1,2,3,4,0,0,5,6};
+  factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+  EXPECT(assert_equal(expected_y, fast_y));
+
+  // now, do it with non-zero y
+  factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+  EXPECT(assert_equal(2*expected_y, fast_y));
+
+  // check some expressions
+  EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal()));
+  EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView()));
+  EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal()));
+  }
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */

gtsam/slam/tests/testRegularJacobianFactor.cpp

@@ -0,0 +1,224 @@
+/**
+ * @file    testRegularJacobianFactor.cpp
+ * @brief   unit test regular jacobian factors
+ * @author  Sungtae An
+ * @date    Nov 12, 2014
+ */
+
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/slam/RegularJacobianFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <boost/assign/std/vector.hpp>
+#include <boost/assign/list_of.hpp>
+#include <boost/range/iterator_range.hpp>
+#include <boost/range/adaptor/map.hpp>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::assign;
+
+static const size_t fixedDim = 3;
+static const size_t nrKeys = 3;
+
+// Keys are assumed to be from 0 to n
+namespace {
+  namespace simple {
+    // Terms we'll use
+    const vector<pair<Key, Matrix> > terms = list_of<pair<Key,Matrix> >
+      (make_pair(0, Matrix3::Identity()))
+      (make_pair(1, 2*Matrix3::Identity()))
+      (make_pair(2, 3*Matrix3::Identity()));
+
+    // RHS and sigmas
+    const Vector b = (Vector(3) << 1., 2., 3.).finished();
+    const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished());
+  }
+
+  namespace simple2 {
+    // Terms
+    const vector<pair<Key, Matrix> > terms2 = list_of<pair<Key,Matrix> >
+      (make_pair(0, 2*Matrix3::Identity()))
+      (make_pair(1, 4*Matrix3::Identity()))
+      (make_pair(2, 6*Matrix3::Identity()));
+
+    // RHS
+    const Vector b2 = (Vector(3) << 2., 4., 6.).finished();
+  }
+}
+
+/* ************************************************************************* */
+// Convert from double* to VectorValues
+VectorValues double2vv(const double* x,
+    const  size_t nrKeys, const size_t dim) {
+  // create map with dimensions
+  std::map<gtsam::Key, size_t> dims;
+  for (size_t i = 0; i < nrKeys; i++)
+    dims.insert(make_pair(i, dim));
+
+  size_t n = nrKeys*dim;
+  Vector xVec(n);
+  for (size_t i = 0; i < n; i++){
+    xVec(i) = x[i];
+  }
+  return VectorValues(xVec, dims);
+}
+
+/* ************************************************************************* */
+void vv2double(const VectorValues& vv, double* y,
+    const  size_t nrKeys, const size_t dim) {
+  // create map with dimensions
+  std::map<gtsam::Key, size_t> dims;
+  for (size_t i = 0; i < nrKeys; i++)
+    dims.insert(make_pair(i, dim));
+
+  Vector yvector = vv.vector(dims);
+  size_t n = nrKeys*dim;
+  for (size_t j = 0; j < n; j++)
+    y[j] = yvector[j];
+}
+
+/* ************************************************************************* */
+TEST(RegularJacobianFactor, constructorNway)
+{
+  using namespace simple;
+  JacobianFactor factor(terms[0].first, terms[0].second,
+        terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
+
+  LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back());
+  EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1)));
+  EXPECT(assert_equal(b, factor.getb()));
+  EXPECT(assert_equal(b, regularFactor.getb()));
+  EXPECT(noise == factor.get_model());
+  EXPECT(noise == regularFactor.get_model());
+}
+
+/* ************************************************************************* */
+TEST(RegularJacobianFactor, hessianDiagonal)
+{
+  using namespace simple;
+  JacobianFactor factor(terms[0].first, terms[0].second,
+          terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
+
+  // we compute hessian diagonal from the standard Jacobian
+  VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
+
+  // we compare against the Raw memory access implementation of hessianDiagonal
+  double actualValue[9]={0};
+  regularFactor.hessianDiagonal(actualValue);
+  VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw));
+}
+
+/* ************************************************************************* */
+TEST(RegularJacobian, gradientAtZero)
+{
+  using namespace simple;
+  JacobianFactor factor(terms[0].first, terms[0].second,
+          terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
+
+  // we compute gradient at zero from the standard Jacobian
+  VectorValues expectedGradientAtZero = factor.gradientAtZero();
+
+  //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
+
+  // we compare against the Raw memory access implementation of gradientAtZero
+  double actualValue[9]={0};
+  regularFactor.gradientAtZero(actualValue);
+  VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
+}
+
+/* ************************************************************************* */
+TEST(RegularJacobian, gradientAtZero_multiFactors)
+{
+  using namespace simple;
+  JacobianFactor factor(terms[0].first, terms[0].second,
+          terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
+
+  // we compute gradient at zero from the standard Jacobian
+  VectorValues expectedGradientAtZero = factor.gradientAtZero();
+
+  // we compare against the Raw memory access implementation of gradientAtZero
+  double actualValue[9]={0};
+  regularFactor.gradientAtZero(actualValue);
+  VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
+
+  // One more factor
+  using namespace simple2;
+  JacobianFactor factor2(terms2[0].first, terms2[0].second,
+          terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise);
+  RegularJacobianFactor<fixedDim> regularFactor2(terms2, b2, noise);
+
+  // we accumulate computed gradient at zero from the standard Jacobian
+  VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero());
+
+  // we compare against the Raw memory access implementation of gradientAtZero
+  regularFactor2.gradientAtZero(actualValue);
+  VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2));
+
+}
+
+/* ************************************************************************* */
+TEST(RegularJacobian, multiplyHessianAdd)
+{
+  using namespace simple;
+  JacobianFactor factor(terms[0].first, terms[0].second,
+            terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
+  RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
+
+  // arbitrary vector X
+  VectorValues X;
+  X.insert(0, (Vector(3) << 10.,20.,30.).finished());
+  X.insert(1, (Vector(3) << 10.,20.,30.).finished());
+  X.insert(2, (Vector(3) << 10.,20.,30.).finished());
+
+  // arbitrary vector Y
+  VectorValues Y;
+  Y.insert(0, (Vector(3) << 10.,10.,10.).finished());
+  Y.insert(1, (Vector(3) << 20.,20.,20.).finished());
+  Y.insert(2, (Vector(3) << 30.,30.,30.).finished());
+
+  // multiplyHessianAdd Y += alpha*A'A*X
+  double alpha = 2.0;
+  VectorValues expectedMHA = Y;
+  factor.multiplyHessianAdd(alpha, X, expectedMHA);
+
+  // create data for raw memory access
+  double XRaw[9];
+  vv2double(X, XRaw, nrKeys, fixedDim);
+
+  // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y)
+  double actualMHARaw[9];
+  vv2double(Y, actualMHARaw, nrKeys, fixedDim);
+  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw);
+  VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedMHA,actualMHARawVV));
+
+  // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector<size_t> keys)
+  double actualMHARaw2[9];
+  vv2double(Y, actualMHARaw2, nrKeys, fixedDim);
+  vector<size_t> dims;
+  size_t accumulatedDim = 0;
+  for (size_t i = 0; i < nrKeys+1; i++){
+    dims.push_back(accumulatedDim);
+    accumulatedDim += fixedDim;
+  }
+  regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims);
+  VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim);
+  EXPECT(assert_equal(expectedMHA,actualMHARawVV2));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */

gtsam_unstable/slam/PartialPriorFactor.h

@@ -78,7 +78,7 @@ namespace gtsam {
     /** Indices Constructor: specify the mask with a set of indices */
     PartialPriorFactor(Key key, const std::vector<size_t>& mask, const Vector& prior,
         const SharedNoiseModel& model) :
-      Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) {
+      Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::dimension)) {
       assert((size_t)prior_.size() == mask.size());
       assert(model->dim() == (size_t) prior.size());
       this->fillH();

matlab/+gtsam/covarianceEllipse.m

@@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
 % it is assumed x and y are the first two components of state x
 % k is scaling for std deviations, defaults to 1 std
 
+hold on
+
 [e,s] = eig(P(1:2,1:2));
 s1 = s(1,1);
 s2 = s(2,2);
@@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
         y = c(2) + points(2,:);
     end
 
-end
+end

matlab/+gtsam/covarianceEllipse3D.m

@@ -1,4 +1,4 @@
-function covarianceEllipse3D(c,P)
+function sc = covarianceEllipse3D(c,P,scale)
 % covarianceEllipse3D plots a Gaussian as an uncertainty ellipse
 % Based on Maybeck Vol 1, page 366
 % k=2.296 corresponds to 1 std, 68.26% of all probability
@@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P)
 %
 % Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
 
+hold on
+
 [e,s] = svd(P);
 k = 11.82; 
 radii = k*sqrt(diag(s));
 
+if exist('scale', 'var')
+    radii = radii * scale;
+end
+
 % generate data for "unrotated" ellipsoid
 [xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
 

matlab/+gtsam/cylinderSampleProjection.m

@@ -0,0 +1,89 @@
+function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
+
+% Input: 
+% Output:
+%   visiblePoints:  data{k} 3D Point in overal point clouds with index k 
+%                   Z{k}    2D measurements in overal point clouds with index k   
+%                   index {i}{j}
+%                   i: the cylinder index;
+%                   j: the point index on the cylinder;
+%                   
+% @Description: Project sampled points on cylinder to camera frame
+% @Authors: Zhaoyang Lv
+
+import gtsam.*
+
+camera = SimpleCamera(pose, K);
+
+%% memory allocation
+cylinderNum = length(cylinders);
+
+%% check visiblity of points on each cylinder
+pointCloudIndex = 0;
+visiblePointIdx = 1;
+for i = 1:cylinderNum
+    
+    pointNum = length(cylinders{i}.Points);
+
+    % to check point visibility        
+    for j = 1:pointNum
+
+        pointCloudIndex  = pointCloudIndex + 1;
+                
+        % Cheirality Exception
+        sampledPoint3 = cylinders{i}.Points{j};
+        sampledPoint3local = pose.transform_to(sampledPoint3);        
+        if sampledPoint3local.z <= 0
+            continue; 
+        end
+        Z2d = camera.project(sampledPoint3);
+
+        % ignore points not visible in the scene
+        if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y 
+            continue;       
+        end            
+
+        % ignore points occluded
+        % use a simple math hack to check occlusion:
+        %   1. All points in front of cylinders' surfaces are visible
+        %   2. For points behind the cylinders' surfaces, the cylinder
+        visible = true;
+        for k = 1:cylinderNum
+
+            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
+            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
+            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
+
+            % Condition 1: all points in front of the cylinders'
+            % surfaces are visible
+            if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
+               continue;
+            else 
+                projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
+                if projectedRay > 0
+                    %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+                    if rayCylinderToPoint(1) > cylinders{k}.radius && ...
+                            rayCylinderToPoint(2) > cylinders{k}.radius
+                       continue;
+                    else
+                        visible = false;
+                        break;
+                    end
+                end
+            end
+            
+        end
+        
+        if visible
+            visiblePoints.data{visiblePointIdx} = sampledPoint3;
+            visiblePoints.Z{visiblePointIdx} = Z2d;
+            visiblePoints.cylinderIdx{visiblePointIdx} = i;
+            visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
+            visiblePointIdx = visiblePointIdx + 1;
+        end
+
+    end
+    
+end
+    
+end

matlab/+gtsam/cylinderSampleProjectionStereo.m

@@ -0,0 +1,93 @@
+function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
+
+import gtsam.*
+
+%% memory allocation
+cylinderNum = length(cylinders);
+
+visiblePoints.data = cell(1);
+visiblePoints.Z = cell(1);
+visiblePoints.cylinderIdx = cell(1);
+visiblePoints.overallIdx = cell(1);
+
+%% check visiblity of points on each cylinder
+pointCloudIndex = 0;
+visiblePointIdx = 1;
+for i = 1:cylinderNum
+    
+    pointNum = length(cylinders{i}.Points);
+
+    % to check point visibility        
+    for j = 1:pointNum
+
+        pointCloudIndex  = pointCloudIndex + 1;
+                
+        % For Cheirality Exception
+        sampledPoint3 = cylinders{i}.Points{j};
+        sampledPoint3local = pose.transform_to(sampledPoint3);
+        if sampledPoint3local.z < 0
+            continue; 
+        end
+        
+        % measurements 
+        Z.du = K.fx() * K.baseline() / sampledPoint3local.z;  
+        Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
+        Z.uR = Z.uL + Z.du;
+        Z.v = K.fy() / sampledPoint3local.z + K.py();
+
+        % ignore points not visible in the scene
+        if Z.uL < 0 || Z.uL >= imageSize.x || ...
+                Z.uR < 0 || Z.uR >= imageSize.x || ...
+                Z.v < 0 || Z.v >= imageSize.y 
+            continue;       
+        end            
+
+        % too small disparity may call indeterminant system exception
+        if Z.du < 0.6
+            continue;
+        end
+        
+        % ignore points occluded
+        % use a simple math hack to check occlusion:
+        %   1. All points in front of cylinders' surfaces are visible
+        %   2. For points behind the cylinders' surfaces, the cylinder
+        visible = true;
+        for k = 1:cylinderNum
+
+            rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
+            rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
+            rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
+
+            % Condition 1: all points in front of the cylinders'
+            % surfaces are visible
+            if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
+               continue;
+            else 
+                projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
+                if projectedRay > 0
+                    %rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
+                    if rayCylinderToPoint(1) > cylinders{k}.radius && ...
+                            rayCylinderToPoint(2) > cylinders{k}.radius
+                       continue;
+                    else
+                        visible = false;
+                        break;
+                    end
+                end
+            end
+            
+        end
+        
+        if visible
+            visiblePoints.data{visiblePointIdx} = sampledPoint3;
+            visiblePoints.Z{visiblePointIdx} = Z;
+            visiblePoints.cylinderIdx{visiblePointIdx} = i;
+            visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
+            visiblePointIdx = visiblePointIdx + 1;
+        end
+        
+    end
+
+end
+
+end

matlab/+gtsam/cylinderSampling.m

@@ -0,0 +1,26 @@
+function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
+% 
+% @author: Zhaoyang Lv
+    import gtsam.*
+    % calculate the cylinder area
+    area = 2 * pi * radius * height;
+    
+    pointsNum = round(area * density);
+    
+    points3 = cell(pointsNum, 1);
+    
+    % sample the points
+    for i = 1:pointsNum
+        theta = 2 * pi * rand;
+        x = radius * cos(theta) + baseCentroid.x;
+        y = radius * sin(theta) + baseCentroid.y;
+        z = height * rand;
+        points3{i,1} = Point3([x,y,z]');
+    end
+   
+    cylinder.area = area;
+    cylinder.radius = radius;
+    cylinder.height = height;
+    cylinder.Points = points3;
+    cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
+end

matlab/+gtsam/plotCamera.m

@@ -1,18 +1,20 @@
 function plotCamera(pose, axisLength)
+	hold on
+
     C = pose.translation().vector();
     R = pose.rotation().matrix();
     
     xAxis = C+R(:,1)*axisLength;
     L = [C xAxis]';
-    line(L(:,1),L(:,2),L(:,3),'Color','r');
+    h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
     
     yAxis = C+R(:,2)*axisLength;
     L = [C yAxis]';
-    line(L(:,1),L(:,2),L(:,3),'Color','g');
+    h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
     
     zAxis = C+R(:,3)*axisLength;
     L = [C zAxis]';
-    line(L(:,1),L(:,2),L(:,3),'Color','b');
+    h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
     
     axis equal
-end
+end

matlab/+gtsam/plotCylinderSamples.m

@@ -0,0 +1,35 @@
+function plotCylinderSamples(cylinders, options, figID)
+% plot the cylinders on the given field
+% @author: Zhaoyang Lv
+
+    figure(figID);
+
+    holdstate = ishold;
+    hold on
+    
+    num = size(cylinders, 1);
+
+    sampleDensity = 120;
+    
+    for i = 1:num                
+        [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
+        
+        X = X + cylinders{i}.centroid.x;
+        Y = Y + cylinders{i}.centroid.y;
+        Z = Z * cylinders{i}.height;
+        
+        cylinderHandle = surf(X,Y,Z);
+        set(cylinderHandle, 'FaceAlpha', 0.5);
+        hold on
+    end
+    
+    axis equal
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
+        
+    grid on
+    
+    if ~holdstate
+        hold off
+    end
+    
+end

matlab/+gtsam/plotFlyingResults.m

@@ -0,0 +1,177 @@
+function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
+% plot the visible points on the cylinders and trajectories
+%
+% author: Zhaoyang Lv
+
+import gtsam.*
+
+figID = 1;
+figure(figID);
+set(gcf, 'Position', [80,1,1800,1000]);
+
+
+%% plot all the cylinders and sampled points
+
+axis equal
+axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+xlabel('X (m)');
+ylabel('Y (m)');
+zlabel('Height (m)');
+
+h = cameratoolbar('Show');
+
+if options.camera.IS_MONO
+    h_title = title('Quadrotor Flight Simulation with Monocular Camera');
+else
+    h_title = title('Quadrotor Flight Simulation with Stereo Camera');
+end
+
+text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
+
+view([30, 30]);
+
+hlight = camlight('headlight'); 
+lighting gouraud
+
+if(options.writeVideo)
+    videoObj = VideoWriter('Camera_Flying_Example.avi');
+    videoObj.Quality = 100;
+    videoObj.FrameRate = options.camera.fps;
+    open(videoObj);
+end
+
+
+sampleDensity = 120;
+cylinderNum = length(cylinders);
+h_cylinder = cell(cylinderNum);
+for i = 1:cylinderNum
+    
+    hold on
+    
+    [X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
+
+    X = X + cylinders{i}.centroid.x;
+    Y = Y + cylinders{i}.centroid.y;
+    Z = Z * cylinders{i}.height;
+
+    h_cylinder{i} = surf(X,Y,Z);
+    set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
+    h_cylinder{i}.AmbientStrength = 0.8;
+    
+end
+
+%% plot trajectories and points 
+posesSize = length(poses);
+pointSize = length(pts3d);
+for i = 1:posesSize
+    if i > 1
+        hold on
+        plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
+    end
+    
+    if exist('h_pose_cov', 'var')
+        delete(h_pose_cov);
+    end
+       
+    %plotCamera(poses{i}, 3);
+    
+    gRp = poses{i}.rotation().matrix();  % rotation from pose to global
+    C = poses{i}.translation().vector();
+    axisLength = 3;
+    
+    xAxis = C+gRp(:,1)*axisLength;
+    L = [C xAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','r');
+    
+    yAxis = C+gRp(:,2)*axisLength;
+    L = [C yAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','g');
+    
+    zAxis = C+gRp(:,3)*axisLength;
+    L = [C zAxis]';
+    line(L(:,1),L(:,2),L(:,3),'Color','b');
+    
+    pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame    
+    gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
+    h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale); 
+    
+    if exist('h_point', 'var')
+        for j = 1:pointSize
+            delete(h_point{j});
+        end
+    end
+    if exist('h_point_cov', 'var')
+        for j = 1:pointSize
+            delete(h_point_cov{j});
+        end
+    end
+    
+    h_point = cell(pointSize, 1);
+    h_point_cov = cell(pointSize, 1);
+    for j = 1:pointSize
+        if ~isempty(pts3d{j}.cov{i})
+            hold on
+            h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
+            h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
+                pts3d{j}.cov{i}, options.plot.covarianceScale);
+        end
+    end 
+    
+    axis equal
+    axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
+
+    drawnow
+    
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+end
+
+
+if exist('h_pose_cov', 'var')
+    delete(h_pose_cov);
+end
+
+% wait for two seconds
+pause(2);
+
+%% change views angle
+for i = 30 : i : 90
+    view([30, i]);
+
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+    
+    drawnow
+end
+
+% changing perspective
+
+
+%% camera flying through video
+camzoom(0.8);
+for i = 1 : posesSize
+    
+    hold on
+    
+    campos([poses{i}.x, poses{i}.y, poses{i}.z]);
+    camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
+    camlight(hlight, 'headlight');
+    
+    if options.writeVideo
+        currFrame = getframe(gcf);
+        writeVideo(videoObj, currFrame);
+    end
+    
+    drawnow
+end
+
+%%close video
+if(options.writeVideo)
+    close(videoObj);
+end
+
+end

matlab/+gtsam/points2DTrackMonocular.m

@@ -0,0 +1,113 @@
+function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
+% Assess how accurately we can reconstruct points from a particular monocular camera setup. 
+% After creation of the factor graph for each track, linearize it around ground truth. 
+% There is no optimization
+% @author: Zhaoyang Lv
+
+import gtsam.*
+
+%% create graph
+graph = NonlinearFactorGraph;
+
+%% create the noise factors
+poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+measurementNoiseSigma = 1.0;
+measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
+
+cameraPosesNum = length(cameraPoses);
+
+%% add measurements and initial camera & points values
+pointsNum = 0;
+cylinderNum = length(cylinders);
+points3d = cell(0);
+for i = 1:cylinderNum
+    cylinderPointsNum = length(cylinders{i}.Points);
+    pointsNum = pointsNum + cylinderPointsNum; 
+    for j = 1:cylinderPointsNum
+        points3d{end+1}.data = cylinders{i}.Points{j};
+        points3d{end}.Z = cell(0);
+        points3d{end}.camConstraintIdx = cell(0);
+        points3d{end}.added = cell(0);
+        points3d{end}.visiblity = false;
+        points3d{end}.cov = cell(cameraPosesNum);
+    end
+end
+
+graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
+
+%% initialize graph and values
+initialEstimate = Values;
+for i = 1:pointsNum
+    point_j = points3d{i}.data.retract(0.1*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j); 
+end
+
+pts3d = cell(cameraPosesNum, 1);
+cameraPosesCov = cell(cameraPosesNum, 1);
+marginals = Values;
+for i = 1:cameraPosesNum     
+    cameraPose = cameraPoses{i};    
+    pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
+    
+    measurementNum = length(pts3d{i}.Z);
+    for j = 1:measurementNum
+        index = pts3d{i}.overallIdx{j};
+        points3d{index}.Z{end+1} = pts3d{i}.Z{j};
+        points3d{index}.camConstraintIdx{end+1} = i;
+        points3d{index}.added{end+1} = false;
+        
+        if length(points3d{index}.Z) < 2
+            continue;
+        else
+            for k = 1:length(points3d{index}.Z)
+                if ~points3d{index}.added{k}                
+                    graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ...
+                        measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ...
+                        symbol('p', index), K) );
+                    points3d{index}.added{k} = true;
+                end
+            end
+        end 
+        
+        points3d{index}.visiblity = true;    
+    end
+
+    pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
+    initialEstimate.insert(symbol('x', i), pose_i);    
+
+    marginals = Marginals(graph, initialEstimate);
+
+    for j = 1:pointsNum
+        if points3d{j}.visiblity
+            points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
+        end
+    end
+
+    cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
+
+end
+  
+%% Print the graph
+graph.print(sprintf('\nFactor graph:\n'));
+
+%% Plot the result
+plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
+
+%% get all the points track information
+for i = 1:pointsNum
+    if ~points3d{i}.visiblity
+        continue;
+    end
+    
+    pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
+    pts2dTracksMono.Z{end+1} = points3d{i}.Z;
+
+    if length(points3d{i}.Z) == 1
+        %pts2dTracksMono.cov{i} singular matrix 
+    else 
+        pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));    
+    end
+end
+
+end

matlab/+gtsam/points2DTrackStereo.m

@@ -0,0 +1,101 @@
+function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
+% Assess how accurately we can reconstruct points from a particular monocular camera setup. 
+% After creation of the factor graph for each track, linearize it around ground truth. 
+% There is no optimization
+%
+% @author: Zhaoyang Lv
+
+import gtsam.*
+
+%% create graph
+graph = NonlinearFactorGraph;
+
+%% create the noise factors
+poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
+
+cameraPosesNum = length(cameraPoses);
+
+%% add measurements and initial camera & points values
+pointsNum = 0;
+cylinderNum = length(cylinders);
+points3d = cell(0);
+for i = 1:cylinderNum
+    cylinderPointsNum = length(cylinders{i}.Points);
+    pointsNum = pointsNum + length(cylinders{i}.Points);
+    for j = 1:cylinderPointsNum
+        points3d{end+1}.data = cylinders{i}.Points{j};
+        points3d{end}.Z = cell(0);
+        points3d{end}.cameraConstraint = cell(0);
+        points3d{end}.visiblity = false;
+        points3d{end}.cov = cell(cameraPosesNum);
+    end
+end
+
+graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
+
+%% initialize graph and values
+initialEstimate = Values;
+for i = 1:pointsNum
+    point_j = points3d{i}.data.retract(0.05*randn(3,1));
+    initialEstimate.insert(symbol('p', i), point_j);    
+end
+
+pts3d = cell(cameraPosesNum, 1);
+cameraPosesCov = cell(cameraPosesNum, 1);
+for i = 1:cameraPosesNum 
+    pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
+    
+    if isempty(pts3d{i}.Z)
+        continue;
+    end
+    
+    measurementNum = length(pts3d{i}.Z);
+    for j = 1:measurementNum
+        index = pts3d{i}.overallIdx{j};
+        points3d{index}.Z{end+1} = pts3d{i}.Z{j};
+        points3d{index}.cameraConstraint{end+1} = i;
+        points3d{index}.visiblity = true;
+  
+        graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
+            stereoNoise, symbol('x', i), symbol('p', index), K));    
+    end
+
+    pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
+    initialEstimate.insert(symbol('x', i), pose_i);
+
+    %% linearize the graph
+    marginals = Marginals(graph, initialEstimate);
+
+    for j = 1:pointsNum
+        if points3d{j}.visiblity
+            points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
+        end       
+    end
+    
+    cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));    
+end
+
+%% Plot the result
+plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
+
+%% get all the 2d points track information
+pts2dTracksStereo.pt3d = cell(0);
+pts2dTracksStereo.Z = cell(0);
+pts2dTracksStereo.cov = cell(0);
+for i = 1:pointsNum
+    if ~points3d{i}.visiblity
+        continue;
+    end
+    
+    pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
+    pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
+    pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));       
+end
+
+% 
+% %% plot the result with covariance ellipses
+% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
+
+end

matlab/gtsam_examples/CameraFlyingExample.m

@@ -0,0 +1,179 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+% Atlanta, Georgia 30332-0415
+% All Rights Reserved
+% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+% 
+% See LICENSE for the license information
+%
+% @brief A camera flying example through a field of cylinder landmarks
+% @author Zhaoyang Lv
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+
+clear all;
+clc;
+clf;
+
+import gtsam.*
+
+% test or run
+options.enableTests = false;
+
+%% cylinder options
+% the number of cylinders in the field
+options.cylinder.cylinderNum = 15; % pls be smaller than 20
+% cylinder size
+options.cylinder.radius = 3;  % pls be smaller than 5
+options.cylinder.height = 10;
+% point density on cylinder
+options.cylinder.pointDensity = 0.1;
+
+%% camera options
+%   parameters set according to the stereo camera:
+%   http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
+
+% set up monocular camera or stereo camera
+options.camera.IS_MONO = false;
+% the field of view of camera
+options.camera.fov = 120;
+% fps for image
+options.camera.fps = 25;
+% camera pixel resolution
+options.camera.resolution = Point2(752, 480);
+% camera horizon
+options.camera.horizon = 60;
+% camera baseline
+options.camera.baseline = 0.05;
+% camera focal length
+options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
+    options.camera.fov);
+% camera focal baseline
+options.camera.fB = options.camera.f * options.camera.baseline;
+% camera disparity
+options.camera.disparity = options.camera.fB / options.camera.horizon;
+% Monocular Camera Calibration
+options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
+    options.camera.resolution.x/2, options.camera.resolution.y/2);
+% Stereo Camera Calibration
+options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
+    options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
+
+% write video output
+options.writeVideo = true;
+% the testing field size (unit: meter)
+options.fieldSize = Point2([100, 100]');
+% camera flying speed (unit: meter / second)
+options.speed = 20;
+% camera flying height
+options.height = 30;
+
+%% ploting options
+% display covariance scaling factor, default to be 1.
+% The covariance visualization default models 99% of all probablity 
+options.plot.covarianceScale = 1;
+% plot the trajectory covariance
+options.plot.DISP_TRAJ_COV = true;
+% plot points covariance
+options.plot.POINTS_COV = true;
+
+%% This is for tests
+if options.enableTests
+    % test1: visibility test in monocular camera 
+    cylinders{1}.centroid = Point3(30, 50, 5);
+    cylinders{2}.centroid = Point3(50, 50, 5);
+    cylinders{3}.centroid = Point3(70, 50, 5);
+
+    for i = 1:3
+        cylinders{i}.radius = 5;
+        cylinders{i}.height = 10;
+
+        cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
+        cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
+    end
+
+    camera = SimpleCamera.Lookat(Point3(10, 50, 10), ... 
+        Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
+        Point3([0,0,1]'), options.monoK); 
+
+    pose = camera.pose;
+    prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
+        options.camera.resolution, cylinders);
+
+    % test2: visibility test in stereo camera  
+    prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
+        pose, options.camera.resolution, cylinders);
+end
+
+%% generate a set of cylinders and point samples on cylinders
+cylinderNum = options.cylinder.cylinderNum;
+cylinders = cell(cylinderNum, 1);
+baseCentroid = cell(cylinderNum, 1);
+theta = 0;
+i = 1;
+while i <= cylinderNum
+    theta = theta + 2*pi/10;
+    x = 40 * rand * cos(theta) + options.fieldSize.x/2;
+    y = 20 * rand * sin(theta) + options.fieldSize.y/2;
+    baseCentroid{i} = Point2([x, y]');
+    
+    % prevent two cylinders interact with each other
+    regenerate = false;
+    for j = 1:i-1
+        if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
+            regenerate = true;
+            break;
+        end
+    end
+    if regenerate 
+       continue;
+    end
+    
+    cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
+         options.cylinder.height, options.cylinder.pointDensity);
+    i = i+1;
+end
+
+%% generate ground truth camera trajectories: a line
+KMono = Cal3_S2(525,525,0,320,240);
+cameraPoses = cell(0);
+theta = 0;
+t = Point3(5, 5, options.height);
+i = 0;
+while 1
+    i = i+1;
+    distance = options.speed / options.camera.fps;
+    angle = 0.1*pi*(rand-0.5);
+    inc_t = Point3(distance * cos(angle), ...
+        distance * sin(angle), 0);
+    t = t.compose(inc_t);
+    
+    if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
+        break;
+    end
+    
+    %t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
+    %    15, 10]');
+    camera = SimpleCamera.Lookat(t, ... 
+        Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
+        Point3([0,0,1]'), options.camera.monoK);    
+    cameraPoses{end+1} = camera.pose;
+end
+
+%% set up camera and get measurements
+if options.camera.IS_MONO 
+    % use Monocular Camera
+    pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
+        options.camera.resolution, cylinders);
+else 
+    % use Stereo Camera
+    pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
+        cameraPoses, options, cylinders);
+end
+
+
+
+
+ 
+
+

matlab/gtsam_tests/testCal3Unified.m

@@ -0,0 +1,7 @@
+% test Cal3Unified
+import gtsam.*;
+
+K = Cal3Unified;
+EXPECT('fx',K.fx()==1);
+EXPECT('fy',K.fy()==1);
+

matlab/gtsam_tests/test_gtsam.m

@@ -1,17 +1,25 @@
 % Test runner script - runs each test 
 
-% display 'Starting: testPriorFactor'
+%% geometry
-% testPriorFactor
+display 'Starting: testCal3Unified'
+testCal3Unified
 
-display 'Starting: testValues'
+%% linear
-testValues
+display 'Starting: testKalmanFilter'
+testKalmanFilter
 
 display 'Starting: testJacobianFactor'
 testJacobianFactor
 
-display 'Starting: testKalmanFilter'
+%% nonlinear
-testKalmanFilter
+display 'Starting: testValues'
+testValues
 
+%% SLAM
+display 'Starting: testPriorFactor'
+testPriorFactor
+
+%% examples
 display 'Starting: testLocalizationExample'
 testLocalizationExample
 
@@ -36,6 +44,7 @@ testStereoVOExample
 display 'Starting: testVisualISAMExample'
 testVisualISAMExample
 
+%% MATLAB specific
 display 'Starting: testUtilities'
 testUtilities
 

matlab/unstable_examples/.gitignore

@@ -1 +1,2 @@
 *.m~
+*.avi

tests/CMakeLists.txt

@@ -2,7 +2,7 @@
 # note the source dir on each 
 set (tests_exclude "")
 
-if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank
+if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank
 	# clang linker segfaults on large testSerializationSLAM 
 	list (APPEND tests_exclude "testSerializationSLAM.cpp") 
 endif()

tests/testPreconditioner.cpp

@@ -87,19 +87,19 @@ TEST(PCGSolver, simpleLinearSystem) {
   simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
   simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
   simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2);
-  simpleGFG.print("system");
+  //simpleGFG.print("system");
 
   // Expected solution
   VectorValues expectedSolution;
   expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished());
   expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished());
   expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished());
-  expectedSolution.print("Expected");
+  //expectedSolution.print("Expected");
 
   // Solve the system using direct method
   VectorValues deltaDirect = simpleGFG.optimize();
   EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5));
-  deltaDirect.print("Direct");
+  //deltaDirect.print("Direct");
 
   // Solve the system using Preconditioned Conjugate Gradient solver
   // Common PCG parameters
@@ -107,19 +107,19 @@ TEST(PCGSolver, simpleLinearSystem) {
   pcg->setMaxIterations(500);
   pcg->setEpsilon_abs(0.0);
   pcg->setEpsilon_rel(0.0);
-  pcg->setVerbosity("ERROR");
+  //pcg->setVerbosity("ERROR");
 
   // With Dummy preconditioner
   pcg->preconditioner_ = boost::make_shared<gtsam::DummyPreconditionerParameters>();
   VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG);
   EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5));
-  deltaPCGDummy.print("PCG Dummy");
+  //deltaPCGDummy.print("PCG Dummy");
 
   // With Block-Jacobi preconditioner
   pcg->preconditioner_ = boost::make_shared<gtsam::BlockJacobiPreconditionerParameters>();
   VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG);
   EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5));
-  deltaPCGJacobi.print("PCG Jacobi");
+  //deltaPCGJacobi.print("PCG Jacobi");
 
 }