C++ (Cpp) DataPoints::concatenate Examples

Example #1

File: map_maintainer.cpp Project: BlazingForests/ethzasl_icp_mapping

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

Example #2

File: mapper.cpp Project: osiloke/ros-mapping

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


}