diff --git a/gtsam.h b/gtsam.h index e59a2f382..5564d38a9 100644 --- a/gtsam.h +++ b/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 { diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index 680a9ab9b..65c31adf9 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/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)); } diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 3f974abed..1998f1691 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/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(f)) ++K; + // now fill + Matrix errors(2,K); + BOOST_FOREACH(const sharedFactor& f, *this) { + boost::shared_ptr p = + boost::dynamic_pointer_cast(f); + if (p) errors.col(k) = p->unwhitenedError(values); + } + return errors; + } + /* ************************************************************************* */ } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index eca576b57..ae2956c5f 100644 --- a/gtsam/slam/visualSLAM.h +++ b/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 /**