WIP | Refactor kimera_vio_ros port #5
Conversation
to access protected imu_single_callback_
So that any future MonoInterface may leverage the same code
TODO: Think of a less hacky workaround
| try { | ||
| // TODO(Toni): here we should consider using toCvShare... | ||
| cv_ptr = cv_bridge::toCvCopy(img_msg); | ||
| cv_constptr = cv_bridge::toCvShare(img_msg); |
There was a problem hiding this comment.
@ruffsl this commit msg caught my eye, bcs I think I tried toCvShare, but last time I tried there was something in the code modifying the images (probably in the frontend, which I am heavily refactoring now) , just FYI
There was a problem hiding this comment.
Ok, I'll revert that commit for the time being. Is there a ticket I could watch on that refactoring upstream?
There was a problem hiding this comment.
Ah, unfortunately, these tickets are in the private repo...
There was a problem hiding this comment.
@ToniRV , with the latest pushes, is this still an issue?
There was a problem hiding this comment.
I'm afraid it still is. I am working on this on another branch that hasn't been merged yet. Sorry for the delay!
| params_folder_ = node_->declare_parameter( | ||
| "params_folder", ""); | ||
| CHECK(!params_folder_.empty()); | ||
| vio_params_ = std::make_shared<VIO::VioParams>( |
There was a problem hiding this comment.
@ToniRV After the recent changes to the front end, it looks like vio_params_ has been made separate from VIO::DataProviderInterface class. This might make it easier to work with when using stuff like camera_info topics to init the extrinsic and intrinsic camera parameters, so thanks.
However, it looks like it seeks a number of yaml files with hardcoded file names, like the left and right camera yamls, but also some new files that where never added to the realsense data set params folder that would include a reference to the default hyperparameters for that device:
Could the reference params folder for the RealSense be updated to re-sync with the recent refactoring updtream?
| // RCLCPP_INFO(this->get_logger(), "Did you forget to register the right frame callback?"); | ||
|
|
||
| left_frame_callback_(VIO::make_unique<VIO::Frame>( | ||
| frame_count_, timestamp_left, left_cam_info, readRosImage(left_msg))); |
There was a problem hiding this comment.
@ToniRV I saw a TODO comment in the KimeraVIO fronted when debugging ec35862 about the idea of removing the camera info argument from the frame callback.
I just wanted to mention that keeping it might be nice for setups with changing extrinsic and intrinsic camera parameters.
E.g. When the camera has automatic focus or zoom, or when the camera to imu tranform is non-static, like with a robot's or drone articulated head with pan tilt control, but the IMU in fixed in place at the center of the chassis. These changes could still be picked up and synchronized with the data per-frame by consciously subscribing to camera_info and tf topics.
| imu_accgyr(2) = imu_msg->linear_acceleration.z; | ||
| imu_accgyr(3) = imu_msg->angular_velocity.x; | ||
| imu_accgyr(4) = imu_msg->angular_velocity.y; | ||
| imu_accgyr(5) = imu_msg->angular_velocity.z; |
There was a problem hiding this comment.
@ToniRV Would 9DoF IMUs ever be supported? E.g. where a magnetometer's orientation reading can be used. Does GTSAM even have an IMU factor that can consider absolute orientation?
|
|
||
| BaseInterface::BaseInterface( | ||
| rclcpp::Node::SharedPtr & node) | ||
| : VIO::DataProviderInterface(), |
There was a problem hiding this comment.
@ToniRV , Instead of inheriting from the VIO::DataProviderInterface class, It might be cleaner to replicate what was changed in the ROS1 wrapper and create a data_provider_ filed:
However, sterio and imu child interfaces here wouldn't then be able to access the private vio callback functions:
I tried creating a protected child class VIO::DataProviderInterface, and listing BaseInterface as a protected friend, but that workaround didn't seem to compile. It might of just have been my error in playing with the C++ classes.
| odometry_msg.twist | ||
| .covariance[i * static_cast<int>( | ||
| sqrt(odometry_msg.twist.covariance.size())) + | ||
| j] = vel_cov_body(i, j); |
There was a problem hiding this comment.
@ToniRV , In relation to MIT-SPARK/Kimera-VIO#41 , I think if there was a way to convert twist.linear and twist.covariance of the odometry_msg (or velocity_body) into a different reference frame, the then the wrapper could do the abstraction of allowing the imu frame and base link frames to have a non-identity displacement transform. This would also be handy if the transform between the IMU and base-link was non-static, like on sterio-imu pan tilt unit. But it would be nice to keep such reference frame transform functaions down in the kimera-vio library for all wrappers to share.
| // Kimera-VIO terms the plumb bob dist. model the as radtan. | ||
| cam_params->distortion_model_ = "radtan"; | ||
| // Kimera-VIO can't take a 6th order radial distortion term. | ||
| CHECK_EQ(cam_info->d.size(), 5); |
There was a problem hiding this comment.
@ToniRV it would be nice if Kimera-VIO understood the plumb_bob type to be the same as radtan:
- https://github.com/ros/common_msgs/blob/ea6c65e8846d9873b8d6e44435c52fb43d6093a3/sensor_msgs/include/sensor_msgs/distortion_models.h#L45
- https://github.com/ros/common_msgs/blob/ea6c65e8846d9873b8d6e44435c52fb43d6093a3/sensor_msgs/msg/CameraInfo.msg#L58
And also if Kimera-VIO library determined for itself whether to use or not the 6th order radial distortion term.
Also would be cool if Kimera-VIO checked if all the distortion terms where zero, so no rectification would be necessary. Not sure if cv::initUndistortRectifyMap internally optimizes for that. Lots of times, ROS image topics will already be rectified to avoid each downstream image consuming node from having to duplicate the rectification.
| auto g_args = rclcpp::init_and_remove_ros_arguments(argc, argv); | ||
| int g_argc = g_args.size(); | ||
| // Initialize Google's flags library. | ||
| google::ParseCommandLineFlags(&g_argc, &argv, true); |
There was a problem hiding this comment.
@ToniRV , when using composed nodes, I don't think we'd be able to acquire the CLI args for the containing process that holds all the nodes. Thus we'll need a way to pass any current run time args for google flag/log using ROS parameters instead. Perhaps we could a ROS parameter for passing a directories path containing google flag files, like with the param path for yaml files. Or would there be would be a way to combine the two?
captured by cv_bridge
No description provided.