From 92371f86deb72a462a3dcfa1d1dd6bff21235903 Mon Sep 17 00:00:00 2001 From: jiseoonlim <74448629+jiseoonlim@users.noreply.github.com> Date: Tue, 8 Oct 2024 15:22:43 +0900 Subject: [PATCH 1/3] Update CMakeLists.txt --- LeGO-LOAM/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/LeGO-LOAM/CMakeLists.txt b/LeGO-LOAM/CMakeLists.txt index efebe5b3..64d1d869 100755 --- a/LeGO-LOAM/CMakeLists.txt +++ b/LeGO-LOAM/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(lego_loam) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3") find_package(catkin REQUIRED COMPONENTS tf @@ -57,4 +57,4 @@ add_executable(mapOptmization src/mapOptmization.cpp) target_link_libraries(mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam) add_executable(transformFusion src/transformFusion.cpp) -target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) \ No newline at end of file +target_link_libraries(transformFusion ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) From b10bad0077e37b17939a53a360ab84ee47e2d4fd Mon Sep 17 00:00:00 2001 From: jiseoonlim <74448629+jiseoonlim@users.noreply.github.com> Date: Tue, 8 Oct 2024 15:26:20 +0900 Subject: [PATCH 2/3] Update utility.h --- LeGO-LOAM/include/utility.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LeGO-LOAM/include/utility.h b/LeGO-LOAM/include/utility.h index 0c13827f..79634c02 100755 --- a/LeGO-LOAM/include/utility.h +++ b/LeGO-LOAM/include/utility.h @@ -10,7 +10,7 @@ #include "cloud_msgs/cloud_info.h" -#include +#include #include #include From 1b2c46830a19f82b8459fe6c7ae2b1d6ba4f9616 Mon Sep 17 00:00:00 2001 From: jiseoonlim <74448629+jiseoonlim@users.noreply.github.com> Date: Fri, 25 Oct 2024 01:02:49 +0900 Subject: [PATCH 3/3] Update mapOptmization.cpp saving map --- LeGO-LOAM/src/mapOptmization.cpp | 67 +++++++++++++++++++------------- 1 file changed, 39 insertions(+), 28 deletions(-) diff --git a/LeGO-LOAM/src/mapOptmization.cpp b/LeGO-LOAM/src/mapOptmization.cpp index 56088327..9d94c4e9 100755 --- a/LeGO-LOAM/src/mapOptmization.cpp +++ b/LeGO-LOAM/src/mapOptmization.cpp @@ -722,38 +722,49 @@ class mapOptimization{ } void visualizeGlobalMapThread(){ - ros::Rate rate(0.2); - while (ros::ok()){ - rate.sleep(); - publishGlobalMap(); - } - // save final point cloud - pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS); + ROS_INFO("Before............."); + + ros::Rate rate(0.2); + while (ros::ok()){ + rate.sleep(); + publishGlobalMap(); // 계속해서 맵을 퍼블리시 + } - string cornerMapString = "/tmp/cornerMap.pcd"; - string surfaceMapString = "/tmp/surfaceMap.pcd"; - string trajectoryString = "/tmp/trajectory.pcd"; + // 루프가 끝난 후 최종 맵 저장 + ROS_INFO("Saving final maps............."); - pcl::PointCloud::Ptr cornerMapCloud(new pcl::PointCloud()); - pcl::PointCloud::Ptr cornerMapCloudDS(new pcl::PointCloud()); - pcl::PointCloud::Ptr surfaceMapCloud(new pcl::PointCloud()); - pcl::PointCloud::Ptr surfaceMapCloudDS(new pcl::PointCloud()); - - for(int i = 0; i < cornerCloudKeyFrames.size(); i++) { - *cornerMapCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); - *surfaceMapCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); - *surfaceMapCloud += *transformPointCloud(outlierCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); - } + string cornerMapString = "/tmp/cornerMap.pcd"; + string surfaceMapString = "/tmp/surfaceMap.pcd"; + string trajectoryString = "/tmp/trajectory.pcd"; - downSizeFilterCorner.setInputCloud(cornerMapCloud); - downSizeFilterCorner.filter(*cornerMapCloudDS); - downSizeFilterSurf.setInputCloud(surfaceMapCloud); - downSizeFilterSurf.filter(*surfaceMapCloudDS); + pcl::PointCloud::Ptr cornerMapCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr cornerMapCloudDS(new pcl::PointCloud()); + pcl::PointCloud::Ptr surfaceMapCloud(new pcl::PointCloud()); + pcl::PointCloud::Ptr surfaceMapCloudDS(new pcl::PointCloud()); + + ROS_INFO("Before for loop............."); + + for(int i = 0; i < cornerCloudKeyFrames.size(); i++) { + *cornerMapCloud += *transformPointCloud(cornerCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + *surfaceMapCloud += *transformPointCloud(surfCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + *surfaceMapCloud += *transformPointCloud(outlierCloudKeyFrames[i], &cloudKeyPoses6D->points[i]); + } + + downSizeFilterCorner.setInputCloud(cornerMapCloud); + downSizeFilterCorner.filter(*cornerMapCloudDS); + downSizeFilterSurf.setInputCloud(surfaceMapCloud); + downSizeFilterSurf.filter(*surfaceMapCloudDS); + + pcl::io::savePCDFileASCII(fileDirectory+"cornerMap.pcd", *cornerMapCloudDS); + pcl::io::savePCDFileASCII(fileDirectory+"surfaceMap.pcd", *surfaceMapCloudDS); + pcl::io::savePCDFileASCII(fileDirectory+"trajectory.pcd", *cloudKeyPoses3D); + + // 최종 글로벌 맵 저장 + pcl::io::savePCDFileASCII(fileDirectory+"finalCloud.pcd", *globalMapKeyFramesDS); + + ROS_INFO("Finished saving final map."); + } - pcl::io::savePCDFileASCII(fileDirectory+"cornerMap.pcd", *cornerMapCloudDS); - pcl::io::savePCDFileASCII(fileDirectory+"surfaceMap.pcd", *surfaceMapCloudDS); - pcl::io::savePCDFileASCII(fileDirectory+"trajectory.pcd", *cloudKeyPoses3D); - } void publishGlobalMap(){