From 1bc4db97c6859ffb4f520e50707d213f131c2393 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 5 Jul 2012 23:32:37 +0000 Subject: [PATCH] More efficient operations from within MATLAB --- gtsam.h | 4 +- gtsam/slam/tests/testVisualSLAM.cpp | 72 +++++++++++++++++++++-------- gtsam/slam/visualSLAM.cpp | 39 ++++++++++++---- gtsam/slam/visualSLAM.h | 12 ++++- 4 files changed, 98 insertions(+), 29 deletions(-) diff --git a/gtsam.h b/gtsam.h index de022bd70..e59a2f382 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1272,6 +1272,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; }; @@ -1312,7 +1314,7 @@ class Graph { 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, + 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, diff --git a/gtsam/slam/tests/testVisualSLAM.cpp b/gtsam/slam/tests/testVisualSLAM.cpp index faf733a22..680a9ab9b 100644 --- a/gtsam/slam/tests/testVisualSLAM.cpp +++ b/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(2), camera2); + 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); @@ -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(2), camera2); + graph.addPoseConstraint(X(1), pose1); + 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(2), camera2); + 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); @@ -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(2), camera2); + graph.addPoseConstraint(X(1), pose1); + 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(2), camera2); + 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); @@ -193,13 +193,13 @@ TEST( VisualSLAM, CHECK_ORDERING) // build a graph visualSLAM::Graph graph = testGraph(); // add 2 camera constraints - graph.addPoseConstraint(X(1), camera1); - graph.addPoseConstraint(X(2), camera2); + graph.addPoseConstraint(X(1), pose1); + 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(2), camera2); + 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); @@ -274,8 +274,8 @@ TEST( VisualSLAM, keys_and_view ) { // create config visualSLAM::Values c; - c.insert(X(1), camera1); - c.insert(X(2), camera2); + c.insert(X(1), pose1); + c.insert(X(2), pose2); c.insert(L(2), landmark2); LONGS_EQUAL(2,c.nrPoses()); LONGS_EQUAL(1,c.nrPoints()); @@ -299,6 +299,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); } /* ************************************************************************* */ diff --git a/gtsam/slam/visualSLAM.cpp b/gtsam/slam/visualSLAM.cpp index 162ba03c7..3f974abed 100644 --- a/gtsam/slam/visualSLAM.cpp +++ b/gtsam/slam/visualSLAM.cpp @@ -18,12 +18,36 @@ #include #include #include +#include #include -using boost::make_shared; - namespace visualSLAM { + using boost::make_shared; + + /* ************************************************************************* */ + 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 points = allPoints(); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); + Sampler sampler(model, seed); + BOOST_FOREACH(const ConstFiltered::KeyValuePair& keyValue, points) { + update(keyValue.key, keyValue.value.retract(sampler.sample())); + } + } + /* ************************************************************************* */ Matrix Values::points() const { size_t j=0; @@ -53,14 +77,13 @@ namespace visualSLAM { } /* ************************************************************************* */ - void Graph::addMeasurements(Key i, const KeyVector& J, const Matrix& Z, + 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( + 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(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; }; @@ -137,7 +145,7 @@ namespace visualSLAM { * @param model the noise model for the measurement * @param K shared pointer to calibration object */ - void addMeasurements(Key i, const KeyVector& J, const Matrix& Z, + void addMeasurements(Key i, const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const shared_ptrK K); /**