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

Version 0.11.0: Refactored Visual/ICP transformation estimation approaches, Added Registration classes for convenience, Added Parameters migration approach, 3D laser scans can be used

Browse Source
This commit is contained in:
matlabbe
2015-11-22 18:08:32 -05:00
parent 1e5bcfded8
commit ae9f21acd7
46 changed files with 4934 additions and 4547 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/include/rtabmap/core/CameraRGB.h
View File

@@ -56,6 +56,16 @@ public:
bool isDepth = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
CameraImages(const std::string & scanPath,
const Transform & scanLocalTransform,
int scanMaxPts,
const std::string & path,
int startAt = 1,
bool refreshDir = false,
bool rectifyImages = false,
bool isDepth = false,
float imageRate = 0,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -79,6 +89,12 @@ private:
int _count;
UDirectory * _dir;
std::string _lastFileName;
int _countScan;
UDirectory * _scanDir;
std::string _lastScanFileName;
std::string _scanPath;
Transform _scanLocalTransform;
int _scanMaxPts;
std::string _cameraName;
CameraModel _model;

11
corelib/include/rtabmap/core/CameraStereo.h
View File

@@ -119,6 +119,17 @@ public:
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
CameraStereoImages(
const std::string & scanPath,
const Transform & scanLocalTransform,
int scanMaxPts,
const std::string & pathLeftImages,
const std::string & pathRightImages,
bool filenamesAreTimestamps = false,
const std::string & timestampsPath = "", // "times.txt"
bool rectifyImages = false,
float imageRate=0.0f,
const Transform & localTransform = Transform::getIdentity());
virtual ~CameraStereoImages();
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");

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

@@ -106,11 +106,13 @@ public:
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float minDepth,
float maxDepth);
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float minDepth,
float maxDepth);
static void filterKeypointsByDisparity(

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

@@ -52,6 +52,8 @@ class VWDictionary;
class VisualWord;
class Feature2D;
class Statistics;
class RegistrationVis;
class RegistrationIcp;
class RTABMAP_EXP Memory
{
@@ -176,21 +178,16 @@ public:
std::map<int, Transform> & poses,
std::multimap<int, Link> & links,
bool lookInDatabase = false);
float getBowInlierDistance() const {return _bowInlierDistance;}
int getBowIterations() const {return _bowIterations;}
int getBowMinInliers() const {return _bowMinInliers;}
bool getBowForce2D() const {return _bowForce2D;}
Transform computeVisualTransform(int oldId, int newId, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeVisualTransform(const Signature & oldS, const Signature & newS, std::string * rejectedMsg = 0, int * inliers = 0, double * variance = 0) const;
Transform computeIcpTransform(int oldId, int newId, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0);
Transform computeIcpTransform(const Signature & oldS, const Signature & newS, Transform guess, bool icp3D, std::string * rejectedMsg = 0, int * correspondences = 0, double * variance = 0, float * correspondencesRatio = 0) const;
Transform computeVisualTransform(int fromId, int toId, std::string * rejectedMsg = 0, int * inliers = 0, float * variance = 0);
Transform computeIcpTransform(int fromId, int toId, Transform guess, std::string * rejectedMsg = 0, int * correspondences = 0, float * variance = 0, float * correspondencesRatio = 0);
Transform computeScanMatchingTransform(
int newId,
int oldId,
const std::map<int, Transform> & poses,
std::string * rejectedMsg = 0,
int * inliers = 0,
double * variance = 0);
float * variance = 0);
private:
void preUpdate();
@@ -241,8 +238,9 @@ private:
bool _generateIds;
bool _badSignaturesIgnored;
int _imageDecimation;
float _laserScanVoxelSize;
float _laserScanDownsampleStepSize;
bool _localSpaceLinksKeptInWM;
bool _reextractLoopClosureFeatures;
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
bool _rehearsalWeightIgnoredWhileMoving;
@@ -268,34 +266,11 @@ private:
bool _tfIdfLikelihoodUsed;
bool _parallelized;
float _wordsMaxDepth; // 0=inf
float _wordsMinDepth;
std::vector<float> _roiRatios; // size 4
// RGBD-SLAM stuff
int _bowMinInliers;
float _bowInlierDistance;
int _bowIterations;
int _bowRefineIterations;
bool _bowForce2D;
float _bowEpipolarGeometryVar;
int _bowEstimationType;
double _bowPnPReprojError;
int _bowPnPFlags;
bool _bowVarianceFromInliersCount;
float _icpMaxTranslation;
float _icpMaxRotation;
int _icpDecimation;
float _icpMaxDepth;
float _icpVoxelSize;
int _icpSamples;
float _icpMaxCorrespondenceDistance;
int _icpMaxIterations;
float _icpCorrespondenceRatio;
bool _icpPointToPlane;
int _icpPointToPlaneNormalNeighbors;
float _icp2MaxCorrespondenceDistance;
int _icp2MaxIterations;
float _icp2CorrespondenceRatio;
float _icp2VoxelSize;
RegistrationVis * _registrationVis;
RegistrationIcp * _registrationIcp;
// Stereo stuff
int _stereoFlowWinSize;

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

@@ -199,14 +199,15 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1) when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, "If > 0.0, voxelize laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1, "If > 1, downsample the laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true, "If local space links are kept in WM.");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM_COND(Kp, NNStrategy, int, RTABMAP_NONFREE, 1, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Kp, IncrementalDictionary, bool, true, "");
RTABMAP_PARAM(Kp, IncrementalFlann, bool, true, "When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary doubles in size).");
RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf)");
RTABMAP_PARAM(Kp, MaxDepth, float, 0.0, "Filter extracted keypoints by depth (0=inf).");
RTABMAP_PARAM(Kp, MinDepth, float, 0.0, "Filter extracted keypoints by depth.");
RTABMAP_PARAM(Kp, WordsPerImage, int, 400, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
RTABMAP_PARAM_COND(Kp, NndrRatio, float, RTABMAP_NONFREE, 0.8, 0.9, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
@@ -216,10 +217,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM_STR(Kp, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM_STR(Kp, DictionaryPath, "", "Path of the pre-computed dictionary");
RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true, "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
RTABMAP_PARAM(Kp, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
RTABMAP_PARAM(Kp, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Kp, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
RTABMAP_PARAM(Kp, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
RTABMAP_PARAM(Kp, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Kp, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
//Database
RTABMAP_PARAM(DbSqlite3, InMemory, bool, false, "Using database in the memory instead of a file on the hard disk.");
@@ -229,7 +229,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(DbSqlite3, TempStore, int, 2, "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
// Keypoints descriptors/detectors
RTABMAP_PARAM(SURF, Extended, bool, false, "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
RTABMAP_PARAM(SURF, Extended, bool, false, "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
RTABMAP_PARAM(SURF, HessianThreshold, float, 500.0, "Threshold for hessian keypoint detector used in SURF.");
RTABMAP_PARAM(SURF, Octaves, int, 4, "Number of pyramid octaves the keypoint detector will use.");
RTABMAP_PARAM(SURF, OctaveLayers, int, 2, "Number of octave layers within each octave.");
@@ -286,7 +286,6 @@ class RTABMAP_EXP Parameters
// RGB-D SLAM
RTABMAP_PARAM(RGBD, Enabled, bool, true, "");
RTABMAP_PARAM(RGBD, PoseScanMatching, bool, false, "Laser scan matching for odometry pose correction (laser scans are required).");
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.1, "Minimum linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.1, "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
@@ -300,18 +299,22 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data.");
RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true, "Save scan matching IDs in link's user data.");
RTABMAP_PARAM(RGBD, IcpOdomRefining, bool, false, "If the newest added node's pose is refined using ICP with the previous node to correct odometry from pose to pose (laser scans required!).");
RTABMAP_PARAM(RGBD, IcpLoopClosureRefining, bool, false, "If the estimated loop closure transformation is refined using ICP (laser scans required!).");
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, true, "Extract features even if there are some already in the nodes.");
// Local loop closure detection
// Local/Proximity loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, true, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathScansMerged, bool, true, "Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 0.5, "Path filtering radius.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
// 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, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "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.");
@@ -319,23 +322,10 @@ class RTABMAP_EXP Parameters
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");
RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Odom, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP)");
RTABMAP_PARAM(Odom, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Odom, InlierDistance, float, 0.1, "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(Odom, RefineIterations, int, 5, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Odom, MaxDepth, float, 0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Odom, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
RTABMAP_PARAM(Odom, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM(Odom, PnPReprojError, double, 5.0, "PnP reprojection error.");
RTABMAP_PARAM(Odom, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Odom, ParticleFiltering, bool, false, "Particle filtering to smooth the odometry trajectory.");
RTABMAP_PARAM(Odom, ParticleSize, unsigned int, 400, "Number of particles of the filter.");
RTABMAP_PARAM(Odom, ParticleNoiseT, float, 0.002, "Noise (m) of translation components (x,y,z).");
@@ -344,17 +334,14 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw).");
// Odometry Bag-of-words
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM(OdomBow, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(OdomBow, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM_STR(OdomBow, FixedLocalMapPath, "", "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.")
// Odometry Mono
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");
RTABMAP_PARAM(OdomMono, InitMinTranslation, float, 0.1, "Minimum translation required for the initialization step.");
RTABMAP_PARAM(OdomMono, MinTranslation, float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map.");
RTABMAP_PARAM(OdomMono, MinTranslation, float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map.");
// Odometry common stuff between BOW and Optical Flow approaches
RTABMAP_PARAM(OdomFlow, WinSize, int, 16, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
@@ -362,49 +349,45 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomFlow, Eps, double, 0.01, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, MaxLevel, int, 3, "Used for optical flow approach. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomSubPix, WinSize, int, 3, "Can be used with BOW and optical flow approaches. See cv::cornerSubPix().");
RTABMAP_PARAM(OdomSubPix, Iterations, int, 0, "Can be used with BOW and optical flow approaches. See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(OdomSubPix, Eps, double, 0.02, "Can be used with BOW and optical flow approaches. See cv::cornerSubPix().");
// Common registration parameters
RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
// Visual registration parameters
RTABMAP_PARAM(Vis, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
RTABMAP_PARAM(Vis, RefineIterations, int, 10, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Vis, PnPReprojError, double, 5.0, "[Vis/EstimationType = 1] PnP reprojection error.");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
RTABMAP_PARAM(Vis, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
RTABMAP_PARAM(Vis, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
RTABMAP_PARAM(Vis, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Vis, MaxDepth, float, 0.0, "Max depth of the features (0 means no limit).");
RTABMAP_PARAM(Vis, MinDepth, float, 0.0, "Min depth of the features (0 means no limit).");
RTABMAP_PARAM_STR(Vis, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Vis, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
RTABMAP_PARAM(Vis, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(Vis, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
RTABMAP_PARAM(LccIcp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
RTABMAP_PARAM(LccIcp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(LccBow, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(LccBow, MinInliers, int, 10, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.1, "Maximum distance for visual word correspondences. Used by 3D->3D estimation approach.");
RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(LccBow, RefineIterations, int, 10, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
RTABMAP_PARAM(LccBow, PnPReprojError, double, 5.0, "PnP reprojection error.");
RTABMAP_PARAM(LccBow, PnPFlags, int, 1, "PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(LccBow, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
RTABMAP_PARAM(LccReextract, FeatureType, int, 4, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(LccReextract, MaxWords, int, 1000, "0 no limits.");
RTABMAP_PARAM(LccReextract, MaxDepth, float, 0.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(LccIcp3, Decimation, int, 4, "Depth image decimation.");
RTABMAP_PARAM(LccIcp3, MaxDepth, float, 3.0, "Max cloud depth.");
RTABMAP_PARAM(LccIcp3, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp3, Samples, int, 0, "Random samples to be used for ICP computation. Not used if voxelSize is set.");
RTABMAP_PARAM(LccIcp3, MaxCorrespondenceDistance, float, 0.05, "ICP 3D: Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp3, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp3, CorrespondenceRatio, float, 0.2, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp3, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(LccIcp3, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.025, "Voxel size to be used for ICP computation.");
// ICP registration parameters
RTABMAP_PARAM(Icp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
RTABMAP_PARAM(Icp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(Icp, 2D, bool, true, "If 2D ICP is done (only 3Dof -> x,y,yaw).");
RTABMAP_PARAM(Icp, VoxelSize, float, 0.025, "Uniform sampling voxel size (0=disabled).");
RTABMAP_PARAM(Icp, DownsamplingStep, int, 1, "Downsampling step size (1=no sampling). This is done before uniform sampling.");
RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05, "Max distance for point correspondences.");
RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.3, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20, "Number of neighbors to compute normals for point to plane.");
// Stereo disparity
RTABMAP_PARAM(Stereo, WinSize, int, 16, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, WinSize, int, 16, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, Iterations, int, 30, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, Eps, double, 0.01, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(Stereo, MaxLevel, int, 3, "See cv::calcOpticalFlowPyrLK().");
@@ -436,6 +419,17 @@ public:
static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
static std::string getDefaultDatabaseName();
/**
* Get removed parameters (backward compatibility)
* <OldKeyName, <isEqual, NewKeyName> >, when isEqual=true, the old value can be safely copied to new parameter
*/
static const std::map<std::string, std::pair<bool, std::string> > & getRemovedParameters();
/**
* <NewKeyName, OldKeyName>
*/
static const ParametersMap & getBackwardCompatibilityMap();
private:
Parameters();
@@ -445,6 +439,9 @@ private:
static ParametersMap parameters_;
static ParametersMap descriptions_;
static Parameters instance_;
static std::map<std::string, std::pair<bool, std::string> > removedParameters_;
static ParametersMap backwardCompatibilityMap_;
};
}

66
corelib/include/rtabmap/core/Registration.h Normal file
View File

@@ -0,0 +1,66 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef REGISTRATION_H_
#define REGISTRATION_H_
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Signature.h>
namespace rtabmap {
class Registration
{
public:
virtual ~Registration() {}
virtual void parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _bowVarianceFromInliersCount);
}
virtual Transform computeTransformation(
const Signature & from,
const Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
int * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0) = 0;
protected:
Registration(const ParametersMap & parameters = ParametersMap()) :
_bowVarianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount())
{
this->parseParameters(parameters);
}
protected:
bool _bowVarianceFromInliersCount;
};
}
#endif /* REGISTRATION_H_ */

78
corelib/include/rtabmap/core/RegistrationIcp.h Normal file
View File

@@ -0,0 +1,78 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef REGISTRATIONICP_H_
#define REGISTRATIONICP_H_
#include <rtabmap/core/Registration.h>
#include <rtabmap/core/Signature.h>
namespace rtabmap {
// Geometrical registration
class RegistrationIcp : public Registration
{
public:
RegistrationIcp(const ParametersMap & parameters = ParametersMap());
virtual ~RegistrationIcp() {}
virtual void parseParameters(const ParametersMap & parameters);
virtual Transform computeTransformation(
const Signature & from,
const Signature & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
int * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0);
Transform computeTransformation(
const SensorData & from,
const SensorData & to,
Transform guess = Transform::getIdentity(),
std::string * rejectedMsg = 0,
int * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0);
private:
float _icpMaxTranslation;
float _icpMaxRotation;
bool _icp2D;
float _icpVoxelSize;
int _icpDownsamplingStep;
float _icpMaxCorrespondenceDistance;
int _icpMaxIterations;
float _icpCorrespondenceRatio;
bool _icpPointToPlane;
int _icpPointToPlaneNormalNeighbors;
};
}
#endif /* REGISTRATIONICP_H_ */

86
corelib/include/rtabmap/core/RegistrationVis.h Normal file
View File

@@ -0,0 +1,86 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef REGISTRATIONVIS_H_
#define REGISTRATIONVIS_H_
#include <rtabmap/core/Registration.h>
#include <rtabmap/core/Signature.h>
namespace rtabmap {
// Visual registration
class RegistrationVis : public Registration
{
public:
RegistrationVis(const ParametersMap & parameters = ParametersMap());
virtual ~RegistrationVis() {}
virtual void parseParameters(const ParametersMap & parameters);
virtual Transform computeTransformation(
const Signature & from,
const Signature & to,
Transform guess = Transform::getIdentity(), // guess is ignored for RegistrationVis
std::string * rejectedMsg = 0,
int * inliersOut = 0,
float * varianceOut = 0,
float * inliersRatioOut = 0);
float getBowInlierDistance() const {return _bowInlierDistance;}
int getBowIterations() const {return _bowIterations;}
int getBowMinInliers() const {return _bowMinInliers;}
bool getBowForce2D() const {return _bowForce2D;}
private:
int _bowMinInliers;
float _bowInlierDistance;
int _bowIterations;
int _bowRefineIterations;
bool _bowForce2D;
float _bowEpipolarGeometryVar;
int _bowEstimationType;
double _bowPnPReprojError;
int _bowPnPFlags;
int _reextractNNType;
float _reextractNNDR;
int _reextractFeatureType;
int _reextractMaxWords;
float _reextractMaxDepth;
float _reextractMinDepth;
std::string _reextractRoiRatios;
int _subPixWinSize;
int _subPixIterations;
double _subPixEps;
};
}
#endif /* REGISTRATION_H_ */

11
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -184,8 +184,8 @@ private:
float _rgbdLinearUpdate;
float _rgbdAngularUpdate;
float _newMapOdomChangeDistance;
int _globalLoopClosureIcpType;
bool _poseScanMatching;
bool _loopClosureIcpRefining;
bool _odomIcpRefining;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;
bool _scanMatchingIdsSavedInLinks;
@@ -194,15 +194,10 @@ private:
int _localDetectMaxGraphDepth;
float _localPathFilteringRadius;
bool _localPathOdomPosesUsed;
bool _localPathScansMerged;
std::string _databasePath;
bool _optimizeFromGraphEnd;
float _optimizationMaxLinearError;
bool _reextractLoopClosureFeatures;
int _reextractNNType;
float _reextractNNDR;
int _reextractFeatureType;
int _reextractMaxWords;
float _reextractMaxDepth;
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _goalsSavedInUserData;

3
corelib/include/rtabmap/core/Statistics.h
View File

@@ -74,6 +74,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(OdomCorrection, Inliers,);
RTABMAP_STATS(OdomCorrection, Inliers_ratio,);
RTABMAP_STATS(OdomCorrection, Variance,);
RTABMAP_STATS(OdomCorrection, Pts,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);
@@ -91,7 +92,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Distance_travelled, m);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Scan_matching, ms);
RTABMAP_STATS(Timing, Odom_correction, ms);
RTABMAP_STATS(Timing, Local_detection_TIME, ms);
RTABMAP_STATS(Timing, Local_detection_SPACE, ms);
RTABMAP_STATS(Timing, Cleaning_neighbors, ms);

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

@@ -124,6 +124,7 @@ public:
static Transform fromEigen3d(const Eigen::Affine3d & matrix);
static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
static Transform fromString(const std::string & string);
private:
cv::Mat data_;

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

@@ -127,12 +127,8 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
float maxDepth = 0,
const Transform & localTransform = Transform::getIdentity());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform = Transform::getIdentity());
cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
const cv::Point2f & pt,
@@ -180,6 +176,8 @@ void RTABMAP_EXP savePCDWords(
const std::multimap<int, pcl::PointXYZ> & words,
const Transform & transform = Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
} // namespace util3d
} // namespace rtabmap

33
corelib/include/rtabmap/core/util3d_filtering.h
View File

@@ -41,6 +41,16 @@ namespace rtabmap
namespace util3d
{
cv::Mat RTABMAP_EXP downsample(
const cv::Mat & cloud,
int step);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP downsample(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int step);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int step);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
@@ -51,11 +61,30 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize);
inline pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
{
return voxelize(cloud, voxelSize);
}
inline pcl::PointCloud<pcl::PointXYZRGB>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize)
{
return voxelize(cloud, voxelSize);
}
inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize)
{
return voxelize(cloud, voxelSize);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP randomSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int samples);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int samples);