From 6af0da0fd03510b40c42beea581e2a97245321df Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 11:33:33 -0500 Subject: [PATCH 01/13] update: make it compile on cu11 --- CMakeLists.txt | 2 +- README.md | 196 ++--------------------------------------- README.old.md | 194 ++++++++++++++++++++++++++++++++++++++++ src/Bundler.cpp | 4 +- src/FeatureManager.cpp | 2 +- src/Frame.cpp | 2 +- 6 files changed, 205 insertions(+), 195 deletions(-) create mode 100644 README.old.md diff --git a/CMakeLists.txt b/CMakeLists.txt index ddc2485..195c241 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(BundleTrack) set(CMAKE_BUILD_TYPE Release) add_compile_options(-std=c++14 -fpermissive -fPIC -g) -set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -Xcompiler;-fPIC;-gencode arch=compute_30,code=sm_30;-gencode arch=compute_61,code=sm_61;-gencode arch=compute_75,code=sm_75;-O3;-std=c++11;-use_fast_math;--default-stream per-thread) +set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -Xcompiler;-fPIC;-gencode arch=compute_86,code=sm_86;-O3;-std=c++11;-use_fast_math;--default-stream per-thread) if (${CMAKE_BUILD_TYPE} STREQUAL "Release") set(COMPILE_OPTIONS -fopenmp -march=native -msse -msse2 -msse3 -msse4 -msse4.2 -mavx2 -ftree-vectorize) diff --git a/README.md b/README.md index 887fca9..098e8a4 100644 --- a/README.md +++ b/README.md @@ -1,194 +1,10 @@ -This is the official implementation of our paper: +# BundleTrack2 -[BundleTrack: 6D Pose Tracking for Novel Objects without Instance or Category-Level 3D Models](https://arxiv.org/abs/2108.00516) +I want to modify BudleTrack, to make it compile on the latest machine. - accepted in International Conference on Intelligent Robots and Systems (IROS) 2021. +Code is not compatible to CUDA11. -# Abstract -Most prior 6D object pose tracking often assume that the target object's CAD model, at least at a category-level, is available for offline training or during online template matching. This work proposes BundleTrack, a general framework for 6D pose tracking of novel objects, which does not depend upon 3D models, either at the instance or category-level. It leverages the complementary attributes of recent advances in deep learning for segmentation and robust feature extraction, as well as memory-augmented pose graph optimization for spatiotemporal consistency. This enables long-term, low-drift tracking under various challenging scenarios, including significant occlusions and object motions. Comprehensive experiments given two public benchmarks demonstrate that the proposed approach significantly outperforms state-of-art, category-level 6D tracking or dynamic SLAM methods. When compared against state-of-art methods that rely on an object instance CAD model, comparable performance is achieved, despite the proposed method's reduced information requirements. An efficient implementation in CUDA provides a real-time performance of 10Hz for the entire framework. +# Build BundleTrack from scratch -

- - -

- - - -**This repo can be readily applied to 6D pose tracking for novel unknown objects. For CAD model-based 6D pose tracking, please check out my another repository of [se(3)-TrackNet](https://github.com/wenbowen123/iros20-6d-pose-tracking)** - - -# Bibtex -```bibtex -@inproceedings{wen2021bundletrack, - title={BundleTrack: 6D Pose Tracking for Novel Objects without Instance or Category-Level 3D Models}, - author={Wen, B and Bekris, Kostas E}, - booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems}, - year={2021} -} -``` - - - -# Supplementary Video -Click to watch - -[](https://www.youtube.com/watch?v=89pnv3M_84g) - - - -# IROS 2021 Presentation -Click to watch - -[](https://www.youtube.com/watch?v=0UorLR0ADd4) - - -# Results - - - - - -# Benchmark Output Results -For convenience of benchmarking and making plots, results of pose outputs can be downloaded below -- For NOCS Dataset: https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/nocs_ours_results_iros2021.tar.gz -- For YCBInEOAT: https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/ycbineoat_ours_results.tar.gz - - - -# Setup -For the environment setup, it's strongly recommended to use our provided docker environment (setting up from scratch is very complicated and not supported in this repo). For this, you don't have to know how docker works. Only some basic commands are needed and will be provided in the below steps. - -- Install docker (https://docs.docker.com/get-docker/). - -- Run - ``` - docker pull wenbowen123/bundletrack:latest - docker pull wenbowen123/lf-net-release-env:latest - ``` - -- Edit the docker/run_container.sh, update the paths of `BUNDLETRACK_DIR`, `NOCS_DIR` and `YCBINEOAT_DIR` - -- Run `bash docker/run_container.sh` - -- `cd [PATH_TO_BUNDLETRACK]` - -- `rm -rf build && mkdir build && cd build && cmake .. && make` - - -# Data -Depending on what you want to run, download those data that are neccessary. -- [Download weights of feature detection network](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/indoor.tar.gz), extract and put it under `lf-net-release/release/models`, so it'll be `BundleTrack/lf-net-release/release/models/indoor` -- [Download weights of video segmentation network](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/pretrained.tar.gz), extract and put it under `./transductive-vos.pytorch`, so it'll be `BundleTrack/transductive-vos.pytorch/pretrained` -- [Download our precomputed masks](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/masks.tar.gz), extract and put in the repo so it becomes `BundleTrack/masks` - -- [Download NOCS Dataset](https://github.com/hughw19/NOCS_CVPR2019), then put it under a folder named "NOCS". [Download the converted ground-truth text pose files](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/real_test_text.tar.gz). And [download the addon](https://drive.google.com/file/d/1BknMsoRKRV-nhLDpozog2Lrz1OWTKqkM/view?usp=sharing). Finally unzip the files and make sure the path structure is like this: - - ``` - NOCS - ├── NOCS-REAL275-additional - ├── real_test - ├── gts - | └── real_test_text - └── obj_models - ``` - -- [Download YCBInEOAT](https://archive.cs.rutgers.edu/archive/a/2020/pracsys/Bowen/iros2020/YCBInEOAT/) to make it like - ``` - YCBInEOAT - ├── bleach0 - | ├──annotated_poses - | ├──depth - | ├──depth_filled - | ├──gt_mask - | ├──masks - | ├──masks_vis - | ├──rgb - | ├──cam_K.txt - | ├──...... - └── .... - ``` - -- [Download YCB Objects](http://ycb-benchmarks.s3-website-us-east-1.amazonaws.com/) - -# Run predictions on NOCS -- Open a separate terminal and run - - ``` - bash lf-net-release/docker/run_container.sh - cd [PATH_TO_BUNDLETRACK] - cd lf-net-release && python run_server.py - ``` - - -- Go back to the terminal where you launched the bundletrack docker in above and run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/`. For more detailed logs, change `LOG` to 2 or higher in `config_nocs.yml`. - - ``` - python scripts/run_nocs.py --nocs_dir [PATH_TO_NOCS] --scene_id 1 --port 5555 --model_name can_arizona_tea_norm - ``` - -- Finally, the results will be saved in `/tmp/BundleTrack/` - - -- For evaluating on the entire NOCS Dataset, run (**NOTE that this will add noise to perturb the initial ground-truth pose for evaluation as explained in the paper. If you want to see how BundleTrack actually performs, run the above section**) - ``` - python scripts/eval_nocs.py --nocs_dir [PAHT TO NOCS] --results_dir [PATH TO THE RUNNING OUTPUTS] - ``` - - -# Run predictions on YCBInEOAT - -- Change the `model_name` and `model_dir` in `config_ycbineoat.yml` to the path to the .obj file (e.g. For folder `bleach0`, the model_name is `021_bleach_cleanser`, and model_dir is `[Your path to YCB Objects]/021_bleach_cleanser/textured_simple.obj`) - -- Open a separate terminal and run - - ``` - bash lf-net-release/docker/run_container.sh - cd [PATH_TO_BUNDLETRACK] - cd lf-net-release && python run_server.py - ``` - -- Go back to the terminal where you launched the bundletrack docker in above, and run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/` - - ``` - python scripts/run_ycbineoat.py --data_dir [PATH_TO_YCBInEOAT] --port 5555 --model_name [The YCB object's name, e.g. 021_bleach_cleanser] - ``` - -- Finally, the results will be saved in `/tmp/BundleTrack/`. For more detailed logs, change `LOG` to 2 or higher in `config_ycbineoat.yml`. - -- For evaluating on the entire YCBInEOAT Dataset, run - ``` - python scripts/eval_nocs.py --ycbineoat_dir [PAHT TO YCBINEOAT] --ycb_model_dir [YCB MODELS FOLDER] --results_dir [PATH TO THE RUN OUTPUTS] - ``` - - -# Run predictions on your own RGBD data - -- Download YCBInEOAT, if you haven't done so in above. - -- Open a separate terminal and run - - ``` - bash lf-net-release/docker/run_container.sh - cd [PATH_TO_BUNDLETRACK] - cd lf-net-release && python run_server.py - ``` - -- Prepare segmentation masks. In YCBInEOAT Dataset, we computed masks from robotic arm forward kinematics. If your scene is not too complicated similar to NOCS Dataset, you can run the video segmentation network to get masks as below: - - - First you need to prepare an initial mask (grayscale image, where 0 means background, else foreground). - - `python transductive-vos.pytorch/run_video.py --img_dir [THE PATH TO COLOR FILES] --init_mask_file [THE INITIAL MASK FILE YOU PREPARED ABOVE] --mask_save_dir [WHERE TO SAVE]` - - Prepare your folder structure same as any folder (e.g. "mustard_easy_00_02") in YCBInEOAT Dataset. Put it under the same directory in YCBInEOAT, i.e. next to "mustard_easy_00_02". Then edit `config_ycbineoat.yml` to make sure the paths at top are right. - - Structure: - ``` - mustard_easy_00_02 - ├── rgb - ├── masks - ├── depth - └── cam_K.txt - ``` - -- Go back to the terminal where you launched the bundletrack docker, run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/` - ``` - python scripts/run_ycbineoat.py --data_dir [PATH TO YOUR FOLDER ABOVE] --port 5555 - ``` +1. Install LibZMQ. +2. Install opencv with cuda support. \ No newline at end of file diff --git a/README.old.md b/README.old.md new file mode 100644 index 0000000..887fca9 --- /dev/null +++ b/README.old.md @@ -0,0 +1,194 @@ +This is the official implementation of our paper: + +[BundleTrack: 6D Pose Tracking for Novel Objects without Instance or Category-Level 3D Models](https://arxiv.org/abs/2108.00516) + + accepted in International Conference on Intelligent Robots and Systems (IROS) 2021. + +# Abstract +Most prior 6D object pose tracking often assume that the target object's CAD model, at least at a category-level, is available for offline training or during online template matching. This work proposes BundleTrack, a general framework for 6D pose tracking of novel objects, which does not depend upon 3D models, either at the instance or category-level. It leverages the complementary attributes of recent advances in deep learning for segmentation and robust feature extraction, as well as memory-augmented pose graph optimization for spatiotemporal consistency. This enables long-term, low-drift tracking under various challenging scenarios, including significant occlusions and object motions. Comprehensive experiments given two public benchmarks demonstrate that the proposed approach significantly outperforms state-of-art, category-level 6D tracking or dynamic SLAM methods. When compared against state-of-art methods that rely on an object instance CAD model, comparable performance is achieved, despite the proposed method's reduced information requirements. An efficient implementation in CUDA provides a real-time performance of 10Hz for the entire framework. + +

+ + +

+ + + +**This repo can be readily applied to 6D pose tracking for novel unknown objects. For CAD model-based 6D pose tracking, please check out my another repository of [se(3)-TrackNet](https://github.com/wenbowen123/iros20-6d-pose-tracking)** + + +# Bibtex +```bibtex +@inproceedings{wen2021bundletrack, + title={BundleTrack: 6D Pose Tracking for Novel Objects without Instance or Category-Level 3D Models}, + author={Wen, B and Bekris, Kostas E}, + booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems}, + year={2021} +} +``` + + + +# Supplementary Video +Click to watch + +[](https://www.youtube.com/watch?v=89pnv3M_84g) + + + +# IROS 2021 Presentation +Click to watch + +[](https://www.youtube.com/watch?v=0UorLR0ADd4) + + +# Results + + + + + +# Benchmark Output Results +For convenience of benchmarking and making plots, results of pose outputs can be downloaded below +- For NOCS Dataset: https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/nocs_ours_results_iros2021.tar.gz +- For YCBInEOAT: https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/ycbineoat_ours_results.tar.gz + + + +# Setup +For the environment setup, it's strongly recommended to use our provided docker environment (setting up from scratch is very complicated and not supported in this repo). For this, you don't have to know how docker works. Only some basic commands are needed and will be provided in the below steps. + +- Install docker (https://docs.docker.com/get-docker/). + +- Run + ``` + docker pull wenbowen123/bundletrack:latest + docker pull wenbowen123/lf-net-release-env:latest + ``` + +- Edit the docker/run_container.sh, update the paths of `BUNDLETRACK_DIR`, `NOCS_DIR` and `YCBINEOAT_DIR` + +- Run `bash docker/run_container.sh` + +- `cd [PATH_TO_BUNDLETRACK]` + +- `rm -rf build && mkdir build && cd build && cmake .. && make` + + +# Data +Depending on what you want to run, download those data that are neccessary. +- [Download weights of feature detection network](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/indoor.tar.gz), extract and put it under `lf-net-release/release/models`, so it'll be `BundleTrack/lf-net-release/release/models/indoor` +- [Download weights of video segmentation network](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/pretrained.tar.gz), extract and put it under `./transductive-vos.pytorch`, so it'll be `BundleTrack/transductive-vos.pytorch/pretrained` +- [Download our precomputed masks](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/masks.tar.gz), extract and put in the repo so it becomes `BundleTrack/masks` + +- [Download NOCS Dataset](https://github.com/hughw19/NOCS_CVPR2019), then put it under a folder named "NOCS". [Download the converted ground-truth text pose files](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/real_test_text.tar.gz). And [download the addon](https://drive.google.com/file/d/1BknMsoRKRV-nhLDpozog2Lrz1OWTKqkM/view?usp=sharing). Finally unzip the files and make sure the path structure is like this: + + ``` + NOCS + ├── NOCS-REAL275-additional + ├── real_test + ├── gts + | └── real_test_text + └── obj_models + ``` + +- [Download YCBInEOAT](https://archive.cs.rutgers.edu/archive/a/2020/pracsys/Bowen/iros2020/YCBInEOAT/) to make it like + ``` + YCBInEOAT + ├── bleach0 + | ├──annotated_poses + | ├──depth + | ├──depth_filled + | ├──gt_mask + | ├──masks + | ├──masks_vis + | ├──rgb + | ├──cam_K.txt + | ├──...... + └── .... + ``` + +- [Download YCB Objects](http://ycb-benchmarks.s3-website-us-east-1.amazonaws.com/) + +# Run predictions on NOCS +- Open a separate terminal and run + + ``` + bash lf-net-release/docker/run_container.sh + cd [PATH_TO_BUNDLETRACK] + cd lf-net-release && python run_server.py + ``` + + +- Go back to the terminal where you launched the bundletrack docker in above and run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/`. For more detailed logs, change `LOG` to 2 or higher in `config_nocs.yml`. + + ``` + python scripts/run_nocs.py --nocs_dir [PATH_TO_NOCS] --scene_id 1 --port 5555 --model_name can_arizona_tea_norm + ``` + +- Finally, the results will be saved in `/tmp/BundleTrack/` + + +- For evaluating on the entire NOCS Dataset, run (**NOTE that this will add noise to perturb the initial ground-truth pose for evaluation as explained in the paper. If you want to see how BundleTrack actually performs, run the above section**) + ``` + python scripts/eval_nocs.py --nocs_dir [PAHT TO NOCS] --results_dir [PATH TO THE RUNNING OUTPUTS] + ``` + + +# Run predictions on YCBInEOAT + +- Change the `model_name` and `model_dir` in `config_ycbineoat.yml` to the path to the .obj file (e.g. For folder `bleach0`, the model_name is `021_bleach_cleanser`, and model_dir is `[Your path to YCB Objects]/021_bleach_cleanser/textured_simple.obj`) + +- Open a separate terminal and run + + ``` + bash lf-net-release/docker/run_container.sh + cd [PATH_TO_BUNDLETRACK] + cd lf-net-release && python run_server.py + ``` + +- Go back to the terminal where you launched the bundletrack docker in above, and run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/` + + ``` + python scripts/run_ycbineoat.py --data_dir [PATH_TO_YCBInEOAT] --port 5555 --model_name [The YCB object's name, e.g. 021_bleach_cleanser] + ``` + +- Finally, the results will be saved in `/tmp/BundleTrack/`. For more detailed logs, change `LOG` to 2 or higher in `config_ycbineoat.yml`. + +- For evaluating on the entire YCBInEOAT Dataset, run + ``` + python scripts/eval_nocs.py --ycbineoat_dir [PAHT TO YCBINEOAT] --ycb_model_dir [YCB MODELS FOLDER] --results_dir [PATH TO THE RUN OUTPUTS] + ``` + + +# Run predictions on your own RGBD data + +- Download YCBInEOAT, if you haven't done so in above. + +- Open a separate terminal and run + + ``` + bash lf-net-release/docker/run_container.sh + cd [PATH_TO_BUNDLETRACK] + cd lf-net-release && python run_server.py + ``` + +- Prepare segmentation masks. In YCBInEOAT Dataset, we computed masks from robotic arm forward kinematics. If your scene is not too complicated similar to NOCS Dataset, you can run the video segmentation network to get masks as below: + + - First you need to prepare an initial mask (grayscale image, where 0 means background, else foreground). + - `python transductive-vos.pytorch/run_video.py --img_dir [THE PATH TO COLOR FILES] --init_mask_file [THE INITIAL MASK FILE YOU PREPARED ABOVE] --mask_save_dir [WHERE TO SAVE]` + - Prepare your folder structure same as any folder (e.g. "mustard_easy_00_02") in YCBInEOAT Dataset. Put it under the same directory in YCBInEOAT, i.e. next to "mustard_easy_00_02". Then edit `config_ycbineoat.yml` to make sure the paths at top are right. + + Structure: + ``` + mustard_easy_00_02 + ├── rgb + ├── masks + ├── depth + └── cam_K.txt + ``` + +- Go back to the terminal where you launched the bundletrack docker, run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/` + ``` + python scripts/run_ycbineoat.py --data_dir [PATH TO YOUR FOLDER ABOVE] --port 5555 + ``` diff --git a/src/Bundler.cpp b/src/Bundler.cpp index 68d675f..c02fcea 100644 --- a/src/Bundler.cpp +++ b/src/Bundler.cpp @@ -399,8 +399,8 @@ void Bundler::saveNewframeResult() cv::putText(color_viz,_newframe->_id_str,{5,30},cv::FONT_HERSHEY_PLAIN,2,{255,0,0},1,8,false); // cv::imshow("color_viz",color_viz); // cv::waitKey(1); - cv::imwrite(debug_dir+"/color_viz/"+_newframe->_id_str+"_color_viz.jpg",color_viz,{CV_IMWRITE_JPEG_QUALITY, 80}); - cv::imwrite(out_dir+"color_viz.jpg",color_viz,{CV_IMWRITE_JPEG_QUALITY, 80}); + cv::imwrite(debug_dir+"/color_viz/"+_newframe->_id_str+"_color_viz.jpg",color_viz,{cv::IMWRITE_JPEG_QUALITY, 80}); + cv::imwrite(out_dir+"color_viz.jpg",color_viz,{cv::IMWRITE_JPEG_QUALITY, 80}); const std::string raw_dir = debug_dir+"/color_raw/"; if (!boost::filesystem::exists(raw_dir)) diff --git a/src/FeatureManager.cpp b/src/FeatureManager.cpp index dbdde17..8784174 100644 --- a/src/FeatureManager.cpp +++ b/src/FeatureManager.cpp @@ -791,7 +791,7 @@ void SiftManager::vizCorresBetween(std::shared_ptr frameA, std::shared_pt cv::Mat out; cv::drawMatches( colorA, kptsA, colorB, kptsB, cv_matches, out, cv::Scalar::all(-1), cv::Scalar::all(-1), std::vector(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); - cv::imwrite(out_match_file, out, {CV_IMWRITE_JPEG_QUALITY,80}); + cv::imwrite(out_match_file, out, {cv::IMWRITE_JPEG_QUALITY,80}); } diff --git a/src/Frame.cpp b/src/Frame.cpp index 3ea1ce8..d888647 100644 --- a/src/Frame.cpp +++ b/src/Frame.cpp @@ -70,7 +70,7 @@ Frame::Frame(const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depth_ra cudaMalloc(&_color_gpu, n_pixels*sizeof(uchar4)); - cv::cvtColor(_color, _gray, CV_BGR2GRAY); + cv::cvtColor(_color, _gray, cv::COLOR_BGR2GRAY); updateDepthGPU(); processDepth(); From 30a985505bb4dba89af11cc1fed750bbe3547fc7 Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 13:43:17 -0500 Subject: [PATCH 02/13] update: update config --- .gitignore | 1 + .vscode/launch.json | 35 +++++++++++++++++++++++++++++++++++ CMakeLists.txt | 2 +- README.md | 3 ++- config_ycbineoat.yml | 10 +++++----- 5 files changed, 44 insertions(+), 7 deletions(-) create mode 100644 .vscode/launch.json diff --git a/.gitignore b/.gitignore index 3fd7e4f..01e3564 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ build/ *.pth* *.pkl lf-net-release/release/models +data \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..44e118a --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,35 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "bundle_track_ycb", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/build/bundle_track_ycbineoat", + "args": [ + "${workspaceFolder}/config_ycbineoat.yml", + ], + "stopAtEntry": false, + "cwd": "${fileDirname}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + }, + { + "description": "Set Disassembly Flavor to Intel", + "text": "-gdb-set disassembly-flavor intel", + "ignoreFailures": true + } + ] + } + + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 195c241..773a24b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10) project(BundleTrack) -set(CMAKE_BUILD_TYPE Release) +set(CMAKE_BUILD_TYPE Debug) add_compile_options(-std=c++14 -fpermissive -fPIC -g) set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -Xcompiler;-fPIC;-gencode arch=compute_86,code=sm_86;-O3;-std=c++11;-use_fast_math;--default-stream per-thread) diff --git a/README.md b/README.md index 098e8a4..7051b84 100644 --- a/README.md +++ b/README.md @@ -7,4 +7,5 @@ Code is not compatible to CUDA11. # Build BundleTrack from scratch 1. Install LibZMQ. -2. Install opencv with cuda support. \ No newline at end of file +2. Install opencv with cuda support. +3. Cooperate R2D2 with BundleTrack. \ No newline at end of file diff --git a/config_ycbineoat.yml b/config_ycbineoat.yml index 6791825..1778710 100644 --- a/config_ycbineoat.yml +++ b/config_ycbineoat.yml @@ -1,8 +1,8 @@ -data_dir: /media/bowen/e25c9489-2f57-42dd-b076-021c59369fec/catkin_ws/src/iros20_dataset/video_rosbag/IROS_SELECTED/FINISHED_LABEL.iros_submission_version/bleach0 -mask_dir: '/media/bowen/e25c9489-2f57-42dd-b076-021c59369fec/catkin_ws/src/iros20_dataset/video_rosbag/IROS_SELECTED/FINISHED_LABEL.iros_submission_version/bleach0/masks' -model_name: 021_bleach_cleanser -model_dir: /media/bowen/e25c9489-2f57-42dd-b076-021c59369fec/DATASET/YCB_Video_Dataset/CADmodels/021_bleach_cleanser/textured_simple.obj -debug_dir: /tmp/BundleTrack/ +data_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/sugar_box1 +mask_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/sugar_box1/gt_mask +model_name: 004_sugar_box +model_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/models/004_sugar_box_berkeley_meshes/004_sugar_box/poisson/textured.obj +debug_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/log LOG: 0 port: '5555' From d0ff63db4cf5d3838ec640461fce7142f55a087b Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 14:34:00 -0500 Subject: [PATCH 03/13] update: add an empty server --- .vscode/launch.json | 14 ++++++++-- config_ycbineoat.yml | 2 +- r2d2/run_server.py | 62 ++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 75 insertions(+), 3 deletions(-) create mode 100644 r2d2/run_server.py diff --git a/.vscode/launch.json b/.vscode/launch.json index 44e118a..c314391 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,17 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "name": "r2d2 (server)", + "type": "python", + "request": "launch", + "program": "${workspaceFolder}/r2d2/run_server.py", + "args": [ + "--port=5556" + ], + "console": "integratedTerminal", + "justMyCode": true + }, { "name": "bundle_track_ycb", "type": "cppdbg", @@ -29,7 +40,6 @@ "ignoreFailures": true } ] - } - + }, ] } \ No newline at end of file diff --git a/config_ycbineoat.yml b/config_ycbineoat.yml index 1778710..6e7ebfa 100644 --- a/config_ycbineoat.yml +++ b/config_ycbineoat.yml @@ -4,7 +4,7 @@ model_name: 004_sugar_box model_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/models/004_sugar_box_berkeley_meshes/004_sugar_box/poisson/textured.obj debug_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/log LOG: 0 -port: '5555' +port: '5556' depth_processing: erode: diff --git a/r2d2/run_server.py b/r2d2/run_server.py new file mode 100644 index 0000000..c1cacee --- /dev/null +++ b/r2d2/run_server.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python +from __future__ import print_function +import numpy as np +import zmq +import cv2 +from tqdm import tqdm + + +def empty_network(feed_dict): + empty_output = { + "kpts": np.zeros((0, 2), dtype=np.float32), + "feats": np.zeros((0, 128), dtype=np.float32), + } + return empty_output + + +if __name__ == "__main__": + import argparse + parser = argparse.ArgumentParser() + parser.add_argument("--config", type=str, default="configs/r2d2.yml") + parser.add_argument("--ckpt", type=str, default="checkpoints/r2d2/r2d2.ckpt") + parser.add_argument("--port", type=int, default=5555) + args = parser.parse_args() + + context = zmq.Context() + socket = context.socket(zmq.REP) + port = f"tcp://*:{args.port}" + print("port", port) + socket.bind(port) + + while 1: + print(f"r2d2 listending to {port}") + msgs = socket.recv_multipart(0) + assert len(msgs) == 2, "#msgs={}".format(len(msgs)) + wh = np.frombuffer(msgs[0], dtype=np.int32) + W = wh[0] + H = wh[1] + print(f"W={W}, H={H}") + msg = msgs[1] + photo = np.frombuffer(msg, dtype=np.uint8).reshape(H, W, -1).squeeze() + photo_ori = photo.copy() + + rgb = photo.copy() + if photo.ndim == 3 and photo.shape[-1] == 3: + photo = cv2.cvtColor(photo, cv2.COLOR_RGB2GRAY) + photo = photo[None, ..., None].astype(np.float32) / 255.0 + assert photo.ndim == 4 + + feed_dict = { + "photo_ph": photo, + } + + outs = empty_network(feed_dict=feed_dict) + + num_feat = len(outs["kpts"]) + feat_dim = outs["feats"].shape[1] + msg = np.array([num_feat, feat_dim]).reshape(-1).astype(np.int32).tobytes() + socket.send(msg, 2) + msg = outs["kpts"].astype(np.float32).reshape(-1).tobytes() + socket.send(msg, 2) + msg = outs["feats"].astype(np.float32).reshape(-1).tobytes() + socket.send(msg, 0) From d20fc1c9a6faa9c0b13618c5ca084b0c8b2268f8 Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 16:48:15 -0500 Subject: [PATCH 04/13] update: update structure --- .vscode/launch.json | 4 ++-- .vscode/settings.json | 44 ++++++++++++++++++++++++++++++++++++- {r2d2 => fnn}/run_server.py | 2 +- 3 files changed, 46 insertions(+), 4 deletions(-) rename {r2d2 => fnn}/run_server.py (97%) diff --git a/.vscode/launch.json b/.vscode/launch.json index c314391..80aaa97 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -5,10 +5,10 @@ "version": "0.2.0", "configurations": [ { - "name": "r2d2 (server)", + "name": "fnn (server)", "type": "python", "request": "launch", - "program": "${workspaceFolder}/r2d2/run_server.py", + "program": "${workspaceFolder}/fnn/run_server.py", "args": [ "--port=5556" ], diff --git a/.vscode/settings.json b/.vscode/settings.json index 03f58a1..d1983e2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -100,6 +100,48 @@ "bufferobject": "cpp", "image": "cpp", "texture": "cpp", - "buffered_value": "cpp" + "buffered_value": "cpp", + "clocale": "cpp", + "csetjmp": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwctype": "cpp", + "any": "cpp", + "strstream": "cpp", + "barrier": "cpp", + "bit": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "coroutine": "cpp", + "cuchar": "cpp", + "forward_list": "cpp", + "set": "cpp", + "unordered_set": "cpp", + "ratio": "cpp", + "source_location": "cpp", + "system_error": "cpp", + "fstream": "cpp", + "future": "cpp", + "iomanip": "cpp", + "latch": "cpp", + "mutex": "cpp", + "numbers": "cpp", + "ranges": "cpp", + "scoped_allocator": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stop_token": "cpp", + "syncstream": "cpp", + "typeindex": "cpp", + "__nullptr": "cpp" } } \ No newline at end of file diff --git a/r2d2/run_server.py b/fnn/run_server.py similarity index 97% rename from r2d2/run_server.py rename to fnn/run_server.py index c1cacee..70472e0 100644 --- a/r2d2/run_server.py +++ b/fnn/run_server.py @@ -29,7 +29,7 @@ def empty_network(feed_dict): socket.bind(port) while 1: - print(f"r2d2 listending to {port}") + print(f"FNN listending to {port}") msgs = socket.recv_multipart(0) assert len(msgs) == 2, "#msgs={}".format(len(msgs)) wh = np.frombuffer(msgs[0], dtype=np.int32) From 95302701af1d2525fe65b38f782a37f481d0cbe1 Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 16:49:30 -0500 Subject: [PATCH 05/13] update: update structure --- .gitmodules | 3 +++ r2d2 | 1 + 2 files changed, 4 insertions(+) create mode 100644 .gitmodules create mode 160000 r2d2 diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..717e30a --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "r2d2"] + path = r2d2 + url = https://github.com/naver/r2d2.git diff --git a/r2d2 b/r2d2 new file mode 160000 index 0000000..0ff8f6a --- /dev/null +++ b/r2d2 @@ -0,0 +1 @@ +Subproject commit 0ff8f6afcbea91f19613d0cb7d93143a977830f5 From 9be06559a7d205421a362f80ccdd81d90848fba0 Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 16:49:37 -0500 Subject: [PATCH 06/13] update: update structure --- .gitmodules | 3 --- r2d2 | 1 - 2 files changed, 4 deletions(-) delete mode 160000 r2d2 diff --git a/.gitmodules b/.gitmodules index 717e30a..e69de29 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "r2d2"] - path = r2d2 - url = https://github.com/naver/r2d2.git diff --git a/r2d2 b/r2d2 deleted file mode 160000 index 0ff8f6a..0000000 --- a/r2d2 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0ff8f6afcbea91f19613d0cb7d93143a977830f5 From db209a1b0f6c17da0ad4d7172abe07b5eb956f6b Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 17:49:13 -0500 Subject: [PATCH 07/13] update: replace lfnet with r2d2 --- .gitmodules | 3 +++ r2d2 | 1 + 2 files changed, 4 insertions(+) create mode 160000 r2d2 diff --git a/.gitmodules b/.gitmodules index e69de29..7ecfbbf 100644 --- a/.gitmodules +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "r2d2"] + path = r2d2 + url = git@github.com:changhaonan/r2d2.git diff --git a/r2d2 b/r2d2 new file mode 160000 index 0000000..0ff8f6a --- /dev/null +++ b/r2d2 @@ -0,0 +1 @@ +Subproject commit 0ff8f6afcbea91f19613d0cb7d93143a977830f5 From 5f075d5c5f431033f4780bf05719226f15b4fed6 Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 17:49:18 -0500 Subject: [PATCH 08/13] update: update --- .gitignore | 4 +++- .vscode/launch.json | 10 ++++++---- r2d2 | 2 +- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 01e3564..579edac 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,6 @@ build/ *.pth* *.pkl lf-net-release/release/models -data \ No newline at end of file +data +r2d2 +log \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index 80aaa97..6f63143 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -5,15 +5,17 @@ "version": "0.2.0", "configurations": [ { - "name": "fnn (server)", + "name": "r2d2 (server)", "type": "python", "request": "launch", - "program": "${workspaceFolder}/fnn/run_server.py", + "program": "${workspaceFolder}/r2d2/run_server.py", "args": [ - "--port=5556" + "--port=5556", + "--model=models/r2d2_WASF_N16.pt" ], "console": "integratedTerminal", - "justMyCode": true + "justMyCode": true, + "cwd": "${workspaceFolder}/r2d2" }, { "name": "bundle_track_ycb", diff --git a/r2d2 b/r2d2 index 0ff8f6a..73b5126 160000 --- a/r2d2 +++ b/r2d2 @@ -1 +1 @@ -Subproject commit 0ff8f6afcbea91f19613d0cb7d93143a977830f5 +Subproject commit 73b5126c9575d1308c68894e25ea382b69985af9 From 8ab311ac4a7f85663295f5511556fe36e0be946c Mon Sep 17 00:00:00 2001 From: changhaonan Date: Mon, 6 Feb 2023 18:14:56 -0500 Subject: [PATCH 09/13] update: change the configuration --- README.md | 2 - README.old.md | 194 ------------------------------------------- config_ycbineoat.yml | 2 +- r2d2 | 2 +- 4 files changed, 2 insertions(+), 198 deletions(-) delete mode 100644 README.old.md diff --git a/README.md b/README.md index 7051b84..a3c9a3f 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,6 @@ I want to modify BudleTrack, to make it compile on the latest machine. -Code is not compatible to CUDA11. - # Build BundleTrack from scratch 1. Install LibZMQ. diff --git a/README.old.md b/README.old.md deleted file mode 100644 index 887fca9..0000000 --- a/README.old.md +++ /dev/null @@ -1,194 +0,0 @@ -This is the official implementation of our paper: - -[BundleTrack: 6D Pose Tracking for Novel Objects without Instance or Category-Level 3D Models](https://arxiv.org/abs/2108.00516) - - accepted in International Conference on Intelligent Robots and Systems (IROS) 2021. - -# Abstract -Most prior 6D object pose tracking often assume that the target object's CAD model, at least at a category-level, is available for offline training or during online template matching. This work proposes BundleTrack, a general framework for 6D pose tracking of novel objects, which does not depend upon 3D models, either at the instance or category-level. It leverages the complementary attributes of recent advances in deep learning for segmentation and robust feature extraction, as well as memory-augmented pose graph optimization for spatiotemporal consistency. This enables long-term, low-drift tracking under various challenging scenarios, including significant occlusions and object motions. Comprehensive experiments given two public benchmarks demonstrate that the proposed approach significantly outperforms state-of-art, category-level 6D tracking or dynamic SLAM methods. When compared against state-of-art methods that rely on an object instance CAD model, comparable performance is achieved, despite the proposed method's reduced information requirements. An efficient implementation in CUDA provides a real-time performance of 10Hz for the entire framework. - -

- - -

- - - -**This repo can be readily applied to 6D pose tracking for novel unknown objects. For CAD model-based 6D pose tracking, please check out my another repository of [se(3)-TrackNet](https://github.com/wenbowen123/iros20-6d-pose-tracking)** - - -# Bibtex -```bibtex -@inproceedings{wen2021bundletrack, - title={BundleTrack: 6D Pose Tracking for Novel Objects without Instance or Category-Level 3D Models}, - author={Wen, B and Bekris, Kostas E}, - booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems}, - year={2021} -} -``` - - - -# Supplementary Video -Click to watch - -[](https://www.youtube.com/watch?v=89pnv3M_84g) - - - -# IROS 2021 Presentation -Click to watch - -[](https://www.youtube.com/watch?v=0UorLR0ADd4) - - -# Results - - - - - -# Benchmark Output Results -For convenience of benchmarking and making plots, results of pose outputs can be downloaded below -- For NOCS Dataset: https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/nocs_ours_results_iros2021.tar.gz -- For YCBInEOAT: https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/ycbineoat_ours_results.tar.gz - - - -# Setup -For the environment setup, it's strongly recommended to use our provided docker environment (setting up from scratch is very complicated and not supported in this repo). For this, you don't have to know how docker works. Only some basic commands are needed and will be provided in the below steps. - -- Install docker (https://docs.docker.com/get-docker/). - -- Run - ``` - docker pull wenbowen123/bundletrack:latest - docker pull wenbowen123/lf-net-release-env:latest - ``` - -- Edit the docker/run_container.sh, update the paths of `BUNDLETRACK_DIR`, `NOCS_DIR` and `YCBINEOAT_DIR` - -- Run `bash docker/run_container.sh` - -- `cd [PATH_TO_BUNDLETRACK]` - -- `rm -rf build && mkdir build && cd build && cmake .. && make` - - -# Data -Depending on what you want to run, download those data that are neccessary. -- [Download weights of feature detection network](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/indoor.tar.gz), extract and put it under `lf-net-release/release/models`, so it'll be `BundleTrack/lf-net-release/release/models/indoor` -- [Download weights of video segmentation network](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/pretrained.tar.gz), extract and put it under `./transductive-vos.pytorch`, so it'll be `BundleTrack/transductive-vos.pytorch/pretrained` -- [Download our precomputed masks](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/masks.tar.gz), extract and put in the repo so it becomes `BundleTrack/masks` - -- [Download NOCS Dataset](https://github.com/hughw19/NOCS_CVPR2019), then put it under a folder named "NOCS". [Download the converted ground-truth text pose files](https://archive.cs.rutgers.edu/archive/a/2021/pracsys/2021_iros_bundletrack/real_test_text.tar.gz). And [download the addon](https://drive.google.com/file/d/1BknMsoRKRV-nhLDpozog2Lrz1OWTKqkM/view?usp=sharing). Finally unzip the files and make sure the path structure is like this: - - ``` - NOCS - ├── NOCS-REAL275-additional - ├── real_test - ├── gts - | └── real_test_text - └── obj_models - ``` - -- [Download YCBInEOAT](https://archive.cs.rutgers.edu/archive/a/2020/pracsys/Bowen/iros2020/YCBInEOAT/) to make it like - ``` - YCBInEOAT - ├── bleach0 - | ├──annotated_poses - | ├──depth - | ├──depth_filled - | ├──gt_mask - | ├──masks - | ├──masks_vis - | ├──rgb - | ├──cam_K.txt - | ├──...... - └── .... - ``` - -- [Download YCB Objects](http://ycb-benchmarks.s3-website-us-east-1.amazonaws.com/) - -# Run predictions on NOCS -- Open a separate terminal and run - - ``` - bash lf-net-release/docker/run_container.sh - cd [PATH_TO_BUNDLETRACK] - cd lf-net-release && python run_server.py - ``` - - -- Go back to the terminal where you launched the bundletrack docker in above and run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/`. For more detailed logs, change `LOG` to 2 or higher in `config_nocs.yml`. - - ``` - python scripts/run_nocs.py --nocs_dir [PATH_TO_NOCS] --scene_id 1 --port 5555 --model_name can_arizona_tea_norm - ``` - -- Finally, the results will be saved in `/tmp/BundleTrack/` - - -- For evaluating on the entire NOCS Dataset, run (**NOTE that this will add noise to perturb the initial ground-truth pose for evaluation as explained in the paper. If you want to see how BundleTrack actually performs, run the above section**) - ``` - python scripts/eval_nocs.py --nocs_dir [PAHT TO NOCS] --results_dir [PATH TO THE RUNNING OUTPUTS] - ``` - - -# Run predictions on YCBInEOAT - -- Change the `model_name` and `model_dir` in `config_ycbineoat.yml` to the path to the .obj file (e.g. For folder `bleach0`, the model_name is `021_bleach_cleanser`, and model_dir is `[Your path to YCB Objects]/021_bleach_cleanser/textured_simple.obj`) - -- Open a separate terminal and run - - ``` - bash lf-net-release/docker/run_container.sh - cd [PATH_TO_BUNDLETRACK] - cd lf-net-release && python run_server.py - ``` - -- Go back to the terminal where you launched the bundletrack docker in above, and run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/` - - ``` - python scripts/run_ycbineoat.py --data_dir [PATH_TO_YCBInEOAT] --port 5555 --model_name [The YCB object's name, e.g. 021_bleach_cleanser] - ``` - -- Finally, the results will be saved in `/tmp/BundleTrack/`. For more detailed logs, change `LOG` to 2 or higher in `config_ycbineoat.yml`. - -- For evaluating on the entire YCBInEOAT Dataset, run - ``` - python scripts/eval_nocs.py --ycbineoat_dir [PAHT TO YCBINEOAT] --ycb_model_dir [YCB MODELS FOLDER] --results_dir [PATH TO THE RUN OUTPUTS] - ``` - - -# Run predictions on your own RGBD data - -- Download YCBInEOAT, if you haven't done so in above. - -- Open a separate terminal and run - - ``` - bash lf-net-release/docker/run_container.sh - cd [PATH_TO_BUNDLETRACK] - cd lf-net-release && python run_server.py - ``` - -- Prepare segmentation masks. In YCBInEOAT Dataset, we computed masks from robotic arm forward kinematics. If your scene is not too complicated similar to NOCS Dataset, you can run the video segmentation network to get masks as below: - - - First you need to prepare an initial mask (grayscale image, where 0 means background, else foreground). - - `python transductive-vos.pytorch/run_video.py --img_dir [THE PATH TO COLOR FILES] --init_mask_file [THE INITIAL MASK FILE YOU PREPARED ABOVE] --mask_save_dir [WHERE TO SAVE]` - - Prepare your folder structure same as any folder (e.g. "mustard_easy_00_02") in YCBInEOAT Dataset. Put it under the same directory in YCBInEOAT, i.e. next to "mustard_easy_00_02". Then edit `config_ycbineoat.yml` to make sure the paths at top are right. - - Structure: - ``` - mustard_easy_00_02 - ├── rgb - ├── masks - ├── depth - └── cam_K.txt - ``` - -- Go back to the terminal where you launched the bundletrack docker, run below. The output will be saved to `debug_dir` specified in config file. By default it's `/tmp/BundleTrack/` - ``` - python scripts/run_ycbineoat.py --data_dir [PATH TO YOUR FOLDER ABOVE] --port 5555 - ``` diff --git a/config_ycbineoat.yml b/config_ycbineoat.yml index 6e7ebfa..7cf7d70 100644 --- a/config_ycbineoat.yml +++ b/config_ycbineoat.yml @@ -3,7 +3,7 @@ mask_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/sugar_box1/gt model_name: 004_sugar_box model_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/models/004_sugar_box_berkeley_meshes/004_sugar_box/poisson/textured.obj debug_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/log -LOG: 0 +LOG: 1 port: '5556' depth_processing: diff --git a/r2d2 b/r2d2 index 73b5126..251ffed 160000 --- a/r2d2 +++ b/r2d2 @@ -1 +1 @@ -Subproject commit 73b5126c9575d1308c68894e25ea382b69985af9 +Subproject commit 251ffedd5ddce530560b44da4b5645e83db4a20d From c99c8b573e76c998688e8998d79ccc31e013c6d7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ckeskarshreesh=E2=80=9D?= Date: Sun, 19 Feb 2023 17:01:37 -0500 Subject: [PATCH 10/13] ycb run and vizualization changes --- config_ycbineoat.yml | 14 ++-- docker/run_container.sh | 6 +- scripts/run_ycbineoat.py | 18 +++-- src/Bundler.cpp | 24 +++++- src/DataLoader.cpp | 41 ++++++++++ src/DataLoader.h | 2 +- src/FeatureManager.cpp | 7 +- src/Frame.cpp | 3 + src/Frame.h | 1 + src/Utils.cpp | 115 ++++++++++++++++++++++++++++- src/Utils.h | 4 +- src/app/bundle_track_ycbineoat.cpp | 7 +- 12 files changed, 216 insertions(+), 26 deletions(-) diff --git a/config_ycbineoat.yml b/config_ycbineoat.yml index 7cf7d70..14b1a4b 100644 --- a/config_ycbineoat.yml +++ b/config_ycbineoat.yml @@ -1,10 +1,10 @@ -data_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/sugar_box1 -mask_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/sugar_box1/gt_mask -model_name: 004_sugar_box -model_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/data/models/004_sugar_box_berkeley_meshes/004_sugar_box/poisson/textured.obj -debug_dir: /home/robot-learning/Projects/ws_Haonan/BundleTrack/log -LOG: 1 -port: '5556' +data_dir: /home/shreesh/DATASET/YCBInEOAT/cracker_box_icg_1 +mask_dir: '/home/shreesh/DATASET/YCBInEOAT/cracker_box_icg_1/masks' +model_name: 003_cracker_box +model_dir: /home/shreesh/DATASET/YCBInEOAT/ycb/003_cracker_box/google_512k/textured.obj +debug_dir: /tmp/BundleTrack/ +LOG: 2 +port: '5555' depth_processing: erode: diff --git a/docker/run_container.sh b/docker/run_container.sh index 63cb279..3a14f66 100644 --- a/docker/run_container.sh +++ b/docker/run_container.sh @@ -1,6 +1,6 @@ -BUNDLETRACK_DIR="/home/bowen/debug/BundleTrack" -NOCS_DIR="/media/bowen/e25c9489-2f57-42dd-b076-021c59369fec/DATASET/NOCS" -YCBINEOAT_DIR="/iros_submission_version" +BUNDLETRACK_DIR="/home/shreesh/BundleTrack" +NOCS_DIR="/home/shreesh/DATASET/NOCS" +YCBINEOAT_DIR="/home/shreesh/DATASET/YCBInEOAT" echo "BUNDLETRACK_DIR $BUNDLETRACK_DIR" echo "NOCS_DIR $NOCS_DIR" echo "YCBINEOAT_DIR $YCBINEOAT_DIR" diff --git a/scripts/run_ycbineoat.py b/scripts/run_ycbineoat.py index 22d7375..82eb98e 100644 --- a/scripts/run_ycbineoat.py +++ b/scripts/run_ycbineoat.py @@ -46,18 +46,19 @@ pass -def run_one_video(data_dir,model_name,model_dir,cfg1,port): +def run_one_video(data_dir,model_name,model_dir,custom_yc,cfg1,port): cfg = copy.deepcopy(cfg1) - name = data_dir.split('/')[-2] + name = data_dir.split('/')[-1] cur_out_dir = '/tmp/BundleTrack/ycbineoat/{}/'.format(name) os.system(f'mkdir -p {cur_out_dir}') cfg['data_dir'] = data_dir - cfg['mask_dir'] = f'{code_dir}/masks/ycbineoat/{name}/masks' + cfg['mask_dir'] = f'/home/shreesh/DATASET/YCBInEOAT/{name}/masks' cfg['model_name'] = model_name cfg['model_dir'] = model_dir cfg['debug_dir'] = cur_out_dir + cfg['custom_yc'] = custom_yc cfg['LOG'] = 0 cfg['port'] = port tmp_config_dir = '/tmp/config_{}.yml'.format(name) @@ -76,10 +77,11 @@ def run_one_video(data_dir,model_name,model_dir,cfg1,port): if __name__=='__main__': parser = argparse.ArgumentParser() - parser.add_argument('--data_dir', type=str, default='/media/bowen/e25c9489-2f57-42dd-b076-021c59369fec/catkin_ws/src/iros20_dataset/video_rosbag/IROS_SELECTED/FINISHED_LABEL.iros_submission_version/bleach0') + parser.add_argument('--data_dir', type=str, default='/home/shreesh/DATASET/YCBInEOAT/cracker_box_icg_1') parser.add_argument('--port', type=int, default=5555) - parser.add_argument('--model_name', type=str, default='021_bleach_cleanser') - parser.add_argument('--model_dir', type=str, default='/media/bowen/e25c9489-2f57-42dd-b076-021c59369fec/DATASET/YCB_Video_Dataset/CADmodels/021_bleach_cleanser/textured.obj') + parser.add_argument('--model_name', type=str, default='003_cracker_box') + parser.add_argument('--model_dir', type=str, default='/home/shreesh/DATASET/YCBInEOAT/ycb/003_cracker_box/google_512k/textured.obj') + parser.add_argument('--custom_yc', type=str, default='false') args = parser.parse_args() @@ -89,8 +91,8 @@ def run_one_video(data_dir,model_name,model_dir,cfg1,port): raise RuntimeError(f"Make sure data_dir={data_dir} exists") code_dir = os.path.dirname(os.path.realpath(__file__)) - config_dir = f'{code_dir}/../config_ycbineoat.yml' + config_dir = '/home/shreesh/BundleTrack/config_ycbineoat.yml' with open(config_dir,'r') as ff: cfg = yaml.safe_load(ff) - run_one_video(args.data_dir,args.model_name,args.model_dir,cfg,args.port) + run_one_video(args.data_dir,args.model_name,args.model_dir,args.custom_yc,cfg,args.port) diff --git a/src/Bundler.cpp b/src/Bundler.cpp index c02fcea..53e8220 100644 --- a/src/Bundler.cpp +++ b/src/Bundler.cpp @@ -160,6 +160,10 @@ void Bundler::processNewFrame(std::shared_ptr frame) if (frame->_id==0) { + std::vector initialBBoxCenter = Utils::getBoundingBoxInitialCenterModelFree(frame->_depth,frame->_fg_mask,_data_loader->_K); + frame->_x_bbox_init = initialBBoxCenter[0]; + frame->_y_bbox_init = initialBBoxCenter[1]; + frame->_z_bbox_init = initialBBoxCenter[2]; checkAndAddKeyframe(frame); return; } @@ -395,12 +399,28 @@ void Bundler::saveNewframeResult() } } } - Utils::drawProjectPoints(cur_model,_data_loader->_K,color_viz); + + if (_newframe->_id==0) + { + std::vector initialBBoxCenter = Utils::getBoundingBoxInitialCenterModelFree(_newframe->_depth,_newframe->_fg_mask,_data_loader->_K); + _newframe->_x_bbox_init = initialBBoxCenter[0]; + _newframe->_y_bbox_init = initialBBoxCenter[1]; + _newframe->_z_bbox_init = initialBBoxCenter[2]; + } + + Utils::drawProjectPoints(cur_model,_data_loader->_K,color_viz,_newframe->_pose_in_model); + Utils::drawBBoxPCL(_newframe->_x_bbox_init,_newframe->_y_bbox_init,_newframe->_z_bbox_init,_data_loader->_K,color_viz,_newframe->_pose_in_model); cv::putText(color_viz,_newframe->_id_str,{5,30},cv::FONT_HERSHEY_PLAIN,2,{255,0,0},1,8,false); // cv::imshow("color_viz",color_viz); // cv::waitKey(1); cv::imwrite(debug_dir+"/color_viz/"+_newframe->_id_str+"_color_viz.jpg",color_viz,{cv::IMWRITE_JPEG_QUALITY, 80}); - cv::imwrite(out_dir+"color_viz.jpg",color_viz,{cv::IMWRITE_JPEG_QUALITY, 80}); + const std::string color_viz_out_dir = debug_dir+"color_viz/"; + if (!boost::filesystem::exists(color_viz_out_dir)) + { + system(std::string("mkdir -p "+color_viz_out_dir).c_str()); + } + + cv::imwrite(color_viz_out_dir+_newframe->_id_str+"_color_viz.jpg",color_viz,{CV_IMWRITE_JPEG_QUALITY, 80}); const std::string raw_dir = debug_dir+"/color_raw/"; if (!boost::filesystem::exists(raw_dir)) diff --git a/src/DataLoader.cpp b/src/DataLoader.cpp index 1135426..9445282 100644 --- a/src/DataLoader.cpp +++ b/src/DataLoader.cpp @@ -383,4 +383,45 @@ std::shared_ptr DataLoaderYcbineoat::next() return frame; } +std::shared_ptr DataLoaderYcbineoat::nextCustom() +{ + assert(_id<_color_files.size()); + const std::string data_dir = (*yml)["data_dir"].as(); + + std::string color_file = _color_files[_id]; + std::cout<<"color file: "< strs; + boost::split(strs, color_file, boost::is_any_of("/")); + boost::split(strs, strs.back(), boost::is_any_of(".")); + index_str = strs[0]; + } + + cv::Mat depth_raw; + std::string depth_dir = data_dir+"/depth/"+index_str+".depth.png"; + Utils::readDepthImage(depth_raw, depth_dir); + + cv::Mat depth_sim; + depth_sim = depth_raw.clone(); + + cv::Mat depth; + depth = depth_raw.clone(); + + Eigen::Matrix4f pose(Eigen::Matrix4f::Identity()); + if (_id==0) + { + pose = _ob_in_cam0.inverse(); + } + + Eigen::Vector4f roi; + roi << 99999,0,99999,0; + + std::shared_ptr frame(new Frame(color,depth,depth_raw,depth_sim, roi, pose, _id, index_str+".seg", _K, yml, NULL, _real_model)); + _id++; + + return frame; +} + diff --git a/src/DataLoader.h b/src/DataLoader.h index 7869a21..a8ffda7 100644 --- a/src/DataLoader.h +++ b/src/DataLoader.h @@ -87,7 +87,7 @@ class DataLoaderYcbineoat : public DataLoaderBase DataLoaderYcbineoat(std::shared_ptr yml1); ~DataLoaderYcbineoat(); std::shared_ptr next(); - + std::shared_ptr nextCustom(); }; diff --git a/src/FeatureManager.cpp b/src/FeatureManager.cpp index 8784174..a335ee4 100644 --- a/src/FeatureManager.cpp +++ b/src/FeatureManager.cpp @@ -760,7 +760,12 @@ int SiftManager::countInlierCorres(std::shared_ptr frameA, std::shared_pt void SiftManager::vizCorresBetween(std::shared_ptr frameA, std::shared_ptr frameB, const std::string &name) { if ((*yml)["LOG"].as()<1) return; - const std::string out_dir = (*yml)["debug_dir"].as()+"/"+_bundler->_newframe->_id_str+"/"; + const std::string out_dir = (*yml)["debug_dir"].as()+"/match_viz/"; + // std::cout<_id_str+"_match_"+frameB->_id_str+"_"+name+".jpg"; std::ifstream tmp(out_match_file); cv::Mat colorA = frameA->_color.clone(); diff --git a/src/Frame.cpp b/src/Frame.cpp index d888647..23c2c1e 100644 --- a/src/Frame.cpp +++ b/src/Frame.cpp @@ -63,6 +63,9 @@ Frame::Frame(const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depth_ra _real_model = real_model; _pose_inited = false; _roi = roi; + _x_bbox_init = 0; + _y_bbox_init = 0; + _z_bbox_init = 0; const int n_pixels = _H*_W; cudaMalloc(&_depth_gpu, n_pixels*sizeof(float)); diff --git a/src/Frame.h b/src/Frame.h index 1663ab2..c26302f 100644 --- a/src/Frame.h +++ b/src/Frame.h @@ -69,6 +69,7 @@ class Frame Status _status; bool _pose_inited; std::map, std::shared_ptr> _map_points; + float _x_bbox_init,_y_bbox_init,_z_bbox_init; float *_depth_gpu; uchar4 *_color_gpu; diff --git a/src/Utils.cpp b/src/Utils.cpp index 2b31dca..8178b00 100644 --- a/src/Utils.cpp +++ b/src/Utils.cpp @@ -111,6 +111,39 @@ void convert3dOrganizedRGB(cv::Mat &objDepth, cv::Mat &colImage, Eigen::Matrix3f } } } + + +std::vector getBoundingBoxInitialCenterModelFree(cv::Mat &objDepth, cv::Mat &fgMask, Eigen::Matrix3f &camIntrinsic) +{ + const int imgWidth = objDepth.cols; + const int imgHeight = objDepth.rows; + + float x_mean = 0; + float y_mean = 0; + float z_mean = 0; + int num_points = 0; + + for(int u = 0; u < imgHeight; u++) + { + for(int v = 0; v < imgWidth; v++) + { + float depth = objDepth.at(u,v); + if(fgMask.at(u,v) == 0 || depth <= 0.1 || depth >= 2.0) + continue; + x_mean += (float)((v - camIntrinsic(0,2)) * depth / camIntrinsic(0,0)); + y_mean += (float)((u - camIntrinsic(1,2)) * depth / camIntrinsic(1,1)); + z_mean += depth; + } + } + + x_mean /= (float)num_points; + y_mean /= (float)num_points; + z_mean /= (float)num_points; + + int gt_mean[3] = {x_mean,y_mean,z_mean}; + return std::vector(gt_mean,gt_mean+3); +} + template void convert3dOrganizedRGB(cv::Mat &objDepth, cv::Mat &colImage, Eigen::Matrix3f &camIntrinsic, boost::shared_ptr> objCloud); template void convert3dOrganizedRGB(cv::Mat &objDepth, cv::Mat &colImage, Eigen::Matrix3f &camIntrinsic, boost::shared_ptr> objCloud); @@ -218,7 +251,31 @@ void solveRigidTransformBetweenPoints(const Eigen::MatrixXf &points1, const Eige } -void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, cv::Mat &out) +std::pair getProjectPointCoord3Dto2D(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out) { + + int u = std::round(x*K(0,0)/z + K(0,2)); + int v = std::round(y*K(1,1)/z + K(1,2)); + if(u < 0) + u = 0; + else if(u >= out.cols) + u = (out.cols - 1); + if(v < 0) + v = 0; + else if(v >= out.rows) + v = (out.rows - 1); + + cv::circle(out, {u,v}, 1, {0,255,255}, -1); + + return std::pair(u,v); +} + +void drawLine(cv::Mat &out, int x1, int y1, int x2, int y2, cv::Scalar color) { + cv::Point p1(x1,y1); + cv::Point p2(x2,y2); + cv::line(out,p1,p2,color,2); +} + +void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose) { Utils::downsamplePointCloud(cloud,cloud,0.01); for (const auto &pt:cloud->points) @@ -230,6 +287,62 @@ void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, } } +std::vector getMeanCoordinatesFromPCLCloud(PointCloudRGBNormal::Ptr initialCloud) +{ + float x_mean = 0; + float y_mean = 0; + float z_mean = 0; + float x_sum = 0; + float y_sum = 0; + float z_sum = 0; + int num_points = 0; + for (const auto &pt:initialCloud->points) + { + x_sum += pt.x; + y_sum += pt.y; + z_sum += pt.z; + num_points++; + } + + x_mean = x_sum/num_points; + y_mean = y_sum/num_points; + z_mean = z_sum/num_points; + + float gt_mean[] = {x_mean,y_mean,z_mean}; + return std::vector(gt_mean,gt_mean+3); +} + +void drawBBoxPCL(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose) +{ + + pcl::PointCloud bbox; + float x_adds[2] = {-0.05,0.05}; + float y_adds[2] = {-0.05,0.05}; + float z_adds[2] = {-0.05,0.05}; + for(auto y_add : y_adds) + for(auto x_add : x_adds) + for(auto z_add : z_adds) + bbox.push_back(pcl::PointXYZ(x + x_add,y + y_add,z + z_add)); + + pcl::PointCloud bboxTrans; + pcl::transformPointCloud(bbox,bboxTrans,pose.inverse()); + + std::pair p[8]; + int idx = 0; + for(const auto &pt : bboxTrans.points) + { + p[idx] = getProjectPointCoord3Dto2D(pt.x,pt.y,pt.z,K,out); + idx++; + } + + int lineStartingPoints[12] = {0,0,0,1,1,2,2,3,4,4,5,6}; + int lineEndingPoints[12] = {1,2,4,3,5,3,6,7,5,6,7,7}; + + for(int i = 0; i < 12; i++) + drawLine(out,p[lineStartingPoints[i]].first,p[lineStartingPoints[i]].second,p[lineEndingPoints[i]].first,p[lineEndingPoints[i]].second,cv::Scalar(0,255,0)); +} + + } // namespace Utils diff --git a/src/Utils.h b/src/Utils.h index 1e0f8f5..a2e2e66 100755 --- a/src/Utils.h +++ b/src/Utils.h @@ -179,7 +179,9 @@ bool isPixelInsideImage(const int H, const int W, float u, float v); void solveRigidTransformBetweenPoints(const Eigen::MatrixXf &points1, const Eigen::MatrixXf &points2, Eigen::Matrix4f &pose); -void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, cv::Mat &out); +void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose); +std::vector getMeanCoordinatesFromPCLCloud(PointCloudRGBNormal::Ptr initialCloud); +void drawBBoxPCL(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose); template void parseMatrixTxt(std::string filename, Eigen::Matrix &out) diff --git a/src/app/bundle_track_ycbineoat.cpp b/src/app/bundle_track_ycbineoat.cpp index cf96249..8e240de 100644 --- a/src/app/bundle_track_ycbineoat.cpp +++ b/src/app/bundle_track_ycbineoat.cpp @@ -63,12 +63,15 @@ int main(int argc, char **argv) Bundler bundler(yml,&data_loader); + const bool custom_yc = (*yml)["custom_yc"].as(); + while (data_loader.hasNext()) { - std::shared_ptr frame = data_loader.next(); + std::shared_ptr frame = (custom_yc) ? data_loader.nextCustom() : data_loader.next(); if (!frame) break; const std::string index_str = frame->_id_str; - const std::string out_dir = (*yml)["debug_dir"].as()+"/"+index_str+"/"; + const std::string out_dir = (*yml)["debug_dir"].as(); + cv::imwrite(out_dir+index_str+"_color.png",frame->_color); Eigen::Matrix4f cur_in_model(data_loader._ob_in_cam0.inverse()); From af07c9ced9399af6e93d2d8acd89448e7eec2925 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ckeskarshreesh=E2=80=9D?= Date: Sun, 19 Feb 2023 17:14:15 -0500 Subject: [PATCH 11/13] removing vscode files --- .vscode/launch.json | 47 -------------- .vscode/settings.json | 147 ------------------------------------------ 2 files changed, 194 deletions(-) delete mode 100644 .vscode/launch.json delete mode 100644 .vscode/settings.json diff --git a/.vscode/launch.json b/.vscode/launch.json deleted file mode 100644 index 6f63143..0000000 --- a/.vscode/launch.json +++ /dev/null @@ -1,47 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - { - "name": "r2d2 (server)", - "type": "python", - "request": "launch", - "program": "${workspaceFolder}/r2d2/run_server.py", - "args": [ - "--port=5556", - "--model=models/r2d2_WASF_N16.pt" - ], - "console": "integratedTerminal", - "justMyCode": true, - "cwd": "${workspaceFolder}/r2d2" - }, - { - "name": "bundle_track_ycb", - "type": "cppdbg", - "request": "launch", - "program": "${workspaceFolder}/build/bundle_track_ycbineoat", - "args": [ - "${workspaceFolder}/config_ycbineoat.yml", - ], - "stopAtEntry": false, - "cwd": "${fileDirname}", - "environment": [], - "externalConsole": false, - "MIMode": "gdb", - "setupCommands": [ - { - "description": "Enable pretty-printing for gdb", - "text": "-enable-pretty-printing", - "ignoreFailures": true - }, - { - "description": "Set Disassembly Flavor to Intel", - "text": "-gdb-set disassembly-flavor intel", - "ignoreFailures": true - } - ] - }, - ] -} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index d1983e2..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,147 +0,0 @@ -{ - "files.associations": { - "*.cu": "cpp", - "*.cuh": "cpp", - "*.vert": "cpp", - "*.frag": "cpp", - "*.glsl": "cpp", - "*.geom": "cpp", - "*.urdf": "xml", - "*.xacro": "xml", - "*.dae": "xml", - "*.pyx": "python", - "*.pxd": "cpp", - "*.srdf": "xml", - "*.sdf": "xml", - "array": "cpp", - "bitset": "cpp", - "string_view": "cpp", - "initializer_list": "cpp", - "regex": "cpp", - "utility": "cpp", - "limits": "cpp", - "atomic": "cpp", - "*.tcc": "cpp", - "cctype": "cpp", - "cmath": "cpp", - "complex": "cpp", - "cstddef": "cpp", - "cstdint": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cwchar": "cpp", - "deque": "cpp", - "list": "cpp", - "unordered_map": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "memory": "cpp", - "new": "cpp", - "ostream": "cpp", - "numeric": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cinttypes": "cpp", - "typeinfo": "cpp", - "iterator": "cpp", - "map": "cpp", - "memory_resource": "cpp", - "random": "cpp", - "string": "cpp", - "core": "cpp", - "numericaldiff": "cpp", - "graphicscontext": "cpp", - "quat": "cpp", - "vec2b": "cpp", - "vec2d": "cpp", - "vec2f": "cpp", - "vec2i": "cpp", - "vec2s": "cpp", - "vec2ub": "cpp", - "vec2ui": "cpp", - "vec2us": "cpp", - "vec3b": "cpp", - "vec3d": "cpp", - "vec3f": "cpp", - "vec3i": "cpp", - "vec3s": "cpp", - "vec3ub": "cpp", - "vec3ui": "cpp", - "vec3us": "cpp", - "vec4b": "cpp", - "vec4d": "cpp", - "vec4f": "cpp", - "vec4i": "cpp", - "vec4s": "cpp", - "vec4ui": "cpp", - "vec4us": "cpp", - "viewport": "cpp", - "*.ipp": "cpp", - "camera": "cpp", - "plane": "cpp", - "polytope": "cpp", - "export": "cpp", - "optional": "cpp", - "variant": "cpp", - "mixinvector": "cpp", - "fast_back_stack": "cpp", - "nonlinearoptimization": "cpp", - "hash_map": "cpp", - "slist": "cpp", - "valarray": "cpp", - "bufferobject": "cpp", - "image": "cpp", - "texture": "cpp", - "buffered_value": "cpp", - "clocale": "cpp", - "csetjmp": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwctype": "cpp", - "any": "cpp", - "strstream": "cpp", - "barrier": "cpp", - "bit": "cpp", - "cfenv": "cpp", - "charconv": "cpp", - "chrono": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "coroutine": "cpp", - "cuchar": "cpp", - "forward_list": "cpp", - "set": "cpp", - "unordered_set": "cpp", - "ratio": "cpp", - "source_location": "cpp", - "system_error": "cpp", - "fstream": "cpp", - "future": "cpp", - "iomanip": "cpp", - "latch": "cpp", - "mutex": "cpp", - "numbers": "cpp", - "ranges": "cpp", - "scoped_allocator": "cpp", - "semaphore": "cpp", - "shared_mutex": "cpp", - "span": "cpp", - "sstream": "cpp", - "stop_token": "cpp", - "syncstream": "cpp", - "typeindex": "cpp", - "__nullptr": "cpp" - } -} \ No newline at end of file From e76f6e14acd74a3cb490f50d4698881ecc865adf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ckeskarshreesh=E2=80=9D?= Date: Sun, 19 Feb 2023 17:30:15 -0500 Subject: [PATCH 12/13] added bbox method definition --- src/Utils.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/Utils.h b/src/Utils.h index a2e2e66..f76b6bd 100755 --- a/src/Utils.h +++ b/src/Utils.h @@ -181,6 +181,7 @@ bool isPixelInsideImage(const int H, const int W, float u, float v); void solveRigidTransformBetweenPoints(const Eigen::MatrixXf &points1, const Eigen::MatrixXf &points2, Eigen::Matrix4f &pose); void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose); std::vector getMeanCoordinatesFromPCLCloud(PointCloudRGBNormal::Ptr initialCloud); +std::vector getBoundingBoxInitialCenterModelFree(cv::Mat &objDepth, cv::Mat &fgMask, Eigen::Matrix3f &camIntrinsic); void drawBBoxPCL(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose); template From 8697eec8f353ed8d6664092f5dd2041d0f89824c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=9Ckeskarshreesh=E2=80=9D?= Date: Sat, 25 Feb 2023 16:36:45 -0500 Subject: [PATCH 13/13] Bounding Box Utility --- src/Utils.cpp | 39 +++++++++++++++++++++++++++++++++++++++ src/Utils.h | 1 + 2 files changed, 40 insertions(+) diff --git a/src/Utils.cpp b/src/Utils.cpp index 8178b00..ae2e4ef 100644 --- a/src/Utils.cpp +++ b/src/Utils.cpp @@ -342,6 +342,45 @@ void drawBBoxPCL(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &o drawLine(out,p[lineStartingPoints[i]].first,p[lineStartingPoints[i]].second,p[lineEndingPoints[i]].first,p[lineEndingPoints[i]].second,cv::Scalar(0,255,0)); } +void drawBBox(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose) +{ + // pcl::PointCloud bbox; + Eigen::MatrixXf bbox = Eigen::MatrixXf::Random(4,9); + int pointIdx = 0; + float x_adds[2] = {-0.05,0.05}; + float y_adds[2] = {-0.05,0.05}; + float z_adds[2] = {-0.05,0.05}; + for(auto y_add : y_adds) + for(auto x_add : x_adds) + for(auto z_add : z_adds) + { + bbox(0,pointIdx) = x + x_add; + bbox(1,pointIdx) = y + y_add; + bbox(2,pointIdx) = z + z_add; + bbox(3,pointIdx) = 1; + pointIdx++; + } + + bbox(0,8) = x; + bbox(1,8) = y; + bbox(2,8) = z; + + // pcl::PointCloud bboxTrans; + // pcl::transformPointCloud(bbox,bboxTrans,pose.inverse()); + Eigen::MatrixXf bboxTrans = pose*bbox; + + std::pair p[9]; + for(int idx = 0; idx < 9; idx++) + p[idx] = getProjectPointCoord3Dto2D(bboxTrans(0,idx),bboxTrans(1,idx),bboxTrans(2,idx),K,out); + + cv::circle(out, {p[8].first,p[8].second}, 4, {0,0,255}, -1); + + int lineStartingPoints[12] = {0,0,0,1,1,2,2,3,4,4,5,6}; + int lineEndingPoints[12] = {1,2,4,3,5,3,6,7,5,6,7,7}; + + for(int i = 0; i < 12; i++) + drawLine(out,p[lineStartingPoints[i]].first,p[lineStartingPoints[i]].second,p[lineEndingPoints[i]].first,p[lineEndingPoints[i]].second,cv::Scalar(0,255,0)); +} } // namespace Utils diff --git a/src/Utils.h b/src/Utils.h index f76b6bd..79a62b1 100755 --- a/src/Utils.h +++ b/src/Utils.h @@ -183,6 +183,7 @@ void drawProjectPoints(PointCloudRGBNormal::Ptr cloud, const Eigen::Matrix3f &K, std::vector getMeanCoordinatesFromPCLCloud(PointCloudRGBNormal::Ptr initialCloud); std::vector getBoundingBoxInitialCenterModelFree(cv::Mat &objDepth, cv::Mat &fgMask, Eigen::Matrix3f &camIntrinsic); void drawBBoxPCL(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose); +void drawBBox(float x, float y, float z, const Eigen::Matrix3f &K, cv::Mat &out, const Eigen::Matrix4f &pose); template void parseMatrixTxt(std::string filename, Eigen::Matrix &out)