Rolling shutter camera: Landmark DV logic and save params to yaml

[hitimo]

• Add all landmarks to optimization problem, independent of first frame (see also #261, thanks @JzHuai0108 for the input)
• Save calibrated parameters to yaml (incl. line delay)

Calibrated params in yaml:

cam0:
  camera_model: pinhole
  distortion_coeffs: [-3.566532960497908e-05, -4.683156500677483e-05, 0.00011840135331319602,
    0.000820039786788532]
  distortion_model: equidistant
  intrinsics: [700.0384938166252, 700.0379540666507, 376.9994602319219, 230.99986639623071]
  line_delay: 3.3966922363735363e-05
  resolution: [640, 480]
  rostopic: /bluefox/image_raw

Ground truth:

    intrinsics << 700.0, 700.0, 377.0, 231.0;
    dist_coeffs << 0.0, 0.0, 0.0, 0.0;
    int64_t line_delay_us = 34;

(Tested with synthetic dataset)

[kartikmohta]

In aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py line 358, keypoint_index should be changed to index otherwise this exits with

CovarianceReprojectionError() assert(_frame->numKeypoints() > _keypointIndex) failed

[JzHuai0108]

In aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py line 358, keypoint_index should be changed to index otherwise this exits with

CovarianceReprojectionError() assert(_frame->numKeypoints() > _keypointIndex) failed

I agree. Moreover, line 352 for setting keypoint index is likely unnecessary.

[goldbattle]

I have fixed the merge conflicts and edited based on the feedback to just directly use index. If anybody could test this to make sure it still works as expected that would be great.

@hitimo What are you using for simulation? I would also like to have a dataset with groundtruth to perform evaluation on. Any suggestion /pointers here would be great.

[JzHuai0108]

@goldbattle I wish I had time to test your pull request, but life uncertainty and high workload have been drive me nuts. Since I have perused your code before, I choose to trust your modifications for the moment :).