Skip to content

Commit e4709bb

Browse files
przembpb-ait
przemb
authored and
pb-ait
committed
Extend and refactor log_point_cloud2
* log_point_cloud2 refactored to avoid redundant allocations * PointCloudProcessor and ColorMapper added * reversed colormap added * ColormapsLUT::supportedColormaps added * additional compilation flags added * rerun-sdk verison updated
1 parent bc8cb32 commit e4709bb

File tree

11 files changed

+448
-212
lines changed

11 files changed

+448
-212
lines changed

‎rerun_bridge/CMakeLists.txt‎

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@ project(rerun_bridge)
44
if(NOT DEFINED CMAKE_CXX_STANDARD)
55
set(CMAKE_CXX_STANDARD 17)
66
endif()
7+
add_compile_options(-Wall -Wextra -Wpedantic)
78

89
# Avoid warning about CMP0135
910
if(CMAKE_VERSION VERSION_GREATER_EQUAL "3.24.0")
@@ -22,11 +23,16 @@ find_package(OpenCV REQUIRED)
2223
find_package(yaml-cpp REQUIRED)
2324

2425
include(FetchContent)
25-
FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.0/rerun_cpp_sdk.zip)
26+
FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.17.0/rerun_cpp_sdk.zip)
2627
FetchContent_MakeAvailable(rerun_sdk)
2728

2829
# setup targets (has to be done before ament_package call)
29-
add_library(${PROJECT_NAME} src/rerun_bridge/rerun_ros_interface.cpp)
30+
add_library(${PROJECT_NAME}
31+
src/rerun_bridge/rerun_ros_interface.cpp
32+
src/rerun_bridge/color_maps.cpp
33+
src/rerun_bridge/point_cloud_processor.cpp
34+
src/rerun_bridge/color_mapper.cpp
35+
)
3036
add_executable(visualizer src/rerun_bridge/visualizer_node.cpp)
3137

3238
# add system dependencies

‎rerun_bridge/include/rerun_bridge/color_mapper.hpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
#pragma once
2+
3+
#include <optional>
4+
#include <string>
5+
#include <vector>
6+
7+
#include "sensor_msgs/point_cloud2_iterator.hpp"
8+
9+
#include "rerun_bridge/color_maps.hpp"
10+
#include "rerun_bridge/rerun_ros_interface.hpp"
11+
12+
using pclConstIter = sensor_msgs::PointCloud2ConstIterator<float>;
13+
14+
struct PointCloud2Options {
15+
std::optional<std::string> colormapName;
16+
std::optional<std::string> colormapField;
17+
std::optional<float> colormapMin;
18+
std::optional<float> colormapMax;
19+
};
20+
21+
class ColorMapper {
22+
std::optional<colormapLUT> colormap_{};
23+
std::vector<float> fieldValues_{};
24+
std::vector<rerun::Color> colors_{};
25+
26+
std::pair<float, float> getMinMax(const std::vector<float>& values) const;
27+
std::size_t calculateIdx(const float& value, const float& minValue, const float& maxValue)
28+
const;
29+
void setColormap();
30+
std::string toUppercase(std::string str) const;
31+
void convertPclMsgToColorapFieldVec(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg);
32+
void remapValuesToColors();
33+
34+
public:
35+
// data from yaml file
36+
const PointCloud2Options options_{};
37+
38+
ColorMapper(const PointCloud2Options& options);
39+
void calculateRerunColors(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg);
40+
bool isValid() const;
41+
void reserve(const size_t& size);
42+
void clear();
43+
const std::vector<rerun::components::Color>& getColors() const;
44+
};

‎rerun_bridge/include/rerun_bridge/color_maps.hpp‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
#pragma once
2+
3+
#include <array>
4+
#include <unordered_map>
5+
6+
using colormapLUT = std::array<std::array<float, 3>, 256>;
7+
8+
namespace ColormapsLUT {
9+
extern const std::unordered_map<std::string, colormapLUT> supportedColormaps;
10+
11+
extern const colormapLUT Turbo;
12+
extern const colormapLUT Rainbow;
13+
} // namespace ColormapsLUT

‎rerun_bridge/include/rerun_bridge/point_cloud_processor.hpp‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
#pragma once
2+
3+
#include <algorithm>
4+
#include <optional>
5+
#include <vector>
6+
7+
#include "rerun.hpp"
8+
#include "rerun_bridge/color_mapper.hpp" // PointCloud2Options
9+
#include "rerun_bridge/rerun_ros_interface.hpp"
10+
11+
class PointCloudProcessor {
12+
ColorMapper colorMapper_;
13+
std::size_t maxNumPerMsg_{};
14+
std::vector<rerun::Position3D> points_{};
15+
16+
std::pair<std::optional<float>, std::optional<float>> getMinMax(const std::vector<float>& values
17+
) const;
18+
19+
void reserve(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg);
20+
bool isMsgLonger(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) const;
21+
const std::vector<rerun::components::Position3D>& convertPclMsgToPosition3DVec(
22+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg
23+
);
24+
void convertPclMsgToColorMapFieldVec(
25+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, std::vector<float>& values
26+
) const;
27+
void convertPclMsgToPosition3DVecIter(
28+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg,
29+
std::vector<rerun::components::Position3D>& points
30+
) const;
31+
bool areFieldNamesSupported(
32+
const rerun::RecordingStream& rec, const std::string& entity_path,
33+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg
34+
) const;
35+
bool isFieldTypeFloat(
36+
const rerun::RecordingStream& rec, const std::string& entity_path,
37+
const sensor_msgs::msg::PointField& field
38+
) const;
39+
40+
public:
41+
PointCloudProcessor(const PointCloud2Options& options);
42+
void logPointCloud2(
43+
const rerun::RecordingStream& rec, const std::string& entity_path,
44+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg
45+
);
46+
};

‎rerun_bridge/include/rerun_bridge/rerun_ros_interface.hpp‎

Lines changed: 6 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#pragma once
22

33
#include <map>
4+
#include <optional>
45
#include <string>
56

67
#include <geometry_msgs/msg/pose_stamped.hpp>
@@ -14,16 +15,16 @@
1415

1516
#include <rerun.hpp>
1617

18+
struct ImageOptions {
19+
std::optional<float> min_depth{};
20+
std::optional<float> max_depth{};
21+
};
22+
1723
void log_imu(
1824
const rerun::RecordingStream& rec, const std::string& entity_path,
1925
const sensor_msgs::msg::Imu::ConstSharedPtr& msg
2026
);
2127

22-
struct ImageOptions {
23-
std::optional<float> min_depth;
24-
std::optional<float> max_depth;
25-
};
26-
2728
void log_image(
2829
const rerun::RecordingStream& rec, const std::string& entity_path,
2930
const sensor_msgs::msg::Image::ConstSharedPtr& msg, const ImageOptions& options = ImageOptions{}
@@ -54,15 +55,3 @@ void log_transform(
5455
const rerun::RecordingStream& rec, const std::string& entity_path,
5556
const geometry_msgs::msg::TransformStamped::ConstSharedPtr& msg
5657
);
57-
58-
struct PointCloud2Options {
59-
std::optional<std::string> colormap;
60-
std::optional<std::string> colormap_field;
61-
std::optional<float> colormap_min;
62-
std::optional<float> colormap_max;
63-
};
64-
65-
void log_point_cloud2(
66-
const rerun::RecordingStream& rec, const std::string& entity_path,
67-
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg, const PointCloud2Options& options
68-
);

‎rerun_bridge/src/rerun_bridge/color_mapper.cpp‎

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
#include <algorithm>
2+
#include <cctype> // std::toupper
3+
#include <iostream>
4+
#include <optional>
5+
#include <string>
6+
7+
#include "rerun_bridge/color_mapper.hpp"
8+
#include "rerun_bridge/color_maps.hpp"
9+
10+
ColorMapper::ColorMapper(const PointCloud2Options& options) : options_{options} {
11+
setColormap();
12+
}
13+
14+
bool ColorMapper::isValid() const {
15+
return (colormap_.has_value() && options_.colormapField.has_value());
16+
}
17+
18+
void ColorMapper::calculateRerunColors(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) {
19+
convertPclMsgToColorapFieldVec(msg);
20+
remapValuesToColors();
21+
}
22+
23+
std::size_t ColorMapper::calculateIdx(
24+
const float& value, const float& minValue, const float& maxValue
25+
) const {
26+
return static_cast<size_t>(255 * (value - minValue) / (maxValue - minValue));
27+
}
28+
29+
std::pair<float, float> ColorMapper::getMinMax(const std::vector<float>& values) const {
30+
float min_value{0}, max_value{0};
31+
if (!options_.colormapMin) {
32+
min_value = *std::min_element(values.begin(), values.end());
33+
} else {
34+
min_value = *options_.colormapMin;
35+
}
36+
37+
if (!options_.colormapMax) {
38+
max_value = *std::max_element(values.begin(), values.end());
39+
} else {
40+
max_value = *options_.colormapMax;
41+
}
42+
return {min_value, max_value};
43+
}
44+
45+
void ColorMapper::reserve(const size_t& size) {
46+
colors_.reserve(size);
47+
fieldValues_.reserve(size);
48+
}
49+
50+
void ColorMapper::clear() {
51+
colors_.clear();
52+
fieldValues_.clear();
53+
}
54+
55+
std::string ColorMapper::toUppercase(std::string str) const {
56+
std::transform(str.begin(), str.end(), str.begin(), [](unsigned char c) {
57+
return std::toupper(c);
58+
});
59+
return str;
60+
}
61+
62+
void ColorMapper::setColormap() {
63+
if (!options_.colormapName.has_value()) {
64+
return;
65+
}
66+
67+
// name from YAML file
68+
auto colormapName = toUppercase(*options_.colormapName);
69+
auto it = ColormapsLUT::supportedColormaps.find(colormapName);
70+
if (it != ColormapsLUT::supportedColormaps.end()) {
71+
colormap_ = it->second;
72+
} else {
73+
std::cout << "Colormap: " << colormapName << " is not supported" << std::endl;
74+
}
75+
}
76+
77+
void ColorMapper::convertPclMsgToColorapFieldVec(
78+
const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg
79+
) {
80+
auto iterColorField = pclConstIter(*msg, options_.colormapField.value());
81+
for (; iterColorField != iterColorField.end(); ++iterColorField) {
82+
fieldValues_.emplace_back(*iterColorField);
83+
}
84+
}
85+
86+
void ColorMapper::remapValuesToColors() {
87+
if (!options_.colormapName || !colormap_) {
88+
return;
89+
}
90+
91+
const auto [min, max] = getMinMax(fieldValues_);
92+
for (const auto& value : fieldValues_) {
93+
const auto idx = calculateIdx(value, min, max);
94+
colors_.emplace_back((*colormap_)[idx][0], (*colormap_)[idx][1], (*colormap_)[idx][2]);
95+
}
96+
}
97+
98+
const std::vector<rerun::components::Color>& ColorMapper::getColors() const {
99+
return colors_;
100+
}

‎rerun_bridge/src/rerun_bridge/color_maps.cpp‎

Lines changed: 84 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,84 @@
1+
#include <array>
2+
#include <string>
3+
#include <tuple>
4+
#include <unordered_map>
5+
6+
#include "rerun_bridge/color_maps.hpp"
7+
8+
constexpr colormapLUT reverse(const colormapLUT& input) {
9+
colormapLUT reversed{};
10+
constexpr std::size_t N = std::tuple_size<colormapLUT>::value;
11+
for (std::size_t i = 0; i < N; ++i) {
12+
reversed[i] = input[N - 1 - i];
13+
}
14+
return reversed;
15+
}
16+
17+
namespace ColormapsLUT {
18+
const std::unordered_map<std::string, colormapLUT> supportedColormaps = {
19+
{"TURBO", Turbo}, {"RAINBOW", Rainbow}};
20+
21+
// Turbo colormap lookup table
22+
// from: https://gist.github.com/mikhailov-work/6a308c20e494d9e0ccc29036b28faa7a
23+
// Copyright 2019 Google LLC.
24+
// SPDX-License-Identifier: Apache-2.0
25+
//
26+
// Author: Anton Mikhailov
27+
28+
constexpr colormapLUT Turbo = {
29+
{{48, 18, 59}, {50, 21, 67}, {51, 24, 74}, {52, 27, 81}, {53, 30, 88},
30+
{54, 33, 95}, {55, 36, 102}, {56, 39, 109}, {57, 42, 115}, {58, 45, 121},
31+
{59, 47, 128}, {60, 50, 134}, {61, 53, 139}, {62, 56, 145}, {63, 59, 151},
32+
{63, 62, 156}, {64, 64, 162}, {65, 67, 167}, {65, 70, 172}, {66, 73, 177},
33+
{66, 75, 181}, {67, 78, 186}, {68, 81, 191}, {68, 84, 195}, {68, 86, 199},
34+
{69, 89, 203}, {69, 92, 207}, {69, 94, 211}, {70, 97, 214}, {70, 100, 218},
35+
{70, 102, 221}, {70, 105, 224}, {70, 107, 227}, {71, 110, 230}, {71, 113, 233},
36+
{71, 115, 235}, {71, 118, 238}, {71, 120, 240}, {71, 123, 242}, {70, 125, 244},
37+
{70, 128, 246}, {70, 130, 248}, {70, 133, 250}, {70, 135, 251}, {69, 138, 252},
38+
{69, 140, 253}, {68, 143, 254}, {67, 145, 254}, {66, 148, 255}, {65, 150, 255},
39+
{64, 153, 255}, {62, 155, 254}, {61, 158, 254}, {59, 160, 253}, {58, 163, 252},
40+
{56, 165, 251}, {55, 168, 250}, {53, 171, 248}, {51, 173, 247}, {49, 175, 245},
41+
{47, 178, 244}, {46, 180, 242}, {44, 183, 240}, {42, 185, 238}, {40, 188, 235},
42+
{39, 190, 233}, {37, 192, 231}, {35, 195, 228}, {34, 197, 226}, {32, 199, 223},
43+
{31, 201, 221}, {30, 203, 218}, {28, 205, 216}, {27, 208, 213}, {26, 210, 210},
44+
{26, 212, 208}, {25, 213, 205}, {24, 215, 202}, {24, 217, 200}, {24, 219, 197},
45+
{24, 221, 194}, {24, 222, 192}, {24, 224, 189}, {25, 226, 187}, {25, 227, 185},
46+
{26, 228, 182}, {28, 230, 180}, {29, 231, 178}, {31, 233, 175}, {32, 234, 172},
47+
{34, 235, 170}, {37, 236, 167}, {39, 238, 164}, {42, 239, 161}, {44, 240, 158},
48+
{47, 241, 155}, {50, 242, 152}, {53, 243, 148}, {56, 244, 145}, {60, 245, 142},
49+
{63, 246, 138}, {67, 247, 135}, {70, 248, 132}, {74, 248, 128}, {78, 249, 125},
50+
{82, 250, 122}, {85, 250, 118}, {89, 251, 115}, {93, 252, 111}, {97, 252, 108},
51+
{101, 253, 105}, {105, 253, 102}, {109, 254, 98}, {113, 254, 95}, {117, 254, 92},
52+
{121, 254, 89}, {125, 255, 86}, {128, 255, 83}, {132, 255, 81}, {136, 255, 78},
53+
{139, 255, 75}, {143, 255, 73}, {146, 255, 71}, {150, 254, 68}, {153, 254, 66},
54+
{156, 254, 64}, {159, 253, 63}, {161, 253, 61}, {164, 252, 60}, {167, 252, 58},
55+
{169, 251, 57}, {172, 251, 56}, {175, 250, 55}, {177, 249, 54}, {180, 248, 54},
56+
{183, 247, 53}, {185, 246, 53}, {188, 245, 52}, {190, 244, 52}, {193, 243, 52},
57+
{195, 241, 52}, {198, 240, 52}, {200, 239, 52}, {203, 237, 52}, {205, 236, 52},
58+
{208, 234, 52}, {210, 233, 53}, {212, 231, 53}, {215, 229, 53}, {217, 228, 54},
59+
{219, 226, 54}, {221, 224, 55}, {223, 223, 55}, {225, 221, 55}, {227, 219, 56},
60+
{229, 217, 56}, {231, 215, 57}, {233, 213, 57}, {235, 211, 57}, {236, 209, 58},
61+
{238, 207, 58}, {239, 205, 58}, {241, 203, 58}, {242, 201, 58}, {244, 199, 58},
62+
{245, 197, 58}, {246, 195, 58}, {247, 193, 58}, {248, 190, 57}, {249, 188, 57},
63+
{250, 186, 57}, {251, 184, 56}, {251, 182, 55}, {252, 179, 54}, {252, 177, 54},
64+
{253, 174, 53}, {253, 172, 52}, {254, 169, 51}, {254, 167, 50}, {254, 164, 49},
65+
{254, 161, 48}, {254, 158, 47}, {254, 155, 45}, {254, 153, 44}, {254, 150, 43},
66+
{254, 147, 42}, {254, 144, 41}, {253, 141, 39}, {253, 138, 38}, {252, 135, 37},
67+
{252, 132, 35}, {251, 129, 34}, {251, 126, 33}, {250, 123, 31}, {249, 120, 30},
68+
{249, 117, 29}, {248, 114, 28}, {247, 111, 26}, {246, 108, 25}, {245, 105, 24},
69+
{244, 102, 23}, {243, 99, 21}, {242, 96, 20}, {241, 93, 19}, {240, 91, 18},
70+
{239, 88, 17}, {237, 85, 16}, {236, 83, 15}, {235, 80, 14}, {234, 78, 13},
71+
{232, 75, 12}, {231, 73, 12}, {229, 71, 11}, {228, 69, 10}, {226, 67, 10},
72+
{225, 65, 9}, {223, 63, 8}, {221, 61, 8}, {220, 59, 7}, {218, 57, 7},
73+
{216, 55, 6}, {214, 53, 6}, {212, 51, 5}, {210, 49, 5}, {208, 47, 5},
74+
{206, 45, 4}, {204, 43, 4}, {202, 42, 4}, {200, 40, 3}, {197, 38, 3},
75+
{195, 37, 3}, {193, 35, 2}, {190, 33, 2}, {188, 32, 2}, {185, 30, 2},
76+
{183, 29, 2}, {180, 27, 1}, {178, 26, 1}, {175, 24, 1}, {172, 23, 1},
77+
{169, 22, 1}, {167, 20, 1}, {164, 19, 1}, {161, 18, 1}, {158, 16, 1},
78+
{155, 15, 1}, {152, 14, 1}, {149, 13, 1}, {146, 11, 1}, {142, 10, 1},
79+
{139, 9, 2}, {136, 8, 2}, {133, 7, 2}, {129, 6, 2}, {126, 5, 2},
80+
{122, 4, 3}}};
81+
82+
constexpr colormapLUT Rainbow = reverse(Turbo);
83+
84+
} // namespace ColormapsLUT

0 commit comments

Comments
 (0)