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
CMakeLists.txt
+2 -2
View File
@@ -19,8 +19,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
# VERSION
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 10)
SET(RTABMAP_PATCH_VERSION 11)
SET(RTABMAP_MINOR_VERSION 11)
SET(RTABMAP_PATCH_VERSION 0)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
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
+66 -69
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.");
// 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).");
// 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().");
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().");
@@ -437,6 +420,17 @@ public:
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();
static std::string getDefaultWorkingDirectory();
@@ -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;
}
}
}
guilib/include/rtabmap/gui/CloudViewer.h
+6
View File
@@ -129,6 +129,12 @@ public:
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Transform & pose = Transform::getIdentity(),
const QColor & color = QColor());
bool addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
guilib/include/rtabmap/gui/DatabaseViewer.h
+3
View File
@@ -84,7 +84,9 @@ private slots:
void generateTOROGraph();
void generateG2OGraph();
void view3DMap();
void view3DLaserScans();
void generate3DMap();
void generate3DLaserScans();
void detectMoreLoopClosures();
void refineAllNeighborLinks();
void refineAllLoopClosureLinks();
@@ -106,6 +108,7 @@ private slots:
void resetConstraint();
void rejectConstraint();
void updateConstraintView();
void updateLoggerLevel();
void updateStereo();
private:
guilib/include/rtabmap/gui/PreferencesDialog.h
+8 -3
View File
@@ -151,6 +151,8 @@ public:
int getCloudPointSize(int index) const; // 0=map, 1=odom
bool isScansShown(int index) const; // 0=map, 1=odom
int getDownsamplingStepScan(int index) const; // 0=map, 1=odom
double getCloudVoxelSizeScan(int index) const; // 0=map, 1=odom
double getScanOpacity(int index) const; // 0=map, 1=odom
int getScanPointSize(int index) const; // 0=map, 1=odom
@@ -177,7 +179,6 @@ public:
// source panel
double getGeneralInputRate() const;
bool isSourceMirroring() const;
QString getCalibrationName() const;
PreferencesDialog::Src getSourceType() const;
PreferencesDialog::Src getSourceDriver() const;
QString getSourceDriverStr() const;
@@ -192,6 +193,7 @@ public:
bool isSourceRGBDColorOnly() const;
bool isSourceStereoDepthGenerated() const;
Transform getSourceLocalTransform() const; //Openni group
Transform getStereoLaserLocalTransform() const; // stereo images
Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null
int getIgnoredDCComponents() const;
@@ -209,7 +211,7 @@ public:
double getSimThr() const;
int getOdomStrategy() const;
int getOdomBufferSize() const;
bool getLccBowVarianceFromInliersCount() const;
bool getRegVarianceFromInliersCount() const;
QString getCameraInfoDir() const; // "workinfDir/camera_info"
//
@@ -254,12 +256,14 @@ private slots:
void updateBasicParameter();
void openDatabaseViewer();
void selectSourceDatabase();
void selectCalibrationPath();
void selectSourceRGBDImagesStamps();
void selectSourceRGBDImagesPathRGB();
void selectSourceRGBDImagesPathDepth();
void selectSourceStereoImagesStamps();
void selectSourceStereoImagesPathLeft();
void selectSourceStereoImagesPathRight();
void selectSourceStereoImagesPathScans();
void selectSourceImagesPath();
void selectSourceVideoPath();
void selectSourceStereoVideoPath();
@@ -307,7 +311,6 @@ private:
void addParameters(const QGroupBox * box);
QList<QGroupBox*> getGroupBoxes();
void readSettingsBegin();
void testOdometry(int type);
protected:
rtabmap::ParametersMap _parameters;
@@ -331,6 +334,8 @@ private:
QVector<QDoubleSpinBox*> _3dRenderingOpacity;
QVector<QSpinBox*> _3dRenderingPtSize;
QVector<QCheckBox*> _3dRenderingShowScans;
QVector<QSpinBox*> _3dRenderingDownsamplingScan;
QVector<QDoubleSpinBox*> _3dRenderingVoxelSizeScan;
QVector<QDoubleSpinBox*> _3dRenderingOpacityScan;
QVector<QSpinBox*> _3dRenderingPtSizeScan;
};
guilib/src/CloudViewer.cpp
+17
View File
@@ -518,6 +518,23 @@ bool CloudViewer::addCloud(
return false;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Transform & pose,
const QColor & color)
{
if(!_addedClouds.contains(id))
{
UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
pcl::toPCLPointCloud2(*cloud, *binaryCloud);
return addCloud(id, binaryCloud, pose, false, true, color);
}
return false;
}
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
guilib/src/CreateSimpleCalibrationDialog.cpp
+1
View File
@@ -102,6 +102,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
if(!name.isEmpty())
{
cameraName_ = name;
std::string base = (dir+QDir::separator()+name).toStdString();
std::string leftPath = base+"_left.yaml";
std::string rightPath = base+"_right.yaml";
guilib/src/DatabaseViewer.cpp
+431 -223
View File
@@ -60,12 +60,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/Compression.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/RegistrationIcp.h"
#include "rtabmap/gui/DataRecorder.h"
#include "rtabmap/core/SensorData.h"
#include "ExportDialog.h"
#include "rtabmap/gui/ProgressDialog.h"
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
@@ -90,6 +93,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->buttonBox->setVisible(false);
connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
ui_->comboBox_logger_level->setVisible(parent==0);
ui_->label_logger_level->setVisible(parent==0);
connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
QString title("RTAB-Map Database Viewer[*]");
this->setWindowTitle(title);
@@ -164,7 +171,9 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph()));
ui_->actionGenerate_g2o_graph_g2o->setEnabled(graph::G2OOptimizer::available());
connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
connect(ui_->actionView_3D_laser_scans, SIGNAL(triggered()), this, SLOT(view3DLaserScans()));
connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
connect(ui_->actionExport_3D_laser_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(generate3DLaserScans()));
connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
@@ -221,6 +230,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
@@ -260,6 +270,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
//connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
// Graph view
connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
@@ -288,11 +299,13 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_voxel, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_icp_downsamplingStepSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_icp_iteration, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_icp_p2plane, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_icp_minCorrespondenceRatio, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
// Visual parameters
connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
@@ -382,6 +395,8 @@ void DatabaseViewer::readSettings()
}
savedMaximized_ = settings.value("maximized", false).toBool();
ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
// GraphViewer settings
ui_->graphViewer->loadSettings(settings, "GraphView");
@@ -423,11 +438,13 @@ void DatabaseViewer::readSettings()
ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
ui_->doubleSpinBox_icp_voxel->setValue(settings.value("voxel", ui_->doubleSpinBox_icp_voxel->value()).toDouble());
ui_->spinBox_icp_downsamplingStepSize->setValue(settings.value("samplingStep", ui_->spinBox_icp_downsamplingStepSize->value()).toInt());
ui_->doubleSpinBox_icp_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value()).toDouble());
ui_->spinBox_icp_iteration->setValue(settings.value("iterations", ui_->spinBox_icp_iteration->value()).toInt());
ui_->checkBox_icp_p2plane->setChecked(settings.value("point2place", ui_->checkBox_icp_p2plane->isChecked()).toBool());
ui_->spinBox_icp_normalKSearch->setValue(settings.value("normalKSearch", ui_->spinBox_icp_normalKSearch->value()).toInt());
ui_->checkBox_icp_2d->setChecked(settings.value("icp2d", ui_->checkBox_icp_2d->isChecked()).toBool());
ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
ui_->doubleSpinBox_icp_minCorrespondenceRatio->setValue(settings.value("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()).toDouble());
settings.endGroup();
@@ -481,6 +498,8 @@ void DatabaseViewer::writeSettings()
settings.setValue("maximized", this->isMaximized());
savedMaximized_ = this->isMaximized();
settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
// save GraphViewer settings
ui_->graphViewer->saveSettings(settings, "GraphView");
@@ -524,11 +543,13 @@ void DatabaseViewer::writeSettings()
settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
settings.setValue("voxel", ui_->doubleSpinBox_icp_voxel->value());
settings.setValue("samplingStep", ui_->spinBox_icp_downsamplingStepSize->value());
settings.setValue("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value());
settings.setValue("iterations", ui_->spinBox_icp_iteration->value());
settings.setValue("point2place", ui_->checkBox_icp_p2plane->isChecked());
settings.setValue("normalKSearch", ui_->spinBox_icp_normalKSearch->value());
settings.setValue("icp2d", ui_->checkBox_icp_2d->isChecked());
settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
settings.setValue("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value());
settings.endGroup();
@@ -1541,6 +1562,112 @@ void DatabaseViewer::view3DMap()
}
}
void DatabaseViewer::view3DLaserScans()
{
if(!ids_.size() || !dbDriver_)
{
QMessageBox::warning(this, tr("Cannot view 3D laser scans"), tr("The database is empty..."));
return;
}
if(graphes_.empty())
{
this->updateGraphView();
if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
return;
}
}
bool ok = false;
int downsamplingStepSize = QInputDialog::getInt(this, tr("Downsampling?"), tr("Downsample step size (1 = no filtering)"), 1, 1, 99999, 1, &ok);
if(ok)
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked())
{
optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
if(optimizedPoses.size() > 0)
{
rtabmap::ProgressDialog progressDialog(this);
progressDialog.setMaximumSteps((int)optimizedPoses.size());
progressDialog.show();
// create a window
QDialog * window = new QDialog(this, Qt::Window);
window->setModal(this->isModal());
window->setWindowTitle(tr("3D Laser Scans"));
window->setMinimumWidth(800);
window->setMinimumHeight(600);
rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
QVBoxLayout *layout = new QVBoxLayout();
layout->addWidget(viewer);
viewer->setCameraLockZ(false);
window->setLayout(layout);
connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
window->show();
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
rtabmap::Transform pose = iter->second;
if(!pose.isNull())
{
SensorData data;
dbDriver_->getNodeData(iter->first, data);
cv::Mat scan;
data.uncompressDataConst(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
if(downsamplingStepSize>1)
{
scan = util3d::downsample(scan, downsamplingStepSize);
}
cloud = util3d::laserScanToPointCloud(scan);
if(cloud->size())
{
QColor color = Qt::red;
int mapId, weight;
Transform odomPose;
std::string label;
double stamp;
if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp))
{
color = (Qt::GlobalColor)(mapId % 12 + 7 );
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, ui_->spinBox_icp_normalKSearch->value());
viewer->addCloud(uFormat("cloud%d", iter->first), cloudNormals, pose, color);
UINFO("Generated %d (%d points)", iter->first, cloud->size());
progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
}
else
{
UINFO("Empty cloud %d", iter->first);
progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
}
progressDialog.incrementStep();
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
}
}
}
void DatabaseViewer::generate3DMap()
{
if(!ids_.size() || !dbDriver_)
@@ -1562,7 +1689,25 @@ void DatabaseViewer::generate3DMap()
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
if(ok)
{
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
QMessageBox::StandardButton b = QMessageBox::question(
this,
tr("Assembling?"),
tr("Do you want to assemble all the point clouds (creating only one file with a density of 1pt/cm)?"),
QMessageBox::Yes|QMessageBox::No,
QMessageBox::Yes);
bool assemble = b == QMessageBox::Yes;
QString path;
if(assemble)
{
path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
pathDatabase_+QDir::separator()+"cloud.ply",
tr("Point Cloud (*.ply *.pcd)"));
}
else
{
path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
}
if(!path.isEmpty())
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
@@ -1578,6 +1723,7 @@ void DatabaseViewer::generate3DMap()
progressDialog.setMaximumSteps((int)optimizedPoses.size());
progressDialog.show();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
const rtabmap::Transform & pose = iter->second;
@@ -1589,27 +1735,66 @@ void DatabaseViewer::generate3DMap()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth);
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
if(cloud->size())
cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, assemble?0.01:0);
if(assemble)
{
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
if(assembledCloud->size() == 0)
{
*assembledCloud = *cloud;
}
else
{
*assembledCloud += *cloud;
}
}
UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
}
else
{
UINFO("Ignored empty cloud %s", name.c_str());
progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
pcl::io::savePCDFile(name, *cloud);
UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
}
else
{
UINFO("Ignored empty cloud %s", name.c_str());
progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
}
}
progressDialog.incrementStep();
QApplication::processEvents();
}
}
progressDialog.setValue(progressDialog.maximumSteps());
if(assemble && assembledCloud->size())
{
//voxelize by default to 1 cm
progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
QApplication::processEvents();
assembledCloud = util3d::voxelize(assembledCloud, 0.01);
if(QFileInfo(path).suffix() == "ply")
{
pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
}
else
{
pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
}
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
QApplication::processEvents();
}
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
progressDialog.setValue(progressDialog.maximumSteps());
}
else
{
@@ -1620,6 +1805,103 @@ void DatabaseViewer::generate3DMap()
}
}
void DatabaseViewer::generate3DLaserScans()
{
if(!ids_.size() || !dbDriver_)
{
QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
return;
}
bool ok = false;
int downsamplingStepSize = QInputDialog::getInt(this, tr("Downsampling?"), tr("Downsample step size (1 = no filtering)"), 1, 1, 99999, 1, &ok);
if(ok)
{
QString path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
pathDatabase_+QDir::separator()+"cloud.ply",
tr("Point Cloud (*.ply *.pcd)"));
if(!path.isEmpty())
{
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked())
{
optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
if(optimizedPoses.size() > 0)
{
rtabmap::ProgressDialog progressDialog;
progressDialog.setMaximumSteps((int)optimizedPoses.size());
progressDialog.show();
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
{
const rtabmap::Transform & pose = iter->second;
if(!pose.isNull())
{
SensorData data;
dbDriver_->getNodeData(iter->first, data);
cv::Mat scan;
data.uncompressDataConst(0, 0, &scan);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
if(downsamplingStepSize > 1)
{
scan = util3d::downsample(scan, downsamplingStepSize);
}
cloud = util3d::laserScanToPointCloud(scan);
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
if(assembledCloud->size() == 0)
{
*assembledCloud = *cloud;
}
else
{
*assembledCloud += *cloud;
}
}
UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
progressDialog.incrementStep();
QApplication::processEvents();
}
}
if(assembledCloud->size())
{
//voxelize by default to 1 cm
progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
QApplication::processEvents();
assembledCloud = util3d::voxelize(assembledCloud, 0.01);
if(QFileInfo(path).suffix() == "ply")
{
pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
}
else
{
pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
}
progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
QApplication::processEvents();
}
QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
progressDialog.setValue(progressDialog.maximumSteps());
}
else
{
QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
}
}
}
}
void DatabaseViewer::detectMoreLoopClosures()
{
const std::map<int, Transform> & optimizedPoses = graphes_.back();
@@ -2080,6 +2362,14 @@ void DatabaseViewer::update(int value,
}
}
void DatabaseViewer::updateLoggerLevel()
{
if(this->parent() == 0)
{
ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
}
}
void DatabaseViewer::updateStereo()
{
if(ui_->horizontalSlider_A->maximum())
@@ -2417,6 +2707,19 @@ void DatabaseViewer::updateConstraintView(
{
link = iterLink->second;
}
else if(ui_->checkBox_ignorePoseCorrection->isChecked())
{
if(link.type() == Link::kNeighbor ||
link.type() == Link::kNeighborMerged)
{
Transform poseFrom = uValue(poses_, link.from(), Transform());
Transform poseTo = uValue(poses_, link.to(), Transform());
if(!poseFrom.isNull() && !poseTo.isNull())
{
link.setTransform(poseFrom.inverse() * poseTo); // recompute raw odom transformation
}
}
}
rtabmap::Transform t = link.transform();
ui_->label_constraint->clear();
@@ -2426,7 +2729,7 @@ void DatabaseViewer::updateConstraintView(
ui_->label_type->setText(tr("%1 (%2)")
.arg(link.type())
.arg(link.type()==Link::kNeighbor?"Neigbor":
.arg(link.type()==Link::kNeighbor?"Neighbor":
link.type()==Link::kNeighbor?"Merged neighbor":
link.type()==Link::kGlobalClosure?"Loop closure":
link.type()==Link::kLocalSpaceClosure?"Space proximity link":
@@ -2964,15 +3267,18 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat laserScan;
data.uncompressDataConst(0, 0, &laserScan);
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(
laserScan,
ground,
obstacles,
ui_->doubleSpinBox_gridCellSize->value(),
ui_->checkBox_gridFillUnkownSpace->isChecked(),
data.laserScanMaxRange());
if(laserScan.type() == CV_32FC2)
{
util3d::occupancy2DFromLaserScan(
laserScan,
ground,
obstacles,
ui_->doubleSpinBox_gridCellSize->value(),
ui_->checkBox_gridFillUnkownSpace->isChecked(),
data.laserScanMaxRange());
added = true;
}
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
added = true;
}
}
if(added)
@@ -3345,207 +3651,104 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
}
}
}
else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
graph::findLink(linksRefined_, from, to) == linksRefined_.end())
{
if(currentLink.type() == Link::kNeighbor ||
currentLink.type() == Link::kNeighborMerged)
{
Transform poseFrom = uValue(poses_, currentLink.from(), Transform());
Transform poseTo = uValue(poses_, currentLink.to(), Transform());
if(!poseFrom.isNull() && !poseTo.isNull())
{
t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
}
}
}
bool hasConverged = false;
double variance = -1.0;
int correspondences = 0;
float variance = -1.0f;
Transform transform;
SensorData dataFrom, dataTo;
dbDriver_->getNodeData(currentLink.from(), dataFrom);
dbDriver_->getNodeData(currentLink.to(), dataTo);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanAVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanBVoxelized(new pcl::PointCloud<pcl::PointXYZ>);
float correspondenceRatio = 0.0f;
if(ui_->checkBox_icp_2d->isChecked())
UTimer timer;
if(!ui_->checkBox_icp_laserScan->isChecked())
{
//2D
cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.laserScanCompressed());
cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.laserScanCompressed());
if(!oldLaserScan.empty() && !newLaserScan.empty())
{
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
scanA = util3d::cvMat2Cloud(oldLaserScan);
scanB = util3d::cvMat2Cloud(newLaserScan, t);
//voxelize
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
{
scanA = util3d::voxelize(scanA, ui_->doubleSpinBox_icp_voxel->value());
scanB = util3d::voxelize(scanB, ui_->doubleSpinBox_icp_voxel->value());
}
else
{
scanAVoxelized = scanA;
scanBVoxelized = scanB;
}
if(scanB->size() && scanA->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
transform = util3d::icp2D(
scanB,
scanA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
*scanBRegistered);
if(!transform.isNull())
{
if(dataTo.laserScanMaxPts())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanBTransformed = scanBRegistered;
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
{
scanBTransformed = util3d::transformPointCloud(scanB, transform);
}
util3d::computeVarianceAndCorrespondences(
scanBTransformed,
scanA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
variance,
correspondences);
correspondenceRatio = float(correspondences)/float(dataTo.laserScanMaxPts());
}
else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
{
UWARN("Laser scan max pts not set, but correspondence ratio is set!");
}
}
}
}
// generate laser scans from depth image
cv::Mat tmpA, tmpB, tmpC, tmpD;
dataFrom.uncompressData(&tmpA, &tmpB, 0);
dataTo.uncompressData(&tmpC, &tmpD, 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
dataFrom,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
dataTo,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value());
int maxLaserScans = cloudFrom->size();
dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
}
else
{
//3D
cv::Mat im,de;
dataFrom.uncompressData(&im, &de, 0);
dataTo.uncompressData(&im, &de, 0);
cloudA = util3d::cloudFromSensorData(dataFrom,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value());
cloudB = util3d::cloudFromSensorData(dataTo,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value());
if(cloudA->size() && cloudB->size())
{
cloudB = util3d::transformPointCloud(cloudB, t);
if(ui_->checkBox_icp_p2plane->isChecked())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
transform = util3d::icpPointToPlane(
cloudBNormals,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
variance,
correspondences);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
transform = util3d::icp(cloudB,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
variance,
correspondences);
}
correspondenceRatio = float(correspondences)/float(cloudA->size()>cloudB->size()?cloudA->size():cloudB->size());
}
else
{
UWARN("No cloud generated!");
}
cv::Mat tmpA, tmpB;
dataFrom.uncompressData(0, 0, &tmpA);
dataTo.uncompressData(0, 0, &tmpB);
}
UINFO("Uncompress time: %f s", timer.ticks());
if(hasConverged && !transform.isNull())
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kIcpDownsamplingStep(), uNumber2Str(ui_->spinBox_icp_downsamplingStepSize->value())));
parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), uNumber2Str(ui_->doubleSpinBox_icp_voxel->value())));
parameters.insert(ParametersPair(Parameters::kIcp2D(), uBool2Str(ui_->checkBox_icp_2d->isChecked())));
parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), uBool2Str(ui_->checkBox_icp_p2plane->isChecked())));
parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneNormalNeighbors(), uNumber2Str(ui_->spinBox_icp_normalKSearch->value())));
parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(ui_->doubleSpinBox_icp_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kIcpIterations(), uNumber2Str(ui_->spinBox_icp_iteration->value())));
parameters.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), uNumber2Str(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())));
RegistrationIcp registration(parameters);
transform = registration.computeTransformation(dataFrom, dataTo, t, 0, 0, &variance);
UINFO("Icp time: %f s", timer.ticks());
if(!transform.isNull())
{
if(correspondenceRatio < ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, variance, variance);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
while(iter != linksRefined_.end() && iter->first == currentLink.from())
{
if(!silent)
if(iter->second.to() == currentLink.to() &&
iter->second.type() == currentLink.type())
{
QMessageBox::warning(this,
tr("Refine link"),
tr("Cannot find a transformation between nodes %1 and %2, correspondence ratio too low (%3).")
.arg(from).arg(to).arg(correspondenceRatio));
iter->second = newLink;
updated = true;
break;
}
++iter;
}
if(!updated)
{
linksRefined_.insert(std::make_pair(newLink.from(), newLink));
if(updateGraph)
{
this->updateGraphView();
}
}
else
if(ui_->dockWidget_constraints->isVisible())
{
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
while(iter != linksRefined_.end() && iter->first == currentLink.from())
{
if(iter->second.to() == currentLink.to() &&
iter->second.type() == currentLink.type())
{
iter->second = newLink;
updated = true;
break;
}
++iter;
}
if(!updated)
{
linksRefined_.insert(std::make_pair(newLink.from(), newLink));
if(updateGraph)
{
this->updateGraphView();
}
}
if(ui_->dockWidget_constraints->isVisible())
{
cloudB = util3d::transformPointCloud(cloudB, transform);
scanBVoxelized = util3d::transformPointCloud(scanBVoxelized, transform);
this->updateConstraintView(newLink, true, cloudA, cloudB, scanAVoxelized, scanBVoxelized);
}
this->updateConstraintView(newLink, true);
}
}
else if(!silent)
{
QMessageBox::warning(this,
@@ -3576,30 +3779,31 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
return;
}
// create a fake memory to compute transform
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kVisInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
parameters.insert(ParametersPair(Parameters::kLccBowVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
parameters.insert(ParametersPair(Parameters::kVisIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kVisMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kVisPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
parameters.insert(ParametersPair(Parameters::kVisForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
parameters.insert(ParametersPair(Parameters::kRegVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
Memory tmpMemory(parameters);
Transform t;
std::string rejectedMsg;
double variance = -1.0;
float variance = -1.0f;
int inliers = -1;
if(ui_->groupBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to compute transform
Memory tmpMemory(parameters);
// Add sensor data to generate features
SensorData dataFrom;
dbDriver_->getNodeData(from, dataFrom);
@@ -3620,7 +3824,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
}
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
}
else
{
@@ -3632,7 +3836,8 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
if(signatures.size() == 2)
{
t = tmpMemory.computeVisualTransform(*signatures.front(), *signatures.back(), &rejectedMsg, &inliers, &variance);
RegistrationVis registration(parameters);
t = registration.computeTransformation(*signatures.back(), *signatures.front(), Transform(), &rejectedMsg, &inliers, &variance);
}
//cleanup
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
@@ -3702,30 +3907,31 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
UASSERT(!containsLink(linksRemoved_, from, to));
UASSERT(!containsLink(linksRefined_, from, to));
// create a fake memory to compute the transform
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kVisInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kLccBowEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
parameters.insert(ParametersPair(Parameters::kLccBowForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
parameters.insert(ParametersPair(Parameters::kLccBowVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
parameters.insert(ParametersPair(Parameters::kVisIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
parameters.insert(ParametersPair(Parameters::kVisMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
parameters.insert(ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(ui_->comboBox_estimationType->currentIndex())));
parameters.insert(ParametersPair(Parameters::kVisPnPFlags(), uNumber2Str(ui_->comboBox_pnpFlags->currentIndex())));
parameters.insert(ParametersPair(Parameters::kVisForce2D(), uBool2Str(ui_->checkBox_visual_2d->isChecked())));
parameters.insert(ParametersPair(Parameters::kRegVarianceFromInliersCount(), uBool2Str(ui_->checkBox_visual_var_inliers->isChecked())));
parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
Memory tmpMemory(parameters);
Transform t;
std::string rejectedMsg;
double variance = -1.0;
float variance = -1.0f;
int inliers = -1;
if(ui_->groupBox_visual_recomputeFeatures->isChecked())
{
// create a fake memory to compute the transform
Memory tmpMemory(parameters);
// Add sensor data to generate features
SensorData dataFrom;
dbDriver_->getNodeData(from, dataFrom);
@@ -3746,7 +3952,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
}
t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
t = tmpMemory.computeVisualTransform(from, to, &rejectedMsg, &inliers, &variance);
if(!silent)
{
@@ -3765,7 +3971,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
if(signatures.size() == 2)
{
t = tmpMemory.computeVisualTransform(*signatures.front(), *signatures.back(), &rejectedMsg, &inliers, &variance);
RegistrationVis registration(parameters);
t = registration.computeTransformation(*signatures.back(), *signatures.front(), Transform(), &rejectedMsg, &inliers, &variance);
}
//cleanup
for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
guilib/src/MainWindow.cpp
+56 -219
View File
@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/DBDriver.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/gui/ImageView.h"
#include "rtabmap/gui/KeypointItem.h"
@@ -94,6 +95,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/RegistrationIcp.h"
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
@@ -481,9 +483,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
// update loop closure viewer parameters
ParametersMap parameters = _preferencesDialog->getAllParameters();
_ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
_ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
_ui->widget_loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
_ui->widget_loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
//update ui
_ui->doubleSpinBox_stats_detectionRate->setValue(_preferencesDialog->getDetectionRate());
@@ -810,21 +811,22 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
_preferencesDialog->isScansShown(1))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
cloud = util3d::transformPointCloud(cloud, pose);
cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw(), pose);
if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
{
cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(1));
}
if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
{
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(odom.data().laserScanRaw());
cloud = util3d::transformPointCloud(cloud, pose);
if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
{
UERROR("Adding scanOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
UERROR("Adding scanOdom to viewer failed!");
}
_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
}
}
}
@@ -1929,6 +1931,14 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::laserScanToPointCloud(depth2D);
if(_preferencesDialog->getDownsamplingStepScan(0) > 0)
{
cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(0));
}
if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
{
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
}
QColor color = Qt::gray;
if(mapId >= 0)
{
@@ -1942,9 +1952,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
{
_createdScans.insert(std::make_pair(nodeId, cloud));
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
if(depth2D.channels() == 2)
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
}
}
_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
_ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
@@ -2376,19 +2389,9 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bo
_ui->widget_cloudViewer->setWorkingDirectory(_preferencesDialog->getWorkingDirectory());
}
// update loop closure viewer parameters
if(uContains(parameters, Parameters::kLccIcp3Decimation()))
{
_ui->widget_loopClosureViewer->setDecimation(atoi(parameters.at(Parameters::kLccIcp3Decimation()).c_str()));
}
if(uContains(parameters, Parameters::kLccIcp3MaxDepth()))
{
_ui->widget_loopClosureViewer->setMaxDepth(uStr2Float(parameters.at(Parameters::kLccIcp3MaxDepth())));
}
if(uContains(parameters, Parameters::kLccIcp3Samples()))
{
_ui->widget_loopClosureViewer->setSamples(atoi(parameters.at(Parameters::kLccIcp3Samples()).c_str()));
}
// update loop closure viewer parameters (Use Map parameters)
_ui->widget_loopClosureViewer->setDecimation(_preferencesDialog->getCloudDecimation(0));
_ui->widget_loopClosureViewer->setMaxDepth(_preferencesDialog->getCloudMaxDepth(0));
// update graph view parameters
if(uContains(parameters, Parameters::kRGBDLocalRadius()))
@@ -3370,31 +3373,6 @@ void MainWindow::postProcessing()
{
odomPoses.insert(*iter); // fill raw poses
}
/*
if(jter->sensorData().cameraModels().size() == 0 && !jter->sensorData().stereoCameraModel().isValid())
{
UWARN("Calibration of %d is null.", iter->first);
allDataAvailable = false;
}
if(refineNeighborLinks || refineLoopClosureLinks || reextractFeatures)
{
// depth data required
if(jter->sensorData().depthOrRightCompressed().empty())
{
UWARN("Depth data of %d missing.", iter->first);
allDataAvailable = false;
}
if(reextractFeatures)
{
// rgb required
if(jter->sensorData().imageCompressed().empty())
{
UWARN("Rgb of %d missing.", iter->first);
allDataAvailable = false;
}
}
}*/
}
else
{
@@ -3442,22 +3420,6 @@ void MainWindow::postProcessing()
if(detectMoreLoopClosures)
{
UDEBUG("");
Memory memory(parameters);
if(reextractFeatures)
{
ParametersMap customParameters;
// override some parameters
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kLccReextractNNType())));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kLccReextractNNDR())));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kLccReextractFeatureType())));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kLccReextractMaxWords())));
customParameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
memory.parseParameters(customParameters);
}
UASSERT(detectLoopClosureIterations>0);
for(int n=0; n<detectLoopClosureIterations; ++n)
@@ -3502,52 +3464,24 @@ void MainWindow::postProcessing()
_initProgressDialog->incrementStep();
QApplication::processEvents();
Signature & signatureFrom = _cachedSignatures[from];
Signature & signatureTo = _cachedSignatures[to];
Signature signatureFrom = _cachedSignatures[from];
Signature signatureTo = _cachedSignatures[to];
if(reextractFeatures)
{
signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
signatureFrom.setWords3(std::multimap<int, pcl::PointXYZ>());
signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
signatureTo.setWords3(std::multimap<int, pcl::PointXYZ>());
}
Transform transform;
std::string rejectedMsg;
int inliers = -1;
double variance = -1.0;
if(reextractFeatures)
{
memory.init("", true); // clear previously added signatures
float variance = -1.0f;
RegistrationVis registration(parameters);
transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &rejectedMsg, &inliers, &variance);
// Add signatures
SensorData dataFrom = signatureFrom.sensorData();
SensorData dataTo = signatureTo.sensorData();
cv::Mat image, depth;
dataFrom.uncompressData(&image, &depth, 0);
dataTo.uncompressData(&image, &depth, 0);
if(dataFrom.isValid() &&
dataTo.isValid() &&
dataFrom.id() != Memory::kIdInvalid &&
signatureFrom.id() != Memory::kIdInvalid)
{
if(from > to)
{
memory.update(dataTo);
memory.update(dataFrom);
}
else
{
memory.update(dataFrom);
memory.update(dataTo);
}
transform = memory.computeVisualTransform(dataTo.id(), dataFrom.id(), &rejectedMsg, &inliers, &variance);
}
else
{
UERROR("not supposed to be here!");
}
}
else
{
transform = memory.computeVisualTransform(signatureTo, signatureFrom, &rejectedMsg, &inliers, &variance);
}
if(!transform.isNull())
{
UINFO("Added new loop closure between %d and %d.", from, to);
@@ -3597,26 +3531,10 @@ void MainWindow::postProcessing()
{
_initProgressDialog->setMaximumSteps(_initProgressDialog->maximumSteps()+loopClosuresAdded);
}
// TODO: support ICP from laser scans?
_initProgressDialog->appendText(tr("Refining links..."));
int decimation=Parameters::defaultLccIcp3Decimation();
float maxDepth=Parameters::defaultLccIcp3MaxDepth();
float voxelSize=Parameters::defaultLccIcp3VoxelSize();
int samples = Parameters::defaultLccIcp3Samples();
float maxCorrespondenceDistance = Parameters::defaultLccIcp3MaxCorrespondenceDistance();
float correspondenceRatio = Parameters::defaultLccIcp3CorrespondenceRatio();
float icpIterations = Parameters::defaultLccIcp3Iterations();
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), decimation);
Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), voxelSize);
Parameters::parse(parameters, Parameters::kLccIcp3Samples(), samples);
Parameters::parse(parameters, Parameters::kLccIcp3CorrespondenceRatio(), correspondenceRatio);
Parameters::parse(parameters, Parameters::kLccIcp3MaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters, Parameters::kLccIcp3Iterations(), icpIterations);
bool pointToPlane = Parameters::defaultLccIcp3PointToPlane();
int pointToPlaneNormalNeighbors = Parameters::defaultLccIcp3PointToPlaneNormalNeighbors();
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlane(), pointToPlane);
Parameters::parse(parameters, Parameters::kLccIcp3PointToPlaneNormalNeighbors(), pointToPlaneNormalNeighbors);
RegistrationIcp regIcp(parameters);
int i=0;
for(std::multimap<int, Link>::iterator iter = _currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter, ++i)
@@ -3646,102 +3564,21 @@ void MainWindow::postProcessing()
Signature & signatureFrom = _cachedSignatures[from];
Signature & signatureTo = _cachedSignatures[to];
//3D
UDEBUG("");
cv::Mat depthA, depthB;
if(signatureFrom.sensorData().stereoCameraModel().isValid())
if(!signatureFrom.sensorData().laserScanRaw().empty() &&
!signatureTo.sensorData().laserScanRaw().empty())
{
cv::Mat leftA, leftB;
signatureFrom.sensorData().uncompressData(&leftA, &depthA, 0);
signatureTo.sensorData().uncompressData(&leftB, &depthB, 0);
}
else
{
signatureFrom.sensorData().uncompressData(0, &depthA, 0);
signatureTo.sensorData().uncompressData(0, &depthB, 0);
}
std::string rejectedMsg;
float variance = -1.0f;
Transform transform = regIcp.computeTransformation(signatureFrom, signatureTo, iter->second.transform(), &rejectedMsg, 0, &variance);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::cloudFromSensorData(
signatureFrom.sensorData(),
decimation,
maxDepth,
voxelSize,
samples);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::cloudFromSensorData(
signatureTo.sensorData(),
decimation,
maxDepth,
voxelSize,
samples);
if(cloudA->size() && cloudB->size())
{
cloudB = util3d::transformPointCloud(cloudB, iter->second.transform());
bool hasConverged = false;
double variance = -1;
int correspondences = 0;
Transform transform;
if(pointToPlane)
{
UDEBUG("");
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, pointToPlaneNormalNeighbors);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, pointToPlaneNormalNeighbors);
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointNormal>);
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
maxCorrespondenceDistance,
icpIterations,
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudANormals,
maxCorrespondenceDistance,
variance,
correspondences);
}
else
{
UDEBUG("");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudBRegistered(new pcl::PointCloud<pcl::PointXYZ>);
transform = util3d::icp(cloudB,
cloudA,
maxCorrespondenceDistance,
icpIterations,
hasConverged,
*cloudBRegistered);
util3d::computeVarianceAndCorrespondences(
cloudBRegistered,
cloudA,
maxCorrespondenceDistance,
variance,
correspondences);
}
float correspondencesRatio = float(correspondences)/float(cloudB->size()>cloudA->size()?cloudB->size():cloudA->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= correspondenceRatio)
if(!transform.isNull())
{
Link newLink(from, to, iter->second.type(), transform*iter->second.transform(), variance, variance);
iter->second = newLink;
}
else
{
QString str = tr("Cannot refine link %1->%2 (converged=%3 variance=%4 correspondencesRatio=%5 (ref=%6))").arg(from).arg(to).arg(hasConverged?"true":"false").arg(variance).arg(correspondencesRatio).arg(correspondenceRatio);
QString str = tr("Cannot refine link %1->%2 (%3").arg(from).arg(to).arg(rejectedMsg.c_str());
_initProgressDialog->appendText(str, Qt::darkYellow);
UWARN("%s", str.toStdString().c_str());
warn = true;
guilib/src/PreferencesDialog.cpp
+197 -183
View File
@@ -129,7 +129,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// remove BruteForceGPU option
_ui->comboBox_dictionary_strategy->removeItem(4);
_ui->odom_bin_nn->removeItem(4);
_ui->reextract_nn->removeItem(4);
}
@@ -145,12 +144,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
_ui->reextract_type->setItemData(0, 0, Qt::UserRole - 1);
_ui->reextract_type->setItemData(1, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(0, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(1, 0, Qt::UserRole - 1);
_ui->comboBox_dictionary_strategy->setItemData(1, 0, Qt::UserRole - 1);
_ui->reextract_nn->setItemData(1, 0, Qt::UserRole - 1);
_ui->odom_bin_nn->setItemData(1, 0, Qt::UserRole - 1);
#if CV_MAJOR_VERSION == 3
_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
@@ -165,12 +161,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->reextract_type->setItemData(4, 0, Qt::UserRole - 1);
_ui->reextract_type->setItemData(5, 0, Qt::UserRole - 1);
_ui->reextract_type->setItemData(6, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(0, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(1, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(3, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(4, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(5, 0, Qt::UserRole - 1);
_ui->odom_type->setItemData(6, 0, Qt::UserRole - 1);
#endif
}
if(!graph::G2OOptimizer::available())
@@ -279,6 +269,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_3dRenderingShowScans[0] = _ui->checkBox_showScans;
_3dRenderingShowScans[1] = _ui->checkBox_showOdomScans;
_3dRenderingDownsamplingScan.resize(2);
_3dRenderingDownsamplingScan[0] = _ui->spinBox_downsamplingScan;
_3dRenderingDownsamplingScan[1] = _ui->spinBox_downsamplingScan_odom;
_3dRenderingVoxelSizeScan.resize(2);
_3dRenderingVoxelSizeScan[0] = _ui->doubleSpinBox_voxelSizeScan;
_3dRenderingVoxelSizeScan[1] = _ui->doubleSpinBox_voxelSizeScan_odom;
_3dRenderingOpacityScan.resize(2);
_3dRenderingOpacityScan[0] = _ui->doubleSpinBox_opacity_scan;
_3dRenderingOpacityScan[1] = _ui->doubleSpinBox_opacity_odom_scan;
@@ -295,6 +293,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_3dRenderingMaxDepth[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_3dRenderingShowScans[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_3dRenderingDownsamplingScan[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_3dRenderingVoxelSizeScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_3dRenderingOpacity[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_3dRenderingPtSize[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
connect(_3dRenderingOpacityScan[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -333,7 +333,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Source panel
connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_calibrationName, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_source_path_calibration, SIGNAL(clicked()), this, SLOT(selectCalibrationPath()));
connect(_ui->lineEdit_calibrationFile, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
_ui->stackedWidget_src->setCurrentIndex(_ui->comboBox_sourceType->currentIndex());
connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_src, SLOT(setCurrentIndex(int)));
connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -398,8 +399,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->lineEdit_cameraStereoImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->toolButton_cameraStereoImages_path_left, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathLeft()));
connect(_ui->toolButton_cameraStereoImages_path_right, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathRight()));
connect(_ui->toolButton_cameraStereoImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceStereoImagesPathScans()));
connect(_ui->lineEdit_cameraStereoImages_path_left, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraStereoImages_path_right, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraStereoImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraStereoImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraStereoImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_stereoImages_timestamps, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_stereoImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -476,7 +481,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_localSpaceLinksKeptInWM->setObjectName(Parameters::kMemLocalSpaceLinksKeptInWM().c_str());
_ui->checkBox_localSpaceScanMatchingIDsSaved->setObjectName(Parameters::kRGBDScanMatchingIdsSavedInLinks().c_str());
_ui->spinBox_imageDecimation->setObjectName(Parameters::kMemImageDecimation().c_str());
_ui->general_doubleSpinBox_laserScanVoxel->setObjectName(Parameters::kMemLaserScanVoxelSize().c_str());
_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
// Database
@@ -503,6 +508,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpWordsPerImage().c_str());
_ui->surf_doubleSpinBox_ratioBadSign->setObjectName(Parameters::kKpBadSignRatio().c_str());
_ui->checkBox_kp_tfIdfLikelihoodUsed->setObjectName(Parameters::kKpTfIdfLikelihoodUsed().c_str());
@@ -511,6 +517,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->lineEdit_dictionaryPath->setObjectName(Parameters::kKpDictionaryPath().c_str());
connect(_ui->toolButton_dictionaryPath, SIGNAL(clicked()), this, SLOT(changeDictionaryPath()));
_ui->checkBox_kp_newWordsComparedTogether->setObjectName(Parameters::kKpNewWordsComparedTogether().c_str());
_ui->subpix_winSize_kp->setObjectName(Parameters::kKpSubPixWinSize().c_str());
_ui->subpix_iterations_kp->setObjectName(Parameters::kKpSubPixIterations().c_str());
_ui->subpix_eps_kp->setObjectName(Parameters::kKpSubPixEps().c_str());
_ui->subpix_winSize->setObjectName(Parameters::kKpSubPixWinSize().c_str());
_ui->subpix_iterations->setObjectName(Parameters::kKpSubPixIterations().c_str());
@@ -581,7 +590,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->rgdb_angularUpdate->setObjectName(Parameters::kRGBDAngularUpdate().c_str());
_ui->rgdb_rehearsalWeightIgnoredWhileMoving->setObjectName(Parameters::kMemRehearsalWeightIgnoredWhileMoving().c_str());
_ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDPoseScanMatching().c_str());
_ui->odomScanHistory->setObjectName(Parameters::kRGBDIcpOdomRefining().c_str());
_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());
_ui->graphOptimization_type->setObjectName(Parameters::kRGBDOptimizeStrategy().c_str());
@@ -605,76 +614,57 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxGraphDepth().c_str());
_ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDLocalLoopDetectionPathFilteringRadius().c_str());
_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed().c_str());
_ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDLocalLoopDetectionPathScansMerged().c_str());
_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kLccBowMinInliers().c_str());
_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kLccBowInlierDistance().c_str());
_ui->loopClosure_bowIterations->setObjectName(Parameters::kLccBowIterations().c_str());
_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kLccBowRefineIterations().c_str());
_ui->loopClosure_bowForce2D->setObjectName(Parameters::kLccBowForce2D().c_str());
_ui->loopClosure_estimationType->setObjectName(Parameters::kLccBowEstimationType().c_str());
_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kVisMinInliers().c_str());
_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kVisInlierDistance().c_str());
_ui->loopClosure_bowIterations->setObjectName(Parameters::kVisIterations().c_str());
_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kVisRefineIterations().c_str());
_ui->loopClosure_bowForce2D->setObjectName(Parameters::kVisForce2D().c_str());
_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultLccBowEstimationType());
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kLccBowEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kLccBowPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kLccBowPnPFlags().c_str());
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kLccBowVarianceFromInliersCount().c_str());
_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
_ui->groupBox_reextract->setObjectName(Parameters::kLccReextractActivated().c_str());
_ui->reextract_nn->setObjectName(Parameters::kLccReextractNNType().c_str());
_ui->reextract_nndrRatio->setObjectName(Parameters::kLccReextractNNDR().c_str());
_ui->reextract_type->setObjectName(Parameters::kLccReextractFeatureType().c_str());
_ui->reextract_maxFeatures->setObjectName(Parameters::kLccReextractMaxWords().c_str());
_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kLccReextractMaxDepth().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
_ui->reextract_nn->setObjectName(Parameters::kVisNNType().c_str());
_ui->reextract_nndrRatio->setObjectName(Parameters::kVisNNDR().c_str());
_ui->reextract_type->setObjectName(Parameters::kVisFeatureType().c_str());
_ui->reextract_maxFeatures->setObjectName(Parameters::kVisMaxFeatures().c_str());
_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
_ui->loopClosure_bowMinDepth->setObjectName(Parameters::kVisMinDepth().c_str());
_ui->loopClosure_roi->setObjectName(Parameters::kVisRoiRatios().c_str());
_ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str());
_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
_ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str());
_ui->globalDetection_icpType->setObjectName(Parameters::kLccIcpType().c_str());
_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kLccIcpMaxTranslation().c_str());
_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kLccIcpMaxRotation().c_str());
_ui->loopClosure_icp->setObjectName(Parameters::kRGBDIcpLoopClosureRefining().c_str());
_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
_ui->loopClosure_icp2D->setObjectName(Parameters::kIcp2D().c_str());
_ui->loopClosure_icpDecimation->setObjectName(Parameters::kLccIcp3Decimation().c_str());
_ui->loopClosure_icpMaxDepth->setObjectName(Parameters::kLccIcp3MaxDepth().c_str());
_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kLccIcp3VoxelSize().c_str());
_ui->loopClosure_icpSamples->setObjectName(Parameters::kLccIcp3Samples().c_str());
_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp3MaxCorrespondenceDistance().c_str());
_ui->loopClosure_icpIterations->setObjectName(Parameters::kLccIcp3Iterations().c_str());
_ui->loopClosure_icpRatio->setObjectName(Parameters::kLccIcp3CorrespondenceRatio().c_str());
_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kLccIcp3PointToPlane().c_str());
_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kLccIcp3PointToPlaneNormalNeighbors().c_str());
_ui->loopClosure_icp2MaxCorrespondenceDistance->setObjectName(Parameters::kLccIcp2MaxCorrespondenceDistance().c_str());
_ui->loopClosure_icp2Iterations->setObjectName(Parameters::kLccIcp2Iterations().c_str());
_ui->loopClosure_icp2Ratio->setObjectName(Parameters::kLccIcp2CorrespondenceRatio().c_str());
_ui->loopClosure_icp2Voxel->setObjectName(Parameters::kLccIcp2VoxelSize().c_str());
_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str());
_ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str());
_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str());
_ui->loopClosure_icpIterations->setObjectName(Parameters::kIcpIterations().c_str());
_ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str());
_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str());
//Odometry
_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
_ui->odom_type->setObjectName(Parameters::kOdomFeatureType().c_str());
_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
_ui->odom_maxFeatures->setObjectName(Parameters::kOdomMaxFeatures().c_str());
_ui->odom_inlierDistance->setObjectName(Parameters::kOdomInlierDistance().c_str());
_ui->odom_iterations->setObjectName(Parameters::kOdomIterations().c_str());
_ui->odom_maxDepth->setObjectName(Parameters::kOdomMaxDepth().c_str());
_ui->odom_minInliers->setObjectName(Parameters::kOdomMinInliers().c_str());
_ui->odom_refine_iterations->setObjectName(Parameters::kOdomRefineIterations().c_str());
_ui->odom_force2D->setObjectName(Parameters::kOdomForce2D().c_str());
_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
_ui->odom_varianceFromInliersCount->setObjectName(Parameters::kOdomVarianceFromInliersCount().c_str());
connect(_ui->odom_varianceFromInliersCount, SIGNAL(clicked(bool)), _ui->loopClosure_bowVarianceFromInliersCount, SLOT(setChecked(bool)));
connect(_ui->loopClosure_bowVarianceFromInliersCount, SIGNAL(clicked(bool)), _ui->odom_varianceFromInliersCount, SLOT(setChecked(bool)));
_ui->lineEdit_odom_roi->setObjectName(Parameters::kOdomRoiRatios().c_str());
_ui->odom_estimationType->setObjectName(Parameters::kOdomEstimationType().c_str());
connect(_ui->odom_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odomEstimation, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_odomEstimation->setCurrentIndex(Parameters::defaultOdomEstimationType());
_ui->odom_pnpReprojError->setObjectName(Parameters::kOdomPnPReprojError().c_str());
_ui->odom_pnpFlags->setObjectName(Parameters::kOdomPnPFlags().c_str());
//Odometry BOW
_ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str());
_ui->odom_bin_nn->setObjectName(Parameters::kOdomBowNNType().c_str());
_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomBowNNDR().c_str());
_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomBowFixedLocalMapPath().c_str());
connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
@@ -683,9 +673,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_flow_maxLevel->setObjectName(Parameters::kOdomFlowMaxLevel().c_str());
_ui->odom_flow_iterations->setObjectName(Parameters::kOdomFlowIterations().c_str());
_ui->odom_flow_eps->setObjectName(Parameters::kOdomFlowEps().c_str());
_ui->odom_subpix_winSize->setObjectName(Parameters::kOdomSubPixWinSize().c_str());
_ui->odom_subpix_iterations->setObjectName(Parameters::kOdomSubPixIterations().c_str());
_ui->odom_subpix_eps->setObjectName(Parameters::kOdomSubPixEps().c_str());
//Odometry Mono
_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
@@ -737,6 +724,7 @@ PreferencesDialog::~PreferencesDialog() {
void PreferencesDialog::init()
{
UDEBUG("");
//First set all default values
const ParametersMap & defaults = Parameters::getDefaultParameters();
for(ParametersMap::const_iterator iter=defaults.begin(); iter!=defaults.end(); ++iter)
@@ -1048,6 +1036,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_3dRenderingMaxDepth[i]->setValue(0.0);
_3dRenderingShowScans[i]->setChecked(true);
_3dRenderingDownsamplingScan[i]->setValue(0);
_3dRenderingVoxelSizeScan[i]->setValue(0.0);
_3dRenderingOpacity[i]->setValue(i==0?1.0:0.5);
_3dRenderingPtSize[i]->setValue(2);
_3dRenderingOpacityScan[i]->setValue(i==0?1.0:0.5);
@@ -1088,10 +1078,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
{
_ui->general_doubleSpinBox_imgRate->setValue(0.0);
_ui->source_mirroring->setChecked(false);
_ui->lineEdit_calibrationName->clear();
_ui->lineEdit_calibrationFile->clear();
_ui->comboBox_sourceType->setCurrentIndex(kSrcRGBD);
_ui->lineEdit_sourceDevice->setText("");
_ui->lineEdit_sourceLocalTransform->setText("0 0 0 -PI_2 0 -PI_2");
_ui->lineEdit_sourceLocalTransform->setText("0 0 1 -1 0 0 0 -1 0");
_ui->source_comboBox_image_type->setCurrentIndex(kSrcUsbDevice-kSrcUsbDevice);
_ui->source_images_spinBox_startPos->setValue(1);
@@ -1160,6 +1150,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->lineEdit_cameraStereoImages_timestamps->setText("");
_ui->lineEdit_cameraStereoImages_path_left->setText("");
_ui->lineEdit_cameraStereoImages_path_right->setText("");
_ui->lineEdit_cameraStereoImages_path_scans->setText("");
_ui->lineEdit_cameraStereoImages_laser_transform->setText("0 0 0 0 0 0");
_ui->spinBox_cameraStereoImages_max_scan_pts->setValue(0);
_ui->checkBox_stereoImages_timestamps->setChecked(false);
_ui->checkBox_stereoImages_rectify->setChecked(false);
_ui->lineEdit_cameraStereoVideo_path->setText("");
@@ -1280,7 +1273,7 @@ void PreferencesDialog::loadConfigFrom()
void PreferencesDialog::readSettings(const QString & filePath)
{
ULOGGER_DEBUG("");
ULOGGER_DEBUG("%s", filePath.toStdString().c_str());
readGuiSettings(filePath);
readCameraSettings(filePath);
if(!readCoreSettings(filePath))
@@ -1348,6 +1341,8 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_3dRenderingMaxDepth[i]->setValue(settings.value(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value()).toDouble());
_3dRenderingShowScans[i]->setChecked(settings.value(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked()).toBool());
_3dRenderingDownsamplingScan[i]->setValue(settings.value(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value()).toInt());
_3dRenderingVoxelSizeScan[i]->setValue(settings.value(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value()).toDouble());
_3dRenderingOpacity[i]->setValue(settings.value(QString("opacity%1").arg(i), _3dRenderingOpacity[i]->value()).toDouble());
_3dRenderingPtSize[i]->setValue(settings.value(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value()).toInt());
_3dRenderingOpacityScan[i]->setValue(settings.value(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value()).toDouble());
@@ -1392,7 +1387,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
settings.beginGroup("Camera");
_ui->general_doubleSpinBox_imgRate->setValue(settings.value("imgRate", _ui->general_doubleSpinBox_imgRate->value()).toDouble());
_ui->source_mirroring->setChecked(settings.value("mirroring", _ui->source_mirroring->isChecked()).toBool());
_ui->lineEdit_calibrationName->setText(settings.value("calibrationName", _ui->lineEdit_calibrationName->text()).toString());
_ui->lineEdit_calibrationFile->setText(settings.value("calibrationName", _ui->lineEdit_calibrationFile->text()).toString());
_ui->comboBox_sourceType->setCurrentIndex(settings.value("type", _ui->comboBox_sourceType->currentIndex()).toInt());
_ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString());
_ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString());
@@ -1446,6 +1441,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->lineEdit_cameraStereoImages_timestamps->setText(settings.value("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text()).toString());
_ui->lineEdit_cameraStereoImages_path_left->setText(settings.value("path_left", _ui->lineEdit_cameraStereoImages_path_left->text()).toString());
_ui->lineEdit_cameraStereoImages_path_right->setText(settings.value("path_right", _ui->lineEdit_cameraStereoImages_path_right->text()).toString());
_ui->lineEdit_cameraStereoImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraStereoImages_path_scans->text()).toString());
_ui->lineEdit_cameraStereoImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraStereoImages_laser_transform->text()).toString());
_ui->spinBox_cameraStereoImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraStereoImages_max_scan_pts->value()).toInt());
_ui->checkBox_stereoImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_stereoImages_timestamps->isChecked()).toBool());
_ui->checkBox_stereoImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_stereoImages_rectify->isChecked()).toBool());
settings.endGroup(); // StereoImages
@@ -1484,13 +1482,14 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
bool PreferencesDialog::readCoreSettings(const QString & filePath)
{
UDEBUG("");
QString path = getIniFilePath();
if(!filePath.isEmpty())
{
path = filePath;
}
UDEBUG("%s", path.toStdString().c_str());
if(!QFile::exists(path))
{
QMessageBox::information(this, tr("INI file doesn't exist..."), tr("The configuration file \"%1\" does not exist, it will be created with default parameters.").arg(path));
@@ -1533,8 +1532,23 @@ bool PreferencesDialog::readCoreSettings(const QString & filePath)
const rtabmap::ParametersMap & parameters = Parameters::getDefaultParameters();
for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
{
QString key((*iter).first.c_str());
QString key(iter->first.c_str());
QString value = settings.value(key, "").toString();
if(value.isEmpty())
{
// look for old parameter name
rtabmap::ParametersMap::const_iterator oldIter = Parameters::getBackwardCompatibilityMap().find(iter->first);
if(oldIter!=Parameters::getBackwardCompatibilityMap().end())
{
value = settings.value(QString(oldIter->second.c_str()), "").toString();
if(!value.isEmpty())
{
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
oldIter->second.c_str(), oldIter->first.c_str(), value.toStdString().c_str());
}
}
}
if(!value.isEmpty())
{
if(key.toStdString().compare(Parameters::kRtabmapWorkingDirectory()) == 0)
@@ -1563,6 +1577,7 @@ bool PreferencesDialog::readCoreSettings(const QString & filePath)
else
{
UDEBUG("key.toStdString()=%s", key.toStdString().c_str());
// Use the default value if the key doesn't exist yet
this->setParameter(key.toStdString(), (*iter).second);
@@ -1668,6 +1683,8 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue(QString("maxDepth%1").arg(i), _3dRenderingMaxDepth[i]->value());
settings.setValue(QString("showScans%1").arg(i), _3dRenderingShowScans[i]->isChecked());
settings.setValue(QString("downsamplingScan%1").arg(i), _3dRenderingDownsamplingScan[i]->value());
settings.setValue(QString("voxelSizeScan%1").arg(i), _3dRenderingVoxelSizeScan[i]->value());
settings.setValue(QString("opacity%1").arg(i), _3dRenderingOpacity[i]->value());
settings.setValue(QString("ptSize%1").arg(i), _3dRenderingPtSize[i]->value());
settings.setValue(QString("opacityScan%1").arg(i), _3dRenderingOpacityScan[i]->value());
@@ -1711,7 +1728,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.beginGroup("Camera");
settings.setValue("imgRate", _ui->general_doubleSpinBox_imgRate->value());
settings.setValue("mirroring", _ui->source_mirroring->isChecked());
settings.setValue("calibrationName", _ui->lineEdit_calibrationName->text());
settings.setValue("calibrationName", _ui->lineEdit_calibrationFile->text());
settings.setValue("type", _ui->comboBox_sourceType->currentIndex());
settings.setValue("device", _ui->lineEdit_sourceDevice->text());
settings.setValue("localTransform", _ui->lineEdit_sourceLocalTransform->text());
@@ -1765,6 +1782,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("stamps", _ui->lineEdit_cameraStereoImages_timestamps->text());
settings.setValue("path_left", _ui->lineEdit_cameraStereoImages_path_left->text());
settings.setValue("path_right", _ui->lineEdit_cameraStereoImages_path_right->text());
settings.setValue("path_scans", _ui->lineEdit_cameraStereoImages_path_scans->text());
settings.setValue("scan_transform", _ui->lineEdit_cameraStereoImages_laser_transform->text());
settings.setValue("scan_max_pts", _ui->spinBox_cameraStereoImages_max_scan_pts->value());
settings.setValue("filenames_as_stamps", _ui->checkBox_stereoImages_timestamps->isChecked());
settings.setValue("rectify", _ui->checkBox_stereoImages_rectify->isChecked());
settings.endGroup(); // StereoImages
@@ -1889,14 +1909,6 @@ bool PreferencesDialog::validateForm()
"of features on loop closure."));
_ui->reextract_type->setCurrentIndex(Feature2D::kFeatureFastBrief);
}
// odom type
if(_ui->odom_type->currentIndex() <= 1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
"with the nonfree module from OpenCV. GFTT/Brief is set instead for odometry."));
_ui->odom_type->setCurrentIndex(Feature2D::kFeatureGfttBrief);
}
}
// optimization strategy
@@ -1956,32 +1968,6 @@ bool PreferencesDialog::validateForm()
_ui->reextract_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
}
// odom type
if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannLSH && _ui->odom_type->currentIndex() <= 1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (SURF or SIFT), parameter \"Odometry->Nearest Neighbor\" "
"cannot be LSH (used for binary descriptor). KD-tree is set instead for odometry."));
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNFlannKdTree);
}
else if(_ui->odom_bin_nn->currentIndex() == VWDictionary::kNNFlannKdTree && _ui->odom_type->currentIndex() >1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("With the selected feature type (ORB, FAST, FREAK or BRIEF), parameter \"Odometry->Nearest Neighbor\" "
"cannot be KD-Tree (used for float descriptor). BruteForce matching is set instead for odometry."));
_ui->odom_bin_nn->setCurrentIndex(VWDictionary::kNNBruteForce);
}
if(_ui->loopClosure_bowVarianceFromInliersCount->isChecked() != _ui->odom_varianceFromInliersCount->isChecked())
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Odometry %1 variance from inliers count but Loop Closure constraint %2. "
"Applying the same parameter to Loop Closure Constraint.")
.arg(_ui->odom_varianceFromInliersCount->isChecked()?tr("uses"):tr("does not use"))
.arg(_ui->odom_varianceFromInliersCount->isChecked()?tr("does not"):tr("does")));
_ui->loopClosure_bowVarianceFromInliersCount->setChecked(_ui->odom_varianceFromInliersCount->isChecked());
}
if(_ui->doubleSpinBox_freenect2MinDepth->value() >= _ui->doubleSpinBox_freenect2MaxDepth->value())
{
QMessageBox::warning(this, tr("Parameter warning"),
@@ -1992,6 +1978,28 @@ bool PreferencesDialog::validateForm()
_ui->doubleSpinBox_freenect2MaxDepth->setValue(_ui->doubleSpinBox_freenect2MaxDepth->value()+1);
}
if(_ui->surf_doubleSpinBox_maxDepth->value() > 0.0 &&
_ui->surf_doubleSpinBox_minDepth->value() >= _ui->surf_doubleSpinBox_maxDepth->value())
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Visual word minimum depth (%1 m) should be lower than maximum depth (%2 m). Setting maximum depth to %3 m.")
.arg(_ui->surf_doubleSpinBox_minDepth->value())
.arg(_ui->surf_doubleSpinBox_maxDepth->value())
.arg(_ui->surf_doubleSpinBox_maxDepth->value()+1));
_ui->doubleSpinBox_freenect2MinDepth->setValue(0);
}
if(_ui->loopClosure_bowMaxDepth->value() > 0.0 &&
_ui->loopClosure_bowMinDepth->value() >= _ui->loopClosure_bowMaxDepth->value())
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Visual registration word minimum depth (%1 m) should be lower than maximum depth (%2 m). Setting maximum depth to %3 m.")
.arg(_ui->loopClosure_bowMinDepth->value())
.arg(_ui->loopClosure_bowMaxDepth->value())
.arg(_ui->loopClosure_bowMaxDepth->value()+1));
_ui->loopClosure_bowMinDepth->setValue(0);
}
return true;
}
@@ -2014,7 +2022,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
_ui->label_dictionaryPath->setEnabled(false);
_ui->groupBox_source0->setEnabled(false);
_ui->groupBox_odometry1->setEnabled(false);
_ui->groupBox_odometry2->setEnabled(false);
this->setWindowTitle(tr("Preferences [Monitoring mode]"));
}
@@ -2029,7 +2037,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
_ui->label_dictionaryPath->setEnabled(true);
_ui->groupBox_source0->setEnabled(true);
_ui->groupBox_odometry1->setEnabled(true);
_ui->groupBox_odometry2->setEnabled(true);
this->setWindowTitle(tr("Preferences"));
}
@@ -2336,6 +2344,20 @@ void PreferencesDialog::openDatabaseViewer()
}
}
void PreferencesDialog::selectCalibrationPath()
{
QString dir = _ui->lineEdit_calibrationFile->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory()+"/camera_info";
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml)"));
if(path.size())
{
_ui->lineEdit_calibrationFile->setText(path);
}
}
void PreferencesDialog::selectSourceRGBDImagesStamps()
{
QString dir = _ui->lineEdit_cameraRGBDImages_timestamps->text();
@@ -2420,6 +2442,20 @@ void PreferencesDialog::selectSourceStereoImagesPathRight()
}
}
void PreferencesDialog::selectSourceStereoImagesPathScans()
{
QString dir = _ui->lineEdit_cameraStereoImages_path_scans->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getExistingDirectory(this, tr("Select scans directory"), dir);
if(path.size())
{
_ui->lineEdit_cameraStereoImages_path_scans->setText(path);
}
}
void PreferencesDialog::selectSourceImagesPath()
{
QString dir = _ui->source_images_lineEdit_path->text();
@@ -2534,8 +2570,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
{
if(valueInt <= 1 &&
(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
combo->objectName().toStdString().compare(Parameters::kLccReextractFeatureType()) == 0 ||
combo->objectName().toStdString().compare(Parameters::kOdomFeatureType()) == 0))
combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
{
UWARN("Trying to set \"%s\" to SIFT/SURF but RTAB-Map isn't built "
"with the nonfree module from OpenCV. Keeping default combo value: %s.",
@@ -2545,8 +2580,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
}
else if(valueInt==1 &&
(combo->objectName().toStdString().compare(Parameters::kKpNNStrategy()) == 0 ||
combo->objectName().toStdString().compare(Parameters::kLccReextractNNType()) == 0 ||
combo->objectName().toStdString().compare(Parameters::kOdomBowNNType()) == 0))
combo->objectName().toStdString().compare(Parameters::kVisNNType()) == 0))
{
UWARN("Trying to set \"%s\" to KdTree but RTAB-Map isn't built "
@@ -2691,7 +2725,6 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
}
}
else if(comboBox == _ui->comboBox_detector_strategy ||
comboBox == _ui->odom_type ||
comboBox == _ui->reextract_type)
{
if(value == 0) // surf
@@ -2731,25 +2764,10 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
this->addParameters(_ui->groupBox_detector_brisk2);
}
}
else if(comboBox == _ui->globalDetection_icpType)
{
if(value == 1) // 1 icp3
{
this->addParameters(_ui->groupBox_loopClosure_icp3);
}
else if(value == 2) // 2 icp2
{
this->addParameters(_ui->groupBox_loopClosure_icp2);
}
}
else if(comboBox == _ui->loopClosure_estimationType)
{
this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex());
}
else if(comboBox == _ui->odom_estimationType)
{
this->addParameters(_ui->stackedWidget_odomEstimation, _ui->stackedWidget_odomEstimation->currentIndex());
}
else if(comboBox == _ui->graphOptimization_type)
{
this->addParameter(_ui->graphOptimization_iterations, _ui->graphOptimization_iterations->value());
@@ -2802,6 +2820,11 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
this->addParameters(_ui->groupBox_localDetection_time);
this->addParameters(_ui->groupBox_localDetection_space);
this->addParameters(_ui->groupBox_visualTransform2);
this->addParameters(_ui->groupBox_icp2);
}
else if(value && checkbox == _ui->loopClosure_icp)
{
this->addParameters(_ui->groupBox_icp2);
}
if(groupBox)
@@ -2813,7 +2836,7 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
}
if(value && groupBox == _ui->groupBox_localDetection_space)
{
this->addParameters(_ui->groupBox_loopClosure_icp2);
this->addParameters(_ui->groupBox_icp2);
}
this->addParameters(groupBox);
@@ -3345,6 +3368,16 @@ bool PreferencesDialog::isScansShown(int index) const
UASSERT(index >= 0 && index <= 1);
return _3dRenderingShowScans[index]->isChecked();
}
int PreferencesDialog::getDownsamplingStepScan(int index) const
{
UASSERT(index >= 0 && index <= 1);
return _3dRenderingDownsamplingScan[index]->value();
}
double PreferencesDialog::getCloudVoxelSizeScan(int index) const
{
UASSERT(index >= 0 && index <= 1);
return _3dRenderingVoxelSizeScan[index]->value();
}
double PreferencesDialog::getScanOpacity(int index) const
{
UASSERT(index >= 0 && index <= 1);
@@ -3426,10 +3459,6 @@ bool PreferencesDialog::isSourceMirroring() const
{
return _ui->source_mirroring->isChecked();
}
QString PreferencesDialog::getCalibrationName() const
{
return _ui->lineEdit_calibrationName->text();
}
PreferencesDialog::Src PreferencesDialog::getSourceType() const
{
int index = _ui->comboBox_sourceType->currentIndex();
@@ -3498,49 +3527,23 @@ QString PreferencesDialog::getSourceDevice() const
{
return _ui->lineEdit_sourceDevice->text();
}
Transform PreferencesDialog::getSourceLocalTransform() const
{
Transform t = Transform::getIdentity();
QString str = _ui->lineEdit_sourceLocalTransform->text();
str.replace("PI_2", QString::number(3.141592/2.0));
QStringList list = str.split(' ');
if(list.size() == 6 || list.size() == 9 || list.size() == 12)
Transform t = Transform::fromString(_ui->lineEdit_sourceLocalTransform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
if(t.isNull())
{
std::vector<float> numbers(list.size());
bool ok = false;
for(int i=0; i<list.size(); ++i)
{
numbers[i] = list.at(i).toDouble(&ok);
if(!ok)
{
UERROR("Parsing local transform failed! \"%s\" not recognized (%s)",
list.at(i).toStdString().c_str(), str.toStdString().c_str());
break;
}
}
if(ok)
{
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]);
}
}
return Transform::getIdentity();
}
else
return t;
}
Transform PreferencesDialog::getStereoLaserLocalTransform() const
{
Transform t = Transform::fromString(_ui->lineEdit_cameraStereoImages_laser_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
if(t.isNull())
{
UERROR("Local transform is wrong! must have 6 or 9 items (%s)", str.toStdString().c_str());
return Transform::getIdentity();
}
return t;
}
@@ -3700,6 +3703,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
else if(driver == kSrcStereoImages)
{
camera = new CameraStereoImages(
_ui->lineEdit_cameraStereoImages_path_scans->text().append(QDir::separator()).toStdString(),
this->getStereoLaserLocalTransform(),
_ui->spinBox_cameraStereoImages_max_scan_pts->value(),
_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
_ui->checkBox_stereoImages_timestamps->isChecked(),
@@ -3755,7 +3761,20 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
if(camera)
{
// don't set calibration folder if we want raw images
if(!camera->init(useRawImages?"":this->getCameraInfoDir().toStdString(), this->getCalibrationName().toStdString()))
QString dir = this->getCameraInfoDir();
QString name = QFileInfo(_ui->lineEdit_calibrationFile->text()
.remove("_left.yaml")
.remove("_right.yaml")
.remove("_pose.yaml")).baseName();
if(!_ui->lineEdit_calibrationFile->text().isEmpty())
{
QDir d = QFileInfo(_ui->lineEdit_calibrationFile->text()).dir();
if(!d.path().isEmpty())
{
dir = d.absolutePath();
}
}
if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
{
UWARN("init camera failed... ");
QMessageBox::warning(this,
@@ -3809,7 +3828,7 @@ int PreferencesDialog::getOdomBufferSize() const
{
return _ui->odom_dataBufferSize->value();
}
bool PreferencesDialog::getLccBowVarianceFromInliersCount() const
bool PreferencesDialog::getRegVarianceFromInliersCount() const
{
return _ui->loopClosure_bowVarianceFromInliersCount->isChecked();
}
@@ -3906,11 +3925,6 @@ void PreferencesDialog::setSLAMMode(bool enabled)
}
void PreferencesDialog::testOdometry()
{
testOdometry(_ui->odom_type->currentIndex());
}
void PreferencesDialog::testOdometry(int type)
{
DBReader dbReader(_ui->source_database_lineEdit_path->text().toStdString(),
_ui->source_checkBox_useDbStamps->isChecked()?-1:this->getGeneralInputRate(),
@@ -4098,7 +4112,7 @@ void PreferencesDialog::calibrate()
void PreferencesDialog::calibrateSimple()
{
CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), _ui->lineEdit_calibrationName->text(), this);
CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), "", this);
dialog.exec();
}
guilib/src/ui/DatabaseViewer.ui
+177 -90
View File
@@ -50,8 +50,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>166</width>
<height>173</height>
<width>181</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -236,8 +236,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>165</width>
<height>173</height>
<width>181</width>
<height>184</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -418,7 +418,7 @@
<x>0</x>
<y>0</y>
<width>1285</width>
<height>25</height>
<height>22</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -430,6 +430,7 @@
<addaction name="actionSave_config"/>
<addaction name="separator"/>
<addaction name="actionGenerate_3D_map_pcd"/>
<addaction name="actionExport_3D_laser_scans_ply_pcd"/>
<addaction name="actionExport"/>
<addaction name="actionExtract_images"/>
<addaction name="separator"/>
@@ -453,6 +454,7 @@
<addaction name="actionReset_all_changes"/>
<addaction name="separator"/>
<addaction name="actionView_3D_map"/>
<addaction name="actionView_3D_laser_scans"/>
</widget>
<widget class="QMenu" name="menuView">
<property name="title">
@@ -826,25 +828,103 @@
</attribute>
<widget class="QWidget" name="dockWidgetContents_3">
<layout class="QVBoxLayout" name="verticalLayout_10">
<item>
<layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
<widget class="QComboBox" name="comboBox_logger_level">
<property name="currentIndex">
<number>1</number>
</property>
<item>
<property name="text">
<string>Debug</string>
</property>
</item>
<item>
<property name="text">
<string>Info</string>
</property>
</item>
<item>
<property name="text">
<string>Warning</string>
</property>
</item>
<item>
<property name="text">
<string>Error</string>
</property>
</item>
</widget>
</item>
<item>
<widget class="QLabel" name="label_logger_level">
<property name="text">
<string>Logger level</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QToolBox" name="toolBox">
<property name="currentIndex">
<number>1</number>
<number>0</number>
</property>
<widget class="QWidget" name="page">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>314</width>
<height>303</height>
<width>312</width>
<height>374</height>
</rect>
</property>
<attribute name="label">
<string>ICP</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
<item row="0" column="0">
<item row="8" column="0">
<widget class="QSpinBox" name="spinBox_icp_iteration">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Point to plane</string>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QSpinBox" name="spinBox_icp_normalKSearch">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_icp_decimation">
<property name="minimum">
<number>1</number>
@@ -857,14 +937,14 @@
</property>
</widget>
</item>
<item row="0" column="1">
<item row="1" column="1">
<widget class="QLabel" name="label_14">
<property name="text">
<string>Decimation</string>
</property>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_maxDepth">
<property name="suffix">
<string> m</string>
@@ -880,14 +960,7 @@
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_17">
<property name="text">
<string>Max depth</string>
</property>
</widget>
</item>
<item row="2" column="0">
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_voxel">
<property name="suffix">
<string> m</string>
@@ -903,14 +976,14 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_13">
<property name="text">
<string>Voxel</string>
<string>Voxel size</string>
</property>
</widget>
</item>
<item row="4" column="0">
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_maxCorrespDistance">
<property name="suffix">
<string> m</string>
@@ -929,14 +1002,31 @@
</property>
</widget>
</item>
<item row="4" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_12">
<property name="text">
<string>Max correspondence distance</string>
</property>
</widget>
</item>
<item row="5" column="0">
<item row="11" column="0">
<widget class="QCheckBox" name="checkBox_icp_2d">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_17">
<property name="text">
<string>Max depth</string>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_icp_minCorrespondenceRatio">
<property name="suffix">
<string/>
@@ -955,34 +1045,14 @@
</property>
</widget>
</item>
<item row="5" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_46">
<property name="text">
<string>Min correspondence ratio</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QSpinBox" name="spinBox_icp_iteration">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="7" column="0">
<item row="9" column="0">
<widget class="QCheckBox" name="checkBox_icp_p2plane">
<property name="text">
<string/>
@@ -992,51 +1062,21 @@
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Point to plane</string>
</property>
</widget>
</item>
<item row="8" column="0">
<widget class="QSpinBox" name="spinBox_icp_normalKSearch">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>20</number>
</property>
</widget>
</item>
<item row="8" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_20">
<property name="text">
<string>Normal K neighbors</string>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="checkBox_icp_2d">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="9" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_27">
<property name="text">
<string>2D icp (laser scans required)</string>
<string>2D icp</string>
</property>
</widget>
</item>
<item row="10" column="0">
<item row="12" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
@@ -1049,6 +1089,43 @@
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_68">
<property name="text">
<string>Use laser scans for ICP</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QCheckBox" name="checkBox_icp_laserScan">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_69">
<property name="text">
<string>Downsample step size</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="spinBox_icp_downsamplingStepSize">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1000</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_2">
@@ -1056,8 +1133,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>351</width>
<height>407</height>
<width>366</width>
<height>420</height>
</rect>
</property>
<attribute name="label">
@@ -1389,8 +1466,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>333</width>
<height>333</height>
<width>338</width>
<height>330</height>
</rect>
</property>
<attribute name="label">
@@ -1628,8 +1705,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>255</width>
<height>377</height>
<width>261</width>
<height>411</height>
</rect>
</property>
<attribute name="label">
@@ -1886,7 +1963,7 @@
<x>0</x>
<y>0</y>
<width>201</width>
<height>117</height>
<height>126</height>
</rect>
</property>
<attribute name="label">
@@ -1985,8 +2062,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>285</width>
<height>309</height>
<width>283</width>
<height>322</height>
</rect>
</property>
<attribute name="label">
@@ -2394,7 +2471,7 @@
</action>
<action name="actionGenerate_3D_map_pcd">
<property name="text">
<string>Export 3D map (*.pcd) ...</string>
<string>Export 3D map (*.ply *.pcd) ...</string>
</property>
</action>
<action name="actionExport">
@@ -2457,6 +2534,16 @@
<string>Generate g2o graph (*.g2o)...</string>
</property>
</action>
<action name="actionView_3D_laser_scans">
<property name="text">
<string>View 3D laser scans...</string>
</property>
</action>
<action name="actionExport_3D_laser_scans_ply_pcd">
<property name="text">
<string>Export 3D laser scans (*.ply *.pcd) ...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/preferencesDialog.ui
+2071 -2486
View File
File diff suppressed because it is too large Load Diff
tools/ConsoleApp/main.cpp
+48
View File
@@ -286,6 +286,54 @@ int main(int argc, char * argv[])
continue;
}
//backward compatibility
// look for old parameter name
std::map<std::string, std::pair<bool, std::string> >::const_iterator oldIter = Parameters::getRemovedParameters().find(key);
if(oldIter!=Parameters::getRemovedParameters().end())
{
++i;
if(i < argc)
{
std::string value = argv[i];
if(value.empty())
{
showUsage();
}
else
{
value = uReplaceChar(value, ',', ' ');
}
if(oldIter->second.first)
{
key = oldIter->second.second;
UWARN("Parameter migration from \"%s\" to \"%s\" (value=%s).",
oldIter->first.c_str(), oldIter->second.second.c_str(), value.c_str());
}
else if(oldIter->second.second.empty())
{
UERROR("Parameter \"%s\" doesn't exist anymore.", oldIter->first.c_str());
}
else
{
UERROR("Parameter \"%s\" doesn't exist anymore, check this similar parameter \"%s\".", oldIter->first.c_str(), oldIter->second.second.c_str());
}
if(oldIter->second.first)
{
std::pair<ParametersMap::iterator, bool> inserted = pm.insert(ParametersPair(key, value));
if(inserted.second == false)
{
inserted.first->second = value;
}
}
}
else
{
showUsage();
}
continue;
}
printf("Unrecognized option : %s\n", argv[i]);
showUsage();
}
tools/OdometryViewer/main.cpp
+19 -19
View File
@@ -100,17 +100,17 @@ int main (int argc, char * argv[])
float rate = 0.0;
std::string inputDatabase;
int driver = 0;
int odomType = rtabmap::Parameters::defaultOdomFeatureType();
int odomType = rtabmap::Parameters::defaultVisFeatureType();
bool icp = false;
bool flow = false;
bool mono = false;
int nnType = rtabmap::Parameters::defaultOdomBowNNType();
float nndr = rtabmap::Parameters::defaultOdomBowNNDR();
float distance = rtabmap::Parameters::defaultOdomInlierDistance();
int maxWords = rtabmap::Parameters::defaultOdomMaxFeatures();
int minInliers = rtabmap::Parameters::defaultOdomMinInliers();
float maxDepth = rtabmap::Parameters::defaultOdomMaxDepth();
int iterations = rtabmap::Parameters::defaultOdomIterations();
int nnType = rtabmap::Parameters::defaultVisNNType();
float nndr = rtabmap::Parameters::defaultVisNNDR();
float distance = rtabmap::Parameters::defaultVisInlierDistance();
int maxWords = rtabmap::Parameters::defaultVisMaxFeatures();
int minInliers = rtabmap::Parameters::defaultVisMinInliers();
float maxDepth = rtabmap::Parameters::defaultVisMaxDepth();
int iterations = rtabmap::Parameters::defaultVisIterations();
int resetCountdown = rtabmap::Parameters::defaultOdomResetCountdown();
int decimation = 4;
float voxel = 0.005;
@@ -620,7 +620,7 @@ int main (int argc, char * argv[])
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxDepth(), uNumber2Str(maxDepth)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMaxDepth(), uNumber2Str(maxDepth)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomResetCountdown(), uNumber2Str(resetCountdown)));
if(!icp)
@@ -630,11 +630,11 @@ int main (int argc, char * argv[])
UINFO("RANSAC iterations = %d", iterations);
UINFO("Max features = %d", maxWords);
UINFO("GPU = %s", gpu?"true":"false");
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMaxFeatures(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisInlierDistance(), uNumber2Str(distance)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), uNumber2Str(minInliers)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisIterations(), uNumber2Str(iterations)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMaxFeatures(), uNumber2Str(maxWords)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisFeatureType(), uNumber2Str(odomType)));
if(odomType == 0)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kSURFGpuVersion(), uBool2Str(gpu)));
@@ -666,15 +666,15 @@ int main (int argc, char * argv[])
UINFO("Nearest neighbor = %s", nnName.c_str());
UINFO("Nearest neighbor ratio = %f", nndr);
UINFO("Local history = %d", localHistory);
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisNNType(), uNumber2Str(nnType)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisNNDR(), uNumber2Str(nndr)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomBowLocalHistorySize(), uNumber2Str(localHistory)));
if(mono)
{
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomPnPReprojError(), "4.0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), "100"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPFlags(), uNumber2Str(0))); //CV_ITERATIVE
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPReprojError(), "4.0"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisIterations(), "100"));
odom = new rtabmap::OdometryMono(parameters);
}
else