diff --git a/gtsam.h b/gtsam.h index 3db42c90f..5af8f4e7c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1288,10 +1288,14 @@ 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, - size_t poseKey, size_t pointKey, 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); + void addMeasurement(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* model, size_t poseKey, size_t pointKey, + const gtsam::Cal3_S2* K); + void addMeasurements(size_t i, const gtsam::KeyVector& 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); }; class ISAM { diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 6f31c345c..162ba03c7 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -47,7 +47,20 @@ namespace visualSLAM { /* ************************************************************************* */ void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key pointKey, const shared_ptrK K) { - push_back(make_shared >(measured, model, poseKey, pointKey, K)); + push_back( + make_shared > + (measured, model, poseKey, pointKey, K)); + } + + /* ************************************************************************* */ + void Graph::addMeasurements(Key i, const KeyVector& 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