multiple changes.

Author: lnexenl

.gitignore

@@ -15,4 +15,5 @@ result_gps.txt
 camera_camera_calib.yaml
 camera_chessboard_data.dat
 *.log
-.idea/
+.idea/
+**/result

README.md

@@ -1,11 +1,48 @@
-# VINS-Fusion
-## An optimization-based multi-sensor state estimator
+# Stereo-VPCCD
+
+## An Pointcloud Change Detection pipeline based on VINS-Fusion
+
+## 1.Prerequisites
+
+You can use any Linux distros as you like, most of the prerequisites are same with VINS-Fusion.
+
+### 1.1 **Linux** and **ROS**
+
+Ubuntu 20.04 recommended, cause this project switch the OpenCV major version from 3 to 4, and
+ROS Noetic is the first ROS 1 distro integrated with OpenCV 4.
+
+### 1.2 Ceres Solver
+
+If you use Ubuntu 20.04, simply do:`sudo apt install libceres* -y`.
+Manually installation please follow [Ceres Installation](http://ceres-solver.org/installation.html).
+
+### 1.3 PointCloud Library(PCL)
+
+I think pcl version 1.10 will be a good choice. If you are using Ubuntu 20.04, `sudo apt install libpcl-dev -y`
+will do that. Else follow [PCL](https://github.com/PointCloudLibrary/pcl). Version lower than 1.10 may works, but v1.12
+never works.
+
+**MAKE SURE YOU INSTALL RIGHT VERSION BEFORE NEXT STEP.**
+
+Cause FAST-GICP will run into bug if you use a higher version of pcl.
+
+### 1.4 FAST-GICP
+
+Follow [fast_gicp](https://github.com/SMRT-AIST/fast_gicp), follow the installation with ros.
+
+
+____
 
 <img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/vins_logo.png" width = 55% height = 55% div align=left />
 <img src="https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/blob/master/support_files/image/kitti.png" width = 34% height = 34% div align=center />
 
-VINS-Fusion is an optimization-based multi-sensor state estimator, which achieves accurate self-localization for autonomous applications (drones, cars, and AR/VR). VINS-Fusion is an extension of [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono), which supports multiple visual-inertial sensor types (mono camera + IMU, stereo cameras + IMU, even stereo cameras only). We also show a toy example of fusing VINS with GPS. 
+VINS-Fusion is an optimization-based multi-sensor state estimator, which achieves accurate self-localization for
+autonomous applications (drones, cars, and AR/VR). VINS-Fusion is an extension
+of [VINS-Mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono), which supports multiple visual-inertial sensor
+types (mono camera + IMU, stereo cameras + IMU, even stereo cameras only). We also show a toy example of fusing VINS
+with GPS.
 **Features:**
+
 - multiple sensors support (stereo cameras / mono camera+IMU / stereo cameras+IMU)
 - online spatial calibration (transformation between camera and IMU)
 - online temporal calibration (time offset between camera and IMU)

config/stereo_450p.yaml

@@ -48,7 +48,7 @@ validate_frames: 3
 multiple_thread: 1
 
 #feature traker paprameters
-max_cnt: 150            # max feature number in feature tracking
+max_cnt: 100            # max feature number in feature tracking
 min_dist: 25            # min distance between two features
 freq: 0                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image
 F_threshold: 1.0        # ransac threshold (pixel)

prior_fusion

@@ -1 +1 @@
-Subproject commit 15eacdbe1aaf10d85481687c844e11615956b9b9
+Subproject commit 0f8bd7722d1c3f89f6e5954420036cbb993c8750

prior_locate

@@ -1,16 +1,43 @@
 import rospy
 import rospy_message_converter.json_message_converter as jmc
 from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped
 import json
 
+ndt_path = Path()
 
-def path_cb(msg: Path):
+
+# def ndt_cb(msg: PoseWithCovarianceStamped):
+#     tmp_pose = PoseStamped()
+#     tmp_pose.header = msg.header
+#     tmp_pose.pose = msg.pose.pose
+#     ndt_path.poses.append(tmp_pose)
+#     ndt_path.header = msg.header
+#     json_msg = jmc.convert_ros_message_to_json(ndt_path)
+#     with open('/home/lnex/ndt_path.json', 'w') as file:
+#         file.write(json_msg)
+
+def gt_cb(msg: Path):
+    json_msg = jmc.convert_ros_message_to_json(msg)
+    with open('/home/lnex/path/gt.json', 'w') as file:
+        file.write(json_msg)
+
+
+def vins_cb(msg: Path):
+    json_msg = jmc.convert_ros_message_to_json(msg)
+    with open('/home/lnex/path/stamped_traj_estimate.json', 'w') as file:
+        file.write(json_msg)
+
+
+def global_cb(msg: Path):
     json_msg = jmc.convert_ros_message_to_json(msg)
-    with open('/home/lnex/path.json', 'w') as file:
+    with open('/home/lnex/path/stamped_prior_estimate.json', 'w') as file:
         file.write(json_msg)
 
 
 if __name__ == '__main__':
     rospy.init_node("asdf", anonymous=True)
-    rospy.Subscriber("/ndt_path", Path, callback=path_cb, queue_size=1)
+    # rospy.Subscriber("/vins_estimator/gt_path", Path, callback=gt_cb, queue_size=1)
+    rospy.Subscriber("/vins_estimator/path", Path, callback=vins_cb, queue_size=1)
+    rospy.Subscriber("/priorOptNode/global_path", Path, callback=global_cb, queue_size=1)
     rospy.spin()

utils/record_gicp_path.py

@@ -0,0 +1,12 @@
+<launch>
+    <node name="vins_estimator" pkg="vins" type="vins_node" output="log" args="$(find vins)/../config/stereo_450p.yaml"/>
+     <node name="priorOptNode" pkg="prior_fusion" type="prior_fusion_node">
+        <param name="resultDir" type="str" value="$(find vins)/../result/1"/>
+    </node>
+    <node name="depth_pred_node" pkg="prior_locate" type="prior_locate_node" launch-prefix="gdb -ex run --args">
+        <param name="priorMapFile" type="str" value="/home/lnex/pcd/prior/4x.pcd"/>
+        <param name="resultDir" type="str" value="$(find vins)/../result/1"/>
+        <param name="confFile" type="str" value="$(find vins)/../config/stereo_450p.yaml"/>
+    </node>
+<!--     <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins)/../config/vins_rviz_config.rviz" /> -->
+</launch>