Stereo vision SLAM

Cpp source

Visualizes stereo vision SLAM on the KITTI dataset.

Used Rerun types

Image, LineStrips3D, Scalar, Transform3D, Pinhole, Points3D, TextLog

Background

This example shows farhad-dalirani's stereo visual SLAM implementation. 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 with the following code:


template <>
struct rerun::CollectionAdapter<uint8_t, cv::Mat>
{
    /* Adapters to borrow an OpenCV image into Rerun
     * images without copying */

    Collection<uint8_t> 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<uint8_t>::borrow(img.data, img.total() * img.channels());
    }

    Collection<uint8_t> 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<uint8_t> img_vec(img.total() * img.channels());
        img_vec.assign(img.data, img.data + img.total() * img.channels());
        return Collection<uint8_t>::take_ownership(std::move(img_vec));
    }
};


template <>
struct rerun::CollectionAdapter<rerun::Position3D, std::vector<Eigen::Vector3f>>
{
    /* Adapters to log eigen vectors as rerun positions*/

    Collection<rerun::Position3D> operator()(const std::vector<Eigen::Vector3f>& container)
    {
        // Borrow for non-temporary.
        return Collection<rerun::Position3D>::borrow(container.data(), container.size());
    }

    Collection<rerun::Position3D> operator()(std::vector<Eigen::Vector3f>&& container)
    {
        /* Do a full copy for temporaries (otherwise the data
         * might be deleted when the temporary is destroyed). */
        std::vector<rerun::Position3D> positions(container.size());
        memcpy(positions.data(), container.data(), container.size() * sizeof(Eigen::Vector3f));
        return Collection<rerun::Position3D>::take_ownership(std::move(positions));
    }
};


template <>
struct rerun::CollectionAdapter<rerun::Position3D, Eigen::Matrix3Xf>
{
    /* 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<rerun::Position3D> operator()(const Eigen::Matrix3Xf& matrix)
    {
        // Borrow for non-temporary.
        static_assert(alignof(rerun::Position3D) <= alignof(Eigen::Matrix3Xf::Scalar));
        return Collection<rerun::Position3D>::borrow(
            // Cast to void because otherwise Rerun will try to do above sanity checks with the wrong type (scalar).
            reinterpret_cast<const void*>(matrix.data()),
            matrix.cols()
        );
    }

    Collection<rerun::Position3D> operator()(Eigen::Matrix3Xf&& matrix)
    {
        /* Do a full copy for temporaries (otherwise the
         * data might be deleted when the temporary is destroyed). */
        std::vector<rerun::Position3D> positions(matrix.cols());
        memcpy(positions.data(), matrix.data(), matrix.size() * sizeof(rerun::Position3D));
        return Collection<rerun::Position3D>::take_ownership(std::move(positions));
    }
};

Images images

// 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 pinhole-camera

The camera frames shown in the space view is generated by the following code:

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 time-series

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 trajectory

rec.log("world/path",
    rerun::Transform3D(
        rerun::Vec3D(camera_position.data()),
        rerun::Mat3x3(camera_orientation.data()), true));

std::vector<rerun::datatypes::Vec3D> path;
// …
rec.log("world/path", rerun::LineStrips3D(rerun::LineStrip3D(path)));

Point cloud point-cloud

rec.log("world/landmarks",
    rerun::Transform3D(
        rerun::Vec3D(camera_position.data()),
        rerun::Mat3x3(camera_orientation.data()), true));

std::vector<Eigen::Vector3f> points3d_vector;
// …
rec.log("world/landmarks", rerun::Points3D(points3d_vector));

Text log text-log

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 on how to run the code.