OpenVINS Update

[borongyuan]

Now this can be used with stereo camera or rgbd camera (mono mode). The implementation of OpenVINS looks better than msckf_vio. Now this is ready for review and test. The dynamic initialization part is not enabled yet. Some updates will be added later.

[matlabbe]

Is it still a draft or ready to be reviewed/merged?

[borongyuan]

I'm still working on this.

[borongyuan]

Hi @matlabbe ,
Could you explain the expected behavior and calling logic of computeTransform()? I am considering adding ov_msckf::Propagator::fast_state_propagate() after ov_msckf::VioManager::feed_measurement_imu(), but there seems to be some problem with the logic. I think computeTransform() should calculate the transform with the same timestamp as the input data. But when publishInterIMU is enabled, the timestamp of calling it may not increase monotonically. Specifically, we may process an IMU data first, and then receive an older camera data. For a well-synchronized camera, IMU data should always arrive before camera data. I'm currently not buffering this data, but feeding it directly to VioManager. This should comply with the OpenVINS processing order, according to this comment. Now I only get the transform after every camera measurement feeding. At least the timing is correct. If I also get the transform after every imu measurement feeding, will rtabmap handle this out-of-order situation?

[matlabbe]

computeTransform() is called for every image and imu frames. I assume that we would receive IMU with stamp always equal or newer before receiving the next image (which would have always a stamp older or equal to last IMU processed).

computeTransform() can return a valid transform with only IMU data if the underlaying library does so, however, we explicitly return null on upper level if it is not an image update:

rtabmap/corelib/src/Odometry.cpp

Lines 700 to 703 in 78c029d

	if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
	{
	return Transform(); // Return null on IMU-only updates
	}

This makes sure that poses returned are sync with input images and their timestamps. Returning an IMU update with future timestamps before an image update may cause issues with the how rtabmap is requiring to receive odometry poses increasing in time.

I don't know how OpenVINS exactly works under the hood, for an image update, assuming it already processed newer IMU data, does it publish the pose of that image at the image stamp, or with the latest received imu stamp? where the pose would be not the pose at the image stamp, but the now corrected IMU pose and latest imu stamp received. rtabmap is expecting the pose of the image stamp when receiving an image, not a future predicted imu pose.

[borongyuan]

Thanks for the explanation. Then we don't need to add fast_state_propagate() for IMU-only input. The ROS version of OpenVINS currently provides odometer output at the same frequency as IMU data in this manner. This is useful for AR/VR applications, but for robotics applications such high-frequency odometry output is generally not required. So let's keep it simple.

OpenVINS will update state->_timestamp to the imaging time after camera input. interpolate_data() is used in select_imu_readings().

[matlabbe]

Big thanks to make it work with a real camera (not just euroc dataset)!

So openvins would provide vio_imu -> base_link (low latency synced with imu stamps) and vio_img->base_link (at image rate synced with image stamps). I think to make rgbd_odometry or stereo_odometry publishing vio_imu->base_link, we could forward that transform

rtabmap/corelib/src/Odometry.cpp

Lines 700 to 703 in 78c029d

	if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
	{
	return Transform(); // Return null on IMU-only updates
	}

upstream (maybe by posting an UEvent that can captured by the ros node) before setting the transform to null... But yeah, for VSLAM, I am only interested to pose at the image frame anyway.