CPP代码学习

1.随机采样

 1     samples.vn = want_sample_num;
 2 
 3     vector<int> nCard = GlobalFun::GetRandomCards(original.vert.size());
 4     for(int i = 0; i < samples.vn; i++) 
 5     {
 6         int index = nCard[i]; //could be not random!
 7 
 8                 if (!use_random_downsample)
 9                 {
10                   index = i;
11                 }
12 
13         CVertex& v = original.vert[index];
14         samples.vert.push_back(v);
15         samples.bbox.Add(v.P());
16     }

 

 1 vector<int> GlobalFun::GetRandomCards(int Max)
 2 {
 3     vector<int> nCard(Max, 0);
 4     srand(time(NULL));
 5     for(int i=0; i < Max; i++)
 6     {
 7         nCard[i] = i;
 8     }
 9     random_shuffle(nCard.begin(), nCard.begin() + Max);
10 
11 
12     return nCard;
13 }

 2.将平面点云投影到XOY平面

 1 //增加仿射变换
 2         planefit fit;
 3         fit.setInputPointCloud(ptsArray);
 4         fit.extractMeanAD();
 5         Eigen::Vector3d normal = fit.getPlane().getNormal();
 6         Eigen::Vector3d center = fit.getPlane().getCenter();
 7         Eigen::Vector3d virtical = Eigen::Vector3d::UnitZ();
 8         //计算叉积
 9         Eigen::Vector3d axis = normal.cross(virtical);
10         double theta = acos(normal.dot(virtical) / normal.norm());
11         Eigen::AngleAxisd rotation_vector(theta, axis);
12         Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();
13         //转换到XOY平面
14         std::vector<POINT_REGIS> ptsRotateArray;
15         for (int i = 0; i < ptsArray.size();i++)
16         {
17             POINT_REGIS pt = ptsArray[i];
18             Eigen::Vector3d pt1(pt.x, pt.y, pt.z);
19             Eigen::Vector3d pt2=pt1 - center;
20             Eigen::Vector3d vpt = rotation_vector*pt2;
21             POINT_REGIS vpt1;
22             vpt1.x = vpt.x();
23             vpt1.y = vpt.y();
24             vpt1.z = vpt.z();
25             ptsRotateArray.push_back(vpt1);
26         }

 3.参数

 1 double DataMgr::getInitRadiuse()
 2 {
 3     double init_para = para->getDouble("Init Radius Para");
 4     if (!isOriginalEmpty())
 5     {
 6         Box3f box = original.bbox;
 7         if ( abs(box.min.X() - box.max.X()) < 1e-5 ||   
 8             abs(box.min.Y() - box.max.Y()) < 1e-5 ||   
 9             abs(box.min.Z() - box.max.Z()) < 1e-5 )
10         {
11             double diagonal_length = sqrt((box.min - box.max).SquaredNorm());
12             double original_size = sqrt(double(original.vn));
13             init_radius = 2 * init_para * diagonal_length / original_size;
14         }
15         else
16         {
17             double diagonal_length = sqrt((box.min - box.max).SquaredNorm());
18             double original_size = pow(double(original.vn), 0.333);
19             init_radius = init_para * diagonal_length / original_size;
20         }
21     }
22 
23  
24   global_paraMgr.setGlobalParameter("CGrid Radius", DoubleValue(init_radius));
25   global_paraMgr.setGlobalParameter("Initial Radius", DoubleValue(init_radius));
26 
27     return init_radius;
28 }
View Code

 

 1 if (input_->points.size() > 3)
 2         {
 3             double scale = 0.0;
 4             std::vector<double> ptScales;
 5             pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
 6             tree->setInputCloud(input_);
 7             for (size_t i = 0; i < input_->points.size(); i++)
 8             {
 9                 double x = input_->points.at(i).x;
10                 double y = input_->points.at(i).y;
11                 std::vector<int> nn_indices;
12                 std::vector<float> nn_dists;
13                 const pcl::PointXYZ pt = input_->at(i);
14                 if (tree->nearestKSearch(pt, 5, nn_indices, nn_dists) == 0)//有错误，程序无法进入
15                 {
16                     continue;
17                 }
18                 double dist = sqrt(nn_dists[1]);//sqrt(nn_dists[3]);为啥是3？？
19                 ptScales.push_back(dist);
20                 scale += dist;
21             }
22 
23             std::sort(ptScales.begin(), ptScales.end(), [](const double& lhs, const double& rhs) { return lhs < rhs; });
24             int idxNinety = min(int(double(ptScales.size()) * 0.9), int(ptScales.size() - 1));
25             gridSideLength = ptScales[idxNinety] * 0.75;
26             scale = scale / input_->points.size();
27             thLineLength = 5 * scale;//thLineLength = 8 * scale;
28         }
View Code

 

posted @ 2021-09-14 17:13  太一吾鱼水  阅读(219)  评论(0)  编辑  收藏  举报