手寫VIO第二課代碼_std::vector< motiondata

本文链接：https://blog.csdn.net/zhangwenhou/article/details/104359393

在手寫VIO中，第二課採用中值法離散表示積分公式，以下是給出的程序。
     for (int i = 0; i < imudata.size(); ++i){
        MotionData imupose = imudata[i];
        MotionData imupose_1=imudata[i+1];
        
        Eigen::Vector3d imu_gyro_i=imupose.imu_gyro;  //w[k]
        Eigen::Vector3d imu_gyro_ii=imupose_1.imu_gyro; //w[k+1]
        
       
    Eigen::Vector3d imu_gyro_2=0.5*(imu_gyro_i+imu_gyro_ii);// 1/2*(w[k]+w[k+1])
     
      Eigen::Vector3d acc_wi = Qwb * (imupose.imu_acc) + gw;  // a[k]
     
        Eigen::Quaterniond dq;  //q[k+1]
        Eigen::Vector3d dtheta_half =  imu_gyro_2 * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();
        Qwb = Qwb * dq;
       
       
        Eigen::Vector3d acc_wii = Qwb * (imupose_1.imu_acc) + gw;   //a[k+1]
        
        Eigen::Vector3d acc_w2=0.5*(acc_wi+acc_wii);  //1/2(a[k]+a[k+1])
       
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w2;  
        Vw = Vw + acc_w2 * dt;
        
在使用中值積分的效果確實比歐拉方法要更加準確。其效果圖如圖所示：

關於上面的這個公式裏面acc_wii = Qwb * (imupose_1.imu_acc) + gw;，在這句代碼裏面兩個問題

1.acc沒有減去bias，這個可能是因爲由上面的數據生成的是不加噪聲的數據，家噪聲的數據在imu_pose_noise.txt中給出的。

2.爲什麼是加上gw，而公式中明明是減？

可能是在前面東北天的有定義中已經有了符號：Eigen::Vector3d gw(0,0,-9.81);

2.在對程序進行分析之後，仍然存在一些問題。

程序所畫出的曲線是運動方程生成的軌跡和由運動方程得出IMU數據積分之後再生成的軌跡。但程序中在得出IMU數據時有些看不懂

MotionData IMU::MotionModel(double t)
{

    MotionData data;
    // param
    float ellipse_x = 15;
    float ellipse_y = 20;
    float z = 1;           // z轴做sin运动
    float K1 = 10;          // z轴的正弦频率是x，y的k1倍
    float K = M_PI/ 10;    // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周

    // translation
    // twb:  body frame in world frame
    Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);
    Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));              // position导数　in world frame
    double K2 = K*K;
    Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));     // position二阶导数

    // Rotation
    double k_roll = 0.1;
    double k_pitch = 0.2;
    Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t );   // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]          why does it set to  this term?
    Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K);      // euler angles 的导数

//    Eigen::Vector3d eulerAngles(0.0,0.0, K*t );   // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
//    Eigen::Vector3d eulerAnglesRates(0.,0. , K);      // euler angles 的导数

    Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);         // body frame to world frame
    Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyro

    Eigen::Vector3d gn (0,0,-9.81);                                   //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)
    Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //  Rbw * Rwn * gn = gs

    data.imu_gyro = imu_gyro;
    data.imu_acc = imu_acc;
    data.Rwb = Rwb;
    data.twb = position;
    data.imu_velocity = dp;
    data.timestamp = t;
    return data;

}

在生成歐拉角時

Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t ); // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi] why does it set to this term?

不太懂爲什麼要這麼生成，而且roll，pitch,yaw的範圍也不是這個範圍。

在整個程序中，我不太清楚相機的作用是神魔，關於相機的那段程序

  // points obs in image
    for(int n = 0; n < camdata.size(); ++n)
    {
        MotionData data = camdata[n];
        Eigen::Matrix4d Twc = Eigen::Matrix4d::Identity();
        Twc.block(0, 0, 3, 3) = data.Rwb;       //extract block 3*3,begin from Twc[0][0]
        Twc.block(0, 3, 3, 1) = data.twb;

        // 遍历所有的特征点，看哪些特征点在视野里
        std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > points_cam;    // ３维点在当前cam视野里
        std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > features_cam;  // 对应的２维图像坐标
        for (int i = 0; i < points.size(); ++i) {
            Eigen::Vector4d pw = points[i];          // 最后一位存着feature id
            pw[3] = 1;                               //改成齐次坐标最后一位
            Eigen::Vector4d pc1 = Twc.inverse() * pw; // T_wc.inverse() * Pw  -- > point in cam frame

            if(pc1(2) < 0) continue; // z必须大于０,在摄像机坐标系前方

            Eigen::Vector2d obs(pc1(0)/pc1(2), pc1(1)/pc1(2)) ;  //in the normalized coordination
            // if( (obs(0)*460 + 255) < params.image_h && ( obs(0) * 460 + 255) > 0 &&
                   // (obs(1)*460 + 255) > 0 && ( obs(1)* 460 + 255) < params.image_w )
            {
                points_cam.push_back(points[i]);
                features_cam.push_back(obs);
            }
        }

        // save points
        std::stringstream filename1;
        filename1<<"keyframe/all_points_"<<n<<".txt";
        save_features(filename1.str(),points_cam,features_cam);
    }

不清楚畫點和畫線究竟實在幹什麼。