GLIM
Loading...
Searching...
No Matches
ros_cloud_converter.hpp
1#pragma once
2
3#include <memory>
4#include <vector>
5#include <iostream>
6#include <spdlog/spdlog.h>
7#include <boost/format.hpp>
8
9#include <Eigen/Core>
10#include <gtsam_points/types/point_cloud.hpp>
11#include <glim/util/raw_points.hpp>
12
13#ifdef GLIM_ROS2
14#include <sensor_msgs/msg/point_cloud2.hpp>
15namespace glim {
16using PointCloud2 = sensor_msgs::msg::PointCloud2;
17using PointCloud2Ptr = sensor_msgs::msg::PointCloud2::SharedPtr;
18using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
19using PointField = sensor_msgs::msg::PointField;
20
21template <typename Stamp>
22double to_sec(const Stamp& stamp) {
23 return stamp.sec + stamp.nanosec / 1e9;
24}
25
26inline builtin_interfaces::msg::Time from_sec(const double time) {
27 builtin_interfaces::msg::Time stamp;
28 stamp.sec = std::floor(time);
29 stamp.nanosec = (time - stamp.sec) * 1e9;
30 return stamp;
31}
32
33} // namespace glim
34#else
35#include <sensor_msgs/PointCloud2.h>
36namespace glim {
37using PointCloud2 = sensor_msgs::PointCloud2;
38using PointCloud2Ptr = sensor_msgs::PointCloud2::Ptr;
39using PointCloud2ConstPtr = sensor_msgs::PointCloud2::ConstPtr;
40using PointField = sensor_msgs::PointField;
41
42template <typename Stamp>
43double to_sec(const Stamp& stamp) {
44 return stamp.toSec();
45}
46
47inline ros::Time from_sec(const double time) {
48 ros::Time stamp;
49 stamp.sec = std::floor(time);
50 stamp.nsec = (time - stamp.sec) * 1e9;
51 return stamp;
52}
53
54} // namespace glim
55#endif
56
57namespace glim {
58
59template <typename T>
60Eigen::Vector4d get_vec4(const void* x, const void* y, const void* z) {
61 return Eigen::Vector4d(*reinterpret_cast<const T*>(x), *reinterpret_cast<const T*>(y), *reinterpret_cast<const T*>(z), 1.0);
62}
63
64static RawPoints::Ptr extract_raw_points(const PointCloud2& points_msg, const std::string& intensity_channel, const std::string& ring_channel) {
65 int num_points = points_msg.width * points_msg.height;
66
67 int x_type = 0;
68 int y_type = 0;
69 int z_type = 0;
70 int time_type = 0; // ouster and livox
71 int intensity_type = 0;
72 int color_type = 0;
73 int ring_type = 0;
74
75 int x_offset = -1;
76 int y_offset = -1;
77 int z_offset = -1;
78 int time_offset = -1;
79 int intensity_offset = -1;
80 int color_offset = -1;
81 int ring_offset = -1;
82
83 std::unordered_map<std::string, std::pair<int*, int*>> fields;
84 fields["x"] = std::make_pair(&x_type, &x_offset);
85 fields["y"] = std::make_pair(&y_type, &y_offset);
86 fields["z"] = std::make_pair(&z_type, &z_offset);
87 fields["t"] = std::make_pair(&time_type, &time_offset);
88 fields["time"] = std::make_pair(&time_type, &time_offset);
89 fields["time_stamp"] = std::make_pair(&time_type, &time_offset);
90 fields["timestamp"] = std::make_pair(&time_type, &time_offset);
91 fields[intensity_channel] = std::make_pair(&intensity_type, &intensity_offset);
92 fields["rgba"] = std::make_pair(&color_type, &color_offset);
93 fields[ring_channel] = std::make_pair(&ring_type, &ring_offset);
94
95 for (const auto& field : points_msg.fields) {
96 auto found = fields.find(field.name);
97 if (found == fields.end()) {
98 continue;
99 }
100
101 *found->second.first = field.datatype;
102 *found->second.second = field.offset;
103 }
104
105 if (x_offset < 0 || y_offset < 0 || z_offset < 0) {
106 spdlog::warn("missing point coordinate fields");
107 return nullptr;
108 }
109
110 if ((x_type != PointField::FLOAT32 && x_type != PointField::FLOAT64) || x_type != y_type || x_type != y_type) {
111 spdlog::warn("unsupported points type");
112 return nullptr;
113 }
114
115 auto raw_points = std::make_shared<RawPoints>();
116
117 raw_points->points.resize(num_points);
118
119 if (x_type == PointField::FLOAT32 && y_offset == x_offset + sizeof(float) && z_offset == y_offset + sizeof(float)) {
120 // Special case: contiguous 3 floats
121 for (int i = 0; i < num_points; i++) {
122 const auto* x_ptr = &points_msg.data[points_msg.point_step * i + x_offset];
123 raw_points->points[i] << Eigen::Map<const Eigen::Vector3f>(reinterpret_cast<const float*>(x_ptr)).cast<double>(), 1.0;
124 }
125 } else if (x_type == PointField::FLOAT64 && y_offset == x_offset + sizeof(double) && z_offset == y_offset + sizeof(double)) {
126 // Special case: contiguous 3 doubles
127 for (int i = 0; i < num_points; i++) {
128 const auto* x_ptr = &points_msg.data[points_msg.point_step * i + x_offset];
129 raw_points->points[i] << Eigen::Map<const Eigen::Vector3d>(reinterpret_cast<const double*>(x_ptr)), 1.0;
130 }
131 } else {
132 for (int i = 0; i < num_points; i++) {
133 const auto* x_ptr = &points_msg.data[points_msg.point_step * i + x_offset];
134 const auto* y_ptr = &points_msg.data[points_msg.point_step * i + y_offset];
135 const auto* z_ptr = &points_msg.data[points_msg.point_step * i + z_offset];
136
137 if (x_type == PointField::FLOAT32) {
138 raw_points->points[i] = get_vec4<float>(x_ptr, y_ptr, z_ptr);
139 } else {
140 raw_points->points[i] = get_vec4<double>(x_ptr, y_ptr, z_ptr);
141 }
142 }
143 }
144
145 if (time_offset >= 0) {
146 raw_points->times.resize(num_points);
147
148 for (int i = 0; i < num_points; i++) {
149 const auto* time_ptr = &points_msg.data[points_msg.point_step * i + time_offset];
150 switch (time_type) {
151 case PointField::UINT32:
152 raw_points->times[i] = *reinterpret_cast<const uint32_t*>(time_ptr) / 1e9;
153 break;
154 case PointField::FLOAT32:
155 raw_points->times[i] = *reinterpret_cast<const float*>(time_ptr);
156 break;
157 case PointField::FLOAT64:
158 raw_points->times[i] = *reinterpret_cast<const double*>(time_ptr);
159 break;
160 default:
161 spdlog::warn("unsupported time type {}", time_type);
162 return nullptr;
163 }
164 }
165 }
166
167 if (intensity_offset >= 0) {
168 raw_points->intensities.resize(num_points);
169
170 for (int i = 0; i < num_points; i++) {
171 const auto* intensity_ptr = &points_msg.data[points_msg.point_step * i + intensity_offset];
172 switch (intensity_type) {
173 case PointField::UINT8:
174 raw_points->intensities[i] = *reinterpret_cast<const std::uint8_t*>(intensity_ptr);
175 break;
176 case PointField::UINT16:
177 raw_points->intensities[i] = *reinterpret_cast<const std::uint16_t*>(intensity_ptr);
178 break;
179 case PointField::UINT32:
180 raw_points->intensities[i] = *reinterpret_cast<const std::uint32_t*>(intensity_ptr);
181 break;
182 case PointField::FLOAT32:
183 raw_points->intensities[i] = *reinterpret_cast<const float*>(intensity_ptr);
184 break;
185 case PointField::FLOAT64:
186 raw_points->intensities[i] = *reinterpret_cast<const double*>(intensity_ptr);
187 break;
188 default:
189 spdlog::warn("unsupported intensity type {}", intensity_type);
190 return nullptr;
191 }
192 }
193 }
194
195 if (color_offset >= 0) {
196 if (color_type != PointField::UINT32) {
197 spdlog::warn("unsupported color type {}", color_type);
198 } else {
199 raw_points->colors.resize(num_points);
200
201 for (int i = 0; i < num_points; i++) {
202 const auto* color_ptr = &points_msg.data[points_msg.point_step * i + color_offset];
203 raw_points->colors[i] = Eigen::Matrix<unsigned char, 4, 1>(reinterpret_cast<const std::uint8_t*>(color_ptr)).cast<double>() / 255.0;
204 }
205 }
206 }
207
208 if (ring_offset >= 0) {
209 raw_points->rings.resize(num_points);
210
211 for (int i = 0; i < num_points; i++) {
212 const auto* ring_ptr = &points_msg.data[points_msg.point_step * i + ring_offset];
213 switch (ring_type) {
214 case PointField::UINT8:
215 raw_points->rings[i] = *reinterpret_cast<const std::uint8_t*>(ring_ptr);
216 break;
217 case PointField::UINT16:
218 raw_points->rings[i] = *reinterpret_cast<const std::uint16_t*>(ring_ptr);
219 break;
220 case PointField::UINT32:
221 raw_points->rings[i] = *reinterpret_cast<const std::uint32_t*>(ring_ptr);
222 break;
223 default:
224 spdlog::warn("unsupported ring type {}", ring_type);
225 return nullptr;
226 }
227 }
228 }
229
230 raw_points->stamp = to_sec(points_msg.header.stamp);
231 return raw_points;
232}
233
234static RawPoints::Ptr extract_raw_points(const PointCloud2& points_msg, const std::string& intensity_channel = "intensity") {
235 return extract_raw_points(points_msg, intensity_channel, "");
236}
237
238static RawPoints::Ptr extract_raw_points(const PointCloud2ConstPtr& points_msg, const std::string& intensity_channel = "intensity") {
239 return extract_raw_points(*points_msg, intensity_channel, "");
240}
241
242static PointCloud2ConstPtr frame_to_pointcloud2(const std::string& frame_id, const double stamp, const gtsam_points::PointCloud& frame) {
243 PointCloud2Ptr msg(new PointCloud2);
244 msg->header.frame_id = frame_id;
245 msg->header.stamp = from_sec(stamp);
246
247 msg->width = frame.size();
248 msg->height = 1;
249
250 const auto create_field = [](const std::string& name, int offset, int datatype, int count) {
251 PointField field;
252 field.name = name;
253 field.offset = offset;
254 field.datatype = datatype;
255 field.count = count;
256 return field;
257 };
258
259 int point_step = 0;
260 msg->fields.reserve(6);
261
262 msg->fields.emplace_back(create_field("x", sizeof(float) * 0, PointField::FLOAT32, 1));
263 msg->fields.emplace_back(create_field("y", sizeof(float) * 1, PointField::FLOAT32, 1));
264 msg->fields.emplace_back(create_field("z", sizeof(float) * 2, PointField::FLOAT32, 1));
265 point_step += sizeof(float) * 3;
266
267 if (frame.times) {
268 msg->fields.emplace_back(create_field("t", point_step, PointField::FLOAT32, 1));
269 point_step += sizeof(float);
270 }
271
272 if (frame.intensities) {
273 msg->fields.emplace_back(create_field("intensity", point_step, PointField::FLOAT32, 1));
274 point_step += sizeof(float);
275 }
276
277 const Eigen::Vector4f* colors = frame.aux_attributes.count("colors") ? frame.aux_attribute<Eigen::Vector4f>("colors") : nullptr;
278 if (colors) {
279 msg->fields.emplace_back(create_field("rgba", point_step, PointField::UINT32, 1));
280 point_step += sizeof(std::uint32_t);
281 }
282
283 msg->is_bigendian = false;
284 msg->point_step = point_step;
285 msg->row_step = point_step * frame.size();
286
287 msg->data.resize(point_step * frame.size());
288 for (int i = 0; i < frame.size(); i++) {
289 unsigned char* point_bytes = msg->data.data() + msg->point_step * i;
290 Eigen::Map<Eigen::Vector3f> xyz(reinterpret_cast<float*>(point_bytes));
291 xyz = frame.points[i].head<3>().cast<float>();
292 point_bytes += sizeof(float) * 3;
293
294 if (frame.times) {
295 *reinterpret_cast<float*>(point_bytes) = frame.times[i];
296 point_bytes += sizeof(float);
297 }
298
299 if (frame.intensities) {
300 *reinterpret_cast<float*>(point_bytes) = frame.intensities[i];
301 point_bytes += sizeof(float);
302 }
303
304 if (colors) {
305 const Eigen::Matrix<std::uint8_t, 4, 1> rgba = (colors[i].array() * 255.0f).min(255.0f).max(0.0f).cast<std::uint8_t>();
306 point_bytes[0] = rgba[0];
307 point_bytes[1] = rgba[1];
308 point_bytes[2] = rgba[2];
309 point_bytes[3] = rgba[3];
310 point_bytes += sizeof(std::uint32_t);
311 }
312 }
313
314 return msg;
315}
316
317} // namespace glim
gtsam_points::IncrementalVoxelMap
Definition loose_initial_state_estimation.hpp:9
Generated by doxygen 1.9.8