C++ (Cpp) DataPoints::save Examples

Example #1

File: map_maintainer.cpp Project: BlazingForests/ethzasl_icp_mapping

void MapMaintener::saveMapCallback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
	// save point cloud
	if (mapPointCloud.features.cols())
	{
		mapPointCloud.save(vtkFinalMapName);
	}
}

Example #2

File: mapper.cpp Project: anuraj-rp/ethzasl_icp_mapping

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

Example #3

File: dynamic_mapper.cpp Project: Jinqiang/ethzasl_icp_mapping

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

Example #4

File: matcher_service.cpp Project: MobileRobotics-Ulaval/pointmatcher-ros

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