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 Bundle Adjustment options to OdometryF2M (OdomF2M/BundleAdjustment) and RegistrationVis (Vis/BundleAdjustment). Added logger thread filter. RegistrationVis: Copy back corresponding input word IDs to output words when available. Memory::computeTransform() added option to use already computed corespondences if possible (proximity detection by time uses this). Updated Optimizer::optimizeBA() interface.

Browse Source
This commit is contained in:
matlabbe
2016-11-14 19:54:31 -05:00
parent 9fbd02b06d
commit 9276607920
39 changed files with 2214 additions and 594 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -87,6 +87,7 @@ public:
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
private:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();

4
corelib/include/rtabmap/core/Memory.h
View File

@@ -193,8 +193,8 @@ public:
std::multimap<int, Link> & links,
bool lookInDatabase = false);
Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
Transform computeIcpTransformMulti(
int newId,

10
corelib/include/rtabmap/core/OdometryF2M.h
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <rtabmap/core/Link.h>
namespace rtabmap {
@@ -62,11 +63,20 @@ private:
int scanMaximumMapSize_;
float scanSubtractRadius_;
std::string fixedMapPath_;
int bundleAdjustment_;
int bundleAdjustmentMaxFrames_;
Registration * regPipeline_;
Signature * map_;
Signature * lastFrame_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
std::map<int, std::map<int, cv::Point2f> > bundleWordReferences_;
std::map<int, Transform> bundlePoses_;
std::multimap<int, Link> bundleLinks_;
std::map<int, CameraModel> bundleModels_;
std::map<int, int> bundlePoseReferences_;
ParametersMap bundleParameters_;
};
}

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

@@ -48,12 +48,13 @@ protected:
virtual void handleEvent(UEvent * event);
private:
void mainLoopKill();
virtual void mainLoopBegin();
virtual void mainLoopKill();
//============================================================
// MAIN LOOP
//============================================================
void mainLoop();
virtual void mainLoop();
void addData(const SensorData & data);
bool getData(SensorData & data);

18
corelib/include/rtabmap/core/Optimizer.h
View File

@@ -83,6 +83,8 @@ public:
void setEpsilon(double epsilon) {epsilon_ = epsilon;}
void setRobust(bool enabled) {robust_ = enabled;}
virtual void parseParameters(const ParametersMap & parameters);
// inherited classes should implement one of these methods
virtual std::map<int, Transform> optimize(
int rootId,
@@ -92,12 +94,24 @@ public:
double * finalError = 0,
int * iterationsDone = 0);
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures);
virtual void parseParameters(const ParametersMap & parameters);
Transform optimizeBA(
const Link & link,
const CameraModel & model,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences);
void computeBACorrespondences(
const std::map<int, Transform> & poses,
@@ -109,7 +123,7 @@ public:
protected:
Optimizer(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust());

6
corelib/include/rtabmap/core/OptimizerCVSBA.h
View File

@@ -42,7 +42,7 @@ public:
public:
OptimizerCVSBA(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
Optimizer(iterations, slam2d, covarianceIgnored) {}
OptimizerCVSBA(const ParametersMap & parameters) :
@@ -55,7 +55,9 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures);
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
};
} /* namespace rtabmap */

4
corelib/include/rtabmap/core/OptimizerG2O.h
View File

@@ -73,7 +73,9 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures);
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>
private:
int solver_;

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

@@ -42,7 +42,7 @@ public:
public:
OptimizerGTSAM(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust()) :

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

@@ -52,7 +52,7 @@ public:
public:
OptimizerTORO(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultOptimizerSlam2D(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}

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

@@ -336,22 +336,23 @@ class RTABMAP_EXP Parameters
// Graph optimization
#ifdef RTABMAP_GTSAM
RTABMAP_PARAM(Optimizer, Strategy, int, 2, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(Optimizer, Iterations, int, 10, "Optimization iterations.");
#else
#ifdef RTABMAP_G2O
RTABMAP_PARAM(Optimizer, Strategy, int, 1, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
RTABMAP_PARAM(Optimizer, Iterations, int, 10, "Optimization iterations.");
#else
RTABMAP_PARAM(Optimizer, Strategy, int, 0, "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
#endif
#endif
RTABMAP_PARAM(Optimizer, Iterations, int, 100, "Optimization iterations.");
RTABMAP_PARAM(Optimizer, Slam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
#endif
#endif
RTABMAP_PARAM(Optimizer, VarianceIgnored, 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(Optimizer, Epsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(Optimizer, Epsilon, double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod");
RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
RTABMAP_PARAM(g2o, PixelVariance, double, 1, "Pixel variance used for SBA.");
RTABMAP_PARAM(g2o, PixelVariance, double, 1.0, "Pixel variance used for bundle adjustment.");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -379,6 +380,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size.");
RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
RTABMAP_PARAM_STR(OdomF2M, FixedMapPath, "", "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
// Odometry Mono
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");
@@ -428,6 +431,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
// ICP registration parameters
RTABMAP_PARAM(Icp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");

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

@@ -81,8 +81,10 @@ private:
int _flowMaxLevel;
float _nndr;
int _guessWinSize;
int _bundleAdjustment;
ParametersMap _featureParameters;
ParametersMap _bundleParameters;
};
}