49 changes: 49 additions & 0 deletions kimera_vio_ros/include/kimera_vio_ros/interfaces/base_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
#ifndef KIMERA_ROS__INTERFACES__BASE_INTERFACE_HPP_
#define KIMERA_ROS__INTERFACES__BASE_INTERFACE_HPP_

#include <future>

#include "glog/logging.h"
#include "kimera-vio/dataprovider/DataProviderInterface.h"
#include "kimera-vio/pipeline/Pipeline.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"

namespace kimera_vio_ros
{
namespace interfaces
{

class BaseInterface : public VIO::DataProviderInterface
{
public:
BaseInterface(
rclcpp::Node::SharedPtr & node);
virtual ~BaseInterface();
void start();

protected:
rclcpp::Node::SharedPtr node_;
VIO::VioParams::Ptr vio_params_;
VIO::Pipeline::UniquePtr vio_pipeline_;

std::string base_link_frame_id_;
std::string map_frame_id_;
std::string world_frame_id_;

std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

private:
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_pipeline_;
rclcpp::TimerBase::SharedPtr pipeline_timer_;
std::future<bool> handle_pipeline_;
};

} // namespace interfaces
} // namespace kimera_vio_ros

#endif // KIMERA_ROS__INTERFACES__BASE_INTERFACE_HPP_
35 changes: 35 additions & 0 deletions kimera_vio_ros/include/kimera_vio_ros/interfaces/image_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
#ifndef KIMERA_ROS__INTERFACES__IMAGE_INTERFACE_HPP_
#define KIMERA_ROS__INTERFACES__IMAGE_INTERFACE_HPP_

#include "cv_bridge/cv_bridge.h"
#include "kimera_vio_ros/interfaces/base_interface.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/image.hpp"

using CameraInfo = sensor_msgs::msg::CameraInfo;
using Image = sensor_msgs::msg::Image;

namespace kimera_vio_ros
{
namespace interfaces
{

class ImageInterface : virtual public BaseInterface
{
public:
ImageInterface(
rclcpp::Node::SharedPtr & node);
virtual ~ImageInterface();

protected:
void msgCamInfoToCameraParams(
const CameraInfo::ConstSharedPtr & cam_info,
VIO::CameraParams * cam_params);
const cv::Mat readRosImage(const Image::ConstSharedPtr & img_msg);

};

} // namespace interfaces
} // namespace kimera_vio_ros

#endif // KIMERA_ROS__INTERFACES__IMAGE_INTERFACE_HPP_
33 changes: 33 additions & 0 deletions kimera_vio_ros/include/kimera_vio_ros/interfaces/imu_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
#ifndef KIMERA_ROS__INTERFACES__IMU_INTERFACE_HPP_
#define KIMERA_ROS__INTERFACES__IMU_INTERFACE_HPP_

#include "kimera_vio_ros/interfaces/base_interface.hpp"
#include "sensor_msgs/msg/imu.hpp"

using Imu = sensor_msgs::msg::Imu;

namespace kimera_vio_ros
{
namespace interfaces
{

class ImuInterface : virtual public BaseInterface
{
public:
ImuInterface(
rclcpp::Node::SharedPtr & node);
virtual ~ImuInterface();

private:
void imu_cb(const Imu::SharedPtr imu_msg);

private:
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_imu_;
rclcpp::Subscription<Imu>::SharedPtr imu_sub_;
rclcpp::Time last_imu_timestamp_;
};

} // namespace interfaces
} // namespace kimera_vio_ros

#endif // KIMERA_ROS__INTERFACES__IMU_INTERFACE_HPP_
54 changes: 54 additions & 0 deletions kimera_vio_ros/include/kimera_vio_ros/interfaces/stereo_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
#ifndef KIMERA_ROS__INTERFACES__STEREO_INTERFACE_HPP_
#define KIMERA_ROS__INTERFACES__STEREO_INTERFACE_HPP_

#include "kimera_vio_ros/interfaces/image_interface.hpp"
// #include "image_transport/subscriber_filter.h"
#include "message_filters/subscriber.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/exact_time.h"

namespace kimera_vio_ros
{
namespace interfaces
{

class StereoInterface : virtual public ImageInterface
{
public:
StereoInterface(
rclcpp::Node::SharedPtr & node);
virtual ~StereoInterface();

private:
void stereo_info_cb(
const CameraInfo::ConstSharedPtr & left_msg,
const CameraInfo::ConstSharedPtr & right_msg);
void stereo_image_cb(
const Image::SharedPtr left_msg,
const Image::SharedPtr right_msg);

private:
rclcpp::callback_group::CallbackGroup::SharedPtr callback_group_stereo_;

typedef message_filters::sync_policies::ExactTime<Image, Image> ExactImagePolicy;
typedef message_filters::Synchronizer<ExactImagePolicy> ExactImageSync;
std::shared_ptr<ExactImageSync> exact_image_sync_;
std::shared_ptr<message_filters::Subscriber<Image>> left_image_sub_;
std::shared_ptr<message_filters::Subscriber<Image>> right_image_sub_;
// image_transport::SubscriberFilter left_sub_, right_sub_;

typedef message_filters::sync_policies::ExactTime<CameraInfo, CameraInfo> ExactInfoPolicy;
typedef message_filters::Synchronizer<ExactInfoPolicy> ExactInfoSync;
std::shared_ptr<ExactInfoSync> exact_info_sync_;
std::shared_ptr<message_filters::Subscriber<CameraInfo>> left_info_sub_;
std::shared_ptr<message_filters::Subscriber<CameraInfo>> right_info_sub_;
bool camera_info_received_ = false;

VIO::FrameId frame_count_;
rclcpp::Time last_stereo_timestamp_;
};

} // namespace interfaces
} // namespace kimera_vio_ros

#endif // KIMERA_ROS__INTERFACES__STEREO_INTERFACE_HPP_
28 changes: 28 additions & 0 deletions kimera_vio_ros/include/kimera_vio_ros/interfaces/stereo_vio_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
#ifndef KIMERA_ROS__INTERFACES__STEREO_VIO_INTERFACE_HPP_
#define KIMERA_ROS__INTERFACES__STEREO_VIO_INTERFACE_HPP_

#include "kimera_vio_ros/interfaces/backend_interface.hpp"
#include "kimera_vio_ros/interfaces/imu_interface.hpp"
#include "kimera_vio_ros/interfaces/stereo_interface.hpp"

namespace kimera_vio_ros
{
namespace interfaces
{

class StereoVioInterface
: public ImuInterface,
public StereoInterface,
public BackendInterface
{
public:
StereoVioInterface(
rclcpp::Node::SharedPtr & node);
~StereoVioInterface();

};

} // namespace interfaces
} // namespace kimera_vio_ros

#endif // KIMERA_ROS__INTERFACES__STEREO_VIO_INTERFACE_HPP_
20 changes: 20 additions & 0 deletions kimera_vio_ros/include/kimera_vio_ros/utils/geometry.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
#ifndef KIMERA_ROS__UTILS__GEOMETRY_HPP_
#define KIMERA_ROS__UTILS__GEOMETRY_HPP_

#include "geometry_msgs/msg/transform.hpp"
#include "glog/logging.h"
#include "gtsam/geometry/Pose3.h"

namespace kimera_vio_ros
{
namespace utils
{

void msgTFtoPose(const geometry_msgs::msg::Transform & tf, gtsam::Pose3 * pose);

void poseToMsgTF(const gtsam::Pose3 & pose, geometry_msgs::msg::Transform * tf);

} // namespace utils
} // namespace kimera_vio_ros

#endif // KIMERA_ROS__UTILS__GEOMETRY_HPP_
116 changes: 116 additions & 0 deletions kimera_vio_ros/launch/kimera_vio_ros.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,116 @@
<launch>
<arg name="dataset_name" default="EuRoC"/>
<arg name="parallel" default="flase" />
<arg name="use_sim_time" default="true"/>

<!-- If true, SparkVio will log output of all modules to the
'log_output_path' location. -->
<arg name="log_output" default="false"/>
<arg name="log_output_path"
default="$(find-pkg-share kimera_vio_ros)/output_logs/"/>


<!-- If true, SparkVio will enable the LoopClosureDetector module. -->
<arg name="use_lcd" default="false"/>

<!-- Parameters -->
<!-- each dataset has its own set of parameter files -->
<!-- the parameters are set in param folder, ordered by the dataset's name -->
<arg name="params_folder"
default="$(find-pkg-share kimera_vio_ros)/param/$(var dataset_name)"/>

<!-- Specify the absolute path to the vocabulary file. -->
<!-- This is usually in the VIO directory (VIO/vocabulary/ORBvoc.yml). -->
<!-- <arg name="path_to_vocab" default="$(find-pkg-share kimera_vio)/vocabulary/ORBvoc.yml"/> -->
<arg name="path_to_vocab" default="/opt/overlay_ws/src/MIT-SPARK/Kimera-VIO/vocabulary/ORBvoc.yml"/>

<!-- Subscribed Topics -->
<arg name="topic.left.image" default="/cam0/image_raw"/>
<arg name="topic.right.image" default="/cam1/image_raw"/>
<arg name="topic.imu.data" default="/imu0"/>

<arg name="use_camera_info" default="true"/>
<arg name="topic.left.info" default="/cam0/camera_info"/>
<arg name="topic.right.info" default="/cam1/camera_info"/>

<!-- Frame IDs -->
<arg name="frame_id.base_link" default="base_link"/>
<arg name="frame_id.map" default="map"/>
<arg name="frame_id.world" default="world"/>

<!-- General parameters -->
<arg name="verbosity" default="0" />
<!-- Visualize pipeline output in OpenCV. -->
<!-- Note that this is duplicated from the flags file -->
<arg name="visualize" default="false" />
<!-- <arg name="rectified" default="true" /> -->

<!-- Launch main node -->
<node name="kimera_vio_ros" pkg="kimera_vio_ros" exec="stereo_vio_node"
output="screen" ns="kimera_vio_ros"
args="--use_lcd=$(var use_lcd) \
--vocabulary_path=$(var path_to_vocab) \
--flagfile=$(var params_folder)/flags/Mesher.flags \
--flagfile=$(var params_folder)/flags/VioBackEnd.flags \
--flagfile=$(var params_folder)/flags/RegularVioBackEnd.flags \
--flagfile=$(var params_folder)/flags/Visualizer3D.flags \
--logtostderr 1 \
--colorlogtostderr 1 \
--log_prefix 1 \
--v=$(var verbosity) \
--log_output=$(var log_output) \
--output_path=$(var log_output_path) \
--visualize=$(var visualize)">
<param name="params_folder" value="$(var params_folder)"/>

<param name="use_sim_time" value="$(var use_sim_time)"/>
<param name="use_lcd" value="$(var use_lcd)"/>

<!-- Frame IDs for Odometry -->
<param name="frame_id">
<param name="base_link" value="$(var frame_id.base_link)"/>
<param name="map" value="$(var frame_id.map)"/>
<param name="world" value="$(var frame_id.world)"/>
</param>

<!-- Subscriber topics -->
<remap from="left/image" to="$(var topic.left.image)"/>
<remap from="right/image" to="$(var topic.right.image)"/>
<remap from="imu/data" to="$(var topic.imu.data)"/>

<param name="use_camera_info" value="$(var use_camera_info)"/>
<remap from="left/camera_info" to="$(var topic.left.info)"/>
<remap from="right/camera_info" to="$(var topic.right.info)"/>

<!-- Remap publisher topics -->
<remap from="odometry" to="odometry"/>
<remap from="resiliency" to="resiliency"/>
<remap from="imu_bias" to="imu_bias"/>
<remap from="optimized_trajectory" to="optimized_trajectory"/>
<remap from="pose_graph" to="pose_graph"/>
<remap from="mesh" to="mesh"/>
<remap from="frontend_stats" to="frontend_stats"/>
<remap from="debug_mesh_img" to="debug_mesh_img"/>
<remap from="time_horizon_pointcloud" to="time_horizon_pointcloud"/>

<!-- Resiliency Thresholds: TODO(Sandro) document -->
<param name="velocity_det_threshold" value="0.1"/>
<param name="position_det_threshold" value="0.3"/>
<param name="stereo_ransac_threshold" value="20"/>
<param name="mono_ransac_threshold" value="30"/>

<!-- Load calibration -->
<!-- <rosparam command="load" file="$(var params_folder)/calibration.yaml"/> -->
</node>

<!-- Launch visualizer -->
<!-- <include file="$(find pose_graph_tools)/launch/posegraph_view.launch" >
<arg name="frame" value="$(var world_frame_id)" />
<arg name="graph_topic" value="pose_graph" />
<arg name="ns" value="kimera_vio_ros"/>
</include> -->

<!-- <node pkg="demo_nodes_cpp" exec="talker"/>
<node pkg="demo_nodes_cpp" exec="listener"/> -->

</launch>
36 changes: 36 additions & 0 deletions kimera_vio_ros/launch/realsense_ir.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
<launch>
<group>
<arg name="dataset_name" default="RealSense_IR"/>
<arg name="parallel" default="true" />
<arg name="use_sim_time" default="false" />
<!-- TODO: rectified should be true for realsense
as the realsense node publishes rectified images -->
<arg name="rectified" default="false" />
<arg name="use_lcd" default="true"/>

<arg name="log_output" default="false"/>

<!-- Subscribed Topics -->
<let name="topic.left.image" value="/camera/infra1/image_rect_raw"/>
<let name="topic.right.image" value="/camera/infra2/image_rect_raw"/>
<let name="topic.imu.data" value="/camera/imu/sample"/>

<let name="use_camera_info" value="true"/>
<let name="topic.left.info" value="/camera/infra1/camera_info"/>
<let name="topic.right.info" value="/camera/infra2/camera_info"/>

<!-- Frame IDs for Odometry -->
<let name="frame_id.base_link" value="base_link" />

<!-- Launch actual pipeline -->
<include file="$(find-pkg-share kimera_vio_ros)/launch/kimera_vio_ros.launch.xml"/>
</group>

<!-- Kimera assumes the imu is fundamentally the `base_link` refrence frame.
To keep the TF tree a tree, we can just publish a static TF
that is just the inverse of the camera_link to camera_imu_optical_frame tf.
E.g: `$ ros2 run tf2_ros tf2_echo camera_imu_optical_frame camera_link` -->
<node name="base_link_to_camera_link" pkg="tf2_ros" exec="static_transform_publisher"
args="-0.006 0.005 0.012 0.500 -0.500 0.500 0.500 base_link camera_link" />

</launch>
37 changes: 37 additions & 0 deletions kimera_vio_ros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
<?xml version="1.0"?>
<package format="2">
<name>kimera_vio_ros</name>
<version>4.0.0</version>
<description>KimeraVIO ROS Wrapper</description>

<maintainer email="yunchang@mit.edu">Yun Chang</maintainer>
<maintainer email="mabate@mit.edu">Marcus Abate</maintainer>
<maintainer email="arosinol@mit.edu">Antoni Rosinol</maintainer>

<license>BSD</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>cv_bridge</depend>
<depend>geometry_msgs</depend>
<depend>kimera_common</depend>
<depend>kimera_vio</depend>
<depend>libboost-dev</depend>
<depend>libgflags-dev</depend>
<depend>libgoogle-glog-dev</depend>
<depend>message_filters</depend>
<depend>nav_msgs</depend>
<depend>pcl_conversions</depend>
<depend>pcl_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>sensor_msgs</depend>
<depend>tf2_ros</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
27 changes: 27 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/ImuParams.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
%YAML:1.0
# Type of IMU preintegration:
# 0: CombinedImuFactor
# 1: ImuFactor
imu_preintegration_type: 1

# These are Euroc Dataset parameters
# Sensor extrinsics wrt. the body-frame.
T_BS:
cols: 4
rows: 4
data: [1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0]
rate_hz: 200

# inertial sensor noise model parameters (static)
gyroscope_noise_density: 0.00011833115133 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 0.00011833115133 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
accelerometer_noise_density: 0.015254156844262 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 0.015254156844262 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )

# Extra parameters
imu_integration_sigma: 1.0e-8
imu_time_shift: 0.0
n_gravity: [0.0, 0.0, -9.81]
60 changes: 60 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/LCDParameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,60 @@
%YAML:1.0
# LoopClosureDetector parameters:
use_nss: 1
alpha: 0.5
min_temporal_matches: 3
dist_local: 20
max_db_results: 50
min_nss_factor: 0.005
min_matches_per_group: 1
max_intragroup_gap: 3
max_distance_between_groups: 3
max_distance_between_queries: 2

geom_check_id: 0
min_correspondences: 12
max_ransac_iterations_mono: 500
ransac_probability_mono: 0.99
ransac_threshold_mono: 1e-6
ransac_randomize_mono: 0
ransac_inlier_threshold_mono: 0.5

pose_recovery_option_id: 0
max_ransac_iterations_stereo: 500
ransac_probability_stereo: 0.995
ransac_threshold_stereo: 0.15
ransac_randomize_stereo: 0
ransac_inlier_threshold_stereo: 0.5
use_mono_rot: 1

lowe_ratio: 0.7
matcher_type: 3

nfeatures: 500
scale_factor: 1.2
nlevels: 8
edge_threshold: 31
first_level: 0
WTA_K: 2
score_type_id: 0
patch_sze: 31
fast_threshold: 20

pgo_rot_threshold: 0.01
pgo_trans_threshold: 0.1

# geom_check_id options:
# 0: NISTER
# 1: NONE

# pose_recovery_option_id options:
# 0: RANSAC_ARUN
# 1: GIVEN_ROT

# matcher_type options:
# 0: FLANNBASED
# 1: BRUTEFORCE
# 2: BRUTEFORCE_L1
# 3: BRUTEFORCE_HAMMING
# 4: BRUTEFORCE_HAMMINGLUT
# 5: BRUTEFORCE_SL2
20 changes: 20 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/LeftCameraParams.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
%YAML:1.0
# General sensor definitions.
camera_id: left_cam

# Sensor extrinsics wrt. the body-frame.
T_BS:
cols: 4
rows: 4
data: [1.0, 0.0, 0.0,-0.006,
0.0, 1.0, 0.0, 0.005,
0.0, 0.0, 1.0, 0.012,
0.0, 0.0, 0.0, 1.0]

# Camera specific definitions.
rate_hz: 30
resolution: [640,480] # width, height
camera_model: pinhole
intrinsics: [385.09918212890625,385.09918212890625,319.0403137207031,243.58929443359375] # fu, fv, cu, cv
distortion_model: radtan
distortion_coefficients: [0.0, 0.0, 0.0, 0.0] # k1, k2, p1, p2
13 changes: 13 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/PipelineParams.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
%YAML:1.0
# frontend_type
# Type of VIO Frontend to use
# 0: StereoImu
frontend_type: 0

# Type of vioBackEnd to use:
# 0: VioBackEnd
# 1: RegularVioBackEnd
backend_type: 1

# Run VIO parallel or sequential
parallel_run: 1
20 changes: 20 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/RightCameraParams.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
%YAML:1.0
# General sensor definitions.
camera_id: right_cam

# Sensor extrinsics wrt. the body-frame.
T_BS:
cols: 4
rows: 4
data: [1.0, 0.0, 0.0, 0.045,
0.0, 1.0, 0.0, 0.005,
0.0, 0.0, 1.0, 0.012,
0.0, 0.0, 0.0, 1.0]

# Camera specific definitions.
rate_hz: 30
resolution: [640,480] # width, height
camera_model: pinhole
intrinsics: [385.09918212890625,385.09918212890625,319.0403137207031,243.58929443359375] # fu, fv, cu, cv
distortion_model: radtan
distortion_coefficients: [0.0, 0.0, 0.0, 0.0] # k1, k2, p1, p2
72 changes: 72 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/calibration/calibration_UCSD_plumb_bob.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,72 @@
# Calibration (!FAKE!)
# ----------------------------------------------------------------------------------
# General Info
responsible: Ruffin White
date: 02.27.2020

# Hardware
camera_hardware: RealSense D435i
IMU_hardware: RealSense D435i


# Cameras
# ----------------------------------------------------------------------------------
# Rate:
camera_rate_hz: 30

# Camera Model:
camera_model: pinhole
distortion_model: plumb_bob

# Resolution:
camera_resolution: [640,480] # width, height

# Left Camera Parameters:
left_camera_intrinsics: [385.09918212890625,385.09918212890625,319.0403137207031,243.58929443359375] # fu, fv, cu, cv
left_camera_distortion_coefficients: [0.0, 0.0, 0.0, 0.0, 0.0] # k1, k2, p1, p2

# Left Camera to IMU Transformation:
left_camera_extrinsics: [1.0, 0.0, 0.0,-0.006,
0.0, 1.0, 0.0, 0.005,
0.0, 0.0, 1.0, 0.012,
0.0, 0.0, 0.0, 1.0]

# Right Camera Parameters:
right_camera_intrinsics: [385.09918212890625,385.09918212890625,319.0403137207031,243.58929443359375] # fu, fv, cu, cv
right_camera_distortion_coefficients: [0.0, 0.0, 0.0, 0.0, 0.0] # k1, k2, p1, p2

# Right Camera to IMU Transformation:
left_camera_extrinsics: [1.0, 0.0, 0.0, 0.045,
0.0, 1.0, 0.0, 0.005,
0.0, 0.0, 1.0, 0.012,
0.0, 0.0, 0.0, 1.0]


# Body
# ----------------------------------------------------------------------------------
# Transformation:
calibration_to_body_frame: [1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0]


# IMU
# ----------------------------------------------------------------------------------
# Rate:
imu_rate_hz: 47

# Timeshift:
imu_shift: 0.000000000 # t_imu0 = t_cam0 + imu_shift

# Noise Model Parameters: (Static)
gyroscope_noise_density: 0.00011833115133 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" )
gyroscope_random_walk: 0.00011833115133 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
accelerometer_noise_density: 0.015254156844262 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" )
accelerometer_random_walk: 0.015254156844262 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion )

# IMU to Body Transformation:
imu_extrinsics: [1.000000000, 0.000000000, 0.000000000, 0.000000000,
0.000000000, 1.000000000, 0.000000000, 0.000000000,
0.000000000, 0.000000000, 1.000000000, 0.000000000,
0.000000000, 0.000000000, 0.000000000, 1.000000000]
58 changes: 58 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/flags/Mesher.flags
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
#!/bin/bash
# Mesher parameters.
# General functionality for the mesher.
--add_extra_lmks_from_stereo=true
--reduce_mesh_to_time_horizon=true

# Visualization.
--visualize_histogram_1D=false
--log_histogram_1D=false
--visualize_histogram_2D=false
--log_histogram_2D=false
--visualize_mesh_2d=false
--visualize_mesh_2d_filtered=false

# Mesh filters.
--max_grad_in_triangle=-1
--min_ratio_btw_largest_smallest_side=0.5
--min_elongation_ratio=0.5
--max_triangle_side=2.0

# Association.
--normal_tolerance_polygon_plane_association=0.021
--distance_tolerance_polygon_plane_association=0.20
--normal_tolerance_plane_plane_association=0.011
--distance_tolerance_plane_plane_association=0.20
--do_double_association=true
--only_associate_a_polygon_to_a_single_plane=true

# Segmentation.
--normal_tolerance_horizontal_surface=0.035
--normal_tolerance_walls=0.021
--only_use_non_clustered_points=true

# Histogram 2D.
--hist_2d_gaussian_kernel_size=3
--hist_2d_nr_of_local_max=2
--hist_2d_min_support=20
--hist_2d_min_dist_btw_local_max=5
--hist_2d_theta_bins=40
--hist_2d_distance_bins=40
--hist_2d_theta_range_min=0
--hist_2d_theta_range_max=3.14159265
--hist_2d_distance_range_min=-6.0
--hist_2d_distance_range_max=6.0

# Z histogram.
--z_histogram_bins=512
--z_histogram_min_range=-0.75
--z_histogram_max_range=3.0
--z_histogram_window_size=3
--z_histogram_peak_per=0.5
--z_histogram_min_support=50
--z_histogram_min_separation=0.1
--z_histogram_gaussian_kernel_size=5
--z_histogram_max_number_of_peaks_to_select=3

# Extras
--return_mesh_2d=true
13 changes: 13 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/flags/RegularVioBackEnd.flags
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,13 @@
#!/bin/bash
# RegularVioBackEnd parameters.
--min_num_of_observations=2
--max_parallax=150
--min_num_obs_for_proj_factor=4
--convert_extra_smart_factors_to_proj_factors=true
--remove_old_reg_factors=true
--min_num_of_plane_constraints_to_add_factors=40
--min_num_of_plane_constraints_to_remove_factors=25
--use_unstable_plane_removal=false
--min_num_of_plane_constraints_to_avoid_seg_fault=10
--prior_noise_sigma_normal=0.1
--prior_noise_sigma_distance=0.1
20 changes: 20 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/flags/StereoVIO.flags
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
#!/bin/bash
--log_output=false
--backend_type=1
--regular_vio_backend_modality=0
--deterministic_random_number_generator=true
--visualize=true
--visualize_lmk_type=false
--visualize_mesh=false
--visualize_mesh_with_colored_polygon_clusters=false
--visualize_point_cloud=true
--visualize_convex_hull=false
--visualize_plane_constraints=false
--visualize_planes=false
--visualize_plane_label=false
--visualize_semantic_mesh=false
--visualize_mesh_in_frustum=false
#--visualize_load_mesh_filename=/home/tonirv/datasets/euroc/V1_01_easy/mav0/pointcloud0/data.ply
--viz_type=3
--min_num_obs_for_mesher_points=3
--extract_planes_from_the_scene=false
5 changes: 5 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/flags/VioBackEnd.flags
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
#!/bin/bash
--debug_graph_before_opt=true
--process_cheirality=true
--max_number_of_cheirality_exceptions=5
--compute_state_covariance=true
9 changes: 9 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/flags/Visualizer3D.flags
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
#!/bin/bash
# Visualizer 3D parameters.
--mesh_shading=0
--mesh_representation=1
--set_mesh_lighting=true
--set_mesh_ambient=true
--log_mesh=false
--log_accumulated_mesh=false
--displayed_trajectory_length=-1
48 changes: 48 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/regularVioParameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
%YAML:1.0
#IMU PARAMETERS
gyroNoiseDensity: 0.00016968
accNoiseDensity: 0.002
gyroBiasSigma: 1.9393e-05
accBiasSigma: 0.003
imuIntegrationSigma: 1.0e-8
n_gravity: [0.0, 0.0, -9.81]
nominalImuRate: 0.005
#INITIALIZATION PARAMETERS
autoInitialize: 1
roundOnAutoInitialize: 0
initialPositionSigma: 1e-05
initialRollPitchSigma: 0.174533 # 10.0/180.0*M_PI
initialYawSigma: 0.00174533 # 0.1/180.0*M_PI
initialVelocitySigma: 0.001
initialAccBiasSigma: 0.1
initialGyroBiasSigma: 0.01
#VISION PARAMETERS
linearizationMode: 0
degeneracyMode: 1
smartNoiseSigma: 3.25
monoNoiseSigma: 1.8
monoNormType: 2
monoNormParam: 4.6851
stereoNoiseSigma: 1.8
stereoNormType: 2
stereoNormParam: 4.6851
regularityNoiseSigma: 0.03
regularityNormType: 1
regularityNormParam: 0.04
rankTolerance: 1
landmarkDistanceThreshold: 15
outlierRejection: 8
retriangulationThreshold: 0.001
addBetweenStereoFactors: 1
betweenRotationPrecision: 0 # Inverse of variance.
betweenTranslationPrecision: 100 # 1/(0.1*0.1)
#OPTIMIZATION PARAMETERS
relinearizeThreshold: 0.01
relinearizeSkip: 1
zeroVelocitySigma: 0.001
noMotionPositionSigma: 0.001
noMotionRotationSigma: 0.0001
constantVelSigma: 0.01
numOptimize: 0
horizon: 6 # In seconds.
useDogLeg: 0
49 changes: 49 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/tracker/trackerParameters_rgbd.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
%YAML:1.0
#TRACKER PARAMETERS
klt_win_size: 24
klt_max_iter: 30
klt_max_level: 4
klt_eps: 0.1
maxFeatureAge: 10
maxFeaturesPerFrame: 600
quality_level: 0.001
min_distance: 20
block_size: 3
use_harris_detector: 0
k: 0.04
equalizeImage: 0
nominalBaseline: 0.05
toleranceTemplateMatching: 0.15
templ_cols: 101 #must be odd
templ_rows: 11
stripe_extra_rows: 0
minPointDist: 0.5
maxPointDist: 10
bidirectionalMatching: 0
subpixelRefinementStereo: 0
featureSelectionCriterion: 0
featureSelectionHorizon: 3
featureSelectionNrCornersToSelect: 600
featureSelectionImuRate: 0.005
featureSelectionDefaultDepth: 2
featureSelectionCosineNeighborhood: 0.984807753012208 # Rad
featureSelectionUseLazyEvaluation: 1
useSuccessProbabilities: 1
useRANSAC: 1
minNrMonoInliers: 10
minNrStereoInliers: 5
ransac_threshold_mono: 1e-06
ransac_threshold_stereo: 1
ransac_use_1point_stereo: 1
ransac_use_2point_mono: 1
ransac_max_iterations: 50
ransac_probability: 0.995
ransac_randomize: 0
intra_keyframe_time: 0.2
minNumberFeatures: 0
useStereoTracking: 1
disparityThreshold: 0.5
#RGB-D PARAMETERS
visionSensorType: 1
minDepthFactor: 0.3
mapDepthFactor: 0.001 # 1/1000
45 changes: 45 additions & 0 deletions kimera_vio_ros/param/RealSense_IR/tracker/trackerParameters_stereo.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
%YAML:1.0
#TRACKER PARAMETERS
klt_win_size: 24
klt_max_iter: 30
klt_max_level: 4
klt_eps: 0.1
maxFeatureAge: 10
maxFeaturesPerFrame: 300
quality_level: 0.001
min_distance: 20
block_size: 3
use_harris_detector: 0
k: 0.04
equalizeImage: 0
nominalBaseline: 0.05
toleranceTemplateMatching: 0.15
templ_cols: 101 #must be odd
templ_rows: 11
stripe_extra_rows: 0
minPointDist: 0.5
maxPointDist: 10
bidirectionalMatching: 0
subpixelRefinementStereo: 0
featureSelectionCriterion: 0
featureSelectionHorizon: 3
featureSelectionNrCornersToSelect: 600
featureSelectionImuRate: 0.005
featureSelectionDefaultDepth: 2
featureSelectionCosineNeighborhood: 0.984807753012208 # Rad
featureSelectionUseLazyEvaluation: 1
useSuccessProbabilities: 1
useRANSAC: 1
minNrMonoInliers: 10
minNrStereoInliers: 5
ransac_threshold_mono: 1e-06
ransac_threshold_stereo: 1
ransac_use_1point_stereo: 1
ransac_use_2point_mono: 1
ransac_max_iterations: 50
ransac_probability: 0.995
ransac_randomize: 0
intra_keyframe_time: 0.2
minNumberFeatures: 0
useStereoTracking: 1
disparityThreshold: 0.5
38 changes: 38 additions & 0 deletions kimera_vio_ros/src/components/stereo_vio.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
#include "kimera_vio_ros/components/stereo_vio.hpp"

namespace kimera_vio_ros
{
namespace components
{

StereoVio::StereoVio(
const rclcpp::NodeOptions & node_options)
: Node("stereo_vio", node_options)
{
init();
}

StereoVio::StereoVio(
const std::string & node_name,
const std::string & ns,
const rclcpp::NodeOptions & node_options)
: Node(node_name, ns, node_options)
{
init();
}

StereoVio::~StereoVio()
{
}

void StereoVio::init()
{
rclcpp::Node::SharedPtr node = std::shared_ptr<rclcpp::Node>(this);
vio_node_ = std::make_unique<interfaces::StereoVioInterface>(node);
}

} // namespace components
} // namespace kimera_vio_ros

#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(kimera_vio_ros::components::StereoVio)
227 changes: 227 additions & 0 deletions kimera_vio_ros/src/interfaces/backend_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,227 @@
#include "kimera_vio_ros/interfaces/backend_interface.hpp"
#include "kimera_vio_ros/utils/geometry.hpp"

namespace kimera_vio_ros
{
namespace interfaces
{

BackendInterface::BackendInterface(
rclcpp::Node::SharedPtr & node)
: BaseInterface(node),
backend_output_queue_("Backend output")
{
vio_pipeline_->registerBackendOutputCallback(
std::bind(
// &BackendInterface::callbackBackendOutput,
&BackendInterface::publishBackendOutput,
this,
std::placeholders::_1));

rclcpp::QoS qos(rclcpp::KeepLast(10));
odometry_pub_ = node_->create_publisher<Odometry>("odometry", qos);
pointcloud_pub_ = node_->create_publisher<PointCloud2>("time_horizon_pointcloud", qos);
}

BackendInterface::~BackendInterface()
{
backend_output_queue_.shutdown();
}

void BackendInterface::publishBackendOutput(
const VIO::BackendOutput::Ptr & output)
{
CHECK(output);
publishTf(output);
if (odometry_pub_->get_subscription_count() > 0) {
publishState(output);
}
// if (imu_bias_pub_.getNumSubscribers() > 0) {
// publishImuBias(output);
// }
if (pointcloud_pub_->get_subscription_count() > 0) {
publishTimeHorizonPointCloud(output);
}
}


void BackendInterface::publishState(
const VIO::BackendOutput::Ptr & output) const
{
CHECK(output);
// Get latest estimates for odometry.
const VIO::Timestamp & ts = output->timestamp_;
const gtsam::Pose3 & pose = output->W_State_Blkf_.pose_;
const gtsam::Rot3 & rotation = pose.rotation();
const gtsam::Quaternion & quaternion = rotation.toQuaternion();
const gtsam::Vector3 & velocity = output->W_State_Blkf_.velocity_;
const gtsam::Matrix6 & pose_cov =
gtsam::sub(output->state_covariance_lkf_, 0, 6, 0, 6);
const gtsam::Matrix3 & vel_cov =
gtsam::sub(output->state_covariance_lkf_, 6, 9, 6, 9);

// First publish odometry estimate
Odometry odometry_msg;

// Create header.
odometry_msg.header.stamp = rclcpp::Time(ts);
odometry_msg.header.frame_id = world_frame_id_;
odometry_msg.child_frame_id = base_link_frame_id_;

// Position
odometry_msg.pose.pose.position.x = pose.x();
odometry_msg.pose.pose.position.y = pose.y();
odometry_msg.pose.pose.position.z = pose.z();

// Orientation
odometry_msg.pose.pose.orientation.w = quaternion.w();
odometry_msg.pose.pose.orientation.x = quaternion.x();
odometry_msg.pose.pose.orientation.y = quaternion.y();
odometry_msg.pose.pose.orientation.z = quaternion.z();

// Remap covariance from GTSAM convention
// to odometry convention and fill in covariance
static const std::vector<int> remapping{3, 4, 5, 0, 1, 2};

// Position covariance first, angular covariance after
DCHECK_EQ(pose_cov.rows(), remapping.size());
DCHECK_EQ(pose_cov.rows() * pose_cov.cols(),
odometry_msg.pose.covariance.size());
for (int i = 0; i < pose_cov.rows(); i++) {
for (int j = 0; j < pose_cov.cols(); j++) {
odometry_msg.pose
.covariance[remapping[i] * pose_cov.cols() + remapping[j]] =
pose_cov(i, j);
}
}

// Linear velocities, trivial values for angular
const gtsam::Matrix3 & inversed_rotation = rotation.transpose();
const VIO::Vector3 velocity_body = inversed_rotation * velocity;
odometry_msg.twist.twist.linear.x = velocity_body(0);
odometry_msg.twist.twist.linear.y = velocity_body(1);
odometry_msg.twist.twist.linear.z = velocity_body(2);

// Velocity covariance: first linear
// and then angular (trivial values for angular)
const gtsam::Matrix3 vel_cov_body =
inversed_rotation.matrix() * vel_cov * rotation.matrix();
DCHECK_EQ(vel_cov_body.rows(), 3);
DCHECK_EQ(vel_cov_body.cols(), 3);
DCHECK_EQ(odometry_msg.twist.covariance.size(), 36);
for (int i = 0; i < vel_cov_body.rows(); i++) {
for (int j = 0; j < vel_cov_body.cols(); j++) {
odometry_msg.twist
.covariance[i * static_cast<int>(
sqrt(odometry_msg.twist.covariance.size())) +
j] = vel_cov_body(i, j);
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

}
}
// Publish message
odometry_pub_->publish(odometry_msg);
}

void BackendInterface::publishTf(const VIO::BackendOutput::Ptr & output)
{
CHECK(output);

const VIO::Timestamp & timestamp = output->timestamp_;
const gtsam::Pose3 & pose = output->W_State_Blkf_.pose_;
const gtsam::Quaternion & quaternion = pose.rotation().toQuaternion();
// Publish base_link TF.
TransformStamped odom_tf;
odom_tf.header.stamp = rclcpp::Time(timestamp);
odom_tf.header.frame_id = world_frame_id_;
odom_tf.child_frame_id = base_link_frame_id_;

utils::poseToMsgTF(pose, &odom_tf.transform);
tf_broadcaster_->sendTransform(odom_tf);
}

void BackendInterface::publishTimeHorizonPointCloud(
const VIO::BackendOutput::Ptr & output) const
{
CHECK(output);
const VIO::Timestamp & timestamp = output->timestamp_;
const VIO::PointsWithIdMap & points_with_id =
output->landmarks_with_id_map_;
const VIO::LmkIdToLmkTypeMap & lmk_id_to_lmk_type_map =
output->lmk_id_to_lmk_type_map_;

if (points_with_id.size() == 0) {
// No points to visualize.
return;
}

PointCloud2::UniquePtr pc_msg(new PointCloud2);
pc_msg->header.stamp = rclcpp::Time(timestamp);
pc_msg->header.frame_id = world_frame_id_;
pc_msg->width = points_with_id.size();
pc_msg->height = 1;
pc_msg->is_dense = true;
pc_msg->point_step = 3 * sizeof(float) + 3 * sizeof(uint8_t);
pc_msg->row_step = pc_msg->point_step * pc_msg->width;
pc_msg->is_dense = true;

sensor_msgs::PointCloud2Modifier modifier(*pc_msg);
modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
modifier.resize(pc_msg->width);

sensor_msgs::PointCloud2Iterator<float> iter_x(*pc_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*pc_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*pc_msg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*pc_msg, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*pc_msg, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*pc_msg, "b");

bool color_the_cloud = false;
if (lmk_id_to_lmk_type_map.size() != 0) {
color_the_cloud = true;
CHECK_EQ(points_with_id.size(), lmk_id_to_lmk_type_map.size());
}

// Populate cloud structure with 3D points.
size_t i = 0;
for (const std::pair<VIO::LandmarkId, gtsam::Point3> & id_point : points_with_id) {
const gtsam::Point3 point_3d = id_point.second;
*iter_x = static_cast<float>(point_3d.x());
*iter_y = static_cast<float>(point_3d.y());
*iter_z = static_cast<float>(point_3d.z());

if (color_the_cloud) {
DCHECK(lmk_id_to_lmk_type_map.find(id_point.first) !=
lmk_id_to_lmk_type_map.end());
switch (lmk_id_to_lmk_type_map.at(id_point.first)) {
case VIO::LandmarkType::SMART: {
*iter_r = 0;
*iter_g = 255;
*iter_b = 0;
break;
}
case VIO::LandmarkType::PROJECTION: {
*iter_r = 0;
*iter_g = 0;
*iter_b = 255;
break;
}
default: {
*iter_r = 255;
*iter_g = 0;
*iter_b = 0;
break;
}
}
}
++iter_x;
++iter_y;
++iter_z;
++iter_r;
++iter_g;
++iter_b;
i++;
}
pointcloud_pub_->publish(std::move(pc_msg));
}

} // namespace interfaces
} // namespace kimera_vio_ros
74 changes: 74 additions & 0 deletions kimera_vio_ros/src/interfaces/base_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
#include <chrono>

#include "kimera_vio_ros/interfaces/base_interface.hpp"

using namespace std::chrono_literals;

namespace kimera_vio_ros
{
namespace interfaces
{

BaseInterface::BaseInterface(
rclcpp::Node::SharedPtr & node)
: VIO::DataProviderInterface(),
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

node_(node),
vio_params_(nullptr),
vio_pipeline_(nullptr),
tf_buffer_(nullptr),
tf_broadcaster_(nullptr),
tf_listener_(nullptr)
{
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(node_);
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, node_, false);

callback_group_pipeline_ = node_->create_callback_group(
rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);

base_link_frame_id_ = node_->declare_parameter(
"frame_id.base_link", "base_link");
map_frame_id_ = node_->declare_parameter(
"frame_id.map", "map");
world_frame_id_ = node_->declare_parameter(
"frame_id.world", "world");

std::string params_folder_;
params_folder_ = node_->declare_parameter(
"params_folder", "");
CHECK(!params_folder_.empty());
vio_params_ = std::make_shared<VIO::VioParams>(
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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?

params_folder_);

vio_pipeline_.reset();
vio_pipeline_ = VIO::make_unique<VIO::Pipeline>(*vio_params_);
}

BaseInterface::~BaseInterface()
{
vio_pipeline_->shutdown();
if (vio_params_->parallel_run_) {
handle_pipeline_.get();
}
}

void BaseInterface::start()
{
if (vio_params_->parallel_run_) {
handle_pipeline_ = std::async(
std::launch::async,
&VIO::Pipeline::spin,
vio_pipeline_.get());
} else {
pipeline_timer_ = node_->create_wall_timer(
10ms,
std::bind(
&VIO::Pipeline::spin,
vio_pipeline_.get()),
callback_group_pipeline_);
}

}

} // namespace interfaces
} // namespace kimera_vio_ros
106 changes: 106 additions & 0 deletions kimera_vio_ros/src/interfaces/image_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
#include "kimera_vio_ros/interfaces/image_interface.hpp"
#include "kimera_vio_ros/utils/geometry.hpp"

namespace kimera_vio_ros
{
namespace interfaces
{

ImageInterface::ImageInterface(
rclcpp::Node::SharedPtr & node)
: BaseInterface(node)
{
}

ImageInterface::~ImageInterface()
{
}

void ImageInterface::msgCamInfoToCameraParams(
const CameraInfo::ConstSharedPtr & cam_info,
VIO::CameraParams * cam_params)
{
CHECK_NOTNULL(cam_params);

// Get intrinsics from incoming CameraInfo messages:
cam_params->camera_id_ = cam_info->header.frame_id;
CHECK(!cam_params->camera_id_.empty());

CHECK(cam_info->distortion_model == "plumb_bob" ||
cam_info->distortion_model == "equidistant");

if (cam_info->distortion_model == "plumb_bob") {
// 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);
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ToniRV it would be nice if Kimera-VIO understood the plumb_bob type to be the same as radtan:

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.

https://github.com/ros-perception/vision_opencv/blob/0bbebb73bea65eca856e732b919439853a04986b/image_geometry/src/pinhole_camera_model.cpp#L133-L148

} else {
cam_params->distortion_model_ = cam_info->distortion_model;
CHECK_EQ(cam_info->d.size(), 4);
}

const std::vector<double> & distortion_coeffs =
std::vector<double>(cam_info->d.begin(), cam_info->d.begin() + 4);

CHECK_EQ(distortion_coeffs.size(), 4);
VIO::CameraParams::convertDistortionVectorToMatrix(
distortion_coeffs, &cam_params->distortion_coeff_);

cam_params->image_size_ = cv::Size(cam_info->width, cam_info->height);

cam_params->frame_rate_ = 0; // TODO(marcus): is there a way to get this?

std::array<double, 4> intrinsics = {
cam_info->k[0], cam_info->k[4], cam_info->k[2], cam_info->k[5]};
cam_params->intrinsics_ = intrinsics;
VIO::CameraParams::convertIntrinsicsVectorToMatrix(
cam_params->intrinsics_, &cam_params->K_);

VIO::CameraParams::createGtsamCalibration(cam_params->distortion_coeff_,
cam_params->intrinsics_,
&cam_params->calibration_);

// Get extrinsics from the TF tree:
static constexpr size_t kTfLookupTimeout = 5u;
geometry_msgs::msg::TransformStamped cam_tf;

try {
cam_tf = tf_buffer_->lookupTransform(
base_link_frame_id_,
cam_params->camera_id_,
cam_info->header.stamp,
rclcpp::Duration(kTfLookupTimeout));
} catch (tf2::TransformException & ex) {
RCLCPP_FATAL(node_->get_logger(),
"TF for left/right camera frames not available. Either publish to "
"tree or provide CameraParameter yaml files.:\n%s", ex.what());
rclcpp::shutdown();
}

utils::msgTFtoPose(cam_tf.transform, &cam_params->body_Pose_cam_);
}

const cv::Mat ImageInterface::readRosImage(
const sensor_msgs::msg::Image::ConstSharedPtr & img_msg)
{
cv_bridge::CvImageConstPtr cv_constptr;
try {
cv_constptr = cv_bridge::toCvShare(img_msg);
} catch (cv_bridge::Exception & exception) {
// RCLCPP_FATAL(this->get_logger(), "cv_bridge exception: %s", exception.what());
// rclcpp::shutdown();
}

if (img_msg->encoding == sensor_msgs::image_encodings::BGR8) {
// LOG(WARNING) << "Converting image...";
cv::cvtColor(cv_constptr->image, cv_constptr->image, cv::COLOR_BGR2GRAY);
} else {
// CHECK_EQ(cv_constptr->encoding, sensor_msgs::image_encodings::MONO8)
// << "Expected image with MONO8 or BGR8 encoding.";
}

return cv_constptr->image;
}

} // namespace interfaces
} // namespace kimera_vio_ros
67 changes: 67 additions & 0 deletions kimera_vio_ros/src/interfaces/imu_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
#include "kimera_vio_ros/interfaces/imu_interface.hpp"

namespace kimera_vio_ros
{
namespace interfaces
{

ImuInterface::ImuInterface(
rclcpp::Node::SharedPtr & node)
: BaseInterface(node),
last_imu_timestamp_(0)
{
this->registerImuSingleCallback(
std::bind(
&VIO::Pipeline::fillSingleImuQueue,
vio_pipeline_.get(),
std::placeholders::_1));

this->registerImuMultiCallback(
std::bind(
&VIO::Pipeline::fillMultiImuQueue,
vio_pipeline_.get(),
std::placeholders::_1));

callback_group_imu_ = node->create_callback_group(
rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
auto imu_opt = rclcpp::SubscriptionOptions();
imu_opt.callback_group = callback_group_imu_;

std::string imu_topic = "imu/data";
auto qos = rclcpp::SensorDataQoS();
imu_sub_ = node->create_subscription<Imu>(
imu_topic,
qos,
std::bind(
&ImuInterface::imu_cb,
this,
std::placeholders::_1),
imu_opt);
}

ImuInterface::~ImuInterface()
{
}

void ImuInterface::imu_cb(const Imu::SharedPtr imu_msg)
{
rclcpp::Time stamp(imu_msg->header.stamp);
if (stamp.nanoseconds() > last_imu_timestamp_.nanoseconds()) {
VIO::Timestamp timestamp = stamp.nanoseconds();
VIO::ImuAccGyr imu_accgyr;

imu_accgyr(0) = imu_msg->linear_acceleration.x;
imu_accgyr(1) = imu_msg->linear_acceleration.y;
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;
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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?


this->imu_single_callback_(VIO::ImuMeasurement(timestamp, imu_accgyr));
// LOG_EVERY_N(INFO, 200) << "Done: KimeraVioNode::imu_cb";
}
last_imu_timestamp_ = stamp;
}

} // namespace interfaces
} // namespace kimera_vio_ros
157 changes: 157 additions & 0 deletions kimera_vio_ros/src/interfaces/stereo_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,157 @@
#include "kimera_vio_ros/interfaces/stereo_interface.hpp"

namespace kimera_vio_ros
{
namespace interfaces
{

StereoInterface::StereoInterface(
rclcpp::Node::SharedPtr & node)
: BaseInterface(node),
ImageInterface(node),
frame_count_(VIO::FrameId(0)),
last_stereo_timestamp_(0)
{
this->registerLeftFrameCallback(
std::bind(
&VIO::Pipeline::fillLeftFrameQueue,
vio_pipeline_.get(),
std::placeholders::_1));

this->registerRightFrameCallback(
std::bind(
&VIO::Pipeline::fillRightFrameQueue,
vio_pipeline_.get(),
std::placeholders::_1));

callback_group_stereo_ = node->create_callback_group(
rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
auto stereo_opt = rclcpp::SubscriptionOptions();
stereo_opt.callback_group = callback_group_stereo_;

int queue_size_ = 10;

auto info_qos = rclcpp::SystemDefaultsQoS();
std::string left_info_topic = "left/camera_info";
std::string right_info_topic = "right/camera_info";
left_info_sub_ = std::make_shared<message_filters::Subscriber<CameraInfo>>(
node_,
left_info_topic,
info_qos.get_rmw_qos_profile());
right_info_sub_ = std::make_shared<message_filters::Subscriber<CameraInfo>>(
node_,
right_info_topic,
info_qos.get_rmw_qos_profile());
exact_info_sync_ = std::make_shared<ExactInfoSync>(
ExactInfoPolicy(queue_size_), *left_info_sub_, *right_info_sub_);
exact_info_sync_->registerCallback(&StereoInterface::stereo_info_cb, this);

auto image_qos = rclcpp::SensorDataQoS();
std::string left_image_topic = "left/image";
std::string right_image_topic = "right/image";
// TODO: Perhaps switch to image_transport to support more transports
// std::string transport = "raw";
// image_transport::TransportHints hints(node, transport);
// left_sub_.subscribe(node, left_topic, hints.getTransport(), qos.get_rmw_qos_profile());
// right_sub_.subscribe(node, right_topic, hints.getTransport(), qos.get_rmw_qos_profile());
// exact_image_sync_ = std::make_shared<ExactImageSync>(
// ExactImagePolicy(queue_size_), left_sub_, right_sub_);

// TODO: Assign message filter subscribers to callback_group_stereo_
// Pending: https://github.com/ros2/message_filters/issues/45
left_image_sub_ = std::make_shared<message_filters::Subscriber<Image>>(
node_,
left_image_topic,
image_qos.get_rmw_qos_profile());
right_image_sub_ = std::make_shared<message_filters::Subscriber<Image>>(
node_,
right_image_topic,
image_qos.get_rmw_qos_profile());
exact_image_sync_ = std::make_shared<ExactImageSync>(
ExactImagePolicy(queue_size_), *left_image_sub_, *right_image_sub_);
exact_image_sync_->registerCallback(&StereoInterface::stereo_image_cb, this);

bool use_camera_info_;
use_camera_info_ = node_->declare_parameter("use_camera_info", true);
if (use_camera_info_) {
RCLCPP_INFO(node_->get_logger(),
"Using online camera parameters instead of YAML parameter files.");
camera_info_received_ = false;
left_image_sub_->unsubscribe();
right_image_sub_->unsubscribe();
} else {
RCLCPP_INFO(node_->get_logger(),
"Using YAML parameter files instead of online camera parameters.");
camera_info_received_ = true;
left_info_sub_->unsubscribe();
right_info_sub_->unsubscribe();
}
}

StereoInterface::~StereoInterface()
{
}

void StereoInterface::stereo_info_cb(
const CameraInfo::ConstSharedPtr & left_msg,
const CameraInfo::ConstSharedPtr & right_msg)
{
CHECK_GE(vio_params_->camera_params_.size(), 2u);

// Initialize CameraParams for pipeline.
msgCamInfoToCameraParams(
left_msg, &vio_params_->camera_params_.at(0));
msgCamInfoToCameraParams(
right_msg, &vio_params_->camera_params_.at(1));

vio_params_->camera_params_.at(0).print();
vio_params_->camera_params_.at(1).print();

// Signal the correct reception of camera info
camera_info_received_ = true;
if (camera_info_received_) {
RCLCPP_INFO(node_->get_logger(),
"Switching subscriptions from CameraInfo to Image after receiving camera parameters.");
// Unregister info callback as it is no longer needed.
left_info_sub_->unsubscribe();
right_info_sub_->unsubscribe();
// Reregister image callback as it is now ready.
left_image_sub_->subscribe();
right_image_sub_->subscribe();
}
}

void StereoInterface::stereo_image_cb(
const Image::SharedPtr left_msg,
const Image::SharedPtr right_msg)
{
rclcpp::Time left_stamp(left_msg->header.stamp);
rclcpp::Time right_stamp(right_msg->header.stamp);
if (left_stamp.nanoseconds() > last_stereo_timestamp_.nanoseconds()) {
static const VIO::CameraParams & left_cam_info =
vio_params_->camera_params_.at(0);
static const VIO::CameraParams & right_cam_info =
vio_params_->camera_params_.at(1);

const VIO::Timestamp & timestamp_left = left_stamp.nanoseconds();
const VIO::Timestamp & timestamp_right = right_stamp.nanoseconds();

// CHECK(left_frame_callback_)
// << "Did you forget to register the left frame callback?";
// CHECK(right_frame_callback_)
// << "Did you forget to register the right frame callback?";
// TODO: Use RCLCPP_INFO inplace of CHECK?
// 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)));
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

https://github.com/ruffsl/Kimera-VIO/blob/e5e799c631b7ae3b8d3afac323996a381297348b/src/dataprovider/EurocDataProvider.cpp#L147-L149

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.

right_frame_callback_(VIO::make_unique<VIO::Frame>(
frame_count_, timestamp_right, right_cam_info, readRosImage(right_msg)));
// LOG_EVERY_N(INFO, 30) << "Done: KimeraVioNode::stereo_image_cb";
frame_count_++;
}
last_stereo_timestamp_ = left_stamp;
}

} // namespace interfaces
} // namespace kimera_vio_ros
24 changes: 24 additions & 0 deletions kimera_vio_ros/src/interfaces/stereo_vio_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
#include "kimera_vio_ros/interfaces/stereo_vio_interface.hpp"

namespace kimera_vio_ros
{
namespace interfaces
{

StereoVioInterface::StereoVioInterface(
rclcpp::Node::SharedPtr & node)
: BaseInterface(node),
ImageInterface(node),
ImuInterface(node),
StereoInterface(node),
BackendInterface(node)
{
BaseInterface::start();
}

StereoVioInterface::~StereoVioInterface()
{
}

} // namespace interfaces
} // namespace kimera_vio_ros
23 changes: 23 additions & 0 deletions kimera_vio_ros/src/stereo_vio_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "kimera_vio_ros/components/stereo_vio.hpp"

using StereoVio = kimera_vio_ros::components::StereoVio;

int main(int argc, char * argv[])
{
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);
Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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?


// Initialize Google's logging library.
google::InitGoogleLogging(argv[0]);

rclcpp::executors::MultiThreadedExecutor executor;
auto stereo_vio_node = std::make_shared<StereoVio>();
executor.add_node(stereo_vio_node);
executor.spin();
rclcpp::shutdown();
return 0;
}
33 changes: 33 additions & 0 deletions kimera_vio_ros/src/utils/geometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
#include "kimera_vio_ros/utils/geometry.hpp"

namespace kimera_vio_ros
{
namespace utils
{

void msgTFtoPose(const geometry_msgs::msg::Transform & tf, gtsam::Pose3 * pose)
{
CHECK_NOTNULL(pose);

*pose = gtsam::Pose3(
gtsam::Rot3(gtsam::Quaternion(
tf.rotation.w, tf.rotation.x, tf.rotation.y, tf.rotation.z)),
gtsam::Point3(tf.translation.x, tf.translation.y, tf.translation.z));
}

void poseToMsgTF(const gtsam::Pose3 & pose, geometry_msgs::msg::Transform * tf)
{
CHECK_NOTNULL(tf);

tf->translation.x = pose.x();
tf->translation.y = pose.y();
tf->translation.z = pose.z();
const gtsam::Quaternion & quat = pose.rotation().toQuaternion();
tf->rotation.w = quat.w();
tf->rotation.x = quat.x();
tf->rotation.y = quat.y();
tf->rotation.z = quat.z();
}

} // namespace utils
} // namespace kimera_vio_ros