点云法向量计算方式不同,结果相差比较大

试了PCL中点云的法向量计算:

// 待计算的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>)
int cld_sz = cloud_xyz->points.size();
pcl::search::Search<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>());
int search_k = 10;
// 整体一起算
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud_xyz);
ne.setSearchMethod(kdtree);
ne.setKSearch(search_k);
ne.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max());
pcl::PointCloud<pcl::Normal>::Ptr normals_1(new pcl::PointCloud<pcl::Normal>);
ne.compute(*normals_1);
// 单个计算: 用pcl的函数,逐点计算;自己组建协方差矩阵,用Eigen求解计算。
kdtree->setInputCloud(cloud_xyz);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne1;
std::vector< std::vector< int > > idx_all(cld_sz);
std::vector< std::vector< float > > dis_all(cld_sz);
std::vector< Eigen::Vector4f > ppara(cld_sz);
std::vector< float > curva(cld_sz);
std::vector< float > difangle(cld_sz), difcurva(cld_sz);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
normals->points.resize(cld_sz);
float max_dif = -1, max_difc = -10;
for (int i = 0; i < cld_sz; i++)
	{
		idx_all[i].resize(search_k);
		dis_all[i].resize(search_k);
		if (kdtree->nearestKSearch(i, search_k, idx_all[i], dis_all[i]) > 0)
		{
			//计算中心点坐标
			double curCenterX = 0, curCenterY = 0, curCenterZ = 0;
			//去除自身,索引从1开始
			for (int j = 1; j != search_k; ++j)
			{
				curCenterX += cloud_xyz->points[idx_all[i][j]].x;
				curCenterY += cloud_xyz->points[idx_all[i][j]].y;
				curCenterZ += cloud_xyz->points[idx_all[i][j]].z;
			}
			curCenterX = curCenterX / (search_k - 1);
			curCenterY = curCenterY / (search_k - 1);
			curCenterZ = curCenterZ / (search_k - 1);
			//计算协方差矩阵
			double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
			for (int j = 1; j != search_k; ++j)
			{
				xx += (cloud_xyz->points[idx_all[i][j]].x - curCenterX)*(cloud_xyz->points[idx_all[i][j]].x - curCenterX);
				xy += (cloud_xyz->points[idx_all[i][j]].x - curCenterX)*(cloud_xyz->points[idx_all[i][j]].y - curCenterY);
				xz += (cloud_xyz->points[idx_all[i][j]].x - curCenterX)*(cloud_xyz->points[idx_all[i][j]].z - curCenterZ);
				yy += (cloud_xyz->points[idx_all[i][j]].y - curCenterY)*(cloud_xyz->points[idx_all[i][j]].y - curCenterY);
				yz += (cloud_xyz->points[idx_all[i][j]].y - curCenterY)*(cloud_xyz->points[idx_all[i][j]].z - curCenterZ);
				zz += (cloud_xyz->points[idx_all[i][j]].z - curCenterZ)*(cloud_xyz->points[idx_all[i][j]].z - curCenterZ);
			}
			Eigen::Matrix3f covMat(3, 3);
			covMat(0, 0) = xx / (search_k - 1);
			covMat(0, 1) = covMat(1, 0) = xy / (search_k - 1);
			covMat(0, 2) = covMat(2, 0) = xz / (search_k - 1);
			covMat(1, 1) = yy / (search_k - 1);
			covMat(1, 2) = covMat(2, 1) = yz / (search_k - 1);
			covMat(2, 2) = zz / (search_k - 1);
			Eigen::EigenSolver<Eigen::Matrix3f> es(covMat);
			Eigen::Matrix3f val = es.pseudoEigenvalueMatrix();
			Eigen::Matrix3f vec = es.pseudoEigenvectors();
			//统计最小特征值及对应的位置,首先默认为第一个特征值为最小值
			double t1 = val(0, 0);
			unsigned short ii = 0;
			//如果第一个特征值比第二个特征值大,则第二个特征值为最小
			if (t1 > val(1, 1))
			{
				ii = 1;
				t1 = val(1, 1);
			}
			//如果第二个特征值比第三个特征值大,则第三个特征值为最小
			if (t1 > val(2, 2))
			{
				ii = 2;
				t1 = val(2, 2);
			}
			Eigen::Vector3f v_n(vec(0, ii), vec(1, ii), vec(2, ii));
			v_n = v_n / v_n.norm();
			if (v_n(2) < 0) v_n = -v_n;
			normals->points[i].normal_x = v_n(0);
			normals->points[i].normal_y = v_n(1);
			normals->points[i].normal_z = v_n(2);
			normals->points[i].curvature = t1 / (val(0, 0) + val(1, 1) + val(2, 2));
			ne1.computePointNormal(*cloud_xyz, idx_all[i], ppara[i], curva[i]);
			Eigen::Vector3f v_n1(ppara[i](0), ppara[i](1), ppara[i](2));
			v_n1 = v_n1 / v_n1.norm();
			if (v_n1(2) < 0) v_n1 = -v_n1;
			difangle[i] = acos(abs(v_n1.dot(v_n))) * 180 / M_PI;
			difcurva[i] = abs(abs(normals->points[i].curvature) - abs(normals_1->points[i].curvature));
			if (difangle[i] > max_dif) max_dif = difangle[i];
			if (difcurva[i] > max_difc) max_difc = difcurva[i];
		}
	}

计算后作差,发现PCL整体一起算和单点计算的法向量夹角差在10^-2级别,可以忽略不计,但是自己组建协方差和PCL组建协方差的方式不一样(PCL协方差矩阵组建见源码,相关推导见https://blog.csdn.net/lming_08/article/details/21171491),计算出来的法向夹角差别最大能达到40多°,曲率差能达到0.08,但理论上两种方式都是正确的,为什么差别这么大还未知,如果有大神知道,麻烦留言告知,谢谢。

  • 1
    点赞
  • 9
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值