手寫VIO第二課代碼

在手寫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]