From 6e9d0d6721f731a45f72efc2d44b0540d0863892 Mon Sep 17 00:00:00 2001 From: Parsley915 <18756283279@163.com> Date: Sat, 17 Aug 2024 13:10:26 +0800 Subject: [PATCH 1/2] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E4=B8=BA=E9=80=82?= =?UTF-8?q?=E7=94=A8=E4=BA=8EKITTI=E6=95=B0=E6=8D=AE=E9=9B=86=E7=9A=84?= =?UTF-8?q?=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- LeGO-LOAM/include/utility.h | 31 ++++++++++++------------------- 1 file changed, 12 insertions(+), 19 deletions(-) diff --git a/LeGO-LOAM/include/utility.h b/LeGO-LOAM/include/utility.h index 0c13827f..50c53ac1 100755 --- a/LeGO-LOAM/include/utility.h +++ b/LeGO-LOAM/include/utility.h @@ -60,12 +60,20 @@ extern const string fileDirectory = "/tmp/"; extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used // VLP-16 -extern const int N_SCAN = 16; +//extern const int N_SCAN = 16; +//extern const int Horizon_SCAN = 1800; +//extern const float ang_res_x = 0.2; +//extern const float ang_res_y = 2.0; +//extern const float ang_bottom = 15.0+0.1; +//extern const int groundScanInd = 7; + +//Vel 64 +extern const int N_SCAN = 64; extern const int Horizon_SCAN = 1800; extern const float ang_res_x = 0.2; -extern const float ang_res_y = 2.0; -extern const float ang_bottom = 15.0+0.1; -extern const int groundScanInd = 7; +extern const float ang_res_y = 0.427; +extern const float ang_bottom = 24.9; +extern const int groundScanInd = 50; // HDL-32E // extern const int N_SCAN = 32; @@ -147,22 +155,7 @@ struct by_value{ } }; -/* - * A point cloud type that has "ring" channel - */ -struct PointXYZIR -{ - PCL_ADD_POINT4D - PCL_ADD_INTENSITY; - uint16_t ring; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; -POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR, - (float, x, x) (float, y, y) - (float, z, z) (float, intensity, intensity) - (uint16_t, ring, ring) -) /* * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) From 73bb9ec2acc5db6d4a55522e6113f26a45181e89 Mon Sep 17 00:00:00 2001 From: Parsley915 <18756283279@163.com> Date: Sat, 17 Aug 2024 15:19:52 +0800 Subject: [PATCH 2/2] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BA=86=E4=BF=9D?= =?UTF-8?q?=E5=AD=98=E4=B8=BAtum=E6=A0=BC=E5=BC=8F=E6=96=87=E4=BB=B6?= =?UTF-8?q?=E7=9A=84=E7=9B=B8=E5=85=B3=E5=86=85=E5=AE=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- LeGO-LOAM/include/utility.h | 15 +++++++++++++++ LeGO-LOAM/src/mapOptmization.cpp | 26 ++++++++++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/LeGO-LOAM/include/utility.h b/LeGO-LOAM/include/utility.h index 50c53ac1..5507ce3e 100755 --- a/LeGO-LOAM/include/utility.h +++ b/LeGO-LOAM/include/utility.h @@ -155,7 +155,22 @@ struct by_value{ } }; +/* + * A point cloud type that has "ring" channel + */ +struct PointXYZIR +{ + PCL_ADD_POINT4D + PCL_ADD_INTENSITY; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; +POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR, + (float, x, x) (float, y, y) + (float, z, z) (float, intensity, intensity) + (uint16_t, ring, ring) +) /* * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) diff --git a/LeGO-LOAM/src/mapOptmization.cpp b/LeGO-LOAM/src/mapOptmization.cpp index 56088327..41851067 100755 --- a/LeGO-LOAM/src/mapOptmization.cpp +++ b/LeGO-LOAM/src/mapOptmization.cpp @@ -727,6 +727,32 @@ class mapOptimization{ rate.sleep(); publishGlobalMap(); } + + std::cout << "start save final point cloud" << std::endl; + std::cout << "======================================================" << std::endl; + + ofstream f; + f.open("/home/roxaneqian/kitti_lego_loam/src/kitti-lego-loam-master/LeGO-LOAM/LeGO-LOAM/getfromcode/traj/myRes.txt"); + f << fixed; + //std::cout << "traj roll" << cloudKeyPoses6D->points[0].roll << std::endl; + for(size_t i = 0;i < cloudKeyPoses3D->size();i++) + { + float cy = cos((cloudKeyPoses6D->points[i].yaw)*0.5); + float sy = sin((cloudKeyPoses6D->points[i].yaw)*0.5); + float cr = cos((cloudKeyPoses6D->points[i].roll)*0.5); + float sr = sin((cloudKeyPoses6D->points[i].roll)*0.5); + float cp = cos((cloudKeyPoses6D->points[i].pitch)*0.5); + float sp = sin((cloudKeyPoses6D->points[i].pitch)*0.5); + + float w = cy * cp * cr + sy * sp * sr; + float x = cy * cp * sr - sy * sp * cr; + float y = sy * cp * sr + cy * sp * cr; + float z = sy * cp * cr - cy * sp * sr; + //save the traj + f << setprecision(6) << cloudKeyPoses6D->points[i].time << " " << setprecision(9) << cloudKeyPoses6D->points[i].x << " " << cloudKeyPoses6D->points[i].y << " " << cloudKeyPoses6D->points[i].z << " " << x << " " << y << " " << z << " " << w << endl; + } + + f.close(); // save final point cloud pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS);