Update 0.15.4.

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.

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 15)
-SET(RTABMAP_PATCH_VERSION 3)
+SET(RTABMAP_PATCH_VERSION 4)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/BayesFilter.h

@@ -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

corelib/include/rtabmap/core/CameraInfo.h

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

corelib/include/rtabmap/core/CameraRGB.h

@@ -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

corelib/include/rtabmap/core/CameraThread.h

@@ -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;

corelib/include/rtabmap/core/FlannIndex.h

@@ -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)

corelib/include/rtabmap/core/Graph.h

@@ -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

corelib/include/rtabmap/core/Memory.h

@@ -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;

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -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_;
 };
 
 }

corelib/include/rtabmap/core/OctoMap.h

@@ -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,

corelib/include/rtabmap/core/Odometry.h

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

corelib/include/rtabmap/core/OdometryF2F.h

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

corelib/include/rtabmap/core/OdometryF2M.h

@@ -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>>

corelib/include/rtabmap/core/OdometryInfo.h

@@ -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;
 

corelib/include/rtabmap/core/Parameters.h

@@ -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();

corelib/include/rtabmap/core/Registration.h

@@ -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;}
 

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -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:

corelib/include/rtabmap/core/RegistrationInfo.h

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

corelib/include/rtabmap/core/RegistrationVis.h

@@ -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;

corelib/include/rtabmap/core/Rtabmap.h

@@ -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

corelib/include/rtabmap/core/Statistics.h

@@ -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);

corelib/include/rtabmap/core/Transform.h

@@ -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];}

corelib/include/rtabmap/core/VWDictionary.h

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

corelib/include/rtabmap/core/util3d.h

@@ -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,

corelib/include/rtabmap/core/util3d_surface.h

@@ -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,

corelib/src/BayesFilter.cpp

@@ -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;

corelib/src/CameraRGB.cpp

@@ -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
 			{

corelib/src/CameraThread.cpp

@@ -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());

corelib/src/DBDriver.cpp

@@ -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

corelib/src/FlannIndex.cpp

@@ -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();

corelib/src/Graph.cpp

@@ -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

corelib/src/Link.cpp

@@ -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;
 }

corelib/src/Memory.cpp

@@ -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;

corelib/src/OccupancyGrid.cpp

@@ -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());
 }

corelib/src/OctoMap.cpp

@@ -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);

corelib/src/Odometry.cpp

@@ -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())
 		{

corelib/src/OdometryF2F.cpp

@@ -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.");

corelib/src/OdometryF2M.cpp

@@ -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())

corelib/src/OdometryFovis.cpp

@@ -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;
 	}
 

corelib/src/OdometryORBSLAM2.cpp

@@ -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;
 		}
 	}
 

corelib/src/OdometryViso2.cpp

@@ -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);

corelib/src/OptimizerG2O.cpp

@@ -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);

corelib/src/Parameters.cpp

@@ -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)
 			{

corelib/src/Recovery.cpp

@@ -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());
 			}

corelib/src/Registration.cpp

@@ -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);

corelib/src/RegistrationVis.cpp

@@ -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);*/
 			}
 		}
 

corelib/src/Rtabmap.cpp

@@ -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)
 			{

corelib/src/Stereo.cpp

@@ -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());

corelib/src/Transform.cpp

@@ -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() ||

corelib/src/VWDictionary.cpp

@@ -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;

corelib/src/util2d.cpp

@@ -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();

corelib/src/util3d.cpp

@@ -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,

corelib/src/util3d_motion_estimation.cpp

@@ -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);
 					}
 				}
 

corelib/src/util3d_surface.cpp

@@ -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,

guilib/src/CloudViewer.cpp

@@ -1857,6 +1857,8 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
 							yAxis[0], yAxis[1], yAxis[2],0,
 							zAxis[0], zAxis[1], zAxis[2],0);
 
+				PR.normalizeRotation();
+
 				Transform P(PR[0], PR[1], PR[2], cameras.front().pos[0],
 							PR[4], PR[5], PR[6], cameras.front().pos[1],
 							PR[8], PR[9], PR[10], cameras.front().pos[2]);

guilib/src/CreateSimpleCalibrationDialog.cpp

@@ -141,9 +141,9 @@ void CreateSimpleCalibrationDialog::updateSaveStatus()
 			 (!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
 	{
 		//advanced
-		QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().split(' ');
-		QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().split(' ');
-		std::string RT = ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString();
+		QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+		QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
+		std::string RT = ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString();
 
 		if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
 				(!ui_->checkBox_stereo->isChecked() || (distorsionsStrListR.size() == 4 || distorsionsStrListR.size() == 5 || distorsionsStrListR.size() == 8)) &&
@@ -192,7 +192,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 			cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
 			K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
 
-			QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().split(' ');
+			QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
 			UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
 			cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
 			bool ok;
@@ -240,7 +240,7 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 				cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
 				K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
 
-				QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().split(' ');
+				QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().split(' ');
 				UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
 				cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
 				bool ok;
@@ -257,8 +257,13 @@ void CreateSimpleCalibrationDialog::saveCalibration()
 				modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
 				UASSERT(modelRight.isValidForRectification());
 
-				UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString()));
+				UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').simplified().trimmed().toStdString()));
 				stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().remove('[').remove(']').replace(',',' ').replace(';',' ').trimmed().toStdString()));
+				if(stereoModel.baseline() < 0)
+				{
+					QMessageBox::warning(this, tr("Save"), tr("Error parsing the extrinsics \"%1\", resulting baseline (%f) is negative!").arg(ui_->lineEdit_RT->text()).arg(stereoModel.baseline()));
+					return;
+				}
 				UASSERT(stereoModel.isValidForRectification());
 			}
 

guilib/src/DatabaseViewer.cpp

@@ -320,6 +320,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
+	connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
@@ -354,15 +355,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
 	// Graph view
-	connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -484,13 +477,6 @@ void DatabaseViewer::readSettings()
 	ui_->graphViewer->loadSettings(settings, "GraphView");
 
 	settings.beginGroup("optimization");
-	ui_->checkBox_spanAllMaps->setChecked(settings.value("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()).toBool());
-	ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool());
-	ui_->checkBox_ignoreGlobalLoop->setChecked(settings.value("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()).toBool());
-	ui_->checkBox_ignoreLocalLoopSpace->setChecked(settings.value("ignoreLocalLoopSpace", ui_->checkBox_ignoreLocalLoopSpace->isChecked()).toBool());
-	ui_->checkBox_ignoreLocalLoopTime->setChecked(settings.value("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked()).toBool());
-	ui_->checkBox_ignoreUserLoop->setChecked(settings.value("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked()).toBool());
-	ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
 	ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
 
@@ -502,10 +488,6 @@ void DatabaseViewer::readSettings()
 	ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
 	ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
 	ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
-	if(ui_->checkBox_octomap->isEnabled())
-	{
-		ui_->checkBox_octomap->setChecked(settings.value("octomap", ui_->checkBox_octomap->isChecked()).toBool());
-	}
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -576,17 +558,6 @@ void DatabaseViewer::writeSettings()
 
 	// save optimization settings
 	settings.beginGroup("optimization");
-	//settings.setValue("iterations", ui_->spinBox_iterations->value());
-	settings.setValue("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked());
-	//settings.setValue("robust", ui_->checkBox_robust->isChecked());
-	settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked());
-	settings.setValue("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked());
-	settings.setValue("ignoreLocalLoopSpace", ui_->checkBox_ignoreLocalLoopSpace->isChecked());
-	settings.setValue("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked());
-	settings.setValue("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked());
-	//settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
-	//settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
-	settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
 	settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
 	settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
 	settings.endGroup();
@@ -598,7 +569,6 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
 	settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
 	settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
-	settings.setValue("octomap", ui_->checkBox_octomap->isChecked());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -666,6 +636,7 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
 	ui_->checkBox_ignoreUserLoop->setChecked(false);
 	ui_->spinBox_optimizationDepth->setValue(0);
+	ui_->doubleSpinBox_optimizationScale->setValue(1.0);
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
 	ui_->doubleSpinBox_voxelSize->setValue(0.0);
 
@@ -925,6 +896,10 @@ bool DatabaseViewer::closeDatabase()
 		ui_->toolBox_statistics->clear();
 		databaseFileName_.clear();
 		ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
+		ui_->doubleSpinBox_optimizationScale->setVisible(false);
+		ui_->label_scale_title->setVisible(false);
+		ui_->label_rmse->setVisible(false);
+		ui_->label_rmse_title->setVisible(false);
 		ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
 		ui_->label_alignPosesWithGroundTruth->setVisible(false);
 		ui_->label_optimizeFrom->setText(tr("Optimize from"));
@@ -968,6 +943,7 @@ bool DatabaseViewer::closeDatabase()
 		ui_->label_timeOptimization->clear();
 		ui_->label_pathLength->clear();
 		ui_->label_poses->clear();
+		ui_->label_rmse->clear();
 		ui_->spinBox_optimizationsFrom->setEnabled(false);
 
 		ui_->graphicsView_A->clear();
@@ -1435,6 +1411,10 @@ void DatabaseViewer::updateIds()
 	gpsPoses_.clear();
 	gpsValues_.clear();
 	ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
+	ui_->doubleSpinBox_optimizationScale->setVisible(false);
+	ui_->label_scale_title->setVisible(false);
+	ui_->label_rmse->setVisible(false);
+	ui_->label_rmse_title->setVisible(false);
 	ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
 	ui_->label_alignPosesWithGroundTruth->setVisible(false);
 	ui_->menuExport_GPS->setEnabled(false);
@@ -1569,6 +1549,10 @@ void DatabaseViewer::updateIds()
 	if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
 	{
 		ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
+		ui_->doubleSpinBox_optimizationScale->setVisible(true);
+		ui_->label_scale_title->setVisible(true);
+		ui_->label_rmse->setVisible(true);
+		ui_->label_rmse_title->setVisible(true);
 		ui_->label_alignPosesWithGroundTruth->setVisible(true);
 		if(!groundTruthPoses_.empty())
 		{
@@ -1595,6 +1579,7 @@ void DatabaseViewer::updateIds()
 
 	UINFO("Update database info...");
 	ui_->textEdit_info->clear();
+	ui_->textEdit_info->append(tr("Path:\t\t%1").arg(dbDriver_->getUrl().c_str()));
 	ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
 	ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(sessions));
 	if(hasReducedGraph)
@@ -1720,7 +1705,7 @@ void DatabaseViewer::updateIds()
 			{
 				neighborLinks_.append(iter->second);
 			}
-			else
+			else if(iter->second.type()!=rtabmap::Link::kPosePrior)
 			{
 				loopLinks_.append(iter->second);
 			}
@@ -1784,6 +1769,7 @@ void DatabaseViewer::updateStatistics()
 		std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
 
 		std::map<std::string, std::pair<std::vector<float>, std::vector<float> > > allData;
+		std::map<std::string, int > allDataOi;
 
 		for(int i=0; i<ids_.size(); ++i)
 		{
@@ -1804,15 +1790,21 @@ void DatabaseViewer::updateStatistics()
 				{
 					//initialize data vectors
 					allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<float>(ids_.size(), 0.0f), std::vector<float>(ids_.size(), 0.0f) )));
+					allDataOi.insert(std::make_pair(iter->first, 0));
 				}
 
-				allData.at(iter->first).first[i] = ui_->checkBox_timeStats->isChecked()?float(stamp-firstStamp):ids_[i];
-				allData.at(iter->first).second[i] = iter->second;
+				int & oi = allDataOi.at(iter->first);
+				allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?float(stamp-firstStamp):ids_[i];
+				allData.at(iter->first).second[oi] = iter->second;
+				++oi;
 			}
 		}
 
 		for(std::map<std::string, std::pair<std::vector<float>, std::vector<float> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
 		{
+			int oi = allDataOi.at(iter->first);
+			iter->second.first.resize(oi);
+			iter->second.second.resize(oi);
 			ui_->toolBox_statistics->updateStat(iter->first.c_str(), iter->second.first, iter->second.second, true);
 		}
 	}
@@ -3420,16 +3412,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 
 		UTimer timer;
 		ParametersMap parameters = ui_->parameters_toolbox->getParameters();
-		bool opticalFlow = uStr2Bool(parameters.at(Parameters::kStereoOpticalFlow()));
-		Stereo * stereo = 0;
-		if(opticalFlow)
-		{
-			stereo = new StereoOpticalFlow(parameters);
-		}
-		else
-		{
-			stereo = new Stereo(parameters);
-		}
+		Stereo * stereo = Stereo::create(parameters);
 
 		// generate kpts
 		std::vector<cv::KeyPoint> kpts;
@@ -3576,7 +3559,7 @@ void DatabaseViewer::updateStereo(const SensorData * data)
 						rightKpts[i].pt.x,
 						rightKpts[i].pt.y,
 						c,
-						QString("%1: (%2,%3) -> (%4,%5)").arg(i).arg(kpts[i].pt.x).arg(kpts[i].pt.y).arg(rightKpts[i].pt.x).arg(rightKpts[i].pt.y));
+						QString("%1: (%2,%3) -> (%4,%5) d=%6").arg(i).arg(kpts[i].pt.x).arg(kpts[i].pt.y).arg(rightKpts[i].pt.x).arg(rightKpts[i].pt.y).arg(kpts[i].pt.x - rightKpts[i].pt.x));
 			}
 		}
 		ui_->graphicsView_stereo->update();
@@ -4329,6 +4312,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					rotational_max);
 
 			// ground truth live statistics
+			ui_->label_rmse->setNum(translational_rmse);
 			UINFO("translational_rmse=%f", translational_rmse);
 			UINFO("translational_mean=%f", translational_mean);
 			UINFO("translational_median=%f", translational_median);
@@ -4611,6 +4595,7 @@ void DatabaseViewer::updateGraphView()
 {
 	ui_->label_loopClosures->clear();
 	ui_->label_poses->clear();
+	ui_->label_rmse->clear();
 
 	if(odomPoses_.size())
 	{
@@ -4649,6 +4634,7 @@ void DatabaseViewer::updateGraphView()
 
 		ui_->menuExport_poses->setEnabled(true);
 		std::multimap<int, rtabmap::Link> links = links_;
+		loopLinks_.clear();
 
 		// filter current map if not spanning to all maps
 		if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
@@ -4718,6 +4704,7 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
+				loopLinks_.push_back(iter->second);
 				++totalGlobal;
 			}
 			else if(iter->second.type() == Link::kLocalSpaceClosure)
@@ -4727,6 +4714,7 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
+				loopLinks_.push_back(iter->second);
 				++totalLocalSpace;
 			}
 			else if(iter->second.type() == Link::kLocalTimeClosure)
@@ -4736,6 +4724,7 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
+				loopLinks_.push_back(iter->second);
 				++totalLocalTime;
 			}
 			else if(iter->second.type() == Link::kUserClosure)
@@ -4745,14 +4734,37 @@ void DatabaseViewer::updateGraphView()
 					links.erase(iter++);
 					continue;
 				}
+				loopLinks_.push_back(iter->second);
 				++totalUser;
 			}
 			else if(iter->second.type() == Link::kPosePrior)
 			{
 				++totalPriors;
 			}
+			else
+			{
+				loopLinks_.push_back(iter->second);
+			}
+			Transform t = iter->second.transform().clone();
+			t.x() *= ui_->doubleSpinBox_optimizationScale->value();
+			t.y() *= ui_->doubleSpinBox_optimizationScale->value();
+			t.z() *= ui_->doubleSpinBox_optimizationScale->value();
+			iter->second.setTransform(t);
 			++iter;
 		}
+		ui_->horizontalSlider_loops->blockSignals(true);
+		if(loopLinks_.size() == 0)
+		{
+			ui_->horizontalSlider_loops->setEnabled(false);
+		}
+		else
+		{
+			ui_->horizontalSlider_loops->setEnabled(true);
+			ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
+		}
+		ui_->horizontalSlider_loops->setValue(0);
+		ui_->horizontalSlider_loops->blockSignals(false);
+
 		ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)")
 				.arg(totalNeighbor)
 				.arg(totalNeighborMerged)
@@ -5447,9 +5459,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
 		linksIn.insert(std::make_pair(newLink.from(), newLink));
 		const Link * maxLinearLink = 0;
-		const Link * maxAngularLink = 0;
-		float maxLinearError = 0.0f;
-		float maxAngularError = 0.0f;
+		float maxLinearErrorRatio = 0.0f;
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
@@ -5480,43 +5490,31 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 							fabs(iter->second.transform().x() - t.x()),
 							fabs(iter->second.transform().y() - t.y()),
 							fabs(iter->second.transform().z() - t.z()));
-					Eigen::Vector3f vA = t1.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
-					Eigen::Vector3f vB = t2.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
-					float angularError = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
-					if(linearError > maxLinearError)
+					float stddev = sqrt(iter->second.transVariance());
+					float linearErrorRatio = linearError/stddev;
+					if(linearErrorRatio > maxLinearErrorRatio)
 					{
-						maxLinearError = linearError;
+						maxLinearErrorRatio = linearErrorRatio;
 						maxLinearLink = &iter->second;
 					}
-					if(angularError > maxAngularError)
-					{
-						maxAngularError = angularError;
-						maxAngularLink = &iter->second;
-					}
 				}
 			}
 			if(maxLinearLink)
 			{
-				UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
-			}
-			if(maxAngularLink)
-			{
-				UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
+				UINFO("Max optimization linear error ratio = %f (link %d->%d)", maxLinearErrorRatio, maxLinearLink->from(), maxLinearLink->to());
 			}
 
-			if(maxLinearError > maxOptimizationError)
+			if(maxLinearErrorRatio > maxOptimizationError)
 			{
 				msg = uFormat("Rejecting edge %d->%d because "
-						  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
-						  "\"%s\" is %f m.",
+						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
+						  "\"%s\" is %f.",
 						  newLink.from(),
 						  newLink.to(),
-						  maxLinearError,
+						  maxLinearErrorRatio,
 						  maxLinearLink->from(),
 						  maxLinearLink->to(),
-						  maxAngularError*180.0f/M_PI,
-						  maxAngularLink?maxAngularLink->from():0,
-						  maxAngularLink?maxAngularLink->to():0,
+						  sqrt(maxLinearLink->transVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
 			}

guilib/src/MainWindow.cpp

@@ -932,6 +932,7 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
 	_ui->statsToolBox->updateStat("Camera/Time decimation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeImageDecimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeDisparity*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeMirroring*1000.0f, _preferencesDialog->isCacheSavedInFigures());
+	_ui->statsToolBox->updateStat("Camera/Time exposure compensation/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeStereoExposureCompensation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 	_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", _preferencesDialog->isTimeUsedInFigures()?info.stamp-_firstStamp:(float)info.id, info.timeScanFromDepth*1000.0f, _preferencesDialog->isCacheSavedInFigures());
 
 	emit(cameraInfoProcessed());
@@ -2105,6 +2106,32 @@ void MainWindow::updateMapCloud(
 		_ui->actionAnchor_clouds_to_ground_truth->setChecked(false);
 	}
 
+	int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
+	if(maxNodes > 0 && poses.size()>1)
+	{
+		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
+		std::map<int, Transform> nearestPoses;
+		nearestPoses.insert(*poses.rbegin());
+		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator pter = poses.find(*iter);
+			if(pter != poses.end())
+			{
+				nearestPoses.insert(*pter);
+			}
+		}
+		//add negative...
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(iter->first > 0)
+			{
+				break;
+			}
+			nearestPoses.insert(*iter);
+		}
+		poses=nearestPoses;
+	}
+
 	// Map updated! regenerate the assembled cloud, last pose is the new one
 	UDEBUG("Update map with %d locations", poses.size());
 	QMap<std::string, Transform> viewerClouds = _cloudViewer->getAddedClouds();
@@ -4650,6 +4677,14 @@ void MainWindow::startDetection()
 				{
 					uInsert(odomParameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(_preferencesDialog->getOdomRegistrationApproach())));
 				}
+				odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+				int odomStrategy = Parameters::defaultOdomStrategy();
+				Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
+				if(odomStrategy == 1)
+				{
+					// Only Frame To Frame supports all VisCorType
+					odomParameters.insert(ParametersPair(Parameters::kVisCorType(), _preferencesDialog->getParameter(Parameters::kVisCorType())));
+				}
 				Odometry * odom = Odometry::create(odomParameters);
 				_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
 

guilib/src/PreferencesDialog.cpp

@@ -451,7 +451,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_octomap_fullUpdate, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_octomap_cubeRendering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -612,6 +611,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_cameraImages_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraImages_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkBox_cameraImages_scanForceGroundNormalsUp, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -641,6 +642,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_publishPdf->setObjectName(Parameters::kRtabmapPublishPdf().c_str());
 	_ui->general_checkBox_publishLikelihood->setObjectName(Parameters::kRtabmapPublishLikelihood().c_str());
 	_ui->general_checkBox_publishRMSE->setObjectName(Parameters::kRtabmapComputeRMSE().c_str());
+	_ui->general_checkBox_publishRAM->setObjectName(Parameters::kRtabmapPublishRAMUsage().c_str());
 	_ui->general_checkBox_statisticLogsBufferedInRAM->setObjectName(Parameters::kRtabmapStatisticLogsBufferedInRAM().c_str());
 	_ui->groupBox_statistics->setObjectName(Parameters::kRtabmapStatisticLogged().c_str());
 	_ui->general_checkBox_statisticLoggedHeaders->setObjectName(Parameters::kRtabmapStatisticLoggedHeaders().c_str());
@@ -703,10 +705,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str());
 	_ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str());
 	_ui->checkBox_kp_incrementalFlann->setObjectName(Parameters::kKpIncrementalFlann().c_str());
+	_ui->surf_doubleSpinBox_rebalancingFactor->setObjectName(Parameters::kKpFlannRebalancingFactor().c_str());
 	_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
 	_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
 	_ui->surf_doubleSpinBox_maxDepth->setObjectName(Parameters::kKpMaxDepth().c_str());
 	_ui->surf_doubleSpinBox_minDepth->setObjectName(Parameters::kKpMinDepth().c_str());
+	_ui->checkBox_memDepthAsMask->setObjectName(Parameters::kMemDepthAsMask().c_str());
 	_ui->surf_spinBox_wordsPerImageTarget->setObjectName(Parameters::kKpMaxFeatures().c_str());
 	_ui->spinBox_KPGridRows->setObjectName(Parameters::kKpGridRows().c_str());
 	_ui->spinBox_KPGridCols->setObjectName(Parameters::kKpGridCols().c_str());
@@ -861,9 +865,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
 	_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
 	_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
-	_ui->loopClosure_correspondencesType->setObjectName(Parameters::kVisCorType().c_str());
-	connect(_ui->loopClosure_correspondencesType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureCorrespondences, SLOT(setCurrentIndex(int)));
-	_ui->stackedWidget_loopClosureCorrespondences->setCurrentIndex(Parameters::defaultVisCorType());
 	_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
 	_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
 	_ui->spinBox_visCorGuessWinSize->setObjectName(Parameters::kVisCorGuessWinSize().c_str());
@@ -874,6 +875,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->reextract_gridcols->setObjectName(Parameters::kVisGridCols().c_str());
 	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
 	_ui->loopClosure_bowMinDepth->setObjectName(Parameters::kVisMinDepth().c_str());
+	_ui->checkBox_visDepthAsMask->setObjectName(Parameters::kVisDepthAsMask().c_str());
 	_ui->loopClosure_roi->setObjectName(Parameters::kVisRoiRatios().c_str());
 	_ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str());
 	_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
@@ -937,6 +939,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	_ui->checkBox_grid_fullUpdate->setObjectName(Parameters::kGridGlobalFullUpdate().c_str());
 	_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
+	_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
 	_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
 	_ui->checkBox_grid_erode->setObjectName(Parameters::kGridGlobalEroded().c_str());
 
@@ -965,6 +968,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
 	_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
 
+	//Odometry Frame To Frame
+	_ui->comboBox_odomf2f_corType->setObjectName(Parameters::kVisCorType().c_str());
+
 	//Odometry Mono
 	_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
 	_ui->doubleSpinBox_minInitTranslation->setObjectName(Parameters::kOdomMonoInitMinTranslation().c_str());
@@ -1041,6 +1047,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_OdomORBSLAM2ThDepth->setObjectName(Parameters::kOdomORBSLAM2ThDepth().c_str());
 	_ui->doubleSpinBox_OdomORBSLAM2Fps->setObjectName(Parameters::kOdomORBSLAM2Fps().c_str());
 	_ui->spinBox_OdomORBSLAM2MaxFeatures->setObjectName(Parameters::kOdomORBSLAM2MaxFeatures().c_str());
+	_ui->spinBox_OdomORBSLAM2MapSize->setObjectName(Parameters::kOdomORBSLAM2MapSize().c_str());
 
 	//Stereo
 	_ui->stereo_winWidth->setObjectName(Parameters::kStereoWinWidth().c_str());
@@ -1457,7 +1464,6 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->groupBox_octomap->setChecked(false);
 		_ui->spinBox_octomap_treeDepth->setValue(16);
-		_ui->checkBox_octomap_fullUpdate->setChecked(false);
 		_ui->checkBox_octomap_2dgrid->setChecked(true);
 		_ui->checkBox_octomap_show3dMap->setChecked(true);
 		_ui->checkBox_octomap_cubeRendering->setChecked(false);
@@ -1595,6 +1601,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.025f);
 		_ui->spinBox_cameraImages_scanNormalsK->setValue(20);
 		_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(0.0);
+		_ui->checkBox_cameraImages_scanForceGroundNormalsUp->setChecked(false);
 
 		_ui->groupBox_depthFromScan->setChecked(false);
 		_ui->groupBox_depthFromScan_fillHoles->setChecked(true);
@@ -1845,7 +1852,6 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 
 	_ui->groupBox_octomap->setChecked(settings.value("octomap", _ui->groupBox_octomap->isChecked()).toBool());
 	_ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt());
-	_ui->checkBox_octomap_fullUpdate->setChecked(settings.value("octomap_full_update", _ui->checkBox_octomap_fullUpdate->isChecked()).toBool());
 	_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
 	_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());
 	_ui->checkBox_octomap_cubeRendering->setChecked(settings.value("octomap_cube", _ui->checkBox_octomap_cubeRendering->isChecked()).toBool());
@@ -1997,6 +2003,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
 	_ui->spinBox_cameraImages_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()).toInt());
 	_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()).toDouble());
+	_ui->checkBox_cameraImages_scanForceGroundNormalsUp->setChecked(settings.value("normalsUp", _ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked()).toBool());
+
 	settings.endGroup();//ScanFromDepth
 
 	settings.beginGroup("DepthFromScan");
@@ -2247,7 +2255,6 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 
 	settings.setValue("octomap",                     _ui->groupBox_octomap->isChecked());
 	settings.setValue("octomap_depth",               _ui->spinBox_octomap_treeDepth->value());
-	settings.setValue("octomap_full_update",         _ui->checkBox_octomap_fullUpdate->isChecked());
 	settings.setValue("octomap_2dgrid",              _ui->checkBox_octomap_2dgrid->isChecked());
 	settings.setValue("octomap_3dmap",               _ui->checkBox_octomap_show3dMap->isChecked());
 	settings.setValue("octomap_cube",                _ui->checkBox_octomap_cubeRendering->isChecked());
@@ -2401,6 +2408,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("voxelSize", 			_ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
 	settings.setValue("normalsK", 			_ui->spinBox_cameraImages_scanNormalsK->value());
 	settings.setValue("normalsRadius", 		_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
+	settings.setValue("normalsUp", 	        _ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
 	settings.endGroup();
 
 	settings.beginGroup("DepthFromScan");
@@ -2584,15 +2592,6 @@ bool PreferencesDialog::validateForm()
 				   "with cvsba. Bundle adjustment is disabled."));
 		_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
 	}
-	if(_ui->odom_strategy->currentIndex() == 0 && // F2M
-		_ui->odom_f2m_bundleStrategy->currentIndex() > 0 &&
-		_ui->loopClosure_correspondencesType->currentIndex() == 1)
-	{
-		QMessageBox::warning(this, tr("Parameter warning"),
-				tr("Odometry local bundle adjustment optimization cannot be used at the same time than Optical Flow correspondences "
-					"strategy (see Visual Registration panel). Bundle adjustment is disabled."));
-		_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
-	}
 
 	// verify that Robust and Reject threshold are not set at the same time
 	if(_ui->graphOptimization_robust->isChecked() && _ui->graphOptimization_maxError->value()>0.0)
@@ -3066,6 +3065,9 @@ rtabmap::ParametersMap PreferencesDialog::getAllParameters() const
 	ParametersMap parameters = _parameters;
 	uInsert(parameters, _modifiedParameters);
 
+	// It will be added manually for odometry
+	parameters.erase(Parameters::kVisCorType());
+
 	return parameters;
 }
 
@@ -3328,7 +3330,7 @@ void PreferencesDialog::selectSourceImagesPathOdom()
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Odometry (*.txt *.log *.toro *.g2o)"));
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Odometry (*.txt *.log *.toro *.g2o *.csv)"));
 	if(path.size())
 	{
 		QStringList list;
@@ -3352,7 +3354,7 @@ void PreferencesDialog::selectSourceImagesPathGt()
 	{
 		dir = getWorkingDirectory();
 	}
-	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt *.log *.toro *.g2o)"));
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt *.log *.toro *.g2o *.csv)"));
 	if(path.size())
 	{
 		QStringList list;
@@ -4334,7 +4336,7 @@ int PreferencesDialog::getOctomapTreeDepth() const
 }
 bool PreferencesDialog::isOctomapFullUpdate() const
 {
-	return _ui->checkBox_octomap_fullUpdate->isChecked();
+	return uStr2Bool(this->getParameter(Parameters::kGridGlobalFullUpdate()));
 }
 double PreferencesDialog::getOctomapOccupancyThr() const
 {
@@ -4851,7 +4853,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
-						this->getLaserLocalTransform());
+						this->getLaserLocalTransform(),
+						_ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
 		((CameraRGBDImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
@@ -4899,7 +4902,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
-						this->getLaserLocalTransform());
+						this->getLaserLocalTransform(),
+						_ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
 		((CameraStereoImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
 				_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
@@ -5005,7 +5009,8 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
-						this->getLaserLocalTransform());
+						this->getLaserLocalTransform(),
+						_ui->checkBox_cameraImages_scanForceGroundNormalsUp->isChecked());
 		((CameraImages*)camera)->setDepthFromScan(
 				_ui->groupBox_depthFromScan->isChecked(),
 				!_ui->groupBox_depthFromScan_fillHoles->isChecked()?0:_ui->radioButton_depthFromScan_vertical->isChecked()?1:-1,
@@ -5220,6 +5225,15 @@ void PreferencesDialog::testOdometry()
 	{
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach())));
 	}
+
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+	if(odomStrategy == 1)
+	{
+		// Only Frame To Frame supports  all VisCorType
+		parameters.insert(ParametersPair(Parameters::kVisCorType(), this->getParameter(Parameters::kVisCorType())));
+	}
+
 	Odometry * odometry = Odometry::create(parameters);
 
 	OdometryThread odomThread(

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>407</width>
+           <width>395</width>
            <height>242</height>
           </rect>
          </property>
@@ -253,7 +253,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>406</width>
+           <width>394</width>
            <height>242</height>
           </rect>
          </property>
@@ -921,7 +921,7 @@
      </item>
      <item>
       <layout class="QGridLayout" name="gridLayout_5" columnstretch="0,1">
-       <item row="7" column="0">
+       <item row="8" column="0">
         <widget class="QLabel" name="label_loopClosures">
          <property name="text">
           <string/>
@@ -954,7 +954,7 @@
        <item row="0" column="0">
         <widget class="QSpinBox" name="spinBox_optimizationsFrom"/>
        </item>
-       <item row="6" column="1">
+       <item row="7" column="1">
         <widget class="QLabel" name="label_52">
          <property name="text">
           <string>Poses</string>
@@ -968,7 +968,7 @@
          </property>
         </widget>
        </item>
-       <item row="7" column="1">
+       <item row="8" column="1">
         <widget class="QLabel" name="label_41">
          <property name="text">
           <string>Links (N, NM, G, LS, LT, U, P)</string>
@@ -1020,7 +1020,7 @@
          </item>
         </layout>
        </item>
-       <item row="6" column="0">
+       <item row="7" column="0">
         <widget class="QLabel" name="label_poses">
          <property name="text">
           <string/>
@@ -1050,6 +1050,16 @@
          </property>
         </widget>
        </item>
+       <item row="2" column="0">
+        <widget class="QCheckBox" name="checkBox_ignoreIntermediateNodes">
+         <property name="text">
+          <string/>
+         </property>
+         <property name="checked">
+          <bool>false</bool>
+         </property>
+        </widget>
+       </item>
        <item row="2" column="1">
         <widget class="QLabel" name="label_alignPosesWithGroundTruth_2">
          <property name="text">
@@ -1060,13 +1070,20 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
-        <widget class="QCheckBox" name="checkBox_ignoreIntermediateNodes">
+       <item row="6" column="1">
+        <widget class="QLabel" name="label_rmse_title">
+         <property name="text">
+          <string>RMSE (m)</string>
+         </property>
+        </widget>
+       </item>
+       <item row="6" column="0">
+        <widget class="QLabel" name="label_rmse">
          <property name="text">
           <string/>
          </property>
-         <property name="checked">
-          <bool>false</bool>
+         <property name="textInteractionFlags">
+          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
          </property>
         </widget>
        </item>
@@ -1146,22 +1163,36 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>0</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>340</width>
-          <height>228</height>
+          <width>318</width>
+          <height>219</height>
          </rect>
         </property>
         <attribute name="label">
          <string>Graph optimization</string>
         </attribute>
-        <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,0,0">
-         <item row="3" column="0">
+        <layout class="QGridLayout" name="gridLayout_7" columnstretch="0,0,1">
+         <item row="0" column="2">
+          <widget class="QLabel" name="label_8">
+           <property name="text">
+            <string>Depth (0=inf)</string>
+           </property>
+          </widget>
+         </item>
+         <item row="5" column="2">
+          <widget class="QLabel" name="label_49">
+           <property name="text">
+            <string>Ignore local loop closures (time)</string>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreLocalLoopSpace">
            <property name="text">
             <string/>
@@ -1171,37 +1202,21 @@
            </property>
           </widget>
          </item>
-         <item row="2" column="2">
+         <item row="3" column="2">
           <widget class="QLabel" name="label_48">
            <property name="text">
             <string>Ignore global loop closures</string>
            </property>
           </widget>
          </item>
-         <item row="0" column="1">
-          <widget class="QSpinBox" name="spinBox_optimizationDepth">
-           <property name="suffix">
-            <string/>
-           </property>
-           <property name="minimum">
-            <number>0</number>
-           </property>
-           <property name="maximum">
-            <number>1000</number>
-           </property>
-           <property name="value">
-            <number>0</number>
-           </property>
-          </widget>
-         </item>
-         <item row="1" column="2">
+         <item row="2" column="2">
           <widget class="QLabel" name="label_35">
            <property name="text">
             <string>Ignore pose correction</string>
            </property>
           </widget>
          </item>
-         <item row="1" column="0">
+         <item row="2" column="0">
           <widget class="QCheckBox" name="checkBox_ignorePoseCorrection">
            <property name="text">
             <string/>
@@ -1211,7 +1226,7 @@
            </property>
           </widget>
          </item>
-         <item row="4" column="0">
+         <item row="5" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreLocalLoopTime">
            <property name="text">
             <string/>
@@ -1221,7 +1236,7 @@
            </property>
           </widget>
          </item>
-         <item row="6" column="0">
+         <item row="7" column="0">
           <spacer name="verticalSpacer_3">
            <property name="orientation">
             <enum>Qt::Vertical</enum>
@@ -1234,28 +1249,7 @@
            </property>
           </spacer>
          </item>
-         <item row="0" column="2">
-          <widget class="QLabel" name="label_8">
-           <property name="text">
-            <string>Depth (0=inf)</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="2">
-          <widget class="QLabel" name="label_50">
-           <property name="text">
-            <string>Ignore user loop closures</string>
-           </property>
-          </widget>
-         </item>
-         <item row="3" column="2">
-          <widget class="QLabel" name="label_47">
-           <property name="text">
-            <string>Ignore local loop closures (space)</string>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="0">
+         <item row="6" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreUserLoop">
            <property name="text">
             <string/>
@@ -1265,7 +1259,14 @@
            </property>
           </widget>
          </item>
-         <item row="2" column="0">
+         <item row="6" column="2">
+          <widget class="QLabel" name="label_50">
+           <property name="text">
+            <string>Ignore user loop closures</string>
+           </property>
+          </widget>
+         </item>
+         <item row="3" column="0">
           <widget class="QCheckBox" name="checkBox_ignoreGlobalLoop">
            <property name="text">
             <string/>
@@ -1276,9 +1277,51 @@
           </widget>
          </item>
          <item row="4" column="2">
-          <widget class="QLabel" name="label_49">
+          <widget class="QLabel" name="label_47">
            <property name="text">
-            <string>Ignore local loop closures (time)</string>
+            <string>Ignore local loop closures (space)</string>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="2">
+          <widget class="QLabel" name="label_scale_title">
+           <property name="text">
+            <string>Scale</string>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_optimizationScale">
+           <property name="decimals">
+            <number>3</number>
+           </property>
+           <property name="minimum">
+            <double>0.100000000000000</double>
+           </property>
+           <property name="maximum">
+            <double>9.900000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.001000000000000</double>
+           </property>
+           <property name="value">
+            <double>1.000000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="0">
+          <widget class="QSpinBox" name="spinBox_optimizationDepth">
+           <property name="suffix">
+            <string/>
+           </property>
+           <property name="minimum">
+            <number>0</number>
+           </property>
+           <property name="maximum">
+            <number>1000</number>
+           </property>
+           <property name="value">
+            <number>0</number>
            </property>
           </widget>
          </item>
@@ -1709,8 +1752,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>298</width>
-          <height>228</height>
+          <width>205</width>
+          <height>117</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1809,8 +1852,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>446</width>
-          <height>228</height>
+          <width>185</width>
+          <height>487</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/utilite/UPlot.cpp

@@ -823,13 +823,13 @@ void UPlotCurve::draw(QPainter * painter, const QRect & limits)
 				}
 			}
 
-			if(limits.contains(item->pos().toPoint()) && limits.contains((item->pos() + QPointF(item->rect().width(), item->rect().height())).toPoint()))
+			/*if(limits.contains(item->pos().toPoint()) && limits.contains((item->pos() + QPointF(item->rect().width(), item->rect().height())).toPoint()))
 			{
 				painter->save();
 				painter->setPen(QPen(_itemsColor));
 				painter->drawEllipse(item->pos()+QPointF(item->rect().width()/2, item->rect().height()/2), (int)item->rect().width()/2, (int)item->rect().height()/2);
 				painter->restore();
-			}
+			}*/
 		}
 	}
 }
@@ -1833,7 +1833,7 @@ UPlot::UPlot(QWidget *parent) :
 	_lowestRefreshRate = 99;
 	_refreshStartTime.start();
 
-	_penStyleCount = rand() % 10 + 1; // rand 1->10
+	_penStyleCount = 0;
 	_workingDirectory = QDir::homePath();
 }
 
@@ -2081,6 +2081,28 @@ bool UPlot::contains(const QString & curveName)
 
 QPen UPlot::getRandomPenColored()
 {
+	int penStyle = 0;
+	bool colorNotUsed = false;
+	for(int i=0; i<12; ++i)
+	{
+		QColor tmp((Qt::GlobalColor)((penStyle+i) % 12 + 7 ));
+		bool colorAlreadyUsed = false;
+		for(QList<UPlotCurve*>::const_iterator iter = _curves.constBegin(); iter!=_curves.constEnd() && !colorAlreadyUsed; ++iter)
+		{
+			colorAlreadyUsed = (*iter)->pen().color() == tmp;
+		}
+		if(!colorAlreadyUsed)
+		{
+			colorNotUsed = true;
+			penStyle+=i;
+			break;
+		}
+	}
+	if(colorNotUsed)
+	{
+		_penStyleCount = penStyle;
+	}
+
 	return QPen((Qt::GlobalColor)(_penStyleCount++ % 12 + 7 ));
 }
 

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.14.2</version>
+  <version>0.15.4</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/CMakeLists.txt

@@ -7,8 +7,9 @@ ADD_SUBDIRECTORY( CameraRGBD )
 ADD_SUBDIRECTORY( StereoEval )
 ADD_SUBDIRECTORY( KittiDataset )
 ADD_SUBDIRECTORY( RgbdDataset )
+ADD_SUBDIRECTORY( EurocDataset )
 ADD_SUBDIRECTORY( Recovery )
-ADD_SUBDIRECTORY( Report )
+ADD_SUBDIRECTORY( Reprocess )
 
 IF(OPENCV_NONFREE_FOUND)
 ADD_SUBDIRECTORY( VocabularyComparison )
@@ -20,6 +21,7 @@ IF(TARGET rtabmap_gui)
 	ADD_SUBDIRECTORY( OdometryViewer )
 	ADD_SUBDIRECTORY( DataRecorder )
 	ADD_SUBDIRECTORY( Calibration )
+	ADD_SUBDIRECTORY( Report )
 ELSE()
    MESSAGE(STATUS "RTAB-Map GUI lib is not built, some tools won't be built...")
 ENDIF()

tools/EurocDataset/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 2.8)
+
+FIND_PACKAGE(yaml-cpp REQUIRED)
+IF(yaml-cpp_FOUND)
+
+# inside rtabmap project (see below for external build)
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+	
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+	${YAML_CPP_LIBRARIES}
+)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(euroc_dataset main.cpp)
+  
+TARGET_LINK_LIBRARIES(euroc_dataset ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( euroc_dataset 
+    PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-euroc_dataset)
+    
+INSTALL(TARGETS euroc_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+	
+ELSE()
+  MESSAGE(STATUS "yaml-cpp not found, euroc_dataset tool won't be built...")
+ENDIF()

tools/EurocDataset/main.cpp

@@ -0,0 +1,538 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/OdometryF2M.h>
+#include "rtabmap/core/Rtabmap.h"
+#include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/Graph.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+#include <pcl/common/common.h>
+#include <yaml-cpp/yaml.h>
+#include <stdio.h>
+#include <signal.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-kitti_dataset [options] path\n"
+			"  path               Folder of the sequence (e.g., \"~/EuRoC/V1_03_difficult\")\n"
+			"                        containing least mav0/cam0/sensor.yaml, mav0/cam1/sensor.yaml, mav0/cam0/data and mav0/cam1/data folders.\n"
+			"                        Optional image_2, image_3 and velodyne folders.\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
+			"  --quiet            Don't show log messages and iteration updates.\n"
+			"  --exposure_comp    Do exposure compensation between left and right images.\n"
+			"  --disp             Generate full disparity.\n"
+			"%s\n"
+			"Example:\n\n"
+			"   $ rtabmap-euroc_dataset --Rtabmap/DetectionRate 4 ~/EuRoC/V1_03_difficult\n\n", rtabmap::Parameters::showUsage());
+	exit(1);
+}
+
+// catch ctrl-c
+bool g_forever = true;
+void sighandler(int sig)
+{
+	printf("\nSignal %d caught...\n", sig);
+	g_forever = false;
+}
+
+int main(int argc, char * argv[])
+{
+	signal(SIGABRT, &sighandler);
+	signal(SIGTERM, &sighandler);
+	signal(SIGINT, &sighandler);
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ParametersMap parameters;
+	std::string path;
+	std::string output;
+	std::string outputName = "rtabmap";
+	std::string seq;
+	bool disp = false;
+	bool exposureCompensation = false;
+	bool quiet = false;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		for(int i=1; i<argc; ++i)
+		{
+			if(std::strcmp(argv[i], "--output") == 0)
+			{
+				output = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
+			else if(std::strcmp(argv[i], "--quiet") == 0)
+			{
+				quiet = true;
+			}
+			else if(std::strcmp(argv[i], "--disp") == 0)
+			{
+				disp = true;
+			}
+			else if(std::strcmp(argv[i], "--exposure_comp") == 0)
+			{
+				exposureCompensation = true;
+			}
+		}
+		parameters = Parameters::parseArguments(argc, argv);
+		path = argv[argc-1];
+		path = uReplaceChar(path, '~', UDirectory::homeDir());
+		path = uReplaceChar(path, '\\', '/');
+		if(output.empty())
+		{
+			output = path;
+		}
+		else
+		{
+			output = uReplaceChar(output, '~', UDirectory::homeDir());
+			UDirectory::makeDir(output);
+		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
+	}
+
+	seq = uSplit(path, '/').back();
+	std::string pathLeftImages  = path+"/mav0/cam0/data";
+	std::string pathRightImages = path+"/mav0/cam1/data";
+	std::string pathCalibLeft = path+"/mav0/cam0/sensor.yaml";
+	std::string pathCalibRight = path+"/mav0/cam1/sensor.yaml";
+	std::string pathGt = path+"/mav0/state_groundtruth_estimate0/data.csv";
+	if(!UFile::exists(pathGt))
+	{
+		UWARN("Ground truth file path doesn't exist: \"%s\", benchmark values won't be computed.", pathGt.c_str());
+		pathGt.clear();
+	}
+
+	printf("Paths:\n"
+			"   Sequence number:  %s\n"
+			"   Sequence path:    %s\n"
+			"   Output:           %s\n"
+			"   Output name:      %s\n"
+			"   left images:      %s\n"
+			"   right images:     %s\n"
+			"   left calib:       %s\n"
+			"   right calib:      %s\n",
+			seq.c_str(),
+			path.c_str(),
+			output.c_str(),
+			outputName.c_str(),
+			pathLeftImages.c_str(),
+			pathRightImages.c_str(),
+			pathCalibLeft.c_str(),
+			pathCalibRight.c_str());
+	if(!pathGt.empty())
+	{
+		printf("   Ground truth:     %s\n", pathGt.c_str());
+	}
+	printf("   Exposure Compensation: %s\n", exposureCompensation?"true":"false");
+	printf("   Disparity:        %s\n", disp?"true":"false");
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
+
+	std::vector<CameraModel> models;
+	int rateHz = 20;
+	for(int k=0; k<2; ++k)
+	{
+		// Left calibration
+		std::string calibPath = k==0?pathCalibLeft:pathCalibRight;
+		YAML::Node config = YAML::LoadFile(calibPath);
+		if(config.IsNull())
+		{
+			UERROR("Cannot open calibration file \"%s\"", calibPath.c_str());
+			return -1;
+		}
+
+		YAML::Node T_BS = config["T_BS"];
+		YAML::Node data = T_BS["data"];
+		UASSERT(data.size() == 16);
+		rateHz = config["rate_hz"].as<int>();
+		YAML::Node resolution = config["resolution"];
+		UASSERT(resolution.size() == 2);
+		YAML::Node intrinsics = config["intrinsics"];
+		UASSERT(intrinsics.size() == 4);
+		YAML::Node distortion_coefficients = config["distortion_coefficients"];
+		UASSERT(distortion_coefficients.size() == 4 || distortion_coefficients.size() == 5 || distortion_coefficients.size() == 8);
+
+		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+		K.at<double>(0,0) = intrinsics[0].as<double>();
+		K.at<double>(1,1) = intrinsics[1].as<double>();
+		K.at<double>(0,2) = intrinsics[2].as<double>();
+		K.at<double>(1,2) = intrinsics[3].as<double>();
+		cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+		cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
+		K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
+
+		cv::Mat D = cv::Mat::zeros(1, distortion_coefficients.size(), CV_64FC1);
+		for(unsigned int i=0; i<distortion_coefficients.size(); ++i)
+		{
+			D.at<double>(i) = distortion_coefficients[i].as<double>();
+		}
+
+		Transform t(data[0].as<float>(), data[1].as<float>(), data[2].as<float>(), data[3].as<float>(),
+					data[4].as<float>(), data[5].as<float>(), data[6].as<float>(), data[7].as<float>(),
+					data[8].as<float>(), data[9].as<float>(), data[10].as<float>(), data[11].as<float>());
+
+		models.push_back(CameraModel(outputName+"_calib", cv::Size(resolution[0].as<int>(),resolution[1].as<int>()), K, D, R, P, t));
+		UASSERT(models.back().isValidForRectification());
+	}
+
+	StereoCameraModel model(outputName+"_calib", models[0], models[1], models[1].localTransform().inverse() * models[0].localTransform());
+	if(!model.save(output, true))
+	{
+		UERROR("Could not save calibration!");
+		return -1;
+	}
+	printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
+
+
+	if(quiet)
+	{
+		ULogger::setLevel(ULogger::kError);
+	}
+
+	// We use CameraThread only to use postUpdate() method
+	Transform opticalRotation(0,0,1,0, 0,-1,0,0, 1,0,0,0);
+	CameraThread cameraThread(new
+		CameraStereoImages(
+				pathLeftImages,
+				pathRightImages,
+				true,
+				0.0f,
+				opticalRotation*models[0].localTransform()), parameters);
+	((CameraStereoImages*)cameraThread.camera())->setTimestamps(true, "", false);
+	if(exposureCompensation)
+	{
+		cameraThread.setStereoExposureCompensation(true);
+	}
+	if(disp)
+	{
+		cameraThread.setStereoToDepth(true);
+	}
+	if(!pathGt.empty())
+	{
+		((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(pathGt, 9);
+	}
+
+	float detectionRate = Parameters::defaultRtabmapDetectionRate();
+	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
+	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+
+	int mapUpdate = rateHz / detectionRate;
+	if(mapUpdate < 1)
+	{
+		mapUpdate = 1;
+	}
+
+	std::string databasePath = output+"/"+outputName+".db";
+	UFile::erase(databasePath);
+	if(cameraThread.camera()->init(output, outputName+"_calib"))
+	{
+		int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
+
+		printf("Processing %d images...\n", totalImages);
+
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
+		Rtabmap rtabmap;
+		rtabmap.init(parameters, databasePath);
+
+		UTimer totalTime;
+		UTimer timer;
+		CameraInfo cameraInfo;
+		SensorData data = cameraThread.camera()->takeImage(&cameraInfo);
+		int iteration = 0;
+
+		/////////////////////////////
+		// Processing dataset begin
+		/////////////////////////////
+		cv::Mat covariance;
+		int odomKeyFrames = 0;
+		while(data.isValid() && g_forever)
+		{
+			cameraThread.postUpdate(&data, &cameraInfo);
+			cameraInfo.timeTotal = timer.ticks();
+
+			OdometryInfo odomInfo;
+			Transform pose = odom->process(data, &odomInfo);
+			if(odomInfo.keyFrameAdded)
+			{
+				++odomKeyFrames;
+			}
+
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+
+			bool processData = true;
+			if(iteration % mapUpdate != 0)
+			{
+				// set negative id so rtabmap will detect it as an intermediate node
+				data.setId(-1);
+				data.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
+				processData = intermediateNodes;
+			}
+			if(covariance.empty() || odomInfo.reg.covariance.at<double>(0,0) > covariance.at<double>(0,0))
+			{
+				covariance = odomInfo.reg.covariance;
+			}
+
+			timer.restart();
+			if(processData)
+			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+				OdometryEvent e(SensorData(), Transform(), odomInfo);
+				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
+				covariance = cv::Mat();
+			}
+
+			++iteration;
+			if(!quiet || iteration == totalImages)
+			{
+				double slamTime = timer.ticks();
+
+				float rmse = -1;
+				if(rtabmap.getStatistics().data().find(Statistics::kGtTranslational_rmse()) != rtabmap.getStatistics().data().end())
+				{
+					rmse = rtabmap.getStatistics().data().at(Statistics::kGtTranslational_rmse());
+				}
+
+				if(data.keypoints().size() == 0 && data.laserScanRaw().cols)
+				{
+					if(rmse >= 0.0f)
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+					}
+					else
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					}
+				}
+				else
+				{
+					if(rmse >= 0.0f)
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
+					}
+					else
+					{
+						printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms",
+								iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f));
+					}
+				}
+				if(processData && rtabmap.getLoopClosureId()>0)
+				{
+					printf(" *");
+				}
+				printf("\n");
+			}
+			else if(iteration % (totalImages/10) == 0)
+			{
+				printf(".");
+				fflush(stdout);
+			}
+
+			cameraInfo = CameraInfo();
+			timer.restart();
+			data = cameraThread.camera()->takeImage(&cameraInfo);
+		}
+		delete odom;
+		printf("Total time=%fs\n", totalTime.ticks());
+		/////////////////////////////
+		// Processing dataset end
+		/////////////////////////////
+
+		// Save trajectory
+		printf("Saving trajectory ...\n");
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		std::map<int, Signature> signatures;
+		std::map<int, double> stamps;
+		rtabmap.getGraph(poses, links, true, true, &signatures);
+		for(std::map<int, Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+		{
+			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
+		}
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
+		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
+		{
+			printf("Saving %s... done!\n", pathTrajectory.c_str());
+		}
+		else
+		{
+			printf("Saving %s... failed!\n", pathTrajectory.c_str());
+		}
+
+		if(!pathGt.empty())
+		{
+			// Log ground truth statistics
+			std::map<int, Transform> groundTruth;
+
+			for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+			{
+				Transform o, gtPose;
+				int m,w;
+				std::string l;
+				double s;
+				std::vector<float> v;
+				GPS gps;
+				rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true);
+				if(!gtPose.isNull())
+				{
+					groundTruth.insert(std::make_pair(iter->first, gtPose));
+				}
+			}
+
+			// compute RMSE statistics
+			float translational_rmse = 0.0f;
+			float translational_mean = 0.0f;
+			float translational_median = 0.0f;
+			float translational_std = 0.0f;
+			float translational_min = 0.0f;
+			float translational_max = 0.0f;
+			float rotational_rmse = 0.0f;
+			float rotational_mean = 0.0f;
+			float rotational_median = 0.0f;
+			float rotational_std = 0.0f;
+			float rotational_min = 0.0f;
+			float rotational_max = 0.0f;
+			graph::calcRMSE(
+					groundTruth,
+					poses,
+					translational_rmse,
+					translational_mean,
+					translational_median,
+					translational_std,
+					translational_min,
+					translational_max,
+					rotational_rmse,
+					rotational_mean,
+					rotational_median,
+					rotational_std,
+					rotational_min,
+					rotational_max);
+
+			printf("   translational_rmse=   %f m\n", translational_rmse);
+			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
+
+			FILE * pFile = 0;
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
+			pFile = fopen(pathErrors.c_str(),"w");
+			if(!pFile)
+			{
+				UERROR("could not save RMSE results to \"%s\"", pathErrors.c_str());
+			}
+			fprintf(pFile, "Ground truth comparison:\n");
+			fprintf(pFile, "  translational_rmse=   %f\n", translational_rmse);
+			fprintf(pFile, "  translational_mean=   %f\n", translational_mean);
+			fprintf(pFile, "  translational_median= %f\n", translational_median);
+			fprintf(pFile, "  translational_std=    %f\n", translational_std);
+			fprintf(pFile, "  translational_min=    %f\n", translational_min);
+			fprintf(pFile, "  translational_max=    %f\n", translational_max);
+			fprintf(pFile, "  rotational_rmse=      %f\n", rotational_rmse);
+			fprintf(pFile, "  rotational_mean=      %f\n", rotational_mean);
+			fprintf(pFile, "  rotational_median=    %f\n", rotational_median);
+			fprintf(pFile, "  rotational_std=       %f\n", rotational_std);
+			fprintf(pFile, "  rotational_min=       %f\n", rotational_min);
+			fprintf(pFile, "  rotational_max=       %f\n", rotational_max);
+			fclose(pFile);
+		}
+	}
+	else
+	{
+		UERROR("Camera init failed!");
+	}
+
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
+	printf("Do:\n"
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
+
+	return 0;
+}

tools/KittiDataset/CMakeLists.txt

@@ -34,3 +34,7 @@ TARGET_LINK_LIBRARIES(kitti_dataset ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( kitti_dataset 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-kitti_dataset)
+    
+INSTALL(TARGETS kitti_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/KittiDataset/main.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
 #include <pcl/common/common.h>
 #include <stdio.h>
 #include <signal.h>
@@ -53,14 +54,17 @@ void showUsage()
 			"                        containing least calib.txt, times.txt, image_0 and image_1 folders.\n"
 			"                        Optional image_2, image_3 and velodyne folders.\n"
 			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
 			"  --gt \"path\"        Ground truth path (e.g., ~/KITTI/devkit/cpp/data/odometry/poses/07.txt)\n"
 			"  --quiet            Don't show log messages and iteration updates.\n"
 			"  --color            Use color images for stereo (image_2 and image_3 folders).\n"
+			"  --scaling          Scale stereo baseline on some sequences (03-04-05-06).\n"
 			"  --disp             Generate full disparity.\n"
+			"  --exposure_comp    Do exposure compensation between left and right images.\n"
 			"  --scan             Include velodyne scan in node's data.\n"
-			"  --scan_step #      Scan downsample step (default=10).\n"
-			"  --scan_voxel #.#   Scan voxel size (default 0.3 m).\n"
-			"  --scan_k           Scan normal K (default 5).\n"
+			"  --scan_step #      Scan downsample step (default=1).\n"
+			"  --scan_voxel #.#   Scan voxel size (default 0.5 m).\n"
+			"  --scan_k           Scan normal K (default 0).\n"
 			"  --scan_radius      Scan normal radius (default 0).\n\n"
 			"%s\n"
 			"Example:\n\n"
@@ -108,13 +112,16 @@ int main(int argc, char * argv[])
 	ParametersMap parameters;
 	std::string path;
 	std::string output;
+	std::string outputName = "rtabmap";
 	std::string seq;
 	bool color = false;
+	bool scaling = false;
 	bool scan = false;
 	bool disp = false;
-	int scanStep = 10;
-	float scanVoxel = 0.3f;
-	int scanNormalK = 5;
+	bool exposureCompensation = false;
+	int scanStep = 1;
+	float scanVoxel = 0.5f;
+	int scanNormalK = 0;
 	float scanNormalRadius = 0.0f;
 	std::string gtPath;
 	bool quiet = false;
@@ -130,6 +137,10 @@ int main(int argc, char * argv[])
 			{
 				output = argv[++i];
 			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
 			else if(std::strcmp(argv[i], "--quiet") == 0)
 			{
 				quiet = true;
@@ -178,6 +189,10 @@ int main(int argc, char * argv[])
 			{
 				color = true;
 			}
+			else if(std::strcmp(argv[i], "--scaling") == 0)
+			{
+				scaling = true;
+			}
 			else if(std::strcmp(argv[i], "--scan") == 0)
 			{
 				scan = true;
@@ -186,6 +201,10 @@ int main(int argc, char * argv[])
 			{
 				disp = true;
 			}
+			else if(std::strcmp(argv[i], "--exposure_comp") == 0)
+			{
+				exposureCompensation = true;
+			}
 		}
 		parameters = Parameters::parseArguments(argc, argv);
 		path = argv[argc-1];
@@ -200,6 +219,8 @@ int main(int argc, char * argv[])
 			output = uReplaceChar(output, '~', UDirectory::homeDir());
 			UDirectory::makeDir(output);
 		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
 	}
 
 	seq = uSplit(path, '/').back();
@@ -218,6 +239,7 @@ int main(int argc, char * argv[])
 			"   Sequence number:  %s\n"
 			"   Sequence path:    %s\n"
 			"   Output:           %s\n"
+			"   Output name:      %s\n"
 			"   left images:      %s\n"
 			"   right images:     %s\n"
 			"   calib.txt:        %s\n"
@@ -225,6 +247,7 @@ int main(int argc, char * argv[])
 			seq.c_str(),
 			path.c_str(),
 			output.c_str(),
+			outputName.c_str(),
 			pathLeftImages.c_str(),
 			pathRightImages.c_str(),
 			pathCalib.c_str(),
@@ -243,10 +266,8 @@ int main(int argc, char * argv[])
 			printf("   Ground Truth:      %s\n", gtPath.c_str());
 		}
 	}
-	if(disp)
-	{
-		printf("   Disparity:         %s\n", disp?"true":"false");
-	}
+	printf("   Exposure Compensation: %s\n", exposureCompensation?"true":"false");
+	printf("   Disparity:         %s\n", disp?"true":"false");
 	if(scan)
 	{
 		pathScan = path+"/velodyne";
@@ -256,15 +277,6 @@ int main(int argc, char * argv[])
 		printf("   Scan normal k:      %d\n", scanNormalK);
 		printf("   Scan normal radius: %f\n", scanNormalRadius);
 	}
-	if(!parameters.empty())
-	{
-		printf("Parameters:\n");
-		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-		{
-			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
-		}
-	}
-	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
 
 	// convert calib.txt to rtabmap format (yaml)
 	FILE * pFile = 0;
@@ -320,7 +332,28 @@ int main(int argc, char * argv[])
 		UERROR("Failed to read first image of \"%s\"", firstImage.c_str());
 		return -1;
 	}
-	StereoCameraModel model("rtabmap_calib"+seq,
+
+	if(scaling)
+	{
+		// scale baseline
+		if(uStr2Int(seq) == 3 || uStr2Int(seq) == 5 || uStr2Int(seq) == 9)
+		{
+			P1.at<double>(0,3) *= 0.9905;
+			printf("   Baseline scaling factor: %f\n", 0.9905);
+		}
+		else if(uStr2Int(seq) == 4)
+		{
+			P1.at<double>(0,3) *= 0.987000;
+			printf("   Baseline scaling factor: %f\n", 0.987000);
+		}
+		else if(uStr2Int(seq) == 6)
+		{
+			P1.at<double>(0,3) *= 0.985000;
+			printf("   Baseline scaling factor: %f\n", 0.985000);
+		}
+	}
+
+	StereoCameraModel model(outputName+"_calib",
 			image.size(), P0.colRange(0,3), cv::Mat(), cv::Mat(), P0,
 			image.size(), P1.colRange(0,3), cv::Mat(), cv::Mat(), P1,
 			cv::Mat(), cv::Mat(), cv::Mat(), cv::Mat());
@@ -329,8 +362,18 @@ int main(int argc, char * argv[])
 		UERROR("Could not save calibration!");
 		return -1;
 	}
-	printf("Saved calibration \"%s\" to \"%s\"\n", ("rtabmap_calib"+seq).c_str(), output.c_str());
+	printf("Saved calibration \"%s\" to \"%s\"\n", (outputName+"_calib").c_str(), output.c_str());
 
+	if(!parameters.empty())
+	{
+		printf("Parameters:\n");
+		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		{
+			printf("   %s=%s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+
+	printf("RTAB-Map version: %s\n", RTABMAP_VERSION);
 
 	if(quiet)
 	{
@@ -347,6 +390,10 @@ int main(int argc, char * argv[])
 				0.0f,
 				opticalRotation), parameters);
 	((CameraStereoImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
+	if(exposureCompensation)
+	{
+		cameraThread.setStereoExposureCompensation(true);
+	}
 	if(disp)
 	{
 		cameraThread.setStereoToDepth(true);
@@ -364,11 +411,14 @@ int main(int argc, char * argv[])
 						scanVoxel,
 						scanNormalK,
 						scanNormalRadius,
-						Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
+						Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f),
+						true);
 	}
 
 	float detectionRate = Parameters::defaultRtabmapDetectionRate();
 	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
 	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
 	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
 
@@ -379,15 +429,17 @@ int main(int argc, char * argv[])
 		mapUpdate = 1;
 	}
 
-	std::string databasePath = output+"/rtabmap" + seq + ".db";
+	std::string databasePath = output+"/"+outputName+".db";
 	UFile::erase(databasePath);
-	if(cameraThread.camera()->init(output, "rtabmap_calib"+seq))
+	if(cameraThread.camera()->init(output, outputName+"_calib"))
 	{
 		int totalImages = (int)((CameraStereoImages*)cameraThread.camera())->filenames().size();
 
 		printf("Processing %d images...\n", totalImages);
 
-		Odometry * odom = Odometry::create(parameters);
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
 		Rtabmap rtabmap;
 		rtabmap.init(parameters, databasePath);
 
@@ -404,43 +456,29 @@ int main(int argc, char * argv[])
 		int odomKeyFrames = 0;
 		while(data.isValid() && g_forever)
 		{
-			std::map<std::string, float> externalStats;
 			cameraThread.postUpdate(&data, &cameraInfo);
 			cameraInfo.timeTotal = timer.ticks();
 
-			// save camera statistics to database
-			externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
-			externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
-			externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
-			externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
-			externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
-			externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
-			externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
-			externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
 
 			OdometryInfo odomInfo;
 			Transform pose = odom->process(data, &odomInfo);
-			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
-			externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
-			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
 			float speed = 0.0f;
 			if(odomInfo.interval>0.0)
 				speed = odomInfo.transform.x()/odomInfo.interval*3.6;
-			externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
-			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
-			externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
-			externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
-			externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
-			externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
-			externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
 			if(odomInfo.keyFrameAdded)
 			{
 				++odomKeyFrames;
 			}
 
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+
 			bool processData = true;
 			if(iteration % mapUpdate != 0)
 			{
@@ -457,6 +495,32 @@ int main(int argc, char * argv[])
 			timer.restart();
 			if(processData)
 			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
 				OdometryEvent e(SensorData(), Transform(), odomInfo);
 				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
 				covariance = cv::Mat();
@@ -477,8 +541,10 @@ int main(int argc, char * argv[])
 				{
 					if(rmse >= 0.0f)
 					{
-						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
-								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+						//printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
+						//		iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%f, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.icpInliersRatio, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
 					}
 					else
 					{
@@ -490,8 +556,10 @@ int main(int argc, char * argv[])
 				{
 					if(rmse >= 0.0f)
 					{
-						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
-								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+						//printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
+						//		iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+						printf("Iteration %d/%d: speed=%dkm/h camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+								iteration, totalImages, int(speed), int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
 					}
 					else
 					{
@@ -526,7 +594,7 @@ int main(int argc, char * argv[])
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
 		rtabmap.getGraph(poses, links, true, true);
-		std::string pathTrajectory = output+"/rtabmap_poses"+seq+".txt";
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
 		if(poses.size() && graph::exportPoses(pathTrajectory, 2, poses, links))
 		{
 			printf("Saving %s... done!\n", pathTrajectory.c_str());
@@ -597,7 +665,7 @@ int main(int argc, char * argv[])
 			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
 
 			pFile = 0;
-			std::string pathErrors = output+"/rtabmap_rmse"+seq+".txt";
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
 			pFile = fopen(pathErrors.c_str(),"w");
 			if(!pFile)
 			{
@@ -626,9 +694,9 @@ int main(int argc, char * argv[])
 		UERROR("Camera init failed!");
 	}
 
-	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/rtabmap" + seq + ".db").c_str());
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
 	printf("Do:\n"
-			" $ rtabmap-databaseViewer %s\n\n", (output+"/rtabmap" + seq + ".db").c_str());
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
 
 	return 0;
 }

tools/Recovery/CMakeLists.txt

@@ -33,6 +33,9 @@ TARGET_LINK_LIBRARIES(recovery ${LIBRARIES})
 SET_TARGET_PROPERTIES( recovery 
 	PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-recovery)
 
+INSTALL(TARGETS recovery
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
 
 
 

tools/Report/CMakeLists.txt

@@ -4,10 +4,12 @@ cmake_minimum_required(VERSION 2.8)
 SET(RTABMap_INCLUDE_DIRS 
     ${PROJECT_SOURCE_DIR}/utilite/include
 	${PROJECT_SOURCE_DIR}/corelib/include
+	${PROJECT_SOURCE_DIR}/guilib/include
 )
 SET(RTABMap_LIBRARIES 
     rtabmap_core
 	rtabmap_utilite
+	rtabmap_gui
 )  
 	
 if(POLICY CMP0020)
@@ -20,10 +22,15 @@ SET(INCLUDE_DIRS
     ${PCL_INCLUDE_DIRS}
 )
 
+IF(QT4_FOUND)
+    INCLUDE(${QT_USE_FILE})
+ENDIF(QT4_FOUND)
+
 SET(LIBRARIES
 	${RTABMap_LIBRARIES}
 	${OpenCV_LIBRARIES}
 	${PCL_LIBRARIES}
+	${QT_LIBRARIES} 
 )
 
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
@@ -34,3 +41,7 @@ TARGET_LINK_LIBRARIES(report ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( report 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-report)
+    
+INSTALL(TARGETS report
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/Report/main.cpp

@@ -26,10 +26,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Optimizer.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UPlot.h>
+#include <QApplication>
 #include <stdio.h>
 
 using namespace rtabmap;
@@ -37,8 +41,8 @@ using namespace rtabmap;
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-report path\n"
-			"  path               Directory containing rtabmap databases.\n\n");
+			"rtabmap-report [\"Statistic/Id\"] [--latex] [--kitti] [--scale] path\n"
+			"  path               Directory containing rtabmap databases or path of a database.\n\n");
 	exit(1);
 }
 
@@ -49,34 +53,113 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
-	std::string path = argv[1];
+	QApplication app(argc, argv);
+
+	bool outputLatex = false;
+	bool outputScaled = false;
+	bool outputKittiError = false;
+	std::map<std::string, UPlot*> figures;
+	for(int i=1; i<argc-1; ++i)
+	{
+		if(strcmp(argv[i], "--latex") == 0)
+		{
+			outputLatex = true;
+		}
+		else if(strcmp(argv[i], "--kitti") == 0)
+		{
+			outputKittiError = true;
+		}
+		else if(strcmp(argv[i], "--scale") == 0)
+		{
+			outputScaled = true;
+		}
+		else
+		{
+			std::string figureTitle = argv[i];
+			printf("Plot %s\n", figureTitle.c_str());
+			UPlot * fig = new UPlot();
+			fig->setTitle(figureTitle.c_str());
+			fig->setXLabel("Time (s)");
+			figures.insert(std::make_pair(figureTitle, fig));
+		}
+	}
+
+	std::string path = argv[argc-1];
 	path = uReplaceChar(path, '~', UDirectory::homeDir());
 
 	std::string fileName;
 	std::list<std::string> paths;
 	paths.push_back(path);
+	std::vector<std::map<std::string, std::vector<float> > > outputLatexStatistics;
+	std::map<std::string, std::vector<float> > outputLatexStatisticsMap;
+	bool odomRAMSet = false;
+	std::set<std::string> topDirs;
 	while(paths.size())
 	{
 		std::string currentPath = paths.front();
 		UDirectory currentDir(currentPath);
 		paths.pop_front();
+		bool currentPathIsDatabase = false;
 		if(!currentDir.isValid())
 		{
-			continue;
+			if(UFile::getExtension(currentPath).compare("db") == 0)
+			{
+				currentPathIsDatabase=true;
+				printf("Database: %s\n", currentPath.c_str());
+			}
+			else
+			{
+				continue;
+			}
 		}
 		std::list<std::string> subDirs;
-		printf("Directory: %s\n", currentPath.c_str());
-		while(!(fileName = currentDir.getNextFileName()).empty())
+		if(!currentPathIsDatabase)
 		{
-			if(UFile::getExtension(fileName).compare("db") == 0)
+			printf("Directory: %s\n", currentPath.c_str());
+			std::list<std::string> fileNames = currentDir.getFileNames();
+			if(topDirs.empty())
 			{
-				std::string filePath = currentPath + UDirectory::separator() + fileName;
+				for(std::list<std::string>::iterator iter = fileNames.begin(); iter!=fileNames.end(); ++iter)
+				{
+					topDirs.insert(currentPath+"/"+*iter);
+				}
+			}
+			else
+			{
+				if(topDirs.find(currentPath) != topDirs.end())
+				{
+					if(outputLatexStatisticsMap.size())
+					{
+						outputLatexStatistics.push_back(outputLatexStatisticsMap);
+						outputLatexStatisticsMap.clear();
+					}
+				}
+			}
+		}
+
+		while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
+		{
+			if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
+			{
+				std::string filePath;
+				if(currentPathIsDatabase)
+				{
+					filePath = currentPath;
+				}
+				else
+				{
+					filePath = currentPath + UDirectory::separator() + fileName;
+				}
+
 				DBDriver * driver = DBDriver::create();
+				ParametersMap params;
 				if(driver->openConnection(filePath))
 				{
+					params = driver->getLastParameters();
 					std::set<int> ids;
 					driver->getAllNodeIds(ids);
 					std::map<int, std::pair<std::map<std::string, float>, double> > stats = driver->getAllStatistics();
+					std::map<int, Transform> odomPoses, gtPoses;
 					std::vector<float> cameraTime;
 					cameraTime.reserve(ids.size());
 					std::vector<float> odomTime;
@@ -85,49 +168,251 @@ int main(int argc, char * argv[])
 					slamTime.reserve(ids.size());
 					float rmse = -1;
 					float maxRMSE = -1;
+					float rmseAng = -1;
+					float maxOdomRAM = -1;
+					float maxMapRAM = -1;
+					std::map<std::string, UPlotCurve*> curves;
+					std::map<std::string, double> firstStamps;
+					for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
+					{
+						curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
+					}
+
 					for(std::set<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 					{
 						Transform p, gt;
 						GPS gps;
-						int m, w;
+						int m=-1, w=-1;
 						std::string l;
 						double s;
 						std::vector<float> v;
-						driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps);
-
-						if(uContains(stats, *iter))
+						if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps))
 						{
-							const std::map<std::string, float> & stat = stats.at(*iter).first;
-							if(uContains(stat, Statistics::kGtTranslational_rmse()))
+							odomPoses.insert(std::make_pair(*iter, p));
+							if(!gt.isNull())
 							{
-								rmse = stat.at(Statistics::kGtTranslational_rmse());
-								if(maxRMSE==-1 || maxRMSE < rmse)
+								gtPoses.insert(std::make_pair(*iter, gt));
+							}
+							if(uContains(stats, *iter))
+							{
+								const std::map<std::string, float> & stat = stats.at(*iter).first;
+								if(uContains(stat, Statistics::kGtTranslational_rmse()))
 								{
-									maxRMSE = rmse;
+									rmse = stat.at(Statistics::kGtTranslational_rmse());
+									if(maxRMSE==-1 || maxRMSE < rmse)
+									{
+										maxRMSE = rmse;
+									}
+								}
+								if(uContains(stat, Statistics::kGtRotational_rmse()))
+								{
+									rmseAng = stat.at(Statistics::kGtRotational_rmse());
+								}
+								if(uContains(stat, std::string("Camera/TotalTime/ms")))
+								{
+									cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
+								}
+								if(uContains(stat, std::string("Odometry/TotalTime/ms")))
+								{
+									odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
+								}
+
+								if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
+								{
+									if(w>=0 || stat.at("RtabmapROS/TotalTime/ms") > 10.0f)
+									{
+										slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
+									}
+								}
+								else if(uContains(stat, Statistics::kTimingTotal()))
+								{
+									if(w>=0 || stat.at(Statistics::kTimingTotal()) > 10.0f)
+									{
+										slamTime.push_back(stat.at(Statistics::kTimingTotal()));
+									}
+								}
+
+								if(uContains(stat, std::string(Statistics::kMemoryRAM_usage())))
+								{
+									float ram = stat.at(Statistics::kMemoryRAM_usage());
+									if(maxMapRAM==-1 || maxMapRAM < ram)
+									{
+										maxMapRAM = ram;
+									}
+								}
+								if(uContains(stat, std::string("Odometry/RAM_usage/MB")))
+								{
+									float ram = stat.at("Odometry/RAM_usage/MB");
+									if(maxOdomRAM==-1 || maxOdomRAM < ram)
+									{
+										maxOdomRAM = ram;
+									}
+								}
+
+								for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
+								{
+									if(uContains(stat, jter->first))
+									{
+										if(!uContains(firstStamps, jter->first))
+										{
+											firstStamps.insert(std::make_pair(jter->first, s));
+										}
+										float x = s - firstStamps.at(jter->first);
+										float y = stat.at(jter->first);
+										jter->second->addValue(x,y);
+									}
 								}
-							}
-							if(uContains(stat, std::string("Camera/TotalTime/ms")))
-							{
-								cameraTime.push_back(stat.at(std::string("Camera/TotalTime/ms")));
-							}
-							if(uContains(stat, std::string("Odometry/TotalTime/ms")))
-							{
-								odomTime.push_back(stat.at(std::string("Odometry/TotalTime/ms")));
-							}
-							if(w >= 0 && uContains(stat, Statistics::kTimingTotal()))
-							{
-								slamTime.push_back(stat.at(Statistics::kTimingTotal()));
 							}
 						}
 					}
-					printf("   %s (%d): %fm (max=%fm), slam: avg=%dms max=%dms, odom: avg=%dms max=%dms, camera: avg=%dms max=%dms\n",
+
+					std::multimap<int, Link> links;
+					driver->getAllLinks(links, true);
+					std::multimap<int, Link> loopClosureLinks;
+					for(std::multimap<int, Link>::iterator jter=links.begin(); jter!=links.end(); ++jter)
+					{
+						if(jter->second.type() == Link::kGlobalClosure &&
+							graph::findLink(loopClosureLinks, jter->second.from(), jter->second.to()) == loopClosureLinks.end())
+						{
+							loopClosureLinks.insert(*jter);
+						}
+					}
+
+					UERROR("");
+					float bestScale = 1.0f;
+					float bestRMSE = rmse;
+					float bestRMSEAng = rmseAng;
+					float kitti_t_err = 0.0f;
+					float kitti_r_err = 0.0f;
+					if(ids.size())
+					{
+						std::map<int, Transform> posesOut;
+						std::multimap<int, Link> linksOut;
+						int firstId = *ids.begin();
+						rtabmap::Optimizer * optimizer = rtabmap::Optimizer::create(params);
+						optimizer->getConnectedGraph(firstId, odomPoses, graph::filterDuplicateLinks(links), posesOut, linksOut);
+
+						std::map<int, Transform> poses = optimizer->optimize(firstId, posesOut, linksOut);
+
+						std::map<int, Transform> groundTruth;
+						for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+						{
+							if(gtPoses.find(iter->first) != gtPoses.end())
+							{
+								groundTruth.insert(*gtPoses.find(iter->first));
+							}
+						}
+
+						if(outputScaled)
+						{
+							for(float scale=0.900f; scale<1.100f; scale+=0.001)
+							{
+								std::map<int, Transform> scaledPoses;
+								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+								{
+									Transform t = iter->second.clone();
+									t.x() *= scale;
+									t.y() *= scale;
+									t.z() *= scale;
+									scaledPoses.insert(std::make_pair(iter->first, t));
+								}
+								// compute RMSE statistics
+								float translational_rmse = 0.0f;
+								float translational_mean = 0.0f;
+								float translational_median = 0.0f;
+								float translational_std = 0.0f;
+								float translational_min = 0.0f;
+								float translational_max = 0.0f;
+								float rotational_rmse = 0.0f;
+								float rotational_mean = 0.0f;
+								float rotational_median = 0.0f;
+								float rotational_std = 0.0f;
+								float rotational_min = 0.0f;
+								float rotational_max = 0.0f;
+								graph::calcRMSE(
+										groundTruth,
+										scaledPoses,
+										translational_rmse,
+										translational_mean,
+										translational_median,
+										translational_std,
+										translational_min,
+										translational_max,
+										rotational_rmse,
+										rotational_mean,
+										rotational_median,
+										rotational_std,
+										rotational_min,
+										rotational_max);
+
+								if(scale!=0.900f && translational_rmse > bestRMSE)
+								{
+									break;
+								}
+								bestRMSE = translational_rmse;
+								bestRMSEAng = rotational_rmse;
+								bestScale = scale;
+							}
+							if(bestScale!=1.0f)
+							{
+								for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+								{
+									iter->second.x()*=bestScale;
+									iter->second.y()*=bestScale;
+									iter->second.z()*=bestScale;
+								}
+							}
+						}
+
+						if(outputKittiError)
+						{
+							if(groundTruth.size() == poses.size())
+							{
+								// compute KITTI statistics
+								graph::calcKittiSequenceErrors(uValues(groundTruth), uValues(poses), kitti_t_err, kitti_r_err);
+							}
+							else
+							{
+								printf("Cannot compute KITTI statistics as optimized poses and ground truth don't have the same size (%d vs %d).\n",
+										(int)poses.size(), (int)groundTruth.size());
+							}
+						}
+					}
+
+					printf("   %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm) ang=%.1fdeg%s, slam: avg=%dms (max=%dms) loops=%d, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
 							fileName.c_str(),
 							(int)ids.size(),
-							rmse,
+							bestScale,
+							bestRMSE,
 							maxRMSE,
+							bestRMSEAng,
+							!outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
 							(int)uMean(slamTime), (int)uMax(slamTime),
+							(int)loopClosureLinks.size(),
 							(int)uMean(odomTime), (int)uMax(odomTime),
-							(int)uMean(cameraTime), (int)uMax(cameraTime));
+							(int)uMean(cameraTime),
+							maxOdomRAM!=-1.0f?uFormat("RAM odom=%dMB ", (int)maxOdomRAM).c_str():"",
+							(int)maxMapRAM);
+
+					if(outputLatex)
+					{
+						std::vector<float> stats;
+						stats.push_back(ids.size());
+						stats.push_back(bestRMSE);
+						stats.push_back(maxRMSE);
+						stats.push_back(bestRMSEAng);
+						stats.push_back(uMean(odomTime));
+						stats.push_back(uMean(slamTime));
+						stats.push_back(uMax(slamTime));
+						stats.push_back(maxOdomRAM);
+						stats.push_back(maxMapRAM);
+						outputLatexStatisticsMap.insert(std::make_pair(filePath, stats));
+
+						if(maxOdomRAM != -1.0f)
+						{
+							odomRAMSet = true;
+						}
+					}
 				}
 				driver->closeConnection();
 				delete driver;
@@ -137,12 +422,112 @@ int main(int argc, char * argv[])
 				//sub directory
 				subDirs.push_front(currentPath + UDirectory::separator() + fileName);
 			}
+			currentPathIsDatabase = false;
 		}
+
 		for(std::list<std::string>::iterator iter=subDirs.begin(); iter!=subDirs.end(); ++iter)
 		{
 			paths.push_front(*iter);
 		}
+
+		if(outputLatexStatisticsMap.size() && paths.empty())
+		{
+			outputLatexStatistics.push_back(outputLatexStatisticsMap);
+			outputLatexStatisticsMap.clear();
+		}
 	}
 
+	if(outputLatex && outputLatexStatistics.size())
+	{
+		printf("\nLaTeX output:\n----------------\n");
+		printf("\\begin{table*}[!t]\n");
+		printf("\\caption{$t_{end}$ is the absolute translational RMSE value at the end "
+				"of the experiment as $ATE_{max}$ is the maximum during the experiment. "
+				"$r_{end}$ is rotational RMSE value at the end of the experiment. "
+				"$o_{avg}$ and $m_{avg}$ are the average computational time "
+				"for odometry (front-end) and map update (back-end). "
+				"$m_{avg}$ is the maximum computational time for map update. "
+				"$O_{end}$ and $M_{end}$ are the RAM usage at the end of the experiment "
+				"for odometry and map management respectively.}\n");
+		printf("\\label{}\n");
+		printf("\\centering\n");
+		if(odomRAMSet)
+		{
+			printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c|c}\n");
+			printf("\\cline{2-10}\n");
+			printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $O_{end}$ & $M_{end}$  \\\\\n");
+			printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB) & (MB) \\\\\n");
+		}
+		else
+		{
+			printf("\\begin{tabular}{l|c|c|c|c|c|c|c|c}\n");
+			printf("\\cline{2-9}\n");
+			printf(" & Size & $t_{end}$ & $t_{max}$ & $r_{end}$ & $o_{avg}$ & $m_{avg}$ & $m_{max}$ & $M_{end}$  \\\\\n");
+			printf(" & (nodes) & (m) & (m) & (deg) & (ms) & (ms) & (ms) & (MB)  \\\\\n");
+		}
+
+		printf("\\hline\n");
+
+		for(unsigned int j=0; j<outputLatexStatistics.size(); ++j)
+		{
+			if(outputLatexStatistics[j].size())
+			{
+				std::vector<int> lowestIndex;
+				if(outputLatexStatistics[j].size() > 1)
+				{
+					std::vector<float> lowestValue(outputLatexStatistics[j].begin()->second.size(),-1);
+					lowestIndex = std::vector<int>(lowestValue.size(),0);
+					int index = 0;
+					for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
+					{
+						UASSERT(lowestValue.size() == iter->second.size());
+						for(unsigned int i=0; i<iter->second.size(); ++i)
+						{
+							if(lowestValue[i] == -1 || (iter->second[i]>0.0f && lowestValue[i]>iter->second[i]))
+							{
+								lowestValue[i] = iter->second[i];
+								lowestIndex[i] = index;
+							}
+						}
+						++index;
+					}
+				}
+
+				int index = 0;
+				for(std::map<std::string, std::vector<float> >::iterator iter=outputLatexStatistics[j].begin(); iter!=outputLatexStatistics[j].end(); ++iter)
+				{
+					UASSERT(iter->second.size() == 9);
+					printf("%s & ", uReplaceChar(iter->first.c_str(), '_', '-').c_str());
+					printf("%d & ", (int)iter->second[0]);
+					printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[1]==index?"\\textbf{":"", iter->second[1], lowestIndex.size()&&lowestIndex[1]==index?"}":"");
+					printf("%s%.3f%s & ", lowestIndex.size()&&lowestIndex[2]==index?"\\textbf{":"", iter->second[2], lowestIndex.size()&&lowestIndex[2]==index?"}":"");
+					printf("%s%.2f%s & ", lowestIndex.size()&&lowestIndex[3]==index?"\\textbf{":"", iter->second[3], lowestIndex.size()&&lowestIndex[3]==index?"}":"");
+					printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[4]==index?"\\textbf{":"", (int)iter->second[4], lowestIndex.size()&&lowestIndex[4]==index?"}":"");
+					printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[5]==index?"\\textbf{":"", (int)iter->second[5], lowestIndex.size()&&lowestIndex[5]==index?"}":"");
+					printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[6]==index?"\\textbf{":"", (int)iter->second[6], lowestIndex.size()&&lowestIndex[6]==index?"}":"");
+					if(odomRAMSet)
+					{
+						printf("%s%d%s & ", lowestIndex.size()&&lowestIndex[7]==index?"\\textbf{":"", (int)iter->second[7], lowestIndex.size()&&lowestIndex[7]==index?"}":"");
+					}
+					printf("%s%d%s ", lowestIndex.size()&&lowestIndex[8]==index?"\\textbf{":"", (int)iter->second[8], lowestIndex.size()&&lowestIndex[8]==index?"}":"");
+					printf("\\\\\n");
+					++index;
+				}
+				printf("\\hline\n");
+			}
+		}
+
+		printf("\\end{tabular}\n");
+		printf("\\end{table*}\n----------------\n");
+	}
+
+	if(figures.size())
+	{
+		for(std::map<std::string, UPlot*>::iterator iter=figures.begin(); iter!=figures.end(); ++iter)
+		{
+			iter->second->show();
+		}
+		return app.exec();
+	}
 	return 0;
 }

tools/Reprocess/CMakeLists.txt

@@ -0,0 +1,52 @@
+
+SET(RTABMap_INCLUDE_DIRS 
+    ${PROJECT_SOURCE_DIR}/utilite/include
+	${PROJECT_SOURCE_DIR}/corelib/include
+)
+SET(RTABMap_LIBRARIES 
+    rtabmap_core
+	rtabmap_utilite
+)  
+
+if(POLICY CMP0020)
+	cmake_policy(SET CMP0020 OLD)
+endif()
+
+SET(INCLUDE_DIRS
+	${RTABMap_INCLUDE_DIRS}
+    ${OpenCV_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+)
+
+SET(LIBRARIES
+	${RTABMap_LIBRARIES}
+	${OpenCV_LIBRARIES}
+	${PCL_LIBRARIES}
+)
+
+IF(OCTOMAP_FOUND)
+    SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${OCTOMAP_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${OCTOMAP_LIBRARIES}
+	)
+ENDIF(OCTOMAP_FOUND)
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
+ADD_EXECUTABLE(reprocess main.cpp)
+  
+TARGET_LINK_LIBRARIES(reprocess ${LIBRARIES})
+
+SET_TARGET_PROPERTIES( reprocess 
+	PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-reprocess)
+
+INSTALL(TARGETS reprocess
+		RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+		BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)
+
+
+

tools/Reprocess/main.cpp

@@ -0,0 +1,428 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/DBDriver.h>
+#include <rtabmap/core/DBReader.h>
+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/OctoMap.h>
+#endif
+#include <rtabmap/core/OccupancyGrid.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UStl.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <pcl/io/pcd_io.h>
+
+using namespace rtabmap;
+
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-reprocess [options] \"input.db\" \"output.db\"\n"
+			"  Options:\n"
+			"     -r          Use database stamps as input rate.\n"
+			"     -g2         Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\".\n"
+			"     -g3         Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
+			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
+			"     -o3         Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
+			"%s\n"
+			"\n", Parameters::showUsage());
+	exit(1);
+}
+
+class RecoveryProgressState: public ProgressState
+{
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+			printf("%s\n", msg.c_str());
+		return true;
+	}
+};
+
+int main(int argc, char * argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kError);
+
+	if(argc < 3)
+	{
+		showUsage();
+	}
+
+	bool assemble2dMap = false;
+	bool assemble3dMap = false;
+	bool assemble2dOctoMap = false;
+	bool assemble3dOctoMap = false;
+	bool useDatabaseRate = false;
+	for(int i=1; i<argc-2; ++i)
+	{
+		if(strcmp(argv[i], "-r") == 0)
+		{
+			useDatabaseRate = true;
+			printf("Using database stamps as input rate.\n");
+		}
+		else if(strcmp(argv[i], "-g2") == 0)
+		{
+			assemble2dMap = true;
+			printf("2D occupancy grid will be assembled (-g2 option).\n");
+		}
+		else if(strcmp(argv[i], "-g3") == 0)
+		{
+			assemble3dMap = true;
+			printf("3D cloud map will be assembled (-g3 option).\n");
+		}
+		else if(strcmp(argv[i], "-o2") == 0)
+		{
+#ifdef RTABMAP_OCTOMAP
+			assemble2dOctoMap = true;
+			printf("OctoMap will be assembled (-o2 option).\n");
+#else
+			printf("RTAB-Map is not built with OctoMap support, cannot set -o2 option!\n");
+#endif
+		}
+		else if(strcmp(argv[i], "-o3") == 0)
+		{
+#ifdef RTABMAP_OCTOMAP
+			assemble3dOctoMap = true;
+			printf("OctoMap will be assembled (-o3 option).\n");
+#else
+			printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
+#endif
+		}
+	}
+
+	ParametersMap customParameters = Parameters::parseArguments(argc, argv);
+
+	std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
+	std::string outputDatabasePath = uReplaceChar(argv[argc-1], '~', UDirectory::homeDir());
+
+	if(!UFile::exists(inputDatabasePath))
+	{
+		printf("Input database \"%s\" doesn't exist!\n", inputDatabasePath.c_str());
+		return -1;
+	}
+
+	if(UFile::getExtension(inputDatabasePath).compare("db") != 0)
+	{
+		printf("File \"%s\" is not a database format (*.db)!\n", inputDatabasePath.c_str());
+		return -1;
+	}
+	if(UFile::getExtension(outputDatabasePath).compare("db") != 0)
+	{
+		printf("File \"%s\" is not a database format (*.db)!\n", outputDatabasePath.c_str());
+		return -1;
+	}
+
+	if(UFile::exists(outputDatabasePath))
+	{
+		UFile::erase(outputDatabasePath);
+	}
+
+	DBDriver * dbDriver = DBDriver::create();
+	if(!dbDriver->openConnection(inputDatabasePath, false))
+	{
+		printf("Failed opening input database!\n");
+		delete dbDriver;
+		return -1;
+	}
+
+	ParametersMap parameters = dbDriver->getLastParameters();
+	if(parameters.empty())
+	{
+		printf("Failed getting parameters from database, reprocessing cannot be done. Database version may be too old.\n");
+		dbDriver->closeConnection(false);
+		delete dbDriver;
+		return -1;
+	}
+	if(customParameters.size())
+	{
+		printf("Custom parameters:\n");
+		for(ParametersMap::iterator iter=customParameters.begin(); iter!=customParameters.end(); ++iter)
+		{
+			printf("  %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
+		}
+	}
+	uInsert(parameters, customParameters);
+	std::set<int> ids;
+	dbDriver->getAllNodeIds(ids);
+	if(ids.empty())
+	{
+		printf("Input database doesn't have any nodes saved in it.\n");
+		dbDriver->closeConnection(false);
+		delete dbDriver;
+		return -1;
+	}
+
+	dbDriver->closeConnection(false);
+	delete dbDriver;
+
+	Rtabmap rtabmap;
+	rtabmap.init(parameters, outputDatabasePath);
+
+	bool odometryIgnored = false;
+	Parameters::parse(parameters, Parameters::kRGBDEnabled(), odometryIgnored);
+	DBReader dbReader(inputDatabasePath, useDatabaseRate?-1:0, !odometryIgnored);
+	dbReader.init();
+
+	OccupancyGrid grid(parameters);
+	grid.setCloudAssembling(assemble3dMap);
+#ifdef RTABMAP_OCTOMAP
+	OctoMap octomap(parameters);
+#endif
+
+	printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
+	std::map<std::string, float> globalMapStats;
+	int processed = 0;
+	CameraInfo info;
+	SensorData data = dbReader.takeImage(&info);
+	while(data.isValid())
+	{
+		UTimer iterationTime;
+		std::string status;
+		if(!odometryIgnored && info.odomPose.isNull())
+		{
+			printf("Skipping node %d as it doesn't have odometry pose set.\n", data.id());
+		}
+		else
+		{
+			if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
+			{
+				printf("High variance detected, triggering a new map...\n");
+				rtabmap.triggerNewMap();
+			}
+			if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
+			{
+				printf("Failed processing node %d.\n", data.id());
+				globalMapStats.clear();
+			}
+			else if(assemble2dMap || assemble3dMap || assemble2dOctoMap || assemble3dOctoMap)
+			{
+				globalMapStats.clear();
+				double timeUpdateInit = 0.0;
+				double timeUpdateGrid = 0.0;
+				double timeUpdateOctoMap = 0.0;
+				const rtabmap::Statistics & stats = rtabmap.getStatistics();
+				UTimer t;
+				if(stats.poses().size() && stats.getSignatures().size())
+				{
+					int id = stats.poses().rbegin()->first;
+					if(stats.getSignatures().find(id)!=stats.getSignatures().end() &&
+					   stats.getSignatures().find(id)->second.sensorData().gridCellSize() > 0.0f)
+					{
+						bool updateGridMap = false;
+						bool updateOctoMap = false;
+						if((assemble2dMap || assemble3dMap) && grid.addedNodes().find(id) == grid.addedNodes().end())
+						{
+							updateGridMap = true;
+						}
+#ifdef RTABMAP_OCTOMAP
+						if((assemble2dOctoMap || assemble3dOctoMap) && octomap.addedNodes().find(id) == octomap.addedNodes().end())
+						{
+							updateOctoMap = true;
+						}
+#endif
+						if(updateGridMap || updateOctoMap)
+						{
+							cv::Mat ground, obstacles;
+							stats.getSignatures().find(id)->second.sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+							const cv::Point3f & viewpoint = stats.getSignatures().find(id)->second.sensorData().gridViewPoint();
+
+							timeUpdateInit = t.ticks();
+
+							if(updateGridMap)
+							{
+								grid.addToCache(id, ground, obstacles);
+								grid.update(stats.poses());
+								timeUpdateGrid = t.ticks() + timeUpdateInit;
+							}
+#ifdef RTABMAP_OCTOMAP
+							if(updateOctoMap)
+							{
+								octomap.addToCache(id, ground, obstacles, viewpoint);
+								octomap.update(stats.poses());
+								timeUpdateOctoMap = t.ticks() + timeUpdateInit;
+							}
+#endif
+						}
+					}
+				}
+
+				//Simulate publishing
+				double timePub2dOctoMap = 0.0;
+				double timePub3dOctoMap = 0.0;
+				if(assemble2dOctoMap)
+				{
+					float xMin, yMin, size;
+					octomap.createProjectionMap(xMin, yMin, size);
+					timePub2dOctoMap = t.ticks();
+				}
+				if(assemble3dOctoMap)
+				{
+					octomap.createCloud();
+					timePub3dOctoMap = t.ticks();
+				}
+
+				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
+				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
+				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
+				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
+			}
+		}
+
+		printf("Processed %d/%d nodes... %dms\n", ++processed, (int)ids.size(), int(iterationTime.ticks()*1000));
+
+		data = dbReader.takeImage(&info);
+	}
+
+	printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
+	rtabmap.close(true);
+	printf("Closing database \"%s\"... done!\n", outputDatabasePath.c_str());
+
+	if(assemble2dMap)
+	{
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pgm";
+		float xMin,yMin;
+		cv::Mat map = grid.getMap(xMin, yMin);
+		if(!map.empty())
+		{
+			cv::Mat map8U(map.rows, map.cols, CV_8U);
+			//convert to gray scaled map
+			for (int i = 0; i < map.rows; ++i)
+			{
+				for (int j = 0; j < map.cols; ++j)
+				{
+					char v = map.at<char>(i, j);
+					unsigned char gray;
+					if(v == 0)
+					{
+						gray = 178;
+					}
+					else if(v == 100)
+					{
+						gray = 0;
+					}
+					else // -1
+					{
+						gray = 89;
+					}
+					map8U.at<unsigned char>(i, j) = gray;
+				}
+			}
+			if(cv::imwrite(outputPath, map8U))
+			{
+				printf("Saving occupancy grid \"%s\"... done!\n", outputPath.c_str());
+			}
+			else
+			{
+				printf("Saving occupancy grid \"%s\"... failed!\n", outputPath.c_str());
+			}
+		}
+		else
+		{
+			printf("2D map is empty! Cannot save it!\n");
+		}
+	}
+	if(assemble3dMap)
+	{
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_map.pcd";
+		if(pcl::io::savePCDFileBinary(outputPath, *grid.getMapObstacles()) == 0)
+		{
+			printf("Saving 3d cloud map \"%s\"... done!\n", outputPath.c_str());
+		}
+		else
+		{
+			printf("Saving 3d cloud map \"%s\"... failed!\n", outputPath.c_str());
+		}
+	}
+#ifdef RTABMAP_OCTOMAP
+	if(assemble2dOctoMap)
+	{
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pgm";
+		float xMin,yMin,cellSize;
+		cv::Mat map = octomap.createProjectionMap(xMin, yMin, cellSize);
+		if(!map.empty())
+		{
+			cv::Mat map8U(map.rows, map.cols, CV_8U);
+			//convert to gray scaled map
+			for (int i = 0; i < map.rows; ++i)
+			{
+				for (int j = 0; j < map.cols; ++j)
+				{
+					char v = map.at<char>(i, j);
+					unsigned char gray;
+					if(v == 0)
+					{
+						gray = 178;
+					}
+					else if(v == 100)
+					{
+						gray = 0;
+					}
+					else // -1
+					{
+						gray = 89;
+					}
+					map8U.at<unsigned char>(i, j) = gray;
+				}
+			}
+			if(cv::imwrite(outputPath, map8U))
+			{
+				printf("Saving octomap 2D projection \"%s\"... done!\n", outputPath.c_str());
+			}
+			else
+			{
+				printf("Saving octomap 2D projection \"%s\"... failed!\n", outputPath.c_str());
+			}
+		}
+		else
+		{
+			printf("OctoMap 2D projection map is empty! Cannot save it!\n");
+		}
+	}
+	if(assemble3dOctoMap)
+	{
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3) + "_octomap.pcd";
+		std::vector<int> obstacles;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap.createCloud(0, &obstacles);
+		if(pcl::io::savePCDFile(outputPath, *cloud, obstacles, true) == 0)
+		{
+			printf("Saving octomap cloud \"%s\"... done!\n", outputPath.c_str());
+		}
+		else
+		{
+			printf("Saving octomap cloud \"%s\"... failed!\n", outputPath.c_str());
+		}
+	}
+#endif
+
+	return 0;
+}

tools/RgbdDataset/CMakeLists.txt

@@ -35,3 +35,7 @@ TARGET_LINK_LIBRARIES(rgbd_dataset ${LIBRARIES})
 
 SET_TARGET_PROPERTIES( rgbd_dataset 
     PROPERTIES OUTPUT_NAME ${PROJECT_PREFIX}-rgbd_dataset)
+    
+INSTALL(TARGETS rgbd_dataset
+	RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT runtime
+	BUNDLE DESTINATION "${CMAKE_BUNDLE_LOCATION}" COMPONENT runtime)

tools/RgbdDataset/main.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UProcessInfo.h"
 #include <pcl/common/common.h>
 #include <stdio.h>
 #include <signal.h>
@@ -54,7 +55,8 @@ void showUsage()
 			"                        synchronized images using associate.py tool (use tool version from\n"
 			"                        https://gist.github.com/matlabbe/484134a2d9da8ad425362c6669824798). If \n"
 			"                        \"groundtruth.txt\" is found in the sequence folder, they will be saved in the database.\n"
-			"  --output           Output directory. By default, results are saved in \"path\".\n\n"
+			"  --output           Output directory. By default, results are saved in \"path\".\n"
+			"  --output_name      Output database name (default \"rtabmap\").\n"
 			"  --quiet            Don't show log messages and iteration updates.\n"
 			"%s\n"
 			"Example:\n\n"
@@ -97,6 +99,7 @@ int main(int argc, char * argv[])
 	ParametersMap parameters;
 	std::string path;
 	std::string output;
+	std::string outputName = "rtabmap";
 	bool quiet = false;
 	if(argc < 2)
 	{
@@ -110,6 +113,10 @@ int main(int argc, char * argv[])
 			{
 				output = argv[++i];
 			}
+			else if(std::strcmp(argv[i], "--output_name") == 0)
+			{
+				outputName = argv[++i];
+			}
 			else if(std::strcmp(argv[i], "--quiet") == 0)
 			{
 				quiet = true;
@@ -128,6 +135,8 @@ int main(int argc, char * argv[])
 			output = uReplaceChar(output, '~', UDirectory::homeDir());
 			UDirectory::makeDir(output);
 		}
+		parameters.insert(ParametersPair(Parameters::kRtabmapWorkingDirectory(), output));
+		parameters.insert(ParametersPair(Parameters::kRtabmapPublishRAMUsage(), "true"));
 	}
 
 	std::string seq = uSplit(path, '/').back();
@@ -150,12 +159,14 @@ int main(int argc, char * argv[])
 			"   Dataset path:    %s\n"
 			"   RGB path:        %s\n"
 			"   Depth path:      %s\n"
-			"   Output:          %s\n",
+			"   Output:          %s\n"
+			"   Output name:     %s\n",
 			seq.c_str(),
 			path.c_str(),
 			pathRgbImages.c_str(),
 			pathDepthImages.c_str(),
-			output.c_str());
+			output.c_str(),
+			outputName.c_str());
 	if(!pathGt.empty())
 	{
 		printf("   groundtruth.txt: %s\n", pathGt.c_str());
@@ -177,16 +188,15 @@ int main(int argc, char * argv[])
 	float depthFactor = 5.0f;
 	if(sequenceName.find("freiburg1") != std::string::npos)
 	{
-		model = CameraModel("rtabmap_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
+		model = CameraModel(outputName+"_calib", 517.3, 516.5, 318.6, 255.3, opticalRotation, 0, cv::Size(640,480));
 	}
 	else if(sequenceName.find("freiburg2") != std::string::npos)
 	{
-		model = CameraModel("rtabmap_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
-		depthFactor = 5.208f; // based on TUM2.yaml ORB_SLAM2 file
+		model = CameraModel(outputName+"_calib", 520.9, 521.0, 325.1, 249.7, opticalRotation, 0, cv::Size(640,480));
 	}
 	else //if(sequenceName.find("freiburg3") != std::string::npos)
 	{
-		model = CameraModel("rtabmap_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
+		model = CameraModel(outputName+"_calib", 535.4, 539.2, 320.1, 247.6, opticalRotation, 0, cv::Size(640,480));
 	}
 	//parameters.insert(ParametersPair(Parameters::kg2oBaseline(), uNumber2Str(40.0f/model.fx())));
 	model.save(path);
@@ -206,17 +216,20 @@ int main(int argc, char * argv[])
 
 	bool intermediateNodes = Parameters::defaultRtabmapCreateIntermediateNodes();
 	float detectionRate = Parameters::defaultRtabmapDetectionRate();
-	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), intermediateNodes);
+	int odomStrategy = Parameters::defaultOdomStrategy();
+	Parameters::parse(parameters, Parameters::kOdomStrategy(), odomStrategy);
 	Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), detectionRate);
-	std::string databasePath = output+"/"+seq+".db";
+	std::string databasePath = output+"/"+outputName+".db";
 	UFile::erase(databasePath);
-	if(cameraThread.camera()->init(path, "rtabmap_calib"))
+	if(cameraThread.camera()->init(path, outputName+"_calib"))
 	{
 		int totalImages = (int)((CameraRGBDImages*)cameraThread.camera())->filenames().size();
 
 		printf("Processing %d images...\n", totalImages);
 
-		Odometry * odom = Odometry::create(parameters);
+		ParametersMap odomParameters = parameters;
+		odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
+		Odometry * odom = Odometry::create(odomParameters);
 		Rtabmap rtabmap;
 		rtabmap.init(parameters, databasePath);
 
@@ -234,34 +247,21 @@ int main(int argc, char * argv[])
 		double previousStamp = 0.0;
 		while(data.isValid() && g_forever)
 		{
-			std::map<std::string, float> externalStats;
 			cameraThread.postUpdate(&data, &cameraInfo);
 			cameraInfo.timeTotal = timer.ticks();
 
-			// save camera statistics to database
-			externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
-			externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
-			externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
-			externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
-			externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
-			externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
-			externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
-			externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
-
 			OdometryInfo odomInfo;
 			Transform pose = odom->process(data, &odomInfo);
-			externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
-			externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
-			externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
-			externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
-			externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
-			externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
-			externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
-			externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
-			externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
-			externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
+			if(odomStrategy == 2)
+			{
+				//special case for FOVIS, set covariance 1 if 9999 is detected
+				if(!odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at<double>(0,0) >= 9999)
+				{
+					odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+				}
+			}
+
 			if(odomInfo.keyFrameAdded)
 			{
 				++odomKeyFrames;
@@ -295,6 +295,31 @@ int main(int argc, char * argv[])
 			timer.restart();
 			if(processData)
 			{
+				std::map<std::string, float> externalStats;
+				// save camera statistics to database
+				externalStats.insert(std::make_pair("Camera/BilateralFiltering/ms", cameraInfo.timeBilateralFiltering*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Capture/ms", cameraInfo.timeCapture*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Disparity/ms", cameraInfo.timeDisparity*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ImageDecimation/ms", cameraInfo.timeImageDecimation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/Mirroring/ms", cameraInfo.timeMirroring*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ExposureCompensation/ms", cameraInfo.timeStereoExposureCompensation*1000.0f));
+				externalStats.insert(std::make_pair("Camera/ScanFromDepth/ms", cameraInfo.timeScanFromDepth*1000.0f));
+				externalStats.insert(std::make_pair("Camera/TotalTime/ms", cameraInfo.timeTotal*1000.0f));
+				externalStats.insert(std::make_pair("Camera/UndistortDepth/ms", cameraInfo.timeUndistortDepth*1000.0f));
+				// save odometry statistics to database
+				externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", odomInfo.localBundleTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", odomInfo.localBundleConstraints));
+				externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", odomInfo.localBundleOutliers));
+				externalStats.insert(std::make_pair("Odometry/TotalTime/ms", odomInfo.timeEstimation*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Registration/ms", odomInfo.reg.totalTime*1000.0f));
+				externalStats.insert(std::make_pair("Odometry/Inliers/", odomInfo.reg.inliers));
+				externalStats.insert(std::make_pair("Odometry/Features/", odomInfo.features));
+				externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", odomInfo.distanceTravelled));
+				externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", odomInfo.keyFrameAdded));
+				externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", odomInfo.localKeyFrames));
+				externalStats.insert(std::make_pair("Odometry/LocalMapSize/", odomInfo.localMapSize));
+				externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize));
+
 				OdometryEvent e(SensorData(), Transform(), odomInfo);
 				rtabmap.process(data, pose, covariance, e.velocity(), externalStats);
 				covariance = cv::Mat();
@@ -313,8 +338,10 @@ int main(int argc, char * argv[])
 
 				if(rmse >= 0.0f)
 				{
-					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
-							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+					//printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad",
+					//		iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at<double>(0,0)), sqrt(odomInfo.reg.covariance.at<double>(3,3)));
+					printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm",
+							iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse);
 				}
 				else
 				{
@@ -354,7 +381,7 @@ int main(int argc, char * argv[])
 		{
 			stamps.insert(std::make_pair(iter->first, iter->second.getStamp()));
 		}
-		std::string pathTrajectory = output+"/"+seq+"_poses.txt";
+		std::string pathTrajectory = output+"/"+outputName+"_poses.txt";
 		if(poses.size() && graph::exportPoses(pathTrajectory, 1, poses, links, stamps))
 		{
 			printf("Saving %s... done!\n", pathTrajectory.c_str());
@@ -418,7 +445,7 @@ int main(int argc, char * argv[])
 			printf("   rotational_rmse=      %f deg\n", rotational_rmse);
 
 			FILE * pFile = 0;
-			std::string pathErrors = output+"/"+seq+"_rmse.txt";
+			std::string pathErrors = output+"/"+outputName+"_rmse.txt";
 			pFile = fopen(pathErrors.c_str(),"w");
 			if(!pFile)
 			{
@@ -445,9 +472,9 @@ int main(int argc, char * argv[])
 		UERROR("Camera init failed!");
 	}
 
-	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+seq+".db").c_str());
+	printf("Saving rtabmap database (with all statistics) to \"%s\"\n", (output+"/"+outputName+".db").c_str());
 	printf("Do:\n"
-			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+seq+".db").c_str());
+			" $ rtabmap-databaseViewer %s\n\n", (output+"/"+outputName+".db").c_str());
 
 	return 0;
 }

utilite/src/UConversion.cpp

@@ -18,6 +18,7 @@
 */
 
 #include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/UStl.h"
 
 #include <sstream>
 #include <string.h>
@@ -153,12 +154,12 @@ std::string uBool2Str(bool boolean)
 
 bool uStr2Bool(const char * str)
 {
-	return !(str && (strcmp(str, "false") == 0 || strcmp(str, "FALSE") == 0 || strcmp(str, "0") == 0));
+	return !(str && (uStrContains(str, "false") || uStrContains(str, "FALSE") || strcmp(str, "0") == 0));
 }
 
 bool uStr2Bool(const std::string & str)
 {
-	return !(str.compare("false") == 0 || str.compare("FALSE") == 0 || str.compare("0") == 0);
+	return !(uStrContains(str, "false") || uStrContains(str, "FALSE") || str.compare("0") == 0);
 }
 
 std::vector<unsigned char> uStr2Bytes(const std::string & str)