diff --git a/examples/cpp/stereo_vision_slam/README.md b/examples/cpp/stereo_vision_slam/README.md new file mode 100644 index 000000000000..70a978187c3c --- /dev/null +++ b/examples/cpp/stereo_vision_slam/README.md @@ -0,0 +1,182 @@ + + +Visualizes stereo vision SLAM on the [KITTI dataset](https://www.cvlibs.net/datasets/kitti/). + + + + + + + + + +# Used Rerun types + +[`Image`](https://www.rerun.io/docs/reference/types/archetypes/image), [`LineStrips3D`](https://rerun.io/docs/reference/types/archetypes/line_strips3d), [`Scalar`](https://rerun.io/docs/reference/types/archetypes/scalar), [`Transform3D`](https://rerun.io/docs/reference/types/archetypes/transform3d), [`Pinhole`](https://rerun.io/docs/reference/types/archetypes/pinhole), [`Points3D`](https://rerun.io/docs/reference/types/archetypes/points3d), [`TextLog`](https://rerun.io/docs/reference/types/archetypes/text_log) + + +# Background + +This example shows [farhad-dalirani's stereo visual SLAM implementation](https://github.com/farhad-dalirani/StereoVision-SLAM). It's input is the video footage from a stereo camera and it produces the trajectory of the vehicle and a point cloud of the surrounding environment. + +# Logging and visualizing with Rerun + +To easily use Opencv/Eigen types and avoid copying images/points when logging to Rerun it uses [`CollectionAdapter`](https://ref.rerun.io/docs/cpp/stable/structrerun_1_1CollectionAdapter.html) with the following code: +```cpp + +template <> +struct rerun::CollectionAdapter +{ + /* Adapters to borrow an OpenCV image into Rerun + * images without copying */ + + Collection operator()(const cv::Mat& img) + { + // Borrow for non-temporary. + + assert("OpenCV matrix expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U); + return Collection::borrow(img.data, img.total() * img.channels()); + } + + Collection operator()(cv::Mat&& img) + { + /* Do a full copy for temporaries (otherwise the data + * might be deleted when the temporary is destroyed). */ + + assert("OpenCV matrix expected have bit depth CV_U8" && CV_MAT_DEPTH(img.type()) == CV_8U); + std::vector img_vec(img.total() * img.channels()); + img_vec.assign(img.data, img.data + img.total() * img.channels()); + return Collection::take_ownership(std::move(img_vec)); + } +}; + + +template <> +struct rerun::CollectionAdapter> +{ + /* Adapters to log eigen vectors as rerun positions*/ + + Collection operator()(const std::vector& container) + { + // Borrow for non-temporary. + return Collection::borrow(container.data(), container.size()); + } + + Collection operator()(std::vector&& container) + { + /* Do a full copy for temporaries (otherwise the data + * might be deleted when the temporary is destroyed). */ + std::vector positions(container.size()); + memcpy(positions.data(), container.data(), container.size() * sizeof(Eigen::Vector3f)); + return Collection::take_ownership(std::move(positions)); + } +}; + + +template <> +struct rerun::CollectionAdapter +{ + /* Adapters so we can log an eigen matrix as rerun positions */ + + // Sanity check that this is binary compatible. + static_assert( + sizeof(rerun::Position3D) == sizeof(Eigen::Matrix3Xf::Scalar) * Eigen::Matrix3Xf::RowsAtCompileTime + ); + + Collection operator()(const Eigen::Matrix3Xf& matrix) + { + // Borrow for non-temporary. + static_assert(alignof(rerun::Position3D) <= alignof(Eigen::Matrix3Xf::Scalar)); + return Collection::borrow( + // Cast to void because otherwise Rerun will try to do above sanity checks with the wrong type (scalar). + reinterpret_cast(matrix.data()), + matrix.cols() + ); + } + + Collection operator()(Eigen::Matrix3Xf&& matrix) + { + /* Do a full copy for temporaries (otherwise the + * data might be deleted when the temporary is destroyed). */ + std::vector positions(matrix.cols()); + memcpy(positions.data(), matrix.data(), matrix.size() * sizeof(rerun::Position3D)); + return Collection::take_ownership(std::move(positions)); + } +}; + +``` + +## Images +```cpp +// Draw stereo left image +rec.log(entity_name, + rerun::Image(tensor_shape(kf_sort[0].second->left_img_), + rerun::TensorBuffer::u8(kf_sort[0].second->left_img_))); +``` + +## Pinhole camera + +The camera frames shown in the space view is generated by the following code: + +```cpp +rec.log(entity_name, + rerun::Transform3D( + rerun::Vec3D(camera_position.data()), + rerun::Mat3x3(camera_orientation.data()), true) +); +// … +rec.log(entity_name, + rerun::Pinhole::from_focal_length_and_resolution({fx, fy}, {img_num_cols, img_num_rows})); +``` + +## Time series +```cpp +void Viewer::Plot(std::string plot_name, double value, unsigned long maxkeyframe_id) +{ + // … + rec.set_time_sequence("max_keyframe_id", maxkeyframe_id); + rec.log(plot_name, rerun::Scalar(value)); +} +``` + +## Trajectory +```cpp +rec.log("world/path", + rerun::Transform3D( + rerun::Vec3D(camera_position.data()), + rerun::Mat3x3(camera_orientation.data()), true)); + +std::vector path; +// … +rec.log("world/path", rerun::LineStrips3D(rerun::LineStrip3D(path))); +``` + +## Point cloud +```cpp +rec.log("world/landmarks", + rerun::Transform3D( + rerun::Vec3D(camera_position.data()), + rerun::Mat3x3(camera_orientation.data()), true)); + +std::vector points3d_vector; +// … +rec.log("world/landmarks", rerun::Points3D(points3d_vector)); +``` + +## Text log + +```cpp +rec.log("world/log", rerun::TextLog(msg).with_color(log_color.at(log_type))); +// … +rec.log("world/log", rerun::TextLog("Finished")); +``` + +# Run the code + +This is an external example, check the [repository](https://github.com/rerun-io/StereoVision-SLAM) on how to run the code. diff --git a/examples/manifest.toml b/examples/manifest.toml index b727acd16cec..7db196c189ba 100644 --- a/examples/manifest.toml +++ b/examples/manifest.toml @@ -93,6 +93,7 @@ examples = [ "open_photogrammetry_format", "kiss-icp", "differentiable_blocks_world", + "stereo_vision_slam", "signed_distance_fields", "raw_mesh", ]