PclSharp1.12.0--计算OBB方向包围盒体积

西~风

已于 2024-03-04 09:35:50 修改

阅读量1.3k

点赞数 25

分类专栏： PCL 文章标签：算法 c# pcl

于 2024-02-29 09:47:46 首次发布

本文链接：https://blog.csdn.net/swmyaopeng/article/details/136346571

版权

PCL 专栏收录该内容

28 篇文章 27 订阅

订阅专栏

一、OBB包围盒

OBB包围盒，包围盒的棱尽可能贴近点云真实分布，与特征向量平行，也称有向包围盒，或者最小包围盒，计算相比AABB那种复杂一点。OBB像二维图像中的带方向的外接矩，不一定是最小的，能够比较好的包容整个点集。

OBB这种方法是根据物体本身的几何形状来决定盒子的大小和方向，盒子无须和坐标轴垂直。这样就可以选择最合适的最紧凑的包容盒子。OBB盒子的生成比较复杂，一般是考虑物体所有的顶点在空间的分布，通过一定的算法找到最好的方向(OBB盒子的几个轴)。

通过使用 MomentOfInertiaEstimation 类，计算OBB包围盒

二、代码部分

注：测试代码均使用PclSharp1.12.0库

/// <summary>
/// 获取OBB包围盒
/// </summary>
/// <param name="inCloud">源点云</param>
/// <param name="min_point_OBB"></param>
/// <param name="max_point_OBB"></param>
/// <returns></returns>
public static Vector3f GetOBB(PointCloudOfXYZ inCloud, out PointXYZ min_point_OBB, out PointXYZ max_point_OBB, out PointXYZ position_OBB,out Matrix3f rotational_matrix_OBB)
{
Vector3f mass_center = new Vector3f();
min_point_OBB = new PointXYZ();
max_point_OBB = new PointXYZ();
position_OBB = new PointXYZ();
rotational_matrix_OBB = new Matrix3f();
using (MomentOfInertiaEstimationOfXYZ feature_extractor = new MomentOfInertiaEstimationOfXYZ())
{
feature_extractor.SetInputCloud(inCloud);
feature_extractor.Compute();

VectorOfFloat moment_of_inertia = new VectorOfFloat();
VectorOfFloat eccentricity = new VectorOfFloat();
float major_value = 0, middle_value = 0, minor_value = 0;
Vector3f major_vector = new Vector3f();
Vector3f middle_vector = new Vector3f();
Vector3f minor_vector = new Vector3f();
// 获取惯性矩
feature_extractor.getMomentOfInertia(moment_of_inertia);
// 获取离心率
feature_extractor.getEccentricity(eccentricity);
// 获取OBB盒
feature_extractor.getOBB(ref min_point_OBB, ref max_point_OBB, ref position_OBB, rotational_matrix_OBB);

feature_extractor.getEigenValues(ref major_value, ref middle_value, ref minor_value);
// 获取主轴major_vector，中轴middle_vector，辅助轴minor_vector
feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
// 获取质心
feature_extractor.getMassCenter(mass_center);
}
//返回质心坐标
return mass_center;
}

/// 可视化显示点云OBB包围盒
/// </summary>
/// <param name="cloud"></param>
/// <param name="min_OBB"></param>
/// <param name="max_OBB"></param>
/// <param name="postion_OBB"></param>
/// <param name="matrix_OBB"></param>
/// <param name="label"></param>
/// <param name="field"></param>
public void ShowPointCloud_OBB(PointCloudOfXYZ cloud, PointXYZ min_point_OBB, PointXYZ max_point_OBB, PointXYZ postion_OBB, Matrix3f matrix_OBB, string label = "", string field = "z")
{
try
{

this.Invoke(new Action(() =>
{
ShowLogInfo($"{label}--");

cloud_current = Filters.RemoveNaNFromPointCloud(cloud);
visualizerWinForm.visualizer.RemoveAllShapes();
visualizerWinForm.visualizer.RemoveAllPointClouds();
visualizerWinForm.visualizer.SetBackgroundColor(0.3f, 0.3f, 0.3f);
//PointCloudShoWinForm.visualizer.AddPointCloud(pointCloud);
visualizerWinForm.visualizer.AddPointCloudColor(cloud_current, field);
//设置点的大小
visualizerWinForm.visualizer.SetPointCloudRenderingProperties(RenderingProperties.PointSize, 2);
visualizerWinForm.visualizer.AddCoordinateSystem(1.0);

//添加OBB包容盒
Vector3f pos_OBB = new Vector3f(postion_OBB.X, postion_OBB.Y, postion_OBB.Z);
visualizerWinForm.visualizer.AddCube(pos_OBB, matrix_OBB, max_point_OBB.X - min_point_OBB.X, max_point_OBB.Y - min_point_OBB.Y, max_point_OBB.Z - min_point_OBB.Z, "OBB");
visualizerWinForm.visualizer.SetShapeRenderingProperties(RenderingProperties.representation, RenderingRepresentationProperties.PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");

//获取每个角点坐标，利用addLine划线
Vector3f p1 = new Vector3f(min_point_OBB.X, min_point_OBB.Y, min_point_OBB.Z);
Vector3f p2 = new Vector3f(min_point_OBB.X, min_point_OBB.Y, max_point_OBB.Z);
Vector3f p3 = new Vector3f(max_point_OBB.X, min_point_OBB.Y, max_point_OBB.Z);
Vector3f p4 = new Vector3f(max_point_OBB.X, min_point_OBB.Y, min_point_OBB.Z);
Vector3f p5 = new Vector3f(min_point_OBB.X, max_point_OBB.Y, min_point_OBB.Z);
Vector3f p6 = new Vector3f(min_point_OBB.X, max_point_OBB.Y, max_point_OBB.Z);
Vector3f p7 = new Vector3f(max_point_OBB.X, max_point_OBB.Y, max_point_OBB.Z);
Vector3f p8 = new Vector3f(max_point_OBB.X, max_point_OBB.Y, min_point_OBB.Z);
//position:中心位置
Vector3f position =new Vector3f(postion_OBB.X, postion_OBB.Y, postion_OBB.Z);
p1 = matrix_OBB * p1 + position;
p2 = matrix_OBB * p2 + position;
p3 = matrix_OBB * p3 + position;
p4 = matrix_OBB * p4 + position;
p5 = matrix_OBB * p5 + position;
p6 = matrix_OBB * p6 + position;
p7 = matrix_OBB * p7 + position;
p8 = matrix_OBB * p8 + position;

PointXYZ pt1 = new PointXYZ() { X = p1[0], Y = p1[1], Z = p1[2] };
PointXYZ pt2 = new PointXYZ() { X = p2[0], Y = p2[1], Z = p2[2] };
PointXYZ pt3 = new PointXYZ() { X = p3[0], Y = p3[1], Z = p3[2] };
PointXYZ pt4 = new PointXYZ() { X = p4[0], Y = p4[1], Z = p4[2] };
PointXYZ pt5 = new PointXYZ() { X = p5[0], Y = p5[1], Z = p5[2] };
PointXYZ pt6 = new PointXYZ() { X = p6[0], Y = p6[1], Z = p6[2] };
PointXYZ pt7 = new PointXYZ() { X = p7[0], Y = p7[1], Z = p7[2] };
PointXYZ pt8 = new PointXYZ() { X = p8[0], Y = p8[1], Z = p8[2] };

visualizerWinForm.visualizer.AddLine(pt1, pt2, 1.0, 0.0, 0.0, "1 edge");
visualizerWinForm.visualizer.AddLine(pt1, pt4, 1.0, 0.0, 0.0, "2 edge");
visualizerWinForm.visualizer.AddLine(pt1, pt5, 1.0, 0.0, 0.0, "3 edge");
visualizerWinForm.visualizer.AddLine(pt5, pt6, 1.0, 0.0, 0.0, "4 edge");
visualizerWinForm.visualizer.AddLine(pt5, pt8, 1.0, 0.0, 0.0, "5 edge");
visualizerWinForm.visualizer.AddLine(pt2, pt6, 1.0, 0.0, 0.0, "6 edge");
visualizerWinForm.visualizer.AddLine(pt6, pt7, 1.0, 0.0, 0.0, "7 edge");
visualizerWinForm.visualizer.AddLine(pt7, pt8, 1.0, 0.0, 0.0, "8 edge");
visualizerWinForm.visualizer.AddLine(pt2, pt3, 1.0, 0.0, 0.0, "9 edge");
visualizerWinForm.visualizer.AddLine(pt4, pt8, 1.0, 0.0, 0.0, "10 edge");
visualizerWinForm.visualizer.AddLine(pt3, pt4, 1.0, 0.0, 0.0, "11 edge");
visualizerWinForm.visualizer.AddLine(pt3, pt7, 1.0, 0.0, 0.0, "12 edge");

visualizer.AddText3D("p1", pt1, 2, 1, 1, 0);
visualizer.AddText3D("p2", pt2, 2, 1, 1, 0);
visualizer.AddText3D("p3", pt3, 2, 1, 1, 0);
visualizer.AddText3D("p4", pt4, 2, 1, 1, 0);
visualizer.AddText3D("p5", pt5, 2, 1, 1, 0);
visualizer.AddText3D("p6", pt6, 2, 1, 1, 0);
visualizer.AddText3D("p7", pt7, 2, 1, 1, 0);
visualizer.AddText3D("p8", pt8, 2, 1, 1, 0);

//包围盒体积
//计算两点间欧式距离
double L = PclHelper.EuclideanDistance(pt1, pt2);
double W = PclHelper.EuclideanDistance(pt1, pt3);
double H = PclHelper.EuclideanDistance(pt1, pt5);
double xVolume = L * W * H;
ShowLogInfo($"包围盒体积:{xVolume.ToString("f3")}");
//设置相机位置
SetCamera(cloud);

}));
//日志信息显示点云数据
ShowLogInfo($"点个数----{cloud.Count}");
}
catch (Exception ex)
{
MessageBox.Show($"{ex.Message}");
}

}
三、结果显示