Merged from branch 'trunk'

.settings/org.eclipse.cdt.core.prefs

@@ -1,6 +1,16 @@
-#Wed Oct 06 11:57:41 EDT 2010
 eclipse.preferences.version=1
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/delimiter=\:
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/operation=append
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/PATH/value=/home/ydjian/matlab/R2012a/bin
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/append=true
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216/appendContributed=true
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/delimiter=\:
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/operation=append
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/PATH/value=/home/ydjian/matlab/R2012a/bin
 environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/append=true
 environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890/appendContributed=true
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
+environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=/home/ydjian/matlab/R2012a/bin
 environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
 environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

CMakeLists.txt

@@ -50,7 +50,6 @@ else()
   option(GTSAM_BUILD_SHARED_LIBRARY        "Enable/Disable building of a shared version of gtsam" ON)
 endif()
 option(GTSAM_BUILD_STATIC_LIBRARY        "Enable/Disable building of a static version of gtsam" ON)
-option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build type to the name of compiled libraries" OFF)
 option(GTSAM_USE_QUATERNIONS             "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices" OFF)
 if(MSVC)
   option(GTSAM_BUILD_CONVENIENCE_LIBRARIES "Enable/Disable use of convenience libraries for faster development rebuilds, but slower install" OFF)

gtsam.h

@@ -458,7 +458,6 @@ class CalibratedCamera {
   double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
 };
 
-
 class SimpleCamera {
   // Standard Constructors and Named Constructors
 	SimpleCamera();
@@ -1011,8 +1010,25 @@ class Marginals {
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 class LevenbergMarquardtParams {
   LevenbergMarquardtParams();
-  LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose);
   void print(string s) const;
+
+  double getMaxIterations() const;
+  double getRelativeErrorTol() const;
+  double getAbsoluteErrorTol() const;
+  double getErrorTol() const;
+  string getVerbosity() const;
+
+  void setMaxIterations(double value);
+  void setRelativeErrorTol(double value);
+  void setAbsoluteErrorTol(double value);
+  void setErrorTol(double value);
+  void setVerbosity(string s);
+
+  bool isMultifrontal() const;
+  bool isSequential() const;
+  bool isCholmod() const;
+  bool isCG() const;
+
   double getlambdaInitial() const ;
   double getlambdaFactor() const ;
   double getlambdaUpperBound() const;
@@ -1035,6 +1051,7 @@ namespace pose2SLAM {
 
 class Values {
 	Values();
+  Values(const pose2SLAM::Values& values);
 	size_t size() const;
 	void print(string s) const;
   bool exists(size_t key);
@@ -1088,6 +1105,7 @@ namespace pose3SLAM {
 
 class Values {
 	Values();
+  Values(const pose3SLAM::Values& values);
 	size_t size() const;
 	void print(string s) const;
   bool exists(size_t key);
@@ -1141,6 +1159,7 @@ namespace planarSLAM {
 
 class Values {
 	Values();
+  Values(const planarSLAM::Values& values);
 	size_t size() const;
 	void print(string s) const;
   bool exists(size_t key);
@@ -1226,6 +1245,7 @@ namespace visualSLAM {
 
 class Values {
   Values();
+  Values(const visualSLAM::Values& values);
   size_t size() const;
   void print(string s) const;
   bool exists(size_t key);
@@ -1251,6 +1271,8 @@ class Values {
   void insertPoint(size_t key, const gtsam::Point3& pose);
   void updatePoint(size_t key, const gtsam::Point3& pose);
   gtsam::Point3 point(size_t j);
+  void insertBackprojections(const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
+  void perturbPoints(double sigma, size_t seed);
   Matrix points() const;
 };
 
@@ -1288,10 +1310,18 @@ class Graph {
   void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::noiseModel::Base* model);
 
   // Measurements
-  void addMeasurement(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model,
+    void addMeasurement(const gtsam::Point2& measured,
-      size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
+        const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
-  void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* model,
+        const gtsam::Cal3_S2* K);
-      size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
+    void addMeasurements(size_t i, Vector J, Matrix Z,
+        const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
+    void addStereoMeasurement(const gtsam::StereoPoint2& measured,
+        const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey,
+        const gtsam::Cal3_S2Stereo* K);
+
+  // Information
+  Matrix reprojectionErrors(const visualSLAM::Values& values) const;
+
 };
 
 class ISAM {
@@ -1327,3 +1357,83 @@ class LevenbergMarquardtOptimizer {
 };
 
 }///\namespace visualSLAM
+
+//************************************************************************
+// sparse BA
+//************************************************************************
+
+#include <gtsam/slam/sparseBA.h>
+namespace sparseBA {
+
+class Values {
+  Values();
+  Values(const sparseBA::Values& values);
+  size_t size() const;
+  void print(string s) const;
+  bool exists(size_t key);
+  gtsam::KeyVector keys() const;
+
+  // Access to cameras
+  sparseBA::Values allSimpleCameras() const ;
+  size_t  nrSimpleCameras() const ;
+  gtsam::KeyVector simpleCameraKeys() const ;
+  void insertSimpleCamera(size_t j, const gtsam::SimpleCamera& camera);
+  void updateSimpleCamera(size_t j, const gtsam::SimpleCamera& camera);
+  gtsam::SimpleCamera simpleCamera(size_t j) const;
+
+  // Access to points, inherited from visualSLAM
+  sparseBA::Values allPoints() const;
+  size_t nrPoints() const;
+  gtsam::KeyVector pointKeys() const; // Note the switch to KeyVector, rather than KeyList
+  void insertPoint(size_t key, const gtsam::Point3& pose);
+  void updatePoint(size_t key, const gtsam::Point3& pose);
+  gtsam::Point3 point(size_t j);
+  Matrix points() const;
+};
+
+class Graph {
+  Graph();
+  Graph(const gtsam::NonlinearFactorGraph& graph);
+  Graph(const sparseBA::Graph& graph);
+
+  // Information
+  Matrix reprojectionErrors(const sparseBA::Values& values) const;
+
+  // inherited from FactorGraph
+  void print(string s) const;
+  bool equals(const sparseBA::Graph& fg, double tol) const;
+  size_t size() const;
+  bool empty() const;
+  void remove(size_t i);
+  size_t nrFactors() const;
+  gtsam::NonlinearFactor* at(size_t i) const;
+
+  double error(const sparseBA::Values& values) const;
+  gtsam::Ordering* orderingCOLAMD(const sparseBA::Values& values) const;
+  gtsam::GaussianFactorGraph* linearize(const sparseBA::Values& values, const gtsam::Ordering& ordering) const;
+
+  sparseBA::Values optimize(const sparseBA::Values& initialEstimate, size_t verbosity) const;
+  sparseBA::LevenbergMarquardtOptimizer optimizer(const sparseBA::Values& initialEstimate, const gtsam::LevenbergMarquardtParams& parameters) const;
+  gtsam::Marginals marginals(const sparseBA::Values& solution) const;
+
+  // inherited from visualSLAM
+  void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
+  void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::noiseModel::Base* model);
+
+  // add factors
+  void addSimpleCameraPrior(size_t cameraKey, const gtsam::SimpleCamera &camera, gtsam::noiseModel::Base* model);
+  void addSimpleCameraConstraint(size_t cameraKey, const gtsam::SimpleCamera &camera);
+  void addSimpleCameraMeasurement(const gtsam::Point2 &z, gtsam::noiseModel::Base* model, size_t cameraKey, size_t pointKey);
+};
+
+class LevenbergMarquardtOptimizer {
+  double lambda() const;
+  void iterate();
+  double error() const;
+  size_t iterations() const;
+  sparseBA::Values optimize();
+  sparseBA::Values optimizeSafely();
+  sparseBA::Values values() const;
+};
+}///\namespace sparseBA
+

gtsam/nonlinear/ISAM2-impl.cpp

@@ -96,18 +96,16 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
 			 VectorValues newDeltaNewton(dims);
 			 VectorValues newDeltaGradSearch(dims);
 			 std::vector<bool> newReplacedKeys(replacedKeys.size() - unusedIndices.size());
-			 Base::Nodes newNodes(nodes.size()); // We still keep unused keys at the end until later in ISAM2::recalculate
+			 Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size());
 
 			 for(size_t j = 0; j < dims.size(); ++j) {
 				 newDelta[j] = delta[unusedToEnd[j]];
 				 newDeltaNewton[j] = deltaNewton[unusedToEnd[j]];
 				 newDeltaGradSearch[j] = deltaGradSearch[unusedToEnd[j]];
 				 newReplacedKeys[j] = replacedKeys[unusedToEnd[j]];
+				 newNodes[j] = nodes[unusedToEnd[j]];
 			 }
 
-			 // Permute the nodes index so the unused variables are the end
-			 unusedToEnd.applyToCollection(newNodes, nodes);
-
 			 // Swap the new data structures with the outputs of this function
 			 delta.swap(newDelta);
 			 deltaNewton.swap(newDeltaNewton);

gtsam/nonlinear/ISAM2.cpp

@@ -203,8 +203,8 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
   return cachedBoundary;
 }
 
-boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
+boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& markedKeys,
-    const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys,
+		const FastSet<Index>& relinKeys, const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices,
     const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result) {
 
   // TODO:  new factors are linearized twice, the newFactors passed in are not used.
@@ -249,9 +249,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
   this->removeTop(markedKeys, affectedBayesNet, orphans);
   toc(1, "removetop");
 
-	// Now that the top is removed, correct the size of the Nodes index
-	this->nodes_.resize(delta_.size());
-
   if(debug) affectedBayesNet.print("Removed top: ");
   if(debug) orphans.print("Orphans: ");
 
@@ -269,14 +266,6 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
   // ordering provides all keys in conditionals, there cannot be others because path to root included
   tic(2,"affectedKeys");
   FastList<Index> affectedKeys = affectedBayesNet.ordering();
-	// The removed top also contained removed variables, which will be ordered
-	// higher than the number of variables in the system since unused variables
-	// were already removed in ISAM2::update.
-	for(FastList<Index>::iterator key = affectedKeys.begin(); key != affectedKeys.end(); )
-		if(*key >= delta_.size())
-			affectedKeys.erase(key++);
-		else
-			++key;
   toc(2,"affectedKeys");
 
   boost::shared_ptr<FastSet<Index> > affectedKeysSet(new FastSet<Index>()); // Will return this result
@@ -329,11 +318,13 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
     toc(1,"reorder");
 
     tic(2,"linearize");
-    linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_);
+    GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_);
+    if(params_.cacheLinearizedFactors)
+      linearFactors_ = linearized;
     toc(2,"linearize");
 
     tic(5,"eliminate");
-    JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_);
+    JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearized, variableIndex_);
     sharedClique newRoot;
     if(params_.factorization == ISAM2Params::CHOLESKY)
       newRoot = jt.eliminate(EliminatePreferCholesky);
@@ -352,7 +343,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
 
     lastAffectedMarkedCount = markedKeys.size();
     lastAffectedVariableCount = affectedKeysSet->size();
-    lastAffectedFactorCount = linearFactors_.size();
+    lastAffectedFactorCount = linearized.size();
 
     // Reeliminated keys for detailed results
     if(params_.enableDetailedResults) {
@@ -428,8 +419,12 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
       reorderingMode.constrainedKeys = FastMap<Index,int>();
       BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); }
     }
+		FastSet<Index> affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve
+		BOOST_FOREACH(Index unused, unusedIndices) {
+			affectedUsedKeys.erase(unused);
+		}
     Impl::PartialSolveResult partialSolveResult =
-        Impl::PartialSolve(factors, *affectedKeysSet, reorderingMode, (params_.factorization == ISAM2Params::QR));
+        Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR));
     toc(2,"PartialSolve");
 
     // We now need to permute everything according this partial reordering: the
@@ -557,33 +552,25 @@ ISAM2Result ISAM2::update(
   // Remove removed factors from the variable index so we do not attempt to relinearize them
   variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_));
 
-	// We now need to start keeping track of the marked keys involved in added or
+	// Compute unused keys and indices
-	// removed factors.
-	FastSet<Index> markedKeys;
-
-	// Remove unused keys and add keys from removed factors that are still used
-	// in other factors to markedKeys.
-	{
-		// Get keys from removed factors
-		FastSet<Key> removedFactorSymbKeys = removeFactors.keys();
-
-		// For each key, if still used in other factors, add to markedKeys to be
-		// recalculated, or if not used, add to unusedKeys to be removed from the
-		// system.  Note that unusedKeys stores Key while markedKeys stores Index.
 	FastSet<Key> unusedKeys;
-		BOOST_FOREACH(Key key, removedFactorSymbKeys) {
+	FastSet<Index> unusedIndices;
-			Index index = ordering_[key];
+	{
-			if(variableIndex_[index].empty())
+		// Get keys from removed factors and new factors, and compute unused keys,
-				unusedKeys.insert(key);
+		// i.e., keys that are empty now and do not appear in the new factors.
-			else
+		FastSet<Key> removedAndEmpty;
-				markedKeys.insert(index);
+		BOOST_FOREACH(Key key, removeFactors.keys()) {
+			if(variableIndex_[ordering_[key]].empty())
+				removedAndEmpty.insert(removedAndEmpty.end(), key);
 		}
+		FastSet<Key> newFactorSymbKeys = newFactors.keys();
+		std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
+			newFactorSymbKeys.begin(), newFactorSymbKeys.end(), std::inserter(unusedKeys, unusedKeys.end()));
 
-		// Remove unused keys.  We must hold on to the new nodes index for now
+		// Get indices for unused keys
-		// instead of placing it into the tree because removeTop will need to
+		BOOST_FOREACH(Key key, unusedKeys) {
-		// update it.
+			unusedIndices.insert(unusedIndices.end(), ordering_[key]);
-		Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
+		}
-			deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
 	}
   toc(1,"push_back factors");
 
@@ -603,9 +590,11 @@ ISAM2Result ISAM2::update(
 
   tic(4,"gather involved keys");
   // 3. Mark linear update
+  FastSet<Index> markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
+  // Also mark keys involved in removed factors
   {
-		FastSet<Index> newFactorIndices = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors
+    FastSet<Index> markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors
-		markedKeys.insert(newFactorIndices.begin(), newFactorIndices.end());
+    markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys
   }
 
 	// Observed keys for detailed results
@@ -617,7 +606,11 @@ ISAM2Result ISAM2::update(
   // NOTE: we use assign instead of the iterator constructor here because this
   // is a vector of size_t, so the constructor unintentionally resolves to
   // vector(size_t count, Index value) instead of the iterator constructor.
-  FastVector<Index> observedKeys; observedKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them
+  FastVector<Index> observedKeys;  observedKeys.reserve(markedKeys.size());
+	BOOST_FOREACH(Index index, markedKeys) {
+		if(unusedIndices.find(index) == unusedIndices.end()) // Only add if not unused
+			observedKeys.push_back(index); // Make a copy of these, as we'll soon add to them
+	}
   toc(4,"gather involved keys");
 
   // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold
@@ -710,7 +703,7 @@ ISAM2Result ISAM2::update(
   }
   boost::shared_ptr<FastSet<Index> > replacedKeys;
   if(!markedKeys.empty() || !observedKeys.empty())
-    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, constrainedIndices, result);
+    replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result);
 
   // Update replaced keys mask (accumulates until back-substitution takes place)
   if(replacedKeys) {
@@ -718,10 +711,11 @@ ISAM2Result ISAM2::update(
       deltaReplacedMask_[var] = true; } }
   toc(9,"recalculate");
 
-  //tic(9,"solve");
+	// After the top of the tree has been redone and may have index gaps from
-  // 9. Solve
+	// unused keys, condense the indices to remove gaps by rearranging indices
-  if(debug) delta_.print("delta_: ");
+	// in all data structures.
-  //toc(9,"solve");
+	Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
+		deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
 
   result.cliques = this->nodes().size();
   deltaDoglegUptodate_ = false;

gtsam/nonlinear/ISAM2.h

@@ -516,7 +516,7 @@ private:
   GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
 
   boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
-      const FastVector<Index>& observedKeys, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
+      const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
   //	void linear_update(const GaussianFactorGraph& newFactors);
   void updateDelta(bool forceFullSolve = false) const;
 

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -46,13 +46,9 @@ public:
   double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5)
   VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity
 
-  LevenbergMarquardtParams() :
+  LevenbergMarquardtParams() : lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {}
-    lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), verbosityLM(SILENT) {}
-
-  LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose) :
-    lambdaInitial(initial), lambdaFactor(factor), lambdaUpperBound(bound), verbosityLM(VerbosityLM(verbose)) {}
-
   virtual ~LevenbergMarquardtParams() {}
+
   virtual void print(const std::string& str = "") const;
 
   inline double getlambdaInitial() const { return lambdaInitial; }

gtsam/slam/PriorFactor.h

@@ -65,8 +65,8 @@ namespace gtsam {
 
 		/** print */
 		virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-			std::cout << s << "PriorFactor(" << keyFormatter(this->key()) << ")\n";
+			std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << ", prior value:\n";
-			prior_.print("  prior");
+			prior_.print();
 			this->noiseModel_->print("  noise model: ");
 		}
 

gtsam/slam/sparseBA.cpp

@@ -0,0 +1,60 @@
+/**
+ * @file   sparseBA.cpp
+ * @brief  
+ * @date   Jul 6, 2012
+ * @author Yong-Dian Jian
+ */
+
+#include <gtsam/slam/sparseBA.h>
+
+namespace sparseBA {
+
+/* ************************************************************************* */
+void Graph::addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) {
+  addCameraConstraint<SimpleCamera>(cameraKey, camera);
+}
+
+/* ************************************************************************* */
+void Graph::addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model) {
+  addCameraPrior<SimpleCamera>(cameraKey, camera, model);
+}
+
+/* ************************************************************************* */
+void Graph::addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey) {
+  addMeasurement<SimpleCamera>(z, model, cameraKey, pointKey);
+}
+
+/* ************************************************************************* */
+Matrix Graph::reprojectionErrors(const Values& values) const {
+
+  // TODO: support the other calibration objects. Now it only works for Cal3_S2.
+
+  typedef GeneralSFMFactor<SimpleCamera, Point3> SFMFactor;
+  typedef GeneralSFMFactor2<Cal3_S2> SFMFactor2;
+
+  // first count
+  size_t K = 0, k=0;
+  BOOST_FOREACH(const sharedFactor& f, *this)
+    if (boost::dynamic_pointer_cast<const SFMFactor>(f)) ++K;
+    else if (boost::dynamic_pointer_cast<const SFMFactor2>(f)) ++K;
+
+  // now fill
+  Matrix errors(2,K);
+  BOOST_FOREACH(const sharedFactor& f, *this) {
+    boost::shared_ptr<const SFMFactor> p = boost::dynamic_pointer_cast<const SFMFactor>(f);
+    if (p) {
+      errors.col(k++) = p->unwhitenedError(values);
+      continue;
+    }
+
+    boost::shared_ptr<const SFMFactor2> p2 = boost::dynamic_pointer_cast<const SFMFactor2>(f);
+    if (p2) {
+      errors.col(k++) = p2->unwhitenedError(values);
+    }
+  }
+  return errors;
+}
+/* ************************************************************************* */
+}
+
+

gtsam/slam/sparseBA.h

@@ -0,0 +1,143 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   sba.h
+ * @brief  a suite for sparse bundle adjustment
+ * @date   Jul 5, 2012
+ * @author ydjian
+ */
+
+#pragma once
+
+#include <gtsam/slam/visualSLAM.h>
+#include <gtsam/slam/GeneralSFMFactor.h>
+#include <gtsam/geometry/SimpleCamera.h>
+
+namespace sparseBA {
+
+  using namespace gtsam;
+
+  /// Values class, inherited from Values, mainly used as a convenience for MATLAB wrapper
+  struct Values: public visualSLAM::Values {
+
+    typedef boost::shared_ptr<Values> shared_ptr;
+    typedef gtsam::Values::ConstFiltered<SimpleCamera> SimpleCameraFiltered;
+    typedef gtsam::Values::ConstFiltered<Cal3_S2> Cal3_S2Filtered;
+
+    /// Default constructor
+    Values() {}
+
+    /// Copy constructor
+    Values(const gtsam::Values& values) : visualSLAM::Values(values) {}
+
+    /// Constructor from filtered values view of poses
+    Values(const SimpleCameraFiltered& view) : visualSLAM::Values(view) {}
+
+    /// Constructor from filtered values view of points
+    Values(const PointFiltered& view) : visualSLAM::Values(view) {}
+
+    SimpleCameraFiltered allSimpleCameras() const { return this->filter<SimpleCamera>(); }  ///< camera view
+    size_t  nrSimpleCameras()  const { return allSimpleCameras().size(); }                  ///< get number of poses
+    KeyList simpleCameraKeys() const { return allSimpleCameras().keys(); }                  ///< get keys to poses only
+
+    /// insert a camera
+    void insertSimpleCamera(Key j, const SimpleCamera& camera) { insert(j, camera); }
+
+    /// update a camera
+    void updateSimpleCamera(Key j, const SimpleCamera& camera) { update(j, camera); }
+
+    /// get a camera
+    SimpleCamera simpleCamera(Key j) const { return at<SimpleCamera>(j); }
+  };
+
+  /**
+   * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper
+   * @addtogroup SLAM
+   */
+  struct Graph: public visualSLAM::Graph {
+
+    /// Default constructor
+    Graph(){}
+
+    /// Copy constructor given any other NonlinearFactorGraph
+    Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {}
+
+    /// check if two graphs are equal
+    bool equals(const Graph& p, double tol = 1e-9) const {
+      return NonlinearFactorGraph::equals(p, tol);
+    }
+
+    /**
+      *  Add a prior on a pose
+      *  @param key variable key of the camera
+      *  @param p around which soft prior is defined
+      *  @param model uncertainty model of this prior
+      */
+    template <typename Camera>
+    void addCameraPrior(Key cameraKey, const Camera &camera, SharedNoiseModel &model = noiseModel::Unit::Create(Camera::Dim())) {
+      sharedFactor factor(new PriorFactor<Camera>(cameraKey, camera, model));
+      push_back(factor);
+    }
+
+    /**
+     *  Add a constraint on a camera
+     *  @param key variable key of the camera
+     *  @param p to which camera to constrain it to
+     */
+    template <typename Camera>
+    void addCameraConstraint(Key cameraKey, const Camera &camera) {
+      sharedFactor factor(new NonlinearEquality<Camera>(cameraKey, camera));
+      push_back(factor);
+    }
+
+    /**
+     *  Add a 2d projection measurement
+     *  @param z the measurement
+     *  @param model the noise model for the measurement
+     *  @param cameraKey variable key for the pose+calibration
+     *  @param pointKey variable key for the point
+     */
+    template <typename Camera>
+    void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) {
+      typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
+      boost::shared_ptr<SFMFactor> factor(new SFMFactor(z, model, cameraKey, pointKey));
+      push_back(factor);
+    }
+
+    /**
+     *  Add a 2d projection measurement, but supports separated (or shared) pose and calibration object
+     *  @param z the measurement
+     *  @param model the noise model for the measurement
+     *  @param poseKey variable key for the pose
+     *  @param pointKey variable key for the point
+     *  @param calibKey variable key for the calibration
+     */
+    template <typename Calib>
+    void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) {
+      typedef GeneralSFMFactor2<Calib> SFMFactor;
+      boost::shared_ptr<SFMFactor> factor(new SFMFactor(z, model, posekey, pointkey, calibkey));
+      push_back(factor);
+    }
+
+    /// Return a 2*K Matrix of reprojection errors
+    Matrix reprojectionErrors(const Values& values) const;
+
+    /**
+     * Matlab-specific wrappers
+     */
+
+    void addSimpleCameraPrior(Key cameraKey, const SimpleCamera &camera, SharedNoiseModel &model);
+    void addSimpleCameraConstraint(Key cameraKey, const SimpleCamera &camera) ;
+    void addSimpleCameraMeasurement(const Point2 &z, SharedNoiseModel& model, Index cameraKey, Index pointKey);
+  };
+
+}

gtsam/slam/tests/testSparseBA.cpp

@@ -0,0 +1,193 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testsparseBA.cpp
+ * @brief  
+ * @date   Jul 5, 2012
+ * @author Yong-Dian Jian
+ */
+
+#include <gtsam/slam/sparseBA.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/shared_ptr.hpp>
+
+using namespace std;
+using namespace boost;
+using namespace gtsam;
+
+/* ************************************************************************* */
+
+static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
+
+// Convenience for named keys
+using symbol_shorthand::X;    /* pose3 */
+using symbol_shorthand::K;    /* calibration */
+using symbol_shorthand::C;    /* camera = [pose calibration] */
+using symbol_shorthand::L;    /* point3 */
+
+static Point3 landmark1(-1.0,-1.0, 0.0);
+static Point3 landmark2(-1.0, 1.0, 0.0);
+static Point3 landmark3( 1.0, 1.0, 0.0);
+static Point3 landmark4( 1.0,-1.0, 0.0);
+
+static Pose3 pose1(Matrix_(3,3,
+                           1., 0., 0.,
+                           0.,-1., 0.,
+                           0., 0.,-1.),
+                   Point3(0,0,6.25));
+
+static Pose3 pose2(Matrix_(3,3,
+                           1., 0., 0.,
+                           0.,-1., 0.,
+                           0., 0.,-1.),
+                   Point3(0,0,5.00));
+
+static Cal3_S2 calib1 (625, 625, 0, 0, 0);
+static Cal3_S2 calib2 (625, 625, 0, 0, 0);
+
+
+typedef PinholeCamera<Cal3_S2> Camera;
+
+static Camera camera1(pose1, calib1);
+static Camera camera2(pose2, calib2);
+
+/* ************************************************************************* */
+sparseBA::Graph testGraph1() {
+  Point2 z11(-100, 100);
+  Point2 z12(-100,-100);
+  Point2 z13( 100,-100);
+  Point2 z14( 100, 100);
+  Point2 z21(-125, 125);
+  Point2 z22(-125,-125);
+  Point2 z23( 125,-125);
+  Point2 z24( 125, 125);
+
+
+  sparseBA::Graph g;
+  g.addMeasurement<Camera>(z11, sigma, C(1), L(1));
+  g.addMeasurement<Camera>(z12, sigma, C(1), L(2));
+  g.addMeasurement<Camera>(z13, sigma, C(1), L(3));
+  g.addMeasurement<Camera>(z14, sigma, C(1), L(4));
+  g.addMeasurement<Camera>(z21, sigma, C(2), L(1));
+  g.addMeasurement<Camera>(z22, sigma, C(2), L(2));
+  g.addMeasurement<Camera>(z23, sigma, C(2), L(3));
+  g.addMeasurement<Camera>(z24, sigma, C(2), L(4));
+  return g;
+}
+
+sparseBA::Graph testGraph2() {
+  Point2 z11(-100, 100);
+  Point2 z12(-100,-100);
+  Point2 z13( 100,-100);
+  Point2 z14( 100, 100);
+  Point2 z21(-125, 125);
+  Point2 z22(-125,-125);
+  Point2 z23( 125,-125);
+  Point2 z24( 125, 125);
+
+  sparseBA::Graph g;
+  g.addMeasurement<Cal3_S2>(z11, sigma, X(1), L(1), K(1));
+  g.addMeasurement<Cal3_S2>(z12, sigma, X(1), L(2), K(1));
+  g.addMeasurement<Cal3_S2>(z13, sigma, X(1), L(3), K(1));
+  g.addMeasurement<Cal3_S2>(z14, sigma, X(1), L(4), K(1));
+  g.addMeasurement<Cal3_S2>(z21, sigma, X(2), L(1), K(1));
+  g.addMeasurement<Cal3_S2>(z22, sigma, X(2), L(2), K(1));
+  g.addMeasurement<Cal3_S2>(z23, sigma, X(2), L(3), K(1));
+  g.addMeasurement<Cal3_S2>(z24, sigma, X(2), L(4), K(1));
+  return g;
+}
+
+/* ************************************************************************* */
+TEST( optimizeLM1, sparseBA )
+{
+  // build a graph
+  sparseBA::Graph graph(testGraph1());
+
+  // add 3 landmark constraints
+  graph.addPointConstraint(L(1), landmark1);
+  graph.addPointConstraint(L(2), landmark2);
+  graph.addPointConstraint(L(3), landmark3);
+
+  // Create an initial values structure corresponding to the ground truth
+  Values initialEstimate;
+  initialEstimate.insert(C(1), camera1);
+  initialEstimate.insert(C(2), camera2);
+  initialEstimate.insert(L(1), landmark1);
+  initialEstimate.insert(L(2), landmark2);
+  initialEstimate.insert(L(3), landmark3);
+  initialEstimate.insert(L(4), landmark4);
+
+  // Create an ordering of the variables
+  Ordering ordering;
+  ordering += L(1),L(2),L(3),L(4),C(1),C(2);
+
+  // Create an optimizer and check its error
+  // We expect the initial to be zero because config is the ground truth
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // Iterate once, and the config should not have changed because we started
+  // with the ground truth
+  optimizer.iterate();
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // check if correct
+  CHECK(assert_equal(initialEstimate, optimizer.values()));
+}
+
+/* ************************************************************************* */
+TEST( optimizeLM2, sparseBA )
+{
+  // build a graph
+  sparseBA::Graph graph(testGraph2());
+
+  // add 3 landmark constraints
+  graph.addPointConstraint(L(1), landmark1);
+  graph.addPointConstraint(L(2), landmark2);
+  graph.addPointConstraint(L(3), landmark3);
+
+  // Create an initial values structure corresponding to the ground truth
+  Values initialEstimate;
+  initialEstimate.insert(X(1), pose1);
+  initialEstimate.insert(X(2), pose2);
+  initialEstimate.insert(L(1), landmark1);
+  initialEstimate.insert(L(2), landmark2);
+  initialEstimate.insert(L(3), landmark3);
+  initialEstimate.insert(L(4), landmark4);
+  initialEstimate.insert(K(1), calib2);
+
+  // Create an ordering of the variables
+  Ordering ordering;
+  ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1);
+
+  // Create an optimizer and check its error
+  // We expect the initial to be zero because config is the ground truth
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering);
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // Iterate once, and the config should not have changed because we started
+  // with the ground truth
+  optimizer.iterate();
+  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
+
+  // check if correct
+  CHECK(assert_equal(initialEstimate, optimizer.values()));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

gtsam/slam/tests/testVisualSLAM.cpp

@@ -42,14 +42,14 @@ static Point3 landmark2(-1.0, 1.0, 0.0);
 static Point3 landmark3( 1.0, 1.0, 0.0);
 static Point3 landmark4( 1.0,-1.0, 0.0);
 
-static Pose3 camera1(Matrix_(3,3,
+static Pose3 pose1(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
 		       ),
 	      Point3(0,0,6.25));
 
-static Pose3 camera2(Matrix_(3,3,
+static Pose3 pose2(Matrix_(3,3,
 		       1., 0., 0.,
 		       0.,-1., 0.,
 		       0., 0.,-1.
@@ -92,8 +92,8 @@ TEST( VisualSLAM, optimizeLM)
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -124,13 +124,13 @@ TEST( VisualSLAM, optimizeLM2)
   // build a graph
   visualSLAM::Graph graph(testGraph());
 	// add 2 camera constraints
-  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(1), pose1);
-  graph.addPoseConstraint(X(2), camera2);
+  graph.addPoseConstraint(X(2), pose2);
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -160,13 +160,13 @@ TEST( VisualSLAM, LMoptimizer)
   // build a graph
   visualSLAM::Graph graph(testGraph());
   // add 2 camera constraints
-  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(1), pose1);
-  graph.addPoseConstraint(X(2), camera2);
+  graph.addPoseConstraint(X(2), pose2);
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -184,6 +184,10 @@ TEST( VisualSLAM, LMoptimizer)
 
   // check if correct
   CHECK(assert_equal(initialEstimate, optimizer.values()));
+
+  // check errors
+  Matrix errors = graph.reprojectionErrors(optimizer.values());
+  CHECK(assert_equal(zeros(2,8), errors));
 }
 
 
@@ -193,13 +197,13 @@ TEST( VisualSLAM, CHECK_ORDERING)
   // build a graph
   visualSLAM::Graph graph = testGraph();
   // add 2 camera constraints
-  graph.addPoseConstraint(X(1), camera1);
+  graph.addPoseConstraint(X(1), pose1);
-  graph.addPoseConstraint(X(2), camera2);
+  graph.addPoseConstraint(X(2), pose2);
 
   // Create an initial values structure corresponding to the ground truth
   Values initialEstimate;
-  initialEstimate.insert(X(1), camera1);
+  initialEstimate.insert(X(1), pose1);
-  initialEstimate.insert(X(2), camera2);
+  initialEstimate.insert(X(2), pose2);
   initialEstimate.insert(L(1), landmark1);
   initialEstimate.insert(L(2), landmark2);
   initialEstimate.insert(L(3), landmark3);
@@ -274,8 +278,8 @@ TEST( VisualSLAM, keys_and_view )
 {
 	// create config
 	visualSLAM::Values c;
-	c.insert(X(1), camera1);
+	c.insert(X(1), pose1);
-	c.insert(X(2), camera2);
+	c.insert(X(2), pose2);
 	c.insert(L(2), landmark2);
 	LONGS_EQUAL(2,c.nrPoses());
 	LONGS_EQUAL(1,c.nrPoints());
@@ -299,6 +303,42 @@ TEST( VisualSLAM, keys_and_view )
 	}
 }
 
+/* ************************************************************************* */
+TEST( VisualSLAM, addMeasurements )
+{
+  // create config
+  visualSLAM::Graph g;
+  Vector J = Vector_(3,1.0,2.0,3.0);
+  Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
+  shared_ptrK sK(new Cal3_S2(625, 625, 0, 0, 0));
+  g.addMeasurements(0,J,Z,sigma,sK);
+  EXPECT_LONGS_EQUAL(3,g.size());
+}
+
+/* ************************************************************************* */
+TEST( VisualSLAM, insertBackProjections )
+{
+  // create config
+  visualSLAM::Values c;
+  SimpleCamera camera(pose1);
+  Vector J = Vector_(3,1.0,2.0,3.0);
+  Matrix Z = Matrix_(2,3, -1.0,0.0,1.0, -1.0,0.0,1.0);
+  c.insertBackprojections(camera,J,Z,1.0);
+  EXPECT_LONGS_EQUAL(3,c.nrPoints());
+}
+
+/* ************************************************************************* */
+TEST( VisualSLAM, perturbPoints )
+{
+  visualSLAM::Values c1,c2;
+  c1.insert(L(2), landmark2);
+  c1.perturbPoints(0.01,42u);
+  CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c1.point(L(2)),1e-6));
+  c2.insert(L(2), landmark2);
+  c2.perturbPoints(0.01,42u);
+  CHECK(assert_equal(Point3(-0.986984, 0.999534, -0.0147962),c2.point(L(2)),1e-6));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/visualSLAM.cpp

@@ -18,11 +18,35 @@
 #include <gtsam/slam/visualSLAM.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/linear/Sampler.h>
 #include <boost/make_shared.hpp>
 
+namespace visualSLAM {
+
   using boost::make_shared;
 
-namespace visualSLAM {
+  /* ************************************************************************* */
+  void Values::insertBackprojections(const SimpleCamera& camera,
+      const Vector& J, const Matrix& Z, double depth) {
+    if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
+    if (Z.cols() != J.size()) throw std::invalid_argument(
+          "insertBackProjections: J and Z must have same number of entries");
+    for(size_t k=0;k<Z.cols();k++) {
+      Point2 p(Z(0,k),Z(1,k));
+      Point3 P = camera.backproject(p, depth);
+      insertPoint(J(k), P);
+    }
+  }
+
+  /* ************************************************************************* */
+  void Values::perturbPoints(double sigma, int32_t seed) {
+    ConstFiltered<Point3> points = allPoints();
+    noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
+    Sampler sampler(model, seed);
+    BOOST_FOREACH(const ConstFiltered<Point3>::KeyValuePair& keyValue, points) {
+      update(keyValue.key, keyValue.value.retract(sampler.sample()));
+    }
+  }
 
   /* ************************************************************************* */
   Matrix Values::points() const {
@@ -47,7 +71,19 @@ namespace visualSLAM {
   /* ************************************************************************* */
   void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
        Key poseKey, Key pointKey, const shared_ptrK K) {
-    push_back(make_shared<GenericProjectionFactor<Pose3, Point3> >(measured, model, poseKey, pointKey, K));
+    push_back(
+        make_shared<GenericProjectionFactor<Pose3, Point3> >
+          (measured, model, poseKey, pointKey, K));
+  }
+
+  /* ************************************************************************* */
+  void Graph::addMeasurements(Key i, const Vector& J, const Matrix& Z,
+      const SharedNoiseModel& model, const shared_ptrK K) {
+    if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
+    if (Z.cols() != J.size()) throw std::invalid_argument(
+          "addMeasurements: J and Z must have same number of entries");
+    for (size_t k = 0; k < Z.cols(); k++)
+      addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
   }
 
   /* ************************************************************************* */
@@ -62,4 +98,19 @@ namespace visualSLAM {
   }
 
   /* ************************************************************************* */
+  Matrix Graph::reprojectionErrors(const Values& values) const {
+    // first count
+    size_t K = 0, k=0;
+    BOOST_FOREACH(const sharedFactor& f, *this)
+      if (boost::dynamic_pointer_cast<const ProjectionFactor>(f)) ++K;
+    // now fill
+    Matrix errors(2,K);
+    BOOST_FOREACH(const sharedFactor& f, *this) {
+      boost::shared_ptr<const ProjectionFactor> p =
+          boost::dynamic_pointer_cast<const ProjectionFactor>(f);
+      if (p) errors.col(k++) = p->unwhitenedError(values);
+    }
+    return errors;
+  }
+  /* ************************************************************************* */
 }

gtsam/slam/visualSLAM.h

@@ -68,7 +68,15 @@ namespace visualSLAM {
     /// get a point
     Point3 point(Key j) const { return at<Point3>(j); }
 
-    Matrix points() const; ///< get all point coordinates in a matrix
+    /// insert a number of initial point values by backprojecting
+    void insertBackprojections(const SimpleCamera& c, const Vector& J,
+        const Matrix& Z, double depth);
+
+    /// perturb all points using normally distributed noise
+    void perturbPoints(double sigma, int32_t seed = 42u);
+
+    /// get all point coordinates in a matrix
+    Matrix points() const;
 
   };
 
@@ -94,14 +102,14 @@ namespace visualSLAM {
 
     /**
      *  Add a constraint on a point (for now, *must* be satisfied in any Values)
-     *  @param key variable key of the landmark
+     *  @param key variable key of the point
      *  @param p point around which soft prior is defined
      */
     void addPointConstraint(Key pointKey, const Point3& p = Point3());
 
     /**
-     *  Add a prior on a landmark
+     *  Add a prior on a point
-     *  @param key variable key of the landmark
+     *  @param key variable key of the point
      *  @param p to which point to constrain it to
      *  @param model uncertainty model of this prior
      */
@@ -109,10 +117,10 @@ namespace visualSLAM {
 				const SharedNoiseModel& model = noiseModel::Unit::Create(3));
 
     /**
-     *  Add a range prior to a landmark
+     *  Add a range prior to a point
      *  @param poseKey variable key of the camera pose
-     *  @param pointKey variable key of the landmark
+     *  @param pointKey variable key of the point
-     *  @param range approximate range to landmark
+     *  @param range approximate range to point
      *  @param model uncertainty model of this prior
      */
     void addRangeFactor(Key poseKey, Key pointKey, double range,
@@ -123,23 +131,37 @@ namespace visualSLAM {
      *  @param measured the measurement
      *  @param model the noise model for the measurement
      *  @param poseKey variable key for the camera pose
-     *  @param pointKey variable key for the landmark
+     *  @param pointKey variable key for the point
      *  @param K shared pointer to calibration object
      */
     void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
         Key poseKey, Key pointKey, const shared_ptrK K);
 
+    /**
+     *  Add a number of measurements at the same time
+     *  @param i variable key for the camera pose
+     *  @param J variable keys for the point, KeyVector of size K
+     *  @param Z the 2*K measurements as a matrix
+     *  @param model the noise model for the measurement
+     *  @param K shared pointer to calibration object
+     */
+    void addMeasurements(Key i, const Vector& J, const Matrix& Z,
+        const SharedNoiseModel& model, const shared_ptrK K);
+
     /**
      *  Add a stereo factor measurement
      *  @param measured the measurement
      *  @param model the noise model for the measurement
      *  @param poseKey variable key for the camera pose
-     *  @param pointKey variable key for the landmark
+     *  @param pointKey variable key for the point
      *  @param K shared pointer to stereo calibration object
      */
     void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
         Key poseKey, Key pointKey, const shared_ptrKStereo K);
 
+    /// Return a 2*K Matrix of reprojection errors
+    Matrix reprojectionErrors(const Values& values) const;
+
   }; // Graph
 
   /**

gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp

@@ -1,22 +0,0 @@
-/**
- * @file testLinearContainerFactors.cpp
- *
- * @brief Tests the use of nonlinear containers for simple integration of linear factors into nonlinear graphs
- *
- * @date Jun 7, 2012
- * @author Alex Cunningham
- */
-
-#include <CppUnitLite/TestHarness.h>
-
-
-/* ************************************************************************* */
-TEST( testLinearContainerFactors, jacobian_factor ) {
-
-//	LinearContainerFactor actualLinFactor();
-
-}
-
-/* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
-/* ************************************************************************* */

matlab/examples/SBAExample.m

@@ -0,0 +1,85 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% 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 An SFM example (adapted from SFMExample.m) optimizing calibration
+% @author Yong-Dian Jian
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%% Assumptions
+%  - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc...
+%  - Cameras are on a circle around the cube, pointing at the world origin
+%  - Each camera sees all landmarks. 
+%  - Visual measurements as 2D points are given, corrupted by Gaussian noise.
+
+% Data Options
+options.triangle = false;
+options.nrCameras = 10;
+options.showImages = false;
+
+%% Generate data
+[data,truth] = VisualISAMGenerateData(options);
+
+measurementNoiseSigma = 1.0;
+pointNoiseSigma = 0.1;
+cameraNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1 ...
+                     0.001*ones(1,5)]';
+
+%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph)
+graph = sparseBAGraph;
+
+ 
+%% Add factors for all measurements
+measurementNoise = gtsamnoiseModelIsotropic_Sigma(2,measurementNoiseSigma);
+for i=1:length(data.Z)
+    for k=1:length(data.Z{i})
+        j = data.J{i}{k};
+        graph.addSimpleCameraMeasurement(data.Z{i}{k}, measurementNoise, symbol('c',i), symbol('p',j));
+    end
+end
+
+%% Add Gaussian priors for a pose and a landmark to constrain the system
+cameraPriorNoise  = gtsamnoiseModelDiagonal_Sigmas(cameraNoiseSigmas);
+firstCamera = gtsamSimpleCamera(truth.cameras{1}.pose, truth.K);
+graph.addSimpleCameraPrior(symbol('c',1), firstCamera, cameraPriorNoise);
+
+pointPriorNoise  = gtsamnoiseModelIsotropic_Sigma(3,pointNoiseSigma);
+graph.addPointPrior(symbol('p',1), truth.points{1}, pointPriorNoise);
+
+%% Print the graph
+graph.print(sprintf('\nFactor graph:\n'));
+
+
+%% Initialize cameras and points close to ground truth in this example
+initialEstimate = sparseBAValues;
+for i=1:size(truth.cameras,2)
+    pose_i = truth.cameras{i}.pose.retract(0.1*randn(6,1));
+    camera_i = gtsamSimpleCamera(pose_i, truth.K);
+    initialEstimate.insertSimpleCamera(symbol('c',i), camera_i);
+end
+for j=1:size(truth.points,2)
+    point_j = truth.points{j}.retract(0.1*randn(3,1));
+    initialEstimate.insertPoint(symbol('p',j), point_j);
+end
+initialEstimate.print(sprintf('\nInitial estimate:\n  '));
+
+%% Fine grain optimization, allowing user to iterate step by step
+
+parameters = gtsamLevenbergMarquardtParams;
+parameters.setlambdaInitial(1.0);
+parameters.setVerbosityLM('trylambda');
+
+optimizer = graph.optimizer(initialEstimate, parameters);
+
+for i=1:5
+    optimizer.iterate();
+end
+
+result = optimizer.values();
+result.print(sprintf('\nFinal result:\n  '));
+
+

matlab/examples/SFMExample.m

@@ -63,7 +63,7 @@ initialEstimate.print(sprintf('\nInitial estimate:\n  '));
 
 %% Fine grain optimization, allowing user to iterate step by step
 
-parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0);
+parameters = gtsamLevenbergMarquardtParams;
 parameters.setlambdaInitial(1.0);
 parameters.setVerbosityLM('trylambda');
 

matlab/tests/testSFMExample.m

@@ -48,7 +48,7 @@ for j=1:size(truth.points,2)
 end
 
 %% Optimization
-parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0);
+parameters = gtsamLevenbergMarquardtParams;
 optimizer = graph.optimizer(initialEstimate, parameters);
 for i=1:5
     optimizer.iterate();

wrap/tests/expected/@Point2/Point2.m

@@ -7,12 +7,12 @@ classdef Point2 < handle
     function obj = Point2(varargin)
       if (nargin == 0), obj.self = new_Point2(0,0); end
       if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Point2(0,1,varargin{1},varargin{2}); end
-      if nargin ==14, new_Point2_(varargin{1},0); end
+      if nargin ==14, new_Point2(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Point2 constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_Point2_(obj.self);
+        new_Point2(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected/@Point3/Point3.m

@@ -6,12 +6,12 @@ classdef Point3 < handle
   methods
     function obj = Point3(varargin)
       if (nargin == 3 && isa(varargin{1},'double') && isa(varargin{2},'double') && isa(varargin{3},'double')), obj.self = new_Point3(0,0,varargin{1},varargin{2},varargin{3}); end
-      if nargin ==14, new_Point3_(varargin{1},0); end
+      if nargin ==14, new_Point3(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Point3 constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_Point3_(obj.self);
+        new_Point3(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected/@Test/Test.m

@@ -7,12 +7,12 @@ classdef Test < handle
     function obj = Test(varargin)
       if (nargin == 0), obj.self = new_Test(0,0); end
       if (nargin == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')), obj.self = new_Test(0,1,varargin{1},varargin{2}); end
-      if nargin ==14, new_Test_(varargin{1},0); end
+      if nargin ==14, new_Test(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('Test constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_Test_(obj.self);
+        new_Test(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected/new_Point2.cpp

@@ -6,8 +6,10 @@ typedef boost::shared_ptr<Point2> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected/new_Point3.cpp

@@ -7,8 +7,10 @@ typedef boost::shared_ptr<Point3> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected/new_Test.cpp

@@ -7,8 +7,10 @@ typedef boost::shared_ptr<Test> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected_namespaces/@ClassD/ClassD.m

@@ -6,12 +6,12 @@ classdef ClassD < handle
   methods
     function obj = ClassD(varargin)
       if (nargin == 0), obj.self = new_ClassD(0,0); end
-      if nargin ==14, new_ClassD_(varargin{1},0); end
+      if nargin ==14, new_ClassD(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ClassD constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_ClassD_(obj.self);
+        new_ClassD(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected_namespaces/@ns1ClassA/ns1ClassA.m

@@ -6,12 +6,12 @@ classdef ns1ClassA < handle
   methods
     function obj = ns1ClassA(varargin)
       if (nargin == 0), obj.self = new_ns1ClassA(0,0); end
-      if nargin ==14, new_ns1ClassA_(varargin{1},0); end
+      if nargin ==14, new_ns1ClassA(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns1ClassA constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_ns1ClassA_(obj.self);
+        new_ns1ClassA(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected_namespaces/@ns1ClassB/ns1ClassB.m

@@ -6,12 +6,12 @@ classdef ns1ClassB < handle
   methods
     function obj = ns1ClassB(varargin)
       if (nargin == 0), obj.self = new_ns1ClassB(0,0); end
-      if nargin ==14, new_ns1ClassB_(varargin{1},0); end
+      if nargin ==14, new_ns1ClassB(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns1ClassB constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_ns1ClassB_(obj.self);
+        new_ns1ClassB(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected_namespaces/@ns2ClassA/ns2ClassA.m

@@ -6,12 +6,12 @@ classdef ns2ClassA < handle
   methods
     function obj = ns2ClassA(varargin)
       if (nargin == 0), obj.self = new_ns2ClassA(0,0); end
-      if nargin ==14, new_ns2ClassA_(varargin{1},0); end
+      if nargin ==14, new_ns2ClassA(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ClassA constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_ns2ClassA_(obj.self);
+        new_ns2ClassA(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected_namespaces/@ns2ClassC/ns2ClassC.m

@@ -6,12 +6,12 @@ classdef ns2ClassC < handle
   methods
     function obj = ns2ClassC(varargin)
       if (nargin == 0), obj.self = new_ns2ClassC(0,0); end
-      if nargin ==14, new_ns2ClassC_(varargin{1},0); end
+      if nargin ==14, new_ns2ClassC(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ClassC constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_ns2ClassC_(obj.self);
+        new_ns2ClassC(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected_namespaces/@ns2ns3ClassB/ns2ns3ClassB.m

@@ -6,12 +6,12 @@ classdef ns2ns3ClassB < handle
   methods
     function obj = ns2ns3ClassB(varargin)
       if (nargin == 0), obj.self = new_ns2ns3ClassB(0,0); end
-      if nargin ==14, new_ns2ns3ClassB_(varargin{1},0); end
+      if nargin ==14, new_ns2ns3ClassB(varargin{1},0); end
       if nargin ~= 13 && nargin ~= 14 && obj.self == 0, error('ns2ns3ClassB constructor failed'); end
     end
     function delete(obj)
       if obj.self ~= 0
-        new_ns2ns3ClassB_(obj.self);
+        new_ns2ns3ClassB(obj.self);
         obj.self = 0;
       end
     end

wrap/tests/expected_namespaces/new_ClassD.cpp

@@ -6,8 +6,10 @@ typedef boost::shared_ptr<ClassD> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected_namespaces/new_ns1ClassA.cpp

@@ -6,8 +6,10 @@ typedef boost::shared_ptr<ns1::ClassA> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected_namespaces/new_ns1ClassB.cpp

@@ -7,8 +7,10 @@ typedef boost::shared_ptr<ns1::ClassB> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected_namespaces/new_ns2ClassA.cpp

@@ -7,8 +7,10 @@ typedef boost::shared_ptr<ns2::ClassA> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected_namespaces/new_ns2ClassC.cpp

@@ -6,8 +6,10 @@ typedef boost::shared_ptr<ns2::ClassC> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {

wrap/tests/expected_namespaces/new_ns2ns3ClassB.cpp

@@ -7,8 +7,10 @@ typedef boost::shared_ptr<ns2::ns3::ClassB> Shared;
 static std::set<Shared*> collector;
 
 void cleanup(void) {
-  BOOST_FOREACH(Shared* p, collector)
+  for(std::set<Shared*>::iterator iter = collector.begin(); iter != collector.end(); ) {
-    collector.erase(p);
+    delete *iter;
+    collector.erase(iter++);
+  }
 }
 void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
 {