0.17.0: saving optimized poses and last localization pose to database (…

…introlab/rtabmap_ros#220). Parameters: fixed Icp default parameters when not built with libpointmatcher, added RGBD/SavedLocalizationIgnored (default false). DbViewer: added Export/Import 2D map (introlab/rtabmap_ros#213).

CMakeLists.txt

@@ -20,8 +20,8 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 # VERSION
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
-SET(RTABMAP_MINOR_VERSION 16)
-SET(RTABMAP_PATCH_VERSION 3)
+SET(RTABMAP_MINOR_VERSION 17)
+SET(RTABMAP_PATCH_VERSION 0)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
 

app/android/jni/RTABMapApp.cpp

@@ -188,8 +188,6 @@ RTABMapApp::RTABMapApp() :
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
 		optMesh_(new pcl::TextureMesh),
-		optRefId_(0),
-		optRefPose_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
@@ -321,12 +319,6 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	// Open visualization while we load (if there is an optimized mesh saved in database)
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
-	optRefId_ = 0;
-	if(optRefPose_)
-	{
-		delete optRefPose_;
-		optRefPose_ = 0;
-	}
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<unsigned int> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -335,25 +327,18 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 	cv::Mat textures;
-	std::map<int, rtabmap::Transform> optPoses;
 	if(!databaseSource.empty())
 	{
 		UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
 		rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
 		if(driver->openConnection(databaseSource))
 		{
-			cloudMat = driver->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+			cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
 			{
 				LOGI("Open: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
-				if(optPoses.size())
-				{
-					// just take the last as reference
-					optRefId_ = optPoses.rbegin()->first;
-					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
-				}
 				if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
@@ -1128,12 +1113,6 @@ int RTABMapApp::Render()
 					mapToOdom_ = stats.mapCorrection();
 				}
 
-				std::map<int, rtabmap::Transform>::const_iterator iter = stats.poses().find(optRefId_);
-				if(iter != stats.poses().end() && !iter->second.isNull() && optRefPose_)
-				{
-					// adjust opt mesh pose
-					main_scene_.setCloudPose(g_optMeshId, opengl_world_T_rtabmap_world * iter->second * (*optRefPose_).inverse());
-				}
 				int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
 				int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
 				int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
@@ -2713,7 +2692,7 @@ bool RTABMapApp::exportMesh(
 						}
 						boost::mutex::scoped_lock  lock(rtabmapMutex_);
 
-						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons);
+						rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons);
 						success = true;
 					}
 				}
@@ -2734,7 +2713,7 @@ bool RTABMapApp::exportMesh(
 						}
 					}
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
-					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons, textureMesh->tex_coordinates, globalTextures);
+					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons, textureMesh->tex_coordinates, globalTextures);
 					success = true;
 				}
 				else
@@ -2857,7 +2836,7 @@ bool RTABMapApp::exportMesh(
 				{
 					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database
 					boost::mutex::scoped_lock  lock(rtabmapMutex_);
-					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses);
+					rtabmap_->getMemory()->saveOptimizedMesh(cloudMat);
 					success = true;
 				}
 			}
@@ -2895,12 +2874,6 @@ bool RTABMapApp::postExportation(bool visualize)
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
 	optMesh_.reset(new pcl::TextureMesh);
 	optTexture_ = cv::Mat();
-	optRefId_ = 0;
-	if(optRefPose_)
-	{
-		delete optRefPose_;
-		optRefPose_ = 0;
-	}
 	exportedMeshUpdated_ = false;
 
 	if(visualize)
@@ -2914,23 +2887,15 @@ bool RTABMapApp::postExportation(bool visualize)
 		std::vector<std::vector<Eigen::Vector2f> > texCoords;
 #endif
 		cv::Mat textures;
-		std::map<int, rtabmap::Transform> optPoses;
 		if(rtabmap_ && rtabmap_->getMemory())
 		{
-			cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures);
+			cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
 			if(!cloudMat.empty())
 			{
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
 				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
 				optTexture_ = textures;
 
-				if(optPoses.size())
-				{
-					// just take the last as reference
-					optRefId_ = optPoses.rbegin()->first;
-					optRefPose_ = new rtabmap::Transform(optPoses.rbegin()->second);
-				}
-
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
 				exportedMeshUpdated_ = true;

app/android/jni/RTABMapApp.h

@@ -235,8 +235,6 @@ class RTABMapApp : public UEventsHandler {
   bool exportedMeshUpdated_;
   pcl::TextureMesh::Ptr optMesh_;
   cv::Mat optTexture_;
-  int optRefId_;
-  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.

corelib/include/rtabmap/core/DBDriver.h

@@ -102,9 +102,12 @@ class RTABMAP_EXP DBDriver : public UThreadNode
 	void addStatistics(const Statistics & statistics) const;
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
@@ -113,7 +116,6 @@ class RTABMAP_EXP DBDriver : public UThreadNode
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
 	cv::Mat loadOptimizedMesh(
-			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
@@ -230,9 +232,12 @@ class RTABMAP_EXP DBDriver : public UThreadNode
 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
 	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose) const = 0;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
 	virtual void saveOptimizedMeshQuery(
 				const cv::Mat & cloud,
-				const std::map<int, Transform> & poses,
 				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -241,7 +246,6 @@ class RTABMAP_EXP DBDriver : public UThreadNode
 #endif
 				const cv::Mat & textures) const = 0;
 	virtual cv::Mat loadOptimizedMeshQuery(
-				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,

corelib/include/rtabmap/core/Memory.h

@@ -95,9 +95,12 @@ class RTABMAP_EXP Memory
 	void saveStatistics(const Statistics & statistics);
 	void savePreviewImage(const cv::Mat & image) const;
 	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
 	void saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses = std::map<int, Transform>(), // if we want to do localization afterward using optimized mesh
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
@@ -106,7 +109,6 @@ class RTABMAP_EXP Memory
 #endif
 			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
 	cv::Mat loadOptimizedMesh(
-			std::map<int, Transform> * poses = 0,
 			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -42,6 +42,7 @@ class RTABMAP_EXP OccupancyGrid
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
+	void setMap(const cv::Mat & map, float xMin, float yMin, float cellSize, const std::map<int, Transform> & poses);
 	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
 	void setCloudAssembling(bool enabled);

corelib/include/rtabmap/core/Parameters.h

@@ -336,6 +336,7 @@ class RTABMAP_EXP Parameters
     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.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, SavedLocalizationIgnored, bool, false, "Ignore last saved localization pose from previous session. If true, RTAB-Map won't assume it is restarting from the same place than where it shut down previously.");
     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.");
@@ -550,11 +551,19 @@ class RTABMAP_EXP Parameters
     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.");
+#ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
+#else
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
+#endif
     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.1,   "Ratio of matching correspondences to accept the transform.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PointToPlane,              bool, true,   "Use point to plane ICP.");
+#else
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
+#endif
     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.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -253,6 +253,7 @@ class RTABMAP_EXP Rtabmap
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _savedLocalizationIgnored;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;

corelib/include/rtabmap/core/util3d_mapping.h

@@ -114,7 +114,8 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 		cv::Mat & grid,
 		bool stopOnObstacle);
 
-cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
+cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false);
+cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false);
 
 cv::Mat RTABMAP_EXP erodeMap(const cv::Mat & map);
 

corelib/src/DBDriver.cpp

@@ -1061,9 +1061,37 @@ cv::Mat DBDriver::loadPreviewImage() const
 	return image;
 }
 
+void DBDriver::saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const
+{
+	_dbSafeAccessMutex.lock();
+	saveOptimizedPosesQuery(optimizedPoses, lastlocalizationPose);
+	_dbSafeAccessMutex.unlock();
+}
+std::map<int, Transform> DBDriver::loadOptimizedPoses(Transform * lastlocalizationPose) const
+{
+	_dbSafeAccessMutex.lock();
+	std::map<int, Transform> poses = loadOptimizedPosesQuery(lastlocalizationPose);
+	_dbSafeAccessMutex.unlock();
+	return poses;
+}
+
+void DBDriver::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
+{
+	_dbSafeAccessMutex.lock();
+	save2DMapQuery(map, xMin, yMin, cellSize);
+	_dbSafeAccessMutex.unlock();
+}
+
+cv::Mat DBDriver::load2DMap(float & xMin, float & yMin, float & cellSize) const
+{
+	_dbSafeAccessMutex.lock();
+	cv::Mat map = load2DMapQuery(xMin, yMin, cellSize);
+	_dbSafeAccessMutex.unlock();
+	return map;
+}
+
 void DBDriver::saveOptimizedMesh(
 			const cv::Mat & cloud,
-			const std::map<int, Transform> & poses,
 			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
@@ -1073,12 +1101,11 @@ void DBDriver::saveOptimizedMesh(
 			const cv::Mat & textures) const
 {
 	_dbSafeAccessMutex.lock();
-	saveOptimizedMeshQuery(cloud, poses, polygons, texCoords, textures);
+	saveOptimizedMeshQuery(cloud, polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 }
 
 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,
@@ -1088,7 +1115,7 @@ cv::Mat DBDriver::loadOptimizedMesh(
 				cv::Mat * textures) const
 {
 	_dbSafeAccessMutex.lock();
-	cv::Mat cloud = loadOptimizedMeshQuery(poses, polygons, texCoords, textures);
+	cv::Mat cloud = loadOptimizedMeshQuery(polygons, texCoords, textures);
 	_dbSafeAccessMutex.unlock();
 	return cloud;
 }