merged master to multicamera branch

corelib/src/DBReader.cpp

@@ -185,8 +185,6 @@ void DBReader::mainLoop()
 
 		if(goalId > 0)
 		{
-			this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
-
 			if(!_ignoreGoalDelay && _currentId != _ids.end())
 			{
 				// get stamp for the next signature to compute the delay
@@ -203,12 +201,19 @@ void DBReader::mainLoop()
 					double delay = stamp - previousStamp;
 					UWARN("Goal %d detected, posting it! Waiting %f seconds before sending next data...",
 							goalId, delay);
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
 					uSleep(delay*1000);
 				}
+				else
+				{
+					UWARN("Goal %d detected, posting it!", goalId);
+					this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
+				}
 			}
 			else
 			{
 				UWARN("Goal %d detected, posting it!", goalId);
+				this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
 			}
 		}
 

corelib/src/Rtabmap.cpp

@@ -2562,17 +2562,16 @@ void Rtabmap::optimizeCurrentMap(
 {
 	//Optimize the map
 	optimizedPoses.clear();
-	UDEBUG("Optimize map: around location %d", id);
+	UINFO("Optimize map: around location %d", id);
 	if(_memory && id > 0)
 	{
 		UTimer timer;
 		std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
-		UDEBUG("get ids=%d", (int)ids.size());
 		if(!_optimizeFromGraphEnd && ids.size() > 1)
 		{
 			id = ids.begin()->first;
 		}
-		UINFO("get ids time %f s", timer.ticks());
+		UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
 
 		optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
 
@@ -2597,7 +2596,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	std::multimap<int, Link> edgeConstraints;
 	UDEBUG("ids=%d", (int)ids.size());
 	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
-	UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
+	UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
 
 	if(constraints)
 	{
@@ -2614,6 +2613,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	{
 		optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
 	}
+	UINFO("Optimization time %f s", timer.ticks());
 
 	return optimizedPoses;
 }
@@ -2801,8 +2801,8 @@ void Rtabmap::getGraph(
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & constraints,
 		bool optimized,
-		bool global,
-		std::map<int, Signature> * signatures)
+		bool global,
+		std::map<int, Signature> * signatures)
 {
 	if(_memory && _memory->getLastWorkingSignature())
 	{
@@ -2824,8 +2824,8 @@ void Rtabmap::getGraph(
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
 			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
-
-		if(signatures)
+
+		if(signatures)
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
@@ -2834,8 +2834,8 @@ void Rtabmap::getGraph(
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
-				std::vector<unsigned char> userData;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
+				std::vector<unsigned char> userData;
+				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
 				signatures->insert(std::make_pair(iter->first,
 						Signature(iter->first,
 							mapId,
@@ -2846,7 +2846,7 @@ void Rtabmap::getGraph(
 							std::multimap<int, pcl::PointXYZ>(),
 							odomPose,
 							userData,
-							SensorData())));
+							SensorData())));
 			}
 		}
 	}
@@ -2986,6 +2986,7 @@ bool Rtabmap::computePath(
 // return true if path is updated
 bool Rtabmap::computePath(int targetNode, bool global)
 {
+	UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
 	this->clearPath();
 
 	if(!_rgbdSlamMode)
@@ -2994,23 +2995,27 @@ bool Rtabmap::computePath(int targetNode, bool global)
 		return false;
 	}
 
+	UTimer totalTimer;
 	UTimer timer;
 	std::map<int, Transform> nodes;
-	std::multimap<int, Link> constraints;
-	this->getGraph(nodes, constraints, true, global);
+	std::multimap<int, Link> constraints;
+	this->getGraph(nodes, constraints, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	if(computePath(targetNode, nodes, constraints))
 	{
 		updateGoalIndex();
 	}
-	UINFO("Time computing path = %fs", timer.ticks());
+	UINFO("Time computing path (A*) = %fs", timer.ticks());
+	UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
 
 	return _path.size()>0;
 }
 
 bool Rtabmap::computePath(const Transform & targetPose, bool global)
 {
+	UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0);
+
 	this->clearPath();
 	std::list<std::pair<int, Transform> > pathPoses;
 
@@ -3027,8 +3032,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
 	std::map<int, int> mapIds;
 	std::map<int, double> stamps;
 	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
-	this->getGraph(nodes, constraints, true, global);
+	std::map<int, std::vector<unsigned char> > userDatas;
+	this->getGraph(nodes, constraints, true, global);
 	UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
 
 	int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);

corelib/src/RtabmapThread.cpp

@@ -134,7 +134,7 @@ void RtabmapThread::publishMap(bool optimized, bool full) const
 			constraints));
 }
 
-void RtabmapThread::publishTOROGraph(bool optimized, bool full) const
+void RtabmapThread::publishGraph(bool optimized, bool full) const
 {
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
@@ -244,10 +244,10 @@ void RtabmapThread::mainLoop()
 		this->publishMap(atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
 	case kStatePublishingTOROGraphLocal:
-		this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, false);
+		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, false);
 		break;
 	case kStatePublishingTOROGraphGlobal:
-		this->publishTOROGraph(atoi(parameters.at("optimized").c_str())!=0, true);
+		this->publishGraph(atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
 	case kStateTriggeringMap:
 		_rtabmap->triggerNewMap();

corelib/src/util2d.cpp

@@ -228,7 +228,7 @@ float getDepth(
 	int u_end = std::min(u+1, depthImage.cols-1);
 	int v_end = std::min(v+1, depthImage.rows-1);
 
-	float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
+	float depth = isInMM?(float)depthImage.at<unsigned short>(v,u)*0.001f:depthImage.at<float>(v,u);
 	if(depth!=0.0f && uIsFinite(depth))
 	{
 		if(smoothing)
@@ -241,7 +241,7 @@ float getDepth(
 				{
 					if(!(uu == u && vv == v))
 					{
-						float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
+						float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
 						// ignore if not valid or depth difference is too high
 						if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
 						{

corelib/src/util3d_filtering.cpp

@@ -52,10 +52,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(voxelSize > 0.0f);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::VoxelGrid<pcl::PointXYZ> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
@@ -66,10 +64,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float voxelSize)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(voxelSize > 0.0f);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::VoxelGrid<pcl::PointXYZRGB> filter;
 	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
 	filter.setInputCloud(cloud);
@@ -81,10 +77,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(samples > 0);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::RandomSample<pcl::PointXYZ> filter;
 	filter.setSample(samples);
 	filter.setInputCloud(cloud);
@@ -94,10 +88,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(samples > 0);
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::RandomSample<pcl::PointXYZRGB> filter;
 	filter.setSample(samples);
 	filter.setInputCloud(cloud);
@@ -112,12 +104,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
 		float min,
 		float max)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(max > min);
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::PassThrough<pcl::PointXYZ> filter;
 	filter.setFilterFieldName(axis);
 	filter.setFilterLimits(min, max);
@@ -132,12 +122,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
 		float min,
 		float max)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef typename PointCloud::Ptr PointCloudPtr;
 	UASSERT(max > min);
 	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
 
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::PassThrough<pcl::PointXYZRGB> filter;
 	filter.setFilterFieldName(axis);
 	filter.setFilterLimits(min, max);
@@ -208,9 +196,7 @@ pcl::IndicesPtr radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius)
 {
-	typedef pcl::search::KdTree<pcl::PointXYZ> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-	KdTreePtr tree (new KdTree(false));
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>(false));
 
 	if(indices->size())
 	{
@@ -255,9 +241,7 @@ pcl::IndicesPtr radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius)
 {
-	typedef pcl::search::KdTree<pcl::PointXYZRGB> KdTree;
-	typedef typename KdTree::Ptr KdTreePtr;
-	KdTreePtr tree (new KdTree(false));
+	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>(false));
 
 	if(indices->size())
 	{
@@ -583,7 +567,7 @@ pcl::IndicesPtr extractNegativeIndices(
 	return output;
 }
 pcl::IndicesPtr extractNegativeIndices(
-		const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices)
 {
 	pcl::IndicesPtr output(new std::vector<int>);

corelib/src/util3d_transforms.cpp

@@ -36,22 +36,18 @@ namespace util3d
 {
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
-		const typename pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & transform)
 {
-	typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
 	return output;
 }
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
-		const typename pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & transform)
 {
-	typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	PointCloudPtr output(new PointCloud);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
 	return output;
 }