rosc++ visualization_msgs::MarkerArray obb包围盒

本文主要是介绍rosc++ visualization_msgs::MarkerArray obb包围盒，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

		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

这篇关于rosc++ visualization_msgs::MarkerArray obb包围盒的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！