【坐标变化】左乘右乘

1. 左乘右乘（方法1）

 // tq_w_bHand - px,py,pz,qw,qx,qy,qz
// 1. 计算P^c//
            // P^c = (T_w_bHead * T_bHead_c)^{-1} * T_w_bHand * T_bHand_gp * P^{gp}
            // T_w_bHead
            Eigen::Vector3d t_w_bHead;
            t_w_bHead << m_tq_w_bHead[0], m_tq_w_bHead[1], m_tq_w_bHead[2];
            Eigen::Quaterniond q_w_bHead(m_tq_w_bHead[3], m_tq_w_bHead[4],
                                         m_tq_w_bHead[5], m_tq_w_bHead[6]);

            // T_bHead_c
            Eigen::Vector3d t_bHead_c;
            t_bHead_c << m_tq_bHead_c[0], m_tq_bHead_c[1], m_tq_bHead_c[2];
            Eigen::Quaterniond q_bHead_c(m_tq_bHead_c[3], m_tq_bHead_c[4],
                                         m_tq_bHead_c[5], m_tq_bHead_c[6]);

            // T_w_bHand
            Eigen::Vector3d t_w_bHand(tq_w_bHand[0], tq_w_bHand[1], tq_w_bHand[2]);
            Eigen::Quaterniond q_w_bHand(tq_w_bHand[3], tq_w_bHand[4], tq_w_bHand[5], tq_w_bHand[6]);

            // T_bHand_gp
            Eigen::Vector3d t_bHand_gp;
            t_bHand_gp << m_tq_bHand_gp[0], m_tq_bHand_gp[1], m_tq_bHand_gp[2];
            Eigen::Quaterniond q_bHand_gp(m_tq_bHand_gp[3], m_tq_bHand_gp[4],
                                          m_tq_bHand_gp[5], m_tq_bHand_gp[6]);

            // T_c_w
            Eigen::Matrix3d R_w_c = q_w_bHead.toRotationMatrix() * q_bHead_c.toRotationMatrix();
            Eigen::Vector3d t_w_c = t_w_bHead + q_w_bHead.toRotationMatrix() * t_bHead_c;
            Eigen::Matrix3d R_c_w = R_w_c.transpose();
            Eigen::Vector3d t_c_w = -R_w_c.transpose() * t_w_c;

            // T_w_gp
            Eigen::Matrix3d R_w_gp = q_w_bHand.toRotationMatrix() * q_bHand_gp.toRotationMatrix();
            Eigen::Vector3d t_w_gp = t_w_bHand + q_w_bHand.toRotationMatrix() * t_bHand_gp;

            // T_c_gp
            Eigen::Matrix3d R_c_gp;
            Eigen::Vector3d t_c_gp;
            R_c_gp = R_c_w * R_w_gp;
            t_c_gp = t_c_w + R_c_w * t_w_gp;

            // P^c = T_c_gp * P^{gp}
            Eigen::Vector4d P_gp_hom;
            P_gp_hom << m_P.x, m_P.y, m_P.z, 1.0;

            Eigen::Matrix<double, 3, 4> T_c_gp;
            T_c_gp.block(0, 0, 3, 3) = R_c_gp;
            T_c_gp.block(0, 3, 3, 1) = t_c_gp;
            Eigen::Matrix<double, 3, 1> P_c = T_c_gp * P_gp_hom;
            

         

2. 左乘右乘（方法2）

#include 
#include 

// 定义给定的变换矩阵 T_f_fb
Eigen::Matrix4d T_f_fb;
// 初始化 T_f_fb，假设它是一个有效的变换矩阵

// 定义fbutton坐标系下的landmarks[pid]
Eigen::Vector3d landmarks_fbutton(x_fbutton, y_fbutton, z_fbutton);

// 进行坐标变换
Eigen::Vector4d landmarks_ftop_homogeneous;
landmarks_ftop_homogeneous << landmarks_fbutton, 1.0; // 将三维向量转换为齐次坐标

Eigen::Vector4d landmarks_ftop = T_f_fb * landmarks_ftop_homogeneous;

// 提取ftop坐标系下的xyz值
Eigen::Vector3d xyz_ftop = landmarks_ftop.head<3>() / landmarks_ftop(3); // 归一化

// 输出结果
std::cout << "xyz_ftop: " << xyz_ftop << std::endl;

在这个示例代码中，我们假设你已经定义并初始化了变换矩阵T_f_fb，它表示从fbutton坐标系到ftop坐标系的变换。

首先，我们将fbutton坐标系下的x、y、z坐标存储在一个Vector3d类型的变量landmarks_fbutton中。然后，我们通过将landmarks_fbutton转换为齐次坐标，将其存储在一个Vector4d类型的变量landmarks_ftop_homogeneous中。

接下来，我们通过左乘变换矩阵T_f_fb，将landmarks_ftop_homogeneous从fbutton坐标系转换到ftop坐标系下。这将得到一个Vector4d类型的结果landmarks_ftop。

最后，我们从landmarks_ftop中提取前三个元素，并通过除以齐次坐标中的缩放因子（第4个元素），将其转换为非齐次坐标。这样，我们得到了在ftop坐标系下的xyz坐标xyz_ftop。

请注意，这只是一个简单的示例代码，假设你已经具备了变换矩阵和坐标的相关知识，并使用了Eigen C++库来进行矩阵运算。在实际应用中，你需要根据具体的问题和数据结构进行适当的调整和修改。