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

已于 2023-11-29 15:16:43 修改
阅读量1k 收藏 10
点赞数 10
分类专栏： C/C++ 算法 文章标签： c++ 开发语言
于 2023-06-23 11:20:17 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_44575789/article/details/131348849
版权

C++示例代码

#include <QtCore/QCoreApplication>

#include <QMatrix4x4>
#include <QVector3D>
#include <QVector>
#include <cmath>
#include <tuple>

#include <Eigen/Dense>
#include <QDebug>

std::tuple<Eigen::Vector4d, Eigen::Vector3d> RodriguesMatrixModel(const QList<Eigen::Vector3d>& src, const QList<Eigen::Vector3d>& dst)
{
    assert(src.size() == dst.size());
    Eigen::MatrixXd src_eigen(src.size(), 3);
    Eigen::MatrixXd dst_eigen(dst.size(), 3);
    for (int i = 0; i < src.size(); ++i) {
        src_eigen(i, 0) = src[i].x();
        src_eigen(i, 1) = src[i].y();
        src_eigen(i, 2) = src[i].z();
        dst_eigen(i, 0) = dst[i].x();
        dst_eigen(i, 1) = dst[i].y();
        dst_eigen(i, 2) = dst[i].z();
    }
    // 计算相邻两点之间的距离比
    double src_dist = 0.0, dst_dist = 0.0;
    for (int i = 0; i < src.size() - 1; ++i) {
        src_dist += (src[i] - src[i + 1]).norm();
        dst_dist += (dst[i] - dst[i + 1]).norm();
    }
    double scale = dst_dist / src_dist;

    Eigen::MatrixXd A = Eigen::MatrixXd::Zero((src.size() - 1) * 3, 3);
    Eigen::MatrixXd L = Eigen::MatrixXd::Zero((src.size() - 1) * 3, 1);
    for (size_t i = 1; i < src.size(); i++) {
        Eigen::Matrix3d skew;
        skew << 0, -src_eigen(i, 2) - src_eigen(0, 2) - (dst_eigen(i, 2) - dst_eigen(0, 2)), -src_eigen(i, 1) - src_eigen(0, 1) - (dst_eigen(i, 1) - dst_eigen(0, 1)),
            -src_eigen(i, 2) - src_eigen(0, 2) - (dst_eigen(i, 2) - dst_eigen(0, 2)), 0, src_eigen(i, 0) + src_eigen(0, 0) + (dst_eigen(i, 0) - dst_eigen(0, 0)),
            src_eigen(i, 1) - src_eigen(0, 1) + (dst_eigen(i, 1) - dst_eigen(0, 1)), src_eigen(i, 0) - src_eigen(0, 0) + (dst_eigen(i, 0) - dst_eigen(0, 0)), 0;
        A.block((i - 1) * 3, 0, 3, 3) = skew;

        Eigen::Vector3d tmp;
        tmp << dst_eigen(i, 0) - dst_eigen(0, 0) - (src_eigen(i, 0) - src_eigen(0, 0)), dst_eigen(i, 1) - dst_eigen(0, 1) - (src_eigen(i, 1) - src_eigen(0, 1)),
            dst_eigen(i, 2) - dst_eigen(0, 2) - (src_eigen(i, 2) - src_eigen(0, 2));
        L.block((i - 1) * 3, 0, 3, 1) = tmp;
    }
    Eigen::MatrixXd ATA = A.transpose() * A;
    Eigen::MatrixXd ATA_1 = ATA.inverse();
    Eigen::MatrixXd ATA_1AT = ATA_1 * A.transpose();
    Eigen::MatrixXd X = ATA_1AT * L;
    double a = 0.0, b = 0.0, c = 0.0;
    a = X(0, 0);
    b = X(1, 0);
    c = X(2, 0);
    Eigen::Matrix3d R;
    R << 1 + pow(a, 2) - pow(b, 2) - pow(c, 2), -2 * (c + a * b), 2 * (a * c - b),
        2 * (c - a * b), 1 - pow(a, 2) + pow(b, 2) - pow(c, 2), -2 * (a + b * c),
        2 * (a * c + b), 2 * (a - b * c), 1 - pow(a, 2) - pow(b, 2) + pow(c, 2);
    R = R * (1.0 / (1 + pow(a, 2) + pow(b, 2) + pow(c, 2)));

    // 计算源点和目标点的质心
    Eigen::Vector3d src_mean = src_eigen.colwise().mean();
    Eigen::Vector3d dst_mean = dst_eigen.colwise().mean();
    // 计算平移矩阵
    auto&& t = dst_mean - scale * R * src_mean;

    Eigen::Vector3d eulerAngle1 = R.eulerAngles(0, 1, 2);
    return std::make_tuple(Eigen::Vector4d(eulerAngle1(0), eulerAngle1(1), eulerAngle1(2), scale), t);
}


/**
 * @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 RT，根据公共点计算旋转矩阵与平移矩阵
 * @param src 源点
 * @param dst 目标点
 * @param R 计算出的旋转矩阵
 * @param t 计算出的平移矩阵目标点
 * @param scale 计算出的缩放比例
 */
void RTMatrixModel(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;
    RTMatrixModel(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();
}

结果

在这里插入图片描述

windSnowLi
关注
专栏目录
坐标转换C++自己做的
12-15
public class CoordTrans7Param { public double[,] values=new double[7,1]; //{{dx},{dy},{dz},{rx},{ry},{rz},{k}}; //public double　　 两个坐标系转换一般需要平移，旋转，缩放共七参数。 Y=（1＋k)*M(x,y,z)*X+dX; public double[,] values=new double[7,1]; //{{dx},{dy},{dz},{rx},{ry},{rz},{k}}; //public double dx,dy,dz,rx,ry,rz,k; public void Set4Param(double dx,double dy,double dz,double k) { this.dx=dx; this.dy=dy; this.dz=dz; this.k=k; this.rx=this.ry=this.rz=0; } public void SetRotationParamRad(double rx,double ry,double rz) { this.rx=rx; this.ry=ry; this.rz=rz; } public void SetRotationParamMM(double rx,double ry,double rz) { SetRotationParamRad(rx*Math.PI/648000,ry*Math.PI/648000,rz*Math.PI/648000); } private double[,] GetMx() { double [,] Mx=new double[,] {{1,0,0}, {0,Math.Cos(rx),Math.Sin(rx)}, {0,-Math.Sin(rx),Math.Cos(rx)}}; return Mx; } private double[,] GetMy() { double [,] My=new double[,] {{Math.Cos(ry),0,-Math.Sin(ry)}, {0,1,0}, {Math.Sin(ry),0,Math.Cos(ry)}}; return My; } private double[,] GetMz() { double [,] Mz=new double[,] {{Math.Cos(rz),Math.Sin(rz),0}, {-Math.Sin(rz),Math.Cos(rz),0}, {0,0,1}}; return Mz; } private double[,] GetM() //M=Mx*My*Mz? or M=Mz*My*Mx? { double [,] M=new double[3,3]; MatrixTool.Multi(GetMz(),GetMy(),ref M); MatrixTool.Multi(M,GetMx(),ref M); return M; } private double[,] GetMdx() { double[,] mt = {{ 0, 0, 0 }, { 0, -Math.Sin(rx), Math.Cos(rx) }, { 0, -Math.Cos(rx), -Math.Sin(rx) }}; double[,] m=new double[3,3]; MatrixTool.Multi(GetMz(),GetMy(),ref m); MatrixTool.Multi(m,mt,ref m); return m; } private double[,] GetMdy() { double[,] mt = {{ -Math.Sin(ry), 0, -Math.Cos(ry) }, { 0, 0, 0 }, { Math.Cos(ry), 0, -Math.Sin(ry) }}; double[,] m=new double[3,3]; MatrixTool.Multi(GetMz(),mt,ref m); MatrixTool.Multi(m,GetMx(),ref m); return m; } private double[,] GetMdz() { double[,] mt = {{ -Math.Sin(rz), Math.Co
Eigen实现坐标转换
自由 分享 合作
05-27 5004
（《视觉SLAM十四讲》第三讲习题7）设有小萝卜一号和二号在世界坐标系中。一号位姿q1 = [0.35, 0.2, 0.3, 0.1]，t1=[0.3, 0.1, 0.1]。二号位姿q2=[-0.5, 0.4, -0.1, 0.2], t2=[-0.1, 0.5, 0.3].某点在一号坐标系下坐标为p=[0.5, 0, 0.2].求p在二号坐标系下的坐标 假设在世界坐标系中p点的坐标为P...
C++ Eigen - 使用旋转矩阵，实现两个传感器之间的坐标变换（仅平移）
最新发布
Adunn的专栏
07-19 358
（1）变换公式为：P_2 = T_2_1 * P_1, 可将1系下的P点，旋转至2系下。 （2）公式中，T_2_1 的旋转部分，是指：由坐标系1 旋转至2的过程中，相对于1产生的角度。该角度的取值依据顺、逆时针进行判定（详见上节内容中的示例2中有详细说明）； （3）公式中，T_2_1 的平移部分，也应该是指由坐标系1 平移至2的过程中，相对于1系产生的平移量。正、负的取值也应以原始坐标系1为基础，分析对1系坐标产生的后果： 向负轴移动，取正数；向正轴移动，取负值。 所以本示例中，应该取值：2，2.
C++坐标转换
Stone_石头
05-12 877
使用场景：机器视觉中的位置修正工具。
C++ --两个坐标系的相互转换
ゃ﹏淸凨ゞ
04-17 2845
坐标系转换
c++直角坐标系与极坐标系的转换_【技术】关于坐标转换的一些基本概念，值得学习！...
weixin_39703926的博客
11-28 1519
每个项目收集到的资料并不一定都是一致的，如坐标类型不同：大地经纬度坐标，平面坐标等，也有可能采用的椭球体不同(坐标系不同)或投影方式不同等等。所以坐标系的相互转换在项目中使用非常普遍，如大地坐标转平面坐标，平面坐标转空间直角坐标，平面坐标转大地坐标等等。下面是关于坐标转换的一些基本概念。1.坐标系转换法(1)空间直角坐标系空间直角坐标系的坐标原点位于参考椭球的中心，Z轴指向参考椭球的北极...
坐标转换源代码，C++/C， 极为精确地大地坐标系转地心坐标系，地心坐标系转站心坐标系
热门推荐
luffv的专栏
05-08 1万+
摘自http://blog.sina.com.cn/s/blog_4e426d410101ahlt.html但是我用OSG进行转换的结果和用这个文章中的方法转换进行的对比，不明白为什么他转出的结果是OSG的正好两倍。//WGS84地理坐标系参变量struct CRDGEODETIC{double latitude;double longitude;double altitude;}; //空间笛卡...
Eigen局部坐标系和世界坐标系转换
沧海一帆的专栏
08-14 9432
本文主要介绍使用Eigen库进行中间坐标的转换。 坐标系转换如图 假设机器在p1处在世界坐标系下的位姿为（x，y，z，θ）=（1，1，0，0），p2处世界坐标系下的位姿（x，y，z，θ）=（2，2，0，45） 求p1 和p2之间的转换t12。
eigen坐标变换_Eigen::局部坐标系和base坐标系转换
weixin_42522009的博客
02-06 1234
"越有故事的人越沉静简单，越肤浅单薄的人越浮躁不安！"无论是做SLAM还是传感器标定，离不开的是坐标之间的转换关系Eigen 一个开源的C++矩阵运算库perfect~言归正传：使用Eigen库进行空间坐标的转换。 １.base坐标系－>局部坐标系已知p1 和p2求 t12假设传感器在base坐标系下p1处的位姿为(x，y，z，θ)=(1，1，0，0)在base坐标系下p2处的位姿为(x，y...
http://bitbucket.org/eigen/eigen/get/3.3.7.tar.gz
01-29
http://bitbucket.org/eigen/eigen/get/3.3.7.tar.gz ...Eigen是一个高层次的C ++库，有效支持线性代数，矩阵和矢量运算，数值分析及其相关的算法。Eigen是一个开源库，从3.1.1版本开始遵从MPL2许可。
c++利用eigen库，实现坐标系转换中的的平移和旋转
04-25
好的，我可以为您讲解如何利用Eigen库实现坐标系的平移和旋转。 首先，您需要引入Eigen库： ```cpp #include <Eigen/Dense> ``` 接下来，我们可以定义一个3D变换矩阵，表示平移和旋转变换： ```cpp Eigen::...
坐标系转换
09-20
这是matlab函数，适合九轴陀螺仪，九轴陀螺仪的加速度实在自身坐标系下的，不符合我们的要求，该实现的主要功能是：我们输入得到的欧拉角，就可以得到加速度在地面坐标系下的三个方向的加速度。 注意事项：欧拉角和坐标轴的对应关系
坐标转换C++
04-29
坐标转换，利用C++程序编写，方便 使用
用C++编写的坐标转换源码
12-05
用C++编写的坐标转换源代码，可完成WGS-84,BJ54等坐标的转换。
实现三维坐标变换、投影变换，C++实现。
04-03
MFC实现的图形在三维坐标下，进行坐标平移、投影、对称等变换。
c++坐标转换
04-15
c++坐标转换
C++ 点云坐标转换/投影变换 [世界坐标转相机坐标]
weixin_43896283的博客
05-28 1213
C++ 点云坐标转换/投影变换 [世界坐标转相机坐标]
c++ 的eigen库：eigen-3.3.7
05-26
去官网下载的目前最新版的eigen库 官网链接：http://eigen.tuxfamily.org/index.php?title=Main_Page#Download
slam十四讲 03 Eigen实践之不同坐标系下的坐标转换
一千零一夜的博客
06-20 1266
相机1下的坐标转换到相机2下的坐标
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值