Merge pull request #96 from koide3/devel

Update of the first node anchor mechanism

.travis.yml

@@ -9,8 +9,12 @@ matrix:
   include:
     - name: "Xenial kinetic"
       env: ROS_DISTRO=kinetic
+    - name: "Xenial kinetic_llvm"
+      env: ROS_DISTRO=kinetic_llvm
     - name: "Bionic melodic"
       env: ROS_DISTRO=melodic
+    - name: "Bionic melodic_llvm"
+      env: ROS_DISTRO=melodic_llvm
 
 script:
   - docker build -f ./docker/$ROS_DISTRO/Dockerfile .

apps/hdl_graph_slam_nodelet.cpp

@@ -199,9 +199,17 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
       // fix the first node
       if(keyframes.empty() && new_keyframes.size() == 1) {
         if(private_nh.param<bool>("fix_first_node", false)) {
+          Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
+          std::stringstream sst(private_nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));
+          for(int i = 0; i < 6; i++) {
+            double stddev = 1.0;
+            sst >> stddev;
+            inf(i, i) = 1.0 / stddev;
+          }
+
           anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
           anchor_node->setFixed(true);
-          anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 6));
+          anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);
         }
       }
 
@@ -568,6 +576,13 @@ class HdlGraphSlamNodelet : public nodelet::Nodelet {
     std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));
     new_keyframes.clear();
 
+    // move the first node anchor position to the current estimate of the first node pose
+    // so the first node moves freely while trying to stay around the origin
+    if(anchor_node && private_nh.param<bool>("fix_first_node_adaptive", true)) {
+      Eigen::Isometry3d anchor_target = static_cast<g2o::VertexSE3*>(anchor_edge->vertices()[1])->estimate();
+      anchor_node->setEstimate(anchor_target);
+    }
+
     // optimize the pose graph
     int num_iterations = private_nh.param<int>("g2o_solver_num_iterations", 1024);
     graph_slam->optimize(num_iterations);

docker/kinetic_llvm/Dockerfile

@@ -0,0 +1,29 @@
+FROM ros:kinetic
+
+RUN apt-get update && apt-get install --no-install-recommends -y \
+ && apt-get install --no-install-recommends -y \
+ wget nano build-essential libomp-dev clang lld-6.0 \
+ ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs \
+ ros-kinetic-rviz ros-kinetic-tf-conversions ros-kinetic-libg2o \
+ && apt-get clean \
+ && rm -rf /var/lib/apt/lists/*
+
+RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-6.0 50
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+# RUN git clone https://github.com/koide3/hdl_graph_slam.git
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; CC=clang CXX=clang++ catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+RUN apt-get clean && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/melodic_llvm/Dockerfile

@@ -0,0 +1,28 @@
+FROM ros:melodic
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+ && apt-get install -y --no-install-recommends \
+ wget nano build-essential libomp-dev clang lld\
+ ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \
+ ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \
+ && apt-get clean \
+ && rm -rf /var/lib/apt/lists/*
+
+RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+# RUN git clone https://github.com/koide3/hdl_graph_slam.git && cd hdl_graph_slam && git checkout arch-melodic
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

launch/hdl_graph_slam.launch

@@ -79,13 +79,18 @@
     <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
     <param name="g2o_solver_type" value="lm_var_cholmod" />
     <param name="g2o_solver_num_iterations" value="512" />
-    <!-- keyframe registration params -->
+    <!-- constraint switches -->
     <param name="enable_gps" value="$(arg enable_gps)" />
     <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
     <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
     <param name="max_keyframes_per_update" value="10" />
     <param name="keyframe_delta_trans" value="2.0" />
     <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
     <!-- loop closure params -->
     <param name="distance_thresh" value="10.0" />
     <param name="accum_distance_thresh" value="15.0" />

launch/hdl_graph_slam_400.launch

@@ -74,13 +74,18 @@
     <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
     <param name="g2o_solver_type" value="lm_var_cholmod" />
     <param name="g2o_solver_num_iterations" value="512" />
-    <!-- keyframe registration params -->
+    <!-- constraint switches -->
     <param name="enable_gps" value="$(arg enable_gps)" />
     <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
     <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
     <param name="max_keyframes_per_update" value="10" />
     <param name="keyframe_delta_trans" value="2.0" />
     <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
     <!-- loop closure params -->
     <param name="distance_thresh" value="15.0" />
     <param name="accum_distance_thresh" value="25.0" />

launch/hdl_graph_slam_501.launch

@@ -74,13 +74,18 @@
     <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
     <param name="g2o_solver_type" value="lm_var_cholmod" />
     <param name="g2o_solver_num_iterations" value="512" />
-    <!-- keyframe registration params -->
+    <!-- constraint switches -->
     <param name="enable_gps" value="$(arg enable_gps)" />
     <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
     <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
     <param name="max_keyframes_per_update" value="10" />
     <param name="keyframe_delta_trans" value="1.0" />
     <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
     <!-- loop closure params -->
     <param name="distance_thresh" value="1.0" />
     <param name="accum_distance_thresh" value="3.0" />