git push origin masterMerge branch 'borongyuan-msckf_vio_update'

CMakeLists.txt

@@ -777,8 +777,8 @@ IF(NOT MSVC)
         message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler if you want to use Qt6.")
       ENDIF()
     ENDIF()
-    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND ((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)))
-     #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
+    IF((NOT (${CMAKE_CXX_STANDARD} STREQUAL "17")) AND (msckf_vio_FOUND OR loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))
+     #MSCKF_VIO, LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
      include(CheckCXXCompilerFlag)
       CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
       IF(COMPILER_SUPPORTS_CXX14)
@@ -801,7 +801,6 @@ IF(NOT MSVC)
            ORB_SLAM_FOUND OR
            okvis_FOUND OR
            open_chisel_FOUND OR
-           msckf_vio_FOUND OR
            vins_FOUND OR
            ov_msckf_FOUND OR
            libpointmatcher_FOUND))

corelib/src/odometry/OdometryMSCKF.cpp

@@ -672,18 +672,17 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio
 		T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
 		T_i_w.translation() = imu_state.position;
 
-		Eigen::Isometry3d T_b_w = msckf_vio::IMUState::T_imu_body * T_i_w *
-				msckf_vio::IMUState::T_imu_body.inverse();
+		Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse();
 		Eigen::Vector3d body_velocity =
 				msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
 
 		// Publish tf
 		/*if (publish_tf) {
-	    tf::Transform T_b_w_tf;
-	    tf::transformEigenToTF(T_b_w, T_b_w_tf);
-	    tf_pub.sendTransform(tf::StampedTransform(
-	          T_b_w_tf, time, fixed_frame_id, child_frame_id));
-	  }*/
+			tf::Transform T_b_w_tf;
+			tf::transformEigenToTF(T_b_w, T_b_w_tf);
+			tf_pub.sendTransform(tf::StampedTransform(
+				T_b_w_tf, time, fixed_frame_id, child_frame_id));
+		}*/
 
 		// Publish the odometry
 		nav_msgs::Odometry odom_msg;
@@ -725,20 +724,18 @@ class MsckfVioNoROS: public msckf_vio::MsckfVio
 		// Publish the 3D positions of the features that
 		// has been initialized.
 		feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
-	  feature_msg_ptr->header.frame_id = fixed_frame_id;
-	  feature_msg_ptr->height = 1;
-	  for (const auto& item : map_server) {
-	    const auto& feature = item.second;
-	    if (feature.is_initialized) {
-	      Eigen::Vector3d feature_position =
-	        msckf_vio::IMUState::T_imu_body.linear() * feature.position;
-	      feature_msg_ptr->points.push_back(pcl::PointXYZ(
-	            feature_position(0), feature_position(1), feature_position(2)));
-	    }
-	  }
-	  feature_msg_ptr->width = feature_msg_ptr->points.size();
-
-	  //feature_pub.publish(feature_msg_ptr);
+		feature_msg_ptr->header.frame_id = fixed_frame_id;
+		feature_msg_ptr->height = 1;
+		for (const auto& item : map_server) {
+			const auto& feature = item.second;
+			if (feature.is_initialized) {
+				feature_msg_ptr->points.push_back(pcl::PointXYZ(
+					feature.position(0), feature.position(1), feature.position(2)));
+			}
+		}
+		feature_msg_ptr->width = feature_msg_ptr->points.size();
+
+		// feature_pub.publish(feature_msg_ptr);
 
 		return odom_msg;
 	}
@@ -755,7 +752,7 @@ OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
 imageProcessor_(0),
 msckf_(0),
 parameters_(parameters),
-fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
+fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0),
 previousPose_(Transform::getIdentity()),
 initGravity_(false)
 #endif

docker/jfr2018/Dockerfile

@@ -127,7 +127,8 @@ WORKDIR /root/catkin_ws/src
 RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace'
 RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout kinetic-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml
 RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout kinetic
-RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/a6d604e730bcbf3dcd7fd8a27302bed4bb94b799/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch
+# msckf: using a patched version to be used outside ros and c++14:
+RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout e69cdb68e9b31a438bbc51bb77cd34fe9b2e80cb
 RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout db2fc39451e59317cf8486d92085da1c8e414785
 RUN git clone https://github.com/ros-perception/vision_opencv.git && cd vision_opencv && git checkout kinetic
 RUN git clone https://github.com/laboshinl/loam_velodyne.git && cd loam_velodyne && git checkout a4c364a677647f2a35831439032dc5a58378b3fd

docker/jfr2018/latest/Dockerfile

@@ -137,8 +137,8 @@ RUN /bin/bash -c 'cd /root/catkin_ws;. /opt/ros/${ROS_DISTRO}/setup.bash; catkin
 RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout ${ROS_DISTRO}-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml
 # VISO2
 RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout ${ROS_DISTRO}
-# MSCKF-VIO
-RUN git clone https://github.com/KumarRobotics/msckf_vio.git && cd msckf_vio && git checkout a9386c5 && wget https://gist.githubusercontent.com/matlabbe/f2518d7427e7f6740af2110e540b1f2b/raw/1b7c96b38e063bacfa77408686e4528f3d27af2e/msckf_vio_a9386c5_ros_commented.patch && git apply --ignore-space-change --ignore-whitespace msckf_vio_a9386c5_ros_commented.patch
+# MSCKF-VIO: using a patched version to be used outside ros and c++14:
+RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout e69cdb68e9b31a438bbc51bb77cd34fe9b2e80cb
 # FOVIS
 RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout 896acc8425e9fd7c5609153b8bad349ae1abbb50
 # LOAM