PCL中点云BoundingBox包围盒绘制（基于PCA）

！！！实现环境：pcl1.8.0+vs2015+win10

大致过程：

1、利用PCA主元分析法获得点云的三个主方向，获取质心，计算协方差，获得协方差矩阵，求取协方差矩阵的特征值和特长向量，特征向量即为主方向。

1. Eigen::Vector4f pcaCentroid;

2. pcl::compute3DCentroid(*cloud, pcaCentroid);

3. Eigen::Matrix3f covariance;

4. pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);

5. Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);

6. Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();

7. Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();

8. eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直

9. eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));

10. eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

2、利用1中获得的主方向和质心，将输入点云转换至原点，且主方向与坐标系方向重回，建立变换到原点的点云的包围盒。

3、给输入点云设置主方向和包围盒，通过输入点云到原点点云变换的逆变换实现。

4、完整代码：

1. #include <vtkAutoInit.h>

2. VTK_MODULE_INIT(vtkRenderingOpenGL);

3. VTK_MODULE_INIT(vtkInteractionStyle);

4. VTK_MODULE_INIT(vtkRenderingFreeType);

5.  

6. #include <iostream>

7. #include <string>

8. #include <pcl/io/pcd_io.h>

9. #include <pcl/point_cloud.h>

10. #include <pcl/point_types.h>

11. #include <Eigen/Core>

12. #include <pcl/common/transforms.h>

13. #include <pcl/common/common.h>

14. #include <pcl/visualization/pcl_visualizer.h>

15.  

16. using namespace std;

17. typedef pcl::PointXYZ PointType;

18.  

19. int main(int argc, char **argv)

20. {

21. pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>());

22.  

23. std::cout << "请输入需要显示的点云文件名：";

24. std::string fileName("rabbit");

25. getline(cin, fileName);

26. fileName += ".pcd";

27. //std::string fileName(argv[1]);

28. pcl::io::loadPCDFile(fileName, *cloud);

29.  

30. Eigen::Vector4f pcaCentroid;

31. pcl::compute3DCentroid(*cloud, pcaCentroid);

32. Eigen::Matrix3f covariance;

33. pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentroid, covariance);

34. Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);

35. Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();

36. Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();

37. eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //校正主方向间垂直

38. eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));

39. eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));

40.  

41. std::cout << "特征值va(3x1):\n" << eigenValuesPCA << std::endl;

42. std::cout << "特征向量ve(3x3):\n" << eigenVectorsPCA << std::endl;

43. std::cout << "质心点(4x1):\n" << pcaCentroid << std::endl;

44. /*

45. // 另一种计算点云协方差矩阵特征值和特征向量的方式:通过pcl中的pca接口，如下，这种情况得到的特征向量相似特征向量

46. pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPCAprojection (new pcl::PointCloud<pcl::PointXYZ>);

47. pcl::PCA<pcl::PointXYZ> pca;

48. pca.setInputCloud(cloudSegmented);

49. pca.project(*cloudSegmented, *cloudPCAprojection);

50. std::cerr << std::endl << "EigenVectors: " << pca.getEigenVectors() << std::endl;//计算特征向量

51. std::cerr << std::endl << "EigenValues: " << pca.getEigenValues() << std::endl;//计算特征值

52. */

53. Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();

54. Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();

55. tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose(); //R.

56. tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) *(pcaCentroid.head<3>());// -R*t

57. tm_inv = tm.inverse();

58.  

59. std::cout << "变换矩阵tm(4x4):\n" << tm << std::endl;

60. std::cout << "逆变矩阵tm'(4x4):\n" << tm_inv << std::endl;

61.  

62. pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);

63. pcl::transformPointCloud(*cloud, *transformedCloud, tm);

64.  

65. PointType min_p1, max_p1;

66. Eigen::Vector3f c1, c;

67. pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);

68. c1 = 0.5f*(min_p1.getVector3fMap() + max_p1.getVector3fMap());

69.  

70. std::cout << "型心c1(3x1):\n" << c1 << std::endl;

71.  

72. Eigen::Affine3f tm_inv_aff(tm_inv);

73. pcl::transformPoint(c1, c, tm_inv_aff);

74.  

75. Eigen::Vector3f whd, whd1;

76. whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();

77. whd = whd1;

78. float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3; //点云平均尺度，用于设置主方向箭头大小

79.  

80. std::cout << "width1=" << whd1(0) << endl;

81. std::cout << "heght1=" << whd1(1) << endl;

82. std::cout << "depth1=" << whd1(2) << endl;

83. std::cout << "scale1=" << sc1 << endl;

84.  

85. const Eigen::Quaternionf bboxQ1(Eigen::Quaternionf::Identity());

86. const Eigen::Vector3f bboxT1(c1);

87.  

88. const Eigen::Quaternionf bboxQ(tm_inv.block<3, 3>(0, 0));

89. const Eigen::Vector3f bboxT(c);

90.  
91.  

92. //变换到原点的点云主方向

93. PointType op;

94. op.x = 0.0;

95. op.y = 0.0;

96. op.z = 0.0;

97. Eigen::Vector3f px, py, pz;

98. Eigen::Affine3f tm_aff(tm);

99. pcl::transformVector(eigenVectorsPCA.col(0), px, tm_aff);

100. pcl::transformVector(eigenVectorsPCA.col(1), py, tm_aff);

101. pcl::transformVector(eigenVectorsPCA.col(2), pz, tm_aff);

102. PointType pcaX;

103. pcaX.x = sc1 * px(0);

104. pcaX.y = sc1 * px(1);

105. pcaX.z = sc1 * px(2);

106. PointType pcaY;

107. pcaY.x = sc1 * py(0);

108. pcaY.y = sc1 * py(1);

109. pcaY.z = sc1 * py(2);

110. PointType pcaZ;

111. pcaZ.x = sc1 * pz(0);

112. pcaZ.y = sc1 * pz(1);

113. pcaZ.z = sc1 * pz(2);

114.  
115.  

116. //初始点云的主方向

117. PointType cp;

118. cp.x = pcaCentroid(0);

119. cp.y = pcaCentroid(1);

120. cp.z = pcaCentroid(2);

121. PointType pcX;

122. pcX.x = sc1 * eigenVectorsPCA(0, 0) + cp.x;

123. pcX.y = sc1 * eigenVectorsPCA(1, 0) + cp.y;

124. pcX.z = sc1 * eigenVectorsPCA(2, 0) + cp.z;

125. PointType pcY;

126. pcY.x = sc1 * eigenVectorsPCA(0, 1) + cp.x;

127. pcY.y = sc1 * eigenVectorsPCA(1, 1) + cp.y;

128. pcY.z = sc1 * eigenVectorsPCA(2, 1) + cp.z;

129. PointType pcZ;

130. pcZ.x = sc1 * eigenVectorsPCA(0, 2) + cp.x;

131. pcZ.y = sc1 * eigenVectorsPCA(1, 2) + cp.y;

132. pcZ.z = sc1 * eigenVectorsPCA(2, 2) + cp.z;

133.  
134.  

135. //visualization

136. pcl::visualization::PCLVisualizer viewer;

137.  

138. pcl::visualization::PointCloudColorHandlerCustom<PointType> tc_handler(transformedCloud, 0, 255, 0); //转换到原点的点云相关

139. viewer.addPointCloud(transformedCloud, tc_handler, "transformCloud");

140. viewer.addCube(bboxT1, bboxQ1, whd1(0), whd1(1), whd1(2), "bbox1");

141. viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox1");

142. viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "bbox1");

143.  

144. viewer.addArrow(pcaX, op, 1.0, 0.0, 0.0, false, "arrow_X");

145. viewer.addArrow(pcaY, op, 0.0, 1.0, 0.0, false, "arrow_Y");

146. viewer.addArrow(pcaZ, op, 0.0, 0.0, 1.0, false, "arrow_Z");

147.  

148. pcl::visualization::PointCloudColorHandlerCustom<PointType> color_handler(cloud, 255, 0, 0); //输入的初始点云相关

149. viewer.addPointCloud(cloud, color_handler, "cloud");

150. viewer.addCube(bboxT, bboxQ, whd(0), whd(1), whd(2), "bbox");

151. viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "bbox");

152. viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "bbox");

153.  

154. viewer.addArrow(pcX, cp, 1.0, 0.0, 0.0, false, "arrow_x");

155. viewer.addArrow(pcY, cp, 0.0, 1.0, 0.0, false, "arrow_y");

156. viewer.addArrow(pcZ, cp, 0.0, 0.0, 1.0, false, "arrow_z");

157.  

158. viewer.addCoordinateSystem(0.5f*sc1);

159. viewer.setBackgroundColor(1.0, 1.0, 1.0);

160. while (!viewer.wasStopped())

161. {

162. viewer.spinOnce(100);

163. }

164.  

165. return 0;

166. }

注：如有问题请批评指正。

参考资料：

[1] Finding oriented bounding box of a cloud

[2] 计算点云的最小BBOX

 

全文地址请点击：https://blog.youkuaiyun.com/WillWinston/article/details/80196895?utm_source=copy