From 2508dc94d38a6c686c7936d30cee3813da9004c8 Mon Sep 17 00:00:00 2001 From: ohtt Date: Fri, 25 Aug 2023 17:57:22 +0900 Subject: [PATCH 1/5] add lla attribute --- apps/hdl_graph_slam_nodelet.cpp | 5 ++--- include/hdl_graph_slam/keyframe.hpp | 1 + src/hdl_graph_slam/keyframe.cpp | 6 ++++++ 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/apps/hdl_graph_slam_nodelet.cpp b/apps/hdl_graph_slam_nodelet.cpp index cc791e76..0950873e 100644 --- a/apps/hdl_graph_slam_nodelet.cpp +++ b/apps/hdl_graph_slam_nodelet.cpp @@ -328,7 +328,7 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet { geodesy::UTMPoint utm; geodesy::fromMsg((*closest_gps)->position, utm); Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude); - + Eigen::Vector3d lla((*closest_gps)->position.latitude, (*closest_gps)->position.longitude, (*closest_gps)->position.altitude); // the first gps data position will be the origin of the map if(!zero_utm) { zero_utm = xyz; @@ -336,6 +336,7 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet { xyz -= (*zero_utm); keyframe->utm_coord = xyz; + keyframe->lla_coord = lla; g2o::OptimizableGraph::Edge* edge; if(std::isnan(xyz.z())) { @@ -808,7 +809,6 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet { return markers; } - /** * @brief load all data from a directory * @param req @@ -922,7 +922,6 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet { return true; } - /** * @brief dump all data to the current directory * @param req diff --git a/include/hdl_graph_slam/keyframe.hpp b/include/hdl_graph_slam/keyframe.hpp index 8714d436..27647275 100644 --- a/include/hdl_graph_slam/keyframe.hpp +++ b/include/hdl_graph_slam/keyframe.hpp @@ -42,6 +42,7 @@ struct KeyFrame { pcl::PointCloud::ConstPtr cloud; // point cloud boost::optional floor_coeffs; // detected floor's coefficients boost::optional utm_coord; // UTM coord obtained by GPS + boost::optional lla_coord; // UTM coord obtained by GPS boost::optional acceleration; // boost::optional orientation; // diff --git a/src/hdl_graph_slam/keyframe.cpp b/src/hdl_graph_slam/keyframe.cpp index 99edbaed..81a08926 100644 --- a/src/hdl_graph_slam/keyframe.cpp +++ b/src/hdl_graph_slam/keyframe.cpp @@ -26,6 +26,8 @@ void KeyFrame::save(const std::string& directory) { std::ofstream ofs(directory + "/data"); ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n"; + ofs << std::fixed << std::setprecision(16); + ofs << "estimate\n"; ofs << node->estimate().matrix() << "\n"; @@ -42,6 +44,10 @@ void KeyFrame::save(const std::string& directory) { ofs << "utm_coord " << utm_coord->transpose() << "\n"; } + if(lla_coord) { + ofs << "lla_coord " << lla_coord->transpose() << "\n"; + } + if(acceleration) { ofs << "acceleration " << acceleration->transpose() << "\n"; } From 8e095a0084b0401174c2a415a8ddb9fa09490bda Mon Sep 17 00:00:00 2001 From: Virgilio Gruppelaar Date: Thu, 17 Oct 2024 16:58:03 +0900 Subject: [PATCH 2/5] remove unavailable msf_updates dependency --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index c1e3ed69..a3e411f1 100644 --- a/package.xml +++ b/package.xml @@ -25,7 +25,7 @@ std_msgs tf_conversions message_runtime - msf_updates + nodelet tf From 42abd6074cdf3e0bd93503f5c88bd1f015af3bf0 Mon Sep 17 00:00:00 2001 From: Virgilio Gruppelaar Date: Wed, 27 Nov 2024 10:47:00 +0900 Subject: [PATCH 3/5] change g2o to normal depend --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index c1e3ed69..7ae625f8 100644 --- a/package.xml +++ b/package.xml @@ -9,8 +9,8 @@ BSD catkin - libg2o message_generation + libg2o eigen fast_gicp geodesy From 5e09fdff557f99b38e6fc659a158c80eb3b831c4 Mon Sep 17 00:00:00 2001 From: Virgilio Gruppelaar Date: Wed, 27 Nov 2024 10:49:00 +0900 Subject: [PATCH 4/5] bump version --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index a2ba7f1e..3596b221 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ hdl_graph_slam - 0.0.0 + 0.0.1 The hdl_graph_slam package koide From 45df375da1f76d9498d7c149e54e69978fe118b5 Mon Sep 17 00:00:00 2001 From: Virgilio Gruppelaar Date: Wed, 11 Dec 2024 10:08:02 +0900 Subject: [PATCH 5/5] also use c++ 14 in ARM --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 79e6312d..f8202e35 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ project(hdl_graph_slam) # Can we use C++17 in indigo? if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") - add_definitions(-std=c++11) - set(CMAKE_CXX_FLAGS "-std=c++11") + add_definitions(-std=c++14) + set(CMAKE_CXX_FLAGS "-std=c++14") else() add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")