diff --git a/gtsam.h b/gtsam.h index 346ded351..578d44a08 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1550,6 +1550,7 @@ namespace utilities { void perturbPoint2(gtsam::Values& values, double sigma, int seed); void perturbPoint3(gtsam::Values& values, double sigma, int seed); void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); + void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); } //\namespace utilities diff --git a/matlab.h b/matlab.h index 1a9af8396..1be686ff4 100644 --- a/matlab.h +++ b/matlab.h @@ -108,6 +108,19 @@ namespace gtsam { } } + /// insert multiple projection factors for a single pose key + void insertProjectionFactors(NonlinearFactorGraph& graph, 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 (int k = 0; k < Z.cols(); k++) { + graph.push_back( + make_shared > + (Point2(Z(0, k), Z(1, k)), model, i, J(k), K)); + } + } + /// calculate the errors of all projection factors in a graph Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { // first count