rename project, add launch files

Author: Shuo

.devcontainer/Dockerfile

@@ -0,0 +1,41 @@
+FROM ros:melodic-perception
+
+ENV CERES_VERSION="1.14.0"
+ENV CATKIN_WS=/root/vins_ws
+
+      # set up thread number for building
+RUN   if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; \
+      else export USE_PROC=$(($(nproc)/2)) ; fi && \
+      apt-get update && apt-get install -y \
+      cmake \
+      libatlas-base-dev \
+      libeigen3-dev \
+      libgoogle-glog-dev \
+      libsuitesparse-dev \
+      python-catkin-tools \
+      ros-${ROS_DISTRO}-cv-bridge \
+      ros-${ROS_DISTRO}-image-transport \
+      ros-${ROS_DISTRO}-message-filters \
+      ros-${ROS_DISTRO}-tf && \
+      rm -rf /var/lib/apt/lists/* && \
+      # Build and install Ceres
+      git clone https://ceres-solver.googlesource.com/ceres-solver && \
+      cd ceres-solver && \
+      git checkout tags/${CERES_VERSION} && \
+      mkdir build && cd build && \
+      cmake .. && \
+      make -j$(USE_PROC) install && \
+      rm -rf ../../ceres-solver && \
+      mkdir -p $CATKIN_WS/src/VINS-Fusion/
+
+# Add VINS-Fusion
+WORKDIR $CATKIN_WS/src
+# use the following line if you only have this dockerfile
+RUN git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
+
+#TODO: add visualizer
+
+# Build VINS-Fusion
+WORKDIR $CATKIN_WS
+RUN /bin/bash -c "source /opt/ros/${ROS_DISTRO}/setup.bash; catkin build; source ${CATKIN_WS}/devel/setup.bash; catkin build"
+RUN echo "source ${CATKIN_WS}/devel/setup.bash" >> /root/.bashrc

.devcontainer/devcontainer.json

@@ -0,0 +1,27 @@
+// For format details, see https://aka.ms/devcontainer.json. For config options, see the README at:
+// https://github.com/microsoft/vscode-dev-containers/tree/v0.238.0/containers/ubuntu
+{
+	"name": "Ubuntu",
+	"build": {
+		"dockerfile": "Dockerfile",
+		// Update 'VARIANT' to pick an Ubuntu version: jammy / ubuntu-22.04, focal / ubuntu-20.04, bionic /ubuntu-18.04
+		// Use ubuntu-22.04 or ubuntu-18.04 on local arm64/Apple Silicon.
+		"args": { "-t": "a1_vins_image"}
+	},
+	
+	// use this to connect to odrive without --privileged
+	"runArgs": ["--network=host", "--cap-add=IPC_LOCK", 
+		        "--cap-add=sys_nice","--device=/dev/input",
+				"--device=/dev/bus",  "--volume=/dev/serial:/dev/serial:ro",
+		        "--name=a1_vins_docker"],
+	"workspaceMount": "source=${localWorkspaceFolder},target=/root/vins_ws/src/vilo,type=bind",
+	"workspaceFolder": "/root/vins_ws/src/vilo"
+	// Use 'forwardPorts' to make a list of ports inside the container available locally.
+	// "forwardPorts": [],
+
+	// Use 'postCreateCommand' to run commands after the container is created.
+	// "postCreateCommand": "uname -a",
+
+	// Comment out to connect as root instead. More info: https://aka.ms/vscode-remote/containers/non-root.
+	// "remoteUser": "vscode"
+}

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.0)
-project(vileom)
+project(vilo)
 
 set(CMAKE_BUILD_TYPE "Release")
 #set(CMAKE_BUILD_TYPE "Debug")
@@ -38,7 +38,7 @@ include_directories(
 
 catkin_package()
 
-add_library(vileom_lib
+add_library(vilo_lib
     src/estimator/estimator.cpp
     src/featureTracker/feature_tracker.cpp
     src/featureTracker/feature_manager.cpp
@@ -61,11 +61,11 @@ add_library(vileom_lib
     src/initial/initial_sfm.cpp
     src/initial/initial_ex_rotation.cpp
         src/factor/imu_leg_factor.cpp src/factor/imu_leg_factor.h src/factor/imu_leg_integration_base.cpp src/factor/imu_leg_integration_base.h)
-target_link_libraries(vileom_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
+target_link_libraries(vilo_lib ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
 
 # this is actually the main function, not just the test...
-add_executable(vileom_feature_track_test src/feature_track_test.cpp )
-target_link_libraries(vileom_feature_track_test vileom_lib)
+add_executable(vilo_feature_track_test src/feature_track_test.cpp )
+target_link_libraries(vilo_feature_track_test vilo_lib)
 
 
 add_executable(ceres_test src/test/ceres_test.cpp src/legKinematics/A1Kinematics.cpp)

README.md

@@ -1,4 +1,4 @@
-This ROS package contains a visual-inertial-leg odometry (VILO) for Unitree A1 robot. The package name is currently called vileom, which is an obsolete name, which will be revised in later versions. 
+This ROS package contains a visual-inertial-leg odometry (VILO) for Unitree A1 robot. 
 
 Our goal is to achieve low-cost minimum sensing on legged robots (sensing suites only has one IMU, one stereo camera, leg joint sensors. Total cost <$1000).
 
@@ -10,24 +10,11 @@ Here are two videos comparing the performance of the VILO and the VINS-Fusion:
 [![video2](https://img.youtube.com/vi/oNwwQ0l-O4U/0.jpg)](https://www.youtube.com/watch?v=oNwwQ0l-O4U)
 
 
-The package is only tested in Ubuntu 18.04 and ROS melodic. 
 
 ## Installation
-Please follow VINS-Fusion's installation guide. 
+use VScode docker container 
 
 
-This package must also working together with 
-
-https://github.com/paulyang1990/A1_visualizer
-
-and
-
-https://github.com/paulyang1990/a1_launch
-
-The A1_visualizer has its own dependencies. A1_launch is just a collection of launch files. Download these two packages (and their dependencies) and compile them together with the VILO (vileom).
-
-The A1_launch contains scripts to run the VILO. And the VILO uses A1_visualizer to visualize the estimation results. 
-
 ## Dataset
 A Google drive folder https://drive.google.com/drive/folders/13GsFDaBkDrslOl9BfE4AJnOn3ECDXVnc
 

config/hardware_a1_vilo_config.yaml

@@ -0,0 +1,95 @@
+%YAML:1.0
+
+#common parameters
+#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
+imu: 1         
+num_of_cam: 2  
+
+imu_topic: "/hardware_a1/imu"
+image0_topic: "/camera_forward/infra1/image_rect_raw"
+image1_topic: "/camera_forward/infra2/image_rect_raw"
+output_path: "/home/shuoy/nv_work/unitree_ws/src/tightly-coupled-visual-inertial-leg-odometry/output"
+
+cam0_calib: "left.yaml"
+cam1_calib: "right.yaml"
+image_width: 640
+image_height: 480
+
+# leg related
+use_leg_odom: 1
+num_of_leg: 4
+leg_topic: "/hardware_a1/joint_foot"
+optimize_leg_bias: 1
+joint_angle_n: 0.00001
+joint_velocity_n: 0.00001
+leg_bias_xy_n: 0.5
+leg_bias_z_n:  0.5
+# contact model
+v_n_force_thres_ratio: 0.8
+v_n_min: 0.0001
+v_n_max: 500.0
+# v_n_w1: 0.333
+# v_n_w2: 0.333
+# v_n_w3: 0.333
+v_n_term1_steep: 10
+v_n_term2_var_rescale: 1.0e-6
+v_n_term3_distance_rescale: 1.0e-3
+v_n_final_ratio: 0.1
+
+# leg kinematics parameter
+lower_leg_length: 0.21
+
+
+# Extrinsic parameter between IMU and Camera.
+estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
+                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
+
+body_T_cam0: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 0,  0,  1, 0.2693, 
+          -1,  0,  0, 0.025, 
+           0, -1,  0, 0.067, 
+           0., 0., 0., 1. ]
+
+body_T_cam1: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 0,  0,  1, 0.2693, 
+          -1,  0,  0, -0.025, 
+           0, -1,  0, 0.067, 
+           0., 0., 0., 1. ]
+
+#Multiple thread support
+multiple_thread: 1
+
+#feature traker paprameters
+max_cnt: 90            # max feature number in feature tracking
+min_dist: 20           # min distance between two features 
+freq: 15                # 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)
+show_track: 1           # publish tracking image as topic
+flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
+
+#optimization parameters
+max_solver_time: 0.1  # max solver itration time (ms), to guarantee real time
+max_num_iterations: 16   # max solver itrations, to guarantee real time
+keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
+
+#imu parameters       The more accurate parameters you provide, the better performance
+acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
+gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05  0.004
+acc_w: 0.004         # accelerometer bias random work noise standard deviation.  #0.002
+gyr_w: 0.002       # gyroscope bias random work noise standard deviation.     #4.0e-5
+g_norm: 9.805         # gravity magnitude
+
+#unsynchronization parameters
+estimate_td: 1                      # online estimate time offset between camera and imu
+td: 0.00240                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
+
+#loop closure parameters
+load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
+pose_graph_save_path: "/home/biorobotics/output/pose_graph/" # save and load path
+save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

config/vins_fusion_real_sense_xsens.yaml config/hardware_a1_vins_config.yaml

@@ -5,19 +5,16 @@
 imu: 1         
 num_of_cam: 2  
 
-imu_topic: "/imu/data"
+imu_topic: "/hardware_a1/imu"
 image0_topic: "/camera_forward/infra1/image_rect_raw"
 image1_topic: "/camera_forward/infra2/image_rect_raw"
-output_path: "/home/shuoy/output"
+output_path: "/home/shuoy/nv_work/unitree_ws/src/tightly-coupled-visual-inertial-leg-odometry/output"
 
 cam0_calib: "left.yaml"
 cam1_calib: "right.yaml"
 image_width: 640
 image_height: 480
-
-use_leg_odom: 0
-num_of_leg: 4
-leg_topic: "/a1/joint_foot"
+   
 
 # Extrinsic parameter between IMU and Camera.
 estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
@@ -27,46 +24,46 @@ body_T_cam0: !!opencv-matrix
    rows: 4
    cols: 4
    dt: d
-   data: [ 0,  0,  1, 0.04, 
-          -1,  0,  0, 0.02, 
-           0, -1,  0, 0.0, 
+   data: [ 0,  0,  1, 0.2693, 
+          -1,  0,  0, 0.025, 
+           0, -1,  0, 0.067, 
            0., 0., 0., 1. ]
 
 body_T_cam1: !!opencv-matrix
    rows: 4
    cols: 4
    dt: d
-   data: [ 0,  0,  1, 0.04, 
-          -1,  0,  0, -0.03, 
-           0, -1,  0, 0.0, 
+   data: [ 0,  0,  1, 0.2693, 
+          -1,  0,  0, -0.025, 
+           0, -1,  0, 0.067, 
            0., 0., 0., 1. ]
 
 #Multiple thread support
-multiple_thread: 0
+multiple_thread: 1
 
 #feature traker paprameters
-max_cnt: 150            # max feature number in feature tracking
-min_dist: 30            # min distance between two features 
+max_cnt: 90            # max feature number in feature tracking
+min_dist: 20           # min distance between two features 
 freq: 15                # 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)
-show_track: 0           # publish tracking image as topic
+show_track: 1           # publish tracking image as topic
 flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
 
 #optimization parameters
-max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
-max_num_iterations: 8   # max solver itrations, to guarantee real time
+max_solver_time: 0.1  # max solver itration time (ms), to guarantee real time
+max_num_iterations: 16   # max solver itrations, to guarantee real time
 keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
 
 #imu parameters       The more accurate parameters you provide, the better performance
 acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
-gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
-acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
-gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
+gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05  0.004
+acc_w: 0.004         # accelerometer bias random work noise standard deviation.  #0.002
+gyr_w: 0.002       # gyroscope bias random work noise standard deviation.     #4.0e-5
 g_norm: 9.805         # gravity magnitude
 
 #unsynchronization parameters
 estimate_td: 1                      # online estimate time offset between camera and imu
-td: -0.00454                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
+td: 0.00240                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
 
 #loop closure parameters
 load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'