vins-mono的bug记录

在imu数据和image数据对齐时，存在bug

vins里面一直在优化image和imu之间实际时间戳的间隔td，然后在对齐时把td加在了image的时间戳上。
getMeasurements()里面

while (imu_buf.front().timestamp < img_msg.timestamp + td)
        {
            IMUs.emplace_back(imu_buf.front());
            imu_buf.pop();
        }

process()里面

for (auto &measurement : measurements)
{
    double t = imu_msg.timestamp;
    double img_t = img_msg.timestamp + td; //认为实际采样图像时间比的时间大了td
}

这里面存在一个问题，process里面td是每一组image更新一次的，而getMeasurements里面用的td是上一次process里面最后一次更新的td。比如measurements里面有两组数据，第一组对齐时用到的td和getMeasurements里面的td是一样的，但是第二组对齐是的td已经是第一组优化更新的td。

如果计算平台特别快，这个bug是很难发现的，因为measurements基本只有一组，就没有问题。如果计算平台比较慢，measurements里面有好几组数据，这样使用的td就是错的。
如果td的优化不稳定，差距很大，我们在getMeasurements里面imu_buf.front().timestamp < img_msg.timestamp + td但是在process里面可能就变成大于，各种时间对齐的问题都会出来。

实验结果，我是5hz图像，100hzimu。

td 0.045023
imudata1 1358500888.971207
td 0.045023
imudata1 1358500888.981210
td 0.045023
imudata1 1358500888.991214
td 0.045023
imudata1 1358500889.001213
td 0.045023
imudata1 1358500889.011216
td 0.045023
imudata1 1358500889.021224
td 0.045023
imudata1 1358500889.031220
td 0.045023
imudata1 1358500889.041224
td 0.045023
imudata1 1358500889.051231
td 0.045023
imudata1 1358500889.061284
td 0.045023
imudata1 1358500889.071315
td 0.045023
imudata1 1358500889.081288
td 0.045023
imudata1 1358500889.091294
td 0.045023
imudata1 1358500889.101240
td 0.045023
imudata1 1358500889.111241
td 0.045023
imudata1 1358500889.121294
td 0.045023
imudata1 1358500889.131295
td 0.045023
imudata1 1358500889.141248
td 0.045023
imudata1 1358500889.151249
td 0.045023
imudata1 1358500889.161257
td 0.045023
imudata1 1358500889.171255
td 0.045023
imudata2 1358500889.171510
imu_dt 0.000254
td 0.029983
imudata1 1358500889.181382
td 0.029983
imudata1 1358500889.191322
td 0.029983
imudata1 1358500889.201265
td 0.029983
imudata1 1358500889.211358
td 0.029983
imudata1 1358500889.221271
td 0.029983
imudata1 1358500889.231319
td 0.029983
imudata1 1358500889.241320
td 0.029983
imudata1 1358500889.251385
td 0.029983
imudata1 1358500889.261331
td 0.029983
imudata1 1358500889.271280
td 0.029983
imudata1 1358500889.281286
td 0.029983
imudata1 1358500889.291286
td 0.029983
imudata1 1358500889.301288
td 0.029983
imudata1 1358500889.311292
td 0.029983
imudata1 1358500889.321294
td 0.029983
imudata1 1358500889.331388
td 0.029983
imudata 1 1358500889.341300
td 0.029983
imudata1 1358500889.351300
td 0.029983
imudata1 1358500889.361385
td 0.029983
imudata2 1358500889.366426
td 0.029983
imudata2 1358500889.366426

上面我打出了imu和image在process里面对齐，每个Imu的时间戳，imudata1是imu_msg.timestamp<=img_msg.timestamp + td的时间戳，imudata2是imu_msg.timestamp>mg_msg.timestamp + td的情况，因为getMeasurements的对齐策略，按理说imudata2只会出现一次。像一次的对齐就是正常的，但是第二次就用两个imudata2。为什么呢？从td也看懂出来，因为getMeasurements用的是0.045023，只有一组imu大于image时间戳，而process用是0.029983，已经有两组imu数据大于image，这两组都进到了imu_msg.timestamp>mg_msg.timestamp + td，用的都是image的时间戳，导致其时间戳相同了。

解决这个Bug有两种方式，
1.一次只允许处理一组image，即measurements只放一组image，这样getMeasurements里面和process就是一一对应了。
2.第二种是measurements使用的td用另一个变量保存，process里面这一次
measurements里面每组都使用这个变量，即不受td改变的影响了。

第一种方法会降低实时性，频繁进入

std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]
        {
            return (measurements = getMeasurements()).size() != 0; 将5/6组imu数据与1组image配对
        });
        lk.unlock();

而且必须有insert才会触发wait，虽然imu会100hz去触发，不会wait很久，但实时性还是不如一下子把buff里面累积的image都拿出来去计算。
但第一种是严格的给初始值，优化，优化作为初始值，再优化。
第二种方法实时性好，但是measurements里面每次使用的初始化值都是上一次measurements里面最后一组优化出来的td。比如measurements有五组数据，它们每次的初始值都是一样的且前四次的优化结果作废了，最后一次的优化结果给了下一次getMeasurements。没有严格的迭代优化。

Vector3d 的问题

代码：

          Eigen::Vector3d angular_velocity{rx, ry, rz};
          printf("imu rx=%f ry=%f rz=%f \n",rx,ry,rz);
          cout << "angular_velocity " << angular_velocity << endl;

输出结果：

imu rx=0.011985 ry=0.001731 rz=-0.007324 
angular_velocity  0.0
 0.0
-0.0
imu rx=0.011052 ry=0.002131 rz=-0.007857 
angular_velocity  0.0
 0.0
-0.0

rx,ry,rz明明有值，但是生成的angular_velocity的值全部为0，诡异！！！偶发的，有时正常有时不正常。

我仔细可eigin关于Vector的初始化，只有两种
一种是：

Matrix3f m;
m << 1, 2, 3,
     4, 5, 6,
     7, 8, 9;

一种是：

 MatrixXd m(2,2);
  m << 2, 3,
       1, 4;
  或者
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;

对于四维以下vectors的,可以用括号的方式直接赋值

Vector2d a(5.0, 6.0);
Vector3d b(5.0, 6.0, 7.0);
Vector4d c(5.0, 6.0, 7.0, 8.0);

没有我写 { }的这种，我把代码改为angular_velocity(rx,ry,rz)，或者 angular_velocity << rx,ry,rz 就正常了，我感觉这种方式可能语法不会报错，但是构造会不成功，造成输出结果为0。

不知道编译器里( 和{的区别是什么？

vins报numerical unsatble in preintegrationnumerical 错误

原因是imu预积分的雅克比矩阵过大或者过小。
有人说把 max solver time 加大一点就可以。
我感觉imu需要动起来，我是静止一段时间就会出现，如果一直运动就不会，我感觉是imu激励不足，静止输出都为0导致。