i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/include/rtabmap/core/CameraRGB.h
+16
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;
corelib/include/rtabmap/core/CameraStereo.h
+11
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 = "");
corelib/include/rtabmap/core/Features2d.h
+2
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(
corelib/include/rtabmap/core/Memory.h
+11 -36
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;
corelib/include/rtabmap/core/Parameters.h
+67 -70
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_;
};
}
corelib/include/rtabmap/core/Registration.h
+66
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_ */
corelib/include/rtabmap/core/RegistrationIcp.h
+78
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_ */
corelib/include/rtabmap/core/RegistrationVis.h
+86
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_ */
corelib/include/rtabmap/core/Rtabmap.h
+3 -8
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;
corelib/include/rtabmap/core/Statistics.h
+2 -1
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);
corelib/include/rtabmap/core/Transform.h
+1
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_;
corelib/include/rtabmap/core/util3d.h
+4 -6
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
corelib/include/rtabmap/core/util3d_filtering.h
+31 -2
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);
corelib/src/CMakeLists.txt
+3
View File
@@ -44,6 +44,9 @@ SET(SRC_FILES
Compression.cpp
Link.cpp
RegistrationIcp.cpp
RegistrationVis.cpp
Odometry.cpp
OdometryThread.cpp
OdometryBOW.cpp
corelib/src/CameraRGB.cpp
+138 -4
View File
@@ -38,6 +38,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/imgproc/imgproc.hpp>
#include <rtabmap/core/util3d.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <cmath>
@@ -61,7 +64,36 @@ CameraImages::CameraImages(const std::string & path,
_rectifyImages(rectifyImages),
_isDepth(isDepth),
_count(0),
_dir(0)
_dir(0),
_countScan(0),
_scanDir(0)
{
}
CameraImages::CameraImages(const std::string & scanPath,
const Transform & scanLocalTransform,
int scanMaxPts,
const std::string & path,
int startAt,
bool refreshDir,
bool rectifyImages,
bool isDepth,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
_path(path),
_startAt(startAt),
_refreshDir(refreshDir),
_rectifyImages(rectifyImages),
_isDepth(isDepth),
_count(0),
_dir(0),
_countScan(0),
_scanDir(0),
_scanPath(scanPath),
_scanLocalTransform(scanLocalTransform),
_scanMaxPts(scanMaxPts)
{
}
@@ -72,11 +104,19 @@ CameraImages::~CameraImages(void)
{
delete _dir;
}
if(_scanDir)
{
delete _scanDir;
}
}
bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
_cameraName = cameraName;
_lastFileName.clear();
_lastScanFileName.clear();
_count = 0;
_countScan = 0;
UDEBUG("");
if(_dir)
@@ -87,7 +127,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
{
_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff");
}
_count = 0;
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
@@ -105,6 +144,47 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
}
// check for scan directory
if(_scanDir)
{
delete _scanDir;
_scanDir = 0;
}
if(!_scanPath.empty())
{
_scanDir = new UDirectory(_scanPath, "pcd bin"); // "bin" is for KITTI format
if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
{
_scanPath.append("/");
}
if(!_scanDir->isValid())
{
UERROR("Scan directory path is not valid \"%s\"", _scanPath.c_str());
delete _scanDir;
_scanDir = 0;
}
else if(_scanDir->getFileNames().size() == 0)
{
UWARN("Scan directory is empty \"%s\"", _scanPath.c_str());
delete _scanDir;
_scanDir = 0;
}
else if(_scanDir->getFileNames().size() != _dir->getFileNames().size())
{
UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
_scanPath.c_str(),
(int)_scanDir->getFileNames().size(),
_path.c_str(),
(int)_dir->getFileNames().size());
delete _scanDir;
_scanDir = 0;
}
else
{
UINFO("path=%s scans=%d", _scanPath.c_str(), (int)this->imagesCount());
}
}
// look for calibration files
if(!calibrationFolder.empty() && !cameraName.empty())
{
@@ -164,12 +244,17 @@ std::vector<std::string> CameraImages::filenames() const
SensorData CameraImages::captureImage()
{
cv::Mat img;
cv::Mat scan;
UDEBUG("");
if(_dir->isValid())
{
if(_refreshDir)
{
_dir->update();
if(_scanDir)
{
_scanDir->update();
}
}
if(_startAt == 0)
{
@@ -183,6 +268,28 @@ SensorData CameraImages::captureImage()
img = cv::imread(fullPath.c_str());
}
}
if(_scanDir)
{
const std::list<std::string> & scanFileNames = _scanDir->getFileNames();
if(scanFileNames.size())
{
if(_lastScanFileName.empty() || uStrNumCmp(_lastScanFileName,*scanFileNames.rbegin()) < 0)
{
_lastScanFileName = *scanFileNames.rbegin();
std::string fullPath = _scanPath + _lastScanFileName;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(UFile::getExtension(_lastScanFileName).compare("bin") == 0)
{
cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
}
else
{
pcl::io::loadPCDFile(fullPath, *cloud);
}
scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
}
}
}
}
else
{
@@ -242,6 +349,33 @@ SensorData CameraImages::captureImage()
}
}
}
if(_scanDir)
{
fileName = _scanDir->getNextFileName();
if(fileName.size())
{
fullPath = _scanPath + fileName;
while(++_countScan < _startAt && (fileName = _scanDir->getNextFileName()).size())
{
fullPath = _scanPath + fileName;
}
if(fileName.size())
{
UDEBUG("Loading scan : %s", fullPath.c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(UFile::getExtension(fileName).compare("bin") == 0)
{
cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
}
else
{
pcl::io::loadPCDFile(fullPath, *cloud);
}
scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
}
}
}
}
if(!img.empty() && _model.isValid() && _rectifyImages)
@@ -256,9 +390,9 @@ SensorData CameraImages::captureImage()
if(_isDepth)
{
return SensorData(cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, cv::Mat(), img, _model, this->getNextSeqID(), UTimer::now());
}
return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
return SensorData(scan, scan.empty()?0:_scanMaxPts, 0, img, cv::Mat(), _model, this->getNextSeqID(), UTimer::now());
}
corelib/src/CameraStereo.cpp
+25 -1
View File
@@ -779,6 +779,28 @@ CameraStereoImages::CameraStereoImages(
}
}
CameraStereoImages::CameraStereoImages(
const std::string & scanPath,
const Transform & scanLocalTransform,
int scanMaxPts,
const std::string & pathLeftImages,
const std::string & pathRightImages,
bool filenamesAreTimestamps,
const std::string & timestampsPath,
bool rectifyImages,
float imageRate,
const Transform & localTransform) :
Camera(imageRate, localTransform),
camera_(0),
camera2_(0),
filenamesAreTimestamps_(filenamesAreTimestamps),
timestampsPath_(timestampsPath),
rectifyImages_(rectifyImages)
{
camera_ = new CameraImages(scanPath, scanLocalTransform, scanMaxPts, pathLeftImages);
camera2_ = new CameraImages(pathRightImages);
}
CameraStereoImages::~CameraStereoImages()
{
if(camera_)
@@ -811,6 +833,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
stereoModel_.baseline());
}
}
stereoModel_.setLocalTransform(this->getLocalTransform());
if(rectifyImages_ && !stereoModel_.isValid())
{
@@ -968,7 +991,7 @@ SensorData CameraStereoImages::captureImage()
leftImage = stereoModel_.left().rectifyImage(leftImage);
rightImage = stereoModel_.right().rectifyImage(rightImage);
}
data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, this->getNextSeqID(), stamp);
}
}
}
@@ -1034,6 +1057,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
stereoModel_.baseline());
}
}
stereoModel_.setLocalTransform(this->getLocalTransform());
if(rectifyImages_ && !stereoModel_.isValid())
{
corelib/src/Compression.cpp
-5
View File
@@ -231,11 +231,6 @@ cv::Mat uncompressData(const unsigned char * bytes, unsigned long size)
int width = *((int*)&bytes[size-2*sizeof(int)]);
int type = *((int*)&bytes[size-1*sizeof(int)]);
// If the size is higher, it may be a wrong data format.
UASSERT_MSG(height>=0 && height<10000 &&
width>=0 && width<10000,
uFormat("size=%d, height=%d width=%d type=%d", size, height, width, type).c_str());
data = cv::Mat(height, width, type);
uLongf totalUncompressed = uLongf(data.total())*uLongf(data.elemSize());
corelib/src/Features2d.cpp
+6 -2
View File
@@ -60,18 +60,22 @@ namespace rtabmap {
void Feature2D::filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float minDepth,
float maxDepth)
{
cv::Mat descriptors;
filterKeypointsByDepth(keypoints, descriptors, depth, maxDepth);
filterKeypointsByDepth(keypoints, descriptors, depth, minDepth, maxDepth);
}
void Feature2D::filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float minDepth,
float maxDepth)
{
UASSERT(minDepth >= 0.0f);
UASSERT(maxDepth <= 0.0f || maxDepth > minDepth);
if(!depth.empty() && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
{
std::vector<cv::KeyPoint> output(keypoints.size());
@@ -85,7 +89,7 @@ void Feature2D::filterKeypointsByDepth(
if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
{
float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
if(uIsFinite(d) && d>0.0f && (maxDepth <= 0.0f || d < maxDepth))
if(uIsFinite(d) && d>minDepth && (maxDepth <= 0.0f || d < maxDepth))
{
output[oi++] = keypoints[i];
indexes[i] = 1;
corelib/src/Memory.cpp
+131 -880
View File
File diff suppressed because it is too large Load Diff
corelib/src/Odometry.cpp
+22 -22
View File
@@ -35,14 +35,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_roiRatios(Parameters::defaultOdomRoiRatios()),
_minInliers(Parameters::defaultOdomMinInliers()),
_inlierDistance(Parameters::defaultOdomInlierDistance()),
_iterations(Parameters::defaultOdomIterations()),
_refineIterations(Parameters::defaultOdomRefineIterations()),
_maxDepth(Parameters::defaultOdomMaxDepth()),
_roiRatios(Parameters::defaultVisRoiRatios()),
_minInliers(Parameters::defaultVisMinInliers()),
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultOdomForce2D()),
_force2D(Parameters::defaultVisForce2D()),
_holonomic(Parameters::defaultOdomHolonomic()),
_particleFiltering(Parameters::defaultOdomParticleFiltering()),
_particleSize(Parameters::defaultOdomParticleSize()),
@@ -51,30 +51,30 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
_fillInfoData(Parameters::defaultOdomFillInfoData()),
_estimationType(Parameters::defaultOdomEstimationType()),
_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
_pnpFlags(Parameters::defaultOdomPnPFlags()),
_varianceFromInliersCount(Parameters::defaultOdomVarianceFromInliersCount()),
_estimationType(Parameters::defaultVisEstimationType()),
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_resetCurrentCount(0),
previousStamp_(0),
previousTransform_(Transform::getIdentity()),
distanceTravelled_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kOdomForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
Parameters::parse(parameters, Parameters::kOdomEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kOdomVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
corelib/src/OdometryBOW.cpp
+14 -14
View File
@@ -65,26 +65,26 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// Memory's stereo parameters, copy from Odometry
int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
int subPixIterations = Parameters::defaultOdomSubPixIterations();
double subPixEps = Parameters::defaultOdomSubPixEps();
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
double subPixEps = Parameters::defaultVisSubPixEps();
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
corelib/src/OdometryMono.cpp
+14 -14
View File
@@ -94,25 +94,25 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
int subPixIterations = Parameters::defaultOdomSubPixIterations();
double subPixEps = Parameters::defaultOdomSubPixEps();
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
double subPixEps = Parameters::defaultVisSubPixEps();
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
corelib/src/OdometryOpticalFlow.cpp
+10 -10
View File
@@ -54,9 +54,9 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
stereoEps_(Parameters::defaultStereoEps()),
stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
subPixIterations_(Parameters::defaultOdomSubPixIterations()),
subPixEps_(Parameters::defaultOdomSubPixEps()),
subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
subPixIterations_(Parameters::defaultVisSubPixIterations()),
subPixEps_(Parameters::defaultVisSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
{
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
@@ -68,20 +68,20 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps_);
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultVisFeatureType();
if((iter=parameters.find(Parameters::kVisFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
ParametersMap customParameters;
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
corelib/src/Parameters.cpp
+91
View File
@@ -39,6 +39,8 @@ namespace rtabmap
ParametersMap Parameters::parameters_;
ParametersMap Parameters::descriptions_;
Parameters Parameters::instance_;
std::map<std::string, std::pair<bool, std::string> > Parameters::removedParameters_;
ParametersMap Parameters::backwardCompatibilityMap_;
Parameters::Parameters()
{
@@ -69,6 +71,95 @@ std::string Parameters::getDefaultDatabaseName()
return "rtabmap.db";
}
const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemovedParameters()
{
if(removedParameters_.empty())
{
// removed parameters
removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize", std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
removedParameters_.insert(std::make_pair("RGBD/PoseScanMatching", std::make_pair(true, Parameters::kRGBDIcpOdomRefining())));
removedParameters_.insert(std::make_pair("Odom/FeatureType", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/EstimationType", std::make_pair(true, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("Odom/MaxFeatures", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("Odom/InlierDistance", std::make_pair(true, Parameters::kVisInlierDistance())));
removedParameters_.insert(std::make_pair("Odom/MinInliers", std::make_pair(true, Parameters::kVisMinInliers())));
removedParameters_.insert(std::make_pair("Odom/Iterations", std::make_pair(true, Parameters::kVisIterations())));
removedParameters_.insert(std::make_pair("Odom/RefineIterations", std::make_pair(true, Parameters::kVisRefineIterations())));
removedParameters_.insert(std::make_pair("Odom/MaxDepth", std::make_pair(true, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("Odom/RoiRatios", std::make_pair(true, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("Odom/Force2D", std::make_pair(true, Parameters::kVisForce2D())));
removedParameters_.insert(std::make_pair("Odom/VarianceFromInliersCount", std::make_pair(true, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("Odom/PnPReprojError", std::make_pair(true, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("Odom/PnPFlags", std::make_pair(true, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("OdomBow/NNType", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("OdomBow/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("OdomSubPix/WinSize", std::make_pair(true, Parameters::kVisSubPixWinSize())));
removedParameters_.insert(std::make_pair("OdomSubPix/Iterations", std::make_pair(true, Parameters::kVisSubPixIterations())));
removedParameters_.insert(std::make_pair("OdomSubPix/Eps", std::make_pair(true, Parameters::kVisSubPixEps())));
removedParameters_.insert(std::make_pair("LccReextract/Activated", std::make_pair(false, Parameters::kRGBDLoopClosureReextractFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/FeatureType", std::make_pair(false, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("LccReextract/MaxWords", std::make_pair(false, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/MaxDepth", std::make_pair(false, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("LccReextract/RoiRatios", std::make_pair(false, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("LccReextract/NNType", std::make_pair(false, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("LccReextract/NNDR", std::make_pair(false, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("LccBow/EstimationType", std::make_pair(false, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("LccBow/InlierDistance", std::make_pair(false, Parameters::kVisInlierDistance())));
removedParameters_.insert(std::make_pair("LccBow/MinInliers", std::make_pair(false, Parameters::kVisMinInliers())));
removedParameters_.insert(std::make_pair("LccBow/Iterations", std::make_pair(false, Parameters::kVisIterations())));
removedParameters_.insert(std::make_pair("LccBow/RefineIterations", std::make_pair(false, Parameters::kVisRefineIterations())));
removedParameters_.insert(std::make_pair("LccBow/Force2D", std::make_pair(false, Parameters::kVisForce2D())));
removedParameters_.insert(std::make_pair("LccBow/VarianceFromInliersCount", std::make_pair(false, Parameters::kRegVarianceFromInliersCount())));
removedParameters_.insert(std::make_pair("LccBow/PnPReprojError", std::make_pair(false, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("LccBow/PnPFlags", std::make_pair(false, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("LccBow/EpipolarGeometryVar", std::make_pair(true, Parameters::kVisEpipolarGeometryVar())));
removedParameters_.insert(std::make_pair("LccIcp/Type", std::make_pair(true, Parameters::kRGBDIcpLoopClosureRefining())));
removedParameters_.insert(std::make_pair("LccIcp3/Decimation", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/MaxDepth", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("LccIcp3/VoxelSize", std::make_pair(false, Parameters::kIcpVoxelSize())));
removedParameters_.insert(std::make_pair("LccIcp3/Samples", std::make_pair(false, Parameters::kIcpDownsamplingStep())));
removedParameters_.insert(std::make_pair("LccIcp3/MaxCorrespondenceDistance", std::make_pair(false, Parameters::kIcpMaxCorrespondenceDistance())));
removedParameters_.insert(std::make_pair("LccIcp3/Iterations", std::make_pair(false, Parameters::kIcpIterations())));
removedParameters_.insert(std::make_pair("LccIcp3/CorrespondenceRatio", std::make_pair(false, Parameters::kIcpCorrespondenceRatio())));
removedParameters_.insert(std::make_pair("LccIcp3/PointToPlane", std::make_pair(true, Parameters::kIcpPointToPlane())));
removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true, Parameters::kIcpPointToPlaneNormalNeighbors())));
removedParameters_.insert(std::make_pair("LccIcp2/MaxCorrespondenceDistance", std::make_pair(true, Parameters::kIcpMaxCorrespondenceDistance())));
removedParameters_.insert(std::make_pair("LccIcp2/Iterations", std::make_pair(true, Parameters::kIcpIterations())));
removedParameters_.insert(std::make_pair("LccIcp2/CorrespondenceRatio", std::make_pair(true, Parameters::kIcpCorrespondenceRatio())));
removedParameters_.insert(std::make_pair("LccIcp2/VoxelSize", std::make_pair(true, Parameters::kIcpVoxelSize())));
}
return removedParameters_;
}
const ParametersMap & Parameters::getBackwardCompatibilityMap()
{
if(backwardCompatibilityMap_.empty())
{
getRemovedParameters(); // make sure removedParameters is filled
// compatibility
for(std::map<std::string, std::pair<bool, std::string> >::iterator iter=removedParameters_.begin();
iter!=removedParameters_.end();
++iter)
{
if(iter->second.first)
{
backwardCompatibilityMap_.insert(ParametersPair(iter->second.second, iter->first));
}
}
}
return backwardCompatibilityMap_;
}
std::string Parameters::getDescription(const std::string & paramKey)
{
std::string description;
corelib/src/RegistrationIcp.cpp
+366
View File
@@ -0,0 +1,366 @@
/*
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.
*/
#include <rtabmap/core/RegistrationIcp.h>
#include <rtabmap/core/util3d_registration.h>
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <pcl/io/pcd_io.h>
namespace rtabmap {
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters) :
_icpMaxTranslation(Parameters::defaultIcpMaxTranslation()),
_icpMaxRotation(Parameters::defaultIcpMaxRotation()),
_icp2D(Parameters::defaultIcp2D()),
_icpVoxelSize(Parameters::defaultIcpVoxelSize()),
_icpDownsamplingStep(Parameters::defaultIcpDownsamplingStep()),
_icpMaxCorrespondenceDistance(Parameters::defaultIcpMaxCorrespondenceDistance()),
_icpMaxIterations(Parameters::defaultIcpIterations()),
_icpCorrespondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
_icpPointToPlane(Parameters::defaultIcpPointToPlane()),
_icpPointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors())
{
this->parseParameters(parameters);
}
void RegistrationIcp::parseParameters(const ParametersMap & parameters)
{
Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _icpMaxTranslation);
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _icpMaxRotation);
Parameters::parse(parameters, Parameters::kIcp2D(), _icp2D);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _icpVoxelSize);
Parameters::parse(parameters, Parameters::kIcpDownsamplingStep(), _icpDownsamplingStep);
Parameters::parse(parameters, Parameters::kIcpMaxCorrespondenceDistance(), _icpMaxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kIcpIterations(), _icpMaxIterations);
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _icpCorrespondenceRatio);
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _icpPointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _icpPointToPlaneNormalNeighbors);
UASSERT_MSG(_icpVoxelSize >= 0, uFormat("value=%d", _icpVoxelSize).c_str());
UASSERT_MSG(_icpDownsamplingStep >= 0, uFormat("value=%d", _icpDownsamplingStep).c_str());
UASSERT_MSG(_icpMaxCorrespondenceDistance > 0.0f, uFormat("value=%f", _icpMaxCorrespondenceDistance).c_str());
UASSERT_MSG(_icpMaxIterations > 0, uFormat("value=%d", _icpMaxIterations).c_str());
UASSERT_MSG(_icpCorrespondenceRatio >=0.0f && _icpCorrespondenceRatio <=1.0f, uFormat("value=%f", _icpCorrespondenceRatio).c_str());
UASSERT_MSG(_icpPointToPlaneNormalNeighbors > 0, uFormat("value=%d", _icpPointToPlaneNormalNeighbors).c_str());
}
Transform RegistrationIcp::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
float * varianceOut,
float * inliersRatioOut)
{
return computeTransformation(
fromSignature.sensorData(),
toSignature.sensorData(),
guess,
rejectedMsg,
inliersOut,
varianceOut,
inliersRatioOut);
}
Transform RegistrationIcp::computeTransformation(
const SensorData & dataFrom,
const SensorData & dataTo,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
float * varianceOut,
float * inliersRatioOut)
{
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _icpVoxelSize);
UDEBUG("2D=%d", _icp2D?1:0);
UDEBUG("PointToPlane=%d", _icpPointToPlane?1:0);
UDEBUG("Normal neighborhood=%d", _icpPointToPlaneNormalNeighbors);
UDEBUG("Max corrrespondence distance=%f", _icpMaxCorrespondenceDistance);
UDEBUG("Max Iterations=%d", _icpMaxIterations);
UDEBUG("Variance from inliers count=%d", _bowVarianceFromInliersCount?1:0);
UDEBUG("Correspondence Ratio=%f", _icpCorrespondenceRatio);
UDEBUG("Max translation=%f", _icpMaxTranslation);
UDEBUG("Max rotation=%f", _icpMaxRotation);
UDEBUG("Downsampling step=%d", _icpDownsamplingStep);
std::string msg;
Transform transform;
// ICP with guess transform
if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
int maxLaserScans = dataTo.laserScanMaxPts();
cv::Mat fromScan = dataFrom.laserScanRaw();
cv::Mat toScan = dataTo.laserScanRaw();
if(_icpDownsamplingStep>1)
{
fromScan = util3d::downsample(fromScan, _icpDownsamplingStep);
toScan = util3d::downsample(toScan, _icpDownsamplingStep);
maxLaserScans/=_icpDownsamplingStep;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
if(toCloud->size() && fromCloud->size())
{
//filtering
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
bool filtered = false;
if(_icpVoxelSize > 0.0f)
{
fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _icpVoxelSize);
toCloudFiltered = util3d::voxelize(toCloudFiltered, _icpVoxelSize);
filtered = true;
}
Transform icpT;
bool hasConverged = false;
float correspondencesRatio = 0.0f;
int correspondences = 0;
double variance = 1.0;
bool correspondencesComputed = false;
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
if(!_icp2D) // 3D ICP
{
if(_icpPointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _icpPointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _icpPointToPlaneNormalNeighbors);
std::vector<int> indices;
toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
if(toCloudNormals->size() && fromCloudNormals->size())
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
icpT = util3d::icpPointToPlane(
toCloudNormals,
fromCloudNormals,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
*newCloudNormalsRegistered);
if(!filtered &&
!icpT.isNull() &&
hasConverged)
{
util3d::computeVarianceAndCorrespondences(
newCloudNormalsRegistered,
fromCloudNormals,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
correspondencesComputed = true;
}
}
}
else
{
icpT = util3d::icp(
toCloudFiltered,
fromCloudFiltered,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
*newCloudRegistered);
}
}
else // 2D ICP
{
icpT = util3d::icp2D(
toCloudFiltered,
fromCloudFiltered,
_icpMaxCorrespondenceDistance,
_icpMaxIterations,
hasConverged,
*newCloudRegistered);
}
/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
pcl::io::savePCDFile("toCloud.pcd", *toCloud);
UWARN("saved fromCloud.pcd and toCloud.pcd");
if(!icpT.isNull())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudTmp = util3d::transformPointCloud(toCloud, icpT);
pcl::io::savePCDFile("newCloudFinal.pcd", *toCloudTmp);
UWARN("saved toCloudFinal.pcd");
}*/
if(!icpT.isNull() &&
hasConverged)
{
float ix,iy,iz, iroll,ipitch,iyaw;
icpT.getTranslationAndEulerAngles(ix,iy,iz,iroll,ipitch,iyaw);
if((_icpMaxTranslation>0.0f &&
(fabs(ix) > _icpMaxTranslation ||
fabs(iy) > _icpMaxTranslation ||
fabs(iz) > _icpMaxTranslation))
||
(_icpMaxRotation>0.0f &&
(fabs(iroll) > _icpMaxRotation ||
fabs(ipitch) > _icpMaxRotation ||
fabs(iyaw) > _icpMaxRotation)))
{
msg = uFormat("Cannot compute transform (ICP correction too large -> %f m %f rad, limits=%f m, %f rad)",
uMax3(fabs(ix), fabs(iy), fabs(iz)),
uMax3(fabs(iroll), fabs(ipitch), fabs(iyaw)),
_icpMaxTranslation,
_icpMaxRotation);
UINFO(msg.c_str());
}
else
{
if(!correspondencesComputed)
{
if(filtered)
{
fromCloud = util3d::transformPointCloud(fromCloud, icpT);
}
else
{
fromCloud = newCloudRegistered;
}
util3d::computeVarianceAndCorrespondences(
toCloud,
fromCloud,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
}
// verify if there are enough correspondences
if(maxLaserScans)
{
correspondencesRatio = float(correspondences)/float(maxLaserScans);
}
else
{
UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
dataTo.id());
correspondencesRatio = float(correspondences)/float(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size());
}
UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
dataTo.id(), dataFrom.id(),
hasConverged?"true":"false",
variance,
correspondences,
maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
correspondencesRatio*100.0f);
if(_bowVarianceFromInliersCount)
{
variance = correspondencesRatio > 0?1.0/double(correspondencesRatio):1.0;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
if(inliersOut)
{
*inliersOut = correspondences;
}
if(inliersRatioOut)
{
*inliersRatioOut = correspondencesRatio;
}
if(correspondencesRatio < _icpCorrespondenceRatio)
{
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f)",
correspondences, correspondencesRatio, _icpCorrespondenceRatio);
UINFO(msg.c_str());
}
else
{
transform = guess*icpT;
}
}
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f)",
hasConverged?"true":"false", variance);
UINFO(msg.c_str());
}
// still compute the variance for information
/*if(variance == 1 && varianceOut)
{
util3d::computeVarianceAndCorrespondences(
toCloudFiltered,
fromCloudFiltered,
_icpMaxCorrespondenceDistance,
variance,
correspondences);
if(variance > 0)
{
*varianceOut = variance;
}
}*/
}
else
{
msg = "Laser scans empty ?!?";
UWARN(msg.c_str());
}
}
else
{
msg = uFormat("Laser scans empty?!? (new[%d]=%d old[%d]=%d)",
dataTo.id(), dataTo.laserScanRaw().total(),
dataFrom.id(), dataFrom.laserScanRaw().total());
UERROR(msg.c_str());
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform;
}
}
corelib/src/RegistrationVis.cpp
+384
View File
@@ -0,0 +1,384 @@
/*
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.
*/
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
namespace rtabmap {
RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
_bowMinInliers(Parameters::defaultVisMinInliers()),
_bowInlierDistance(Parameters::defaultVisInlierDistance()),
_bowIterations(Parameters::defaultVisIterations()),
_bowRefineIterations(Parameters::defaultVisRefineIterations()),
_bowForce2D(Parameters::defaultVisForce2D()),
_bowEpipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_bowEstimationType(Parameters::defaultVisEstimationType()),
_bowPnPReprojError(Parameters::defaultVisPnPReprojError()),
_bowPnPFlags(Parameters::defaultVisPnPFlags()),
_reextractNNType(Parameters::defaultVisNNType()),
_reextractNNDR(Parameters::defaultVisNNDR()),
_reextractFeatureType(Parameters::defaultVisFeatureType()),
_reextractMaxWords(Parameters::defaultVisMaxFeatures()),
_reextractMaxDepth(Parameters::defaultVisMaxDepth()),
_reextractMinDepth(Parameters::defaultVisMinDepth()),
_reextractRoiRatios(Parameters::defaultVisRoiRatios()),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
{
this->parseParameters(parameters);
}
void RegistrationVis::parseParameters(const ParametersMap & parameters)
{
Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kVisMinInliers(), _bowMinInliers);
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _bowInlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _bowIterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _bowRefineIterations);
Parameters::parse(parameters, Parameters::kVisForce2D(), _bowForce2D);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _bowEstimationType);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _bowEpipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _bowPnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _bowPnPFlags);
Parameters::parse(parameters, Parameters::kVisNNType(), _reextractNNType);
Parameters::parse(parameters, Parameters::kVisNNDR(), _reextractNNDR);
Parameters::parse(parameters, Parameters::kVisFeatureType(), _reextractFeatureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _reextractMaxDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
}
Transform RegistrationVis::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform guess, // guess is ignored for RegistrationVis
std::string * rejectedMsg,
int * inliersOut,
float * varianceOut,
float * inliersRatioOut)
{
Transform transform;
std::string msg;
// Guess transform from visual words
int inliersCount= 0;
double variance = 1.0;
// Extract features?
const std::multimap<int, cv::KeyPoint> * wordsFrom = 0;
const std::multimap<int, cv::KeyPoint> * wordsTo = 0;
const std::multimap<int, pcl::PointXYZ> * words3From = 0;
const std::multimap<int, pcl::PointXYZ> * words3To = 0;
std::multimap<int, cv::KeyPoint> extractedWordsFrom, extractedWordsTo;
std::multimap<int, pcl::PointXYZ> extractedWords3From, extractedWords3To;
if(fromSignature.getWords().size() == 0 && toSignature.getWords().size() == 0)
{
// Use the Memory class to extract features
ParametersMap customParameters;
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_reextractMinDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations)));
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _reextractRoiRatios));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
Memory memory(customParameters);
// Add signatures
SensorData dataFrom = fromSignature.sensorData();
SensorData dataTo = toSignature.sensorData();
// make sure there are no features already in the SensorData
dataFrom.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
dataTo.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
UTimer timeT;
memory.update(dataFrom);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataFrom.id());
}
else
{
extractedWordsFrom = memory.getLastWorkingSignature()->getWords();
extractedWords3From = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeTo = %fs", timeT.ticks());
memory.update(dataTo);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataTo.id());
}
else
{
extractedWordsTo = memory.getLastWorkingSignature()->getWords();
extractedWords3To = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeFrom = %fs", timeT.ticks());
}
}
wordsFrom = &extractedWordsFrom;
wordsTo = &extractedWordsTo;
words3From = &extractedWords3From;
words3To = &extractedWords3To;
}
else
{
wordsFrom = &fromSignature.getWords();
wordsTo = &toSignature.getWords();
words3From = &fromSignature.getWords3();
words3To = &toSignature.getWords3();
}
if(_bowEstimationType == 2) // Epipolar Geometry
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else if((int)wordsFrom->size() >= _bowMinInliers &&
(int)wordsTo->size() >= _bowMinInliers)
{
UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
*wordsFrom,
*wordsTo,
cameraModel,
cameraTransform,
_bowIterations,
_bowPnPReprojError,
_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE
1.0f,
0.99f,
*words3From, // for scale estimation
&variance);
inliersCount = (int)inliers3D.size();
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _bowMinInliers)
{
if(variance <= _bowEpipolarGeometryVar)
{
transform = cameraTransform;
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(words3From->size() == 0)
{
msg = uFormat("No 3D guess words found");
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_bowEstimationType == 1) // PnP
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
toSignature.id(),
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModel().isValid()?1:0,
toSignature.getWeight());
}
else
{
// 3D to 2D
if((int)words3From->size() >= _bowMinInliers &&
(int)wordsTo->size() >= _bowMinInliers)
{
UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo2D(
uMultimapToMap(*words3From),
uMultimapToMap(*wordsTo),
cameraModel,
_bowMinInliers,
_bowIterations,
_bowPnPReprojError,
_bowPnPFlags,
Transform::getIdentity(),
uMultimapToMap(*words3To),
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)wordsTo->size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
}
else
{
// 3D -> 3D
if((int)words3From->size() >= _bowMinInliers &&
(int)words3To->size() >= _bowMinInliers)
{
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D(
uMultimapToMap(*words3From),
uMultimapToMap(*words3To),
_bowMinInliers,
_bowInlierDistance,
_bowIterations,
_bowRefineIterations,
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)words3To->size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
if(!transform.isNull())
{
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(roll) > CV_PI/2 ||
fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
{
transform.setNull();
msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)",
roll, pitch, yaw);
UWARN(msg.c_str());
}
else if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
transform = Transform(x,y,0, 0, 0, yaw);
}
}
if(_bowVarianceFromInliersCount)
{
variance = inliersCount > 0?1.0/double(inliersCount):1.0;
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(inliersOut)
{
*inliersOut = inliersCount;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform
}
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
}
}
corelib/src/Rtabmap.cpp
+89 -187
View File
@@ -90,8 +90,8 @@ Rtabmap::Rtabmap() :
_rgbdLinearUpdate(Parameters::defaultRGBDLinearUpdate()),
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
_loopClosureIcpRefining(Parameters::defaultRGBDIcpLoopClosureRefining()),
_odomIcpRefining(Parameters::defaultRGBDIcpOdomRefining()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_scanMatchingIdsSavedInLinks(Parameters::defaultRGBDScanMatchingIdsSavedInLinks()),
@@ -100,15 +100,10 @@ Rtabmap::Rtabmap() :
_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
_localPathScansMerged(Parameters::defaultRGBDLocalLoopDetectionPathScansMerged()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
_reextractNNType(Parameters::defaultLccReextractNNType()),
_reextractNNDR(Parameters::defaultLccReextractNNDR()),
_reextractFeatureType(Parameters::defaultLccReextractFeatureType()),
_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
_reextractMaxDepth(Parameters::defaultLccReextractMaxDepth()),
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
@@ -402,7 +397,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rgbdLinearUpdate);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
Parameters::parse(parameters, Parameters::kRGBDIcpOdomRefining(), _odomIcpRefining);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
Parameters::parse(parameters, Parameters::kRGBDScanMatchingIdsSavedInLinks(), _scanMatchingIdsSavedInLinks);
@@ -411,38 +406,20 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathScansMerged(), _localPathScansMerged);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
Parameters::parse(parameters, Parameters::kLccReextractNNDR(), _reextractNNDR);
Parameters::parse(parameters, Parameters::kLccReextractFeatureType(), _reextractFeatureType);
Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kLccReextractMaxDepth(), _reextractMaxDepth);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
Parameters::parse(parameters, Parameters::kRGBDIcpLoopClosureRefining(), _loopClosureIcpRefining);
UASSERT(_rgbdLinearUpdate >= 0.0f);
UASSERT(_rgbdAngularUpdate >= 0.0f);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
{
int icpType = std::atoi((*iter).second.c_str());
if(icpType >= 0 && icpType <= 2)
{
_globalLoopClosureIcpType = icpType;
}
else
{
UERROR("Icp type must be 0, 1 or 2 (value=%d)", icpType);
}
}
// By default, we create our strategies if they are not already created.
// If they already exists, we check the parameters if a change is requested
@@ -1023,17 +1000,17 @@ bool Rtabmap::process(
//============================================================
// Scan matching
//============================================================
if(_poseScanMatching &&
if(_odomIcpRefining &&
!signature->sensorData().laserScanCompressed().empty() &&
rehearsedId == 0) // don't do it if rehearsal happened
{
UINFO("Odometry correction by scan matching");
Transform guess = signature->getLinks().begin()->second.transform();
double variance = 1.0;
Transform guess = signature->getLinks().begin()->second.transform().inverse();
float variance = 1.0f;
int inliers = 0;
float inliersRatio = 0;
std::string rejectedMsg;
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, false, &rejectedMsg, &inliers, &variance, &inliersRatio);
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &rejectedMsg, &inliers, &variance, &inliersRatio);
if(!t.isNull())
{
UINFO("Scan matching: update neighbor link (%d->%d, variance=%f) from %s to %s",
@@ -1043,14 +1020,14 @@ bool Rtabmap::process(
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
UASSERT(variance > 0.0);
_memory->updateLink(signature->id(), oldId, t, variance, variance);
_memory->updateLink(oldId, signature->id(), t, variance, variance);
if(_optimizeFromGraphEnd)
{
// update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t;
Transform u = guess * t.inverse();
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(jter!=_optimizedPoses.end());
Transform up = jter->second * u * jter->second.inverse();
@@ -1074,6 +1051,7 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kOdomCorrectionInliers(), inliers);
statistics_.addStatistic(Statistics::kOdomCorrectionInliers_ratio(), inliersRatio);
statistics_.addStatistic(Statistics::kOdomCorrectionVariance(), variance);
statistics_.addStatistic(Statistics::kOdomCorrectionPts(), signature->sensorData().laserScanRaw().cols);
}
timeScanMatching = timer.ticks();
ULOGGER_INFO("timeScanMatching=%fs", timeScanMatching);
@@ -1179,12 +1157,12 @@ bool Rtabmap::process(
{
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
double variance = 1.0;
float variance = 1.0f;
int inliers = -1;
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
Transform transform = _memory->computeVisualTransform(signature->id(), *iter, &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _loopClosureIcpRefining)
{
transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
transform = _memory->computeIcpTransform(signature->id(), *iter, transform, &rejectedMsg, 0, &variance);
}
if(!transform.isNull())
{
@@ -1733,72 +1711,16 @@ bool Rtabmap::process(
{
//Compute transform if metric data are present
Transform transform;
double variance = 1;
float variance = 1.0f;
if(_rgbdSlamMode)
{
std::string rejectedMsg;
if(_reextractLoopClosureFeatures)
transform = _memory->computeVisualTransform(signature->id(), _loopClosureHypothesis.first, &rejectedMsg, &loopClosureVisualInliers, &variance);
if(!transform.isNull() && _loopClosureIcpRefining)
{
ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
//{
// UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
//}
Memory memory(customParameters);
UTimer timeT;
// Add signatures
SensorData dataFrom = data;
dataFrom.setId(signature->id());
SensorData dataTo = _memory->getNodeData(_loopClosureHypothesis.first, true);
UDEBUG("timeTo = %fs", timeT.ticks());
if(!dataFrom.depthOrRightRaw().empty() &&
!dataTo.depthOrRightRaw().empty() &&
dataFrom.id() != Memory::kIdInvalid &&
dataTo.id() != Memory::kIdInvalid)
{
memory.update(dataTo);
UDEBUG("timeUpTo = %fs", timeT.ticks());
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
{
// Fallback to normal way (raw data not kept in database...)
UWARN("Loop closure: Some images not found in memory for re-extracting "
"features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
}
else
{
transform = _memory->computeVisualTransform(_loopClosureHypothesis.first, signature->id(), &rejectedMsg, &loopClosureVisualInliers, &variance);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
transform = _memory->computeIcpTransform(signature->id(), _loopClosureHypothesis.first, transform, &rejectedMsg, 0, &variance);
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
@@ -1896,70 +1818,11 @@ bool Rtabmap::process(
(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
{
double variance = 1.0;
Transform transform;
if(_reextractLoopClosureFeatures)
float variance = 1.0f;
Transform transform = _memory->computeVisualTransform(signature->id(), nearestId, 0, 0, &variance);
if(!transform.isNull() && _loopClosureIcpRefining)
{
ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
// override some parameters
uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), "0.0 0.0 0.0 0.0"));
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "false"));
//for(ParametersMap::iterator iter = customParameters.begin(); iter!=customParameters.end(); ++iter)
//{
// UDEBUG("%s=%s", iter->first.c_str(), iter->second.c_str());
//}
Memory memory(customParameters);
UTimer timeT;
// Add signatures
SensorData dataFrom = data;
dataFrom.setId(signature->id());
SensorData dataTo = _memory->getNodeData(nearestId, true);
UDEBUG("timeTo = %fs", timeT.ticks());
if(!dataFrom.depthOrRightRaw().empty() &&
!dataTo.depthOrRightRaw().empty() &&
dataFrom.id() != Memory::kIdInvalid &&
dataTo.id() != Memory::kIdInvalid)
{
memory.update(dataTo);
UDEBUG("timeUpTo = %fs", timeT.ticks());
memory.update(dataFrom);
UDEBUG("timeUpFrom = %fs", timeT.ticks());
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), 0, 0, &variance);
UDEBUG("timeTransform = %fs", timeT.ticks());
}
else
{
// Fallback to normal way (raw data not kept in database...)
UWARN("Loop closure: Some images not found in memory for re-extracting "
"features, is Mem/RawDataKept=false? Falling back with already extracted 3D features.");
transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
}
}
else
{
transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(nearestId, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
transform = _memory->computeIcpTransform(signature->id(), nearestId, transform, 0, 0, &variance);
}
if(!transform.isNull())
{
@@ -2019,38 +1882,48 @@ bool Rtabmap::process(
(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius*_localPathFilteringRadius))
{
// Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed)
if(!_localPathScansMerged)
{
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), false);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
//only keep the nearest node
std::map<int, Transform> tmp;
tmp.insert(*path.find(nearestId));
path = tmp;
}
else
{
// Assemble scans in the path and do ICP only
if(_localPathOdomPosesUsed)
{
jter->second = t * jter->second;
//optimize the path's poses locally
path = optimizeGraph(nearestId, uKeysSet(path), false);
// transform local poses in optimized graph referential
UASSERT(uContains(path, nearestId));
Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
{
jter->second = t * jter->second;
}
}
if(path.size() > 2 && _localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
}
if(_localPathFilteringRadius > 0.0f)
{
// path filtering
std::map<int, Transform> filteredPath = graph::radiusPosesFiltering(path, _localPathFilteringRadius, 0, true);
// make sure the nearest and farthest poses are still here
filteredPath.insert(*path.find(nearestId));
filteredPath.insert(*path.begin());
filteredPath.insert(*path.rbegin());
path = filteredPath;
}
if(path.size() > 2) // more than current+nearest
if(path.size() > 0)
{
// add current node to poses
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
double variance = 1.0;
float variance = 1.0f;
Transform transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
if(!transform.isNull())
{
@@ -2340,7 +2213,7 @@ bool Rtabmap::process(
// timings...
statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000);
statistics_.addStatistic(Statistics::kTimingOdom_correction(), timeScanMatching*1000);
statistics_.addStatistic(Statistics::kTimingLocal_detection_TIME(), timeLocalTimeDetection*1000);
statistics_.addStatistic(Statistics::kTimingLocal_detection_SPACE(), timeLocalSpaceDetection*1000);
statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
@@ -3836,12 +3709,41 @@ void Rtabmap::readParameters(const std::string & configFile, ParametersMap & par
else
{
key = uReplaceChar(key, '\\', '/'); // Ini files use \ by default for separators, so replace them
// look for old parameter name
bool addParameter = true;
std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
if(oldIter!=Parameters::getRemovedParameters().end())
{
addParameter = oldIter->second.first;
if(addParameter)
{
key = oldIter->second.second;
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
oldIter->first.c_str(), oldIter->second.second.c_str(), iter->second);
}
else if(oldIter->second.second.empty())
{
UWARN("Parameter \"%s\" doesn't exist anymore.",
oldIter->first.c_str());
}
else
{
UWARN("Parameter \"%s\" doesn't exist anymore, you may want to use this similar parameter \"%s\":\"%s\".",
oldIter->first.c_str(), oldIter->second.second.c_str(), Parameters::getDescription(oldIter->second.second).c_str());
}
}
ParametersMap::iterator jter = parameters.find(key);
if(jter != parameters.end())
{
parameters.erase(jter);
}
parameters.insert(ParametersPair(key, (*iter).second));
if(addParameter)
{
parameters.insert(ParametersPair(key, iter->second));
}
}
}
}
corelib/src/RtabmapThread.cpp
+4 -6
View File
@@ -553,12 +553,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
}
if(ignoreFrame)
{
// remove data from the frame, keeping only constraints
SensorData tmp(
cv::Mat(),
odomEvent.data().id(),
odomEvent.data().stamp(),
odomEvent.data().userDataRaw());
// set negative id so rtabmap will detect it as an intermediate node
SensorData tmp = odomEvent.data();
tmp.setId(-1);
tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
}
else
corelib/src/SensorData.cpp
+3 -3
View File
@@ -199,7 +199,7 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
{
_laserScanRaw = laserScan;
}
@@ -306,7 +306,7 @@ SensorData::SensorData(
_depthOrRightRaw = depth;
}
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
{
_laserScanRaw = laserScan;
}
@@ -412,7 +412,7 @@ SensorData::SensorData(
_depthOrRightRaw = right;
}
if(laserScan.type() == CV_32FC2)
if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
{
_laserScanRaw = laserScan;
}
corelib/src/Transform.cpp
+42
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <iomanip>
namespace rtabmap {
@@ -315,4 +316,45 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
/**
* Format (6 values): x y z roll pitch yaw.
* Format (9 [+3] values): r11 r12 r13 r21 r22 r23 r31 r32 r33 [tx ty tz].
*/
Transform Transform::fromString(const std::string & string)
{
Transform t;
std::list<std::string> list = uSplit(string, ' ');
if(list.size() == 6 || list.size() == 9 || list.size() == 12)
{
std::vector<float> numbers(list.size());
int i = 0;
for(std::list<std::string>::iterator iter=list.begin(); iter!=list.end(); ++iter)
{
numbers[i++] = uStr2Float(*iter);
}
if(numbers.size() == 6)
{
t = Transform(numbers[0], numbers[1], numbers[2], numbers[3], numbers[4], numbers[5]);
}
else if(numbers.size() == 9)
{
t = Transform(numbers[0], numbers[1], numbers[2], 0,
numbers[3], numbers[4], numbers[5], 0,
numbers[6], numbers[7], numbers[8], 0);
}
else if(numbers.size() == 12)
{
t = Transform(numbers[0], numbers[1], numbers[2], numbers[9],
numbers[3], numbers[4], numbers[5], numbers[10],
numbers[6], numbers[7], numbers[8], numbers[11]);
}
}
else
{
UERROR("Local transform is wrong! must have 6 or 9 items (%s)", string.c_str());
}
return t;
}
}
corelib/src/util3d.cpp
+76 -43
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>
#include <opencv2/imgproc/imgproc.hpp>
@@ -550,7 +551,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
tmp = util3d::randomSampling(tmp, samples);
filtered = true;
}
@@ -685,7 +686,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
if(tmp->size() && samples)
{
tmp = util3d::sampling(tmp, samples);
tmp = util3d::randomSampling(tmp, samples);
filtered = true;
}
@@ -789,64 +790,60 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
return scan;
}
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC3);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec3f>(i)[0] = pt.x;
laserScan.at<cv::Vec3f>(i)[1] = pt.y;
laserScan.at<cv::Vec3f>(i)[2] = pt.z;
}
else
{
laserScan.at<cv::Vec3f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec3f>(i)[1] = cloud.at(i).y;
laserScan.at<cv::Vec3f>(i)[2] = cloud.at(i).z;
}
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
output->resize(laserScan.cols);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
if(laserScan.type() == CV_32FC2)
{
output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
}
else
{
output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
}
if(!nullTransform)
{
output->at(i) = pcl::transformPoint(output->at(i), transform3f);
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
const cv::Mat & matrix,
const Transform & tranform)
{
UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
UASSERT(matrix.rows == 1);
Eigen::Affine3f t = tranform.toEigen3f();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(matrix.cols);
if(matrix.channels() == 2)
{
for(int i=0; i<matrix.cols; ++i)
{
cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
cloud->at(i).z = 0.0f;
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
}
}
else // channels=3
{
for(int i=0; i<matrix.cols; ++i)
{
cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
}
}
return cloud;
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
pcl::PointXYZ projectDisparityTo3D(
const cv::Point2f & pt,
@@ -950,6 +947,42 @@ void savePCDWords(
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
{
UASSERT(dim > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
long bytes = UFile::length(fileName);
if(bytes)
{
UASSERT(bytes % sizeof(float) == 0);
int32_t num = bytes/sizeof(float);
UASSERT(num % dim == 0);
float *data = (float*)malloc(num*sizeof(float));
// pointers
float *px = data+0;
float *py = data+1;
float *pz = data+2;
float *pr = data+3;
// load point cloud
FILE *stream;
stream = fopen (fileName.c_str(),"rb");
num = fread(data,sizeof(float),num,stream)/4;
cloud->resize(num);
for (int32_t i=0; i<num; i++) {
(*cloud)[i].x = *px;
(*cloud)[i].y = *py;
(*cloud)[i].z = *pz;
px+=4; py+=4; pz+=4; pr+=4;
}
fclose(stream);
}
return cloud;
}
}
}
corelib/src/util3d_filtering.cpp
+97 -2
View File
@@ -49,6 +49,101 @@ namespace rtabmap
namespace util3d
{
cv::Mat downsample(
const cv::Mat & cloud,
int step)
{
// 2D or 3D point clouds (laser scans)
UASSERT(cloud.type() == CV_32FC2 || cloud.type() == CV_32FC3);
UASSERT(step > 0);
cv::Mat output;
if(step == 1)
{
// no sampling
output = cloud.clone();
}
else
{
if(cloud.cols > step)
{
int finalSize = cloud.cols/step;
output = cv::Mat(1, finalSize, cloud.type());
int oi = 0;
for(unsigned int i=0; i<cloud.cols-step+1; i+=step)
{
cv::Mat(cloud, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
}
else if(cloud.cols)
{
output = cv::Mat(1, 1, cloud.type());
cv::Mat(cloud, cv::Range::all(), cv::Range(0,1)).copyTo(output); // first point
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr downsample(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int step)
{
UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
if(step == 1)
{
// no sampling
*output = *cloud;
}
else
{
if(cloud->size() > step)
{
int finalSize = cloud->size()/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
}
else if(cloud->size())
{
output->push_back(cloud->at(0));
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsample(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int step)
{
UASSERT(step > 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
if(step == 1)
{
// no sampling
*output = *cloud;
}
else
{
if(cloud->size() > step)
{
int finalSize = cloud->size()/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
}
else if(cloud->size())
{
output->push_back(cloud->at(0));
}
}
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
@@ -87,7 +182,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
}
pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
{
UASSERT(samples > 0);
@@ -98,7 +193,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
filter.filter(*output);
return output;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
{
UASSERT(samples > 0);
corelib/src/util3d_registration.cpp
+4 -5
View File
@@ -306,7 +306,7 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
//icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -340,7 +340,7 @@ Transform icpPointToPlane(
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
//icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -373,7 +373,7 @@ Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
//icp.setTransformationEpsilon (transformationEpsilon);
//icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
@@ -423,7 +423,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
}
else if(samples>0 && (int)cloud->size() > samples)
{
cloud = sampling(cloud, samples);
cloud = randomSampling(cloud, samples);
}
if(cloud->size())
@@ -439,7 +439,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
return cloud;
}
}
}