├── CMakeLists.txt
├── README.md
├── config
    └── param.yaml
├── image
    ├── robione_0.png
    ├── robione_1.png
    ├── slam_0.png
    ├── slam_1.png
    ├── vls128_0.png
    └── vls128_1.png
├── include
    └── color_point_cloud
    │   ├── ColorPointCloud.hpp
    │   ├── data_type
    │       ├── Camera.hpp
    │       └── PointCloudType.hpp
    │   └── utils
    │       └── TransformeProvider.hpp
├── launch
    └── color_point_cloud.launch.xml
├── package.xml
└── src
    ├── ColorPointCloud.cpp
    └── color_point_cloud_node.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.8)
 2 | project(color_point_cloud)
 3 | 
 4 | find_package(OpenCV REQUIRED)
 5 | find_package(Eigen3 REQUIRED)
 6 | 
 7 | find_package(ament_cmake REQUIRED)
 8 | find_package(rclcpp REQUIRED)
 9 | find_package(sensor_msgs REQUIRED)
10 | find_package(geometry_msgs REQUIRED)
11 | find_package(cv_bridge REQUIRED)
12 | find_package(tf2_ros REQUIRED)
13 | find_package(tf2_eigen REQUIRED)
14 | 
15 | include_directories(include
16 |     ${OpenCV_INCLUDE_DIRS}
17 |     ${EIGEN3_INCLUDE_DIRS}
18 | )
19 | 
20 | add_executable(color_point_cloud
21 |         src/color_point_cloud_node.cpp
22 |         src/ColorPointCloud.cpp)
23 | 
24 | ament_target_dependencies(color_point_cloud
25 |         rclcpp
26 |         sensor_msgs
27 |         geometry_msgs
28 |         cv_bridge
29 |         tf2_ros
30 |         tf2_eigen
31 | )
32 | 
33 | target_link_libraries(color_point_cloud
34 |         ${OpenCV_LIBRARIES}
35 |         ${EIGEN3_LIBRARIES}
36 | )
37 | 
38 | install(DIRECTORY
39 |         launch
40 |         config
41 |         DESTINATION share/${PROJECT_NAME}/
42 | )
43 | 
44 | install(TARGETS color_point_cloud
45 |         DESTINATION lib/${PROJECT_NAME})
46 | 
47 | ament_package()
48 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # color-point-cloud
 2 | 
 3 | Tool for colorize point cloud with images from camera. This tool takes a point cloud and
 4 | dynamic number of images from camera via ROS2 and colorize the point cloud with the images.
 5 | 
 6 | ## Installation
 7 | 
 8 | ### Prerequisites
 9 | 
10 | - ROS2 (tested with Humble)
11 | - OpenCV
12 | - PCL
13 | - Eigen3
14 | 
15 | 
16 | ### Build
17 | 
18 | ```bash
19 | mkdir -p ~/color-point-cloud_ws/src
20 | cd ~/color-point-cloud_ws/src
21 | git clone https://github.com/leo-drive/color-point-cloud
22 | cd ..
23 | colcon build --symlink-install
24 | ```
25 | 
26 | ## Usage
27 | 
28 | ### Run
29 | 
30 | ```bash
31 | source ~/color-point-cloud_ws/install/setup.bash
32 | ros2 launch color_point_cloud color_point_cloud.launch.xml
33 | ```
34 | 
35 | ### Parameters
36 | 
37 | Change parameters in `color_point_cloud.launch.xml` to fit your environment.
38 | 
39 | | Parameter | Description                             | 
40 | | :-------- |:----------------------------------------| 
41 | | `point_cloud_topic` | your lidar topic as `string`            |
42 | | `point_cloud_frame_id` | your lidar topic as `string`            |
43 | | `camera_topics` | array of your camera topics as `string` |
44 | | `image_type` | your image format `enum`                |
45 | 
46 | ### Result
47 | 
48 | #### Projection Lucid Vision Triton 5.4 MP 120° camera onto Velodyne Alpha Prime LiDAR
49 | 
50 | <p align='center'>
51 |     <img src="image/vls128_0.png" alt="vls-128" width="400"/>
52 |     <img src="image/vls128_1.png" alt="vls-128" width="400"/>
53 | </p>
54 | 
55 | #### Projection AC-IMX390 camera onto RoboSense Helios 32 beam LiDAR
56 | 
57 | <p align='center'>
58 |     <img src="image/robione_0.png" alt="vls-128" width="400"/>
59 |     <img src="image/robione_1.png" alt="vls-128" width="400"/>
60 | </p>
61 | 
62 | #### SLAM results with LIO-SAM ([link](https://github.com/leo-drive/LIO-SAM-COLOR))
63 | 
64 | <p align='center'>
65 |     <img src="image/slam_0.png" alt="slam" width="400"/>
66 |     <img src="image/slam_1.png" alt="slam" width="400"/>
67 | </p>


--------------------------------------------------------------------------------
/config/param.yaml:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/config/param.yaml


--------------------------------------------------------------------------------
/image/robione_0.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/image/robione_0.png


--------------------------------------------------------------------------------
/image/robione_1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/image/robione_1.png


--------------------------------------------------------------------------------
/image/slam_0.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/image/slam_0.png


--------------------------------------------------------------------------------
/image/slam_1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/image/slam_1.png


--------------------------------------------------------------------------------
/image/vls128_0.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/image/vls128_0.png


--------------------------------------------------------------------------------
/image/vls128_1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/leo-drive/color-point-cloud/e431fb76a2ad68310de5251539fdca08f9dcd370/image/vls128_1.png


--------------------------------------------------------------------------------
/include/color_point_cloud/ColorPointCloud.hpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by bzeren on 05.10.2023.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include "color_point_cloud/data_type/Camera.hpp"
 8 | #include "color_point_cloud/data_type/PointCloudType.hpp"
 9 | #include "color_point_cloud/utils/TransformeProvider.hpp"
10 | 
11 | #include <rclcpp/rclcpp.hpp>
12 | 
13 | #include <sensor_msgs/msg/point_cloud2.hpp>
14 | #include <sensor_msgs/msg/image.hpp>
15 | #include <sensor_msgs/msg/camera_info.hpp>
16 | 
17 | #include <tf2_ros/transform_broadcaster.h>
18 | #include <tf2_ros/transform_listener.h>
19 | #include <tf2_ros/buffer.h>
20 | 
21 | #include "cv_bridge/cv_bridge.h"
22 | 
23 | #include <opencv2/opencv.hpp>
24 | 
25 | namespace color_point_cloud {
26 |     class ColorPointCloud : public rclcpp::Node {
27 |     public:
28 |         explicit ColorPointCloud(const rclcpp::NodeOptions &options);
29 | 
30 |     private:
31 |         rclcpp::TimerBase::SharedPtr timer_;
32 | 
33 |         rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_subscriber_;
34 |         rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_publisher_;
35 | 
36 |         double timeout_sec_;
37 | 
38 |         std::string point_cloud_topic_;
39 | 
40 |         std::string point_cloud_frame_id_;
41 | 
42 |         ImageType image_type_;
43 | 
44 |         std::string image_topic_last_name_;
45 | 
46 |         std::string camera_info_topic_last_name_;
47 | 
48 |         std::vector<std::string> camera_topics_;
49 | 
50 |         std::map<std::string, CameraTypePtr> camera_type_stdmap_;
51 | 
52 |         std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> image_subscribers_;
53 |         std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> camera_info_subscribers_;
54 | 
55 |         void timer_callback();
56 | 
57 |         void point_cloud_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg);
58 | 
59 |         TransformProviderConstPtr transform_provider_ptr_;
60 |     };
61 | } // namespace color_point_cloud
62 | 


--------------------------------------------------------------------------------
/include/color_point_cloud/data_type/Camera.hpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by bzeren on 05.10.2023.
  3 | //
  4 | 
  5 | #pragma once
  6 | 
  7 | #include <utility>
  8 | 
  9 | #include <rclcpp/rclcpp.hpp>
 10 | 
 11 | #include <sensor_msgs/msg/image.hpp>
 12 | #include <sensor_msgs/msg/camera_info.hpp>
 13 | 
 14 | #include <geometry_msgs/msg/transform_stamped.h>
 15 | #include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
 16 | 
 17 | #include <tf2_eigen/tf2_eigen.hpp>
 18 | 
 19 | #include <Eigen/Dense>
 20 | 
 21 | #include <opencv2/opencv.hpp>
 22 | 
 23 | #include <cv_bridge/cv_bridge.h>
 24 | 
 25 | enum class ImageType {
 26 |     RAW,
 27 |     RECTIFIED
 28 | };
 29 | 
 30 | namespace color_point_cloud {
 31 |     class CameraType {
 32 |     public:
 33 |         CameraType(std::string image_topic, std::string camera_info_topic) :
 34 |                 image_topic_(std::move(image_topic)), camera_info_topic_(std::move(camera_info_topic)),
 35 |                 is_info_initialized_(false), is_transform_initialized_(false), image_width_(0), image_height_(0) {
 36 | 
 37 |         }
 38 | 
 39 |         void set_cv_image(const sensor_msgs::msg::Image::ConstSharedPtr &msg, ImageType image_type) {
 40 |             cv_bridge::CvImageConstPtr cv_ptr;
 41 |             if (image_type == ImageType::RAW) {
 42 |                 try {
 43 |                     cv_ptr = cv_bridge::toCvShare(msg, get_image_msg()->encoding);
 44 |                 } catch (cv_bridge::Exception &e) {
 45 | //                    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
 46 |                     return;
 47 |                 }
 48 |                 cv::undistort(cv_ptr->image, cv_image_, get_camera_matrix_cv(),
 49 |                               get_distortion_matrix_cv());
 50 |             } else if (image_type == ImageType::RECTIFIED) {
 51 |                 try {
 52 |                     cv_ptr = cv_bridge::toCvShare(msg, get_image_msg()->encoding);
 53 |                 } catch (cv_bridge::Exception &e) {
 54 | //                    RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
 55 |                     return;
 56 |                 }
 57 |                 cv_image_ = cv_ptr->image;
 58 |             } else {
 59 | //                RCLCPP_ERROR(this->get_logger(), "Unknown image type");
 60 |                 return;
 61 |             }
 62 |         }
 63 | 
 64 |         void set_image_msg(const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
 65 |             image_msg_ = msg;
 66 |         }
 67 | 
 68 |         void set_camera_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr &msg) {
 69 |             camera_info_ = msg;
 70 |         }
 71 | 
 72 |         cv::Mat get_cv_image() {
 73 |             return cv_image_;
 74 |         }
 75 | 
 76 |         sensor_msgs::msg::Image::ConstSharedPtr get_image_msg() {
 77 |             return image_msg_;
 78 |         }
 79 | 
 80 |         sensor_msgs::msg::CameraInfo::ConstSharedPtr get_camera_info() {
 81 |             return camera_info_;
 82 |         }
 83 | 
 84 |         bool is_info_initialized() const {
 85 |             return is_info_initialized_;
 86 |         }
 87 | 
 88 |         bool is_transform_initialized() const {
 89 |             return is_transform_initialized_;
 90 |         }
 91 | 
 92 |         void set_camera_utils(const sensor_msgs::msg::CameraInfo::ConstSharedPtr &msg) {
 93 |             camera_frame_id_ = msg->header.frame_id;
 94 |             distortion_model_ = msg->distortion_model;
 95 | 
 96 |             image_width_ = msg->width;
 97 |             image_height_ = msg->height;
 98 | 
 99 |             camera_matrix_(0, 0) = msg->k[0];
100 |             camera_matrix_(0, 2) = msg->k[2];
101 |             camera_matrix_(1, 1) = msg->k[4];
102 |             camera_matrix_(1, 2) = msg->k[5];
103 |             camera_matrix_(2, 2) = 1;
104 | 
105 |             camera_matrix_cv_ = (cv::Mat_<double>(3, 3)
106 |                     << msg->k[0], 0.000000, msg->k[2], 0.000000, msg->k[4], msg->k[5], 0.000000, 0.000000, 1.000000);
107 | 
108 |             rectification_matrix_(0, 0) = msg->r[0];
109 |             rectification_matrix_(0, 1) = msg->r[1];
110 |             rectification_matrix_(0, 2) = msg->r[2];
111 |             rectification_matrix_(1, 0) = msg->r[3];
112 |             rectification_matrix_(1, 1) = msg->r[4];
113 |             rectification_matrix_(1, 2) = msg->r[5];
114 |             rectification_matrix_(2, 0) = msg->r[6];
115 |             rectification_matrix_(2, 1) = msg->r[7];
116 |             rectification_matrix_(2, 2) = msg->r[8];
117 | 
118 |             projection_matrix_(0, 0) = msg->p[0];
119 |             projection_matrix_(0, 2) = msg->p[2];
120 |             projection_matrix_(1, 1) = msg->p[5];
121 |             projection_matrix_(1, 2) = msg->p[6];
122 |             projection_matrix_(2, 2) = 1;
123 | 
124 |             distortion_matrix_(0, 0) = msg->d[0];
125 |             distortion_matrix_(0, 1) = msg->d[1];
126 |             distortion_matrix_(0, 2) = msg->d[3];
127 |             distortion_matrix_(0, 3) = msg->d[4];
128 |             distortion_matrix_(0, 4) = msg->d[2];
129 | 
130 |             distortion_matrix_cv_ = (cv::Mat_<double>(1, 5) << msg->d[0], msg->d[1], msg->d[3], msg->d[4], msg->d[2]);
131 | 
132 |             is_info_initialized_ = true;
133 |         }
134 | 
135 |         void set_lidar_to_camera_matrix(geometry_msgs::msg::TransformStamped &msg) {
136 |             Eigen::Affine3d affine_lidar_to_camera_matrix = tf2::transformToEigen(msg);
137 | 
138 |             lidar_to_camera_matrix_ = Eigen::Matrix4d::Identity();
139 |             lidar_to_camera_matrix_ = affine_lidar_to_camera_matrix.matrix();
140 | 
141 |             is_transform_initialized_ = true;
142 |         }
143 | 
144 |         void set_lidar_to_camera_projection_matrix() {
145 |             lidar_to_camera_projection_matrix_ = projection_matrix_ * lidar_to_camera_matrix_.block<4, 4>(0, 0);
146 |         }
147 | 
148 |         std::string get_image_topic() {
149 |             return image_topic_;
150 |         }
151 | 
152 |         std::string get_camera_info_topic() {
153 |             return camera_info_topic_;
154 |         }
155 | 
156 |         std::string get_camera_frame_id() {
157 |             return camera_frame_id_;
158 |         }
159 | 
160 |         std::string get_distortion_model() {
161 |             return distortion_model_;
162 |         }
163 | 
164 |         double get_image_width() const {
165 |             return image_width_;
166 |         }
167 | 
168 |         double get_image_height() const {
169 |             return image_height_;
170 |         }
171 | 
172 |         Eigen::Matrix<double, 3, 3> get_camera_matrix() {
173 |             return camera_matrix_;
174 |         }
175 | 
176 |         cv::Mat get_camera_matrix_cv() {
177 |             return camera_matrix_cv_;
178 |         }
179 | 
180 |         Eigen::Matrix<double, 3, 3> get_rectification_matrix() {
181 |             return rectification_matrix_;
182 |         }
183 | 
184 |         Eigen::Matrix<double, 3, 4> get_projection_matrix() {
185 |             return projection_matrix_;
186 |         }
187 | 
188 |         Eigen::Matrix<double, 1, 5> get_distortion_matrix() {
189 |             return distortion_matrix_;
190 |         }
191 | 
192 |         cv::Mat get_distortion_matrix_cv() {
193 |             return distortion_matrix_cv_;
194 |         }
195 | 
196 |         Eigen::Matrix4d get_lidar_to_camera_matrix() {
197 |             return lidar_to_camera_matrix_;
198 |         }
199 | 
200 |         Eigen::Matrix<double, 3, 4> get_lidar_to_camera_projection_matrix() {
201 |             return lidar_to_camera_projection_matrix_;
202 |         }
203 | 
204 | 
205 | 
206 |     private:
207 |         std::string image_topic_;
208 |         std::string camera_info_topic_;
209 | 
210 |         cv::Mat cv_image_;
211 | 
212 |         sensor_msgs::msg::Image::ConstSharedPtr image_msg_;
213 |         sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_;
214 | 
215 |         bool is_info_initialized_;
216 |         bool is_transform_initialized_;
217 | 
218 |         std::string camera_frame_id_;
219 |         std::string distortion_model_;
220 | 
221 |         double image_width_;
222 |         double image_height_;
223 | 
224 |         Eigen::Matrix<double, 3, 3> camera_matrix_;
225 |         Eigen::Matrix<double, 3, 3> rectification_matrix_;
226 |         Eigen::Matrix<double, 3, 4> projection_matrix_;
227 |         Eigen::Matrix<double, 1, 5> distortion_matrix_;
228 | 
229 |         cv::Mat camera_matrix_cv_;
230 |         cv::Mat distortion_matrix_cv_;
231 | 
232 |         Eigen::Matrix4d lidar_to_camera_matrix_;
233 | 
234 |         Eigen::Matrix<double, 3, 4> lidar_to_camera_projection_matrix_;
235 |     };
236 | 
237 |     typedef std::shared_ptr<CameraType> CameraTypePtr;
238 |     typedef std::shared_ptr<const CameraType> CameraTypeConstPtr;
239 | 
240 | } // namespace color_point_cloud
241 | 
242 | 


--------------------------------------------------------------------------------
/include/color_point_cloud/data_type/PointCloudType.hpp:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <iostream>
  4 | #include <optional>
  5 | #include "sensor_msgs/msg/point_cloud2.hpp"
  6 | #include "sensor_msgs/point_cloud2_iterator.hpp"
  7 | #include "tf2/LinearMath/Transform.h"
  8 | 
  9 | namespace pc2_combiner
 10 | {
 11 | 
 12 |     struct Point
 13 |     {
 14 |         float x;
 15 |         float y;
 16 |         float z;
 17 |         float intensity;
 18 |         unsigned short ring;
 19 | 
 20 |         void transform(const geometry_msgs::msg::TransformStamped& t_transform)
 21 |         {
 22 |             const tf2::Quaternion rotation{ t_transform.transform.rotation.x,
 23 |                                             t_transform.transform.rotation.y,
 24 |                                             t_transform.transform.rotation.z,
 25 |                                             t_transform.transform.rotation.w };
 26 |             const tf2::Vector3 translation{ t_transform.transform.translation.x,
 27 |                                             t_transform.transform.translation.y,
 28 |                                             t_transform.transform.translation.z };
 29 |             const tf2::Transform transform{ rotation, translation };
 30 | 
 31 |             const tf2::Vector3 old_point{ x, y, z };
 32 |             const tf2::Vector3 new_point{ transform(old_point) };
 33 | 
 34 |             x = new_point.getX();
 35 |             y = new_point.getY();
 36 |             z = new_point.getZ();
 37 |         }
 38 |     };
 39 | 
 40 | /**
 41 |  * @brief This class takes the reference of an existing const PointCloud2 message and enables
 42 |  * iterating its points.
 43 |  *
 44 |  */
 45 |     class PointCloudConst
 46 |     {
 47 |     public:
 48 |         /**
 49 |          * @brief Constructs a new PointCloud object with the reference of an existing PointCloud2 msg.
 50 |          *
 51 |          * @param t_pointcloud2 The PointCloud2 message.
 52 |          */
 53 |         PointCloudConst(const sensor_msgs::msg::PointCloud2& t_pointcloud2)
 54 |                 : m_pointcloud2{ t_pointcloud2 }
 55 |                 , m_point_count{ m_pointcloud2.height * m_pointcloud2.width }
 56 |                 , m_current_index{ 0 }
 57 |                 , m_iter_x{ m_pointcloud2, "x" }
 58 |                 , m_iter_y{ m_pointcloud2, "y" }
 59 |                 , m_iter_z{ m_pointcloud2, "z" }
 60 |                 , m_iter_intensity{ m_pointcloud2, "intensity" }
 61 |                 , m_iter_ring{ m_pointcloud2, "ring" }
 62 |         {
 63 |         }
 64 | 
 65 |         /**
 66 |          * @brief Get the point count of the cloud.
 67 |          *
 68 |          * @return Point count.
 69 |          */
 70 |         unsigned int getPointCount() const
 71 |         {
 72 |             return m_point_count;
 73 |         }
 74 | 
 75 |         /**
 76 |          * @brief Get the current point of the cloud.
 77 |          *
 78 |          * @return Current point.
 79 |          */
 80 |         Point getCurrentPoint() const
 81 |         {
 82 |             const Point point{ *m_iter_x, *m_iter_y, *m_iter_z, *m_iter_intensity, *m_iter_ring };
 83 |             return point;
 84 |         }
 85 | 
 86 |         /**
 87 |          * @brief Increases iterators by 1 to go to next point.
 88 |          *
 89 |          */
 90 |         void nextPoint()
 91 |         {
 92 |             if (m_current_index < m_point_count)
 93 |             {
 94 |                 ++m_current_index;
 95 |                 ++m_iter_x;
 96 |                 ++m_iter_y;
 97 |                 ++m_iter_z;
 98 |                 ++m_iter_intensity;
 99 |                 ++m_iter_ring;
100 |             }
101 |         }
102 | 
103 |         size_t getCurrentIndex() const
104 |         {
105 |             return m_current_index;
106 |         }
107 | 
108 |     private:
109 |         // Reference of the existing PointCloud2.
110 |         const sensor_msgs::msg::PointCloud2& m_pointcloud2;
111 | 
112 |         // Number of the points in the cloud.
113 |         const unsigned int m_point_count;
114 | 
115 |         // Index of the current point.
116 |         size_t m_current_index;
117 | 
118 |         // PointCloud2 iterators.
119 |         sensor_msgs::PointCloud2ConstIterator<float> m_iter_x;
120 |         sensor_msgs::PointCloud2ConstIterator<float> m_iter_y;
121 |         sensor_msgs::PointCloud2ConstIterator<float> m_iter_z;
122 |         sensor_msgs::PointCloud2ConstIterator<float> m_iter_intensity;
123 |         sensor_msgs::PointCloud2ConstIterator<unsigned short> m_iter_ring;
124 |     };
125 | 
126 | /**
127 |  * @brief This class takes the reference of an existing const PointCloud2 message and enables
128 |  * iterating its points.
129 |  *
130 |  */
131 |     class PointCloud
132 |     {
133 |     public:
134 |         /**
135 |          * @brief Constructs a new PointCloud object with the reference of an existing PointCloud2 msg.
136 |          *
137 |          * @param t_pointcloud2 The PointCloud2 message.
138 |          */
139 |         PointCloud(sensor_msgs::msg::PointCloud2& t_pointcloud2)
140 |                 : m_pointcloud2{ t_pointcloud2 }
141 |                 , m_point_count{ m_pointcloud2.height * m_pointcloud2.width }
142 |                 , m_current_index{ 0 }
143 |         {
144 |         }
145 | 
146 |         /**
147 |          * @brief Get the point count of the cloud.
148 |          *
149 |          * @return Point count.
150 |          */
151 |         unsigned int getPointCount() const
152 |         {
153 |             return m_point_count;
154 |         }
155 | 
156 |         /**
157 |          * @brief Get the current point of the cloud.
158 |          *
159 |          * @return Current point.
160 |          */
161 |         Point getCurrentPoint() const
162 |         {
163 |             const Point point{ *(*m_iter_x), *(*m_iter_y), *(*m_iter_z), *(*m_iter_intensity),
164 |                                *(*m_iter_ring) };
165 |             return point;
166 |         }
167 | 
168 |         /**
169 |          * @brief Increases iterators by 1 to go to next point.
170 |          *
171 |          */
172 |         void nextPoint()
173 |         {
174 |             if (m_current_index < m_point_count)
175 |             {
176 |                 ++*(m_iter_x);
177 |                 ++*(m_iter_y);
178 |                 ++*(m_iter_z);
179 |                 ++*(m_iter_intensity);
180 |                 ++*(m_iter_ring);
181 |             }
182 |         }
183 | 
184 |         /**
185 |          * @brief Sets PointCloud2 fileds and resizes.
186 |          *
187 |          * @param t_point_count Number of points.
188 |          *
189 |          */
190 |         void setFieldsAndResize(const size_t& t_point_count)
191 |         {
192 |             m_point_count = t_point_count;
193 |             sensor_msgs::PointCloud2Modifier modifier{ m_pointcloud2 };
194 |             modifier.resize(m_point_count);
195 |             modifier.setPointCloud2Fields(5, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
196 |                                           sensor_msgs::msg::PointField::FLOAT32, "z", 1,
197 |                                           sensor_msgs::msg::PointField::FLOAT32, "intensity", 1,
198 |                                           sensor_msgs::msg::PointField::FLOAT32, "ring", 1,
199 |                                           sensor_msgs::msg::PointField::UINT16);
200 | 
201 |             m_iter_x.emplace(m_pointcloud2, "x");
202 |             m_iter_y.emplace(m_pointcloud2, "y");
203 |             m_iter_z.emplace(m_pointcloud2, "z");
204 |             m_iter_intensity.emplace(m_pointcloud2, "intensity");
205 |             m_iter_ring.emplace(m_pointcloud2, "ring");
206 | 
207 |             m_pointcloud2.header.frame_id = "base_link";
208 |         }
209 | 
210 |         /**
211 |          * @brief Sets the current point.
212 |          *
213 |          * @param t_point The point to set.
214 |          */
215 |         void setCurrentPoint(const Point& t_point)
216 |         {
217 |             *(*m_iter_x) = t_point.x;
218 |             *(*m_iter_y) = t_point.y;
219 |             *(*m_iter_z) = t_point.z;
220 |             *(*m_iter_intensity) = t_point.intensity;
221 |             *(*m_iter_ring) = t_point.ring;
222 |         }
223 | 
224 |         void append(const Point& t_point)
225 |         {
226 |             setCurrentPoint(t_point);
227 |             nextPoint();
228 |         }
229 | 
230 |     private:
231 |         // Reference of the existing PointCloud2.
232 |         sensor_msgs::msg::PointCloud2& m_pointcloud2;
233 | 
234 |         // Number of the points in the cloud.
235 |         unsigned int m_point_count;
236 | 
237 |         // Index of the current point.
238 |         size_t m_current_index;
239 | 
240 |         // PointCloud2 iterators.
241 |         std::optional<sensor_msgs::PointCloud2Iterator<float>> m_iter_x;
242 |         std::optional<sensor_msgs::PointCloud2Iterator<float>> m_iter_y;
243 |         std::optional<sensor_msgs::PointCloud2Iterator<float>> m_iter_z;
244 |         std::optional<sensor_msgs::PointCloud2Iterator<float>> m_iter_intensity;
245 |         std::optional<sensor_msgs::PointCloud2Iterator<unsigned short>> m_iter_ring;
246 |     };
247 | 
248 | }  // namespace pc2_combiner


--------------------------------------------------------------------------------
/include/color_point_cloud/utils/TransformeProvider.hpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by bzeren on 05.10.2023.
 3 | //
 4 | 
 5 | #pragma once
 6 | 
 7 | #include <tf2_ros/transform_broadcaster.h>
 8 | #include <tf2_ros/transform_listener.h>
 9 | #include <tf2_ros/buffer.h>
10 | 
11 | #include <geometry_msgs/msg/transform_stamped.h>
12 | 
13 | namespace color_point_cloud {
14 |     class TransformProvider {
15 |     public:
16 |         explicit TransformProvider(rclcpp::Clock::SharedPtr clock) {
17 |             std::cout << "TransformProvider constructor" << std::endl;
18 | 
19 |             tf_buffer_ = std::make_unique<tf2_ros::Buffer>(clock);
20 |             tf_listener_ = std::make_unique<tf2_ros::TransformListener>(*tf_buffer_);
21 |         }
22 | 
23 |         std::optional<geometry_msgs::msg::TransformStamped> operator()(const std::string &target_frame,
24 |                                                         const std::string &source_frame) const {
25 |             std::optional<geometry_msgs::msg::TransformStamped> transform_stamped;
26 |             try {
27 |                 transform_stamped = tf_buffer_->lookupTransform(target_frame, source_frame, tf2::TimePointZero);
28 |             } catch (tf2::TransformException &ex) {
29 | //                RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "%s", ex.what());
30 |                 return std::nullopt;
31 |             }
32 |             return transform_stamped;
33 |         }
34 | 
35 |     private:
36 |         std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
37 |         std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
38 |     };
39 | 
40 |     typedef std::shared_ptr<TransformProvider> TransformProviderPtr;
41 |     typedef std::shared_ptr<const TransformProvider> TransformProviderConstPtr;
42 | 
43 | } // namespace color_point_cloud


--------------------------------------------------------------------------------
/launch/color_point_cloud.launch.xml:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     <arg name="timeout_sec" default="0.1"/>
 3 |     <arg name="point_cloud_topic" default="/sensing/lidar/top/pointcloud_raw"/>
 4 |     <arg name="point_cloud_frame_id" default="rs_helios_top"/>
 5 |     <arg name="camera_topics"
 6 |          default="[/sensing/camera/camera0/, /sensing/camera/camera1/, /sensing/camera/camera2/, /sensing/camera/camera3/]"/>
 7 |     <arg name="image_type" default="0" description="0:RAW, 1:RECTIFIED"/>
 8 |     <arg name="image_topic_last_name" default="image_raw"/>
 9 |     <arg name="camera_info_topic_last_name" default="camera_info"/>
10 | 
11 |     <node pkg="color_point_cloud" exec="color_point_cloud" name="color_point_cloud" output="screen">
12 |         <param name="timeout_sec" value="$(var timeout_sec)"/>
13 |         <param name="point_cloud_topic" value="$(var point_cloud_topic)"/>
14 |         <param name="point_cloud_frame_id" value="$(var point_cloud_frame_id)"/>
15 |         <param name="camera_topics" value="$(var camera_topics)"/>
16 |         <param name="image_type" value="$(var image_type)"/>
17 |         <param name="image_topic_last_name" value="$(var image_topic_last_name)"/>
18 |         <param name="camera_info_topic_last_name" value="$(var camera_info_topic_last_name)"/>
19 |         <param from="$(var param_file)"/>
20 |     </node>
21 | </launch>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>color_point_cloud</name>
 5 |   <version>0.0.0</version>
 6 |   <description>TODO: Package description</description>
 7 |   <maintainer email="bzeren1819@gmail.com">bzeren</maintainer>
 8 |   <license>TODO: License declaration</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 | 
12 |   <test_depend>ament_lint_auto</test_depend>
13 |   <test_depend>ament_lint_common</test_depend>
14 | 
15 |   <export>
16 |     <build_type>ament_cmake</build_type>
17 |   </export>
18 | </package>
19 | 


--------------------------------------------------------------------------------
/src/ColorPointCloud.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by bzeren on 05.10.2023.
  3 | //
  4 | 
  5 | #include "color_point_cloud/ColorPointCloud.hpp"
  6 | 
  7 | namespace color_point_cloud {
  8 |     ColorPointCloud::ColorPointCloud(const rclcpp::NodeOptions &options) : Node("color_point_cloud_node", options),
  9 |                                                                            transform_provider_ptr_(
 10 |                                                                                    std::make_shared<TransformProvider>(
 11 |                                                                                            this->get_clock())) {
 12 |         RCLCPP_INFO(this->get_logger(), "ColorPointCloud node started");
 13 | 
 14 |         // Declare and get parameters
 15 |         {
 16 |             // timeout_sec
 17 |             this->declare_parameter<double>("timeout_sec", 0.1);
 18 |             timeout_sec_ = this->get_parameter("timeout_sec").as_double();
 19 | 
 20 |             this->declare_parameter<std::string>("point_cloud_topic", "/points_raw");
 21 |             point_cloud_topic_ = this->get_parameter("point_cloud_topic").as_string();
 22 | 
 23 |             this->declare_parameter<std::string>("point_cloud_frame_id", "lidar");
 24 |             point_cloud_frame_id_ = this->get_parameter("point_cloud_frame_id").as_string();
 25 | 
 26 |             this->declare_parameter<int>("image_type", 0);
 27 |             image_type_ = static_cast<ImageType>(this->get_parameter("image_type").as_int());
 28 | 
 29 |             this->declare_parameter<std::string>("image_topic_last_name", "/image_raw");
 30 |             image_topic_last_name_ = this->get_parameter("image_topic_last_name").as_string();
 31 | 
 32 |             this->declare_parameter<std::string>("camera_info_topic_last_name", "/camera_info");
 33 |             camera_info_topic_last_name_ = this->get_parameter("camera_info_topic_last_name").as_string();
 34 | 
 35 |             // camera_topics
 36 |             this->declare_parameter<std::vector<std::string>>("camera_topics", std::vector<std::string>());
 37 |             camera_topics_ = this->get_parameter("camera_topics").as_string_array();
 38 | 
 39 |             for (auto &camera_topic: camera_topics_) {
 40 |                 RCLCPP_INFO(this->get_logger(), "camera_topic: %s", camera_topic.c_str());
 41 |             }
 42 |         }
 43 | 
 44 |         // Create camera object, create subscriber to image and camera_info
 45 |         {
 46 |             for (const auto &camera_topic: camera_topics_) {
 47 |                 std::string image_topic = camera_topic + image_topic_last_name_;
 48 |                 std::string camera_info_topic = camera_topic + camera_info_topic_last_name_;
 49 | 
 50 |                 CameraTypePtr camera_type_ptr = std::make_shared<CameraType>(image_topic, camera_info_topic);
 51 |                 camera_type_stdmap_[camera_topic] = camera_type_ptr;
 52 | 
 53 |                 this->image_subscribers_.push_back(this->create_subscription<sensor_msgs::msg::Image>(
 54 |                         image_topic, rclcpp::SensorDataQoS(),
 55 |                         [this, image_topic, camera_topic](const sensor_msgs::msg::Image::ConstSharedPtr &msg) {
 56 |                             // RCLCPP_INFO(this->get_logger(), "Received image on topic %s", camera_topic.c_str());
 57 |                             camera_type_stdmap_[camera_topic]->set_image_msg(msg);
 58 |                         }));
 59 | 
 60 |                 this->camera_info_subscribers_.push_back(this->create_subscription<sensor_msgs::msg::CameraInfo>(
 61 |                         camera_info_topic, rclcpp::SensorDataQoS(), [this, camera_info_topic, camera_topic](
 62 |                                 const sensor_msgs::msg::CameraInfo::ConstSharedPtr &msg) {
 63 |                             // RCLCPP_INFO(this->get_logger(), "Received camera_info on topic %s", camera_topic.c_str());
 64 |                             camera_type_stdmap_[camera_topic]->set_camera_info(msg);
 65 |                         }));
 66 |             }
 67 |         }
 68 | 
 69 |         {
 70 |             point_cloud_subscriber_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
 71 |                     point_cloud_topic_, rclcpp::SensorDataQoS(),
 72 |                     std::bind(&ColorPointCloud::point_cloud_callback, this, std::placeholders::_1));
 73 | 
 74 |             point_cloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
 75 |                     "/points_color", rclcpp::SensorDataQoS());
 76 |         }
 77 | 
 78 |         // Set timer
 79 |         {
 80 |             const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
 81 |                     std::chrono::duration<double>(timeout_sec_));
 82 |             timer_ = rclcpp::create_timer(this, this->get_clock(), period_ns,
 83 |                                           std::bind(&ColorPointCloud::timer_callback, this));
 84 |         }
 85 | 
 86 |     }
 87 | 
 88 |     void ColorPointCloud::timer_callback() {
 89 |         for (const auto &pair: camera_type_stdmap_) {
 90 |             if (pair.second->get_image_msg() == nullptr || pair.second->get_camera_info() == nullptr) {
 91 | //                RCLCPP_INFO(this->get_logger(), "Image or camera info is null for topic: %s", pair.first.c_str());
 92 |                 continue;
 93 |             }
 94 | 
 95 |             if (!pair.second->is_info_initialized()) {
 96 |                 RCLCPP_INFO(this->get_logger(), "Camera info is setting: %s", pair.first.c_str());
 97 |                 pair.second->set_camera_utils(pair.second->get_camera_info());
 98 |             }
 99 | 
100 |             if (!pair.second->is_transform_initialized() && pair.second->is_info_initialized()) {
101 |                 std::optional<geometry_msgs::msg::TransformStamped> transform = (*transform_provider_ptr_)(
102 |                         pair.second->get_camera_frame_id(), point_cloud_frame_id_);
103 |                 if (!transform.has_value()) {
104 |                     continue;
105 |                 }
106 |                 pair.second->set_lidar_to_camera_matrix(transform.value());
107 |                 pair.second->set_lidar_to_camera_projection_matrix();
108 |             }
109 |         }
110 |     }
111 | 
112 |     void ColorPointCloud::point_cloud_callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
113 | 
114 |         sensor_msgs::msg::PointCloud2 cloud_color_msg;
115 |         std::for_each(camera_type_stdmap_.begin(), camera_type_stdmap_.end(),
116 |                       [this, &cloud_color_msg, msg](std::pair<std::string, CameraTypePtr> pair) {
117 |                           if (pair.second->get_image_msg() == nullptr || pair.second->get_camera_info() == nullptr ||
118 |                               !pair.second->is_info_initialized() || !pair.second->is_transform_initialized()) {
119 |                               RCLCPP_INFO(this->get_logger(),
120 |                                           "Cant project camera: %s \n\n Expected reasons: \n don't receive image \n don't receive \n can't get transforme \n can't init camera utils",
121 |                                           pair.first.c_str());
122 |                               return;
123 |                           }
124 |                           pair.second->set_cv_image(pair.second->get_image_msg(), image_type_);
125 | 
126 |                           sensor_msgs::PointCloud2Modifier modifier(cloud_color_msg);
127 |                           modifier.setPointCloud2Fields(6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1,
128 |                                                         sensor_msgs::msg::PointField::FLOAT32, "z", 1,
129 |                                                         sensor_msgs::msg::PointField::FLOAT32, "rgb", 1,
130 |                                                         sensor_msgs::msg::PointField::FLOAT32, "ring", 1,
131 |                                                         sensor_msgs::msg::PointField::UINT16, "intensity", 1,
132 |                                                         sensor_msgs::msg::PointField::FLOAT32);
133 |                           modifier.resize(msg->width * msg->height);
134 | 
135 |                           sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_color_msg, "x");
136 |                           sensor_msgs::PointCloud2Iterator<float> iter_y(cloud_color_msg, "y");
137 |                           sensor_msgs::PointCloud2Iterator<float> iter_z(cloud_color_msg, "z");
138 |                           sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_color_msg, "r");
139 |                           sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_color_msg, "g");
140 |                           sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_color_msg, "b");
141 |                           sensor_msgs::PointCloud2Iterator<uint16_t> iter_ring(cloud_color_msg, "ring");
142 |                           sensor_msgs::PointCloud2Iterator<float> iter_intensity(cloud_color_msg, "intensity");
143 | 
144 |                           pc2_combiner::PointCloudConst cloud{*msg};
145 |                           for (size_t i = 0; i < cloud.getPointCount(); ++i) {
146 |                               pc2_combiner::Point point{cloud.getCurrentPoint()};
147 | 
148 |                               Eigen::Vector4d point4d(point.x, point.y, point.z, 1.0);
149 | 
150 |                               Eigen::Vector3d point3d_transformed_camera =
151 |                                       pair.second->get_lidar_to_camera_projection_matrix() * point4d;
152 | 
153 |                               Eigen::Vector2d point2d_transformed_camera = Eigen::Vector2d(
154 |                                       point3d_transformed_camera[0] / point3d_transformed_camera[2],
155 |                                       point3d_transformed_camera[1] / point3d_transformed_camera[2]);
156 | 
157 |                               double x = point2d_transformed_camera[0];
158 |                               double y = point2d_transformed_camera[1];
159 | 
160 |                               if (x < 0 || x > pair.second->get_image_width() || y < 0 ||
161 |                                   y > pair.second->get_image_height() ||
162 |                                   point3d_transformed_camera[2] < 0) {
163 | 
164 |                                   iter_x[0] = point.x;
165 |                                   iter_y[0] = point.y;
166 |                                   iter_z[0] = point.z;
167 |                                   iter_ring[0] = point.ring;
168 |                                   iter_intensity[0] = point.intensity;
169 | 
170 |                               } else {
171 |                                   cv::Vec3d color = pair.second->get_cv_image().at<cv::Vec3b>(cv::Point(x, y));
172 |                                   cv::Scalar color_scalar(color[0], color[1], color[2]);
173 | 
174 |                                   iter_x[0] = point.x;
175 |                                   iter_y[0] = point.y;
176 |                                   iter_z[0] = point.z;
177 |                                   iter_ring[0] = point.ring;
178 |                                   iter_intensity[0] = point.intensity;
179 | 
180 |                                   if (pair.second->get_image_msg()->encoding == "rgb8") {
181 |                                       iter_r[0] = color[0];
182 |                                       iter_g[0] = color[1];
183 |                                       iter_b[0] = color[2];
184 |                                   } else if (pair.second->get_image_msg()->encoding == "bgr8") {
185 |                                       iter_r[0] = color[2];
186 |                                       iter_g[0] = color[1];
187 |                                       iter_b[0] = color[0];
188 |                                   } else {
189 |                                       iter_r[0] = color[2];
190 |                                       iter_g[0] = color[1];
191 |                                       iter_b[0] = color[0];
192 |                                   }
193 |                               }
194 | 
195 |                               ++iter_x;
196 |                               ++iter_y;
197 |                               ++iter_z;
198 |                               ++iter_r;
199 |                               ++iter_g;
200 |                               ++iter_b;
201 |                               ++iter_ring;
202 |                               ++iter_intensity;
203 | 
204 |                               cloud.nextPoint();
205 |                           }
206 | //                          cv::imshow(pair.first, pair.second->get_cv_image());
207 | //                          cv::waitKey(1);
208 |                       });
209 | 
210 |         cloud_color_msg.header = msg->header;
211 |         cloud_color_msg.height = 1;
212 |         cloud_color_msg.width = msg->width * msg->height;
213 |         point_cloud_publisher_->publish(cloud_color_msg);
214 |     }
215 | 
216 | 
217 | } // namespace color_point_cloud


--------------------------------------------------------------------------------
/src/color_point_cloud_node.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by bzeren on 05.10.2023.
 3 | //
 4 | 
 5 | #include "color_point_cloud/ColorPointCloud.hpp"
 6 | 
 7 | int main(int argc, char **argv) {
 8 |     rclcpp::init(argc, argv);
 9 |     rclcpp::NodeOptions options;
10 |     rclcpp::spin(std::make_shared<color_point_cloud::ColorPointCloud>(options));
11 |     rclcpp::shutdown();
12 |     return 0;
13 | }


--------------------------------------------------------------------------------