Skip to content

Commit

Permalink
Ros2 v0.8.0 lidar apollo instance segmentation (autowarefoundation#256)
Browse files Browse the repository at this point in the history
* restore file name for v0.8.0 update

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix typos in perception (autowarefoundation#862)

* Revert "restore file name for v0.8.0 update"

This reverts commit f8e44568acf0613a0385c9323f2d25bcf45025d2.

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
  • Loading branch information
2 people authored and wep21 committed Jan 30, 2021
1 parent 32d7e18 commit 1937501
Show file tree
Hide file tree
Showing 5 changed files with 16 additions and 16 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ Original URL

Supported lidars are velodyne 16, 64 and 128, but you can also use velodyne 32 and other lidars with good accuracy.

1. [apollo 3D Obstacle Percption description](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md)
1. [apollo 3D Obstacle Perception description](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md)
```
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,14 @@ struct FeatureMapInterface
int width;
int height;
int range;
float * max_height_data; // channnel 0
float * mean_height_data; // channnel 1
float * count_data; // channnel 2
float * direction_data; // channnel 3
float * top_intensity_data; // channnel 4
float * mean_intensity_data; // channnel 5
float * distance_data; // channnel 6
float * nonempty_data; // channnel 7
float * max_height_data; // channel 0
float * mean_height_data; // channel 1
float * count_data; // channel 2
float * direction_data; // channel 3
float * top_intensity_data; // channel 4
float * mean_intensity_data; // channel 5
float * distance_data; // channel 6
float * nonempty_data; // channel 7
std::vector<float> map_data;
virtual void initializeMap(std::vector<float> & map) = 0;
virtual void resetMap(std::vector<float> & map) = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
#include <cmath>
#include <string>

// project point cloud to 2d map. clac in which grid point is.
// project point cloud to 2d map. calc in which grid point is.
// pointcloud to pixel
inline int F2I(float val, float ori, float scale)
{
Expand All @@ -47,7 +47,7 @@ inline int Pc2Pixel(float in_pc, float in_range, float out_size)
return static_cast<int>(std::floor((in_range - in_pc) * inv_res));
}

// retutn the distance from my car to center of the grid.
// return the distance from my car to center of the grid.
// Pc means point cloud = real world scale. so transform pixel scale to real
// world scale
inline float Pixel2Pc(int in_pixel, float in_size, float out_range)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ bool LidarApolloInstanceSegmentation::transformCloud(
pcl::PointCloud<pcl::PointXYZI> pcl_input, pcl_transformed_cloud;
pcl::fromROSMsg(input, pcl_input);

// transform pointcloud to tagret_frame
// transform pointcloud to target_frame
if (target_frame_ != input.header.frame_id) {
try {
geometry_msgs::msg::TransformStamped transform_stamped;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -94,12 +94,12 @@ void FeatureMapWithConstant::initializeMap(std::vector<float> & map)
for (int col = 0; col < width; ++col) {
int idx = row * width + col;
// * row <-> x, column <-> y
// retutn the distance from my car to center of the grid.
// return the distance from my car to center of the grid.
// Pc means point cloud = real world scale. so transform pixel scale to
// real world scale
float center_x = Pixel2Pc(row, height, range);
float center_y = Pixel2Pc(col, width, range);
// normaliztion. -0.5~0.5
// normalization. -0.5~0.5
direction_data[idx] = static_cast<float>(std::atan2(center_y, center_x) / (2.0 * M_PI));
distance_data[idx] = static_cast<float>(std::hypot(center_x, center_y) / 60.0 - 0.5);
}
Expand Down Expand Up @@ -136,12 +136,12 @@ void FeatureMapWithConstantAndIntensity::initializeMap(std::vector<float> & map)
for (int col = 0; col < width; ++col) {
int idx = row * width + col;
// * row <-> x, column <-> y
// retutn the distance from my car to center of the grid.
// return the distance from my car to center of the grid.
// Pc means point cloud = real world scale. so transform pixel scale to
// real world scale
float center_x = Pixel2Pc(row, height, range);
float center_y = Pixel2Pc(col, width, range);
// normaliztion. -0.5~0.5
// normalization. -0.5~0.5
direction_data[idx] = static_cast<float>(std::atan2(center_y, center_x) / (2.0 * M_PI));
distance_data[idx] = static_cast<float>(std::hypot(center_x, center_y) / 60.0 - 0.5);
}
Expand Down

0 comments on commit 1937501

Please sign in to comment.