C++/Eigen实现简单的坐标系转换、罗德里格公共点转换

C++示例代码

#include 

#include 
#include 
#include 
#include 
#include 

#include 
#include 

/**
 * @brief input: 子坐标系的point{ x0, y0, z0 }, 子坐标系与零点坐标系旋转参数route{ rx, ry, rz }，output: 零点坐标系的点point{ x1, y1, z1 }
 *
 * @param point 子坐标系的点
 * @param route 子坐标系与零点坐标系旋转参数
 * @param origin 子坐标系在零点坐标系下的原点坐标
 * @return Eigen::Vector3d 零点坐标系下的点坐标
 */
inline Eigen::Vector3d Transform(const Eigen::Vector3d& point,
    const Eigen::Vector3d& route,
    const Eigen::Vector3d& origin,
    double scale = 1)
{

    auto [x0, y0, z0] = std::make_tuple(point.x(), point.y(), point.z());
    auto [rx, ry, rz] = std::make_tuple(route.x(), route.y(), route.z());
    auto [x, y, z] = std::make_tuple(origin.x(), origin.y(), origin.z());

    double A[9];
    A[0] = cos(ry) * cos(rz);
    A[1] = -cos(ry) * sin(rz);
    A[2] = sin(ry);
    A[3] = sin(rx) * sin(ry) * cos(rz) + cos(rx) * sin(rz);
    A[4] = -sin(rx) * sin(ry) * sin(rz) + cos(rx) * cos(rz);
    A[5] = -sin(rx) * cos(ry);
    A[6] = -cos(rx) * sin(ry) * cos(rz) + sin(rx) * sin(rz);
    A[7] = cos(rx) * sin(ry) * sin(rz) + sin(rx) * cos(rz);
    A[8] = cos(rx) * cos(ry);

    double x1 = (A[0] * x0 + A[1] * y0 + A[2] * z0) / scale + x;
    double y1 = (A[3] * x0 + A[4] * y0 + A[5] * z0) / scale + y;
    double z1 = (A[6] * x0 + A[7] * y0 + A[8] * z0) / scale + z;
    return { x1, y1, z1 };
}

/**
 * @brief input: 零点坐标系的point{ x0, y0, z0 }, 子坐标系与零点坐标系旋转参数route{ rx, ry, rz }，output: 子坐标系的点point{ x1, y1, z1 }
 *
 * @param point 零点坐标系的点
 * @param route 子坐标系与零点坐标系旋转参数
 * @return Eigen::Vector3d 子坐标系的点坐标
 */
inline Eigen::Vector3d ReTransform(Eigen::Vector3d point,
    Eigen::Vector3d route,
    Eigen::Vector3d origin,
    double scale = 1)
{
    auto [x0, y0, z0] = std::make_tuple(point.x(), point.y(), point.z());
    auto [rx, ry, rz] = std::make_tuple(route.x(), route.y(), route.z());
    auto [x, y, z] = std::make_tuple(origin.x(), origin.y(), origin.z());
    double x1 = 0, y1 = 0, z1 = 0;

    double A[9];
    A[0] = cos(ry) * cos(rz);
    A[1] = -cos(ry) * sin(rz);
    A[2] = sin(ry);
    A[3] = sin(rx) * sin(ry) * cos(rz) + cos(rx) * sin(rz);
    A[4] = -sin(rx) * sin(ry) * sin(rz) + cos(rx) * cos(rz);
    A[5] = -sin(rx) * cos(ry);
    A[6] = -cos(rx) * sin(ry) * cos(rz) + sin(rx) * sin(rz);
    A[7] = cos(rx) * sin(ry) * sin(rz) + sin(rx) * cos(rz);
    A[8] = cos(rx) * cos(ry);

    x1 = (A[0] * (x0 - x) + A[3] * (y0 - y) + A[6] * (z0 - z)) * scale;
    y1 = (A[1] * (x0 - x) + A[4] * (y0 - y) + A[7] * (z0 - z)) * scale;
    z1 = (A[2] * (x0 - x) + A[5] * (y0 - y) + A[8] * (z0 - z)) * scale;

    return { x1, y1, z1 };
}

/**
 * @brief 转换点到零点坐标系
 * @param point 子坐标系下的点
 * @param axis 零点坐标系下的子坐标系
 * @param return x,y,z
 */
QVector3D TransformAxis(QVector3D& point, QVector3D axisO, QVector3D axisR)
{
    auto v3 = Transform({ point.x(), point.y(), point.z() }, { axisR.x(), axisR.y(), axisR.z() }, { axisO.x(), axisO.y(), axisO.z() });
    QVector3D p;
    p.setX(v3.x());
    p.setY(v3.y());
    p.setZ(v3.z());
    return p;
}

/**
 * @brief 零坐标系转到子坐标系
 * @param point 零坐标系下的点坐标
 * @param axisO 子坐标系在零坐标系下的位置
 * @param axisR 子坐标系与零坐标系的旋转参数
 * @param return x,y,z
 */
QVector3D ReTransformAxis(const QVector3D& point, const QVector3D& axisO, const QVector3D& axisR)
{
    auto v3 = ReTransform({ point.x(), point.y(), point.z() }, { axisR.x(), axisR.y(), axisR.z() }, { axisO.x(), axisO.y(), axisO.z() });
    QVector3D p;
    p.setX(v3.x());
    p.setY(v3.y());
    p.setZ(v3.z());
    return p;
}

/**
 * @brief 根据旋转矩阵计算rx,ry,rz
 * @param R 旋转矩阵
 * @return rx,ry,rz
 */
std::tuple<double, double, double> GetEulerAngles(const Eigen::MatrixXd& R)
{
    return { atan2(R(2, 1), R(2, 2)),
        atan2(-R(2, 0), sqrt(R(2, 1) * R(2, 1) + R(2, 2) * R(2, 2))),
        atan2(R(1, 0), R(0, 0)) };
}

/**
 * @brief 罗德里格矩阵模型，根据公共点计算旋转矩阵与平移矩阵
 * @param src 源点
 * @param dst 目标点
 * @param R 计算出的旋转矩阵
 * @param t 计算出的平移矩阵目标点
 * @param scale 计算出的缩放比例
 */
void RodriguesMatrixModel(const Eigen::MatrixXd& src, const Eigen::MatrixXd& dst, Eigen::MatrixXd& R, Eigen::Vector3d& t, float& scale)
{
    // 计算源点和目标点的质心
    Eigen::Vector3d src_mean = src.colwise().mean();
    Eigen::Vector3d dst_mean = dst.colwise().mean();

    // 计算去质心后的坐标矩阵
    Eigen::MatrixXd src_demean = src.rowwise() - src_mean.transpose();
    Eigen::MatrixXd dst_demean = dst.rowwise() - dst_mean.transpose();

    // 计算每行的平方和并开根号
    Eigen::VectorXd src_norm = src_demean.rowwise().norm();
    Eigen::VectorXd dst_norm = dst_demean.rowwise().norm();

    // 计算尺度因子
    scale = dst_norm.sum() / src_norm.sum();

    // 公共点转换罗德里格模型计算旋转矩阵
    Eigen::MatrixXd H = src_demean.transpose() * dst_demean;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    R = svd.matrixV() * svd.matrixU().transpose();

    // 计算平移矩阵
    t = dst_mean - scale * R * src_mean;
}
int main(int argc, char* argv[])
{
    QCoreApplication a(argc, argv);

    Eigen::MatrixXd src(4, 3), dst(4, 3);
    src << -4.39564,
        -3.4181,
        -1.35831,
        -3.66698,
        -4.76856,
        -1.36094,
        -1.40378,
        -4.75369,
        -1.35691,
        0.469564,
        -1.93813,
        -1.3715;

    dst << -4.489,
        -3.42713,
        -1.35089,
        -3.62411,
        -4.67908,
        -1.36181,
        -1.36258,
        -4.41787,
        -1.36174,
        0.16377,
        -1.42107,
        -1.36566;

    Eigen::MatrixXd R;
    Eigen::Vector3d t;
    float scale;
    RodriguesMatrixModel(src, dst, R, t, scale);

    // 保留三位小数输出
    qDebug() << "R:" << R(0, 0) << R(0, 1) << R(0, 2) << '\n'
             << R(1, 0) << R(1, 1) << R(1, 2) << '\n'
             << R(2, 0) << R(2, 1) << R(2, 2) << '\n';

    // 平移矩阵
    qDebug() << "t:" << t(0) << t(1) << t(2);

    qDebug() << "s:" << scale;

    // rx ry rz
    auto&& [rx, ry, rz] = GetEulerAngles(R);

    qDebug() << "rx:" << rx << "ry:" << ry << "rz:" << rz << '\n';
    auto&& p = Transform({
                             -4.39564,
                             -3.4181,
                             -1.35831,
                         },
        { rx, ry, rz }, { t(0), t(1), t(2) });
    qDebug() << "测试:" << p(0) << p(1) << p(2);
    return a.exec();
}

结果