diff --git a/docs/COLOR_THRESHOLD.md b/docs/COLOR_THRESHOLD.md new file mode 100644 index 0000000..e69de29 diff --git a/docs/RUN.md b/docs/RUN.md index c796a43..2c2b81e 100644 --- a/docs/RUN.md +++ b/docs/RUN.md @@ -16,13 +16,11 @@ Installation and execution of TrackDLO was verified with the below dependencies * [Pillow](https://pillow.readthedocs.io/en/stable/installation.html) (Our version: 9.2.0) * [ROS Numpy](https://pypi.org/project/rosnumpy/) (Our version: 0.0.5) -We also provide Docker files for compatibility with other system configurations, refer to the ![DOCKER.md](https://github.com/RMDLO/trackdlo/blob/master/docs/DOCKER.md) for more information. +We also provide Docker files for compatibility with other system configurations, refer to [DOCKER.md](https://github.com/RMDLO/trackdlo/blob/master/docs/DOCKER.md) for more information. ## Other Requirements -We used an Intel RealSense d435 camera in all of the experiments performed in our paper. - -* [librealsense](https://github.com/IntelRealSense/librealsense) and [realsense-ros](https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy) (for testing with the RealSense D435 camera and the corresponding camera configuration file we provided) +We used an Intel RealSense d435 camera in all of the experiments performed in our paper. We used the [librealsense](https://github.com/IntelRealSense/librealsense) and [realsense-ros](https://github.com/IntelRealSense/realsense-ros/tree/ros1-legacy) packages for testing with the RealSense D435 camera and for obtaining the [camera configuration file](https://github.com/RMDLO/trackdlo/blob/master/config/preset_decimation_4.0_depth_step_100.json). ## Installation @@ -55,9 +53,7 @@ To run TrackDLO, the three ROS topic names below should be modified in `launch/t **Note: TrackDLO assumes the RGB image and its corresponding depth image are ALIGNED and SYNCHRONIZED. This means depth_image(i, j) should contain the depth value of pixel rgb_image(i, j) and the two images should be published with the same ROS timestamp.** -TrackDLO uses color thresholding to obtain the DLO segmentation mask. Below are two different ways to set the color thresholding parameters: -* If the DLO of interest only has one color: you can use the parameters `hsv_threshold_upper/lower_limit` and set their values with format `h_value s_value v_value` (`h_values_valuev_value`). -* If the DLO of interest has multiple colors: set `multi_color_dlo` to `true` in `launch/trackdlo.launch`, then you can modify the function `color_thresholding` in `trackdlo/src/initialize.py` and `trackdlo/src/trackdlo_node.cpp` to customize the DLO segmentation process. +TrackDLO uses color thresholding in Hue, Saturation, and Value (HSV) color space to obtain the DLO segmentation mask. A tutorial on how to obtain the HSV limits is provided in [`COLOR_THRESHOLD.md`](https://github.com/RMDLO/trackdlo/blob/master/docs/COLOR_THRESHOLD.md) Other useful parameters: * `num_of_nodes`: the number of nodes initialized for the DLO diff --git a/images/rope.png b/images/rope.png new file mode 100644 index 0000000..2c92e3e Binary files /dev/null and b/images/rope.png differ diff --git a/images/thresholder.png b/images/thresholder.png new file mode 100644 index 0000000..0e8a3d1 Binary files /dev/null and b/images/thresholder.png differ diff --git a/utils/color_picker.py b/utils/color_picker.py new file mode 100644 index 0000000..9b555b3 --- /dev/null +++ b/utils/color_picker.py @@ -0,0 +1,76 @@ +import cv2 +import argparse +import numpy as np + +def nothing(x): + pass + + +parser = argparse.ArgumentParser(description="Pick HSV color threshold") +parser.add_argument("path", help="Indicate /full-or-relative/path/to/image_file.png") +args = parser.parse_args() +img_path = f"{args.path}" + +# Create a window +cv2.namedWindow('image') + +# create trackbars for color change +cv2.createTrackbar('HMin','image',0,179,nothing) # Hue is from 0-179 for Opencv +cv2.createTrackbar('SMin','image',0,255,nothing) +cv2.createTrackbar('VMin','image',0,255,nothing) +cv2.createTrackbar('HMax','image',0,179,nothing) +cv2.createTrackbar('SMax','image',0,255,nothing) +cv2.createTrackbar('VMax','image',0,255,nothing) + +# Set default value for MAX HSV trackbars. +cv2.setTrackbarPos('HMax', 'image', 179) +cv2.setTrackbarPos('SMax', 'image', 255) +cv2.setTrackbarPos('VMax', 'image', 255) + +# Initialize to check if HSV min/max value changes +hMin = sMin = vMin = hMax = sMax = vMax = 0 +phMin = psMin = pvMin = phMax = psMax = pvMax = 0 + +img = cv2.imread(img_path) +img = cv2.resize(img, (640, 480)) +output = img +waitTime = 33 + +while(1): + + # get current positions of all trackbars + hMin = cv2.getTrackbarPos('HMin','image') + sMin = cv2.getTrackbarPos('SMin','image') + vMin = cv2.getTrackbarPos('VMin','image') + + hMax = cv2.getTrackbarPos('HMax','image') + sMax = cv2.getTrackbarPos('SMax','image') + vMax = cv2.getTrackbarPos('VMax','image') + + # Set minimum and max HSV values to display + lower = np.array([hMin, sMin, vMin]) + upper = np.array([hMax, sMax, vMax]) + + # Create HSV Image and threshold into a range. + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv, lower, upper) + output = cv2.bitwise_and(img,img, mask= mask) + + # Print if there is a change in HSV value + if( (phMin != hMin) | (psMin != sMin) | (pvMin != vMin) | (phMax != hMax) | (psMax != sMax) | (pvMax != vMax) ): + print("(hMin = %d , sMin = %d, vMin = %d), (hMax = %d , sMax = %d, vMax = %d)" % (hMin , sMin , vMin, hMax, sMax , vMax)) + phMin = hMin + psMin = sMin + pvMin = vMin + phMax = hMax + psMax = sMax + pvMax = vMax + + # Display output image + cv2.imshow('image',output) + + # Wait longer to prevent freeze for videos. + if cv2.waitKey(waitTime) & 0xFF == ord('q'): + break + +cv2.destroyAllWindows()