12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

More efficient operations from within MATLAB

release/4.3a0
Frank Dellaert 2012-07-05 23:32:37 +00:00
parent 134951f21c
commit 1bc4db97c6
4 changed files with 98 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam.h
View File

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

72
gtsam/slam/tests/testVisualSLAM.cpp
View File

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

39
gtsam/slam/visualSLAM.cpp
View File

@ -18,12 +18,36 @@
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <boost/make_shared.hpp>
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<Z.cols();k++) {
Point2 p(Z(0,k),Z(1,k));
Point3 P = camera.backproject(p, depth);
insertPoint(J(k), P);
}
}
/* ************************************************************************* */
void Values::perturbPoints(double sigma, int32_t seed) {
ConstFiltered<Point3> points = allPoints();
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const ConstFiltered<Point3>::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<Z.cols();k++)
addMeasurement(Point2(Z(0,k),Z(1,k)),model,i, J[k], K);
for (size_t k = 0; k < Z.cols(); k++)
addMeasurement(Point2(Z(0, k), Z(1, k)), model, i, J(k), K);
}
/* ************************************************************************* */

12
gtsam/slam/visualSLAM.h
View File

@ -68,7 +68,15 @@ namespace visualSLAM {
/// get a point
Point3 point(Key j) const { return at<Point3>(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);
/**