Skip to content

Commit 8b04586

Browse files
MarkChenYutiancaomuqingandrewjong
authored
Fix MAC-VO Disparity Output (#274)
* Add disparity output * Replace dependency to private MAC-SLAM * update * Completely remove MACVO2 * Use UFM * Add disparity output * Replace dependency to private MAC-SLAM * update * Completely remove MACVO2 * Use UFM * Remove old macvo repo * Rename submodule to reflect the repo name * Reduce submodule nesting * Update MAC-VO * Add TensorRT support * Update scaling of robots to use replicas instead, much cleaner. Can now prepend NUM_ROBOTS=N and Update docs about airstack cli * Add AUTOLAUNCH option to airstack up * Only launch test under test profile. so airstack --profile test up * Fix missing semicolon for isaac launch * Reorder variable * Make sure profile for test overrides * Remove haleqiu/MAC-SLAM * Move to MAC-SLAM-Stable --------- Co-authored-by: caomuqing <caomuqing@163.com> Co-authored-by: Andrew Jong <ajong@andrew.cmu.edu>
1 parent 7df6126 commit 8b04586

37 files changed

Lines changed: 867 additions & 519 deletions

‎.env‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@ PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
1616

1717
# ================= Common ===================
1818
AUTOLAUNCH="true" # If false, the docker-compose will automatically just spawn containers with no launch command.
19+
NUM_ROBOTS="1"
1920

2021
# ================ SIMULATION =================
2122
ISAAC_SIM_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/AFCA/fire_academy_faro_with_sky.scene.usd"
@@ -25,8 +26,6 @@ PLAY_SIM_ON_START="true"
2526
# ================= ROBOT =====================
2627
# See robot/docker/docker-compose.yaml for how these variables get propagated in
2728
# the container's entry command.
28-
NUM_ROBOTS="1"
29-
3029
ROBOT_LAUNCH_PACKAGE="robot_bringup"
3130
ROBOT_LAUNCH_FILE="robot.launch.xml"
3231

‎.gitmodules‎

Lines changed: 3 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,6 @@
77
[submodule "ground_control_station/ros_ws/src/ros-gst-bridge"]
88
path = ground_control_station/ros_ws/src/ros-gst-bridge
99
url = https://github.com/BrettRD/ros-gst-bridge.git
10-
[submodule "robot/ros_ws/src/autonomy/2_perception/macvo/macvo/src"]
11-
path = robot/ros_ws/src/autonomy/2_perception/macvo/macvo/src
12-
url = https://github.com/MAC-VO/MAC-VO.git
13-
[submodule "robot/ros_ws/src/autonomy/2_perception/macvo2"]
14-
path = robot/ros_ws/src/autonomy/2_perception/macvo2
15-
url = git@github.com:castacks/MAC-VO-ROS2.git
10+
[submodule "robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/macvo2/src"]
11+
path = robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/macvo2/src
12+
url = git@github.com:castacks/MAC-SLAM-Stable.git

‎robot/docker/Dockerfile.robot‎

Lines changed: 38 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,14 @@ RUN apt update -y && apt install -y \
8686

8787
RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh
8888

89+
# Install TensorRT
90+
RUN wget https://developer.download.nvidia.com/compute/cuda/repos/ubuntu$(lsb_release -rs | tr -d .)/x86_64/cuda-keyring_1.1-1_all.deb && \
91+
dpkg -i cuda-keyring_1.1-1_all.deb && \
92+
apt update -y && \
93+
apt install -y \
94+
libnvinfer8 libnvinfer-dev libnvinfer-plugin8 \
95+
python3-libnvinfer python3-libnvinfer-dev
96+
8997
# Install Python dependencies
9098
RUN pip3 install \
9199
empy \
@@ -118,7 +126,13 @@ RUN pip3 install \
118126
timm==0.9.12 \
119127
rerun-sdk==0.22.0 \
120128
yacs \
121-
wandb
129+
wandb \
130+
loguru \
131+
jaxtyping \
132+
kornia \
133+
typeguard==2.13.3 \
134+
onnx \
135+
tensorrt
122136

123137
# Override install newer openvdb 9.1.0 for compatibility with Ubuntu 22.04 https://bugs.launchpad.net/bugs/1970108
124138
RUN apt remove -y libopenvdb*; \
@@ -153,9 +167,11 @@ fi
153167
# Downloading model weights for MACVO
154168
WORKDIR /root/model_weights
155169
RUN wget -r "https://github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_FrontendCov.pth" && \
156-
wget -r "https://github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_posenet.pkl" && \
170+
wget -r "https://github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_posenet.pkl" && \
171+
wget -r "https://github.com/castacks/MAC-VO-ROS2/releases/download/dsta-efficient-v0/dsta_efficient.ckpt" && \
157172
mv /root/model_weights/github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_FrontendCov.pth /root/model_weights/MACVO_FrontendCov.pth && \
158173
mv /root/model_weights/github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_posenet.pkl /root/model_weights/MACVO_posenet.pkl && \
174+
mv /root/model_weights/github.com/castacks/MAC-VO-ROS2/releases/download/dsta-efficient-v0/dsta_efficient.ckpt /root/model_weights/dsta_efficient.ckpt && \
159175
rm -rf /root/model_weights/github.com
160176

161177
WORKDIR /root/ros_ws
@@ -170,4 +186,23 @@ RUN pip install -U colcon-common-extensions
170186

171187
# Fixes for MACVO Integration
172188
RUN pip install huggingface_hub
173-
RUN pip uninstall matplotlib -y
189+
RUN pip uninstall matplotlib -y
190+
191+
# Temporary fix for UFM
192+
WORKDIR /root/model_weights
193+
RUN wget -r "https://github.com/castacks/MAC-VO-ROS2/releases/download/dsta-efficient-v0/UFM_Env2.zip" && \
194+
apt update && apt install -y unzip && \
195+
mv /root/model_weights/github.com/castacks/MAC-VO-ROS2/releases/download/dsta-efficient-v0/UFM_Env2.zip /root/model_weights/UFM_Env2.zip && \
196+
unzip UFM_Env2.zip && \
197+
rm UFM_Env2.zip
198+
199+
WORKDIR /root/model_weights/UFM
200+
RUN pip install -e .
201+
202+
WORKDIR /root/model_weights/UFM/UniCeption
203+
RUN pip install -e .
204+
205+
WORKDIR /root/model_weights/UFM/benchmarks
206+
RUN pip install -e .
207+
208+
WORKDIR /root/ros_ws

‎robot/docker/docker-compose.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ services:
7373
# ===================================================================================================================
7474
# for running tests on the robot
7575
robot-test:
76-
profiles:
76+
profiles: !override
7777
- "test"
7878
extends:
7979
service: robot

‎robot/ros_ws/.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
build/
22
install/
33
log/
4+
cache/

‎robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/.gitignore‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
build/*
2+
install/*
3+
log/*
4+
MACVO_ROS2/Model/*
5+
*.pth
6+
*.pkl
7+
*.pyc
8+
__pycache__/

‎robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/.gitmodules‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
[submodule "macvo2/src"]
2+
path = macvo2/src
3+
url = git@github.com:haleqiu/MAC-SLAM.git

‎robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/README.md‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# MAC-VO ROS2 Wrapper
2+
3+
This repository is the ROS-2 wrapper for our work *MAC-VO: Metrics-aware Covariance for Learning-based Stereo Visual Odometry*. To visit our original codebase, see [https://github.com/MAC-VO/MAC-VO](https://github.com/MAC-VO/MAC-VO).
4+
5+
## Install and Configuration
6+
7+
To install as a ROS2 node, clone this directory in your ROS2 workspace and run
8+
9+
> [!NOTE]
10+
> Please clone the repository with `--recursive` flag to clone all the submodules automatically.
11+
12+
```bash
13+
colcon build
14+
source install/local_setup.bash
15+
```
16+
17+
To launch the MAC-VO Node, use the following config, substitute `[PATH_TO_CONFIG]` with your own `.yaml` config path.
18+
19+
```
20+
ros2 run MACVO_ROS2 MACVO --config [PATH_TO_CONFIG]
21+
```
22+
23+
## Pretrained Model
24+
25+
Please follow the `README.md` on [https://github.com/MAC-VO/MAC-VO](https://github.com/MAC-VO/MAC-VO) to download the pre-trained model. The default path for the pre-trained model is set to be `MACVO_ROS2/src/Module`

‎robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/config/interface_config.yaml‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
/**:
2+
ros__parameters:
3+
camera_name: "front_stereo"
4+
camera_param_server_client_topic: "sensors/get_camera_params"
5+
coordinate_frame: "robot_origin_ned"
6+
imageL_sub_topic: "left/image_rect"
7+
imageR_sub_topic: "right/image_rect"
8+
pose_pub_topic: "pose"
9+
odom_pub_topic: "odometry"
10+
point_pub_topic: "point_cloud"
11+
disp_pub_topic: "disparity"
12+
img_pub_topic: "image_features"
13+
inference_dim_u: 420
14+
inference_dim_v: 420

‎robot/ros_ws/src/autonomy/2_perception/MAC-VO-ROS/config/model_config.yaml‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,89 @@
1+
Common:
2+
# Some configurations are shared across multiple modules in Odometry, so I write them here.
3+
device: &device cuda
4+
5+
Odometry:
6+
name: MACVO_Airstack_TRT
7+
args:
8+
# Device directive to the VO system
9+
# NOTE: the system may not follow this device config strictly since some module
10+
# e.g. those rely on PWC-Net, only support running on cuda device.
11+
device: *device
12+
edgewidth: 8
13+
num_point: 200 # Upper bound of KPs in each frame
14+
15+
# Match covariance for keypoint on first observation (sub-pixel uncertainty
16+
# caused by the quantized pixel)
17+
match_cov_default: 0.25
18+
19+
# Profiling the system using torch, generate chrome json trace file.
20+
profile: false
21+
22+
# Mapping mode provides the dense mapping
23+
mapping: 5000
24+
25+
# Online mode does not keep accumulated data inside the VO module.
26+
online: true
27+
28+
cov:
29+
obs:
30+
type: MatchCovariance
31+
args:
32+
device: *device
33+
kernel_size: 7
34+
match_cov_default: 0.25
35+
min_depth_cov: 0.05
36+
min_flow_cov: 0.25
37+
38+
keypoint:
39+
type: CovAwareSelector_NoDepth
40+
args:
41+
device: *device
42+
kernel_size: 7
43+
mask_width: 8
44+
max_match_cov: 100.0
45+
46+
mappoint:
47+
# Mapping feature can be conveniently turn off by using
48+
# "type: NoKeypointSelector" instead of the config below.
49+
type: MappingPointSelector
50+
args:
51+
device: *device
52+
max_depth: 5.0
53+
max_depth_cov: 0.003
54+
mask_width: 8
55+
56+
frontend:
57+
type: TRT_FlowFormerCovFrontend
58+
args:
59+
device: *device
60+
weight: /root/model_weights/MACVO_FrontendCov.pth
61+
dtype: fp32
62+
max_flow: -1
63+
enforce_positive_disparity: false
64+
decoder_depth: 4
65+
66+
motion:
67+
type: StaticMotionModel
68+
args:
69+
70+
outlier:
71+
type: CovarianceSanityFilter
72+
args:
73+
74+
postprocess:
75+
type: MotionInterpolate
76+
args:
77+
78+
keyframe:
79+
type: AllKeyframe
80+
args:
81+
82+
optimizer:
83+
type: Local_TwoFrame_PGO
84+
args:
85+
device: cpu
86+
vectorize: true
87+
autodiff: false
88+
parallel: true
89+
graph_type: disp

0 commit comments

Comments
 (0)