finished wrapping visualSLAM namespace

release/4.3a0
Chris Beall 2012-06-04 02:41:14 +00:00
parent 877e9d4045
commit ae78b89c6f
4 changed files with 92 additions and 6 deletions

View File

@ -78,7 +78,7 @@ void readAllDataISAM() {
/**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
@ -99,7 +99,7 @@ void createNewFactors(shared_ptr<visualSLAM::Graph>& newFactors, boost::shared_p
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<Values> (new Values());
initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
initialValues->insert(X(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(L(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);

74
gtsam.h
View File

@ -81,6 +81,18 @@ class Point2 {
gtsam::Point2 retract(Vector v);
};
class StereoPoint2 {
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
static gtsam::StereoPoint2 Expmap(Vector v);
static Vector Logmap(const gtsam::StereoPoint2& p);
void print(string s) const;
Vector localCoordinates(const gtsam::StereoPoint2& p);
gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2);
gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2);
gtsam::StereoPoint2 retract(Vector v);
};
class Point3 {
Point3();
Point3(double x, double y, double z);
@ -207,6 +219,20 @@ class Pose3 {
gtsam::Rot3 rotation() const;
};
class Cal3_S2 {
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
void print(string s) const;
};
class Cal3_S2Stereo {
Cal3_S2Stereo();
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
void print(string s) const;
};
class CalibratedCamera {
CalibratedCamera();
@ -635,3 +661,51 @@ class Graph {
// TODO: add factors, etc.
}///\namespace simulated2DOriented
//*************************************************************************
// VisualSLAM
//*************************************************************************
#include <gtsam/slam/visualSLAM.h>
namespace visualSLAM {
class Values {
Values();
void insertPose(size_t key, const gtsam::Pose3& pose);
void insertPoint(size_t key, const gtsam::Point3& pose);
size_t size() const;
void print(string s) const;
gtsam::Pose3 pose(size_t i);
gtsam::Point3 point(size_t j);
};
class Graph {
Graph();
void print(string s) const;
double error(const pose2SLAM::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const pose2SLAM::Values& values) const;
gtsam::GaussianFactorGraph* linearize(const pose2SLAM::Values& values,
const gtsam::Ordering& ordering) const;
// Measurements
void addMeasurement(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2* K);
void addStereoMeasurement(const gtsam::StereoPoint2& measured, const gtsam::SharedNoiseModel& model,
size_t poseKey, size_t pointKey, const gtsam::Cal3_S2Stereo* K);
// Constraints
void addPoseConstraint(size_t poseKey, const gtsam::Pose3& p);
void addPointConstraint(size_t pointKey, const gtsam::Point3& p);
// Priors
void addPosePrior(size_t poseKey, const gtsam::Pose3& p, const gtsam::SharedNoiseModel& model);
void addPointPrior(size_t pointKey, const gtsam::Point3& p, const gtsam::SharedNoiseModel& model);
void addRangeFactor(size_t poseKey, size_t pointKey, double range, const gtsam::SharedNoiseModel& model);
visualSLAM::Values optimize(const visualSLAM::Values& initialEstimate) const;
gtsam::Marginals marginals(const visualSLAM::Values& solution) const;
};
}///\namespace visualSLAM

View File

@ -22,14 +22,14 @@ namespace visualSLAM {
/* ************************************************************************* */
void Graph::addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK& K) {
Key poseKey, Key pointKey, const shared_ptrK K) {
boost::shared_ptr<ProjectionFactor> factor(new ProjectionFactor(measured, model, poseKey, pointKey, K));
push_back(factor);
}
/* ************************************************************************* */
void Graph::addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo& K) {
Key poseKey, Key pointKey, const shared_ptrKStereo K) {
boost::shared_ptr<StereoFactor> factor(new StereoFactor(measured, model, poseKey, pointKey, K));
push_back(factor);
}

View File

@ -26,6 +26,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/geometry/SimpleCamera.h>
@ -62,9 +63,15 @@ namespace visualSLAM {
/// insert a pose
void insertPose(Key i, const Pose3& pose) { insert(i, pose); }
/// insert a point
void insertPoint(Key j, const Point3& point) { insert(j, point); }
/// get a pose
Pose3 pose(Key i) const { return at<Pose3>(i); }
/// get a point
Point3 point(Key j) const { return at<Point3>(j); }
};
/**
@ -99,7 +106,7 @@ namespace visualSLAM {
* @param K shared pointer to calibration object
*/
void addMeasurement(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrK& K);
Key poseKey, Key pointKey, const shared_ptrK K);
/**
* Add a stereo factor measurement
@ -110,7 +117,7 @@ namespace visualSLAM {
* @param K shared pointer to stereo calibration object
*/
void addStereoMeasurement(const StereoPoint2& measured, const SharedNoiseModel& model,
Key poseKey, Key pointKey, const shared_ptrKStereo& K);
Key poseKey, Key pointKey, const shared_ptrKStereo K);
/**
* Add a constraint on a pose (for now, *must* be satisfied in any Values)
@ -162,6 +169,11 @@ namespace visualSLAM {
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
}
/// Return a Marginals object
Marginals marginals(const Values& solution) const {
return Marginals(*this,solution);
}
}; // Graph
} // namespaces