运行 feature-based-icp-on-rgb-data实验记录

最新推荐文章于 2024-06-13 10:17:15 发布

svai哔

最新推荐文章于 2024-06-13 10:17:15 发布

阅读量39

点赞数

文章标签： opencv 人工智能计算机视觉

本文链接：https://blog.csdn.net/weixin_43414985/article/details/130450719

版权

运行feature-based-icp-on-rgb-data

ROS中用rosbag记录的数据仿真时发布TF在RVIZ中遇到“Message removed because it is too old”的问题解决

即在launch文件顶部加入：<param name="use_sim_time" value="true"/>

然后在rosbag 节点加入：--clock

这样做的含义是所有node都使用仿真时间而不是ros time

rivz配置并保存

<launch>
    <node pkg="rviz" type="rviz" name="rviz" args="-d ~/catkin_ws/src/test.rviz" required="true" />
</launch>

请添加图片描述

//图像点云配准算法
Eigen::Matrix4f MapRecon::get_transformation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& old_pcl, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& new_pcl){
    Eigen::Matrix4f Trans;  //T
    Trans = Matrix4f::Identity();
		//计算点云质心
        Centroid_A = compute_center_of_mass_xyz(old_pcl, this->idx_xyz[0]);
        //cout << "pointcloud A size: " << old_pcl->points.size()  << "\n "<< "Cemtroid_A: \n" << Cemtroid_A << endl;
        Centroid_B = compute_center_of_mass_xyz(new_pcl, this->idx_xyz[1]);
        //cout << "pointcloud B size: " << new_pcl->points.size() << "\n "<< "Cemtroid_B: \n" << Cemtroid_B << endl;
                                        // (1) Compute Cross Covariance Matrix from xyz points
    	//计算协方差矩阵
        Eigen::Matrix3f cov_A_B = MapRecon::cross_covariance_matrix( old_pcl, new_pcl);

                                        // (2) compute singular value decomposition (SVD) --> openCV2
                                        // A = USV*
                                        // Singular values --> D (Diagonal Matrix)
                                        // svd.singularValues();
                                        // svd.matrixU();  
                                        // svd.matrixV();
    //计算SVD分解，得到旋转矩阵
        Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov_A_B, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::Matrix3f Rotation = svd.matrixU() * (svd.matrixV().transpose());
	//计算平移向量
        Eigen::Vector3f translation = Centroid_A - Rotation * Centroid_B;
        Trans << Rotation(0,0), Rotation(0,1), Rotation(0,2), translation(0),
                 Rotation(1,0), Rotation(1,1), Rotation(1,2), translation(1),
                 Rotation(2,0), Rotation(2,1), Rotation(2,2), translation(2),
                 0,             0,             0,             1; 
    } 
    return Trans;             
}