utilities/src/PCUtils.cpp
// Fill towards centroid
// Density -> extra points per current point. recommended = 2
Fill towards centroid
Density -> extra points per current point. recommended = 2
Fill towards centroid Density - > extra points per current point . recommended = 2
void PCUtils::fillCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out, int density)
{
    bool block = true;
    pcl::PointXYZ centroid;
    pcl::computeCentroid(*cloud_in, centroid);
    pcl::copyPointCloud(*cloud_in, *cloud_out);
    std::cout << "out cloud size : " << cloud_out->size() << std::endl;
    for (int i = 0; i < cloud_in->size(); ++i)
    {
        Eigen::Vector3f current_point = cloud_in->points[i].getVector3fMap();
        Eigen::Vector3f direction = centroid.getVector3fMap() - current_point; 
        Eigen::Vector3f increment = direction / density;
        Eigen::Vector3f new_point = current_point;
        for (int j = 0; j < density - 1; ++j)
        {
            new_point += increment;
            cloud_out->push_back(pcl::PointXYZ(new_point[0], new_point[1], new_point[2])); 
        }
    }
}
void PCUtils :: fillCloud ( const pcl :: PointCloud < pcl :: PointXYZ > :: Ptr & cloud_in , pcl :: PointCloud < pcl :: PointXYZ > :: Ptr & cloud_out , int density ) { bool block = true ; pcl :: PointXYZ centroid ; pcl :: computeCentroid ( * cloud_in , centroid ) ; pcl :: copyPointCloud ( * cloud_in , * cloud_out ) ; std :: cout << " " << cloud_out -> size ( ) << std :: endl ; for ( int i = 0 ; i < cloud_in -> size ( ) ; ++ i ) { Eigen :: Vector3f current_point = cloud_in -> points [ i ] . getVector3fMap ( ) ; Eigen :: Vector3f direction = centroid . getVector3fMap ( ) - current_point ; Eigen :: Vector3f increment = direction / density ; Eigen :: Vector3f new_point = current_point ; for ( int j = 0 ; j < density - 1 ; ++ j ) { new_point += increment ; cloud_out -> push_back ( pcl :: PointXYZ ( new_point [ 0 ] , new_point [ 1 ] , new_point [ 2 ] ) ) ; } } }
Fill towards centroid
Density -> extra points per current point.
Fill towards centroid Density - > extra points per current point .
// std::cout << "####### ITERATION: " << i << " ##############" << std::endl; // std::cout << "current_point: " << cloud_in->points[i] << std::endl; // std::cout << "cenroid: " << centroid << std::endl; // vector // std::cout << "direction: " << direction << std::endl; //point insertion // std::cout << "increment: " << increment << std::endl; // std::cout << "new_point: " << new_point << std::endl; //Preallocate ! // std::cout << "checking .. " << cloud_out->points[cloud_out->size()-1] << std::endl; // if(block){ // char c = std::cin.get(); // if(c == 'c') // block = false; // }