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

Update 0.15.4.

Browse Source 
Parameters:
-Added "GridGlobal/MaxNodes=0", "Rtabmap/PublishRAMUsage=false", "Mem/DepthAsMask=true", "Kp/FlannRebalancingFactor=2.0", "Vis/DepthAsMask=true".
-Modified "Kp/DetectorStrategy=6", "Kp/MaxFeatures=500",  "Mem/UseOdomFeatures=true", "GFTT/QualityLevel=0.001", "GFTT/MinDistance=3", "RGBD/OptimizeMaxError=1", "RGBD/ProximityPathFilteringRadius=1", "Odom/GuessMotion=true", "Odom/VisKeyFrameThr=150", "OdomF2M/BundleAdjustment=1", "Vis/Iterations=300" if built with g2o, "OdomF2M/BundleAdjustmentMaxFrames=10", "OdomFovis/MinFeaturesForEstimate=20", "OdomORBSLAM2/MapSize=3000", "Reg/RepeatOnce=true", "Vis/PnPRefineIterations=0" if built with g2o, "Vis/CorGuessMatchToProjection=true", "Vis/BundleAdjustment=1" if built with g2o, "Icp/MaxCorrespondenceDistance=0.1", "Icp/PointToPlaneK=5", "Icp/PointToPlaneRadius=1", "Icp/PM=true" if built with libpointmatcher, "Stereo/MaxLevel=5", "Stereo/MinDisparity=0.5".

BayesFilter: optimized prediction matrix update. Use of new argument "ignoreLocalSpaceLoopIds" of Memory::getNeighborsId() to ignore loop closure link by space in prediction update.
CameraThread: Added stereo exposure compensation option.
CameraRGB: Added forceGroundNormalsUp option and added support of ground truth from EuRoC dataset.
Statistics: Added "Memory/RAM_usage/MB".
Transform: Added clone() method to do deep copy.
Graph::importPoses(): EuRoC format support (9).
Rtabmap: Local visual loop closures are now identified as GlobalClosure link type.
OccupancyGrid/OctoMap: updated how cache is used (old node retrieved can be re-added to map without re-assembling the whole map).
OdometryF2F: when using ICP, increasing correspondence distance for first two frames. If Vis/CorType=1 and registration fails, second guess without motion is done with Vis/CorType=0.
OdometryF2M/RegVis: updated how features are removed from the map, using new projectedIDs filled in RegistrationInfo by RegistrationVis.
OdometryORBSLAM2: Maximum size of the feature map can be set with "OdomORBSLAM2/MapSize" parameter.
CloudViewer: fixed opengl camera drifting in follow mode.
DatabaseViewer: Added optimization scale option. ConstraintsView: hide loop closure links if type is ignored in gui parameters.
MainWindow: Support of "GridGlobal/MaxNodes" parameters when updating the maps.
UPlot: don't show ellipses when not in graphics view mode, updated how "random" colors are attributed to curves
Added rtabmap-euroc_dataset tool. Updated rtabmap-kitti_dataset and rtabmap-rgbd_dataset tools.
Added rtabmap-reprocess tool.
This commit is contained in:
matlabbe
2018-02-01 22:17:46 -05:00
parent 9f80f4ac42
commit 977d21eed5
76 changed files with 3844 additions and 1307 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/BayesFilter.h
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/core/core.hpp>
#include <list>
#include <set>
#include <unordered_map>
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/core/Parameters.h"
@@ -59,18 +60,18 @@ public:
const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
private:
cv::Mat updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const;
const std::vector<int> & newIds);
void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
float addNeighborProb(cv::Mat & prediction,
unsigned int col,
const std::map<int, int> & neighbors,
const std::map<int, int> & idToIndexMap) const;
const std::unordered_map<int, int> & idToIndex) const;
void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
private:
@@ -80,6 +81,7 @@ private:
std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
bool _fullPredictionUpdate;
float _totalPredictionLCValues;
std::map<int, std::map<int, int> > _neighborsIndex;
};
} // namespace rtabmap

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

@@ -59,6 +59,7 @@ public:
float timeCapture;
float timeDisparity;
float timeMirroring;
float timeStereoExposureCompensation;
float timeImageDecimation;
float timeScanFromDepth;
float timeUndistortDepth;

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

@@ -87,7 +87,8 @@ public:
float voxelSize = 0.0f,
int normalsK = 0, // compute normals if > 0
float normalsRadius = 0, // compute normals if > 0
const Transform & localTransform=Transform::getIdentity())
const Transform & localTransform=Transform::getIdentity(),
bool forceGroundNormalsUp = false)
{
_scanPath = dir;
_scanLocalTransform = localTransform;
@@ -96,6 +97,7 @@ public:
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanVoxelSize = voxelSize;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
@@ -161,6 +163,7 @@ private:
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
bool _depthFromScan;
int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical

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

@@ -60,6 +60,7 @@ public:
virtual ~CameraThread();
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
@@ -100,6 +101,7 @@ private:
private:
Camera * _camera;
bool _mirroring;
bool _stereoExposureCompensation;
bool _colorOnly;
int _imageDecimation;
bool _stereoToDepth;

13
corelib/include/rtabmap/core/FlannIndex.h
View File

@@ -49,21 +49,25 @@ public:
// Note that useDistanceL1 doesn't have any effect if LSH is used
void buildLinearIndex(
const cv::Mat & features,
bool useDistanceL1 = false);
bool useDistanceL1 = false,
float rebalancingFactor = 2.0f);
void buildKDTreeIndex(
const cv::Mat & features,
int trees = 4,
bool useDistanceL1 = false);
bool useDistanceL1 = false,
float rebalancingFactor = 2.0f);
void buildKDTreeSingleIndex(
const cv::Mat & features,
int leafMaxSize = 10,
bool reorder = true,
bool useDistanceL1 = false);
bool useDistanceL1 = false,
float rebalancingFactor = 2.0f);
void buildLSHIndex(
const cv::Mat & features,
unsigned int table_number = 12,
unsigned int key_size = 20,
unsigned int multi_probe_level = 2);
unsigned int multi_probe_level = 2,
float rebalancingFactor = 2.0f);
bool isBuilt();
@@ -102,6 +106,7 @@ private:
int featuresDim_;
bool isLSH_;
bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
float rebalancingFactor_;
// keep feature in memory until the tree is rebuilt
// (in case the word is deleted when removed from the VWDictionary)

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

@@ -123,6 +123,8 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
int to,
bool checkBothWays = true);
std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
const std::multimap<int, Link> & links);
std::multimap<int, Link> RTABMAP_EXP filterLinks(
const std::multimap<int, Link> & links,
Link::Type filteredType);
@@ -227,6 +229,11 @@ int RTABMAP_EXP findNearestNode(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose);
std::vector<int> RTABMAP_EXP findNearestNodes(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose,
int k);
/**
* Get nodes near the query
* @param nodeId the query id

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

@@ -127,6 +127,7 @@ public:
bool incrementMarginOnLoop = false,
bool ignoreLoopIds = false,
bool ignoreIntermediateNodes = false,
bool ignoreLocalSpaceLoopIds = false,
const std::set<int> & nodesSet = std::set<int>(),
double * dbAccessTime = 0) const;
std::map<int, float> getNeighborsIdRadius(
@@ -281,6 +282,7 @@ private:
bool _generateIds;
bool _badSignaturesIgnored;
bool _mapLabelsAdded;
bool _depthAsMask;
int _imagePreDecimation;
int _imagePostDecimation;
bool _compressionParallelized;
@@ -295,6 +297,7 @@ private:
bool _useOdometryFeatures;
bool _createOccupancyGrid;
int _visMaxFeatures;
int _visCorType;
int _idCount;
int _idMapCount;

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

@@ -44,6 +44,7 @@ public:
void parseParameters(const ParametersMap & parameters);
void setCellSize(float cellSize);
float getCellSize() const {return cellSize_;}
void setCloudAssembling(bool enabled);
float getMinMapSize() const {return minMapSize_;}
bool isGridFromDepth() const {return occupancyFromCloud_;}
bool isFullUpdate() const {return fullUpdate_;}
@@ -73,7 +74,9 @@ public:
const cv::Mat & ground,
const cv::Mat & obstacles);
void update(const std::map<int, Transform> & poses);
const cv::Mat getMap(float & xMin, float & yMin) const;
cv::Mat getMap(float & xMin, float & yMin) const;
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
private:
ParametersMap parameters_;
@@ -109,13 +112,17 @@ private:
bool erode_;
float footprintRadius_;
std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
std::map<int, std::pair<cv::Mat, cv::Mat> > cache_; //<node id, <ground, obstacles> >
cv::Mat map_;
cv::Mat mapInfo_;
std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
float xMin_;
float yMin_;
std::map<int, Transform> addedNodes_;
bool cloudAssembling_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
};
}

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

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Parameters.h>
#include <map>
#include <string>
@@ -57,7 +58,8 @@ public:
class RTABMAP_EXP OctoMap {
public:
OctoMap(float voxelSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
OctoMap(const ParametersMap & parameters, float occupancyThr = 0.5f);
OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false);
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
void addToCache(int nodeId,

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

@@ -93,6 +93,7 @@ private:
float _kalmanMeasurementNoise;
int _imageDecimation;
bool _alignWithGround;
bool _publishRAMUsage;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;

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

@@ -59,6 +59,7 @@ private:
Registration * registrationPipeline_;
Signature refFrame_;
Transform lastKeyFramePose_;
ParametersMap parameters_;
};
}

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

@@ -71,6 +71,7 @@ private:
Registration * regPipeline_;
Signature * map_;
Signature * lastFrame_;
int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
std::map<int, std::map<int, cv::Point3f> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>

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

@@ -54,6 +54,7 @@ public:
stamp(0),
interval(0),
distanceTravelled(0.0f),
memoryUsage(0),
type(0)
{}
@@ -80,6 +81,7 @@ public:
output.transformFiltered = transformFiltered;
output.transformGroundTruth = transformGroundTruth;
output.distanceTravelled = distanceTravelled;
output.memoryUsage = memoryUsage;
output.type = type;
return output;
}
@@ -104,6 +106,7 @@ public:
Transform transformFiltered;
Transform transformGroundTruth;
float distanceTravelled;
int memoryUsage; //MB
int type;

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

@@ -172,6 +172,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Rtabmap, PublishLastSignature, bool, true, "Publishing last signature.");
RTABMAP_PARAM(Rtabmap, PublishPdf, bool, true, "Publishing pdf.");
RTABMAP_PARAM(Rtabmap, PublishLikelihood, bool, true, "Publishing likelihood.");
RTABMAP_PARAM(Rtabmap, PublishRAMUsage, bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
RTABMAP_PARAM(Rtabmap, ComputeRMSE, bool, true, "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0, "Maximum time allowed for the detector (ms) (0 means infinity).");
RTABMAP_PARAM(Rtabmap, MemoryThr, int, 0, "Maximum signatures in the Working Memory (ms) (0 means infinity).");
@@ -208,6 +209,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs.");
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, DepthAsMask, bool, true, "Use depth image as mask when extracting features for vocabulary.");
RTABMAP_PARAM(Mem, ImagePreDecimation, int, 1, "Image decimation (>=1) before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
RTABMAP_PARAM(Mem, ImagePostDecimation, int, 1, "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
RTABMAP_PARAM(Mem, CompressionParallelized, bool, true, "Compression of sensor data is multi-threaded.");
@@ -215,21 +217,27 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
RTABMAP_PARAM(Mem, LaserScanNormalK, int, 0, "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanNormalRadius, int, 0, "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, false, "Use odometry features.");
RTABMAP_PARAM(Mem, UseOdomFeatures, bool, true, "Use odometry features.");
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, NNStrategy, int, 1, "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, IncrementalFlann, bool, true, uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
RTABMAP_PARAM(Kp, FlannRebalancingFactor, float, 2.0, uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
RTABMAP_PARAM(Kp, MaxDepth, float, 0, "Filter extracted keypoints by depth (0=inf).");
RTABMAP_PARAM(Kp, MinDepth, float, 0, "Filter extracted keypoints by depth.");
RTABMAP_PARAM(Kp, MaxFeatures, int, 400, "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
RTABMAP_PARAM(Kp, MaxFeatures, int, 500, "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(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
#ifdef RTABMAP_NONFREE
RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
#ifndef RTABMAP_NONFREE
#ifdef RTABMAP_OPENCV3
// OpenCV 3 without xFeatures2D module doesn't have BRIEF
RTABMAP_PARAM(Kp, DetectorStrategy, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
#else
RTABMAP_PARAM(Kp, DetectorStrategy, int, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
#endif
#else
RTABMAP_PARAM(Kp, DetectorStrategy, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE.");
#endif
RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood.");
RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized.");
@@ -275,8 +283,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(FAST, GridRows, int, 4, "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
RTABMAP_PARAM(FAST, GridCols, int, 4, "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, "");
RTABMAP_PARAM(GFTT, MinDistance, double, 3, "");
RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
RTABMAP_PARAM(GFTT, K, double, 0.04, "");
@@ -325,7 +333,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, AngularSpeedUpdate, float, 0.0, "Maximum angular speed (rad/s) to update the map (0 means not limit).");
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).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, OptimizeMaxError, float, 1.0, uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanStuckIterations, int, 0, "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
RTABMAP_PARAM(RGBD, PlanLinearVelocity, float, 0, "Linear velocity (m/sec) used to compute path weights.");
@@ -344,7 +352,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, ProximityBySpace, bool, true, "Detection over locations (in Working Memory) near in space.");
RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, ProximityMaxPaths, int, 3, "Maximum paths compared (from the most recent) for proximity detection by space. 0 means no limit.");
RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 1, "Path filtering radius to reduce the number of nodes to compare in a path. A path should also be inside that radius to be considered for proximity detection.");
RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors, int, 0, "Maximum neighbor nodes compared on each path. Set to 0 to disable merging the laser scans.");
RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
RTABMAP_PARAM(RGBD, ProximityAngle, float, 45, "Maximum angle (degrees) for visual proximity detection.");
@@ -395,9 +403,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw).");
RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value.");
RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value.");
RTABMAP_PARAM(Odom, GuessMotion, bool, false, "Guess next transformation from the last motion computed.");
RTABMAP_PARAM(Odom, GuessMotion, bool, true, "Guess next transformation from the last motion computed.");
RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.3, "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, VisKeyFrameThr, int, 100, "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, VisKeyFrameThr, int, 150, "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ImageDecimation, int, 1, "Decimation of the images before registration. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization.");
@@ -408,8 +416,12 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomF2M, ScanMaxSize, int, 2000, "[Geometry] Maximum local scan map size.");
RTABMAP_PARAM(OdomF2M, ScanSubtractRadius, float, 0.05, "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
#else
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
#endif
RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
// Odometry Mono
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");
@@ -435,7 +447,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomFovis, InlierMaxReprojectionError, double, 1.5, "The maximum image-space reprojection error (in pixels) a feature match is allowed to have and still be considered an inlier in the set of features used for motion estimation.");
RTABMAP_PARAM(OdomFovis, CliqueInlierThreshold, double, 0.1, "See Howard's greedy max-clique algorithm for determining the maximum set of mutually consisten feature matches. This specifies the compatibility threshold, in meters.");
RTABMAP_PARAM(OdomFovis, MinFeaturesForEstimate, int, 10, "Minimum number of features in the inlier set for the motion estimate to be considered valid.");
RTABMAP_PARAM(OdomFovis, MinFeaturesForEstimate, int, 20, "Minimum number of features in the inlier set for the motion estimate to be considered valid.");
RTABMAP_PARAM(OdomFovis, MaxMeanReprojectionError, double, 10.0, "Maximum mean reprojection error over the inlier feature matches for the motion estimate to be considered valid.");
RTABMAP_PARAM(OdomFovis, UseSubpixelRefinement, bool, true, "Specifies whether or not to refine feature matches to subpixel resolution.");
RTABMAP_PARAM(OdomFovis, FeatureSearchWindow, int, 25, "Specifies the size of the search window to apply when searching for feature matches across time frames. The search is conducted around the feature location predicted by the initial rotation estimate.");
@@ -470,9 +482,10 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomORBSLAM2, ThDepth, double, 40.0, "Close/Far threshold. Baseline times.");
RTABMAP_PARAM(OdomORBSLAM2, Fps, float, 0.0, "Camera FPS.");
RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures, int, 1000, "Maximum ORB features extracted per frame.");
RTABMAP_PARAM(OdomORBSLAM2, MapSize, int, 3000, "Maximum size of the feature map (0 means infinite).");
// Common registration parameters
RTABMAP_PARAM(Reg, RepeatOnce, bool, false, "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration. It can be useful if the registration approach used can use a guess to get better matches.");
RTABMAP_PARAM(Reg, RepeatOnce, bool, true, "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");
RTABMAP_PARAM(Reg, Force3DoF, bool, false, "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
@@ -483,10 +496,15 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
#else
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
#endif
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
RTABMAP_PARAM(Vis, Iterations, int, 300, "Maximum iterations to compute the transform.");
#ifndef RTABMAP_NONFREE
#ifdef RTABMAP_OPENCV3
// OpenCV 3 without xFeatures2D module doesn't have BRIEF
@@ -500,6 +518,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits.");
RTABMAP_PARAM(Vis, MaxDepth, float, 0, "Max depth of the features (0 means no limit).");
RTABMAP_PARAM(Vis, MinDepth, float, 0, "Min depth of the features (0 means no limit).");
RTABMAP_PARAM(Vis, DepthAsMask, bool, true, "Use depth image as mask when extracting features.");
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.");
@@ -510,29 +529,37 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, CorNNType, int, 1, uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorNNDR, float, 0.6, uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorGuessWinSize, int, 20, uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, true, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool, false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
#else
RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
#endif
// 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, VoxelSize, float, 0.0, "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, MaxCorrespondenceDistance, float, 0.1, "Max distance for point correspondences.");
RTABMAP_PARAM(Icp, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.2, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(Icp, PointToPlane, bool, false, "Use point to plane ICP.");
RTABMAP_PARAM(Icp, PointToPlaneK, int, 20, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 0.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneK, int, 5, "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneRadius, float, 1.0, "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
RTABMAP_PARAM(Icp, PointToPlaneMinComplexity, float, 0.02, "Minimum structural complexity (0.0=low, 1.0=high) of the scan to do point to plane registration, otherwise point to point registration is done instead.");
// libpointmatcher
#ifdef RTABMAP_POINTMATCHER
RTABMAP_PARAM(Icp, PM, bool, true, "Use libpointmatcher for ICP registration instead of PCL's implementation.");
#else
RTABMAP_PARAM(Icp, PM, bool, false, "Use libpointmatcher for ICP registration instead of PCL's implementation.");
#endif
RTABMAP_PARAM_STR(Icp, PMConfig, "", uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
RTABMAP_PARAM(Icp, PMMatcherKnn, int, 1, "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
RTABMAP_PARAM(Icp, PMMatcherEpsilon, float, 0.0, "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
@@ -542,8 +569,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Stereo, WinWidth, int, 15, "Window width.");
RTABMAP_PARAM(Stereo, WinHeight, int, 3, "Window height.");
RTABMAP_PARAM(Stereo, Iterations, int, 30, "Maximum iterations.");
RTABMAP_PARAM(Stereo, MaxLevel, int, 3, "Maximum pyramid level.");
RTABMAP_PARAM(Stereo, MinDisparity, float, 1.0, "Minimum disparity.");
RTABMAP_PARAM(Stereo, MaxLevel, int, 5, "Maximum pyramid level.");
RTABMAP_PARAM(Stereo, MinDisparity, float, 0.5, "Minimum disparity.");
RTABMAP_PARAM(Stereo, MaxDisparity, float, 128.0, "Maximum disparity.");
RTABMAP_PARAM(Stereo, OpticalFlow, bool, true, "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
RTABMAP_PARAM(Stereo, SSD, bool, true, uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
@@ -574,7 +601,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Grid, NormalsSegmentation, bool, true, "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
RTABMAP_PARAM(Grid, MaxObstacleHeight, float, 0.0, "Maximum obstacles height (0=disabled).");
RTABMAP_PARAM(Grid, MinGroundHeight, float, 0.0, "Minimum ground height (0=disabled).");
RTABMAP_PARAM(Grid, MaxGroundHeight, float, 0.0, uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, MaxGroundHeight, float, 0.0, uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is false.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, MaxGroundAngle, float, 45, uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NormalK, int, 20, uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, ClusterRadius, float, 0.1, uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
@@ -596,6 +623,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(GridGlobal, FootprintRadius, float, 0.0, "Footprint radius (m) used to clear all obstacles under the graph.");
RTABMAP_PARAM(GridGlobal, MinSize, float, 0.0, "Minimum map size (m).");
RTABMAP_PARAM(GridGlobal, Eroded, bool, false, "Erode obstacle cells.");
RTABMAP_PARAM(GridGlobal, MaxNodes, int, 0, "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
public:
virtual ~Parameters();

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

@@ -59,6 +59,8 @@ public:
bool isScanRequired() const;
bool isUserDataRequired() const;
bool canUseGuess() const;
int getMinVisualCorrespondences() const;
float getMinGeometryCorrespondencesRatio() const;
@@ -100,6 +102,7 @@ protected:
virtual bool isImageRequiredImpl() const {return false;}
virtual bool isScanRequiredImpl() const {return false;}
virtual bool isUserDataRequiredImpl() const {return false;}
virtual bool canUseGuessImpl() const {return false;}
virtual int getMinVisualCorrespondencesImpl() const {return 0;}
virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}

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

@@ -52,6 +52,7 @@ protected:
Transform guess,
RegistrationInfo & info) const;
virtual bool isScanRequiredImpl() const {return true;}
virtual bool canUseGuessImpl() const {return true;}
virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
private:

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

@@ -70,6 +70,7 @@ public:
std::vector<int> inliersIDs;
int matches;
std::vector<int> matchesIDs;
std::vector<int> projectedIDs; // "From" IDs
// RegistrationIcp
float icpInliersRatio;

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

@@ -61,6 +61,7 @@ protected:
RegistrationInfo & info) const;
virtual bool isImageRequiredImpl() const {return true;}
virtual bool canUseGuessImpl() const {return _correspondencesApproach != 0 || _guessWinSize>0;}
virtual int getMinVisualCorrespondencesImpl() const {return _minInliers;}
private:
@@ -83,6 +84,7 @@ private:
int _guessWinSize;
bool _guessMatchToProjection;
int _bundleAdjustment;
bool _depthAsMask;
ParametersMap _featureParameters;
ParametersMap _bundleParameters;

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

@@ -118,6 +118,7 @@ public:
const Statistics & getStatistics() const;
//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
Transform getPose(int locationId) const;
Transform getMapCorrection() const {return _mapCorrection;}
const Memory * getMemory() const {return _memory;}
@@ -211,6 +212,7 @@ private:
bool _publishLastSignatureData;
bool _publishPdf;
bool _publishLikelihood;
bool _publishRAMUsage;
bool _computeRMSE;
float _maxTimeAllowed; // in ms
unsigned int _maxMemoryAllowed; // signatures count in WM

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

@@ -103,6 +103,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Memory, Odometry_variance_ang,);
RTABMAP_STATS(Memory, Odometry_variance_lin,);
RTABMAP_STATS(Memory, Distance_travelled, m);
RTABMAP_STATS(Memory, RAM_usage, MB);
RTABMAP_STATS(Timing, Memory_update, ms);
RTABMAP_STATS(Timing, Neighbor_link_refining, ms);

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

@@ -56,6 +56,8 @@ public:
// x,y, theta
Transform(float x, float y, float theta);
Transform clone() const;
float r11() const {return data()[0];}
float r12() const {return data()[1];}
float r13() const {return data()[2];}

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

@@ -114,6 +114,7 @@ private:
bool _newWordsComparedTogether;
int _lastWordId;
bool useDistanceL1_;
float _rebalancingFactor;
FlannIndex * _flannIndex;
cv::Mat _dataTree;
NNStrategy _strategy;

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

@@ -74,6 +74,12 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
bool smoothing,
float depthErrorRatio = 0.02f);
Eigen::Vector3f RTABMAP_EXP projectDepthTo3DRay(
const cv::Size & imageSize,
float x, float y,
float cx, float cy,
float fx, float fy);
RTABMAP_DEPRECATED (pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,

8
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -304,6 +304,14 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
float dilationVoxelSize = 1.0f, // VOXEL_GRID_DILATION
int dilationIterations = 0); // VOXEL_GRID_DILATION
void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
bool forceGroundNormalsUp = false);
void RTABMAP_EXP adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint = Eigen::Vector3f(0,0,0),
bool forceGroundNormalsUp = false);
void RTABMAP_EXP adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,

152
corelib/src/BayesFilter.cpp
View File

@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/Parameters.h"
#include <iostream>
#include <set>
#include <unordered_set>
#include "rtabmap/utilite/UtiLite.h"
@@ -125,6 +127,7 @@ void BayesFilter::reset()
{
_posterior.clear();
_prediction = cv::Mat();
_neighborsIndex.clear();
}
const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory, const std::map<int, float> & likelihood)
@@ -219,7 +222,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
return _posterior;
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids) const
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
{
if(!_fullPredictionUpdate && !_prediction.empty())
{
@@ -236,13 +239,17 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
UTimer timerGlobal;
timerGlobal.start();
std::map<int, int> idToIndexMap;
std::unordered_map<int,int> idToIndexMap;
idToIndexMap.reserve(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
UASSERT_MSG(ids[i] != 0, "Signature id is null ?!?");
idToIndexMap.insert(idToIndexMap.end(), std::make_pair(ids[i], i));
if(ids[i]>0)
{
idToIndexMap[ids[i]] = i;
}
}
//int rows = prediction.rows;
cv::Mat prediction = cv::Mat::zeros(ids.size(), ids.size(), CV_32FC1);
int cols = prediction.cols;
@@ -260,7 +267,13 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
// Set high values (gaussians curves) to loop closure neighbors
// ADD prob for each neighbors
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true);
std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, true, true);
if(!_fullPredictionUpdate)
{
uInsert(_neighborsIndex, std::make_pair(ids[i], neighbors));
}
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -271,7 +284,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
}
else
{
if(iter->second == 0)
if(iter->second == 0 && idToIndexMap.find(iter->first)!=idToIndexMap.end())
{
idsLoopMargin.push_back(iter->first);
}
@@ -288,10 +301,16 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
if(!_fullPredictionUpdate)
{
uInsert(_neighborsIndex, std::make_pair(*iter, neighbors));
}
float sum = 0.0f; // sum values added
sum += this->addNeighborProb(prediction, idToIndexMap.at(*iter), neighbors, idToIndexMap);
int index = idToIndexMap.at(*iter);
sum += this->addNeighborProb(prediction, index, neighbors, idToIndexMap);
idsDone.insert(*iter);
this->normalize(prediction, idToIndexMap.at(*iter), sum, ids[0]<0);
this->normalize(prediction, index, sum, ids[0]<0);
}
}
else
@@ -405,7 +424,7 @@ void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float adde
cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
const Memory * memory,
const std::vector<int> & oldIds,
const std::vector<int> & newIds) const
const std::vector<int> & newIds)
{
UTimer timer;
UDEBUG("");
@@ -417,34 +436,32 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
oldIds.size() == (unsigned int)oldPrediction.rows);
cv::Mat prediction = cv::Mat::zeros(newIds.size(), newIds.size(), CV_32FC1);
UDEBUG("time creating prediction = %fs", timer.restart());
// Create id to index maps
std::map<int, int> oldIdToIndexMap;
std::map<int, int> newIdToIndexMap;
for(unsigned int i=0; i<oldIds.size() || i<newIds.size(); ++i)
std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
UDEBUG("time creating old ids set = %fs", timer.restart());
std::unordered_map<int,int> newIdToIndexMap;
newIdToIndexMap.reserve(newIds.size());
for(unsigned int i=0; i<newIds.size(); ++i)
{
if(i<oldIds.size())
if(newIds[i]>0)
{
UASSERT(oldIds[i]);
oldIdToIndexMap.insert(oldIdToIndexMap.end(), std::make_pair(oldIds[i], i));
//UDEBUG("oldIdToIndexMap[%d] = %d", oldIds[i], i);
}
if(i<newIds.size())
{
UASSERT(newIds[i]);
newIdToIndexMap.insert(newIdToIndexMap.end(), std::make_pair(newIds[i], i));
//UDEBUG("newIdToIndexMap[%d] = %d", newIds[i], i);
newIdToIndexMap[newIds[i]] = i;
}
}
UDEBUG("time creating id-index maps = %fs", timer.restart());
UDEBUG("time creating id-index vector (size=%d oldIds.back()=%d newIds.back()=%d) = %fs", (int)newIdToIndexMap.size(), oldIds.back(), newIds.back(), timer.restart());
//Get removed ids
std::set<int> removedIds;
for(unsigned int i=0; i<oldIds.size(); ++i)
{
if(!uContains(newIdToIndexMap, oldIds[i]))
if(oldIds[i] > 0 && newIdToIndexMap.find(oldIds[i]) == newIdToIndexMap.end())
{
removedIds.insert(removedIds.end(), oldIds[i]);
_neighborsIndex.erase(oldIds[i]);
UDEBUG("removed id=%d at oldIndex=%d", oldIds[i], i);
}
}
@@ -476,16 +493,31 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
UDEBUG("From removed id %d, %d neighbors to update.", oldIds[i], count);
}
}
if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
if(i<newIds.size() && oldIdsSet.find(newIds[i]) == oldIdsSet.end())
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true);
if(_neighborsIndex.find(newIds[i]) == _neighborsIndex.end())
{
std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, true, true);
for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
std::map<int, std::map<int, int> >::iterator jter = _neighborsIndex.find(iter->first);
if(jter != _neighborsIndex.end())
{
uInsert(jter->second, std::make_pair(newIds[i], iter->second));
}
}
_neighborsIndex.insert(std::make_pair(newIds[i], neighbors));
}
const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
this->normalize(prediction, i, sum, newIds[0]<0);
++added;
int count = 0;
for(std::map<int,int>::iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
for(std::map<int,int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
if(uContains(oldIdToIndexMap, iter->first) &&
if(oldIdsSet.find(iter->first)!=oldIdsSet.end() &&
removedIds.find(iter->first) == removedIds.end())
{
idsToUpdate.insert(iter->first);
@@ -497,51 +529,35 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
}
UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
UTimer t1;
double e0=0,e1=0, e2=0, e3=0, e4=0;
// update modified/added ids
int modified = 0;
std::set<int> idsDone;
for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
{
if(idsDone.find(*iter) == idsDone.end() && *iter > 0)
int id = *iter;
if(id > 0 && id<(int)newIdToIndexMap.size())
{
std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, true);
int index = newIdToIndexMap.at(id);
std::list<int> idsLoopMargin;
//filter neighbors in STM
for(std::map<int, int>::iterator jter=neighbors.begin(); jter!=neighbors.end();)
if(index > 0)
{
if(memory->isInSTM(jter->first))
{
neighbors.erase(jter++);
}
else
{
if(jter->second == 0)
{
idsLoopMargin.push_back(jter->first);
}
++jter;
}
}
e0 = t1.ticks();
std::map<int, std::map<int, int> >::iterator kter = _neighborsIndex.find(id);
UASSERT_MSG(kter != _neighborsIndex.end(), uFormat("Did not find %d (current index size=%d)", id, (int)_neighborsIndex.size()).c_str());
const std::map<int, int> & neighbors = kter->second;
e1+=t1.ticks();
// should at least have 1 id in idsMarginLoop
if(idsLoopMargin.size() == 0)
{
UFATAL("No 0 margin neighbor for signature %d !?!?", *iter);
}
// same neighbor tree for loop signatures (margin = 0)
for(std::list<int>::iterator iter = idsLoopMargin.begin(); iter!=idsLoopMargin.end(); ++iter)
{
int index = newIdToIndexMap.at(*iter);
float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
idsDone.insert(*iter);
e3+=t1.ticks();
this->normalize(prediction, index, sum, newIds[0]<0);
++modified;
e4+=t1.ticks();
}
}
}
UDEBUG("time updating modified/added %d ids = %fs", idsToUpdate.size(), timer.restart());
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
//UDEBUG("oldIds.size()=%d, oldPrediction.cols=%d, oldPrediction.rows=%d", oldIds.size(), oldPrediction.cols, oldPrediction.rows);
//UDEBUG("newIdToIndexMap.size()=%d, prediction.cols=%d, prediction.rows=%d", newIdToIndexMap.size(), prediction.cols, prediction.rows);
@@ -624,20 +640,22 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
_posterior = newPosterior;
}
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::map<int, int> & idToIndexMap) const
float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::unordered_map<int, int> & idToIndex) const
{
UASSERT((unsigned int)prediction.cols == idToIndexMap.size() &&
(unsigned int)prediction.rows == idToIndexMap.size() &&
col < (unsigned int)prediction.cols &&
UASSERT(col < (unsigned int)prediction.cols &&
col < (unsigned int)prediction.rows);
float sum=0;
float sum=0.0f;
float * dataPtr = (float*)prediction.data;
for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
{
int index = uValue(idToIndexMap, iter->first, -1);
if(index >= 0)
if(iter->first>=0)
{
sum += ((float*)prediction.data)[col + index*prediction.cols] = _predictionLC[iter->second+1];
std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
if(jter != idToIndex.end())
{
sum += dataPtr[col + jter->second*prediction.cols] = _predictionLC[iter->second+1];
}
}
}
return sum;

57
corelib/src/CameraRGB.cpp
View File

@@ -71,6 +71,7 @@ CameraImages::CameraImages() :
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -102,6 +103,7 @@ CameraImages::CameraImages(const std::string & path,
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -242,15 +244,43 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
const std::list<std::string> & filenames = _dir->getFileNames();
for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
{
// format is text_12234456.12334_text.png
// format is text_1223445645.12334_text.png or text_122344564512334_text.png
// If no decimals, 10 first number are the seconds
std::list<std::string> list = uSplit(*iter, '.');
if(list.size() == 3)
if(list.size() == 3 || list.size() == 2)
{
list.pop_back(); // remove extension
std::string decimals = uSplitNumChar(list.back()).front();
list.pop_back();
std::string sec = uSplitNumChar(list.back()).back();
double stamp = uStr2Double(sec + "." + decimals);
double stamp = 0.0;
if(list.size() == 1)
{
std::list<std::string> numberList = uSplitNumChar(list.front());
for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
{
if(uIsNumber(*iter))
{
std::string decimals;
std::string sec;
if(iter->length()>10)
{
decimals = iter->substr(10, iter->size()-10);
sec = iter->substr(0, 10);
}
else
{
sec = *iter;
}
stamp = uStr2Double(sec + "." + decimals);
break;
}
}
}
else
{
std::string decimals = uSplitNumChar(list.back()).front();
list.pop_back();
std::string sec = uSplitNumChar(list.back()).back();
stamp = uStr2Double(sec + "." + decimals);
}
if(stamp > 0.0)
{
_stamps.push_back(stamp);
@@ -338,19 +368,19 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
UERROR("Cannot read pose file \"%s\".", filePath.c_str());
return false;
}
else if((format != 1 && format != 5 && format != 6 && format != 7) && poses.size() != this->imagesCount())
else if((format != 1 && format != 5 && format != 6 && format != 7 && format != 9) && poses.size() != this->imagesCount())
{
UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
"the pose file path if you don't want to use it (current file path=%s).",
(int)poses.size(), this->imagesCount(), filePath.c_str());
return false;
}
else if((format == 1 || format == 5 || format == 6 || format == 7) && inOutStamps.size() == 0)
else if((format == 1 || format == 5 || format == 6 || format == 7 || format == 9) && inOutStamps.size() == 0)
{
UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats, images must have timestamps!");
UERROR("When using RGBD-SLAM, GPS, MALAGA, ST LUCIA and EuRoC MAV formats, images must have timestamps!");
return false;
}
else if(format == 1 || format == 5 || format == 6 || format == 7)
else if(format == 1 || format == 5 || format == 6 || format == 7 || format == 9)
{
UDEBUG("");
//Match ground truth values with images
@@ -384,7 +414,6 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
if(fabs(*ster-stampEnd) > maxTimeDiff || fabs(*ster-stampBeg) > maxTimeDiff)
{
warned = true;
if(!warned)
{
UWARN("Cannot interpolate pose for stamp %f between %f and %f (> maximum time diff of %f sec)",
@@ -393,6 +422,7 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
stampEnd,
maxTimeDiff);
}
warned=true;
}
else
{
@@ -744,7 +774,12 @@ SensorData CameraImages::captureImage(CameraInfo * info)
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
if(_scanForceGroundNormalsUp)
{
util3d::adjustNormalsToViewPoint(cloudNormals, Eigen::Vector3f(0,0,0), _scanForceGroundNormalsUp);
}
scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
UDEBUG("Normals computed (k=%d radius=%f)", _scanNormalsK, _scanNormalsRadius);
}
else
{

33
corelib/src/CameraThread.cpp
View File

@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/StereoDense.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <opencv2/stitching/detail/exposure_compensate.hpp>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
@@ -49,6 +49,7 @@ namespace rtabmap
CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_camera(camera),
_mirroring(false),
_stereoExposureCompensation(false),
_colorOnly(false),
_imageDecimation(1),
_stereoToDepth(false),
@@ -269,6 +270,33 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
if(info) info->timeMirroring = timer.ticks();
}
if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
{
#if CV_MAJOR_VERSION < 3
UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.")
#else
UDEBUG("");
UTimer timer;
cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
std::vector<cv::UMat> images;
std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
compensator->feed(topLeftCorners, images, masks);
cv::Mat img = data.imageRaw().clone();
compensator->apply(0, cv::Point(0,0), img, masks[0]);
data.setImageRaw(img);
img = data.rightRaw().clone();
compensator->apply(1, cv::Point(0,0), img, masks[1]);
data.setDepthOrRightRaw(img);
cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
if(info) info->timeStereoExposureCompensation = timer.ticks();
#endif
}
if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
@@ -284,7 +312,8 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().localTransform(),
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
data.stereoCameraModel().left().imageSize());
data.setCameraModel(model);
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());

2
corelib/src/DBDriver.cpp
View File

@@ -1070,7 +1070,7 @@ cv::Mat DBDriver::loadOptimizedMesh(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> > * texCoords,
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
#else
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
#endif

29
corelib/src/FlannIndex.cpp
View File

@@ -38,7 +38,8 @@ FlannIndex::FlannIndex():
featuresType_(0),
featuresDim_(0),
isLSH_(false),
useDistanceL1_(false)
useDistanceL1_(false),
rebalancingFactor_(2.0f)
{
}
FlannIndex::~FlannIndex()
@@ -134,7 +135,8 @@ unsigned int FlannIndex::memoryUsed() const
void FlannIndex::buildLinearIndex(
const cv::Mat & features,
bool useDistanceL1)
bool useDistanceL1,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -142,6 +144,7 @@ void FlannIndex::buildLinearIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::LinearIndexParams params;
@@ -180,7 +183,8 @@ void FlannIndex::buildLinearIndex(
void FlannIndex::buildKDTreeIndex(
const cv::Mat & features,
int trees,
bool useDistanceL1)
bool useDistanceL1,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -188,6 +192,7 @@ void FlannIndex::buildKDTreeIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::KDTreeIndexParams params(trees);
@@ -227,7 +232,8 @@ void FlannIndex::buildKDTreeSingleIndex(
const cv::Mat & features,
int leafMaxSize,
bool reorder,
bool useDistanceL1)
bool useDistanceL1,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -235,6 +241,7 @@ void FlannIndex::buildKDTreeSingleIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = useDistanceL1;
rebalancingFactor_ = rebalancingFactor;
rtflann::KDTreeSingleIndexParams params(leafMaxSize, reorder);
@@ -274,7 +281,8 @@ void FlannIndex::buildLSHIndex(
const cv::Mat & features,
unsigned int table_number,
unsigned int key_size,
unsigned int multi_probe_level)
unsigned int multi_probe_level,
float rebalancingFactor)
{
this->release();
UASSERT(index_ == 0);
@@ -282,6 +290,7 @@ void FlannIndex::buildLSHIndex(
featuresType_ = features.type();
featuresDim_ = features.cols;
useDistanceL1_ = true;
rebalancingFactor_ = rebalancingFactor;
rtflann::Matrix<unsigned char> dataset(features.data, features.rows, features.cols);
index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
@@ -315,8 +324,8 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
rtflann::Index<rtflann::Hamming<unsigned char> > * index = (rtflann::Index<rtflann::Hamming<unsigned char> >*)index_;
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
// Rebuild index if it is now X times in size
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
@@ -333,7 +342,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
@@ -347,7 +356,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();
@@ -361,7 +370,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features)
removedPts = index->removedCount();
index->addPoints(points, 0);
// Rebuild index if it doubles in size
if(index->sizeAtBuild() * 2 < index->size()+index->removedCount())
if(rebalancingFactor_ > 1.0f && size_t(float(index->sizeAtBuild()) * rebalancingFactor_) < index->size()+index->removedCount())
{
UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount()));
index->buildIndex();

85
corelib/src/Graph.cpp
View File

@@ -161,10 +161,10 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1
std::map<int, double> * stamps) // optional for format 1 and 9
{
UDEBUG("%s format=%d", filePath.c_str(), format);
if(format==3) // TORO
@@ -202,12 +202,50 @@ bool importPoses(
std::string str;
std::getline(file, str);
if(str.size() && str.at(str.size()-1) == '\r')
{
str = str.substr(0, str.size()-1);
}
if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
{
continue;
}
if(format == 8) // Karlsruhe format
if(format == 9) // EuRoC format
{
std::list<std::string> strList = uSplit(str, ',');
if(strList.size() == 17)
{
double stamp = uStr2Double(strList.front())/1000000000.0;
strList.pop_front();
std::vector<std::string> v = uListToVector(strList);
Transform pose(uStr2Float(v[0]), uStr2Float(v[1]), uStr2Float(v[2]), // x y z
uStr2Float(v[4]), uStr2Float(v[5]), uStr2Float(v[6]), uStr2Float(v[3])); // qw qx qy qz -> qx qy qz qw
if(pose.isNull())
{
UWARN("Null transform read!? line parsed: \"%s\"", str.c_str());
}
else
{
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
// we need to rotate from IMU frame to world frame
Transform t( 0, 0, 1, 0,
0, -1, 0, 0,
1, 0, 0, 0);
pose = pose * t;
poses.insert(std::make_pair(id, pose));
}
}
else
{
UERROR("Error parsing \"%s\" with EuRoC MAV format (should have 17 values: stamp x y z qw qx qy qz vx vy vz vr vp vy ax ay az)", str.c_str());
}
}
else if(format == 8) // Karlsruhe format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 10)
@@ -364,7 +402,7 @@ bool importPoses(
}
else
{
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y)", str.c_str());
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y, found %d)", str.c_str(), (int)strList.size());
}
}
else if(format == 1) // rgbd-slam format
@@ -911,6 +949,20 @@ std::multimap<int, int>::const_iterator findLink(
return links.end();
}
std::multimap<int, Link> filterDuplicateLinks(
const std::multimap<int, Link> & links)
{
std::multimap<int, Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(graph::findLink(output, iter->second.from(), iter->second.to(), true) == output.end())
{
output.insert(*iter);
}
}
return output;
}
std::multimap<int, Link> filterLinks(
const std::multimap<int, Link> & links,
Link::Type filteredType)
@@ -1820,6 +1872,20 @@ int findNearestNode(
const rtabmap::Transform & targetPose)
{
int id = 0;
std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
if(nearestNodes.size())
{
id = nearestNodes[0];
}
return id;
}
std::vector<int> findNearestNodes(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose,
int k)
{
std::vector<int> nearestIds;
if(nodes.size() && !targetPose.isNull())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -1837,14 +1903,15 @@ int findNearestNode(
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
kdTree->nearestKSearch(pt, 1, ind, dist);
if(ind.size() && dist.size() && ind[0] >= 0)
kdTree->nearestKSearch(pt, k, ind, dist);
nearestIds.resize(ind.size());
for(unsigned int i=0; i<ind.size(); ++i)
{
//UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
id = ids[ind[0]];
nearestIds[i] = ids[ind[i]];
}
}
return id;
return nearestIds;
}
// return <id, sqrd distance>, excluding query

4
corelib/src/Link.cpp
View File

@@ -65,13 +65,13 @@ Link::Link(int from,
double Link::rotVariance() const
{
double min = uMin3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
double min = uMax3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
UASSERT(min > 0.0);
return 1.0/min;
}
double Link::transVariance() const
{
double min = uMin3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
double min = uMax3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
UASSERT(min > 0.0);
return 1.0/min;
}

165
corelib/src/Memory.cpp
View File

@@ -84,6 +84,7 @@ Memory::Memory(const ParametersMap & parameters) :
_generateIds(Parameters::defaultMemGenerateIds()),
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_mapLabelsAdded(Parameters::defaultMemMapLabelsAdded()),
_depthAsMask(Parameters::defaultMemDepthAsMask()),
_imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
@@ -98,6 +99,7 @@ Memory::Memory(const ParametersMap & parameters) :
_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
_visCorType(Parameters::defaultVisCorType()),
_idCount(kIdStart),
_idMapCount(kIdStart),
_lastSignature(0),
@@ -428,38 +430,49 @@ Memory::~Memory()
void Memory::parseParameters(const ParametersMap & parameters)
{
uInsert(parameters_, parameters);
ParametersMap params = parameters;
UDEBUG("");
ParametersMap::const_iterator iter;
Parameters::parse(parameters, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(parameters, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(parameters, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(parameters, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(parameters, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(parameters, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
Parameters::parse(parameters, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(parameters, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(parameters, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
Parameters::parse(parameters, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
Parameters::parse(parameters, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(parameters, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(parameters, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kMemBinDataKept(), _binDataKept);
Parameters::parse(params, Parameters::kMemRawDescriptorsKept(), _rawDescriptorsKept);
Parameters::parse(params, Parameters::kMemSaveDepth16Format(), _saveDepth16Format);
Parameters::parse(params, Parameters::kMemReduceGraph(), _reduceGraph);
Parameters::parse(params, Parameters::kMemNotLinkedNodesKept(), _notLinkedNodesKeptInDb);
Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
Parameters::parse(params, Parameters::kMemMapLabelsAdded(), _mapLabelsAdded);
Parameters::parse(params, Parameters::kMemRehearsalSimilarity(), _similarityThreshold);
Parameters::parse(params, Parameters::kMemRecentWmRatio(), _recentWmRatio);
Parameters::parse(params, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(params, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(params, Parameters::kMemDepthAsMask(), _depthAsMask);
Parameters::parse(params, Parameters::kMemImagePreDecimation(), _imagePreDecimation);
Parameters::parse(params, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
Parameters::parse(params, Parameters::kMemCompressionParallelized(), _compressionParallelized);
Parameters::parse(params, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
Parameters::parse(params, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
Parameters::parse(params, Parameters::kVisCorType(), _visCorType);
if(_visCorType != 0)
{
UWARN("%s is not 0 (Features Matching), the only approach supported for loop closure transformation estimation. Setting to 0...",
Parameters::kVisCorType().c_str());
_visCorType = 0;
uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
}
UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -477,23 +490,23 @@ void Memory::parseParameters(const ParametersMap & parameters)
if(_dbDriver)
{
_dbDriver->parseParameters(parameters);
_dbDriver->parseParameters(params);
}
// Keypoint stuff
if(_vwd)
{
_vwd->parseParameters(parameters);
_vwd->parseParameters(params);
}
Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
Parameters::parse(params, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(params, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(params, Parameters::kKpBadSignRatio(), _badSignRatio);
//Keypoint detector
UASSERT(_feature2D != 0);
Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
if((iter=parameters.find(Parameters::kKpDetectorStrategy())) != parameters.end())
if((iter=params.find(Parameters::kKpDetectorStrategy())) != params.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
@@ -517,11 +530,11 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
else if(_feature2D)
{
_feature2D->parseParameters(parameters);
_feature2D->parseParameters(params);
}
Registration::Type regStrategy = Registration::kTypeUndef;
if((iter=parameters.find(Parameters::kRegStrategy())) != parameters.end())
if((iter=params.find(Parameters::kRegStrategy())) != params.end())
{
regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
}
@@ -538,23 +551,23 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
else if(_registrationPipeline)
{
_registrationPipeline->parseParameters(parameters);
_registrationPipeline->parseParameters(params);
}
if(_registrationIcp)
{
_registrationIcp->parseParameters(parameters);
_registrationIcp->parseParameters(params);
}
if(_occupancy)
{
_occupancy->parseParameters(parameters);
_occupancy->parseParameters(params);
}
// do this after all parameters are parsed
// do this after all params are parsed
// SLAM mode vs Localization mode
iter = parameters.find(Parameters::kMemIncrementalMemory());
if(iter != parameters.end())
iter = params.find(Parameters::kMemIncrementalMemory());
if(iter != params.end())
{
bool value = uStr2Bool(iter->second.c_str());
if(value == false && _incrementalMemory)
@@ -578,6 +591,24 @@ void Memory::parseParameters(const ParametersMap & parameters)
}
_incrementalMemory = value;
}
if(_useOdometryFeatures)
{
int visFeatureType = Parameters::defaultVisFeatureType();
int kpDetectorStrategy = Parameters::defaultKpDetectorStrategy();
Parameters::parse(parameters_, Parameters::kVisFeatureType(), visFeatureType);
Parameters::parse(parameters_, Parameters::kKpDetectorStrategy(), kpDetectorStrategy);
if(visFeatureType != kpDetectorStrategy)
{
UWARN("%s is enabled, but %s and %s parameters are not the same! Disabling %s...",
Parameters::kMemUseOdomFeatures().c_str(),
Parameters::kVisFeatureType().c_str(),
Parameters::kKpDetectorStrategy().c_str(),
Parameters::kMemUseOdomFeatures().c_str());
_useOdometryFeatures = false;
uInsert(parameters_, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
}
}
}
void Memory::preUpdate()
@@ -1065,6 +1096,7 @@ std::map<int, int> Memory::getNeighborsId(
bool incrementMarginOnLoop, // default false
bool ignoreLoopIds, // default false
bool ignoreIntermediateNodes, // default false
bool ignoreLocalSpaceLoopIds, // default false, ignored if ignoreLoopIds=true
const std::set<int> & nodesSet,
double * dbAccessTime
) const
@@ -1150,7 +1182,7 @@ std::map<int, int> Memory::getNeighborsId(
nextMargin.insert(iter->first);
}
}
else if(!ignoreLoopIds)
else if(!ignoreLoopIds && (!ignoreLocalSpaceLoopIds || iter->second.type()!=Link::kLocalSpaceClosure))
{
if(incrementMarginOnLoop)
{
@@ -1709,7 +1741,7 @@ cv::Mat Memory::loadOptimizedMesh(
std::map<int, Transform> * poses,
std::vector<std::vector<std::vector<unsigned int> > > * polygons,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>> > * texCoords,
std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
#else
std::vector<std::vector<Eigen::Vector2f> > * texCoords,
#endif
@@ -2277,13 +2309,13 @@ Transform Memory::computeTransform(
// make sure we have all data needed
// load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null)
if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && fromS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty()))
{
fromS.sensorData() = getNodeData(fromS.id());
}
if(((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
if((((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired()) && toS.sensorData().imageCompressed().empty()) ||
(_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) ||
(_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty()))
{
@@ -2292,13 +2324,13 @@ Transform Memory::computeTransform(
// uncompress only what we need
cv::Mat imgBuf, depthBuf, laserBuf, userBuf;
fromS.sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
toS.sensorData().uncompressData(
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0,
(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&imgBuf:0,
((_reextractLoopClosureFeatures || _visCorType==1) && _registrationPipeline->isImageRequired())?&depthBuf:0,
_registrationPipeline->isScanRequired()?&laserBuf:0,
_registrationPipeline->isUserDataRequired()?&userBuf:0);
@@ -2331,11 +2363,14 @@ Transform Memory::computeTransform(
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
}
if(guess.isNull() && !_registrationPipeline->isImageRequired())
if(guess.isNull() && (!_registrationPipeline->isImageRequired() || _visCorType==1))
{
UDEBUG("");
// no visual in the pipeline, make visual registration for guess
RegistrationVis regVis(parameters_);
// make sure feature matching is used instead of optical flow to compute the guess
ParametersMap parameters = parameters_;
uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
RegistrationVis regVis(parameters);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
if(!guess.isNull())
{
@@ -3399,7 +3434,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
meanWordsPerLocation = _vwd->getTotalActiveReferences() / treeSize;
}
if(_parallelized)
if(_parallelized && !isIntermediateNode)
{
UDEBUG("Start dictionary update thread");
preUpdateThread.start();
@@ -3447,7 +3482,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
cv::Mat depthMask;
if(!decimatedData.depthRaw().empty())
if(!decimatedData.depthRaw().empty() && _depthAsMask)
{
if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3607,20 +3642,20 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
UDEBUG("Joining dictionary update thread... thread finished!");
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
if(_parallelized)
{
UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
else
{
UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
std::list<int> wordIds;
if(descriptors.rows)
{
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemJoining_dictionary_update(), t*1000.0f);
if(_parallelized)
{
UDEBUG("time descriptor and memory update (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
else
{
UDEBUG("time descriptor (%d of size=%d) = %fs", descriptors.rows, descriptors.cols, t);
}
// In case the number of features we want to do quantization is lower
// than extracted ones (that would be used for transform estimation)
std::vector<bool> inliers;

151
corelib/src/OccupancyGrid.cpp
View File

@@ -70,7 +70,10 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
erode_(Parameters::defaultGridGlobalEroded()),
footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
xMin_(0.0f),
yMin_(0.0f)
yMin_(0.0f),
cloudAssembling_(false),
assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>)
{
this->parseParameters(parameters);
}
@@ -198,6 +201,16 @@ void OccupancyGrid::setCellSize(float cellSize)
}
}
void OccupancyGrid::setCloudAssembling(bool enabled)
{
cloudAssembling_ = enabled;
if(!cloudAssembling_)
{
assembledGround_->clear();
assembledObstacles_->clear();
}
}
void OccupancyGrid::createLocalMap(
const Signature & node,
cv::Mat & ground,
@@ -386,9 +399,11 @@ void OccupancyGrid::clear()
xMin_ = 0.0f;
yMin_ = 0.0f;
addedNodes_.clear();
assembledGround_->clear();
assembledObstacles_->clear();
}
const cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
{
xMin = xMin_;
yMin = yMin_;
@@ -470,6 +485,9 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
bool assembledGroundUpdated = false;
bool assembledObstaclesUpdated = false;
if(graphOptimized || graphChanged)
{
if(graphChanged)
@@ -481,6 +499,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
UINFO("Graph optimized!");
}
if(cloudAssembling_)
{
assembledGround_->clear();
assembledObstacles_->clear();
}
if(!fullUpdate_ && !graphChanged && !map_.empty()) // incremental, just move cells
{
// 1) recreate all local maps
@@ -489,27 +513,32 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
std::map<int, std::pair<int, int> > tmpIndices;
for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
{
if(iter->second.first)
if(!uContains(cache_, iter->first) && transforms.find(iter->first) != transforms.end())
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
if(iter->second.first)
{
emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
if(iter->second.second)
{
occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
}
tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
}
for(int y=1; y<map_.rows-1; ++y)
for(int y=0; y<map_.rows; ++y)
{
for(int x=1; x<map_.cols-1; ++x)
for(int x=0; x<map_.cols; ++x)
{
float * info = mapInfo_.ptr<float>(y,x);
int nodeId = (int)info[0];
if(nodeId > 0 && map_.at<char>(y,x) >= 0)
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
if(tter != transforms.end() && !uContains(cache_, nodeId))
if(tmpIndices.find(nodeId)!=tmpIndices.end())
{
std::map<int, Transform>::iterator tter = transforms.find(nodeId);
UASSERT(tter != transforms.end());
cv::Point3f pt(info[1], info[2], 0.0f);
pt = util3d::transformPoint(pt, tter->second);
@@ -547,9 +576,26 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
}
else if(nodeId > 0)
{
UERROR("Cell referred b node %d is unknown!?", nodeId);
}
}
}
//verify if all cells were added
for(std::map<int, std::pair<int, int> >::iterator iter=tmpIndices.begin(); iter!=tmpIndices.end(); ++iter)
{
std::map<int, cv::Mat>::iterator jter = emptyLocalMaps.find(iter->first);
UASSERT_MSG((iter->second.first == 0 && (jter==emptyLocalMaps.end() || jter->second.empty())) ||
(iter->second.first != 0 && jter!=emptyLocalMaps.end() && jter->second.cols == iter->second.first),
uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
jter = occupiedLocalMaps.find(iter->first);
UASSERT_MSG((iter->second.second == 0 && (jter==occupiedLocalMaps.end() || jter->second.empty())) ||
(iter->second.second != 0 && jter!=occupiedLocalMaps.end() && jter->second.cols == iter->second.second),
uFormat("iter->second.first=%d jter->second.cols=%d", iter->second.first, jter!=emptyLocalMaps.end()?jter->second.cols:-1).c_str());
}
UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
}
@@ -575,25 +621,29 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
std::list<std::pair<int, Transform> > poses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
// add old poses that were not in the current map (they were just retrieved from LTM)
for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(0); iter!=posesIn.end(); ++iter)
{
for(std::map<int, Transform>::const_iterator iter=posesIn.upper_bound(lastId); iter!=posesIn.end(); ++iter)
if(addedNodes_.find(iter->first) == addedNodes_.end())
{
poses.push_back(*iter);
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
{
if(iter->first < 0)
{
if(iter->first < 0)
{
poses.push_back(*iter);
}
else
{
break;
}
poses.push_back(*iter);
}
else
{
break;
}
}
for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
UASSERT(!iter->second.isNull());
@@ -662,6 +712,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
maxY = vo[1];
}
uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
if(cloudAssembling_)
{
*assembledGround_ += *util3d::laserScanToPointCloudRGB(pair.first, iter->second, 0, 255, 0);
assembledGroundUpdated = true;
}
}
//obstacles
@@ -698,6 +754,12 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
maxY = vo[1];
}
uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
if(cloudAssembling_)
{
*assembledObstacles_ += *util3d::laserScanToPointCloudRGB(pair.second, iter->second, 255, 0, 0);
assembledObstaclesUpdated = true;
}
}
}
}
@@ -770,6 +832,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
newMapSize.height = (yMax - yMin) / cellSize_+0.5f;
UDEBUG("%d/%d -> %d/%d", map_.cols, map_.rows, newMapSize.width, newMapSize.height);
UASSERT(newMapSize.width >= map_.cols && newMapSize.height >= map_.rows);
UASSERT(newMapSize.width >= map_.cols+deltaX && newMapSize.height >= map_.rows+deltaY);
UASSERT(deltaX>=0 && deltaY>=0);
map = cv::Mat::ones(newMapSize, CV_8S)*-1;
mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
@@ -802,8 +866,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
float * ptf = iter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y >=0 && pt.y < map.rows && pt.x >= 0 && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2 && (!incrementalGraphUpdate || value==-1))
{
@@ -912,8 +976,8 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
float * ptf = jter->second.ptr<float>(0,i);
cv::Point2i pt((ptf[0]-xMin)/cellSize_, (ptf[1]-yMin)/cellSize_);
UASSERT_MSG(pt.y>=0 && pt.y < map.rows && pt.x>=0 && pt.x < map.cols,
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d (graph modified=%d)",
kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1, (graphOptimized || graphChanged)?1:0).c_str());
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -1091,10 +1155,37 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
}
}
if(!fullUpdate_)
if(cloudAssembling_)
{
if(assembledGroundUpdated && assembledGround_->size() > 1)
{
assembledGround_ = util3d::voxelize(assembledGround_, cellSize_);
}
if(assembledObstaclesUpdated && assembledGround_->size() > 1)
{
assembledObstacles_ = util3d::voxelize(assembledObstacles_, cellSize_);
}
}
if(!fullUpdate_ && !cloudAssembling_)
{
cache_.clear();
}
else
{
//clear only negative ids
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=cache_.begin(); iter!=cache_.end();)
{
if(iter->first < 0)
{
cache_.erase(iter++);
}
else
{
break;
}
}
}
UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
}

76
corelib/src/OctoMap.cpp
View File

@@ -35,13 +35,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
OctoMap::OctoMap(float voxelSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(voxelSize)),
OctoMap::OctoMap(const ParametersMap & parameters, float occupancyThr) :
hasColor_(false),
fullUpdate_(Parameters::defaultGridGlobalFullUpdate())
{
float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
UASSERT(cellSize>0.0f);
octree_ = new octomap::ColorOcTree(cellSize);
octree_->setOccupancyThres(occupancyThr);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
}
OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate) :
octree_(new octomap::ColorOcTree(cellSize)),
hasColor_(false),
fullUpdate_(fullUpdate)
{
octree_->setOccupancyThres(occupancyThr);
UASSERT(voxelSize>0.0f);
UASSERT(cellSize>0.0f);
}
OctoMap::~OctoMap()
@@ -174,7 +187,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
}
else if(jter == transforms.end() && iter->second.nodeRefId_ > 0)
{
UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
// Note: normal if old nodes were transfered to LTM
//UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
}
}
UDEBUG("%d/%d", copied, (int)occupiedCells_.size());
@@ -191,25 +205,29 @@ void OctoMap::update(const std::map<int, Transform> & poses)
// https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
//
std::list<std::pair<int, Transform> > orderedPoses;
int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
UDEBUG("Last id = %d", lastId);
if(lastId >= 0)
// add old poses that were not in the current map (they were just retrieved from LTM)
for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(0); iter!=poses.end(); ++iter)
{
for(std::map<int, Transform>::const_iterator iter=poses.upper_bound(lastId); iter!=poses.end(); ++iter)
if(addedNodes_.find(iter->first) == addedNodes_.end())
{
orderedPoses.push_back(*iter);
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
}
// insert negative after
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first < 0)
{
if(iter->first < 0)
{
orderedPoses.push_back(*iter);
}
else
{
break;
}
orderedPoses.push_back(*iter);
}
else
{
break;
}
}
@@ -259,7 +277,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
// only clear space (ground points)
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
if ((iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
@@ -267,6 +286,16 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
octomap::ColorOcTreeNode * n = octree_->search(key);
if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
ground_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
@@ -309,7 +338,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::point3d point(pt.x, pt.y, pt.z);
// free cells
if (octree_->computeRayKeys(sensorOrigin, point, keyRay_))
if ((iter->first < 0 || iter->first>lastId) &&
octree_->computeRayKeys(sensorOrigin, point, keyRay_))
{
free_cells.insert(keyRay_.begin(), keyRay_.end());
}
@@ -317,6 +347,16 @@ void OctoMap::update(const std::map<int, Transform> & poses)
octomap::OcTreeKey key;
if (octree_->coordToKeyChecked(point, key))
{
if(iter->first >0 && iter->first<lastId)
{
octomap::ColorOcTreeNode * n = octree_->search(key);
if(n && occupiedCells_.find(n) != occupiedCells_.end() && occupiedCells_.at(n).nodeRefId_ > iter->first)
{
// The cell has been updated from more recent node, don't update the cell
continue;
}
}
occupied_cells.insert(key);
octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);

8
corelib/src/Odometry.cpp
View File

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UProcessInfo.h"
#include "rtabmap/core/ParticleFilter.h"
#include "rtabmap/core/util2d.h"
@@ -99,6 +100,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
_imageDecimation(Parameters::defaultOdomImageDecimation()),
_alignWithGround(Parameters::defaultOdomAlignWithGround()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0),
previousStamp_(0),
@@ -125,6 +127,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
if(_imageDecimation == 0)
{
_imageDecimation = 1;
@@ -398,6 +402,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
info->stamp = data.stamp();
info->interval = dt;
info->transform = t;
if(_publishRAMUsage)
{
info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
}
if(!data.groundTruth().isNull())
{

46
corelib/src/OdometryF2F.cpp
View File

@@ -49,6 +49,8 @@ OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
UASSERT(visKeyFrameThr_>=0);
UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
parameters_ = parameters;
}
OdometryF2F::~OdometryF2F()
@@ -96,6 +98,24 @@ Transform OdometryF2F::computeTransform(
Signature newFrame(data);
if(refFrame_.sensorData().isValid())
{
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
if(guess.isNull() &&
!registrationPipeline_->isImageRequired() &&
registrationPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
registrationPipeline_->parseParameters(params);
}
Signature tmpRefFrame = refFrame_;
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
@@ -104,6 +124,15 @@ Transform OdometryF2F::computeTransform(
!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
registrationPipeline_->parseParameters(params);
}
if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
{
tmpRefFrame = refFrame_;
@@ -112,12 +141,29 @@ Transform OdometryF2F::computeTransform(
newFrame.setWords3(std::multimap<int, cv::Point3f>());
newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
// If optical flow is used, switch temporary to feature matching
int visCorTypeBackup = Parameters::defaultVisCorType();
Parameters::parse(parameters_, Parameters::kVisCorType(), visCorTypeBackup);
if(visCorTypeBackup == 1)
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kVisCorType(), "0"));
registrationPipeline_->parseParameters(params);
}
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
newFrame,
Transform(), // null guess
&regInfo);
if(visCorTypeBackup == 1)
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kVisCorType(), "1"));
registrationPipeline_->parseParameters(params);
}
if(output.isNull())
{
UWARN("Trial with no guess still fail.");

525
corelib/src/OdometryF2M.cpp
View File

@@ -167,6 +167,7 @@ void OdometryF2M::reset(const Transform & initialPose)
bundleModels_.clear();
bundlePoseReferences_.clear();
bundleSeq_ = 0;
lastFrameOldestNewId_ = 0;
}
// return not null transform if odometry is correctly computed
@@ -209,250 +210,275 @@ Transform OdometryF2M::computeTransform(
if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().empty()) &&
lastFrame_->sensorData().isValid())
{
Signature tmpMap = *map_;
Signature tmpMap;
Transform transform;
UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
if(guess.isNull() &&
!regPipeline_->isImageRequired() &&
regPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
// bundle adjustment stuff if used
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, StereoCameraModel> bundleStereoModels;
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
// special case for ICP-only odom, set guess to identity if we just started or reset
!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
regPipeline_->parseParameters(params);
}
if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
for(int guessIteration=0;
guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
++guessIteration)
{
tmpMap = *map_;
// reset matches, but keep already extracted features in lastFrame_->sensorData()
lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
points3DMap.clear();
bundlePoses.clear();
bundleLinks.clear();
bundleModels.clear();
bundleStereoModels.clear();
float maxCorrespondenceDistance = 0.0f;
float pmOutlierRatio = 0.0f;
if(guess.isNull() &&
!regPipeline_->isImageRequired() &&
regPipeline_->isScanRequired() &&
this->framesProcessed() < 2)
{
// only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
regPipeline_->parseParameters(params);
}
if(guessIteration == 1)
{
UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
}
transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
Transform(), // null guess
// special case for ICP-only odom, set guess to identity if we just started or reset
guessIteration==0 && !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
&regInfo);
if(maxCorrespondenceDistance>0.0f)
{
// set it back
ParametersMap params;
params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
regPipeline_->parseParameters(params);
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
UDEBUG("Registration time = %fs", regInfo.totalTime);
if(!transform.isNull())
{
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
UASSERT(map_->getWords().size() && tmpMap.getWords().size());
if(map_->getWords().size() != tmpMap.getWords().size() ||
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
bundleAdjustment_ = 0;
}
else
{
UASSERT(bundlePoses_.size());
UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
UASSERT(bundlePoses_.size() == bundleModels_.size());
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, cv::Point3f> > wordReferences;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
// 3D point
std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
UASSERT(iter3D!=tmpMap.getWords3().end());
points3DMap.insert(*iter3D);
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
// all other references
std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
std::map<int, cv::Point3f> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0)
{
step = 1;
}
int oi=0;
for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
{
if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
{
references.insert(*jter);
++totalBundleWordReferencesUsed;
}
}
//make sure the last reference is here
if(refIter->second.size() > 1)
{
if(references.insert(*refIter->second.rbegin()).second)
{
++totalBundleWordReferencesUsed;
}
}
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
}
wordReferences.insert(std::make_pair(wordId, references));
//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
//{
// UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
//}
}
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
UTimer bundleTimer;
bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
bundleTime = bundleTimer.ticks();
UDEBUG("sba...end");
totalBundleOutliers = (int)sbaOutliers.size();
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
if(info)
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(bundlePoses.size() == bundlePoses_.size()+1)
{
if(!bundlePoses.rbegin()->second.isNull())
{
if(sbaOutliers.size())
{
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
regInfo.inliers = (int)newInliers.size();
regInfo.inliersIDs = newInliers;
}
if(regInfo.inliers < regPipeline_->getMinVisualCorrespondences())
{
regInfo.rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", regInfo.inliers, regPipeline_->getMinVisualCorrespondences());
transform.setNull();
}
else
{
transform = bundlePoses.rbegin()->second;
bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Local bundle adjustment failed! transform is not refined.");
}
}
}
if(!transform.isNull())
{
// make it incremental
transform = this->getPose().inverse() * transform;
}
}
if(transform.isNull())
{
UWARN("Trial with no guess still fail.");
}
else
{
UWARN("Trial with no guess succeeded.");
}
}
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
UDEBUG("Registration time = %fs", regInfo.totalTime);
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, StereoCameraModel> bundleStereoModels;
if(!transform.isNull())
{
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
UASSERT(map_->getWords().size() && tmpMap.getWords().size());
if(map_->getWords().size() != tmpMap.getWords().size() ||
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
if(guessIteration == 1)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
bundleAdjustment_ = 0;
UWARN("Trial with no guess still fail.");
}
if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UASSERT(bundlePoses_.size());
UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
UASSERT(bundlePoses_.size() == bundleModels_.size());
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("no valid camera model!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse();
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, cv::Point3f> > wordReferences;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
int wordId =regInfo.inliersIDs[i];
// 3D point
std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
UASSERT(iter3D!=tmpMap.getWords3().end());
points3DMap.insert(*iter3D);
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
// all other references
std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
std::map<int, cv::Point3f> references;
int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
if(step == 0)
{
step = 1;
}
int oi=0;
for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
{
if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
{
references.insert(*jter);
++totalBundleWordReferencesUsed;
}
}
//make sure the last reference is here
if(refIter->second.size() > 1)
{
if(references.insert(*refIter->second.rbegin()).second)
{
++totalBundleWordReferencesUsed;
}
}
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
//move back point in camera frame (to get depth along z)
cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
}
wordReferences.insert(std::make_pair(wordId, references));
//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
//{
// UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
//}
}
UDEBUG("sba...start");
// set root negative to fix all other poses
std::set<int> sbaOutliers;
UTimer bundleTimer;
bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
bundleTime = bundleTimer.ticks();
UDEBUG("sba...end");
totalBundleOutliers = (int)sbaOutliers.size();
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
if(info)
{
info->localBundlePoses = bundlePoses;
info->localBundleModels = bundleModels;
}
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
if(bundlePoses.size() == bundlePoses_.size()+1)
{
if(!bundlePoses.rbegin()->second.isNull())
{
transform = bundlePoses.rbegin()->second;
bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
if(sbaOutliers.size())
{
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
regInfo.inliers = (int)newInliers.size();
regInfo.inliersIDs = newInliers;
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Local bundle adjustment failed! transform is not refined.");
}
UWARN("Unknown registration error");
}
}
// make it incremental
transform = this->getPose().inverse() * transform;
}
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Unknown registration error");
else if(guessIteration == 1)
{
UWARN("Trial with no guess succeeded!");
}
}
if(!transform.isNull())
@@ -579,6 +605,8 @@ Transform OdometryF2M::computeTransform(
}
UDEBUG("newIds=%d", (int)newIds.size());
int lastFrameOldestNewId = lastFrameOldestNewId_;
lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
iter!=newIds.rend();
++iter)
@@ -610,6 +638,10 @@ Transform OdometryF2M::computeTransform(
mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
if(lastFrameOldestNewId_ > iter->second.first)
{
lastFrameOldestNewId_ = iter->second.first;
}
++added;
}
}
@@ -617,14 +649,55 @@ Transform OdometryF2M::computeTransform(
// remove words in map if max size is reached
if((int)mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features with their aliases
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
// remove oldest outliers first
std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
std::vector<int> ids = regInfo.matchesIDs;
if(regInfo.projectedIDs.size())
{
ids.resize(ids.size() + regInfo.projectedIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.projectedIDs.size(); ++i)
{
if(regInfo.projectedIDs[i]>=lastFrameOldestNewId)
{
ids[regInfo.matchesIDs.size()+oi++] = regInfo.projectedIDs[i];
}
}
ids.resize(regInfo.matchesIDs.size()+oi);
UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
}
for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
{
int id = ids.at(i);
if(inliers.find(id) == inliers.end())
{
std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(id);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
{
if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterFrame->first) -= 1;
}
}
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(id);
mapDescriptors.erase(id);
mapWords.erase(id);
++removed;
}
}
// remove oldest first
std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
{
if(matches.find(iter->first) == matches.end())
if(inliers.find(iter->first) == inliers.end())
{
std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())

7
corelib/src/OdometryFovis.cpp
View File

@@ -143,7 +143,12 @@ Transform OdometryFovis::computeTransform(
data.stereoCameraModel().left().isValidForReprojection() &&
data.stereoCameraModel().right().isValidForReprojection())))
{
UERROR("Invalid camera model!");
UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo camera=%d (reproj=%d|%d)",
(int)data.cameraModels().size(),
data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
data.stereoCameraModel().isValidForProjection()?1:0,
data.stereoCameraModel().left().isValidForReprojection()?1:0,
data.stereoCameraModel().right().isValidForReprojection()?1:0);
return t;
}

73
corelib/src/OdometryORBSLAM2.cpp
View File

@@ -46,11 +46,14 @@ class Tracker: public Tracking
{
public:
Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor) :
Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor)
KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor, long unsigned int maxFeatureMapSize) :
Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor),
maxFeatureMapSize_(maxFeatureMapSize)
{
}
private:
long unsigned int maxFeatureMapSize_;
protected:
void Track()
@@ -248,7 +251,60 @@ protected:
// Check if we need to insert a new keyframe
if(NeedNewKeyFrame())
{
CreateNewKeyFrame();
}
if(maxFeatureMapSize_ > 0)
{
//limit size of the feature map, keep last X recent ones
if(mpMap->KeyFramesInMap()>1 && mpMap->MapPointsInMap()>maxFeatureMapSize_)
{
std::vector<KeyFrame*> kfs = mpMap->GetAllKeyFrames();
std::map<long unsigned int, KeyFrame*> kfsSorted;
for(unsigned int i=1; i<kfs.size(); ++i)
{
kfsSorted.insert(std::make_pair(kfs[i]->mnId, kfs[i]));
}
KeyFrame * lastFrame = kfsSorted.rbegin()->second;
std::vector<MapPoint*> mapPoints = mpMap->GetAllMapPoints();
std::map<long unsigned int, MapPoint*> mapPointsSorted;
for(unsigned int i=0; i<mapPoints.size(); ++i)
{
mapPointsSorted.insert(std::make_pair(mapPoints[i]->mnId, mapPoints[i]));
}
for(std::map<long unsigned int, MapPoint*>::iterator iter=mapPointsSorted.begin();
iter != mapPointsSorted.end() && mpMap->MapPointsInMap()>maxFeatureMapSize_;
++iter)
{
if(!iter->second->IsInKeyFrame(lastFrame))
{
// FIXME: Memory leak: ORB_SLAM2 doesn't delete after removing from the map...
// Not sure when it is safe to delete it, as if I delete just
// after setting the bad flag, the app crashes.
iter->second->SetBadFlag();
}
}
// remove kfs without observations
for(std::map<long unsigned int, KeyFrame*>::iterator iter=kfsSorted.begin();
iter != kfsSorted.end();
++iter)
{
if(iter->second!=lastFrame && iter->second->GetMapPoints().size()==0)
{
// FIXME: Memory leak: ORB_SLAM2 doesn't delete after removing from the map...
// Not sure when it is safe to delete it, as if I delete just
// after setting the bad flag, the app crashes.
iter->second->SetBadFlag();
}
else
{
break;
}
}
}
}
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
@@ -466,7 +522,7 @@ public:
if(CheckFinish())
break;
usleep(30000);
usleep(1000000); // 1 sec
}
SetFinish();
@@ -639,6 +695,9 @@ public:
ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
ofs << std::endl;
int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAM2MapSize();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MapSize(), maxFeatureMapSize);
ofs.close();
//Create KeyFrame Database
@@ -649,7 +708,7 @@ public:
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD);
mpTracker = new ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD, maxFeatureMapSize);
//Initialize the Local Mapping thread and launch
mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, false);
@@ -891,9 +950,9 @@ Transform OdometryORBSLAM2::computeTransform(
covariance.at<double>(0,0) = linearVar;
covariance.at<double>(1,1) = linearVar;
covariance.at<double>(2,2) = linearVar;
covariance.at<double>(3,3) = 0.01;
covariance.at<double>(4,4) = 0.01;
covariance.at<double>(5,5) = 0.01;
covariance.at<double>(3,3) = 0.0001;
covariance.at<double>(4,4) = 0.0001;
covariance.at<double>(5,5) = 0.0001;
}
}

8
corelib/src/OdometryViso2.cpp
View File

@@ -163,10 +163,10 @@ Transform OdometryViso2::computeTransform(
if(viso2_ == 0)
{
VisualOdometryStereo::parameters params;
params.base = data.stereoCameraModel().baseline();
params.calib.cu = data.stereoCameraModel().left().cx();
params.calib.cv = data.stereoCameraModel().left().cy();
params.calib.f = data.stereoCameraModel().left().fx();
params.base = params.match.base = data.stereoCameraModel().baseline();
params.calib.cu = params.match.cu = data.stereoCameraModel().left().cx();
params.calib.cv = params.match.cv = data.stereoCameraModel().left().cy();
params.calib.f = params.match.f = data.stereoCameraModel().left().fx();
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2RansacIters(), params.ransac_iters);
Parameters::parse(viso2Parameters_, Parameters::kOdomViso2InlierThreshold(), params.inlier_threshold);

2
corelib/src/OptimizerG2O.cpp
View File

@@ -965,7 +965,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
if(baseline > 0.0)
{
UWARN("Stereo camera model detected but current "
UDEBUG("Stereo camera model detected but current "
"observation (pt=%d to cam=%d) has null depth (%f m), adding "
"mono observation instead.",
vpt3d->id()-stepVertexId, camId, depth);

3
corelib/src/Parameters.cpp
View File

@@ -172,7 +172,8 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
(icp && group.compare("Icp") == 0) ||
(vis && Parameters::isFeatureParameter(iter->first)) ||
group.compare("Reg") == 0 ||
(vis && group.compare("Vis") == 0))
(vis && group.compare("Vis") == 0) ||
iter->first.compare(kRtabmapPublishRAMUsage())==0)
{
if(stereo)
{

7
corelib/src/Recovery.cpp
View File

@@ -155,7 +155,12 @@ bool databaseRecovery(
}
else
{
if(!rtabmap.process(data, info.odomPose, info.odomCovariance))
if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
{
status = uFormat("High variance detected, triggering a new map...");
rtabmap.triggerNewMap();
}
if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity))
{
status = uFormat("Failed processing node %d.", data.id());
}

12
corelib/src/Registration.cpp
View File

@@ -119,6 +119,16 @@ bool Registration::isUserDataRequired() const
return val;
}
bool Registration::canUseGuess() const
{
bool val = canUseGuessImpl();
if(!val && child_)
{
val = child_->canUseGuess();
}
return val;
}
int Registration::getMinVisualCorrespondences() const
{
int min = this->getMinVisualCorrespondencesImpl();
@@ -197,7 +207,7 @@ Transform Registration::computeTransformationMod(
}
Transform t = computeTransformationImpl(from, to, guess, info);
if(repeatOnce_ && guess.isNull() && !t.isNull())
if(repeatOnce_ && guess.isNull() && !t.isNull() && this->canUseGuess())
{
// redo with guess to get a more accurate transform
t = computeTransformationImpl(from, to, t, info);

74
corelib/src/RegistrationVis.cpp
View File

@@ -67,7 +67,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_nndr(Parameters::defaultVisCorNNDR()),
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment())
_bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
_depthAsMask(Parameters::defaultVisDepthAsMask())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -110,6 +111,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
uInsert(_bundleParameters, parameters);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
@@ -229,6 +231,7 @@ Transform RegistrationVis::computeTransformationImpl(
toSignature.sensorData().imageRaw().rows);
std::string msg;
info.projectedIDs.clear();
////////////////////
// Find correspondences
@@ -288,7 +291,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
cv::Mat depthMask;
if(!fromSignature.sensorData().depthRaw().empty())
if(!fromSignature.sensorData().depthRaw().empty() && _depthAsMask)
{
if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
@@ -496,7 +499,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
cv::Mat depthMask;
if(!toSignature.sensorData().depthRaw().empty())
if(!toSignature.sensorData().depthRaw().empty() && _depthAsMask)
{
if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
@@ -604,14 +607,14 @@ Transform RegistrationVis::computeTransformationImpl(
if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
{
UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).",
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
kptsFrom.size(),
fromSignature.getWords3().size());
}
else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
{
UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
"is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsfrom).",
"is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
kptsFrom.size(),
fromSignature.sensorData().keypoints3D().size());
}
@@ -674,14 +677,14 @@ Transform RegistrationVis::computeTransformationImpl(
if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
{
UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).",
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
(int)kptsTo.size(),
(int)toSignature.getWords3().size());
}
else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
{
UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
"is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo).",
"is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
(int)kptsTo.size(),
(int)toSignature.sensorData().keypoints3D().size());
}
@@ -961,6 +964,12 @@ Transform RegistrationVis::computeTransformationImpl(
for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
{
int matchedIndexFrom = projectedIndexToDescIndex[i];
if(indices[i].size())
{
info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
}
if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
{
int matchedIndexTo = -1;
@@ -973,6 +982,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
descriptors.resize(indices[i].size());
}
std::list<int> indicesToIgnoretmp;
for(unsigned int j=0; j<indices[i].size(); ++j)
{
int octave = kptsTo[indices[i].at(j)].octave;
@@ -981,10 +991,7 @@ Transform RegistrationVis::computeTransformationImpl(
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
descriptorsIndices[oi++] = indices[i].at(j);
if(dists[i].at(j) < radius)
{
indicesToIgnore.insert(indices[i].at(j));
}
indicesToIgnoretmp.push_back(indices[i].at(j));
}
}
bruteForceDescCopy += bruteForceTimer.ticks();
@@ -999,6 +1006,8 @@ Transform RegistrationVis::computeTransformationImpl(
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
indicesToIgnore.insert(indicesToIgnore.begin(), indicesToIgnore.end());
}
}
else if(oi == 1)
@@ -1186,6 +1195,8 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
int inliersCount = 0;
int matchesCount = 0;
info.inliersIDs.clear();
info.matchesIDs.clear();
if(toSignature.getWords().size())
{
Transform transforms[2];
@@ -1565,22 +1576,6 @@ Transform RegistrationVis::computeTransformationImpl(
!optimizedPoses.rbegin()->second.isNull())
{
UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
transforms[0] = optimizedPoses.rbegin()->second;
transforms[1].setNull();
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
Transform invT = transforms[0].inverse();
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
cpyWordsFrom3.find(iter->first)->second = iter->second;
if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
{
cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
}
}
fromSignature.setWords3(cpyWordsFrom3);
toSignature.setWords3(cpyWordsTo3);*/
if(sbaOutliers.size())
{
@@ -1597,6 +1592,31 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
allInliers = newInliers;
}
if((int)allInliers.size() < _minInliers)
{
msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
(int)allInliers.size(), _minInliers, fromSignature.id(), toSignature.id());
transforms[0].setNull();
}
else
{
transforms[0] = optimizedPoses.rbegin()->second;
}
transforms[1].setNull();
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
Transform invT = transforms[0].inverse();
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
cpyWordsFrom3.find(iter->first)->second = iter->second;
if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
{
cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
}
}
fromSignature.setWords3(cpyWordsFrom3);
toSignature.setWords3(cpyWordsTo3);*/
}
}

29
corelib/src/Rtabmap.cpp
View File

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UProcessInfo.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/crop_box.h>
@@ -79,6 +80,7 @@ Rtabmap::Rtabmap() :
_publishLastSignatureData(Parameters::defaultRtabmapPublishLastSignature()),
_publishPdf(Parameters::defaultRtabmapPublishPdf()),
_publishLikelihood(Parameters::defaultRtabmapPublishLikelihood()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_computeRMSE(Parameters::defaultRtabmapComputeRMSE()),
_maxTimeAllowed(Parameters::defaultRtabmapTimeThr()), // 700 ms
_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
@@ -401,6 +403,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapPublishLastSignature(), _publishLastSignatureData);
Parameters::parse(parameters, Parameters::kRtabmapPublishPdf(), _publishPdf);
Parameters::parse(parameters, Parameters::kRtabmapPublishLikelihood(), _publishLikelihood);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
Parameters::parse(parameters, Parameters::kRtabmapComputeRMSE(), _computeRMSE);
Parameters::parse(parameters, Parameters::kRtabmapTimeThr(), _maxTimeAllowed);
Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
@@ -1506,6 +1509,7 @@ bool Rtabmap::process(
true,
true,
false,
true,
std::set<int>(),
&timeGetNeighborsTimeDb);
ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
@@ -1561,6 +1565,7 @@ bool Rtabmap::process(
true,
false,
false,
false,
std::set<int>(),
&timeGetNeighborsSpaceDb);
ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
@@ -1975,7 +1980,7 @@ bool Rtabmap::process(
nearestId,
transform.prettyPrint().c_str());
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.covariance.inv()));
_memory->addLink(Link(signature->id(), nearestId, Link::kGlobalClosure, transform, info.covariance.inv()));
loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));
if(loopClosureVisualInliers == 0)
@@ -2246,14 +2251,14 @@ bool Rtabmap::process(
{
UWARN("Graph optimization failed! Rejecting last loop closures added.");
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedHypothesis = true;
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastProximitySpaceClosureId = 0;
rejectedHypothesis = true;
}
else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
_optimizationMaxLinearError > 0.0f &&
@@ -2438,6 +2443,10 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
if(_publishRAMUsage)
{
statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
}
if(_publishLikelihood || _publishPdf)
{
@@ -3068,7 +3077,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
std::map<int, Transform> path;
// select nearest pose and iterate neighbors from there
int nearestId = rtabmap::graph::findNearestNode(poses, target);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, nodesSet);
std::map<int, int> ids = _memory->getNeighborsId(nearestId, maxGraphDepth, 0, true, true, true, true, nodesSet);
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{

7
corelib/src/Stereo.cpp
View File

@@ -151,9 +151,10 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed, images are not calibrated, "
"the background is too far (no disparity between the images) or "
"maximum disparity may be too small (%d).",
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
countFlowRejected+countDisparityRejected,
(int)status.size(),
this->maxDisparity());

5
corelib/src/Transform.cpp
View File

@@ -92,6 +92,11 @@ Transform::Transform(float x, float y, float theta)
*this = fromEigen3f(t);
}
Transform Transform::clone() const
{
return Transform(data_.clone());
}
bool Transform::isNull() const
{
return (data_.empty() ||

14
corelib/src/VWDictionary.cpp
View File

@@ -64,6 +64,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
_totalActiveReferences(0),
_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
_nndrRatio(Parameters::defaultKpNndrRatio()),
_dictionaryPath(Parameters::defaultKpDictionaryPath()),
_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
@@ -88,6 +89,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
@@ -363,15 +365,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_);
_flannIndex->buildLinearIndex(descriptor, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannKdTree:
UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_);
_flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannLSH:
UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2);
_flannIndex->buildLSHIndex(descriptor, 12, 20, 2, _rebalancingFactor);
break;
default:
UFATAL("Not supposed to be here!");
@@ -486,15 +488,15 @@ void VWDictionary::update()
switch(_strategy)
{
case kNNFlannNaive:
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_);
_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannKdTree:
UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_);
_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
break;
case kNNFlannLSH:
UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2);
_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
break;
default:
break;

2
corelib/src/util2d.cpp
View File

@@ -2019,7 +2019,7 @@ cv::Mat exposureFusion(const std::vector<cv::Mat> & images)
fusion.convertTo(rgb8, CV_8UC3, 255.0);
fusion = rgb8;
#else
UWARN("Exposure fusion is only avaiable when rtabmap is built with OpenCV3.");
UWARN("Exposure fusion is only available when rtabmap is built with OpenCV3.");
if (images.size())
{
fusion = images[0].clone();

20
corelib/src/util3d.cpp
View File

@@ -240,6 +240,26 @@ pcl::PointXYZ projectDepthTo3D(
return pt;
}
Eigen::Vector3f projectDepthTo3DRay(
const cv::Size & imageSize,
float x, float y,
float cx, float cy,
float fx, float fy)
{
Eigen::Vector3f ray;
// Use correct principal point from calibration
cx = cx > 0.0f ? cx : float(imageSize.width/2) - 0.5f; //cameraInfo.K.at(2)
cy = cy > 0.0f ? cy : float(imageSize.height/2) - 0.5f; //cameraInfo.K.at(5)
// Fill in XYZ
ray[0] = (x - cx) / fx;
ray[1] = (y - cy) / fy;
ray[2] = 1.0f;
return ray;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
const cv::Mat & imageDepth,
float cx, float cy,

2
corelib/src/util3d_motion_estimation.cpp
View File

@@ -156,7 +156,7 @@ Transform estimateMotion3DTo2D(
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2)*10.0f;
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
}
}

53
corelib/src/util3d_surface.cpp
View File

@@ -2629,6 +2629,59 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
return cloud_with_normals;
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].normal_z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//UWARN("Reverse %d of n=%f,%f,%f result=%f", i, cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z, result);
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
}
}
void adjustNormalsToViewPoint(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const Eigen::Vector3f & viewpoint,
bool forceGroundNormalsUp)
{
for(unsigned int i=0; i<cloud->size(); ++i)
{
pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
if(pcl::isFinite(normal))
{
Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
Eigen::Vector3f n(normal.x, normal.y, normal.z);
float result = v.dot(n);
if(result < 0
|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].normal_z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
cloud->points[i].normal_x *= -1.0f;
cloud->points[i].normal_y *= -1.0f;
cloud->points[i].normal_z *= -1.0f;
}
}
}
}
void adjustNormalsToViewPoints(
const std::map<int, Transform> & poses,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,