/* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ #define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ #include <vector> #include "Eigen/Core" #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/sensor/rangefinder_point.h" #include "cartographer/transform/rigid_transform.h" #include "glog/logging.h" namespace cartographer { namespace sensor { // Stores 3D positions of points. // For 2D points, the third entry is 0.f. using PointCloud = std::vector<RangefinderPoint>; // Stores 3D positions of points with their relative measurement time in the // fourth entry. Time is in seconds, increasing and relative to the moment when // the last point was acquired. So, the fourth entry for the last point is 0.f. // If timing is not available, all fourth entries are 0.f. For 2D points, the // third entry is 0.f (and the fourth entry is time). using TimedPointCloud = std::vector<TimedRangefinderPoint>; struct PointCloudWithIntensities { TimedPointCloud points; std::vector<float> intensities; }; // Transforms 'point_cloud' according to 'transform'. PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform); // Transforms 'point_cloud' according to 'transform'. TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, const transform::Rigid3f& transform); // Returns a new point cloud without points that fall outside the region defined // by 'min_z' and 'max_z'. PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z, float max_z); // Returns a new point cloud without points that fall outside the region defined // by 'min_z' and 'max_z'. TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, float min_z, float max_z); /////////////////////////////////////////////////////////////////////////// // Copy from LOAM struct LoamPoint; #define LOAM_MAX_RING 64 #define LOAM_LIDAR_TYPE_32E 0 #define LOAM_LIDAR_TYPE_40P 1 class LoamProjection { public: LoamProjection(int lidar_type); ~LoamProjection(); int FilterCloud(TimedPointCloud& laserCloud, const float highResRange); private: void ProjectPointCloud(const TimedPointCloud& laserCloudIn); void GroundRemoval(); void LabelComponents(int row, int col); private: const float sensorMountAngle = 0.0; // installation angle std::vector<std::pair<int8_t, int8_t>> neighbors; int size; LoamPoint* projCloud; uint16_t* queueRow; // breadth-first search process of segmentation uint16_t* queueCol; int labelCount; size_t N_SCAN, HORIZON_SCAN, N_GROUND_SCAN; const float* N_SCAN_ANGLE; float HORIZON_ANGLE; float N_SCAN_RAD[LOAM_MAX_RING]; float HORIZON_RAD; const float SCAN_ANGLE_32E[LOAM_MAX_RING] = { -30.67, -29.33, -28.00, -26.67, -25.33, -24.00, -22.67, -21.33, -20.00, -18.67, -17.33, -16.00, -14.67, -13.33, -12.00, -10.67, -9.33, -8.00, -6.67, -5.33, -4.00, -2.67, -1.33, 0.00, 1.33, 2.67, 4.00, 5.33, 6.67, 8.00, 9.33, 10.67}; const float SCAN_ANGLE_40P[LOAM_MAX_RING] = { -25, -19, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5.67, -5.33, -5, -4.67, -4.33, -4, -3.67, -3.33, -3, -2.67, -2.33, -2, -1.67, -1.33, -1, -0.67, -0.33, 0, 0.33, 0.67, 1, 1.33, 1.67, 2, 3, 5, 8, 11, 15}; const float SENSOR_MIN_RANGE = 1.0; const float SEGMENT_GROUND_ANGLE = 6; // default is 10 const float SEGMENT_HORIZON_PLANE = 10.0 / 180.0 * M_PI;// default is 10 const float SEGMENT_SAME_LABEL = 50.0 / 180.0 * M_PI; // default is 60 const int SEGMENT_VALID_NUM = 50; const int SEGMENT_VALID_MIN_WIDTH = 10; const int SEGMENT_VALID_MIN_LINE = 5; }; } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_