Merge pull request #2 from sensyn-robotics/add-missing-dep

Add missing dep

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")

apps/hdl_graph_slam_nodelet.cpp

@@ -328,14 +328,15 @@ 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;
       }
       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

include/hdl_graph_slam/keyframe.hpp

@@ -42,6 +42,7 @@ struct KeyFrame {
   pcl::PointCloud<PointT>::ConstPtr cloud;        // point cloud
   boost::optional<Eigen::Vector4d> floor_coeffs;  // detected floor's coefficients
   boost::optional<Eigen::Vector3d> utm_coord;     // UTM coord obtained by GPS
+  boost::optional<Eigen::Vector3d> lla_coord;     // UTM coord obtained by GPS
 
   boost::optional<Eigen::Vector3d> acceleration;    //
   boost::optional<Eigen::Quaterniond> orientation;  //

package.xml

@@ -1,16 +1,16 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>hdl_graph_slam</name>
-  <version>0.0.0</version>
+  <version>0.0.1</version>
   <description>The hdl_graph_slam package</description>
 
   <maintainer email="[email protected]">koide</maintainer>
 
   <license>BSD</license>
 
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>libg2o</build_depend>
   <build_depend>message_generation</build_depend>
+  <depend>libg2o</depend>
   <depend>eigen</depend>
   <depend>fast_gicp</depend>
   <depend>geodesy</depend>
@@ -25,7 +25,7 @@
   <depend>std_msgs</depend>
   <depend>tf_conversions</depend>
   <exec_depend>message_runtime</exec_depend>
-  <exec_depend>msf_updates</exec_depend>
+  <!-- <exec_depend>msf_updates</exec_depend> -->
   <exec_depend>nodelet</exec_depend>
   <exec_depend>tf</exec_depend>
 

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";
   }