├── CMakeLists.txt ├── LICENSE ├── README.md ├── ground_remove2.cpp ├── ground_remove2.h ├── image ├── cloud.png ├── ouput2.png ├── range_image.png ├── range_image_only.png ├── range_image_with_cluster.png └── threshood.jpeg ├── range_image_cluster.cpp └── test.bin /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.12) 2 | project(point) 3 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fopenmp") 4 | set(CMAKE_BUILD_TYPE "Release") 5 | find_package(PCL 1.8 REQUIRED) 6 | find_package(Eigen3 REQUIRED) 7 | find_package(Boost REQUIRED COMPONENTS system iostreams) 8 | 9 | find_package(OpenCV REQUIRED) 10 | 11 | add_definitions( ${PCL_DEFINITIONS} ) 12 | include_directories(${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} ) 13 | 14 | add_executable(range_image_cluster range_image_cluster.cpp ground_remove2.cpp) 15 | target_link_libraries(range_image_cluster ${EIGEN3_INCLUDE_DIR} ${PCL_LIBRARIES} ${Boost_INCLUDE_DIRS} ${OpenCV_LIBS}) 16 | 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 WX96 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Range-Image-based-segementation 2 | An implementation on "I. Bogoslavskyi and C. Stachniss, "Fast range image-based segmentation of sparse 3D laser scans for online operation," 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, 2016, pp. 163-169, doi: 10.1109/IROS.2016.7759050." 3 | 4 | An official implementaiton is here:https://github.com/PRBonn/depth_clustering 5 | 6 | The ground remove code is from https://github.com/AbangLZU/plane_fit_ground_filter. We achieve multi-plane fitting by using multi-thread. 7 | 8 | ### Different from paper: 9 | 10 | 1. We changed the ground segmentation method 11 | 2. Using hash map replace the image-based search 12 | 13 | To the horizon neighbor search, we didn't use the method in paper. The method that we used to classify two neighbor objects as follows: 14 | 15 | ![Image text](https://github.com/WAN96/Range-Image-based-segementation/blob/master/image/threshood.jpeg) 16 | 17 | 18 | 19 | ### How to use 20 | 21 | mkdir build 22 | cd build 23 | cmake .. 24 | make 25 | ./range_cluster test.bin 26 | 27 | ### The Result 28 | 29 | ![Image text](https://github.com/WAN96/Range-Image-based-segementation/blob/master/image/cloud.png) 30 | ![Image text](https://github.com/WAN96/Range-Image-based-segementation/blob/master/image/range_image_only.png) 31 | ![Image text](https://github.com/WAN96/Range-Image-based-segementation/blob/master/image/range_image_with_cluster.png) 32 | ![Image text](https://github.com/WAN96/Range-Image-based-segementation/blob/master/image/ouput2.png) 33 | 34 | 35 | -------------------------------------------------------------------------------- /ground_remove2.cpp: -------------------------------------------------------------------------------- 1 | /*MIT License 2 | 3 | Copyright (c) 2020 WX96 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE.*/ 22 | 23 | #include "ground_remove2.h" 24 | 25 | MatrixXf normal_2(3, 1); 26 | vector normal2(3); 27 | float th_dist_d_2; 28 | 29 | static int64_t gtm() { 30 | struct timeval tm; 31 | gettimeofday(&tm, 0); 32 | // return ms 33 | int64_t re = (((int64_t) tm.tv_sec) * 1000 * 1000 + tm.tv_usec); 34 | return re; 35 | } 36 | 37 | bool point_cmp(Velodyne_32_HDLE a, Velodyne_32_HDLE b) { 38 | return a.z < b.z; 39 | } 40 | 41 | GroundRemove2::GroundRemove2(int num_iter, int num_lpr, double th_seeds, 42 | double th_dist) { 43 | num_iter_ = num_iter; 44 | num_lpr_ = num_lpr; 45 | th_seeds_ = th_seeds; 46 | th_dist_ = th_dist; 47 | } 48 | 49 | void GroundRemove2::extract_initial_seeds_2( 50 | const pcl::PointCloud& p_sorted, 51 | pcl::PointCloud& g_seeds_pc) { 52 | // LPR is the mean of low point representative 53 | double sum = 0; 54 | int cnt = 0; 55 | // Calculate the mean height value. 56 | for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; ++i) { 57 | sum += p_sorted.points[i].z; 58 | cnt++; 59 | } 60 | double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0 61 | g_seeds_pc.clear(); 62 | // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ 63 | for (int i = 0; i < p_sorted.points.size(); ++i) { 64 | if (p_sorted.points[i].z < lpr_height + th_seeds_) { 65 | g_seeds_pc.points.push_back(p_sorted.points[i]); 66 | } 67 | } 68 | // return seeds points 69 | } 70 | 71 | void GroundRemove2::estimate_plane_2( 72 | const pcl::PointCloud& g_ground_pc) { 73 | // Create covarian matrix in single pass. 74 | // TODO: compare the efficiency. 75 | Eigen::Matrix3f cov; 76 | Eigen::Vector4f pc_mean; 77 | vector conv, mean; 78 | 79 | pcl::computeMeanAndCovarianceMatrix(g_ground_pc, cov, pc_mean); 80 | 81 | //computeMeanAndCovarianceMatrix(g_ground_pc, conv, mean); 82 | 83 | //static float A[9] = { conv[0], conv[1], conv[2], conv[3], 84 | //conv[4], conv[5], conv[6], conv[7], conv[8],}; 85 | //static Eigen::MatrixXf conv2 = Eigen::Map >(A); 86 | // Singular Value Decomposition: SVD 87 | JacobiSVD svd(cov, Eigen::DecompositionOptions::ComputeFullU); 88 | // use the least singular vector as normal 89 | normal_2 = (svd.matrixU().col(2)); 90 | // mean ground seeds value 91 | //Eigen::Vector3f seeds_mean = pc_mean.head<3>(); 92 | 93 | // according to normal.T*[x,y,z] = -d 94 | //float d_ = -(normal_.transpose()*seeds_mean)(0,0); 95 | 96 | float d_ = -(normal_2(0, 0) * pc_mean(0) + normal_2(1, 0) * pc_mean(1) 97 | + normal_2(2, 0) * pc_mean(2)); 98 | 99 | // set distance threhold to `th_dist - d` 100 | th_dist_d_2 = th_dist_ - d_; 101 | 102 | // return the equation parameters 103 | } 104 | 105 | void GroundRemove2::RemoveGround_Thread2(pcl::PointCloud& cloudIn, 106 | pcl::PointCloud& cloudgc, 107 | pcl::PointCloud& cloudngc, 108 | pcl::PointCloud& g_ground_pc1, 109 | pcl::PointCloud& g_not_ground_pc1) { 110 | 111 | std::lock_guard < std::mutex > lock(regionmutex); 112 | pcl::PointCloud::Ptr g_seeds_pc( 113 | new pcl::PointCloud()); 114 | 115 | sort(cloudIn.points.begin(), cloudIn.points.end(), point_cmp); 116 | 117 | extract_initial_seeds_2(cloudIn, *g_seeds_pc); 118 | 119 | cloudgc = *g_seeds_pc; 120 | 121 | for (int i = 0; i < num_iter_; ++i) { 122 | 123 | estimate_plane_2(cloudgc); 124 | 125 | cloudgc.clear(); 126 | cloudngc.clear(); 127 | 128 | float xd = normal_2(0, 0); 129 | float yd = normal_2(1, 0); 130 | float zd = normal_2(2, 0); 131 | for (auto p : cloudIn.points) { 132 | float distance = p.x * xd + p.y * yd + p.z * zd; 133 | if (distance < th_dist_d_2) { 134 | //g_all_pc->points[r].label = 1u;// means ground 135 | cloudgc.points.push_back(p); 136 | } else { 137 | //g_all_pc->points[r].label = 0u;// means not ground and non clusterred 138 | cloudngc.points.push_back(p); 139 | } 140 | 141 | } 142 | 143 | } 144 | 145 | for (int k = 0; k < cloudgc.points.size(); ++k) { 146 | 147 | g_ground_pc1.points.push_back(cloudgc.points[k]); 148 | } 149 | 150 | for (int k = 0; k < cloudngc.points.size(); ++k) { 151 | 152 | g_not_ground_pc1.points.push_back(cloudngc.points[k]); 153 | } 154 | 155 | } 156 | 157 | void GroundRemove2::RemoveGround2(pcl::PointCloud& cloudIn, 158 | pcl::PointCloud& g_ground_pc, 159 | pcl::PointCloud& g_not_ground_pc) { 160 | pcl::PointCloud::Ptr g_seeds_region1( 161 | new pcl::PointCloud()); 162 | pcl::PointCloud::Ptr g_seeds_region2( 163 | new pcl::PointCloud()); 164 | pcl::PointCloud::Ptr g_seeds_region3( 165 | new pcl::PointCloud()); 166 | pcl::PointCloud::Ptr g_ground_pc1( 167 | new pcl::PointCloud()); 168 | pcl::PointCloud::Ptr g_not_ground_pc1( 169 | new pcl::PointCloud()); 170 | 171 | float xmin = -35, xmax = 35, ymin = -30, ymax = 30, zmin = -1.0, zmax = 3.0; 172 | float regionsize = (ymax - ymin) / 3; 173 | for (int i = 0; i < cloudIn.points.size(); ++i) { 174 | if (cloudIn.points[i].z < 0.75) { 175 | 176 | if (cloudIn.points[i].y < ymax - 0 * regionsize 177 | && cloudIn.points[i].y > ymax - 1 * regionsize) { 178 | g_seeds_region1->points.push_back(cloudIn.points[i]); 179 | } 180 | if (cloudIn.points[i].y < ymax - 1 * regionsize 181 | && cloudIn.points[i].y > ymax - 2 * regionsize) { 182 | g_seeds_region2->points.push_back(cloudIn.points[i]); 183 | } 184 | if (cloudIn.points[i].y < ymax - 2 * regionsize 185 | && cloudIn.points[i].y > ymax - 3 * regionsize) { 186 | g_seeds_region3->points.push_back(cloudIn.points[i]); 187 | } 188 | } else { 189 | g_not_ground_pc.points.push_back(cloudIn.points[i]); 190 | } 191 | 192 | } 193 | 194 | cloudIn.clear(); 195 | 196 | vector::Ptr> pcregion(3); 197 | 198 | pcregion[0] = g_seeds_region1; 199 | pcregion[1] = g_seeds_region2; 200 | pcregion[2] = g_seeds_region3; 201 | 202 | std::vector thread_vec(num_seg_); 203 | 204 | for (int ri = 0; ri < num_seg_; ++ri) { 205 | 206 | thread_vec[ri] = std::thread(&GroundRemove2::RemoveGround_Thread2, this, 207 | std::ref(*pcregion[ri]), std::ref(*g_ground_pc1), 208 | std::ref(*g_not_ground_pc1), std::ref(g_ground_pc), 209 | std::ref(g_not_ground_pc)); 210 | 211 | } 212 | 213 | for (auto it = thread_vec.begin(); it != thread_vec.end(); ++it) { 214 | it->join(); 215 | } 216 | 217 | } 218 | 219 | 220 | /*template 221 | void CloudFilter(const pcl::PointCloud& cloudIn, 222 | pcl::PointCloud& cloudOut, float x_min, float x_max, 223 | float y_min, float y_max, float z_min, float z_max) { 224 | cloudOut.header = cloudIn.header; 225 | cloudOut.sensor_orientation_ = cloudIn.sensor_orientation_; 226 | cloudOut.sensor_origin_ = cloudIn.sensor_origin_; 227 | cloudOut.points.clear(); 228 | //1) set parameters for removing cloud reflect on ego vehicle 229 | float x_limit_min = -1.8, x_limit_max = 1.8, y_limit_forward = 5.0, 230 | y_limit_backward = -4.5; 231 | //2 apply the filter 232 | for (int i = 0; i < cloudIn.size(); ++i) { 233 | float x = cloudIn.points[i].x; 234 | float y = cloudIn.points[i].y; 235 | float z = cloudIn.points[i].z; 236 | // whether on ego vehicle 237 | if ((x > x_limit_min && x < x_limit_max && y > y_limit_backward 238 | && y < y_limit_forward)) 239 | continue; 240 | if ((x > x_min && x < x_max && y > y_min && y < y_max && z > z_min 241 | && z < z_max)) { 242 | 243 | cloudOut.points.push_back(cloudIn.points[i]); 244 | 245 | } 246 | 247 | } 248 | } 249 | 250 | void Cloudcolor(const pcl::PointCloud& cloudIn, 251 | const pcl::PointCloud& cloudground, 252 | pcl::PointCloud& cloudOut) { 253 | 254 | int groundsize = cloudground.points.size(); 255 | int ngroundsize = cloudIn.points.size(); 256 | int size = groundsize + ngroundsize; 257 | 258 | for (int i = 0; i < ngroundsize; ++i) { 259 | Velodyne_32_HDLERGB p; 260 | p.x = cloudIn.points[i].x; 261 | p.y = cloudIn.points[i].y; 262 | p.z = cloudIn.points[i].z; 263 | p.r = 225; 264 | p.g = 0; 265 | p.b = 0; 266 | cloudOut.points.push_back(p); 267 | } 268 | 269 | for (int i = 0; i < groundsize; ++i) { 270 | Velodyne_32_HDLERGB p; 271 | p.x = cloudground.points[i].x; 272 | p.y = cloudground.points[i].y; 273 | p.z = cloudground.points[i].z; 274 | p.r = 225; 275 | p.g = 255; 276 | p.b = 255; 277 | cloudOut.points.push_back(p); 278 | } 279 | } 280 | 281 | class SubscribeAndPublish { 282 | public: 283 | SubscribeAndPublish(ros::NodeHandle nh, std::string lidar_topic_name, 284 | std::string imu_topic_name); 285 | 286 | void callback(const sensor_msgs::PointCloud2ConstPtr& cloudmsg, 287 | const sensor_driver_msgs::GpswithHeadingConstPtr& gps_msg) { 288 | pcl::PointCloud::Ptr cloud( 289 | new pcl::PointCloud); 290 | pcl::PointCloud::Ptr cloud_t( 291 | new pcl::PointCloud); 292 | pcl::PointCloud::Ptr cloud_f( 293 | new pcl::PointCloud); 294 | pcl::PointCloud::Ptr cloud_color( 295 | new pcl::PointCloud); 296 | 297 | pcl::PointCloud::Ptr g_ground_pc( 298 | new pcl::PointCloud()); 299 | pcl::PointCloud::Ptr g_not_ground_pc( 300 | new pcl::PointCloud()); 301 | pcl::fromROSMsg(*cloudmsg, *cloud); 302 | 303 | float xmin = -35, xmax = 35, ymin = -30, ymax = 30, zmin = -1.0, zmax = 304 | 3.0; 305 | 306 | CloudFilter(*cloud, *cloud_t, xmin, xmax, ymin, ymax, zmin, zmax); 307 | TransformKittiCloud( *cloud_t,*cloud_f); 308 | 309 | cloud->clear(); 310 | int64_t tm0 = gtm(); 311 | GroundRemove grobject(3,20,1.0,0.15); 312 | grobject.RemoveGround(*cloud_f,*g_ground_pc,*g_not_ground_pc); 313 | int64_t tm1 = gtm(); 314 | printf("[INFO]region build cast time:%ld us\n", tm1 - tm0); 315 | 316 | Cloudcolor(*g_ground_pc, *g_not_ground_pc, *cloud_color); 317 | 318 | cloud_color->height = 1; 319 | cloud_color->width = cloud_color->points.size(); 320 | cloud_color->is_dense = false; //最终优化结果 321 | 322 | sensor_msgs::PointCloud2 ros_cloud; 323 | pcl::toROSMsg(*cloud_color, ros_cloud); 324 | ros_cloud.header.frame_id = "global_init_frame"; 325 | pub_.publish(ros_cloud); 326 | //pcl::io::savePCDFileASCII ("test_simple.pcd", *cloud_color); 327 | //pcl::visualization::CloudViewer viewer("PCD2"); 328 | //viewer.showCloud(cloud_color); 329 | 330 | } 331 | private: 332 | ros::NodeHandle n_; 333 | ros::Publisher pub_; 334 | ros::Subscriber sub_; 335 | message_filters::Subscriber Sub_Lidar; 336 | //message_filters::Subscriber Sub_IMU; 337 | message_filters::Subscriber Sub_IMU; 338 | typedef sync_policies::ApproximateTime MySyncPolicy; 340 | // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) 341 | Synchronizer sync; 342 | 343 | }; 344 | 345 | SubscribeAndPublish::SubscribeAndPublish(ros::NodeHandle nh, 346 | std::string lidar_topic_name, std::string imu_topic_name) : 347 | n_(nh), Sub_Lidar(nh, lidar_topic_name, 10), Sub_IMU(nh, imu_topic_name, 348 | 20), sync(MySyncPolicy(10), Sub_Lidar, Sub_IMU) { 349 | //Topic you want to publish 350 | pub_ = nh.advertise < sensor_msgs::PointCloud2 > ("/groundremove", 1); 351 | 352 | //Topic you want to subscribe 353 | //sub_ = n_.subscribe("lidar_cloud_calibrated", 1, &SubscribeAndPublish::callback, this); 354 | // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10) 355 | sync.registerCallback( 356 | boost::bind(&SubscribeAndPublish::callback, this, _1, _2)); 357 | } 358 | 359 | 360 | int main(int argc, char** argv) { 361 | 362 | ros::init(argc, argv, "ground_node"); 363 | SubscribeAndPublish SAPObject(ros::NodeHandle(), "lidar_cloud_calibrated", 364 | "gpsdata"); 365 | ROS_INFO("waiting for data!"); 366 | ros::spin(); 367 | 368 | return 0; 369 | }*/ 370 | 371 | -------------------------------------------------------------------------------- /ground_remove2.h: -------------------------------------------------------------------------------- 1 | /*MIT License 2 | 3 | Copyright (c) 2020 WX96 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE.*/ 22 | 23 | 24 | 25 | #ifndef GROUND_REMOVE2_H 26 | #define GROUND_REMOVE2_H 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | //PCL 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include //自定义点云类型时要加 47 | #include 48 | #include 49 | #include 50 | //Eigen 51 | #include 52 | /*//ROS 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include */ 60 | 61 | using namespace std; 62 | //using namespace message_filters; 63 | using Eigen::MatrixXf; 64 | using Eigen::JacobiSVD; 65 | using Eigen::VectorXf; 66 | 67 | struct Velodyne_32_HDLE{ 68 | PCL_ADD_POINT4D; 69 | int ring; 70 | int pcaketnum; 71 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 72 | }EIGEN_ALIGN16; 73 | 74 | POINT_CLOUD_REGISTER_POINT_STRUCT(Velodyne_32_HDLE, 75 | (float,x,x) 76 | (float,y,y) 77 | (float,z,z) 78 | (int,ring,ring) 79 | (int,pcaketnum,pcaketnum) 80 | ) 81 | 82 | 83 | class GroundRemove2 { 84 | 85 | public: 86 | 87 | GroundRemove2(int num_iter, int num_lpr, double th_seeds, double th_dist); 88 | void extract_initial_seeds_2(const pcl::PointCloud& p_sorted, 89 | pcl::PointCloud& g_seeds_pc); 90 | void estimate_plane_2(const pcl::PointCloud& g_ground_pc); 91 | void RemoveGround_Thread2(pcl::PointCloud& cloudIn, 92 | pcl::PointCloud& cloudgc, 93 | pcl::PointCloud& cloudngc, 94 | pcl::PointCloud& g_ground_pc1, 95 | pcl::PointCloud& g_not_ground_pc1); 96 | void RemoveGround2(pcl::PointCloud& cloudIn, 97 | pcl::PointCloud& g_ground_pc, 98 | pcl::PointCloud& g_not_ground_pc); 99 | 100 | private: 101 | 102 | mutex regionmutex; 103 | int num_iter_ = 3; 104 | int num_lpr_ = 20; 105 | double th_seeds_ = 1.0; 106 | double th_dist_ = 0.15; 107 | int num_seg_ = 3; 108 | 109 | }; 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /image/cloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/image/cloud.png -------------------------------------------------------------------------------- /image/ouput2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/image/ouput2.png -------------------------------------------------------------------------------- /image/range_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/image/range_image.png -------------------------------------------------------------------------------- /image/range_image_only.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/image/range_image_only.png -------------------------------------------------------------------------------- /image/range_image_with_cluster.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/image/range_image_with_cluster.png -------------------------------------------------------------------------------- /image/threshood.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/image/threshood.jpeg -------------------------------------------------------------------------------- /range_image_cluster.cpp: -------------------------------------------------------------------------------- 1 | /*MIT License 2 | 3 | Copyright (c) 2020 WX96 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE.*/ 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include "ground_remove2.h" 51 | 52 | 53 | #define PCL_NO_PRECOMPILE 54 | using namespace std; 55 | const float PI = 3.1415926; 56 | 57 | 58 | 59 | 60 | float Polar_angle_cal(float x, float y) { 61 | float temp_tangle = 0; 62 | if (x == 0 && y == 0) { 63 | temp_tangle = 0; 64 | } else if (y >= 0) { 65 | temp_tangle = (float) atan2(y, x); 66 | } else if (y <= 0) { 67 | temp_tangle = (float) atan2(y, x) + 2 * PI; 68 | } 69 | return temp_tangle; 70 | } 71 | //HSV转rgb 72 | vector hsv2rgb(vector& hsv) { 73 | vector rgb(3); 74 | float R, G, B, H, S, V; 75 | H = hsv[0]; 76 | S = hsv[1]; 77 | V = hsv[2]; 78 | if (S == 0) { 79 | rgb[0] = rgb[1] = rgb[2] = V; 80 | } else { 81 | 82 | int i = int(H * 6); 83 | float f = (H * 6) - i; 84 | float a = V * (1 - S); 85 | float b = V * (1 - S * f); 86 | float c = V * (1 - S * (1 - f)); 87 | i = i % 6; 88 | switch (i) { 89 | case 0: { 90 | rgb[0] = V; 91 | rgb[1] = c; 92 | rgb[2] = a; 93 | break; 94 | } 95 | case 1: { 96 | rgb[0] = b; 97 | rgb[1] = V; 98 | rgb[2] = a; 99 | break; 100 | } 101 | case 2: { 102 | rgb[0] = a; 103 | rgb[1] = V; 104 | rgb[2] = c; 105 | break; 106 | } 107 | case 3: { 108 | rgb[0] = a; 109 | rgb[1] = b; 110 | rgb[2] = V; 111 | break; 112 | } 113 | case 4: { 114 | rgb[0] = c; 115 | rgb[1] = a; 116 | rgb[2] = V; 117 | break; 118 | } 119 | case 5: { 120 | rgb[0] = V; 121 | rgb[1] = a; 122 | rgb[2] = b; 123 | break; 124 | } 125 | } 126 | } 127 | 128 | return rgb; 129 | } 130 | 131 | //可视化 132 | 133 | template string toString(const T& t) { 134 | ostringstream oss; 135 | oss << t; 136 | return oss.str(); 137 | } 138 | 139 | 140 | 141 | template 142 | float CalculateRangeXY(const PointInT pointIn) { 143 | 144 | return sqrt(pointIn.x * pointIn.x + pointIn.y * pointIn.y); 145 | } 146 | 147 | template 148 | float CalculateRangeZXY(const PointInT pointIn) { 149 | 150 | return sqrt( 151 | pointIn.x * pointIn.x + pointIn.y * pointIn.y 152 | + (pointIn.z-2.1) * (pointIn.z-2.1)); 153 | } 154 | 155 | 156 | 157 | struct Range { 158 | 159 | float range_xy; 160 | float range_zxy; 161 | int ring_i; 162 | int frame_j; 163 | int count_num; 164 | 165 | }; 166 | 167 | 168 | 169 | 170 | void CloudFilter(const pcl::PointCloud& cloudIn, 171 | pcl::PointCloud& cloudOut, float x_min, float x_max, 172 | float y_min, float y_max, float z_min, float z_max) { 173 | cloudOut.header = cloudIn.header; 174 | cloudOut.sensor_orientation_ = cloudIn.sensor_orientation_; 175 | cloudOut.sensor_origin_ = cloudIn.sensor_origin_; 176 | cloudOut.points.clear(); 177 | //1) set parameters for removing cloud reflect on ego vehicle 178 | float x_limit_min = -1.8, x_limit_max = 1.8, y_limit_forward = 5.0, 179 | y_limit_backward = -4.5; 180 | //2 apply the filter 181 | for (int i = 0; i < cloudIn.size(); ++i) { 182 | float x = cloudIn.points[i].x; 183 | float y = cloudIn.points[i].y; 184 | float z = cloudIn.points[i].z; 185 | // whether on ego vehicle 186 | if ((x > x_limit_min && x < x_limit_max && y > y_limit_backward 187 | && y < y_limit_forward)) 188 | continue; 189 | if ((x > x_min && x < x_max && y > y_min && y < y_max && z > z_min 190 | && z < z_max)) { 191 | 192 | cloudOut.points.push_back(cloudIn.points[i]); 193 | 194 | } 195 | 196 | } 197 | } 198 | 199 | 200 | 201 | int total_frame = 0; 202 | 203 | void Transform2RangeImage(const pcl::PointCloud& cloudIn, 204 | pcl::PointCloud& ng_cloudOut, 205 | pcl::PointCloud& g_cloudOut, unordered_map &unordered_map_out) { 206 | 207 | total_frame = int(cloudIn.points.size() / 32) - 1; 208 | pcl::PointCloud::Ptr cloud2range( 209 | new pcl::PointCloud); 210 | 211 | pcl::PointCloud::Ptr cloud_filtered( 212 | new pcl::PointCloud); 213 | cloud2range->points.resize(cloudIn.points.size()); 214 | 215 | for (int j = 0; j < total_frame; ++j) { 216 | int num_odd = 16; //基数从16位开始排 217 | int num_even = 0; //偶数从头 218 | 219 | for (int i = 0; i < 32; ++i) { 220 | if (float(i % 2) == 0.0) { 221 | cloud2range->points[j * 32 + i].x = cloudIn.points[j * 32 + i].x ; 222 | cloud2range->points[j * 32 + i].y = cloudIn.points[j * 32 + i].y ; 223 | cloud2range->points[j * 32 + i].z = cloudIn.points[j * 32 + i].z ; 224 | cloud2range->points[j * 32 + i].ring =num_even; 225 | cloud2range->points[j * 32 + i].pcaketnum =j; 226 | num_even++; 227 | } else { 228 | cloud2range->points[j * 32 + i].x = cloudIn.points[j * 32 + i].x; 229 | cloud2range->points[j * 32 + i].y = cloudIn.points[j * 32 + i].y ; 230 | cloud2range->points[j * 32 + i].z = cloudIn.points[j * 32 + i].z ; 231 | cloud2range->points[j * 32 + i].ring =num_odd; 232 | cloud2range->points[j * 32 + i].pcaketnum =j; 233 | num_odd++; 234 | } 235 | } //按索引顺序排列 236 | 237 | } 238 | 239 | cloud2range->height = 1; 240 | cloud2range->width = cloud2range->points.size(); 241 | 242 | 243 | float xmin = -35, xmax = 35, ymin = -30, ymax = 30, zmin = -1.0, zmax = 3.0; 244 | CloudFilter(*cloud2range, *cloud_filtered, xmin, xmax, ymin, ymax, zmin, zmax); 245 | 246 | GroundRemove2 ground_remove(3, 20, 1.0, 0.15); 247 | 248 | ground_remove.RemoveGround2(*cloud_filtered, g_cloudOut, ng_cloudOut); 249 | 250 | 251 | float x_limit_min = -1, x_limit_max = 1, y_limit_forward = 4.0, 252 | y_limit_backward = -3; 253 | 254 | for(int i=0; i 30) 261 | continue; 262 | if ((x > x_limit_min && x < x_limit_max && y > y_limit_backward 263 | && y < y_limit_forward)) 264 | continue; 265 | 266 | if ((z < -1 || z > 3)) 267 | continue;*/ 268 | 269 | 270 | int ringnum = ng_cloudOut.points[i].ring; 271 | 272 | int image_index = ng_cloudOut.points[i].pcaketnum * 32 + (31 - ringnum); 273 | Range r; 274 | r.ring_i = 31 -ringnum; 275 | r.frame_j = ng_cloudOut.points[i].pcaketnum; 276 | r.count_num = i; 277 | r.range_xy = ng_cloudOut.points[i].z; 278 | r.range_zxy = CalculateRangeZXY(ng_cloudOut.points[i]); 279 | unordered_map_out.insert(make_pair(image_index, r)); 280 | 281 | } 282 | 283 | 284 | 285 | 286 | 287 | } 288 | 289 | 290 | 291 | void find_neighbors(int Ix, int Iy, vector& neighborudindex, 292 | vector& neighborlrindex) { 293 | //cout<<"silinyu " << Ix<< " "< total_frame-1) { 304 | px = 0; 305 | // cout<<"RR"< 31) 315 | continue; 316 | //cout<& cluster_indices, int idx1, int idx2) { 322 | for (int i = 0; i < cluster_indices.size(); i++) { 323 | if (cluster_indices[i] == idx1) { 324 | cluster_indices[i] = idx2; 325 | } 326 | } 327 | } 328 | 329 | 330 | bool compare_index(pair a, pair b) { 331 | return a.first < b.first; 332 | } //升序 333 | 334 | vector range_cluster(unordered_map &unordered_map_in, double ringnum) { 335 | 336 | int current_cluster = 0; 337 | vector cluster_indices = vector(total_frame * 32, -1); 338 | float horizon_angle_resolution = 0.16 * PI / 180; 339 | float vertical_angle_resolution = 1.33 * PI / 180; 340 | vector> tr(unordered_map_in.begin(), unordered_map_in.end()); 341 | sort(tr.begin(), tr.end(), compare_index); 342 | for(int i =0 ; i< tr.size(); ++i){ 343 | } 344 | 345 | int for_num = 0; 346 | float theta_thresh = 8 * PI / 180; 347 | float theta_thresh2 =30 * PI / 180; 348 | 349 | for (int i = 0; i < tr.size(); ++i) { 350 | unordered_map::iterator it_find; 351 | it_find = unordered_map_in.find(tr[i].first); 352 | 353 | 354 | if (it_find != unordered_map_in.end() && cluster_indices[tr[i].first] == -1) { 355 | queue> q; 356 | vector indexxy(2); 357 | indexxy[0] = it_find->second.frame_j; 358 | indexxy[1] = it_find->second.ring_i; 359 | q.push(indexxy); 360 | while (q.size()>0) { 361 | 362 | if (cluster_indices[q.front()[0] * 32 + q.front()[1]] 363 | != -1) { 364 | q.pop(); 365 | continue; 366 | } 367 | 368 | cluster_indices[q.front()[0] * 32 + q.front()[1]] = 369 | current_cluster; 370 | vector neighborudid; 371 | vector neighborlfid; 372 | unordered_map::iterator it_findo; 373 | it_findo = unordered_map_in.find(q.front()[0] * 32 + q.front()[1]); 374 | find_neighbors(q.front()[0], q.front()[1], neighborudid, 375 | neighborlfid); 376 | 377 | if (neighborudid.size() > 0) { 378 | for (int in = 0; in < neighborudid.size(); ++in) { 379 | unordered_map::iterator it_findn; 380 | it_findn = unordered_map_in.find(neighborudid[in]); 381 | if (it_findn != unordered_map_in.end()) { 382 | float d1 = max(it_findo->second.range_zxy, 383 | it_findn->second.range_zxy); 384 | float d2 = min(it_findo->second.range_zxy, 385 | it_findn->second.range_zxy); 386 | 387 | 388 | float angle = fabs((float) atan2(d2* sin(vertical_angle_resolution),d1- d2* cos(vertical_angle_resolution))); 389 | float dmax = (it_findo->second.range_zxy) * sin(1.33*PI/180)/sin(50*PI/180 -1.33*PI/180) + 3*0.2; 390 | if (it_findo->second.range_xy>1.2 && fabs(d2-d1) < dmax) { 391 | vector indexxy(2); 392 | indexxy[0] = it_findn->second.frame_j; 393 | indexxy[1] = it_findn->second.ring_i; 394 | q.push(indexxy); 395 | }else if (angle > theta_thresh2) { 396 | vector indexxy(2); 397 | indexxy[0] = it_findn->second.frame_j; 398 | indexxy[1] = it_findn->second.ring_i; 399 | q.push(indexxy); 400 | } 401 | } 402 | } 403 | } 404 | 405 | if (neighborlfid.size() > 0) { 406 | for (int in = 0; in < neighborlfid.size(); ++in) { 407 | unordered_map::iterator it_findn; 408 | it_findn = unordered_map_in.find(neighborlfid[in]); 409 | if (it_findn != unordered_map_in.end()) { 410 | float d1 = max(it_findo->second.range_zxy, 411 | it_findn->second.range_zxy); 412 | float d2 = min(it_findo->second.range_zxy, 413 | it_findn->second.range_zxy); 414 | 415 | float angle = fabs((float) atan2(d2* sin(horizon_angle_resolution),d1- d2* cos(horizon_angle_resolution))); 416 | float dmax = (it_findo->second.range_zxy) * sin(360/ringnum*PI/180)/sin(30*PI/180 -360/ringnum*PI/180) + 3*0.2; 417 | //if (fabs(it_findo->second.range_zxy-it_findn->second.range_zxy) < dmax) { 418 | if (angle > theta_thresh) { 419 | //cluster_indices[neighborlfid[in]] = 420 | //current_cluster; 421 | vector indexxy(2); 422 | indexxy[0] = it_findn->second.frame_j; 423 | indexxy[1] = it_findn->second.ring_i; 424 | //cout<<"LLL2 "< a, pair b) { 441 | return a.second < b.second; 442 | } //升序 443 | 444 | bool most_frequent_value(vector values, vector &cluster_index) { 445 | unordered_map histcounts; 446 | for (int i = 0; i < values.size(); i++) { 447 | if (histcounts.find(values[i]) == histcounts.end()) { 448 | histcounts[values[i]] = 1; 449 | } else { 450 | histcounts[values[i]] += 1; 451 | } 452 | } 453 | 454 | int max = 0, maxi; 455 | vector> tr(histcounts.begin(), histcounts.end()); 456 | sort(tr.begin(), tr.end(), compare_cluster); 457 | for (int i = 0; i < tr.size(); ++i) { 458 | if (tr[i].second > 10) { 459 | cluster_index.push_back(tr[i].first); 460 | } 461 | } 462 | 463 | return true; 464 | } 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | int main(int argc, char** argv) { 476 | 477 | pcl::PointCloud::Ptr cloud( 478 | new pcl::PointCloud); 479 | pcl::PointCloud::Ptr cloud_ng( 480 | new pcl::PointCloud); 481 | pcl::PointCloud::Ptr cloud_g( 482 | new pcl::PointCloud); 483 | 484 | pcl::PointCloud::Ptr cloud_new( 485 | new pcl::PointCloud); 486 | 487 | ifstream inputfile; 488 | inputfile.open(argv[1], ios::binary); 489 | if (!inputfile) { 490 | cerr << "Open input file error!" << endl; 491 | exit(-1); 492 | } 493 | 494 | inputfile.seekg(0, ios::beg); 495 | 496 | for (int i = 0; inputfile.good() && !inputfile.eof(); i++) { 497 | pcl::PointXYZI p; 498 | inputfile.read((char *) &p.x, 3 * sizeof(float)); 499 | inputfile.read((char *) &p.intensity, sizeof(float)); 500 | cloud->points.push_back(p); 501 | } 502 | 503 | cloud->height = 1; 504 | cloud->width = cloud->points.size(); 505 | 506 | double ringnum=(cloud->points.size()/32)-1; 507 | unordered_map range_image; 508 | Transform2RangeImage(*cloud, *cloud_ng, *cloud_g, range_image); 509 | 510 | 511 | cloud_ng->height = 1; 512 | cloud_ng->width = cloud_ng->points.size(); 513 | cloud_ng->is_dense = false; 514 | 515 | cloud_g->height = 1; 516 | cloud_g->width = cloud_ng->points.size(); 517 | cloud_g->is_dense = false; 518 | vector cluster_index = range_cluster(range_image,ringnum); 519 | cv::Mat bvimage = cv::Mat::zeros(32, total_frame, CV_8UC1); 520 | cv::Mat range_imagec = cv::Mat::zeros(32, total_frame, CV_8UC3); 521 | 522 | for (auto it = range_image.begin(); it != range_image.end(); ++it) { 523 | int index = it->second.count_num; 524 | //cout<<"final "<points.push_back(cloud_ng->points[index]); 526 | float anglepix = Polar_angle_cal(cloud_ng->points[index].x, 527 | cloud_ng->points[index].y); 528 | bvimage.at(it->second.ring_i, it->second.frame_j) = 529 | it->second.range_zxy / 30 * 256; 530 | 531 | } 532 | cv::resize(bvimage, bvimage, cv::Size(1000, 100)); 533 | cv::imwrite("range_image2.png", bvimage); 534 | 535 | 536 | boost::shared_ptr viewer( 537 | new pcl::visualization::PCLVisualizer("pcd")); //PCLVisualizer 可视化类 538 | viewer->setBackgroundColor(0.8, 0.8, 0.8); 539 | viewer->addCoordinateSystem(1); 540 | //pcl::visualization::PointCloudColorHandlerCustom color(cloud_g, 0,0, 0); 542 | //viewer->addPointCloud(cloud_g, color, "cloud"); 543 | vector cluster_id; 544 | most_frequent_value(cluster_index, cluster_id); 545 | cv::RNG rng(12345); 546 | 547 | for (int k = 0; k < cluster_id.size(); ++k) { 548 | pcl::PointCloud::Ptr Colorcloud2( 549 | new pcl::PointCloud); 550 | vector hsv(3); 551 | hsv[0] = float(k) / float(cluster_id.size()); 552 | hsv[1] = 1; 553 | hsv[2] = 1; 554 | 555 | int r = rng.uniform(0, 255); 556 | int g = rng.uniform(0, 255); 557 | int b = rng.uniform(0, 255); 558 | vector rgb = hsv2rgb(hsv); 559 | for (int i = 0; i < total_frame; ++i) { 560 | for (int j = 0; j < 32; ++j) { 561 | if (cluster_index[i * 32 + j] == cluster_id[k] 562 | && cluster_id[k] != -1) { 563 | unordered_map::iterator it_find; 564 | it_find = range_image.find(i * 32 + j); 565 | if (it_find != range_image.end()) { 566 | pcl::PointXYZRGB p; 567 | p.x = cloud_ng->points[it_find->second.count_num].x; 568 | p.y = cloud_ng->points[it_find->second.count_num].y; 569 | p.z = cloud_ng->points[it_find->second.count_num].z; 570 | p.r = 255; 571 | p.g = 0; 572 | p.b = 0; 573 | Colorcloud2->points.push_back(p); 574 | range_imagec.at(it_find->second.ring_i, it_find->second.frame_j) = cv::Vec3b(r,g,b); 575 | } 576 | } 577 | } 578 | } 579 | 580 | if (Colorcloud2->points.size() > 5) { 581 | Colorcloud2->height = 1; 582 | Colorcloud2->width = Colorcloud2->points.size(); 583 | float xmax1 = Colorcloud2->points[0].x; 584 | float xmin1 = Colorcloud2->points[0].x; 585 | float ymax1 = Colorcloud2->points[0].y; 586 | float ymin1 = Colorcloud2->points[0].y; 587 | float zmax1 = Colorcloud2->points[0].z; 588 | float zmin1 = Colorcloud2->points[0].z; 589 | //find the xmax, xmin, ymax, ymin 590 | for (int i = 0; i < Colorcloud2->points.size(); ++i) { 591 | if (Colorcloud2->points[i].z > zmax1) { 592 | zmax1 = Colorcloud2->points[i].z; 593 | } 594 | if (Colorcloud2->points[i].z < zmin1) { 595 | zmin1 = Colorcloud2->points[i].z; 596 | } 597 | if (Colorcloud2->points[i].x > xmax1) { 598 | xmax1 = Colorcloud2->points[i].x; 599 | } 600 | if (Colorcloud2->points[i].x < xmin1) { 601 | xmin1 = Colorcloud2->points[i].x; 602 | } 603 | if (Colorcloud2->points[i].y > ymax1) { 604 | ymax1 = Colorcloud2->points[i].y; 605 | } 606 | if (Colorcloud2->points[i].y < ymin1) { 607 | ymin1 = Colorcloud2->points[i].y; 608 | } 609 | 610 | } 611 | 612 | if (zmin1 < 0) 613 | zmin1 = 0; //make sure object is up the ground 614 | 615 | double depth = zmax1 - zmin1; 616 | 617 | Eigen::Vector3f eulerAngle(0.0, 0.0, 0.0); 618 | Eigen::AngleAxisf rollAngle( 619 | Eigen::AngleAxisf(eulerAngle(2), Eigen::Vector3f::UnitX())); 620 | Eigen::AngleAxisf pitchAngle( 621 | Eigen::AngleAxisf(eulerAngle(1), Eigen::Vector3f::UnitY())); 622 | Eigen::AngleAxisf yawAngle( 623 | Eigen::AngleAxisf(eulerAngle(0), Eigen::Vector3f::UnitZ())); 624 | 625 | const Eigen::Quaternionf bboxQ1(yawAngle * pitchAngle * rollAngle); 626 | 627 | Eigen::Vector3f translation; 628 | translation(0) = (xmax1 - xmin1) / 2 + xmin1; 629 | translation(1) = (ymax1 - ymin1) / 2 + ymin1; 630 | translation(2) = (zmax1 - zmin1) / 2 + zmin1; 631 | double length = ymax1 - ymin1; 632 | double width = xmax1 - xmin1; 633 | 634 | pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZRGB 635 | > color2(Colorcloud2, (b), (g), 636 | (r)); 637 | viewer->addPointCloud(Colorcloud2, color2, "cloud2" + toString(k)); 638 | } 639 | } 640 | 641 | cout << range_image.size() << endl; 642 | 643 | int count_num = 0; 644 | for (auto it = range_image.begin(); it != range_image.end(); ++it) { 645 | count_num++; 646 | } 647 | 648 | 649 | //cv::imshow("bv", range_imagec); 650 | //cv::waitKey(0); 651 | cv::resize(range_imagec, range_imagec, cv::Size(1000, 100)); 652 | cv::imwrite("range_image3.png", range_imagec); 653 | while (!viewer->wasStopped()) { 654 | viewer->spin(); 655 | } 656 | 657 | 658 | return 0; 659 | } 660 | 661 | 662 | 663 | -------------------------------------------------------------------------------- /test.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wangx1996/Range-Image-based-segementation/be0289b2b59e204239dc0742c1b5ae56e6bfb80b/test.bin --------------------------------------------------------------------------------