CPP代码学习

1.随机采样

    samples.vn = want_sample_num;

    vector<int> nCard = GlobalFun::GetRandomCards(original.vert.size());
    for(int i = 0; i < samples.vn; i++) 
    {
        int index = nCard[i]; //could be not random!

                if (!use_random_downsample)
                {
                  index = i;
                }

        CVertex& v = original.vert[index];
        samples.vert.push_back(v);
        samples.bbox.Add(v.P());
    }    

vector<int> GlobalFun::GetRandomCards(int Max)
{
    vector<int> nCard(Max, 0);
    srand(time(NULL));
    for(int i=0; i < Max; i++)
    {
        nCard[i] = i;
    }
    random_shuffle(nCard.begin(), nCard.begin() + Max);


    return nCard;
}

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

//增加仿射变换
        planefit fit;
        fit.setInputPointCloud(ptsArray);
        fit.extractMeanAD();
        Eigen::Vector3d normal = fit.getPlane().getNormal();
        Eigen::Vector3d center = fit.getPlane().getCenter();
        Eigen::Vector3d virtical = Eigen::Vector3d::UnitZ();
        //计算叉积
        Eigen::Vector3d axis = normal.cross(virtical);
        double theta = acos(normal.dot(virtical) / normal.norm());
        Eigen::AngleAxisd rotation_vector(theta, axis);
        Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();
        //转换到XOY平面
        std::vector<POINT_REGIS> ptsRotateArray;
        for (int i = 0; i < ptsArray.size();i++)
        {
            POINT_REGIS pt = ptsArray[i];
            Eigen::Vector3d pt1(pt.x, pt.y, pt.z);
            Eigen::Vector3d pt2=pt1 - center;
            Eigen::Vector3d vpt = rotation_vector*pt2;
            POINT_REGIS vpt1;
            vpt1.x = vpt.x();
            vpt1.y = vpt.y();
            vpt1.z = vpt.z();
            ptsRotateArray.push_back(vpt1);
        }

 3.参数

double DataMgr::getInitRadiuse()
{
    double init_para = para->getDouble("Init Radius Para");
    if (!isOriginalEmpty())
    {
        Box3f box = original.bbox;
        if ( abs(box.min.X() - box.max.X()) < 1e-5 ||   
            abs(box.min.Y() - box.max.Y()) < 1e-5 ||   
            abs(box.min.Z() - box.max.Z()) < 1e-5 )
        {
            double diagonal_length = sqrt((box.min - box.max).SquaredNorm());
            double original_size = sqrt(double(original.vn));
            init_radius = 2 * init_para * diagonal_length / original_size;
        }
        else
        {
            double diagonal_length = sqrt((box.min - box.max).SquaredNorm());
            double original_size = pow(double(original.vn), 0.333);
            init_radius = init_para * diagonal_length / original_size;
        }
    }

 
  global_paraMgr.setGlobalParameter("CGrid Radius", DoubleValue(init_radius));
  global_paraMgr.setGlobalParameter("Initial Radius", DoubleValue(init_radius));

    return init_radius;
}

if (input_->points.size() > 3)
        {
            double scale = 0.0;
            std::vector<double> ptScales;
            pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
            tree->setInputCloud(input_);
            for (size_t i = 0; i < input_->points.size(); i++)
            {
                double x = input_->points.at(i).x;
                double y = input_->points.at(i).y;
                std::vector<int> nn_indices;
                std::vector<float> nn_dists;
                const pcl::PointXYZ pt = input_->at(i);
                if (tree->nearestKSearch(pt, 5, nn_indices, nn_dists) == 0)//有错误，程序无法进入
                {
                    continue;
                }
                double dist = sqrt(nn_dists[1]);//sqrt(nn_dists[3]);为啥是3？？
                ptScales.push_back(dist);
                scale += dist;
            }

            std::sort(ptScales.begin(), ptScales.end(), [](const double& lhs, const double& rhs) { return lhs < rhs; });
            int idxNinety = min(int(double(ptScales.size()) * 0.9), int(ptScales.size() - 1));
            gridSideLength = ptScales[idxNinety] * 0.75;
            scale = scale / input_->points.size();
            thLineLength = 5 * scale;//thLineLength = 8 * scale;
        }