-
Notifications
You must be signed in to change notification settings - Fork 0
/
lidar_2d_utils.cc
40 lines (32 loc) · 1.53 KB
/
lidar_2d_utils.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
#include "2dmapping/lidar_2d_utils.h"
#include <opencv2/imgproc.hpp>
namespace zqx {
void Visualize2DScan(Scan2d::Ptr scan, const SE2& pose, cv::Mat& image, const Vec3b& color,
int image_size, float resolution, const SE2& pose_submap) {
if (image.data == nullptr) {
image = cv::Mat(image_size, image_size, CV_8UC3, cv::Vec3b(255, 255, 255));
}
for (size_t i = 0; i < scan->ranges.size(); i++) {
if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
continue;
}
double real_angle = scan->angle_min + i * scan->angle_increment;
double x = scan->ranges[i] * std::cos(real_angle);
double y = scan->ranges[i] * std::sin(real_angle);
if (real_angle < scan->angle_min + 30 * M_PI / 180.0 ||
real_angle > scan->angle_max - 30 * M_PI / 180.0) {
continue;
}
Vec2d psubmap = pose_submap.inverse() * (pose * Vec2d(x, y));
int image_x = int(psubmap[0] * resolution + image_size / 2);
int image_y = int(psubmap[1] * resolution + image_size / 2);
if (image_x >= 0 && image_x < image.cols && image_y >= 0 && image_y < image.rows) {
image.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(color[0], color[1], color[2]);
}
}
Vec2d pose_in_image = pose_submap.inverse() * (pose.translation() * double(resolution) +
Vec2d(image_size / 2, image_size / 2));
cv::circle(image, cv::Point2f(pose_in_image[0], pose_in_image[1]), 5,
cv::Scalar(color[0], color[1], color[2]));
}
} // namespace zqx