姿态对之间的估计刚体变换

问题描述

我有一些已知的姿势对，包括 dx、dy、dz、yaw、pitch、roll。与此相关的是，这些姿势对是放置在汽车上的两个传感器的姿势。固定框架称为“地图”框架。我随时知道激光雷达的姿态和 gnss 相对于地图框的姿态。两个传感器之间存在相对于地图框的静态转换。我的目标是估计两个传感器之间的静态 3D 刚体变换。目标帧是 gnss。我知道这是一个优化问题。我应用遗传算法来估计变换，但即使平移正确，也无法正确找到旋转。这是预期的结果还是我的错在于目标函数。我分享了我的目标函数。我该如何解决这个问题？

Link: Pose pairs respect to the map frame

我的目标函数：

static bool eval_solution(const MySolution& p,MyMiddleCost &c)
{
    const double& dx=p.dx;
    const double& dy=p.dy;
    const double& dz=p.dz;
    const double& yaw=p.yaw;
    const double& pitch=p.pitch;
    const double& roll=p.roll;

    Eigen::AngleAxisd yawAngle(yaw,Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch,Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd rollAngle(roll,Eigen::Vector3d::UnitZ());
    Eigen::Quaternion<double> q = rollAngle * yawAngle * pitchAngle;
    Eigen::Matrix3d solution_R = q.matrix();

    Eigen::Matrix4d trans_solution;
    trans_solution.setIdentity();

    trans_solution(0,3) = dx;
    trans_solution(1,3) = dy;
    trans_solution(2,3) = dz;

    trans_solution.topLeftCorner(1,3) = solution_R;

    auto vector_trans_velo = std::get<0>(pairs);
    auto vector_trans_gnss = std::get<1>(pairs);
    int sample_count(vector_trans_gnss.size());

    // iterate over all samples
    double total_cost = 0;

    for(size_t i = 0; i<sample_count; i++)
    {
        auto mat_estimation = trans_solution*vector_trans_velo[i];
        Eigen::Matrix3d rot_est = mat_estimation.topLeftCorner(1,3);
        Eigen::Vector3d ea = rot_est.eulerAngles(0,1,2);
        double pitch_est = ea[0];
        double yaw_est = ea[1];
        double roll_est = ea[2];
        double est_dx = mat_estimation(0,3);
        double est_dy = mat_estimation(1,3);
        double est_dz = mat_estimation(2,3);

        auto mat_target = vector_trans_gnss[i];
        Eigen::Matrix3d rot_target= mat_target.topLeftCorner(1,3);
        Eigen::Vector3d ea2 = rot_target.eulerAngles(0,2);
        double pitch_target = ea2[0];
        double yaw_target = ea2[1];
        double roll_target = ea2[2];
        double target_dx = mat_target(0,3);
        double target_dy = mat_target(1,3);
        double target_dz = mat_target(2,3);

        double loss =
                pow(pitch_target-pitch_est,2)*1000 +
                pow(yaw_target-yaw_est,2)*1000 +
                pow(roll_target-roll_est,2)*1000 +
                pow(est_dx-target_dx,2) +
                pow(est_dy-target_dy,2) +
                pow(est_dz-target_dz,2);

        total_cost += pow(loss,2);

    }
    c.objective1 = total_cost/sample_count;
    return true; // solution is accepted
}

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）