[CC]点云密度计算

1.计算点云最近点的平均距离（点云的平均距离）http://pointclouds.org/documentation/tutorials/correspondence_grouping.php

 1 double computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
 2 {
 3   double res = 0.0;
 4   int n_points = 0;
 5   int nres;
 6   std::vector<int> indices (2);
 7   std::vector<float> sqr_distances (2);
 8   pcl::search::KdTree<PointType> tree;
 9   tree.setInputCloud (cloud);
10 
11   for (size_t i = 0; i < cloud->size (); ++i)
12   {
13     if (! pcl_isfinite ((*cloud)[i].x))
14     {
15       continue;
16     }
17     //Considering the second neighbor since the first is the point itself.
18     nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
19     if (nres == 2)
20     {
21       res += sqrt (sqr_distances[1]);
22       ++n_points;
23     }
24   }
25   if (n_points != 0)
26   {
27     res /= n_points;
28   }
29   return res;
30 }

 

 1 assert(m_app);
 2     if (!m_app)
 3         return;
 4 
 5     const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
 6     size_t selNum = selectedEntities.size();
 7     if (selNum!=1)
 8     {
 9         m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
10         return;
11     }
12 
13     ccHObject* ent = selectedEntities[0];
14     assert(ent);
15     if (!ent || !ent->isA(CC_TYPES::POINT_CLOUD))
16     {
17         m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
18         return;
19     }
20 
21     ccPointCloud* m_cc_cloud = static_cast<ccPointCloud*>(ent);
22 
23     //input cloud
24     unsigned count = m_cc_cloud->size();
25     bool hasNorms = m_cc_cloud->hasNormals();
26     CCVector3 bbMin, bbMax;
27     m_cc_cloud->getBoundingBox(bbMin,bbMax);
28     const CCVector3d& globalShift = m_cc_cloud->getGlobalShift();
29     double globalScale = m_cc_cloud->getGlobalScale();
30 
31     pcl::PointCloud<PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<PointXYZ>);
32     try
33     {
34         unsigned pointCount = m_cc_cloud->size();
35         pcl_cloud->resize(pointCount);
36 
37         for (unsigned i = 0; i < pointCount; ++i)
38         {
39             const CCVector3* P = m_cc_cloud->getPoint(i);
40             pcl_cloud->at(i).x = static_cast<float>(P->x);
41             pcl_cloud->at(i).y = static_cast<float>(P->y);
42             pcl_cloud->at(i).z = static_cast<float>(P->z);
43         }
44     }
45     catch(...)
46     {
47         //any error (memory, etc.)
48         pcl_cloud.reset();
49     }
50     if (!target_indices_ || target_indices_->size () == 0)
51     {
52         target_indices_.reset (new std::vector <int> (static_cast <int> (pcl_cloud->size ())));
53         int index = 0;
54         for (std::vector <int>::iterator it = target_indices_->begin (), it_e = target_indices_->end (); it != it_e; it++)
55             *it = index++;
56     }
57     Eigen::Vector4f pt_min, pt_max;
58     pcl::getMinMax3D (*pcl_cloud, *target_indices_, pt_min, pt_max);
59     double diameter_ = (pt_max - pt_min).norm ();//点云中最远的两个点的距离
60     double dis=computeCloudResolution(pcl_cloud);
61     m_app->dispToConsole(QString("cross distance (c=%1)\nmean distance(m=%2)").arg(diameter_,0,'f').arg(dis,0,'f'),ccMainAppInterface::STD_CONSOLE_MESSAGE);
CC版

 

posted @ 2016-05-19 16:10  太一吾鱼水  阅读(5240)  评论(0)  编辑  收藏  举报