void MapMaintener::saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	// save point cloud
	if (mapPointCloud.features.cols())
	{
		mapPointCloud.save(vtkFinalMapName);
	}
}
Example #2
0
bool Mapper::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
{
	if (!mapPointCloud)
		return false;
	
	try
	{
		mapPointCloud->save(req.filename.data);
	}
	catch (const std::runtime_error& e)
	{
		ROS_ERROR_STREAM("Unable to save: " << e.what());
		return false;
	}
	
	ROS_INFO_STREAM("Map saved at " <<  req.filename.data << "with " << mapPointCloud->features.cols() << " points.");
	return true;
}
bool Mapper::saveMap(map_msgs::SaveMap::Request &req, map_msgs::SaveMap::Response &res)
{
	if (!mapPointCloud)
		return false;

  int lastindex = req.filename.data.find_last_of("."); 
  string rawname = req.filename.data.substr(0, lastindex);
	
  try
	{
    savePathToCsv(rawname+"_path.csv");
		mapPointCloud->save(req.filename.data);
	}
	catch (const std::runtime_error& e)
	{
		ROS_ERROR_STREAM("Unable to save: " << e.what());
		return false;
	}
	
	ROS_INFO_STREAM("Map saved at " <<  req.filename.data << " with " << mapPointCloud->features.cols() << " points.");
	return true;
}
Mapper::DP* Mapper::updateMap(DP* newMap, const PM::TransformationParameters Ticp, bool updateExisting)
{
	timer t;
 
  // Prepare empty field if not existing
  // FIXME: this is only needed for the none overlaping part
	if(newMap->descriptorExists("probabilityStatic") == false)
	{
		//newMap->addDescriptor("probabilityStatic", PM::Matrix::Zero(1, newMap->features.cols()));
		newMap->addDescriptor("probabilityStatic", PM::Matrix::Constant(1, newMap->features.cols(), priorStatic));
	}
	
	if(newMap->descriptorExists("probabilityDynamic") == false)
	{
		//newMap->addDescriptor("probabilityDynamic", PM::Matrix::Zero(1, newMap->features.cols()));
		newMap->addDescriptor("probabilityDynamic", PM::Matrix::Constant(1, newMap->features.cols(), priorDyn));
	}
	
	if(newMap->descriptorExists("debug") == false)
	{
		newMap->addDescriptor("debug", PM::Matrix::Zero(1, newMap->features.cols()));
	}

	if (!updateExisting)
	{
		// FIXME: correct that, ugly
		cout << "Jumping map creation" << endl;
		*newMap = transformation->compute(*newMap, Ticp); 
		mapPostFilters.apply(*newMap);
		return newMap;
	}

	 // FIXME: only for debug
	mapPointCloud->getDescriptorViewByName("debug") = PM::Matrix::Zero(1, mapPointCloud->features.cols());


	
	const int newMapPts(newMap->features.cols());
	const int mapPtsCount(mapPointCloud->features.cols());
	
	// Build a range image of the reading point cloud (local coordinates)
	PM::Matrix radius_newMap = newMap->features.topRows(3).colwise().norm();

	PM::Matrix angles_newMap(2, newMapPts); // 0=inclination, 1=azimuth

	// No atan in Eigen, so we are for to loop through it...
	for(int i=0; i<newMapPts; i++)
	{
		const float ratio = newMap->features(2,i)/radius_newMap(0,i);

		angles_newMap(0,i) = acos(ratio);
		angles_newMap(1,i) = atan2(newMap->features(1,i), newMap->features(0,i));
	}

	std::shared_ptr<NNS> kdtree;
	kdtree.reset( NNS::create(angles_newMap));

  //-------------- Global map ------------------------------
	// Transform the global map in local coordinates
	DP mapLocal = transformation->compute(*mapPointCloud, Ticp.inverse());

  // ROI: Region of Interest
  // We reduce the global map to the minimum for the processing

	//const float sensorMaxRange = 80.0; // ICRA
	
  PM::Matrix globalId(1, mapPtsCount); 

	int ROIpts = 0;
	int notROIpts = 0;

  // Split mapLocal
	DP mapLocalROI(mapLocal.createSimilarEmpty());
	for (int i = 0; i < mapPtsCount; i++)
	{
    // Copy the points of the ROI in a new map
		if (mapLocal.features.col(i).head(3).norm() < sensorMaxRange)
		{
			mapLocalROI.setColFrom(ROIpts, mapLocal, i);
			globalId(0,ROIpts) = i;
			ROIpts++;
		}
    else // Remove the points of the ROI from the global map
    {
			mapLocal.setColFrom(notROIpts, mapLocal, i);
			notROIpts++;
    }
	}

	mapLocalROI.conservativeResize(ROIpts);
	mapLocal.conservativeResize(notROIpts);
	

  // Convert the map in spherical coordinates
	PM::Matrix radius_map = mapLocalROI.features.topRows(3).colwise().norm();
	PM::Matrix angles_map(2, ROIpts); // 0=inclination, 1=azimuth

	// No atan in Eigen, so we are looping through it...
  // TODO: check for: A.binaryExpr(B, std::ptr_fun(atan2))
	for(int i=0; i < ROIpts; i++)
	{
		const float ratio = mapLocalROI.features(2,i)/radius_map(0,i);
		//if(ratio < -1 || ratio > 1)
			//cout << "Error angle!" << endl;

		angles_map(0,i) = acos(ratio);

		angles_map(1,i) = atan2(mapLocalROI.features(1,i), mapLocalROI.features(0,i));
	}

  // Prepare access to descriptors
	DP::View viewOn_Msec_overlap = newMap->getDescriptorViewByName("stamps_Msec");
	DP::View viewOn_sec_overlap = newMap->getDescriptorViewByName("stamps_sec");
	DP::View viewOn_nsec_overlap = newMap->getDescriptorViewByName("stamps_nsec");

	DP::View viewOnProbabilityStatic = mapLocalROI.getDescriptorViewByName("probabilityStatic");
	DP::View viewOnProbabilityDynamic = mapLocalROI.getDescriptorViewByName("probabilityDynamic");
	DP::View viewDebug = mapLocalROI.getDescriptorViewByName("debug");
	
	DP::View viewOn_normals_map = mapLocalROI.getDescriptorViewByName("normals");
	DP::View viewOn_Msec_map = mapLocalROI.getDescriptorViewByName("stamps_Msec");
	DP::View viewOn_sec_map = mapLocalROI.getDescriptorViewByName("stamps_sec");
	DP::View viewOn_nsec_map = mapLocalROI.getDescriptorViewByName("stamps_nsec");
	
	// Search for the nearest point in newMap
	Matches::Dists dists(1, ROIpts);
	Matches::Ids ids(1, ROIpts);
	
	kdtree->knn(angles_map, ids, dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxAngle);


  // update probability of being dynamic for all points in ROI
	for(int i=0; i < ROIpts; i++)
	{
    const int mapId = i;
    viewDebug(0,mapId) = 1; //FIXME: debug

		if(dists(i) != numeric_limits<float>::infinity())
		{
			const int readId = ids(0,i);
			//const int mapId = globalId(0,i);
						
			// in local coordinates
			const Eigen::Vector3f readPt = newMap->features.col(readId).head(3);
			const Eigen::Vector3f mapPt = mapLocalROI.features.col(i).head(3);
			const Eigen::Vector3f mapPt_n = mapPt.normalized();
			const float delta = (readPt - mapPt).norm();
			const float d_max = eps_a * readPt.norm();

      const Eigen::Vector3f normal_map = viewOn_normals_map.col(mapId);
			
      // Weight for dynamic elements
			const float w_v = eps + (1 - eps)*fabs(normal_map.dot(mapPt_n));
			const float w_d1 =  eps + (1 - eps)*(1 - sqrt(dists(i))/maxAngle);
			
			const float offset = delta - eps_d;
			float w_d2 = 1;
			if(delta < eps_d || mapPt.norm() > readPt.norm())
			{
				w_d2 = eps;
			}
			else 
			{
				if (offset < d_max)
				{
					w_d2 = eps + (1 - eps )*offset/d_max;
				}
			}


			float w_p2 = eps;
			if(delta < eps_d)
			{
				w_p2 = 1;
			}
			else
			{
				if(offset < d_max)
				{
					w_p2 = eps + (1 - eps)*(1 - offset/d_max);
				}
			}


			// We don't update point behind the reading
			if((readPt.norm() + eps_d + d_max) >= mapPt.norm())
			{
				const float lastDyn = viewOnProbabilityDynamic(0,mapId);
				const float lastStatic = viewOnProbabilityStatic(0, mapId);

				const float c1 = (1 - (w_v*(1 - w_d1)));
				const float c2 = w_v*(1 - w_d1);
			
        //Lock dynamic point to stay dynamic under a threshold
				if(lastDyn < maxDyn)
				{
					viewOnProbabilityDynamic(0,mapId) = c1*lastDyn + c2*w_d2*((1 - alpha)*lastStatic + beta*lastDyn);
					viewOnProbabilityStatic(0, mapId) = c1*lastStatic + c2*w_p2*(alpha*lastStatic + (1 - beta)*lastDyn);
				}
				else
				{
					viewOnProbabilityStatic(0,mapId) = eps;
					viewOnProbabilityDynamic(0,mapId) = 1-eps;
				}
				
				// normalization
				const float sumZ = viewOnProbabilityDynamic(0,mapId) + viewOnProbabilityStatic(0, mapId);
				assert(sumZ >= eps);	
				
				viewOnProbabilityDynamic(0,mapId) /= sumZ;
				viewOnProbabilityStatic(0,mapId) /= sumZ;
				
				//viewDebug(0,mapId) =viewOnProbabilityDynamic(0, mapId);
				//viewDebug(0,mapId) = w_d2;

				// Refresh time
				viewOn_Msec_map(0,mapId) = viewOn_Msec_overlap(0,readId);	
				viewOn_sec_map(0,mapId) = viewOn_sec_overlap(0,readId);	
				viewOn_nsec_map(0,mapId) = viewOn_nsec_overlap(0,readId);	
			}
		}
	}

	// Compute density
  const int mapLocalROIPts = mapLocalROI.features.cols();
	cout << "build first kdtree with " << mapLocalROIPts << endl;
	// build and populate NNS
	std::shared_ptr<NNS> kdtree2;
	kdtree2.reset( NNS::create(mapLocalROI.features, mapLocalROI.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
	
	PM::Matches matches_overlap(
		Matches::Dists(1, newMapPts),
		Matches::Ids(1, newMapPts)
	);
	
	kdtree2->knn(newMap->features, matches_overlap.ids, matches_overlap.dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxDistNewPoint);
	
  //-------------- New point cloud ------------------------------

	DP newMapOverlap(newMap->createSimilarEmpty());// Not used for now
  PM::Matrix minDist = PM::Matrix::Constant(1,mapLocalROIPts, std::numeric_limits<float>::max());
	
  // Reduce newMap to its none overlapping part
	int ptsOut = 0;
	int ptsIn = 0;

  bool hasSensorNoise = mapLocalROI.descriptorExists("simpleSensorNoise") && newMap->descriptorExists("simpleSensorNoise");
  if(hasSensorNoise) // Split and update point with lower noise
  {
    DP::View viewOn_noise_mapLocal = mapLocalROI.getDescriptorViewByName("simpleSensorNoise");
    DP::View viewOn_noise_newMap = newMap->getDescriptorViewByName("simpleSensorNoise");

    for (int i = 0; i < newMapPts; ++i)
    {
      const int localMapId = matches_overlap.ids(i);

      if (matches_overlap.dists(i) == numeric_limits<float>::infinity())
      {
        newMap->setColFrom(ptsOut, *newMap, i);
        ptsOut++;
      }
      else // Overlapping points
      {
        // Update point with lower sensor noise
        if(viewOn_noise_newMap(0,i) < viewOn_noise_mapLocal(0,localMapId))
        {
          if(matches_overlap.dists(i) < minDist(localMapId))
          {
            minDist(localMapId) = matches_overlap.dists(i);
            const float debug = viewDebug(0, localMapId);
            const float probDyn = viewOnProbabilityDynamic(0, localMapId);
            const float probStatic = viewOnProbabilityStatic(0, localMapId);

            mapLocalROI.setColFrom(localMapId, *newMap, i);
            viewDebug(0, localMapId) = 2;  
            viewOnProbabilityDynamic(0, localMapId) = probDyn;
            viewOnProbabilityStatic(0, localMapId) = probStatic;
          }
        }

        newMapOverlap.setColFrom(ptsIn, *newMap, i);
        ptsIn++;
      }
    }
  }
  else // Only split newMap
  {
    for (int i = 0; i < newMapPts; ++i)
    {
      if (matches_overlap.dists(i) == numeric_limits<float>::infinity())
      {
        newMap->setColFrom(ptsOut, *newMap, i);
        ptsOut++;
      }
      else // Overlapping points
      {
        newMapOverlap.setColFrom(ptsIn, *newMap, i);
        ptsIn++;
      }
    }
  }

	// shrink the newMap to the new information
	newMap->conservativeResize(ptsOut);
	newMapOverlap.conservativeResize(ptsIn);

	cout << "ptsOut=" << ptsOut << ", ptsIn=" << ptsIn << endl;

	// Publish debug
	//if (debugPub.getNumSubscribers())
	//{
  //    debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*newMap, mapFrame, mapCreationTime));
	//}

  //no_overlap.addDescriptor("debug", PM::Matrix::Zero(1, no_overlap.features.cols()));
	


	// Add the ROI to the none overlapping part
	newMap->concatenate(mapLocalROI);
  
  // Apply the filters only on the ROI
  mapPostFilters.apply(*newMap);

  // Add the rest of the map
	newMap->concatenate(mapLocal);
	
  // Transform the map back to global coordinates
	*newMap = transformation->compute(*newMap, Ticp);

	cout << "... end map creation" << endl;
	ROS_INFO_STREAM("[TIME] New map available (" << newMap->features.cols() << " pts), update took " << t.elapsed() << " [s]");
	
	return newMap;
}
// Point cloud processing
void MapMaintener::processCloud(DP newPointCloud, const TP TScannerToMap)
{
	string reason;
	timer t;
	
	// Convert point cloud
	const size_t goodCount(newPointCloud.features.cols());
	if (goodCount == 0)
	{
		ROS_ERROR("I found no good points in the cloud");
		return;
	}
	
	ROS_INFO("Processing new point cloud");
	{
		timer t; // Print how long take the algo
		
		// Apply filters to incoming cloud, in scanner coordinates
		inputFilters.apply(newPointCloud);
		
		ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
	}
	
	
	// Correct new points using ICP result and move them in their own frame
	//cout << "TObjectToMap: " << endl << TObjectToMap << endl;
	//cout << "TScannerToMap: " << endl << TScannerToMap << endl;
	//cout << "concat: " << endl << TObjectToMap.inverse() * TScannerToMap << endl;
	newPointCloud = transformation->compute(newPointCloud, transformation->correctParameters(TObjectToMap.inverse() * TScannerToMap)); 
	
	// Preparation of cloud for inclusion in map
	mapPreFilters.apply(newPointCloud);
	
	// FIXME put that as parameter
	if(newPointCloud.features.cols() < 400)
		return;

	// Generate first map
	if(!mapPointCloud.features.rows())	
	{
		mapPointCloud = newPointCloud;
		return;
	}
	

	// Check dimension
	if (newPointCloud.features.rows() != mapPointCloud.features.rows())
	{
		ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud.features.rows()-1 << " while map is " << mapPointCloud.features.rows()-1);
		return;
	}


	PM::TransformationParameters Tcorr;
	try
	{
		Tcorr = icp(newPointCloud, mapPointCloud);
		TObjectToMap = TObjectToMap * Tcorr.inverse();
	}
	catch (PM::ConvergenceError error)
	{
		ROS_WARN_STREAM("ICP failed to converge: " << error.what());
		return;
	}


	const double estimatedOverlap = icp.errorMinimizer->getOverlap();
	if(estimatedOverlap < 0.40)
	{
		ROS_WARN_STREAM("Estimated overlap too small: " << estimatedOverlap);
		return;
	}

	ROS_DEBUG_STREAM("Tcorr:\n" << Tcorr);

	cout << "Tcorr: " << endl << Tcorr << endl;
	newPointCloud = transformation->compute(newPointCloud, Tcorr); 
	// Merge point clouds to map
	mapPointCloud.concatenate(newPointCloud);

	// Map maintenance
	mapPostFilters.apply(mapPointCloud);
	
	ROS_INFO_STREAM("New map available (" << mapPointCloud.features.cols() << " pts), update took " << t.elapsed() << " [s]");

	// Publish map point cloud
	// FIXME this crash when used without descriptor
	if (mapPub.getNumSubscribers())
		mapPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(mapPointCloud, objectFrame, ros::Time::now()));

	ROS_INFO_STREAM("Total map merging: " << t.elapsed() << " [s]");

	//ros::Rate r(2);
	//r.sleep();
}
bool CloudMatcher::match(pointmatcher_ros::MatchClouds::Request& req, pointmatcher_ros::MatchClouds::Response& res)
{
	// get and check reference
	const DP referenceCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.reference));
	const unsigned referenceGoodCount(referenceCloud.features.cols());
	const unsigned referencePointCount(req.reference.width * req.reference.height);
	const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
	
	if (referenceGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reference cloud");
		return false;
	}
	if (referenceGoodRatio < 0.5)
	{
        ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 <<
                        "% of the cloud (received " << referenceGoodCount << ")");
	}
	
	// get and check reading
	const DP readingCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.readings));
	const unsigned readingGoodCount(referenceCloud.features.cols());
	const unsigned readingPointCount(req.readings.width * req.readings.height);
	const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
	
	if (readingGoodCount == 0)
	{
		ROS_ERROR("I found no good points in the reading cloud");
		return false;
	}
	if (readingGoodRatio < 0.5)
	{
		ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
	}
	
	// check dimensions
	if (referenceCloud.features.rows() != readingCloud.features.rows())
	{
        ROS_ERROR_STREAM("Dimensionality missmatch: reference cloud is " <<
                         referenceCloud.features.rows() - 1 << " while reading cloud is " <<
                         readingCloud.features.rows() - 1);
		return false;
	}
	
	// call ICP
    PM::TransformationParameters transform;
	try 
	{
        transform = icp(readingCloud, referenceCloud);
		tf::transformTFToMsg(PointMatcher_ros::eigenMatrixToTransform<float>(transform), res.transform);
		ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return false;
	}

    if(traceMatching)
    {
        std::stringstream ss;
        ss << "reading_" << matchedCloudsCount << ".vtk";
        readingCloud.save(ss.str());

        ss.str(std::string());
        ss << "reference_" << matchedCloudsCount << ".vtk";
        referenceCloud.save(ss.str());

        PM::Transformation* rigidTrans;
        rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");
        PM::DataPoints matchedCloud = rigidTrans->compute(readingCloud, transform);

        ss.str(std::string());
        ss << "matched_" << matchedCloudsCount << ".vtk";
        matchedCloud.save(ss.str());
    }

    matchedCloudsCount++;
	
	return true;
}
Example #7
0
void Mapper::gotCloud(const sensor_msgs::PointCloud2& cloudMsgIn)
{
    PM::Parameters params;
    PM::TransformationParameters T = PM::TransformationParameters::Identity(4,4);

    sensor_msgs::PointCloud2 cloudMsg;

    pcl_ros::transformPointCloud(mapFrame, cloudMsgIn, cloudMsg, tf_listener);

    size_t goodCount(0);
    DP newPointCloud(rosMsgToPointMatcherCloud(cloudMsg, goodCount));

    if (goodCount == 0)
    {
        ROS_ERROR("I found no good points in the cloud");
        return;
    }
    else
    {
        ROS_INFO("Processing new point cloud");
    }

    bool icpWasSuccess = true;

    newPointCloud = filterScannerPtsCloud(newPointCloud);

    // Ensure a minimum amount of point after filtering
    const int ptsCount = newPointCloud.features.cols();
    if(ptsCount < minReadingPointCount)
    {
        ROS_ERROR_STREAM("Not enough points in newPointCloud: " << ptsCount << "pts.");
        return;
    }

    // Keep the map in memory
    if(mapPointCloud.features.rows() == 0)
    {
        // Initialize the map
        mapPointCloud = newPointCloud;
        return;
    }

    // Apply ICP
    try
    {
        T = icp(newPointCloud, mapPointCloud);
        mapPointCloud.features = T.inverse() * mapPointCloud.features;

        // Ensure minimum overlap between scans
        const double estimatedOverlap = icp.errorMinimizer->getWeightedPointUsedRatio();
        if (estimatedOverlap < minOverlap)
        {
            ROS_ERROR("Estimated overlap too small, move back");
            return;
        }

        ROS_INFO_STREAM("**** Adding new points to the map with " << estimatedOverlap << " overlap");

        // Create point cloud filters
        PM::DataPointsFilter* uniformSubsample;
        params = PM::Parameters({{"aggressivity", toParam(0.5)}});
        uniformSubsample = pm.DataPointsFilterRegistrar.create("UniformizeDensityDataPointsFilter", params);

        // Merge point clouds to map
        mapPointCloud.concatenate(newPointCloud);
        mapPointCloud = uniformSubsample->filter(mapPointCloud);

        // Controle the size of the point cloud
        const double probToKeep = maxMapPointCount/(double)mapPointCloud.features.cols();
        if(probToKeep < 1.0)
        {
            PM::DataPointsFilter* randSubsample;
            params = PM::Parameters({{"prob", toParam(probToKeep)}});
            randSubsample = pm.DataPointsFilterRegistrar.create("RandomSamplingDataPointsFilter", params);
            mapPointCloud = randSubsample->filter(mapPointCloud);
        }

        //Publish map point cloud
        stringstream nameStream;
        nameStream << "nifti_map_" << cloudMsg.header.seq;
        PM::saveVTK(mapPointCloud, nameStream.str());
        mapPub.publish(pointMatcherCloudToRosMsg(mapPointCloud, mapFrame));

    }
    catch (PM::ConvergenceError error)
    {
        icpWasSuccess = false;
        ROS_WARN_STREAM("ICP failed to converge: " << error.what());
    }


}