slam中地图地图原点坐标系或world或grid坐标系在map坐标系下的转换关系

 slam中地图地图原点坐标系或world或grid坐标系在map坐标系下的坐标计算

Matrix<double, 4, 4> m_world_to_map; //amcl3 地图world或grid坐标系或地图原点坐标系在map坐标系下的关系,即: mapTgrid =  mapTworld
ros::Subscriber m_sub_map_topic = m_node_handle.subscribe("/map", 1, &NeoLocalizationNode::map_callback, this);	

void map_callback(const nav_msgs::OccupancyGrid::ConstPtr& ros_map)
	{
		std::lock_guard<std::mutex> lock(m_node_mutex);

		ROS_INFO_STREAM("NeoLocalizationNode: Got new map with dimensions " << ros_map->info.width << " x " << ros_map->info.height
				<< " and cell size " << ros_map->info.resolution);

		{
			tf::Transform tmp;
            //地图原点，小车启点
			tf::poseMsgToTF(ros_map->info.origin, tmp);
            //mapTgrid =  mapTworld
			 m_world_to_map = convert_transform_25(tmp);
		}

	}

计算小车base_link在world或grid坐标系或地图原点坐标系下的坐标

const Matrix<double, 4,4>gridTbase_link=mapTgrid.inverse()*convert_transform_25(mapTodom)*odomTbase_link;

/*
   amcl3
 * Converts ROS 3D Transform to a 2.5D matrix.
 * 这个函数的目的是将 ROS 的 3D 变换表示（由 tf::Transform 类型表示）转换为一个 4x4 的矩阵。在这个转换中，
 * Z 轴方向的旋转角度被映射到了矩阵的第三行第三列，而其他变换分量则映射到了矩阵的其他位置。
 */
inline
Matrix<double, 4, 4> convert_transform_25(const tf::Transform& trans)
{
	Matrix<double, 4, 4> res;// 创建一个 4x4 的双精度浮点型矩阵
     将变换的旋转部分的 X、Y 分量分别存入矩阵的第一行和第二行
	res(0, 0) = trans.getBasis()[0][0];
	res(1, 0) = trans.getBasis()[1][0];
    // // 将变换的旋转部分的 X、Y 分量分别存入矩阵的第二行和第一行（交换）
	res(0, 1) = trans.getBasis()[0][1];
	res(1, 1) = trans.getBasis()[1][1];
    // 将变换的平移部分的 X、Y、Z 分量分别存入矩阵的第一、二、三行的第四列
	res(0, 3) = trans.getOrigin()[0];
	res(1, 3) = trans.getOrigin()[1];
	res(2, 3) = tf::getYaw(trans.getRotation());// 获取变换的旋转部分的偏航角并存入矩阵的第三行第四列
	res(2, 2) = 1;// 设置矩阵的第三行第三列为1（表示常量）
	res(3, 3) = 1;// 设置矩阵的第四行第四列为1（表示常量）
	return res;// 返回转换后的矩阵
}

/*
 * Converts ROS 3D Transform to a 3D matrix.
 *  * 这个函数的目的是将 ROS 的 3D 变换表示（由 tf::Transform 类型表示）转换为一个 4x4 的矩阵。在这个转换中，
 * 变换的旋转部分的矩阵元素被映射到矩阵的前三行前三列，而变换的平移部分则映射到矩阵的第一、二、三行的第四列。
 */
inline
Matrix<double, 4, 4> convert_transform_3(const tf::Transform& trans)
{
	Matrix<double, 4, 4> res; 创建一个 4x4 的双精度浮点型矩阵
	 使用双重循环将变换的旋转部分的矩阵元素存入矩阵的前三行前三列
	for(int j = 0; j < 3; ++j) {
		for(int i = 0; i < 3; ++i) {
			res(i, j) = trans.getBasis()[i][j];
		}
	}
    // 将变换的平移部分的 X、Y、Z 分量分别存入矩阵的第一、二、三行的第四列
	res(0, 3) = trans.getOrigin()[0];
	res(1, 3) = trans.getOrigin()[1];
	res(2, 3) = trans.getOrigin()[2];
	res(3, 3) = 1;// 设置矩阵的第四