reprojectionErrors

gtsam.h

@@ -1319,6 +1319,10 @@ class Graph {
     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 {

gtsam/slam/tests/testVisualSLAM.cpp

@@ -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));
 }
 
 

gtsam/slam/visualSLAM.cpp

@@ -98,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

@@ -159,6 +159,9 @@ namespace visualSLAM {
     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
 
   /**