visualization_msgs::MarkerArray marker_array;
for(int i = 0; i < clusters.size(); i++) {
if(marker_array_pub_.getNumSubscribers() > 0) {
Eigen::Vector4f centroid; // 重心
pcl::compute3DCentroid(*clusters[i], centroid);
PointType min_pt, max_pt;
Eigen::Vector3f center; // 中心
pcl::getMinMax3D(*clusters[i], min_pt, max_pt);
center = (max_pt.getVector3fMap() + min_pt.getVector3fMap())/2;
// 计算协方差矩阵的特征向量
Eigen::Matrix3f covariance;
pcl::computeCovarianceMatrixNormalized(*clusters[i], centroid, covariance); // 这里必须用重心
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
Eigen::Vector3f eigenValuesPCA = eigen_solver.eigenvalues();
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //正交化
eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0));
// 计算变换矩阵,只考虑绕全局坐标系Z轴的转动
Eigen::Vector3f ea = (eigenVectorsPCA).eulerAngles(2, 1, 0);
Eigen::AngleAxisf keep_Z_Rot(ea[0], Eigen::Vector3f::UnitZ());
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translate(center); // translate与rotate书写的顺序不可搞反
transform.rotate(keep_Z_Rot); // radians
//这里主要获得仿射变换矩阵transform3
pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
pcl::transformPointCloud(*clusters[i], *transformedCloud, transform.inverse());
pcl::PointXYZI min_pt_T, max_pt_T;
pcl::getMinMax3D(*transformedCloud, min_pt_T, max_pt_T);
Eigen::Vector3f center_new = (max_pt_T.getVector3fMap() + min_pt_T.getVector3fMap()) / 2;
Eigen::Affine3f transform2 = Eigen::Affine3f::Identity();
transform2.translate(center_new);
Eigen::Affine3f transform3 = transform * transform2;
//初始化
visualization_msgs::Marker marker;
marker.header = ros_pc2_in->header;
marker.header.frame_id="base_link";
marker.ns = "points";
marker.id = i;
marker.type = visualization_msgs::Marker::LINE_LIST;
geometry_msgs::Point p[24];
p[0].x = max_pt_T.x; p[0].y = max_pt_T.y; p[0].z = max_pt_T.z;
p[1].x = min_pt_T.x; p[1].y = max_pt_T.y; p[1].z = max_pt_T.z;
p[2].x = max_pt_T.x; p[2].y = max_pt_T.y; p[2].z = max_pt_T.z;
p[3].x = max_pt_T.x; p[3].y = min_pt_T.y; p[3].z = max_pt_T.z;
p[4].x = max_pt_T.x; p[4].y = max_pt_T.y; p[4].z = max_pt_T.z;
p[5].x = max_pt_T.x; p[5].y = max_pt_T.y; p[5].z = min_pt_T.z;
p[6].x = min_pt_T.x; p[6].y = min_pt_T.y; p[6].z = min_pt_T.z;
p[7].x = max_pt_T.x; p[7].y = min_pt_T.y; p[7].z = min_pt_T.z;
p[8].x = min_pt_T.x; p[8].y = min_pt_T.y; p[8].z = min_pt_T.z;
p[9].x = min_pt_T.x; p[9].y = max_pt_T.y; p[9].z = min_pt_T.z;
p[10].x = min_pt_T.x; p[10].y = min_pt_T.y; p[10].z = min_pt_T.z;
p[11].x = min_pt_T.x; p[11].y = min_pt_T.y; p[11].z = max_pt_T.z;
p[12].x = min_pt_T.x; p[12].y = max_pt_T.y; p[12].z = max_pt_T.z;
p[13].x = min_pt_T.x; p[13].y = max_pt_T.y; p[13].z = min_pt_T.z;
p[14].x = min_pt_T.x; p[14].y = max_pt_T.y; p[14].z = max_pt_T.z;
p[15].x = min_pt_T.x; p[15].y = min_pt_T.y; p[15].z = max_pt_T.z;
p[16].x = max_pt_T.x; p[16].y = min_pt_T.y; p[16].z = max_pt_T.z;
p[17].x = max_pt_T.x; p[17].y = min_pt_T.y; p[17].z = min_pt_T.z;
p[18].x = max_pt_T.x; p[18].y = min_pt_T.y; p[18].z = max_pt_T.z;
p[19].x = min_pt_T.x; p[19].y = min_pt_T.y; p[19].z = max_pt_T.z;
p[20].x = max_pt_T.x; p[20].y = max_pt_T.y; p[20].z = min_pt_T.z;
p[21].x = min_pt_T.x; p[21].y = max_pt_T.y; p[21].z = min_pt_T.z;
p[22].x = max_pt_T.x; p[22].y = max_pt_T.y; p[22].z = min_pt_T.z;
p[23].x = max_pt_T.x; p[23].y = min_pt_T.y; p[23].z = min_pt_T.z;
for(int i = 0; i < 24; i++) {
//将包围盒转回原来的坐标系
Eigen::Vector3f v3f_a(p[i].x, p[i].y, p[i].z);
Eigen::Vector3f v3f_b;
v3f_b=transform3*v3f_a;
p[i].x=v3f_b[0];p[i].y=v3f_b[1];p[i].z=v3f_b[2];
marker.points.push_back(p[i]);
}
marker.scale.x = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 0.5;
marker.lifetime = ros::Duration(0.1);
marker_array.markers.push_back(marker);
}
}
if (marker_array_pub_.getNumSubscribers() != 0){
if(marker_array.markers.size()) {
marker_array_pub_.publish(marker_array);
}
}
>参考
>https://blog.csdn.net/robinvista/article/details/118251128
>https://github.com/yzrobot/adaptive_clustering