pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
  if(keyframes.empty()) {
    return nullptr;
  }

  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());

  for(const auto& keyframe : keyframes) {
    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
    for(const auto& src_pt : keyframe->cloud->points) {
      PointT dst_pt;
      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
      dst_pt.intensity = src_pt.intensity;
      cloud->push_back(dst_pt);
    }
  }

  cloud->width = cloud->size();
  cloud->height = 1;
  cloud->is_dense = false;

  pcl::octree::OctreePointCloud<PointT> octree(resolution);
  octree.setInputCloud(cloud);
  octree.addPointsFromInputCloud();

  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
  octree.getOccupiedVoxelCenters(filtered->points);

  filtered->width = filtered->size();
  filtered->height = 1;
  filtered->is_dense = false;

  return filtered;
}
Пример #2
0
std::vector<float>
WorkerStemFit::compute_distances(std::vector<pcl::ModelCoefficients>  &cylinders)
{

    std::vector<float> distances ( _cloud->points.size () );
    std::fill ( distances.begin (), distances.end (), 0.5 );

    pcl::octree::OctreePointCloudSearch<PointI> octree ( 0.02f );
    octree.setInputCloud ( _cloud );
    octree.addPointsFromInputCloud ();
    for (size_t i = 0; i < cylinders.size(); i++) {

        pcl::ModelCoefficients cylinder = cylinders.at(i);
        std::vector<int> indices = indexOfPointsNearCylinder ( octree, cylinder );
        for ( size_t i = 0; i < indices.size (); i++ ) {
            PointI point = _cloud->points[indices[i]];
            simpleTree::Cylinder cyl (cylinder);

            float dist = cyl.distToPoint ( point );
            if ( std::abs ( dist ) < std::abs ( distances[indices[i]] ) ) {
                distances[indices[i]] = dist;
            }
        }
    }
    return distances;
}
Пример #3
0
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
    pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
    octree.setInputCloud (input_);
    octree.addPointsFromInputCloud ();

    typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
    octree.getOccupiedVoxelCenters (occupied_cells);

    // Determine the voxels crosses along the line segments
    // formed by every pair of occupied cells.
    std::vector< std::vector<int> > line_histograms;
    for (size_t i = 0; i < occupied_cells.size (); ++i)
    {
        Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();

        for (size_t j = i+1; j < occupied_cells.size (); ++j)
        {
            typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
            Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
            octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);

            // Intersected cells are ordered from closest to furthest w.r.t. the origin.
            std::vector<int> histogram;
            for (size_t k = 0; k < intersected_cells.size (); ++k)
            {
                std::vector<int> indices;
                octree.voxelSearch (intersected_cells[k], indices);
                int label = emptyLabel ();
                if (indices.size () != 0)
                {
                    label = getDominantLabel (indices);
                }
                histogram.push_back (label);
            }

            line_histograms.push_back(histogram);
        }
    }

    std::vector< std::vector<int> > transition_histograms;
    computeTransitionHistograms (line_histograms, transition_histograms);

    std::vector<float> distances;
    computeDistancesToMean (transition_histograms, distances);

    std::vector<float> gfpfh_histogram;
    computeDistanceHistogram (distances, gfpfh_histogram);

    output.clear ();
    output.width = 1;
    output.height = 1;
    output.points.resize (1);
    std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
}
Пример #4
0
void OctreeTerrain:: execute()
{

//qWarning()<<"octree terrain starts";
emit percentage( 5);

    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp2(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp3(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_tmp4(new pcl::PointCloud<pcl::PointXYZI>);

      //velky cyklus
emit percentage( 10);
    octree(m_resolution*5, m_baseCloud->get_Cloud(), cloud_tmp, cloud_tmp2);
emit percentage( 70);
    octree(m_resolution/2, cloud_tmp2, cloud_tmp3, cloud_tmp4);
emit percentage( 80);
    *cloud_tmp += *cloud_tmp3;
    m_vegetation->set_Cloud(cloud_tmp);
    m_terrain->set_Cloud(cloud_tmp4);
emit percentage( 90);
      sendData();
}
Пример #5
0
void OctreeVisualizer::incomingOctreeCallback()
{
  scan_utils::Octree<char> octree(0, 0, 0, 0, 0, 0, 1, 0);
  octree.setFromMsg(octree_message_);

  std::list<scan_utils::Triangle> triangles;
  octree.getAllTriangles(triangles);

  V_Vector3 vertices;
  V_Vector3 normals;

  vertices.resize( triangles.size() * 3 );
  normals.resize( triangles.size() );
  size_t vertexIndex = 0;
  size_t normalIndex = 0;
  std::list<scan_utils::Triangle>::iterator it = triangles.begin();
  std::list<scan_utils::Triangle>::iterator end = triangles.end();
  for ( ; it != end; it++ )
  {
    Ogre::Vector3& v1 = vertices[vertexIndex++];
    Ogre::Vector3& v2 = vertices[vertexIndex++];
    Ogre::Vector3& v3 = vertices[vertexIndex++];
    Ogre::Vector3& n = normals[normalIndex++];

    v1 = Ogre::Vector3( it->p1.x, it->p1.y, it->p1.z );
    v2 = Ogre::Vector3( it->p2.x, it->p2.y, it->p2.z );
    v3 = Ogre::Vector3( it->p3.x, it->p3.y, it->p3.z );
    robotToOgre(v1);
    robotToOgre(v2);
    robotToOgre(v3);

    n = ( v2 - v1 ).crossProduct( v3 - v1 );
    n.normalise();
  }

  triangles_mutex_.lock();

  vertices_.clear();
  normals_.clear();

  vertices.swap( vertices_ );
  normals.swap( normals_ );

  new_message_ = true;
  triangles_mutex_.unlock();
}
Пример #6
0
//get intersection points
void get_intr_points(MyPointCloud& source_mpc, MyPointCloud& sample_mpc, float search_r, int* intr_points_num){
  *intr_points_num=0;

  PointCloudPtr source_cloud(new PointCloud);
  MyPointCloud2PointCloud(source_mpc, source_cloud);

  PointCloudPtr sample_cloud(new PointCloud);
  MyPointCloud2PointCloud(sample_mpc, sample_cloud);

  PointCloudPtr intr_cloud(new PointCloud);

  float resolution = 0.005f;

  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);

  octree.setInputCloud (source_cloud);
  octree.addPointsFromInputCloud ();

  for(int i=0;i<sample_mpc.mypoints.size();i++){
    // Neighbors within radius search
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    float radius = search_r;

    if (octree.radiusSearch(sample_cloud->at(i), radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
    {
      if(pointIdxRadiusSearch.size()>0){
        PointCloudPtr cloud_tem(new PointCloud);

        for (size_t j = 0; j < pointIdxRadiusSearch.size (); ++j){
          cloud_tem->push_back(source_cloud->points[ pointIdxRadiusSearch[j]]);
        }

        Point finded_pt;

        if(findNearestNeighbor(cloud_tem, intr_cloud, sample_cloud->at(i), finded_pt)){
          intr_cloud->push_back(finded_pt);
          (*intr_points_num)+=1;
        }
      }
    }
  }
}
Пример #7
0
int getDifferencesCloud(const PCPtr& src, 
						const PCPtr& tgt, 
						PCPtr& diff,
						float octreeRes)
{
	pcl::octree::OctreePointCloudChangeDetector<PCXYZ> octree (octreeRes);
	std::vector<int> newPointIdxVector;

	octree.setInputCloud(src);
	octree.addPointsFromInputCloud();
	octree.switchBuffers();
	octree.setInputCloud(tgt);
	octree.addPointsFromInputCloud();
	octree.getPointIndicesFromNewVoxels (newPointIdxVector);
	diff->points.reserve(newPointIdxVector.size());
	for (std::vector<int>::iterator it = newPointIdxVector.begin(); it != newPointIdxVector.end(); it++)
		diff->points.push_back(tgt->points[*it]);

	return newPointIdxVector.size();
}
Пример #8
0
void Viewer::SelfTest(int nx,int ny,int nz)
{
	Log::printf("Testing viewer...\n");
	Box3f world_box(Vec3f(0,0,0),Vec3f(nx,ny,nz));

	SmartPointer<Batch> cube=Graph::cuboid(3)->getBatch();
	std::vector<SmartPointer<Batch> > batches;

	for (int x=0;x<nx;x++)
	for (int y=0;y<ny;y++)
	for (int z=0;z<nz;z++)
	{
		SmartPointer<Batch> batch(new Batch(*cube));
		batch->matrix=Mat4f::translate(x,y,z) * Mat4f::scale(0.8f,0.8f,0.8f);
		batches.push_back(batch);
	}
	SmartPointer<Octree> octree(new Octree(batches));
	Viewer v(octree);
	v.Run();
	v.Wait();
}
Пример #9
0
int main(int argc, char** argv)
{
	// create a random point cloud
	srand((unsigned int) time(NULL));

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	// Generate pointcloud data
	cloud->width = 100;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);

	for(size_t i = 0; i < cloud->points.size(); ++i)
	{
		cloud->points[i].x = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024.0f * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024.0f * rand() / (RAND_MAX + 1.0f);
	}

	// octree
	const float resolution = 128.0f;
	//pcl::octree::OctreePointCloud<pcl::PointXYZ> octree(resolution);
	//octree.setInputCloud(cloud);
	//octree.addPointsFromInputCloud();
	GPMap::OctreePointCloudVoxelGaussian3D<pcl::PointXYZ> octree(resolution);
	octree.addPointsFromCloud(cloud);

	//pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> centroid(resolution);
	//centroid.setInputCloud(cloud);
	//centroid.addPointsFromInputCloud();
	//pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNodeIterator leafIter(centroid);
	//while(*++leafIter)
	//{
	//	dynamic_cast<OctreeContainerDataTVectorGaussian3D*>(*leafIter)->update(*input_);
	//}

  return 0;
}
void runTest(PointCloud& cloud, const std::string& name, std::default_random_engine& random)
{
    std::cout << "****************************************" << std::endl;
    std::cout << "Testing : " << name << std::endl;
    std::cout << "****************************************" << std::endl;
    Octree octree(cloud, 30);
    std::cout << "Octree loaded !" << std::endl;

    std::vector<SharedPlane> planes;
    auto begin = std::chrono::steady_clock::now();

    // (depthThreshold, epsilon, numStartPoints, numPoints, steps, countRatio, generateur, planes, colors, dCos)
    octree.detectPlanes(100, 0.05, 10, 30, 10, 0.005, random, planes, cloud.colors(), std::cos(3.1415/180 * /*Angle in degrees: */ 15));

    auto end = std::chrono::steady_clock::now();
    std::chrono::duration<double> elapsed_secs = end - begin;

    std::cout << std::endl << planes.size() << " planes :" << std::endl;
    std::sort(planes.begin(), planes.end(), [](const SharedPlane& a, const SharedPlane& b){return a->getCount() < b->getCount();});

    std::ofstream out((name + ".planes").c_str());
    for (unsigned int i = 0 ; i < planes.size() ; ++i)
    {
        std::cout << *planes[i] << std::endl;
        out << *planes[planes.size() - i - 1] << std::endl;
    }
    out.close();

    std::cout << std::endl << "Running time: " << elapsed_secs.count() << " seconds." << std::endl;

    std::cout << "Saving..." << std::endl;
    cloud.toPly(name + ".ply", true);
    std::cout << "****************************************" << std::endl;
    std::cout << "End of test : " << name << std::endl;
    std::cout << "****************************************" << std::endl;
}
Пример #11
0
std::vector<SmartPointer<Batch> > Batch::Optimize(std::vector<SmartPointer<Batch> > batches,int max_vertices_per_batch,int max_depth,float LOOSE_K) 
{
	std::vector<SmartPointer<Batch> > ret;

	Clock t1;
	//Log::printf("Optimizing the octree....\n");
	//Log::printf("   Number of input batches %d\n",(int)batches.size());

	//calculate world box
	Box3f world_box;
	for (int i=0;i<(int)batches.size();i++)
		world_box.add(batches[i]->getBox());

	//build the octree for doing optimization
	Octree octree(world_box,max_depth,LOOSE_K);
	for (int i=0;i<(int)batches.size();i++)
		octree.getNode(batches[i]->getBox())->batches.push_back(batches[i]);

	std::stack<OctreeNode*> stack;
	stack.push(octree.root);

	//count number of vertices
	int tot_number_of_vertices=0;
	while (stack.size())
	{
		OctreeNode* node=stack.top();
		stack.pop();

		for (int i=0;i<8;i++)
			{if (node->childs[i]) stack.push(node->childs[i]);}

		//rebuild the batches
		std::set<int> merged;

		int N=(int)node->batches.size();
		for (int I=0;I<N;I++)
		{
			if (merged.find(I)!=merged.end()) continue;

			SmartPointer<Batch> A=node->batches[I];
			if (A->vertices)
			{
				int nva=A->vertices->size()/3;
				tot_number_of_vertices+=nva;

				for (int J=I+1;nva < max_vertices_per_batch && J<N;J++)
				{	
					//already merged, skip
					if (merged.find(J)!=merged.end()) continue;
					SmartPointer<Batch> B=node->batches[J];
					if (B->vertices)
					{
						int nvb=B->vertices->size()/3;
						if ((nva+nvb)<=max_vertices_per_batch) //vertex limits
						{
							SmartPointer<Batch> C=Batch::Merge(A,B);
							if (!C) 
								continue;
							A=C;
							merged.insert(J);
							nva+=nvb;
						}
					}
				}
				//Log::printf("   num vertices %d\n",A->vertices->size()/3);
			}
			ret.push_back(A);
		}
	}

	//Log::printf("   total number vertices    %d\n",(int)tot_number_of_vertices);
	//Log::printf("   Number of output batches %d\n",(int)ret.size());
	//Log::printf("   Batch vertex media       %d\n",(int)(tot_number_of_vertices/(float)ret.size()));
	//Log::printf("...done in %d msec\n",(int)t1.msec());
	return ret;
}
int
main (int argc, char** argv)
{
    CloudPtr cloud_in (new Cloud);
    CloudPtr cloud_out (new Cloud);
    pcl::ScopeTime time("performance");
    float endTime;
    pcl::io::loadPCDFile (argv[1], *cloud_in);
    std::cout << "Input size: " << cloud_in->width << " by " << cloud_in->height << std::endl;

///////////////////////////////////////////////////////////////////////////////////////////
//
    pcl::PassThrough<PointType> pass;
    pass.setInputCloud (cloud_in);
    pass.setFilterLimitsNegative(1);
//pass.setKeepOrganized(true);

    pass.setFilterFieldName ("x");
    pass.setFilterLimits (1300, 2200.0);
    pass.filter (*cloud_out);

    pass.setInputCloud(cloud_out);

    pass.setFilterFieldName ("y");
    pass.setFilterLimits (8000, 10000);
    pass.filter (*cloud_out);
//
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (-2000, -200);
    pass.filter (*cloud_out);



    pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZI> octree (10.0);

    // Add points from cloudA to octree
    octree.setInputCloud (cloud_out);
    octree.addPointsFromInputCloud ();

    // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
    octree.switchBuffers ();

    // Add points from cloudB to octree
    octree.setInputCloud (cloud_in);
    octree.addPointsFromInputCloud ();

    std::vector<int> newPointIdxVector;
    // Get vector of point indices from octree voxels which did not exist in previous buffer
    octree.getPointIndicesFromNewVoxels (newPointIdxVector);
//
///////////////////////////////////////////////////////////////////////////////////////////
//
//  std::vector<int> unused;
//  pcl::removeNaNFromPointCloud (*cloud_in, *cloud_in, unused);
//
///////////////////////////////////////////////////////////////////////////////////////////
//
//  pcl::search::Search<PointType>::Ptr searcher (new pcl::search::KdTree<PointType> (false));
//  searcher->setInputCloud (cloud_in);
//  PointType point;
//  point.x = -10000;
//  point.y = 0;
//  point.z = 0;
//  std::vector<int> k_indices;
//  std::vector<float> unused2;
//  //searcher->radiusSearch (point, 100, k_indices, unused2);
//  searcher->nearestKSearch (point, 500000, k_indices, unused2);
//  cloud_out->width = k_indices.size ();
//  cloud_out->height = 1;
//  cloud_out->is_dense = false;
//  cloud_out->points.resize (cloud_out->width);
//  for (size_t i = 0; i < cloud_out->points.size (); ++i)
//  {
//    cloud_out->points[i] = cloud_in->points[k_indices[i]];
//  }
//
///////////////////////////////////////////////////////////////////////////////////////////
//
//  cloud_out->width = cloud_in->width / 10;
//  cloud_out->height = 1;
//  cloud_out->is_dense = false;
//  cloud_out->points.resize (cloud_out->width);
//  for (size_t i = 0; i < cloud_out->points.size (); ++i)
//  {
//    cloud_out->points[i] = cloud_in->points[i * 10];
//  }
//
///////////////////////////////////////////////////////////////////////////////////////////

//     pcl::BilateralFilter<PointType> filter;
//     filter.setInputCloud (cloud_in);
//     pcl::octree::OctreeLeafDataTVector<int> leafT;
//     pcl::search::Search<PointType>::Ptr searcher (new pcl::search::Octree
//             < PointType,
//             pcl::octree::OctreeLeafDataTVector<int> ,
//             pcl::octree::OctreeBase<int, pcl::octree::OctreeLeafDataTVector<int> >
//             > (500) );
//     //pcl::search::Octree <PointType> ocTreeSearch(1);
//     filter.setSearchMethod (searcher);
//     double sigma_s, sigma_r;
//
//     filter.setHalfSize (500);
//     time.reset();
//     filter.filter (*cloud_out);
//     time.getTime();
//     std::cout << "Benchmark Bilateral filer using: kdtree searching, sigma_s = 50, sigma_r = default" << std::endl;
// std::cout << "Results: clocks = " << end - start << ", clockspersec = " << CLOCKS_PER_SEC << std::endl;

//  std::ofstream filestream;
//  filestream.open ("timer_results.txt");
//  char filename[50];
//  for (sigma_s = 1000; sigma_s <= 1000; sigma_s *= 10)
//    for (sigma_r = 0.001; sigma_r <= 1000; sigma_r *= 10)
//    {
//      std::cout << "sigma_s = " << sigma_s << ", sigma_r = " << sigma_r << " clockspersec = " << CLOCKS_PER_SEC
//          << std::endl;
//      filter.setHalfSize (sigma_s);
//      filter.setStdDev (sigma_r);
//      start = clock ();
//      filter.filter (*cloud_out);
//      end = clock ();
//      sprintf (filename, "bruteforce-s%g-r%g.pcd", sigma_s, sigma_r);
//      pcl::io::savePCDFileBinary (filename, *cloud_out);
//      filestream << "sigma_s = " << sigma_s << ", sigma_r = " << sigma_r << " time = " << end - start
//          << " clockspersec = " << CLOCKS_PER_SEC << std::endl;
//    }
//  filestream.close ();

//  std::cout << "Output size: " << cloud_out->width << " by " << cloud_out->height << std::endl;
    CloudPtr cloud_n (new Cloud);
    CloudPtr cloud_buff (new Cloud);
    pcl::io::loadPCDFile ("only_leaves.pcd", *cloud_n);

    pcl::copyPointCloud(*cloud_in, newPointIdxVector, *cloud_buff);

// pcl::io::savePCDFileBinary<pcl::PointXYZI>("delt.pcd",*cloud_buff);

    cloud_buff->operator+=(*cloud_n);

    pcl::io::savePCDFileBinary<pcl::PointXYZI>("cropped_cloud.pcd",*cloud_buff);
    std::cout << std::endl << "Goodbye World" << std::endl << std::endl;
    return (0);
}
void
NMBasedCloudIntegration<PointT>::compute (typename pcl::PointCloud<PointT>::Ptr & output)
{
    if(input_clouds_.empty())
    {
        LOG(ERROR) << "No input clouds set for cloud integration!";
        return;
    }

    big_cloud_info_.clear();

    collectInfo();

    if(param_.reason_about_points_)
        reasonAboutPts();

    pcl::octree::OctreePointCloudPointVector<PointT> octree( param_.octree_resolution_ );
    typename pcl::PointCloud<PointT>::Ptr big_cloud ( new pcl::PointCloud<PointT>());
    big_cloud->width = big_cloud_info_.size();
    big_cloud->height = 1;
    big_cloud->points.resize( big_cloud_info_.size() );
    for(size_t i=0; i < big_cloud_info_.size(); i++)
        big_cloud->points[i] = big_cloud_info_[i].pt;
    octree.setInputCloud( big_cloud );
    octree.addPointsFromInputCloud();

    typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator leaf_it;
    const typename pcl::octree::OctreePointCloudPointVector<PointT>::LeafNodeIterator it2_end = octree.leaf_end();

    size_t kept = 0;
    size_t total_used = 0;

    std::vector<PointInfo> filtered_cloud_info ( big_cloud_info_.size() );

    for (leaf_it = octree.leaf_begin(); leaf_it != it2_end; ++leaf_it)
    {
        pcl::octree::OctreeContainerPointIndices& container = leaf_it.getLeafContainer();

        // add points from leaf node to indexVector
        std::vector<int> indexVector;
        container.getPointIndices (indexVector);

        if(indexVector.empty())
            continue;

        std::vector<PointInfo> voxel_pts ( indexVector.size() );

        for(size_t k=0; k < indexVector.size(); k++)
            voxel_pts[k] = big_cloud_info_ [indexVector[k]];

        PointInfo p;

        size_t num_good_pts = 0;
        if(param_.average_)
        {
            for(const PointInfo &pt_tmp : voxel_pts)
            {
                if (pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
                {
                    p.moving_average( pt_tmp );
                    num_good_pts++;
                }
            }

            if(  num_good_pts < param_.min_points_per_voxel_ )
                continue;

            total_used += num_good_pts;
        }
        else // take only point with min weight
        {
            for(const PointInfo &pt_tmp : voxel_pts)
            {
                if ( pt_tmp.distance_to_depth_discontinuity > param_.min_px_distance_to_depth_discontinuity_)
                {
                    num_good_pts++;
                    if ( pt_tmp.weight < p.weight || num_good_pts == 1)
                        p = pt_tmp;
                }
            }

            if( num_good_pts < param_.min_points_per_voxel_ )
                continue;

            total_used++;
        }
        filtered_cloud_info[kept++] = p;
    }

    LOG(INFO) << "Number of points in final noise model based integrated cloud: " << kept << " used: " << total_used << std::endl;


    if(!output)
        output.reset(new pcl::PointCloud<PointT>);

    if(!output_normals_)
        output_normals_.reset( new pcl::PointCloud<pcl::Normal>);

    filtered_cloud_info.resize(kept);
    output->points.resize(kept);
    output_normals_->points.resize(kept);
    output->width = output_normals_->width = kept;
    output->height = output_normals_->height = 1;
    output->is_dense = output_normals_->is_dense = true;

    PointT na;
    na.x = na.y = na.z = std::numeric_limits<float>::quiet_NaN();


    input_clouds_used_.resize( input_clouds_.size() );
    for(size_t i=0; i<input_clouds_used_.size(); i++) {
        input_clouds_used_[i].reset( new pcl::PointCloud<PointT> );
        input_clouds_used_[i]->points.resize( input_clouds_[i]->points.size(), na);
        input_clouds_used_[i]->width =  input_clouds_[i]->width;
        input_clouds_used_[i]->height =  input_clouds_[i]->height;
    }

    for(size_t i=0; i<filtered_cloud_info.size(); i++)
    {
        output->points[i] = filtered_cloud_info[i].pt;
        output_normals_->points[i] = filtered_cloud_info[i].normal;
        int origin = filtered_cloud_info[i].origin;
        input_clouds_used_[origin]->points[filtered_cloud_info[i].pt_idx] = filtered_cloud_info[i].pt;
    }

    cleanUp();
}
Пример #14
0
// The MAIN function, from here we start the application and run the main loop
int main()
{
    ///*************************************************************************************///
    ///******************************** PCL LOADING PART ***********************************///
    ///*************************************************************************************///

    // (This part will be separated later!)

    // Later, this will be according to the command line argument
    // Now I test it, with "fovam2a_bin_compressed.pcd"
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB> ("fovam2a_bin_compressed.pcd", *cloud) == -1) // load the file
    {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
    }
    std::cout << "Loaded " << cloud->width * cloud->height << " data points from test_pcd.pcd with the following fields: " << std::endl;


    std::cout << cloud->width << std::endl;
    std::cout << cloud->height << std::endl;

    // Set the initial minimum value of each coordinate:
    min_x = cloud->points[0].x;
    min_y = cloud->points[0].y;
    min_z = cloud->points[0].z;

    // Calculate the minimum value of each coordinate:
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        if(cloud->points[i].x < min_x) min_x = cloud->points[i].x;
        if(cloud->points[i].y < min_y) min_y = cloud->points[i].y;
        if(cloud->points[i].z < min_z) min_z = cloud->points[i].z;
    }

    // Transform the cloud to the origin of its coordinate system, for easier handling of the cloud data.
    // This part of the code should be removed later. (Just helped me at the beginning)
    for(size_t i = 0; i < cloud->points.size (); ++i)
    {
        cloud->points[i].x -= min_x;
        cloud->points[i].y -= min_y;
        cloud->points[i].z -= min_z;
    }

    ///*************************************************************************************///
    ///******************************** GLFW INIT PART ***********************************///
    ///*************************************************************************************///

    // Init GLFW
    glfwInit();

    // Set all the required options for GLFW
    glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
    glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
    glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
    glfwWindowHint(GLFW_RESIZABLE, GL_FALSE);

    // Create a GLFWwindow object that we can use for GLFW's functions
    GLFWwindow* window = glfwCreateWindow(WIDTH, HEIGHT, "PCL with OpenGL", nullptr, nullptr);
    glfwMakeContextCurrent(window);

    // Set the required callback functions
    glfwSetKeyCallback(window, key_callback);
    glfwSetCursorPosCallback(window, mouse_callback);
    glfwSetScrollCallback(window, scroll_callback);

    // Options
    glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED);

    // This is how GLEW knows that it should use a modern approach for retrieving function pointers and extensions
    glewExperimental = GL_TRUE;

    // Initialize GLEW to setup the OpenGL Function pointers
    glewInit();

    // Define the viewport dimensions
    glViewport(0, 0, WIDTH, HEIGHT);

    glEnable(GL_DEPTH_TEST);
    glPointSize(1.3f);

    ///*************************************************************************************///
    ///******************************** VBO LOADING PART ***********************************///
    ///*************************************************************************************///

    Shader ourShader("shader.vs", "shader.frag"); // load the shaders

    std::size_t v_size = cloud->points.size() * 6; // size of my points
    // (all of them contains 6 member)

    // I will fill up this vector with all the data from cloud->points
    std::vector<GLfloat> vertices(v_size);

    // for efficient data reading, I start a new thread for reading the half of my data.
    std::future<void> result( std::async([&]()
    {
        for(size_t i = 1; i < cloud->points.size (); i+=2)
        {
            size_t num = (i * 6);

            vertices[num + 0] = cloud->points[i].x;
            vertices[num + 1] = cloud->points[i].y;
            vertices[num + 2] = cloud->points[i].z;

            vertices[num + 3] = (float)cloud->points[i].r / 256.f;
            vertices[num + 4] = (float)cloud->points[i].g / 256.f;
            vertices[num + 5] = (float)cloud->points[i].b / 256.f;
        }

    }));

    // another half of my points
    for(size_t i = 0; i < cloud->points.size (); i+=2)
    {
        size_t num = (i * 6);

        vertices[num + 0] = cloud->points[i].x;
        vertices[num + 1] = cloud->points[i].y;
        vertices[num + 2] = cloud->points[i].z;

        vertices[num + 3] = (float)cloud->points[i].r / 256.f;
        vertices[num + 4] = (float)cloud->points[i].g / 256.f;
        vertices[num + 5] = (float)cloud->points[i].b / 256.f;
    }

    result.get(); // wait for the other thread
    // Now, I've filled up my vector!

    std::cout << "*****!!!DONE! (read)!!!*****" << std::endl;

    /// Create the VBO from the data:
    GLuint VBO, VAO;
    glGenVertexArrays(1, &VAO);
    glGenBuffers(1, &VBO);

    // Bind the Vertex Array Object first, then bind and set the vertex buffer(s) and the attribute pointer(s).
    glBindVertexArray(VAO);

    glBindBuffer(GL_ARRAY_BUFFER, VBO);
    glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * vertices.size(), vertices.data(), GL_STATIC_DRAW);

    // Position attribute
    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(GLfloat), (GLvoid*)0);
    glEnableVertexAttribArray(0);

    // Color attribute
    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 6 * sizeof(GLfloat), (GLvoid*)(3 * sizeof(GLfloat)));
    glEnableVertexAttribArray(1);

    glBindVertexArray(0); // Unbind VAO

    std::cout << "*****!!!DONE! (VBO load)!!!*****" << std::endl;

    ourShader.Use();

    /// Load transformation matrices
    GLuint MatrixID_modelview = glGetUniformLocation(ourShader.Program, "modelview");
    GLuint MatrixID_proj = glGetUniformLocation(ourShader.Program, "projection");

    // Send our transformations to the currently bound shader
    glUniformMatrix4fv(MatrixID_modelview, 1, GL_FALSE, &ModelView[0][0]);
    glUniformMatrix4fv(MatrixID_proj, 1, GL_FALSE, &Proj[0][0]);

    vertices.clear();
    //cloud.reset();

    VeryNaiveSphere mySphere(500, 500, glm::vec3(10000.f, 10000.f, 10000.f));
    mySphere.init_sphere();

    float resolution = 128.0f; // for the leaf level of the octree

    pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree (resolution);

    // Fill up my tree:
    octree.setInputCloud (cloud);
    octree.addPointsFromInputCloud();

    ///*************************************************************************************///
    ///******************************** MAIN LOOP PART ***********************************///
    ///*************************************************************************************///

    std::cout << "*****!!!LOOP!!!*****" << std::endl;
    // Main loop
    while (!glfwWindowShouldClose(window))
    {
        // Set frame time
        GLfloat currentFrame = glfwGetTime();
        deltaTime = currentFrame - lastFrame;
        lastFrame = currentFrame;

        // Check if any events have been activiated (key pressed, mouse moved etc.) and call corresponding response functions
        glfwPollEvents();
        Do_Camera_movement();
        Do_Sphere_movement(mySphere, octree);

        // Render
        // Clear the colorbuffer and the depthbuffer
        glClearColor(0.2f, 0.2f, 0.2f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        // Draw
        ourShader.Use();

        Proj = glm::perspective(camera.Zoom, (float)WIDTH / (float)HEIGHT, cam_near, cam_far);

        ModelView = camera.GetViewMatrix();

        glUniformMatrix4fv(MatrixID_proj, 1, GL_FALSE, &Proj[0][0]);
        glUniformMatrix4fv(MatrixID_modelview, 1, GL_FALSE, &ModelView[0][0]);

        glBindVertexArray(VAO);
        glDrawArrays(GL_POINTS, 0, v_size / 6);
        glBindVertexArray(0);

        mySphere.draw(MatrixID_modelview, ModelView);

        // Swap the screen buffers
        glfwSwapBuffers(window);
    }
    // Properly de-allocate all resources once they've outlived their purpose
    glDeleteVertexArrays(1, &VAO);
    glDeleteBuffers(1, &VBO);

    // Terminate GLFW, clearing any resources allocated by GLFW.
    glfwTerminate();
    return 0;
}
Пример #15
0
int
main (int argc, char** argv)
{
  srand ((unsigned int) time (NULL));

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // Generate pointcloud data
  cloud->width = 1000;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
    cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
  }

  float resolution = 128.0f;

  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (resolution);

  octree.setInputCloud (cloud);
  octree.addPointsFromInputCloud ();

  pcl::PointXYZ searchPoint;

  searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
  searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
  searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);

  // Neighbors within voxel search

  std::vector<int> pointIdxVec;

  if (octree.voxelSearch (searchPoint, pointIdxVec))
  {
    std::cout << "Neighbors within voxel search at (" << searchPoint.x 
     << " " << searchPoint.y 
     << " " << searchPoint.z << ")" 
     << std::endl;
              
    for (size_t i = 0; i < pointIdxVec.size (); ++i)
   std::cout << "    " << cloud->points[pointIdxVec[i]].x 
       << " " << cloud->points[pointIdxVec[i]].y 
       << " " << cloud->points[pointIdxVec[i]].z << std::endl;
  }

  // K nearest neighbor search

  int K = 10;

  std::vector<int> pointIdxNKNSearch;
  std::vector<float> pointNKNSquaredDistance;

  std::cout << "K nearest neighbor search at (" << searchPoint.x 
            << " " << searchPoint.y 
            << " " << searchPoint.z
            << ") with K=" << K << std::endl;

  if (octree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
  {
    for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
      std::cout << "    "  <<   cloud->points[ pointIdxNKNSearch[i] ].x 
                << " " << cloud->points[ pointIdxNKNSearch[i] ].y 
                << " " << cloud->points[ pointIdxNKNSearch[i] ].z 
                << " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
  }

  // Neighbors within radius search

  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;

  float radius = 256.0f * rand () / (RAND_MAX + 1.0f);

  std::cout << "Neighbors within radius search at (" << searchPoint.x 
      << " " << searchPoint.y 
      << " " << searchPoint.z
      << ") with radius=" << radius << std::endl;


  if (octree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
  {
    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
      std::cout << "    "  <<   cloud->points[ pointIdxRadiusSearch[i] ].x 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].y 
                << " " << cloud->points[ pointIdxRadiusSearch[i] ].z 
                << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
  }

}
Пример #16
0
void processCloud(const sensor_msgs::PointCloud2 msg)
{
	//********* Retirive and process raw pointcloud************
	std::cout<<"Recieved cloud"<<std::endl;
	std::cout<<"Create Octomap"<<std::endl;
	//octomap::OcTree tree(res);
	std::cout<<"Load points "<<std::endl;
	pcl::PCLPointCloud2 cloud;
	pcl_conversions::toPCL(msg,cloud);
	pcl::PointCloud<pcl::PointXYZ> pcl_pc;
	pcl::fromPCLPointCloud2(cloud,pcl_pc);
	std::cout<<"Filter point clouds for NAN"<<std::endl;
	std::vector<int> nan_indices;
	pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
	//octomap::Pointcloud oct_pc;
	//octomap::point3d origin(0.0f,0.0f,0.0f);
	std::cout<<"Adding point cloud to octomap"<<std::endl;
	//octomap::point3d origin(0.0f,0.0f,0.0f);
	//for(int i = 0;i<pcl_pc.points.size();i++){
		//oct_pc.push_back((float) pcl_pc.points[i].x,(float) pcl_pc.points[i].y,(float) pcl_pc.points[i].z);
	//}
	//tree.insertPointCloud(oct_pc,origin,-1,false,false);
	
	//*********** Remove the oldest data, update the data***************	
	cloud_seq_loaded.push_back(pcl_pc);
	std::cout<<cloud_seq_loaded.size()<<std::endl;
	if(cloud_seq_loaded.size()>2){
		cloud_seq_loaded.pop_front();
	
	}
	
	//*********** Process currently observerd and buffered data*********

	if(cloud_seq_loaded.size()==2){
		std::cout<< "Generating octomap"<<std::endl;
		
		pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>); 
		*prev_pc = cloud_seq_loaded.front();
		pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
		*curr_pc =pcl_pc;
		pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (32.0f);
		octree.setInputCloud(prev_pc);
		octree.addPointsFromInputCloud();
		octree.switchBuffers();
		octree.setInputCloud(curr_pc);
		octree.addPointsFromInputCloud();
		std::vector<int> newPointIdxVector;
		// Get vector of point indices from octree voxels which did not exist in previous buffer
		octree.getPointIndicesFromNewVoxels (newPointIdxVector);
		std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
		for (size_t i = 0; i < newPointIdxVector.size (); ++i){
				std::cout << i << "# Index:" << newPointIdxVector[i]<< "  Point:" << pcl_pc.points[newPointIdxVector[i]].x << " "<< pcl_pc.points[newPointIdxVector[i]].y << " "<< pcl_pc.points[newPointIdxVector[i]].z << std::endl;
			//std::cout<< curr_coord<<std::endl;
		}	
			//std::cout << "Node center: " << it.getCoordinate() << std::endl;
			//std::cout << "Node size: " << it.getSize() << std::endl;
			//std::cout << "Node value: " << it->getValue() << std::endl;
				

		}
		//********** print out the statistics ******************
		
	
	//**************Process Point cloud in octree data structure *****************
	
	
	
	
	
	/*
	//******************Traverse the tree ********************
	for(octomap::OcTree::tree_iterator it =tree.begin_tree(), end = tree.end_tree();it!= end;it++){
		 //manipulate node, e.g.:
		std::cout << "_____________________________________"<<std::endl;
		std::cout << "Node center: " << it.getCoordinate() << std::endl;
		std::cout << "Node size: " << it.getSize() << std::endl;
		std::cout << "Node depth: "<<it.getDepth() << std::endl;
		std::cout << "Is Leaf : "<< it.isLeaf()<< std::endl;
		std::cout << "_____________________________________"<<std::endl;
		}
	//**********************************************************	
	*/
	std::cout<<"finished"<<std::endl;
	std::cout<<std::endl;
	}
Пример #17
0
int
main (int argc, char** argv)
{
  srand ((unsigned int) time (NULL));

  // Octree resolution - side length of octree voxels
  float resolution = 32.0f;

  // Instantiate octree-based point cloud change detection class
  pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (resolution);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );

  // Generate pointcloud data for cloudA
  cloudA->width = 128;
  cloudA->height = 1;
  cloudA->points.resize (cloudA->width * cloudA->height);

  for (size_t i = 0; i < cloudA->points.size (); ++i)
  {
    cloudA->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudA->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudA->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudA to octree
  octree.setInputCloud (cloudA);
  octree.addPointsFromInputCloud ();

  // Switch octree buffers: This resets octree but keeps previous tree structure in memory.
  octree.switchBuffers ();

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
   
  // Generate pointcloud data for cloudB 
  cloudB->width = 128;
  cloudB->height = 1;
  cloudB->points.resize (cloudB->width * cloudB->height);

  for (size_t i = 0; i < cloudB->points.size (); ++i)
  {
    cloudB->points[i].x = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudB->points[i].y = 64.0f * rand () / (RAND_MAX + 1.0f);
    cloudB->points[i].z = 64.0f * rand () / (RAND_MAX + 1.0f);
  }

  // Add points from cloudB to octree
  octree.setInputCloud (cloudB);
  octree.addPointsFromInputCloud ();

  std::vector<int> newPointIdxVector;

  // Get vector of point indices from octree voxels which did not exist in previous buffer
  octree.getPointIndicesFromNewVoxels (newPointIdxVector);

  // Output points
  std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
  for (size_t i = 0; i < newPointIdxVector.size (); ++i)
    std::cout << i << "# Index:" << newPointIdxVector[i]
              << "  Point:" << cloudB->points[newPointIdxVector[i]].x << " "
              << cloudB->points[newPointIdxVector[i]].y << " "
              << cloudB->points[newPointIdxVector[i]].z << std::endl;

}
Пример #18
0
	//--------------------------------------------------------------
	map<int,TrackedCloudPtr> ObjectsThread::computeOcclusions(const list<TrackedCloudPtr>& potentialOcclusions)
	{
		map<int,TrackedCloudPtr> occlusions;

		ofVec3f origin = PCXYZ_OFVEC3F(eyePos());

		inCloudMutex.lock();
		PCPtr cloud = PCPtr(new PC(*inRawCloud));
		inRawCloud.reset();
		inCloudMutex.unlock();

		saveCloud("rawInternal.pcd",*cloud);
		pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree(Constants::CLOUD_VOXEL_SIZE*2);
		pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::AlignedPointTVector voxelList;
		if(cloud->size() > 0)
		{
			octree.setInputCloud(cloud);
			octree.defineBoundingBox();
			octree.addPointsFromInputCloud();

			gModel->objectsMutex.lock();
			for (list<TrackedCloudPtr>::const_iterator iter = potentialOcclusions.begin(); iter != potentialOcclusions.end(); iter++) 
			{
				if((*iter)->hasObject())
				{
					bool occludedPol = true;
					PCPolyhedron* polyhedron = dynamic_cast<PCPolyhedron*>((*iter)->getTrackedObject().get());
					polyhedron->resetOccludedFaces();
					vector<IPolygonPtr> pols = polyhedron->getVisiblePolygons();
					int occludedFaces = 0;
					for(int i = 0; i < pols.size(); i++)
					{
						vector<ofVec3f> vexs = pols.at(i)->getMathModel().getVertexs();
						int occludedVertexs = 0;

						for(int o = 0; o < vexs.size(); o++)
						{
							bool occludedVertex = false;
							ofVec3f end = vexs.at(o);
							Eigen::Vector3f endPoint(end.x,end.y,end.z);
							Eigen::Vector3f originPoint = PCXYZ_EIGEN3F(eyePos());
							voxelList.clear();

							int voxs = octree.getApproxIntersectedVoxelCentersBySegment(originPoint,endPoint,voxelList,Constants::CLOUD_VOXEL_SIZE*2);

							for(int i = 0; i < voxelList.size(); i ++)
							{
								if(octree.isVoxelOccupiedAtPoint(voxelList.at(i)))
								{
									ofVec3f intersect (voxelList.at(i).x,voxelList.at(i).y,voxelList.at(i).z);
									if(((intersect - end).length() > Constants::CLOUD_VOXEL_SIZE*5) &&
										(intersect - origin).length() < (end - origin).length())
										occludedVertexs++;
								}
							}
						}

						if(occludedVertexs >= 2)
						{
							polyhedron->setOccludedFace(pols.at(i)->getName());
							occludedFaces++;
						}
					}
					if(occludedFaces > 1)
					{
						occlusions[(*iter)->getTrackedObject()->getId()] = (*iter);
						//cout << "	occluded pol " << endl;
					}
				}
			}
			gModel->objectsMutex.unlock();

		}
		return occlusions;
	}
Пример #19
0
/**
 *  texture function
 *
 *  This function colors the 3D model returned by the mergePointClouds function
 *
 *  @param point cloud mesh
 *  @param std::vector<Frame3D> frames
 */
void Functions3D::texture(pcl::PolygonMesh mesh, std::vector<Frame3D> frames, std::string filename)
{
    // load values from the mesh
    auto polygons = mesh.polygons;

    // create cloud to save the points in
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());;

    // create clouds to add the rgb points to
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>());;

    // copy mesh cloud to both placeholders
    pcl::fromPCLPointCloud2(mesh.cloud, *cloud);
    pcl::fromPCLPointCloud2(mesh.cloud, *cloud_rgb);

    // create texture mapping variable
    auto texture_mapping = new pcl::TextureMapping<pcl::PointXYZ>();

    // save uv coordinates
    Eigen::Vector2f uv_coordinates;

    /**
     *  Loop over all frames in the vector
     */
    for (int i = frames.size()-1; i>=0; i--)
    {
        // Save reference instead of copy
        const Frame3D &current_frame = frames[i];

        // get data from the frame
        auto depth = current_frame.depth_image_;
        auto focal = current_frame.focal_length_;
        auto depth_map_size = depth.size();
        auto rgb_image = current_frame.rgb_image_;

        // save and resize rgb image
        cv::Mat rgb_img;
        cv::resize(rgb_image, rgb_img, depth_map_size);

        // inverse the camera pose
        auto camera_pose = current_frame.getEigenTransform();
        camera_pose(3,3) = 1;
        auto inv_camera = camera_pose.inverse().eval();

        // create texture mapping camera
        pcl::texture_mapping::Camera camera;
        camera.focal_length = focal;
        camera.height = rgb_img.size().height;
        camera.width = rgb_img.size().width;

        // reverse the point cloud transformation
        pcl::PointCloud<pcl::PointXYZ>::Ptr trans_cloud(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::transformPointCloud(*cloud, *trans_cloud, inv_camera);

        // create octree for texture mapping
        pcl::TextureMapping<pcl::PointXYZ>::Octree::Ptr octree(new pcl::TextureMapping<pcl::PointXYZ>::Octree(0.01));
        octree->setInputCloud(trans_cloud->makeShared());
        octree->addPointsFromInputCloud();

        /**
         *  Loop over all polygons
         */
        for (pcl::Vertices polygon : polygons) 
        {
            /**
             *  Loop over all vertices in the polygon
             */
            for (uint32_t vertex : polygon.vertices)
            {
                // check if the point is occluded
                auto vertex_point = trans_cloud->points.at(vertex);
                if (!texture_mapping->isPointOccluded(vertex_point, octree)) 
                {
                    // if it is not, get the uv coordinates
                    if (texture_mapping->getPointUVCoordinates(vertex_point, camera, uv_coordinates)) 
                    {
                        // get the coordinates
                        int x = round(uv_coordinates.x() * camera.width);
                        int y = round(camera.height - uv_coordinates.y() * camera.height);
                        float z = depth.at<ushort>(y, x) * 0.001;

                        // if they have the correct depth, save them
                        if (z < 1.2) 
                        {
                            auto pixel = rgb_img.at<cv::Vec3b>(cv::Point(x, y));
                            cloud_rgb->points.at(vertex).b = pixel[0];
                            cloud_rgb->points.at(vertex).g = pixel[1];
                            cloud_rgb->points.at(vertex).r = pixel[2];
                        }
                    }
                }
            }
        }
    }

    // set the rgb cloud as point cloud for the mesh
    pcl::toPCLPointCloud2(*cloud_rgb, mesh.cloud);

    // save the mesh to a file 
    pcl::io::saveVTKFile("../data/" + filename, mesh);
}
Пример #20
0
void processCloud(const sensor_msgs::PointCloud2 msg)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr curr_pc (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCLPointCloud2 cloud;
	pcl::PointCloud<pcl::PointXYZ> pcl_pc;
	std::vector<int> nan_indices;
	pcl::PointCloud<pcl::PointXYZ>::Ptr prev_pc (new pcl::PointCloud<pcl::PointXYZ>); 
		
	
	//********* Retirive and process raw pointcloud************
	pcl_conversions::toPCL(msg,cloud);
	pcl::fromPCLPointCloud2(cloud,pcl_pc);
	pcl::removeNaNFromPointCloud(pcl_pc,pcl_pc,nan_indices);
	*curr_pc =pcl_pc;

	//*********** Remove old data, update the data***************	
	cloud_seq_loaded.push_back(pcl_pc);
	std::cout<<cloud_seq_loaded.size()<<std::endl;
	
	if(cloud_seq_loaded.size()>2){
		cloud_seq_loaded.pop_front();
	
	}
	if(cloud_seq_loaded.size()==1){
		static_pc = pcl_pc;
		}
	
	//*********** Process currently observerd and buffered data*********

	if(cloud_seq_loaded.size()==2){
		
		
		*prev_pc =static_pc; //cloud_seq_loaded.front(); ss
		
		//*************Create octree structure and search
		pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ> octree (0.5);
		octree.setInputCloud(prev_pc);
		octree.addPointsFromInputCloud();
		octree.switchBuffers();
		octree.setInputCloud(curr_pc);
		octree.addPointsFromInputCloud();
		std::vector<int> newPointIdxVector;
		// Get vector of point indices from octree voxels which did not exist in previous buffer
		octree.getPointIndicesFromNewVoxels (newPointIdxVector);
		std::cout << "Output from getPointIndicesFromNewVoxels:" << std::endl;
		pcl::PointCloud<pcl::PointXYZ>::Ptr dynamic_points (new pcl::PointCloud<pcl::PointXYZ>);
		dynamic_points->header.frame_id = "some_tf_frame";
		
		for (size_t i = 0; i < newPointIdxVector.size (); ++i){
			pcl::PointXYZ point;
			point.x = pcl_pc.points[newPointIdxVector[i]].x;
			point.y = pcl_pc.points[newPointIdxVector[i]].y;
			point.z = pcl_pc.points[newPointIdxVector[i]].z;
			dynamic_points->push_back(point);
			//std::cout << i << "# Index:" << newPointIdxVector[i]<< "  Point:" << pcl_pc.points[newPointIdxVector[i]].x << " "<< pcl_pc.points[newPointIdxVector[i]].y << " "<< pcl_pc.points[newPointIdxVector[i]].z << std::endl;
			
		}	
		std::cout<<newPointIdxVector.size ()<<std::endl;
		
		//***************Filter point cloud to detect nearby changes only *****************
		pcl::PassThrough<pcl::PointXYZ> pass;
		pass.setInputCloud (dynamic_points);
		pass.setFilterFieldName ("z");
		pass.setFilterLimits (0.0, 3.0);
		pass.filter (*dynamic_points);
		
		pcl::StatisticalOutlierRemoval<pcl::PointXYZ> dy_sor;
		dy_sor.setInputCloud (dynamic_points);
		dy_sor.setMeanK (50);
		dy_sor.setStddevMulThresh (1.0);
		dy_sor.filter (*dynamic_points);
		
		//**********************Publish the data************************************
		ros::NodeHandle k;
		ros::Publisher pub = k.advertise<pcl::PointCloud<pcl::PointXYZ> >("dynamicPoints",2);
		pub.publish(dynamic_points);
		ros::Time time = ros::Time::now();
		//Wait a duration of one second.
		ros::Duration d = ros::Duration(1.5, 0);
		d.sleep();
		ros::spinOnce();
		
				

		}
	std::cout<<"finished"<<std::endl;
	std::cout<<std::endl;
	}