Question

我的代码存在问题。

我从激光扫描仪获取点云，并且通过这种方法，我想计算一个范围窗口内曲率变化多少的度量。为此，我试图执行反距离加权平均。问题是变量tot_distance，初始化为0的float变为无穷大。

pointRadiusSquaredDistance由我已经检查的radiusSearch正确填写。

{   
double range = 0.2;
double th = 1;
for (int i=0; i < point_cloud->points.size(); i++){
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;


    PointTypeFull p = point_cloud->points[i];
    Eigen::Map<const Eigen::Vector3f> norm_p = p.normal;
    pcl17::PointXYZ point(p.x, p.y, p.z);

    int n_points = search_tree->radiusSearch(point,range,pointIdxRadiusSearch,pointRadiusSquaredDistance);

    float tot_distance = 0;

    if (n_points > 0){
        for(int j=0; j<pointRadiusSquaredDistance.size(); j++){
                    tot_distance += 1/pointRadiusSquaredDistance[j];


        }


        Eigen::Vector3f av_grad = Eigen::Vector3f(0.0,0.0,0.0);

        for(int j=0; j<pointIdxRadiusSearch.size(); j++){
                float weight = (1/pointRadiusSquaredDistance[j])/tot_distance;

                Eigen::Map<const Eigen::Vector3f> norm_j = point_cloud->points[pointIdxRadiusSearch[j]].normal;
                Eigen::Vector3f diff = norm_p - norm_j;
                for (int w= 0; w<3; w++){
                    if (diff(w) < 0)
                            diff(w) = -1*diff(w);
                }
                av_grad += weight*diff;

        }
        float norm = av_grad.norm();

        if (norm>th)
            point_clud_curvature->push_back(p);

    }

}

Answer 1

确保您获得tot_distance＆gt; 0.0之后，

if (n_points > 0){
    for(int j=0; j<pointRadiusSquaredDistance.size(); j++){
                tot_distance += 1/pointRadiusSquaredDistance[j];
    }
cout<<tot_distance; // should be > 0.0

浮点变量变为无穷大

1 个答案: