Eigen::Vector3f p_0(2,3, 4); Eigen::Vector3f p_2; Eigen::Vector3f p_3; p_2 = tracking_frame.lidar2origin_trans * p_0; p_3 = tracking_frame.lidar2origin_trans.inverse() * p_2; for (int i = 0; i <3;i++) { std::cout<< p_2[i] << " "<< p_3[i]; }

这段代码使用了Eigen库中的Vector3f类来进行3D向量的计算。首先定义了一个名为p_0的Vector3f对象，表示一个三维向量(2,3,4)。接着又定义了两个空的Vector3f对象p_2和p_3。然后通过将p_0乘以一个变换矩阵tracking_frame.lidar2origin_trans来得到p_2，表示将p_0从lidar坐标系转换到origin坐标系。接着再将p_2乘以tracking_frame.lidar2origin_trans的逆矩阵来得到p_3，表示将p_2从origin坐标系转换回lidar坐标系。最后通过循环输出p_2和p_3的每一个元素。