我对这段代码有疑问。
我正在从激光扫描仪获取点云,我想使用这种方法计算一个范围窗口内曲率变化的量度。为此,我正在尝试执行反距离加权平均。问题是变量 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);
}
}
最佳答案
确保你得到 tot_distance > 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
关于c++ - float 变量变为无穷大,我们在Stack Overflow上找到一个类似的问题: https://stackoverflow.com/questions/20839761/