ROS FoundationPose + Model Generation

About

This repository is a ROS1 implementation of Foundation Pose from the fork ROS_FoundationPose. It is useful for model-based 6DoF pose tracking with added support for model generation from images.

This was originally developed by Bowen et. al., describled in their paper titled "FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects".

RGB-D images data are obtained from an Intel RealSense Depth camera using the realsense-ros wrapper.

The binary mask of the first frame, as required by the model-based implementation of FoundationPose (refer to the original paper), is generated using Grounded-SAM, from IDEA-Research. This uses Grounding-DINO, developed by IDEA-Research and Segment Anything Model (SAM), developed by Meta.

Setup

This repository consists of three directories, FoundationPose_ROS, Grounded-Segment-Anything, and Bundlesdf. Once the repository has been cloned, weights for each model need to be downloaded.

The weights for FoundationPose can be downloaded from here. The two folders downloaded should be placed as-is in FoundationPose_ROS/weights/.

The weights for GroundedSAM can be downloaded using the following commands:

cd Grounded-Segment-Anything
wget https://dl.fbaipublicfiles.com/segment_anything/sam_vit_h_4b8939.pth
wget https://github.com/IDEA-Research/GroundingDINO/releases/download/v0.1.0-alpha/groundingdino_swint_ogc.pth

Due to dependecy clashes, FoundationPose and GroundedSAM ROS nodes work in separate docker containers. You can pull working containers for each from:

docker pull ghcr.io/shubho-upenn/ros_fp_new_env:latest	## FoundationPose Container Image
docker pull ghcr.io/shubho-upenn/gsa_ros:latest		## Grounded-Segment-Anything Container Image

Additionally, you will need to build the docker image for BundleSDF (this only needs to do once and can take some time).

cd BundleSDF/docker
docker build --network host -t nvcr.io/nvidian/bundlesdf .

Prerequisites

1. GPU with Cuda >= 11.7
2. ROS Noetic

ROS Implementation

1. Run the docker containers

To run the docker containers for the ROS implementation, you will need to open two separate terminals. In the first terminal run the GroundedSAM ROS container using:

cd Grounded-Segment-Anything
bash run_gsa_ros.sh

In the other terminal run the FoundationPose ROS container using:

cd FoundationPose_ROS/docker
bash run_container_new_env.sh

2. Launch the Transformation Broadcaster

Next, while I integrate the tf broadcaster into run_ROS.py, you can run this broadcaster script from any terminal to get the transform from the static camera to the base link. You should be able to use your conda environment.

conda activate py310
# Source ROS here (Jan you can use the IP/Master in boot_scripts/source_ros.sh)
cd ~/FoundationPoseExtension/Calibrate
python broadcast_transform.py --path T_base_static.json

# Note: If you wish to recalibrate, see the README in /Calibrate

2. Launch the RealSense Node

Note: Only useful for calibration, see the calibration README for more. Skip if using MT-GMM

Launch the RealSense node in a new terminal on the host computer. Be sure to set align_depth:=true and make sure you have installed tmux

Note, this requires ROS1 Noetic on your host machine. Installation directions can be found here.

cd boot_scripts
bash start_ros.sh

3. Run the GroundedSAM ROS node

In the gsa_ros (GroundedSAM) container do the following to run the ROS GroundedSAM:

# Source ROS here (Jan you can use the IP/Master in boot_scripts/source_ros.sh)
cd Grounded-Segment-Anything
python3 ros_gsam_endpoint.py

Enter the object that you want to track to enable the GroundedSAM to detect and segment the object in the image and press enter. Accurate and general inputs such as 'mustard bottle' or 'orange hand drill' (as in examples) will work well.

4. Run the FoundationPose ROS node

Before running the command, place the .obj file of the object you want to track in the mesh directory. Then, in the ros_fp_new_env (FoundationPose) container, do the following to run the ROS Foundation Pose tracking:

# Source ROS here (Jan you can use the IP/Master in boot_scripts/source_ros.sh)
conda activate test_env
python3 run_ROS.py -in path/to/obj/file 
#Banana: ~/FoundationPoseExtension/FoundationPose_ROS/mesh/banana/textured_simple.obj

Now both endpoints are waiting to recieve data...

You should see the object being tracked. ROS Topics are created for the object you are tracking.

Troubleshooting

If there is an error like _C build doesn't exist, check to ensure you added the right weights and the build was successful. If so, feel free to fix haha, but otherwise, you can use my user to run the Docker Images.

xhost +SI:localuser:erich && pkexec --user erich env DISPLAY=$DISPLAY XAUTHORITY=$XAUTHORITY bash
cd ~/FoundationPose-ROS/FoundationPose
# Then retry step 4.

Model Generation Implementation

In order to run FoundationPose on an object, a model of that object is needed. In the case that you do not have a model, or if you wish to use a unique object, the following section will walk you through how to create an object model of any object from a series of input images. This uses RGB-D images from Realsense2 RGBD Camera and auto-generates masked images using GroundedSAM.

1. Collect Object Data

To collect data, you will need to run GroundedSAM

# Start  docker container if it isn't running already
cd Grounded-Segment-Anything
bash run_gsa_ros.sh

# Install dependencies for realsense2 and opencv
pip install pyrealsense2
pip uninstall opencv-python-headless -y 
pip install opencv-python --upgrade

# Run data generation script
python run_data_generator.py --path /path/to/output/folder --save_failures

Use this output folder as the input for BundleSDF in the following section.

2. Run the BundleSDF for Model Generation

To start the BundleSDF docker container the first time:

cd BundleSDF/docker && bash run_container.sh

# Inside docker container, compile the packages which are machine dependent
bash build.sh

The container can be restarted without building after the initial run using this command:

docker start bundlesdf
xhost + && docker exec -e DISPLAY=${DISPLAY} -it bundlesdf bash

Finally, you can run the following command to create a model out of your data

python run_custom.py 
  --mode run_video 
  --video_dir /path/to/input/folder/location 
  --out_folder /path/to/output/folder/location 
  --use_segmenter 1 
  --use_gui 1 
  --debug_level 2

The output will contain a mesh folder which contains a textured .obj file which you can use to run

Troubleshooting:

• If you encounter issues with LD Library, you may need to export the path to your local instance:

export LD_LIBRARY_PATH=/usr/local/cuda/lib64    
export LD_PRELOAD=/opt/conda/envs/py38/lib/libstdc++.so.6

Acknowledegement

This repository is based largely from shubho-upenn and adapted to include mechanics to create your own datasets.

This repository is also largely based on the work and takes major components from FoundationPose developed by Bowen et. al. (NVLabs) and GroundedSAM from IDEA-Research and Meta.

Additionally, this repository uses the work of

The Licence for FoundationPose can be found in ./FoundationPose_ROS/LICENCE.

The Licence for Grounded-SAM can be found in ./Grounded-Segment-Anything/LICENCE.

The Licence for segment-anything can be found in ./Grounded-Segment-Anything/segment_anything/LICENCE.

The original readme.md of each of these repos can also be found in their respective aforementioned directories.

Bibtex Citation for FoundationPose:

@InProceedings{foundationposewen2024,
author        = {Bowen Wen, Wei Yang, Jan Kautz, Stan Birchfield},
title         = {{FoundationPose}: Unified 6D Pose Estimation and Tracking of Novel Objects},
booktitle     = {CVPR},
year          = {2024},
}

Bibtex Citation for BundleSDF

@InProceedings{bundlesdfwen2023,
author        = {Bowen Wen and Jonathan Tremblay and Valts Blukis and Stephen Tyree and Thomas M\"{u}ller and Alex Evans and Dieter Fox and Jan Kautz and Stan Birchfield},
title         = {{BundleSDF}: {N}eural 6-{DoF} Tracking and {3D} Reconstruction of Unknown Objects},
booktitle     = {CVPR},
year          = {2023},
}

Bibtex Citation for Grounded-SAM (GroundingDINO and SegmentAnything):

@article{kirillov2023segany,
  title={Segment Anything}, 
  author={Kirillov, Alexander and Mintun, Eric and Ravi, Nikhila and Mao, Hanzi and Rolland, Chloe and Gustafson, Laura and Xiao, Tete and Whitehead, Spencer and Berg, Alexander C. and Lo, Wan-Yen and Doll{\'a}r, Piotr and Girshick, Ross},
  journal={arXiv:2304.02643},
  year={2023}
}

@article{liu2023grounding,
  title={Grounding dino: Marrying dino with grounded pre-training for open-set object detection},
  author={Liu, Shilong and Zeng, Zhaoyang and Ren, Tianhe and Li, Feng and Zhang, Hao and Yang, Jie and Li, Chunyuan and Yang, Jianwei and Su, Hang and Zhu, Jun and others},
  journal={arXiv preprint arXiv:2303.05499},
  year={2023}
}

@misc{ren2024grounded,
      title={Grounded SAM: Assembling Open-World Models for Diverse Visual Tasks}, 
      author={Tianhe Ren and Shilong Liu and Ailing Zeng and Jing Lin and Kunchang Li and He Cao and Jiayu Chen and Xinyu Huang and Yukang Chen and Feng Yan and Zhaoyang Zeng and Hao Zhang and Feng Li and Jie Yang and Hongyang Li and Qing Jiang and Lei Zhang},
      year={2024},
      eprint={2401.14159},
      archivePrefix={arXiv},
      primaryClass={cs.CV}
}