Skip to content

Commit

Permalink
[DOCS] update documentation and RUN.md instructions to clarify how to…
Browse files Browse the repository at this point in the history 
… run trackdlo on provided evaluation .bag files
  • Loading branch information
@hollydinkel
hollydinkel committed Nov 19, 2023
1 parent 5e1b574 commit 252deaf
Show file tree
Hide file tree
Showing 4 changed files with 106 additions and 16 deletions.
24 changes: 16 additions & 8 deletions docs/RUN.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,15 @@ The repository is organized into the following directories:
└── utils/ # contains scripts used for testing and evaluation
```

First, [create a ROS workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). Next, `cd YOUR_ROS_WORKSPACE/src`. Clone the TrackDLO repository into this workspace and build the package:
First, [create a ROS workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace). Next, `cd YOUR_TRACKING_ROS_WORKSPACE/src`. Clone the TrackDLO repository into this workspace and build the package:

```bash
git clone https://github.com/RMDLO/trackdlo.git
catkin build trackdlo
source ../devel/setup.bash
```

All configurable parameters for the TrackDLO algorithm are in [`launch/trackdlo.launch`](https://github.com/RMDLO/trackdlo/blob/master/launch/trackdlo.launch). Rebuilding the package is not required for any parameter modifications to take effect. However, `catkin build` is required after modifying any C++ files.
All configurable parameters for the TrackDLO algorithm are in [`launch/trackdlo.launch`](https://github.com/RMDLO/trackdlo/blob/master/launch/trackdlo.launch). Rebuilding the package is not required for any parameter modifications to take effect. However, `catkin build` is required after modifying any C++ files. Remember that `source <YOUR_TRACKING_ROS_WS>/devel/setup.bash` is required in every terminal running TrackDLO ROS nodes.

## Usage

Expand Down Expand Up @@ -88,18 +88,26 @@ roslaunch trackdlo trackdlo.launch
```

## Run TrackDLO with Recorded ROS Bag Data:
1. Download the experiment data from [here](https://drive.google.com/file/d/1C7uM515fHXnbsEyx5X38xZUXzBI99mxg/view?usp=drive_link). After unzipping, place the `.bag` files in your ROS workspace. Note: the files are quite large! After unzipping, the bag files will take up around 120 GB of space in total.
1. Download the experiment data from [here](https://drive.google.com/file/d/1C7uM515fHXnbsEyx5X38xZUXzBI99mxg/view?usp=drive_link). After unzipping, place the `.bag` files in your ROS workspace. Note: the files are quite large! After unzipping, the bag files will require 120 GB of space in total.
2. Open a new terminal and run
```bash
roslaunch trackdlo visualize_output.launch
roslaunch trackdlo visualize_output.launch bag:=True
```
3. In another terminal, run the below command to start the tracking algorithm with parameters for the `stationary.bag`, `perpendicular_motion.bag`, and `parallel_motion.bag` files used for quantitative evaluation in the TrackDLO paper.
```bash
roslaunch trackdlo trackdlo_eval.launch
```
3. In another terminal, run the below command to start the tracking algorithm:
If testing any of the other provided `.bag` files, run the below command:
```bash
roslaunch trackdlo trackdlo.launch
```
4. Open a third ternimal and run the below command to replay the `.bag` file and publish its topics:
```bash
rosbag play <name_of_the_bag_file>.bag
rosbag play --clock <name_of_the_bag_file>.bag
```
Occlusion can also be injected using our provided `simulate_occlusion_eval.py` script. Run the below command and draw bounding rectangles for the occlusion mask in the graphical user interface that appears:
```bash
rosrun trackdlo simulate_occlusion_eval.py
```

## Data:
Expand All @@ -115,5 +123,5 @@ The ROS bag files used in our paper and the supplementary video can be found [he

### Notes on Running the Bag Files

* The rope and the rubber tubing require different hsv thresholding values. Both of them have hsv upper limit of `130 255 255`, however the rope has hsv lower limit `90 90 30` and the tubing has hsv lower limit `100 200 60`.
* For bag files in `rope/`, `rubber_tubing/`, and `failure_cases/`, the camera info is published under topic `/camera/aligned_depth_to_color/camera_info`. For bag files in `quantitative_eval/`, the camera info is published under `/camera/color/camera_info`.
* The rope and the rubber tubing require different hsv thresholding values. Both of these objects use the `hsv_threshold_upper_limit` default value = `130 255 255` however the rope uses the `hsv_threshold_lower_limit` default value = `90 90 30` and the rubber tubing uses the `hsv_threshold_upper_limit` default value = `100 200 60`.
* For `.bag` files in `rope/`, `rubber_tubing/`, and `failure_cases/`, the `camera_info_topic` is published under `/camera/aligned_depth_to_color/camera_info`. For `.bag` files in `quantitative_eval/`, the `camera_info_topic` is published under `/camera/color/camera_info`.
78 changes: 78 additions & 0 deletions launch/trackdlo_eval.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,78 @@
<launch>
<!-- change the below parameters to match your camera/scene setup -->
<arg name="camera_info_topic" default="/camera/color/camera_info" />
<!-- <arg name="camera_info_topic" default="/camera/aligned_depth_to_color/camera_info" /> -->
<arg name="rgb_topic" default="/camera/color/image_raw" />
<arg name="depth_topic" default="/camera/aligned_depth_to_color/image_raw" />
<arg name="result_frame_id" default="camera_color_optical_frame" />
<arg name="hsv_threshold_upper_limit" default="130 255 255" />
<!-- <arg name="hsv_threshold_lower_limit" default="100 200 60" /> -->
<arg name="hsv_threshold_lower_limit" default="90 90 30" />
<!-- <arg name="hsv_threshold_lower_limit" default="90 90 90" /> -->
<arg name="num_of_nodes" default="40" />
<arg name="visualize_initialization_process" default="false" />
<arg name="multi_color_dlo" default="true" />

<!-- load parameters to corresponding nodes -->
<node name="trackdlo" pkg="trackdlo" type="trackdlo" output="screen">
<param name="camera_info_topic" type="string" value="$(arg camera_info_topic)" />
<param name="rgb_topic" type="string" value="$(arg rgb_topic)" />
<param name="depth_topic" type="string" value="$(arg depth_topic)" />
<param name="result_frame_id" type="string" value="$(arg result_frame_id)" />

<param name="hsv_threshold_upper_limit" type="string" value="$(arg hsv_threshold_upper_limit)" />
<param name="hsv_threshold_lower_limit" type="string" value="$(arg hsv_threshold_lower_limit)" />

<!-- beta and lambda: MCT weights. the larger they are, the more rigid the object becomes -->
<param name="beta" value="0.5" />
<param name="lambda" value="50000" />

<!-- alpha: the alignment strength -->
<param name="alpha" value="3" />

<!-- mu: ranges from 0 to 1, large mu indicates the point cloud is noisy -->
<param name="mu" value="0.1" />

<!-- max_iter: the maximum number of iterations the EM loop undergoes before termination -->
<param name="max_iter" value="50" />

<!-- tol: EM optimization convergence tolerance -->
<param name="tol" value="0.0002" />

<!-- k_vis: the strength of visibility information's effect on membership probability computation -->
<param name="k_vis" value="500" />

<!-- d_vis: the max geodesic distance between two adjacent visible nodes for the nodes between them to be considered visible -->
<param name="d_vis" value="0.06" />

<!-- visibility_threshold (tau_vis): the max distance a node can be away from the current point cloud to be considered visible -->
<param name="visibility_threshold" type="double" value="0.005" />

<!-- dlo_pixel_width (w): the approximate dlo width when projected onto 2D -->
<param name="dlo_pixel_width" value="30" />

<!-- below are parameters for the GLTP registration during pre-processing -->
<param name="beta_pre_proc" value="3.0" />
<param name="lambda_pre_proc" value="1.0" />
<param name="lle_weight" value="10.0" />

<param name="downsample_leaf_size" value="0.005" />
<param name="multi_color_dlo" type="bool" value="$(arg multi_color_dlo)" />
</node>

<!-- launch python node for initialization -->
<node name="init_tracker" pkg="trackdlo" type="initialize.py" output="screen">
<param name="camera_info_topic" type="string" value="$(arg camera_info_topic)" />
<param name="rgb_topic" type="string" value="$(arg rgb_topic)" />
<param name="depth_topic" type="string" value="$(arg depth_topic)" />
<param name="result_frame_id" type="string" value="$(arg result_frame_id)" />

<param name="num_of_nodes" value="$(arg num_of_nodes)" />
<param name="multi_color_dlo" type="bool" value="$(arg multi_color_dlo)" />
<param name="visualize_initialization_process" type="bool" value="$(arg visualize_initialization_process)" />

<param name="hsv_threshold_upper_limit" type="string" value="$(arg hsv_threshold_upper_limit)" />
<param name="hsv_threshold_lower_limit" type="string" value="$(arg hsv_threshold_lower_limit)" />
</node>

</launch>
16 changes: 10 additions & 6 deletions launch/visualize_output.launch
View file
Original file line number Diff line number Diff line change
@@ -1,12 +1,16 @@
<launch>
<arg name="eval" default="false"/>
<arg name="bag" default="false"/>

<!-- Publish camera tfs if using the example .bag files-->
<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera_color_optical_frame_tf" args="0.5308947503950723 0.030109485611943067 0.5874 -0.7071068 0.7071068 0 0 base_link camera_color_optical_frame 10" />
<node pkg="tf" type="static_transform_publisher" name="camera_color_optical_frame_to_camera_color_frame_tf" args="0 0 0 0.5 -0.5 0.5 0.5 camera_color_optical_frame camera_color_frame 10" />
<node pkg="tf" type="static_transform_publisher" name="camera_color_frame_to_camera_link_tf" args="-0.000351057737134 -0.0148385819048 -0.000117231989861 0.00429561594501 0.000667857821099 -0.00226634810679 0.999987959862 camera_color_optical_frame camera_color_frame 10" />
<node pkg="tf" type="static_transform_publisher" name="camera_link_to_camera_depth_frame_tf" args="0 0 0 0 0.0 0.0 1.0 camera_color_optical_frame camera_color_frame 10" />
<node pkg="tf" type="static_transform_publisher" name="camera_depth_frame_to_camera_depth_optical_frame_tf" args="0 0 0 -0.5 0.5 -0.5 0.5 camera_depth_frame camera_depth_optical_frame 10" />
<group if="$(arg bag)">
<param name="use_sim_time" type="bool" value="true" />
<!-- Publish camera tfs if using the example .bag files-->
<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera_color_optical_frame_tf" args="0.5308947503950723 0.030109485611943067 0.5874 -0.7071068 0.7071068 0 0 base_link camera_color_optical_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_color_optical_frame_to_camera_color_frame_tf" args="0 0 0 0.5 -0.5 0.5 0.5 camera_color_optical_frame camera_color_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_color_frame_to_camera_link_tf" args="-0.000351057737134 -0.0148385819048 -0.000117231989861 0.00429561594501 0.000667857821099 -0.00226634810679 0.999987959862 camera_color_frame camera_link 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_link_to_camera_depth_frame_tf" args="0 0 0 0 0.0 0.0 1.0 camera_link camera_depth_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_depth_frame_to_camera_depth_optical_frame_tf" args="0 0 0 -0.5 0.5 -0.5 0.5 camera_depth_frame camera_depth_optical_frame 100" />
</group>

<!-- <node name="tracking_method_ours" pkg="trackdlo" type="trackdlo_node"/> -->

Expand Down
4 changes: 2 additions & 2 deletions trackdlo/src/initialize.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ def color_thresholding (hsv_image, cur_depth):
mask = cv2.bitwise_or(mask_marker.copy(), mask_dlo.copy())

# filter mask base on depth values
mask[cur_depth < 0.58*1000] = 0
mask[cur_depth < 0.59*1000] = 0

return mask

Expand Down Expand Up @@ -100,7 +100,7 @@ def callback (rgb, depth):
extracted_chains_3d = extracted_chains_3d[((extracted_chains_3d[:, 0] != 0) | (extracted_chains_3d[:, 1] != 0) | (extracted_chains_3d[:, 2] != 0))]

if multi_color_dlo:
depth_threshold = 0.58 # m
depth_threshold = 0.59 # m
extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold]

# tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.001)
Expand Down

0 comments on commit 252deaf

Please sign in to comment.