i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added parameter "RGBD/OptimizeRobust" (default true) to use Vertigo robust graph optimization (only for g2o and GTSAM optimization strategies). Added GTSAM support.

Browse Source
This commit is contained in:
matlabbe
2015-09-02 17:51:43 -04:00
parent a2b9c9f9a0
commit 7a721105ae
40 changed files with 2125 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
corelib/include/rtabmap/core/Graph.h
View File

@@ -51,7 +51,8 @@ public:
kTypeUndef = -1,
kTypeTORO = 0,
kTypeG2O = 1,
kTypeCVSBA = 2
kTypeGTSAM = 2,
kTypeCVSBA = 3
};
static Optimizer * create(const ParametersMap & parameters);
static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap());
@@ -74,6 +75,7 @@ public:
bool isSlam2d() const {return slam2d_;}
bool isCovarianceIgnored() const {return covarianceIgnored_;}
double epsilon() const {return epsilon_;}
bool isRobust() const {return robust_;}
// inherited classes should implement one of these methods
virtual std::map<int, Transform> optimize(
@@ -94,7 +96,8 @@ protected:
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon());
double epsilon = Parameters::defaultRGBDOptimizeEpsilon(),
bool robust = Parameters::defaultRGBDOptimizeRobust());
Optimizer(const ParametersMap & parameters);
private:
@@ -102,6 +105,7 @@ private:
bool slam2d_;
bool covarianceIgnored_;
double epsilon_;
bool robust_;
};
class RTABMAP_EXP TOROOptimizer : public Optimizer
@@ -117,8 +121,12 @@ public:
std::multimap<int, Link> & edgeConstraints);
public:
TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
Optimizer(iterations, slam2d, covarianceIgnored) {}
TOROOptimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
TOROOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~TOROOptimizer() {}
@@ -136,10 +144,21 @@ class RTABMAP_EXP G2OOptimizer : public Optimizer
{
public:
static bool available();
static bool saveGraph(
const std::string & fileName,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
bool useRobustConstraints = false);
public:
G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) :
Optimizer(iterations, slam2d, covarianceIgnored) {}
G2OOptimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon(),
bool robust = Parameters::defaultRGBDOptimizeRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
G2OOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~G2OOptimizer() {}
@@ -153,6 +172,33 @@ public:
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
};
class RTABMAP_EXP GTSAMOptimizer : public Optimizer
{
public:
static bool available();
public:
GTSAMOptimizer(
int iterations = Parameters::defaultRGBDOptimizeIterations(),
bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(),
bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(),
double epsilon = Parameters::defaultRGBDOptimizeEpsilon(),
bool robust = Parameters::defaultRGBDOptimizeRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
GTSAMOptimizer(const ParametersMap & parameters) :
Optimizer(parameters) {}
virtual ~GTSAMOptimizer() {}
virtual Type type() const {return kTypeGTSAM;}
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
};
class RTABMAP_EXP CVSBAOptimizer : public Optimizer
{
public:

5
corelib/include/rtabmap/core/Parameters.h
View File

@@ -306,10 +306,11 @@ class RTABMAP_EXP Parameters
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(RGBD, OptimizeIterations, int, 10, "Optimization iterations.");
RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(RGBD, OptimizeRobust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies).");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");

2
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -65,7 +65,7 @@ public:
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO)
kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
kCmdCleanDataBuffer,
kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly
kCmdTriggerNewMap,

2
corelib/include/rtabmap/core/Transform.h
View File

@@ -51,6 +51,8 @@ public:
Transform(const cv::Mat & transformationMatrix);
// x,y,z, roll,pitch,yaw
Transform(float x, float y, float z, float roll, float pitch, float yaw);
// x,y, theta
Transform(float x, float y, float theta);
float r11() const {return data()[0];}
float r12() const {return data()[1];}