剛剛開源的VINS系統可以算是SLAM屆的良心作品了框仔，在同行中也激起了不小得轟動。沈老師連續(xù)兩天分別在將門創(chuàng)投和泡泡機器人上做了兩場公開課酥诽，大家反映很強烈匙隔。估計有上千人實時聽了這場干貨頗多的講座。從沈老師實驗室網站的數據流量看涌韩，公開課前后幾天網站訪問量有相當大的增加畔柔，而且IP地址遍布世界各地（除了非洲，拉丁美洲）臣樱，主要集中在中國靶擦，歐洲，美國雇毫，日韓玄捕。可想而知VINS的影響力有多廣棚放，大家對SLAM的熱情有多高枚粘。
閑言少敘，因為本人對SLAM也有很濃烈的興趣飘蚯，所以在VINS剛剛發(fā)布的時候便去下載研究馍迄。至今也看了有三四遍福也，雖然好多細節(jié)沒有理解透徹，但是對代碼整體框架和流程有了一定的了解攀圈∧馍迹看過代碼的同學應該知道，跟ORB-SLAM相比VINS里注釋很少量承，而且視覺融合本身就比較難搬设，所以從本次博客開始，我想記錄下自己研究VINS的心得以及疑惑的地方（iOS版本）撕捍，希望對自己和大家都有所幫助拿穴。我構想的撰寫順序是先后端，再前端忧风。后端是VINS最具創(chuàng)新的地方默色，就是圍繞ceresSolver里的誤差項：IMU殘差，重投影殘差狮腿，回環(huán)檢測殘差腿宰，以及邊緣化殘差進行。然后再關注前端圖像是如何處理缘厢，如何初始化吃度，關鍵幀選取和存儲等等。
本次博客主要內容有：
- 圖像數據獲取
- IMU 數據獲取
- IMU 與圖像的同步
- IMU預積分
- 誤差優(yōu)化函數中IMU誤差和Jacobian的計算
1. 圖像數據獲取
在程序啟動時贴硫，viewDidLoad函數就會在一開始創(chuàng)建獲取圖像的類CvVideoCamera：
#if VINS_FRAMEWORK
self.videoCamera = [[CvVideoCamera alloc] init];
ui_main = true;
#else
self.videoCamera = [[CvVideoCamera alloc]
initWithParentView:imageView];
#endif
self.videoCamera.delegate = self;
self.videoCamera.defaultAVCaptureDevicePosition =
AVCaptureDevicePositionBack;
self.videoCamera.defaultAVCaptureVideoOrientation = AVCaptureVideoOrientationPortrait;
self.videoCamera.defaultAVCaptureSessionPreset =
AVCaptureSessionPreset640x480;
#ifdef DATA_EXPORT
self.videoCamera.defaultFPS = 1;
#else
self.videoCamera.defaultFPS = 30;
#endif
isCapturing = NO;
[CameraUtils setExposureOffset: -1.0f];
//開始獲取圖像
[videoCamera start];
執(zhí)行start就會開始獲取圖像椿每，而且獲取到的圖像由CvVideoCameraDelegate的precessImage函數進行處理，這里請不要與VINS.cpp類的processImage函數混淆英遭。因為今天主要講IMU部分间护，所以具體如何通過光流算法跟蹤圖像等前端問題留到以后講解。這里只需要知道這段代碼即可：
img_msg->point_clouds = featuretracker.image_msg;
//img_msg callback
m_buf.lock();
img_msg_buf.push(img_msg);
is_calculate = true;
//NSLog(@"Img timestamp %lf",img_msg_buf.front()->header);
m_buf.unlock();
con.notify_one();
這就是圖像數據的緩沖隊列如何工作的挖诸。
2. IMU數據獲取
在viewcontroller.mm文件中汁尺，作者新開辟了三個線程，包括mainLoop多律，saveData以及回環(huán)檢測線程痴突。mainLoop是負責IMU數據與圖像數據融合的主要部分，也是我關注的重點內容菱涤。在mainLoop線程啟動前苞也，有個imuStartUpdate函數，來通過創(chuàng)建一個CMMotionManager單例粘秆，負責獲取IMU數據。
iOS關于CMMotionManager的介紹如下：
A CMMotionManager object is the gateway to the motion services provided by iOS.These services provide an app with accelerometer data, rotation-rate data, magnetometer data, and other device-motion data such as attitude. These types of data originate with a device’s accelerometers and (on some models) its magnetometer and gyroscope.
Note: Methods, properties, and data types for processing magnetometer data were introduced in iOS 5.0.
After creating an instance of CMMotionManager, an app can use it to receive four types of motion:
raw accelerometer data, raw gyroscope data, raw magnetometer data, and processed device-motion data (which includes accelerometer, rotation-rate, and attitude measurements). The processed device-motion data provided by Core Motion’s sensor fusion algorithms gives the device’s attitude, rotation rate, calibrated magnetic fields, the direction of gravity, and the acceleration the user is imparting to the device.
Important: An app should create only a single instance of the [CMMotionManager]class. Multiple instances of this classcan affect the rate at which data is received from the accelerometer and gyroscope.
An app can take one of two approaches when receiving motion data, by handling it at specified update intervals or periodically sampling the motion data. With both of these approaches, the app should call the appropriate stop method([stopAccelerometerUpdates], [stopGyroUpdates], [stopMagnetometerUpdates], and [stopDeviceMotionUpdates]) when it has finished processing accelerometer, rotation-rate, magnetometer, or device-motion data.
根據這個CMMotionManager的描述收毫，大家也就很容易理解代碼了攻走。
獲取加速度數據：
[motionManager startAccelerometerUpdatesToQueue:[NSOperationQueue currentQueue]
withHandler:^(CMAccelerometerData *latestAcc, NSError *error)
{
double header = motionManager.deviceMotion.timestamp;
rotation_imu << motionManager.deviceMotion.attitude.yaw * 180.0 / M_PI, //yaw
motionManager.deviceMotion.attitude.roll * 180.0 / M_PI, //pitch for vins
motionManager.deviceMotion.attitude.pitch * 180.0 / M_PI; //roll for vins
if(imu_prepare<10)
{
imu_prepare++;
}
shared_ptr<IMU_MSG> acc_msg(new IMU_MSG());
acc_msg->header = latestAcc.timestamp;
acc_msg->acc << -latestAcc.acceleration.x * GRAVITY,
-latestAcc.acceleration.y * GRAVITY,
-latestAcc.acceleration.z * GRAVITY;
cur_acc = acc_msg;
}];
上面的代碼花括號里面部分就是Handler的內容殷勘，每當加速度有更新的時候，就會觸發(fā)這個handler代碼塊昔搂×嵯可以看到里面有個變量來記錄何時獲得了十個加速度數據，這個是留個下面處理角速度計使用的摘符。cur_acc一直記錄著當前的加速度數據贤斜。
獲取角速度計數據的代碼為：
[motionManager startGyroUpdatesToQueue:[NSOperationQueue currentQueue] withHandler:^(CMGyroData *latestGyro, NSError *error)
{
//The time stamp is the amount of time in seconds since the device booted.
NSTimeInterval header = latestGyro.timestamp;
if(header<=0)
return;
if(imu_prepare < 10)
return;
IMU_MSG gyro_msg;
gyro_msg.header = header;
gyro_msg.gyr << latestGyro.rotationRate.x,
latestGyro.rotationRate.y,
latestGyro.rotationRate.z;
if(gyro_buf.size() == 0)
{
gyro_buf.push_back(gyro_msg);
gyro_buf.push_back(gyro_msg);
return;
}
else
{
gyro_buf[0] = gyro_buf[1];
gyro_buf[1] = gyro_msg;
}
//interpolation
shared_ptr<IMU_MSG> imu_msg(new IMU_MSG());
if(cur_acc->header >= gyro_buf[0].header && cur_acc->header < gyro_buf[1].header)
{
imu_msg->header = cur_acc->header;
imu_msg->acc = cur_acc->acc;
imu_msg->gyr = gyro_buf[0].gyr + (cur_acc->header - gyro_buf[0].header)*(gyro_buf[1].gyr - gyro_buf[0].gyr)/(gyro_buf[1].header - gyro_buf[0].header);
//printf("imu gyro update %lf %lf %lf\n", gyro_buf[0].header, imu_msg->header, gyro_buf[1].header);
//printf("imu inte update %lf %lf %lf %lf\n", imu_msg->header, gyro_buf[0].gyr.x(), imu_msg->gyr.x(), gyro_buf[1].gyr.x());
}
else
{
printf("imu error %lf %lf %lf\n", gyro_buf[0].header, cur_acc->header, gyro_buf[1].header);
return;
}
//為了重點突出如何獲取IMU數據，此處刪去了當需要save data,start_record以及predict status部分
//for save data
if(start_playback)
{
...
}
if(start_record)
{
...
}
m_time.lock();
lateast_imu_time = imu_msg->header;
m_time.unlock();
#if VINS_FRAMEWORK
//predict status
if(!CAMERA_MODE && ENABLE_IMU_PRIDICT)
{
...
}
#endif
m_buf.lock();
imu_msg_buf.push(imu_msg);
//NSLog(@"IMU_buf timestamp %lf, acc_x = %lf",imu_msg_buf.front()->header,imu_msg_buf.front()->acc.x());
m_buf.unlock();
con.notify_one();
}];
從代碼中可以看到加速度和角速度也不是完全同步的逛裤，也需要通過判斷兩者數據的時間瘩绒，進行插值處理。處理后的IMU數據被加入到imu_msg_buf,也就是IMU數據緩沖隊列中带族。
3. IMU與圖像的同步
在mainLoop的process函數中锁荔，作者把數據同步封裝到getMeasurement函數中，通過比較IMU的時間數據和Image的時間數據來獲得正確的匹配：
std::vector<std::pair<std::vector<ImuConstPtr>, ImgConstPtr>>
getMeasurements()
{
std::vector<std::pair<std::vector<ImuConstPtr>, ImgConstPtr>> measurements;
while (true)
{
if (imu_msg_buf.empty() || img_msg_buf.empty())
return measurements;
if (!(imu_msg_buf.back()->header > img_msg_buf.front()->header))
{
NSLog(@"wait for imu, only should happen at the beginning");
return measurements;
}
if (!(imu_msg_buf.front()->header < img_msg_buf.front()->header))
{
NSLog(@"throw img, only should happen at the beginning");
img_msg_buf.pop();
continue;
}
ImgConstPtr img_msg = img_msg_buf.front();
img_msg_buf.pop();
std::vector<ImuConstPtr> IMUs;
while (imu_msg_buf.front()->header <= img_msg->header)
{
IMUs.emplace_back(imu_msg_buf.front());
imu_msg_buf.pop();
}
//NSLog(@"IMU_buf = %d",IMUs.size());
measurements.emplace_back(IMUs, img_msg);
}
return measurements;
}
4. IMU預積分
下面到了最關鍵的部分了蝙砌，當得到同步好的數據之后：
std::vector<std::pair<std::vector<ImuConstPtr>, ImgConstPtr>> measurements;
std::unique_lock<std::mutex> lk(m_buf);
con.wait(lk, [&]
{
return (measurements = getMeasurements()).size() != 0;
});
lk.unlock();
waiting_lists = measurements.size();
process會從measurement里一個一個提取imu_msg 與img_msg數據阳堕，這里始終要記得，一個圖img_msg對應著一批imu_msg數據择克。
for(auto &measurement : measurements)
處理某一個imu_msg 隊列
for(auto &imu_msg : measurement.first)
{
//VINS 處理IMU
send_imu(imu_msg);
}
每次當得到一批imu_msg后恬总，send_imu函數會創(chuàng)建且只創(chuàng)建一個pre_integration類進行預積分，并且把它加入到pre_integrations隊列里（最后pre_integrations 隊列的大小會跟WINDOW_SIZE一樣）肚邢。
void send_imu(const ImuConstPtr &imu_msg)
{
NSTimeInterval t = imu_msg->header;
if (current_time < 0)
current_time = t;
double dt = (t - current_time);
current_time = t;
double ba[]{0.0, 0.0, 0.0};
double bg[]{0.0, 0.0, 0.0};
double dx = imu_msg->acc.x() - ba[0];
double dy = imu_msg->acc.y() - ba[1];
double dz = imu_msg->acc.z() - ba[2];
double rx = imu_msg->gyr.x() - bg[0];
double ry = imu_msg->gyr.y() - bg[1];
double rz = imu_msg->gyr.z() - bg[2];
//NSLog(@"IMU %f, dt: %f, acc: %f %f %f, gyr: %f %f %f", t, dt, dx, dy, dz, rx, ry, rz);
vins.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));
}
void VINS::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
//請注意frame_count參數的作用
if (!pre_integrations[frame_count])
{
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
if (frame_count != 0)
{
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
if(solver_flag != NON_LINEAR) //comments because of recovering
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
//midpoint integration
{
Vector3d g{0,0,GRAVITY};
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
}
}
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
每次pre_integration 推入一個IMU數據后就會進行積分：
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
propagate(dt, acc, gyr);
}
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
你以為propogate函數就是真正的積分么越驻，其實不是，最中心的在midPointIntegration函數中道偷，里面的積分公式部分很容易理解缀旁，但是這里的Jacobian并不是IMU誤差的Jacobian，但是這個Jacobian對計算IMU誤差的Jacobian很重要：
//每一次處理 Jacobian都會更新勺鸦，但是請注意這個Jacobian并不是IMU殘差的Jacobian
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
if(update_jacobian)
{
//這個jacobian 和IMU_factor jacobian 有什么不同
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<< 0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<< 0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<< 0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(15, 15);
//不懂 F 的作用
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
}
5. 誤差優(yōu)化函數中IMU殘差的計算
當處理完一批imu_msg后并巍，在process函數中就會緊接著處理圖像數據, 當圖像數量達到窗口大小時，在solve_ceres函數中就會把IMU誤差項加進去進行優(yōu)化换途，在這之前有一些邊緣化的操作懊渡，而且這個操作會影響pre_integrations數組，此處先略去军拟。
//IMU factor
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
可以發(fā)現剃执，一個IMUfactor誤差項對應著一個pre_integration實例，而且我們計算的是兩個幀之間IMU的誤差懈息。IMU誤差的計算如下肾档，但是這里的公式有些我也不太明白，如果有人能指點一下，那就太好了怒见。
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual;
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
//不懂 residual 為什么這么算
Vector3d G{0,0,GRAVITY};
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}
下面就是IMU誤差的Jacobian矩陣的計算,這里就使用到了pre_integration實例里面的Jacobian的部分結果俗慈，Jacobian數組里每一項都是IMU誤差關于兩幀圖像狀態(tài)的導數，只不過這里把pose和speedBias分開了：
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
Vector3d G{0,0,GRAVITY};
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
#if 0
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
jacobian_pose_i = sqrt_info * jacobian_pose_i;
if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
{
std::cout << sqrt_info << std::endl;
assert(false);
}
}
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
jacobian_speedbias_i.setZero();
jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
#if 0
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif
jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;
jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;
assert(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
assert(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
}
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
jacobian_pose_j.setZero();
jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();
#if 0
jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif
jacobian_pose_j = sqrt_info * jacobian_pose_j;
assert(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
assert(fabs(jacobian_pose_j.minCoeff()) < 1e8);
}
if (jacobians[3])
{
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
jacobian_speedbias_j.setZero();
jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();
jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();
jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();
jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;
assert(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
assert(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
}
}
至此遣耍，我們把優(yōu)化后端的IMU部分梳理完畢闺阱，下次開始梳理邊緣化的計算。
