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

support adding odometry factor to visualSLAM graph and add NonlinearISAM to visualSLAM namespace for uses in MATLAB

release/4.3a0
Duy-Nguyen Ta 2012-06-06 09:31:18 +00:00
parent 0e4f2b414c
commit 5256130afd
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/slam/visualSLAM.cpp
View File

@ -16,7 +16,7 @@
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
namespace visualSLAM {
@ -103,5 +103,9 @@ namespace visualSLAM {
}
/* ************************************************************************* */
void Graph::addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model) {
push_back(boost::shared_ptr<BetweenFactor<Pose3> >(new BetweenFactor<Pose3>(x1, x2, odometry, model)));
}
/* ************************************************************************* */
}

16
gtsam/slam/visualSLAM.h
View File

@ -27,9 +27,9 @@
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/geometry/SimpleCamera.h>
namespace visualSLAM {
using namespace gtsam;
@ -173,6 +173,15 @@ namespace visualSLAM {
*/
void addRangeFactor(Key poseKey, Key pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1));
/**
* Add an odometry between two poses
* @param x1 variable key of the first camera pose
* @param x2 variable key of the second camera pose
* @param odometry measurement from x1 to x2 (x1.between(x2))
* @param model uncertainty model of this measurement
*/
void addOdometry(Key x1, Key x2, const Pose3& odometry, const SharedNoiseModel& model);
/**
* Optimize the graph
* @param initialEstimate initial estimate of all variables in the graph
@ -191,4 +200,9 @@ namespace visualSLAM {
}; // Graph
/**
* Non-linear ISAM for vanilla incremental visual SLAM inference
*/
typedef gtsam::NonlinearISAM<Graph> ISAM;
} // namespaces