From 3f1180b3204f4387f984f53e5eec128f6ce8a945 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Tue, 10 Oct 2023 14:09:51 -0500 Subject: [PATCH 01/16] [DEPS] upgrade python dependencies for docker, update extrinsic calibration in realsense_node.launch --- docker/requirements.txt | 6 +++--- launch/realsense_node.launch | 18 ++++++++---------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/docker/requirements.txt b/docker/requirements.txt index 272649b..e9c27f6 100644 --- a/docker/requirements.txt +++ b/docker/requirements.txt @@ -1,7 +1,7 @@ -numpy==1.17.4 -scipy==1.5.2 +numpy>=1.22 +scipy>=1.10.0 opencv_python==4.4.0.44 -Pillow==9.3.0 +Pillow>=10.0.1 rosnumpy==0.0.5.2 scikit-image==0.21.0 pyrealsense2==2.54.1.5217 \ No newline at end of file diff --git a/launch/realsense_node.launch b/launch/realsense_node.launch index 88dd846..662c31b 100644 --- a/launch/realsense_node.launch +++ b/launch/realsense_node.launch @@ -2,25 +2,23 @@ - - + - + - - - - - - + + + + + - + From 2bde01d663a2a9c2b96500a0136e8b91673cafcf Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Tue, 10 Oct 2023 15:07:12 -0500 Subject: [PATCH 02/16] [DEPS] update docker dependencies and corresponding documentation --- docker/Dockerfile.noetic | 1 + docker/requirements.txt | 4 ++-- docs/RUN.md | 16 ++++++++-------- trackdlo/src/utils.py | 4 ++-- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/docker/Dockerfile.noetic b/docker/Dockerfile.noetic index 7c64c5d..813de3b 100644 --- a/docker/Dockerfile.noetic +++ b/docker/Dockerfile.noetic @@ -24,6 +24,7 @@ RUN apt-get update && apt-get -y --no-install-recommends install \ git \ python3-pip \ python3-catkin-tools \ + python3-rosbag \ libeigen3-dev \ libpcl-dev \ libopencv-dev \ diff --git a/docker/requirements.txt b/docker/requirements.txt index e9c27f6..8689dc5 100644 --- a/docker/requirements.txt +++ b/docker/requirements.txt @@ -2,6 +2,6 @@ numpy>=1.22 scipy>=1.10.0 opencv_python==4.4.0.44 Pillow>=10.0.1 -rosnumpy==0.0.5.2 -scikit-image==0.21.0 +rosnumpy +scikit-image==0.20.0 pyrealsense2==2.54.1.5217 \ No newline at end of file diff --git a/docs/RUN.md b/docs/RUN.md index c796a43..3489644 100644 --- a/docs/RUN.md +++ b/docs/RUN.md @@ -9,14 +9,14 @@ Installation and execution of TrackDLO was verified with the below dependencies * [ROS Noetic](http://wiki.ros.org/noetic/Installation) * [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page) (Our version: 3.3.7) * [Point Cloud Library](https://pointclouds.org/) (Our version: 1.10.0) -* [OpenCV](https://opencv.org/releases/) (Our version: 4.2.0) -* [NumPy](https://numpy.org/install/) (Our version: 1.23.3) -* [SciPy](https://scipy.org/install/) (Our version: 1.9.1) -* [scikit-image](https://scikit-image.org/) (Our version: 0.18.0) -* [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. +* [OpenCV](https://opencv.org/releases/) +* [NumPy](https://numpy.org/install/) +* [SciPy](https://scipy.org/install/) +* [scikit-image](https://scikit-image.org/) +* [Pillow](https://pillow.readthedocs.io/en/stable/installation.html) +* [ROS Numpy](https://pypi.org/project/rosnumpy/) + +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 on using docker, and see the docker ![requirements.txt](https://github.com/RMDLO/trackdlo/blob/master/docker/requirements.txt) file for a list of the tested versions of TrackDLO package dependencies. ## Other Requirements diff --git a/trackdlo/src/utils.py b/trackdlo/src/utils.py index 8e23ec5..c6fa027 100644 --- a/trackdlo/src/utils.py +++ b/trackdlo/src/utils.py @@ -40,7 +40,7 @@ def orientation(p, q, r): # See https://www.geeksforgeeks.org/orientation-3-ordered-points/amp/ # for details of below formula. - val = (float(q.y - p.y) * (r.x - q.x)) - (float(q.x - p.x) * (r.y - q.y)) + val = (np.float64(q.y - p.y) * (r.x - q.x)) - (np.float64(q.x - p.x) * (r.y - q.y)) if (val > 0): # Clockwise orientation @@ -213,7 +213,7 @@ def extract_connected_skeleton (visualize_process, mask, img_scale=10, seg_lengt break # graph for visualization - mask = cv2.line(mask, contour[i][0], contour[i+1][0], 255, 1) + mask = cv2.line(mask, tuple(contour[i][0]), tuple(contour[i+1][0]), 255, 1) # if haven't initialized, perform initialization if cur_seg_start_point is None: From 24f145f8243b6f4a6893f79b71447b195e01e62f Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Tue, 10 Oct 2023 15:08:07 -0500 Subject: [PATCH 03/16] [DOCS] update .gitignore --- .gitignore | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/.gitignore b/.gitignore index ba0abc3..ff3dc94 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,5 @@ __pycache__/ .vscode/ -data/output/*.png -data/output/*/*.json -data/*.bag -data/ -test.cpp \ No newline at end of file +data/* +test.cpp +*.bag \ No newline at end of file From 0d9ddf7ad3084f680faca2f7574c661ceac4c6cb Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Mon, 16 Oct 2023 13:49:08 -0500 Subject: [PATCH 04/16] [FIX] fix eye-to-hand calibration in realsense_node.launch, update num_nodes to 35 --- launch/realsense_node.launch | 8 ++++---- launch/trackdlo.launch | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/realsense_node.launch b/launch/realsense_node.launch index 662c31b..4ecd716 100644 --- a/launch/realsense_node.launch +++ b/launch/realsense_node.launch @@ -12,10 +12,10 @@ - - - - + + + + diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index d8dc402..7e39a09 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -9,7 +9,7 @@ - + From b77d80fc8cdbb8fbc7be5b6bc1b1f4b62479e296 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Mon, 30 Oct 2023 15:30:44 -0500 Subject: [PATCH 05/16] [FIX] automatically respawn if tracking fails at runtime --- launch/trackdlo.launch | 4 +- trackdlo/src/initialize.py | 137 +++++++++++++++++++------------------ 2 files changed, 73 insertions(+), 68 deletions(-) diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index 7e39a09..2832f35 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -14,7 +14,7 @@ - + @@ -61,7 +61,7 @@ - + diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index 401b227..465d61a 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -11,7 +11,6 @@ import time import cv2 import numpy as np -import time from visualization_msgs.msg import MarkerArray from scipy import interpolate @@ -68,75 +67,80 @@ def callback (rgb, depth): # color thresholding mask = color_thresholding(hsv_image, cur_depth) - start_time = time.time() - mask = cv2.cvtColor(mask.copy(), cv2.COLOR_GRAY2BGR) - - # returns the pixel coord of points (in order). a list of lists - img_scale = 1 - extracted_chains = extract_connected_skeleton(visualize_initialization_process, mask, img_scale=img_scale, seg_length=8, max_curvature=25) - - all_pixel_coords = [] - for chain in extracted_chains: - all_pixel_coords += chain - print('Finished extracting chains. Time taken:', time.time()-start_time) - - all_pixel_coords = np.array(all_pixel_coords) * img_scale - all_pixel_coords = np.flip(all_pixel_coords, 1) - - pc_z = cur_depth[tuple(map(tuple, all_pixel_coords.T))] / 1000.0 - f = proj_matrix[0, 0] - cx = proj_matrix[0, 2] - cy = proj_matrix[1, 2] - pixel_x = all_pixel_coords[:, 1] - pixel_y = all_pixel_coords[:, 0] - - pc_x = (pixel_x - cx) * pc_z / f - pc_y = (pixel_y - cy) * pc_z / f - extracted_chains_3d = np.vstack((pc_x, pc_y)) - extracted_chains_3d = np.vstack((extracted_chains_3d, pc_z)) - extracted_chains_3d = extracted_chains_3d.T - - # do not include those without depth values - 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 - extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold] - - # tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.001) - tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.0005) - # 1st fit, less points - u_fine = np.linspace(0, 1, 300) # <-- num fit points - x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck) - spline_pts = np.vstack((x_fine, y_fine, z_fine)).T - - # 2nd fit, higher accuracy - num_true_pts = int(np.sum(np.sqrt(np.sum(np.square(np.diff(spline_pts, axis=0)), axis=1))) * 1000) - u_fine = np.linspace(0, 1, num_true_pts) # <-- num true points - x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck) - spline_pts = np.vstack((x_fine, y_fine, z_fine)).T - total_spline_len = np.sum(np.sqrt(np.sum(np.square(np.diff(spline_pts, axis=0)), axis=1))) - - init_nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)] - - results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) - results_pub.publish(results) - - # add color - pc_rgba = struct.unpack('I', struct.pack('BBBB', 255, 40, 40, 255))[0] - pc_rgba_arr = np.full((len(init_nodes), 1), pc_rgba) - pc_colored = np.hstack((init_nodes, pc_rgba_arr)).astype(object) - pc_colored[:, 3] = pc_colored[:, 3].astype(int) - - header.stamp = rospy.Time.now() - converted_points = pcl2.create_cloud(header, fields, pc_colored) - pc_pub.publish(converted_points) - - rospy.signal_shutdown('Finished initial node set computation.') + try: + start_time = time.time() + mask = cv2.cvtColor(mask.copy(), cv2.COLOR_GRAY2BGR) + + # returns the pixel coord of points (in order). a list of lists + img_scale = 1 + extracted_chains = extract_connected_skeleton(visualize_initialization_process, mask, img_scale=img_scale, seg_length=8, max_curvature=25) + + all_pixel_coords = [] + for chain in extracted_chains: + all_pixel_coords += chain + print('Finished extracting chains. Time taken:', time.time()-start_time) + + all_pixel_coords = np.array(all_pixel_coords) * img_scale + all_pixel_coords = np.flip(all_pixel_coords, 1) + + pc_z = cur_depth[tuple(map(tuple, all_pixel_coords.T))] / 1000.0 + f = proj_matrix[0, 0] + cx = proj_matrix[0, 2] + cy = proj_matrix[1, 2] + pixel_x = all_pixel_coords[:, 1] + pixel_y = all_pixel_coords[:, 0] + + pc_x = (pixel_x - cx) * pc_z / f + pc_y = (pixel_y - cy) * pc_z / f + extracted_chains_3d = np.vstack((pc_x, pc_y)) + extracted_chains_3d = np.vstack((extracted_chains_3d, pc_z)) + extracted_chains_3d = extracted_chains_3d.T + + # do not include those without depth values + 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 + extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold] + + # tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.001) + tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.0005) + # 1st fit, less points + u_fine = np.linspace(0, 1, 300) # <-- num fit points + x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck) + spline_pts = np.vstack((x_fine, y_fine, z_fine)).T + + # 2nd fit, higher accuracy + num_true_pts = int(np.sum(np.sqrt(np.sum(np.square(np.diff(spline_pts, axis=0)), axis=1))) * 1000) + u_fine = np.linspace(0, 1, num_true_pts) # <-- num true points + x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck) + spline_pts = np.vstack((x_fine, y_fine, z_fine)).T + total_spline_len = np.sum(np.sqrt(np.sum(np.square(np.diff(spline_pts, axis=0)), axis=1))) + + init_nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)] + + results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) + results_pub.publish(results) + + # add color + pc_rgba = struct.unpack('I', struct.pack('BBBB', 255, 40, 40, 255))[0] + pc_rgba_arr = np.full((len(init_nodes), 1), pc_rgba) + pc_colored = np.hstack((init_nodes, pc_rgba_arr)).astype(object) + pc_colored[:, 3] = pc_colored[:, 3].astype(int) + + header.stamp = rospy.Time.now() + converted_points = pcl2.create_cloud(header, fields, pc_colored) + pc_pub.publish(converted_points) + except: + rospy.logerr("Failed to extract splines.") + rospy.signal_shutdown('Stopping initialization.') if __name__=='__main__': rospy.init_node('init_tracker', anonymous=True) + global new_messages + new_messages=False + num_of_nodes = rospy.get_param('/init_tracker/num_of_nodes') multi_color_dlo = rospy.get_param('/init_tracker/multi_color_dlo') camera_info_topic = rospy.get_param('/init_tracker/camera_info_topic') @@ -156,6 +160,7 @@ def callback (rgb, depth): camera_info_sub = rospy.Subscriber(camera_info_topic, CameraInfo, camera_info_callback) rgb_sub = message_filters.Subscriber(rgb_topic, Image) depth_sub = message_filters.Subscriber(depth_topic, Image) + trackdlo_sub = rospy.Subscriber('/trackdlo/results_pc', PointCloud2, received_new_messages) # header header = std_msgs.msg.Header() From 4f41f7bdd0cc5553569eeba58fb65e4b1ef91d04 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Mon, 30 Oct 2023 15:49:12 -0500 Subject: [PATCH 06/16] [FIX] rm extra commented code in initialize.py for auto-respawning trackdlo --- trackdlo/src/initialize.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index 465d61a..bcdff88 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -131,6 +131,7 @@ def callback (rgb, depth): header.stamp = rospy.Time.now() converted_points = pcl2.create_cloud(header, fields, pc_colored) pc_pub.publish(converted_points) + # rospy.signal_shutdown('Finished initial node set computation.') except: rospy.logerr("Failed to extract splines.") rospy.signal_shutdown('Stopping initialization.') @@ -160,7 +161,6 @@ def callback (rgb, depth): camera_info_sub = rospy.Subscriber(camera_info_topic, CameraInfo, camera_info_callback) rgb_sub = message_filters.Subscriber(rgb_topic, Image) depth_sub = message_filters.Subscriber(depth_topic, Image) - trackdlo_sub = rospy.Subscriber('/trackdlo/results_pc', PointCloud2, received_new_messages) # header header = std_msgs.msg.Header() From 2410695191fa8bfaf9e8ca3501bdae85dd73e707 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 17:56:16 -0600 Subject: [PATCH 07/16] [DOCS] updating RUN.md to match main repo --- docs/RUN.md | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/docs/RUN.md b/docs/RUN.md index a8d25db..210985b 100644 --- a/docs/RUN.md +++ b/docs/RUN.md @@ -16,7 +16,7 @@ Installation and execution of TrackDLO was verified with the below dependencies * [Pillow](https://pillow.readthedocs.io/en/stable/installation.html) * [ROS Numpy](https://pypi.org/project/rosnumpy/) -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 on using docker, and see the docker ![requirements.txt](https://github.com/RMDLO/trackdlo/blob/master/docker/requirements.txt) file for a list of the tested versions of TrackDLO package dependencies. +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 on using docker, and see the docker [requirements.txt](https://github.com/RMDLO/trackdlo/blob/master/docker/requirements.txt) file for a list of the tested versions of TrackDLO package dependencies. ## Other Requirements @@ -34,7 +34,7 @@ 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 @@ -42,7 +42,7 @@ 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 /devel/setup.bash` is required in every terminal running TrackDLO ROS nodes. ## Usage @@ -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 one of the provided rosbag experiment `.bag` files [here](https://drive.google.com/drive/folders/1YjX-xfbNfm_G9FYbdw1voYxmd9VA-Aho?usp=sharing). Note: the file sizes are large--each will require 5-10GB of storage! 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 .bag +rosbag play --clock /path/to/filename.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: @@ -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`. \ No newline at end of file From 81e1633bf2de36e1fceb2842563f66c3b4522414 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 17:57:26 -0600 Subject: [PATCH 08/16] [CLEAN] very minor syntax changes to color_picker.py --- utils/color_picker.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/utils/color_picker.py b/utils/color_picker.py index 9b555b3..8ea340e 100644 --- a/utils/color_picker.py +++ b/utils/color_picker.py @@ -51,14 +51,9 @@ def nothing(x): 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)) + print("HSV min: (%d %d %d), HSV max: (%d %d %d)" % (hMin, sMin, vMin, hMax, sMax, vMax)) phMin = hMin psMin = sMin pvMin = vMin @@ -66,6 +61,12 @@ def nothing(x): psMax = sMax pvMax = vMax + # Create HSV Image and threshold into a range. + hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv.copy(), lower, upper) + + output = cv2.bitwise_and(img, img, mask= mask) + # Display output image cv2.imshow('image',output) From 5c4a6bdd8c050e34b7b53bbf19941640345f995e Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 17:58:28 -0600 Subject: [PATCH 09/16] [DOCS] update visualize_output.launch to match main repo --- launch/visualize_output.launch | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/launch/visualize_output.launch b/launch/visualize_output.launch index 2723a34..69da7d4 100644 --- a/launch/visualize_output.launch +++ b/launch/visualize_output.launch @@ -1,12 +1,16 @@ + - - - - - - + + + + + + + + + From ea9dbafe50b2c7febbc7d3cf36eb378c7ec02395 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 18:01:02 -0600 Subject: [PATCH 10/16] [RM] rm line_sphere_intersection.py->ported to .cpp --- utils/line_sphere_intersection.py | 103 ------------------------------ 1 file changed, 103 deletions(-) delete mode 100644 utils/line_sphere_intersection.py diff --git a/utils/line_sphere_intersection.py b/utils/line_sphere_intersection.py deleted file mode 100644 index 2b7fe17..0000000 --- a/utils/line_sphere_intersection.py +++ /dev/null @@ -1,103 +0,0 @@ -import math -import numpy as np - -def isBetween(x, a, b): - #check if x is between the segment composed by point a and point b - if (a <= x <= b or a >= x >= b): - return True - else: - return False - - -def line_sphere_intersection(point_A, point_B, sphere_center, radius): - # line has two points(pointA and pointB) - # sphere_center(x, y, z) - # radius is the sphere's radius - a = (point_B[0] - point_A[0])**2 + (point_B[1] - point_A[1])**2 + (point_B[2] - point_A[2])**2 - - b = 2* ((point_B[0] - point_A[0])*(point_A[0] - sphere_center[0]) + - (point_B[1] - point_A[1])*(point_A[1] - sphere_center[1]) + - (point_B[2] - point_A[2])*(point_A[2] - sphere_center[2])) - c = (point_A[0] - sphere_center[0])**2 + (point_A[1] - sphere_center[1])**2 + (point_A[2] - sphere_center[2])**2 - radius**2 - - delta = b**2 - (4*a*c) - - d1 = (-b+np.sqrt(delta))/(2*a) - d2 = (-b-np.sqrt(delta))/(2*a) - - if (delta < 0) : - print("delta < 0") - return None - elif (delta > 0): - print("delta > 0") - # one point - x1 = point_A[0] + d1*(point_B[0] - point_A[0]) - y1 = point_A[1] + d1*(point_B[1] - point_A[1]) - z1 = point_A[2] + d1*(point_B[2] - point_A[2]) - # the other one - x2 = point_A[0] + d2*(point_B[0] - point_A[0]) - y2 = point_A[1] + d2*(point_B[1] - point_A[1]) - z2 = point_A[2] + d2*(point_B[2] - point_A[2]) - - result = [] - if isBetween((x1, y1, z1), point_A, point_B): - result.append((x1, y1, z1)) - if isBetween((x2, y2, z2), point_A, point_B): - result.append((x2, y2, z2)) - if len(result) == 0: - return None - else: - return result - else: - print("delta = 0") - d1 = -b / (2*a) - # one point - x1 = point_A[0] + d1*(point_B[0] - point_A[0]) - y1 = point_A[1] + d1*(point_B[1] - point_A[1]) - z1 = point_A[2] + d1*(point_B[2] - point_A[2]) - if isBetween((x1, y1, z1), point_A, point_B): - return (x1, y1, z1) - else: - return None - -# one point intersection (work) -# sphere = (0, 0, 0) -# redius = 1 -# point_A = (1, 1, 0) -# point_B = (1, -1, 0) -# result = line_sphere_intersection(point_A, point_B, sphere, redius) -# print(result) - -# two point intersection (work) -# sphere = (0, 0, 0) -# redius = 1 -# point_A = (1, 0, 0) -# point_B = (-1, 0, 0) -# result = line_sphere_intersection(point_A, point_B, sphere, redius) -# print(result) - -# no intersection (work) -# sphere = (0, 0, 0) -# redius = 1 -# point_A = (2, 0, 0) -# point_B = (2, 2, 0) -# result = line_sphere_intersection(point_A, point_B, sphere, redius) -# print(result) - -# no intersection (work) -# suppose to be one, but it is not in the segment -# sphere = (0, 0, 0) -# redius = 1 -# point_A = (2, 0, 0) -# point_B = (3, 0, 0) -# result = line_sphere_intersection(point_A, point_B, sphere, redius) -# print(result) - -# one point intersection (work) -# suppose to be two, but one is not in the segment -# sphere = (0, 0, 0) -# redius = 1 -# point_A = (0, 0, 0) -# point_B = (3, 0, 0) -# result = line_sphere_intersection(point_A, point_B, sphere, redius) -# print(result) \ No newline at end of file From 1a50872938b21282839deafae7605d80e112ea8c Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 18:06:54 -0600 Subject: [PATCH 11/16] [ADD] Update init depth_threshold, change marker result color, change color_thresholding for green tip, update trackdlo.launch for green tip rope --- launch/trackdlo.launch | 14 ++++++------- trackdlo/src/initialize.py | 31 ++++++++++++----------------- trackdlo/src/trackdlo_node.cpp | 36 +++++++++++----------------------- 3 files changed, 30 insertions(+), 51 deletions(-) diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index 2832f35..1f75776 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -1,20 +1,18 @@ - - + - - - + - + + - + @@ -61,7 +59,7 @@ - + diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index bcdff88..e91a36c 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -26,25 +26,20 @@ def camera_info_callback (info): camera_info_sub.unregister() def color_thresholding (hsv_image, cur_depth): - # --- rope blue --- - lower = (90, 90, 60) - upper = (130, 255, 255) - mask_dlo = cv2.inRange(hsv_image, lower, upper).astype('uint8') - - # --- tape red --- - lower = (130, 60, 40) - upper = (255, 255, 255) - mask_red_1 = cv2.inRange(hsv_image, lower, upper).astype('uint8') - lower = (0, 60, 40) - upper = (10, 255, 255) - mask_red_2 = cv2.inRange(hsv_image, lower, upper).astype('uint8') - mask_marker = cv2.bitwise_or(mask_red_1.copy(), mask_red_2.copy()).astype('uint8') + global lower, upper + + mask_dlo = cv2.inRange(hsv_image.copy(), lower, upper).astype('uint8') + + # tape green + lower_green = (58, 130, 50) + upper_green = (90, 255, 89) + mask_green = cv2.inRange(hsv_image.copy(), lower_green, upper_green).astype('uint8') # combine masks - mask = cv2.bitwise_or(mask_marker.copy(), mask_dlo.copy()) + mask = cv2.bitwise_or(mask_green.copy(), mask_dlo.copy()) # filter mask base on depth values - mask[cur_depth < 0.58*1000] = 0 + mask[cur_depth < 0.57*1000] = 0 return mask @@ -100,7 +95,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.57 # m extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold] # tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.001) @@ -119,7 +114,8 @@ def callback (rgb, depth): init_nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)] - results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) + # results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) + results = ndarray2MarkerArray(init_nodes, result_frame_id, [0, 149/255, 203/255, 0.75], [0, 149/255, 203/255, 0.75]) results_pub.publish(results) # add color @@ -131,7 +127,6 @@ def callback (rgb, depth): header.stamp = rospy.Time.now() converted_points = pcl2.create_cloud(header, fields, pc_colored) pc_pub.publish(converted_points) - # rospy.signal_shutdown('Finished initial node set computation.') except: rospy.logerr("Failed to extract splines.") rospy.signal_shutdown('Stopping initialization.') diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index f2d2626..2cab74f 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -86,34 +86,21 @@ double pub_data_total = 0; int frames = 0; Mat color_thresholding (Mat cur_image_hsv) { - std::vector lower_blue = {90, 90, 60}; + std::vector lower_blue = {90, 90, 30}; std::vector upper_blue = {130, 255, 255}; - std::vector lower_red_1 = {130, 60, 50}; - std::vector upper_red_1 = {255, 255, 255}; + std::vector lower_green = {58, 130, 50}; + std::vector upper_green = {90, 255, 255}; - std::vector lower_red_2 = {0, 60, 50}; - std::vector upper_red_2 = {10, 255, 255}; - std::vector lower_yellow = {15, 100, 80}; - std::vector upper_yellow = {40, 255, 255}; + Mat mask_blue, mask_green, mask; - Mat mask_blue, mask_red_1, mask_red_2, mask_red, mask_yellow, mask; // filter blue cv::inRange(cur_image_hsv, cv::Scalar(lower_blue[0], lower_blue[1], lower_blue[2]), cv::Scalar(upper_blue[0], upper_blue[1], upper_blue[2]), mask_blue); - // filter red - cv::inRange(cur_image_hsv, cv::Scalar(lower_red_1[0], lower_red_1[1], lower_red_1[2]), cv::Scalar(upper_red_1[0], upper_red_1[1], upper_red_1[2]), mask_red_1); - cv::inRange(cur_image_hsv, cv::Scalar(lower_red_2[0], lower_red_2[1], lower_red_2[2]), cv::Scalar(upper_red_2[0], upper_red_2[1], upper_red_2[2]), mask_red_2); + cv::inRange(cur_image_hsv, cv::Scalar(lower_green[0], lower_green[1], lower_green[2]), cv::Scalar(upper_green[0], upper_green[1], upper_green[2]), mask_green_2); - // filter yellow - cv::inRange(cur_image_hsv, cv::Scalar(lower_yellow[0], lower_yellow[1], lower_yellow[2]), cv::Scalar(upper_yellow[0], upper_yellow[1], upper_yellow[2]), mask_yellow); - - // combine red mask - cv::bitwise_or(mask_red_1, mask_red_2, mask_red); - // combine overall mask - cv::bitwise_or(mask_red, mask_blue, mask); - cv::bitwise_or(mask_yellow, mask, mask); + cv::bitwise_or(mask_green, mask_blue, mask); return mask; } @@ -410,8 +397,8 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons cv::Scalar line_color; if (std::find(vis.begin(), vis.end(), idx) != vis.end()) { - point_color = cv::Scalar(0, 150, 255); - line_color = cv::Scalar(0, 255, 0); + point_color = cv::Scalar(203, 149, 0); + line_color = cv::Scalar(203, 149, 0); } else { point_color = cv::Scalar(0, 0, 255); @@ -421,7 +408,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons line_color = cv::Scalar(0, 0, 255); } else { - line_color = cv::Scalar(0, 255, 0); + line_color = cv::Scalar(203, 149, 0); } } @@ -433,7 +420,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons cv::circle(tracking_img, cv::Point(x, y), 7, point_color, -1); if (std::find(vis.begin(), vis.end(), idx+1) != vis.end()) { - point_color = cv::Scalar(0, 150, 255); + point_color = cv::Scalar(203, 149, 0); } else { point_color = cv::Scalar(0, 0, 255); @@ -452,7 +439,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons tracking_img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", tracking_img).toImageMsg(); // publish the results as a marker array - visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {1.0, 150.0/255.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 1.0}, 0.01, 0.005, vis, {1.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0, 1.0}); + visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {0.0, 149.0/255.0, 203.0/255.0, 1.0}, {0.0, 149.0/255.0, 203.0/255.0, 1.0}, 0.01, 0.005, vis, {1.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0, 1.0}); // visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {1.0, 150.0/255.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 1.0}, 0.01, 0.005); visualization_msgs::MarkerArray guide_nodes_results = MatrixXd2MarkerArray(guide_nodes, result_frame_id, "guide_node_results", {0.0, 0.0, 0.0, 0.5}, {0.0, 0.0, 1.0, 0.5}); visualization_msgs::MarkerArray corr_priors_results = MatrixXd2MarkerArray(priors, result_frame_id, "corr_prior_results", {0.0, 0.0, 0.0, 0.5}, {1.0, 0.0, 0.0, 0.5}); @@ -600,7 +587,6 @@ int main(int argc, char **argv) { init_nodes_sub = nh.subscribe("/trackdlo/init_nodes", 1, update_init_nodes); camera_info_sub = nh.subscribe(camera_info_topic, 1, update_camera_info); - image_transport::Publisher mask_pub = it.advertise("/trackdlo/mask", pub_queue_size); image_transport::Publisher tracking_img_pub = it.advertise("/trackdlo/results_img", pub_queue_size); pc_pub = nh.advertise("/trackdlo/filtered_pointcloud", pub_queue_size); results_pub = nh.advertise("/trackdlo/results_marker", pub_queue_size); From 3d23d4c4807385dabc66770916495ca02a10d8d5 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 18:08:11 -0600 Subject: [PATCH 12/16] Revert "[ADD] Update init depth_threshold, change marker result color, change color_thresholding for green tip, update trackdlo.launch for green tip rope" This reverts commit 1a50872938b21282839deafae7605d80e112ea8c. --- launch/trackdlo.launch | 14 +++++++------ trackdlo/src/initialize.py | 31 +++++++++++++++++------------ trackdlo/src/trackdlo_node.cpp | 36 +++++++++++++++++++++++----------- 3 files changed, 51 insertions(+), 30 deletions(-) diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index 1f75776..2832f35 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -1,18 +1,20 @@ + - + + - + + - - + - + @@ -59,7 +61,7 @@ - + diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index e91a36c..bcdff88 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -26,20 +26,25 @@ def camera_info_callback (info): camera_info_sub.unregister() def color_thresholding (hsv_image, cur_depth): - global lower, upper - - mask_dlo = cv2.inRange(hsv_image.copy(), lower, upper).astype('uint8') - - # tape green - lower_green = (58, 130, 50) - upper_green = (90, 255, 89) - mask_green = cv2.inRange(hsv_image.copy(), lower_green, upper_green).astype('uint8') + # --- rope blue --- + lower = (90, 90, 60) + upper = (130, 255, 255) + mask_dlo = cv2.inRange(hsv_image, lower, upper).astype('uint8') + + # --- tape red --- + lower = (130, 60, 40) + upper = (255, 255, 255) + mask_red_1 = cv2.inRange(hsv_image, lower, upper).astype('uint8') + lower = (0, 60, 40) + upper = (10, 255, 255) + mask_red_2 = cv2.inRange(hsv_image, lower, upper).astype('uint8') + mask_marker = cv2.bitwise_or(mask_red_1.copy(), mask_red_2.copy()).astype('uint8') # combine masks - mask = cv2.bitwise_or(mask_green.copy(), mask_dlo.copy()) + mask = cv2.bitwise_or(mask_marker.copy(), mask_dlo.copy()) # filter mask base on depth values - mask[cur_depth < 0.57*1000] = 0 + mask[cur_depth < 0.58*1000] = 0 return mask @@ -95,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.57 # m + depth_threshold = 0.58 # m extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold] # tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.001) @@ -114,8 +119,7 @@ def callback (rgb, depth): init_nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)] - # results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) - results = ndarray2MarkerArray(init_nodes, result_frame_id, [0, 149/255, 203/255, 0.75], [0, 149/255, 203/255, 0.75]) + results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) results_pub.publish(results) # add color @@ -127,6 +131,7 @@ def callback (rgb, depth): header.stamp = rospy.Time.now() converted_points = pcl2.create_cloud(header, fields, pc_colored) pc_pub.publish(converted_points) + # rospy.signal_shutdown('Finished initial node set computation.') except: rospy.logerr("Failed to extract splines.") rospy.signal_shutdown('Stopping initialization.') diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index 2cab74f..f2d2626 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -86,21 +86,34 @@ double pub_data_total = 0; int frames = 0; Mat color_thresholding (Mat cur_image_hsv) { - std::vector lower_blue = {90, 90, 30}; + std::vector lower_blue = {90, 90, 60}; std::vector upper_blue = {130, 255, 255}; - std::vector lower_green = {58, 130, 50}; - std::vector upper_green = {90, 255, 255}; + std::vector lower_red_1 = {130, 60, 50}; + std::vector upper_red_1 = {255, 255, 255}; + std::vector lower_red_2 = {0, 60, 50}; + std::vector upper_red_2 = {10, 255, 255}; - Mat mask_blue, mask_green, mask; + std::vector lower_yellow = {15, 100, 80}; + std::vector upper_yellow = {40, 255, 255}; + Mat mask_blue, mask_red_1, mask_red_2, mask_red, mask_yellow, mask; // filter blue cv::inRange(cur_image_hsv, cv::Scalar(lower_blue[0], lower_blue[1], lower_blue[2]), cv::Scalar(upper_blue[0], upper_blue[1], upper_blue[2]), mask_blue); - cv::inRange(cur_image_hsv, cv::Scalar(lower_green[0], lower_green[1], lower_green[2]), cv::Scalar(upper_green[0], upper_green[1], upper_green[2]), mask_green_2); + // filter red + cv::inRange(cur_image_hsv, cv::Scalar(lower_red_1[0], lower_red_1[1], lower_red_1[2]), cv::Scalar(upper_red_1[0], upper_red_1[1], upper_red_1[2]), mask_red_1); + cv::inRange(cur_image_hsv, cv::Scalar(lower_red_2[0], lower_red_2[1], lower_red_2[2]), cv::Scalar(upper_red_2[0], upper_red_2[1], upper_red_2[2]), mask_red_2); - cv::bitwise_or(mask_green, mask_blue, mask); + // filter yellow + cv::inRange(cur_image_hsv, cv::Scalar(lower_yellow[0], lower_yellow[1], lower_yellow[2]), cv::Scalar(upper_yellow[0], upper_yellow[1], upper_yellow[2]), mask_yellow); + + // combine red mask + cv::bitwise_or(mask_red_1, mask_red_2, mask_red); + // combine overall mask + cv::bitwise_or(mask_red, mask_blue, mask); + cv::bitwise_or(mask_yellow, mask, mask); return mask; } @@ -397,8 +410,8 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons cv::Scalar line_color; if (std::find(vis.begin(), vis.end(), idx) != vis.end()) { - point_color = cv::Scalar(203, 149, 0); - line_color = cv::Scalar(203, 149, 0); + point_color = cv::Scalar(0, 150, 255); + line_color = cv::Scalar(0, 255, 0); } else { point_color = cv::Scalar(0, 0, 255); @@ -408,7 +421,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons line_color = cv::Scalar(0, 0, 255); } else { - line_color = cv::Scalar(203, 149, 0); + line_color = cv::Scalar(0, 255, 0); } } @@ -420,7 +433,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons cv::circle(tracking_img, cv::Point(x, y), 7, point_color, -1); if (std::find(vis.begin(), vis.end(), idx+1) != vis.end()) { - point_color = cv::Scalar(203, 149, 0); + point_color = cv::Scalar(0, 150, 255); } else { point_color = cv::Scalar(0, 0, 255); @@ -439,7 +452,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons tracking_img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", tracking_img).toImageMsg(); // publish the results as a marker array - visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {0.0, 149.0/255.0, 203.0/255.0, 1.0}, {0.0, 149.0/255.0, 203.0/255.0, 1.0}, 0.01, 0.005, vis, {1.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0, 1.0}); + visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {1.0, 150.0/255.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 1.0}, 0.01, 0.005, vis, {1.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0, 1.0}); // visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {1.0, 150.0/255.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 1.0}, 0.01, 0.005); visualization_msgs::MarkerArray guide_nodes_results = MatrixXd2MarkerArray(guide_nodes, result_frame_id, "guide_node_results", {0.0, 0.0, 0.0, 0.5}, {0.0, 0.0, 1.0, 0.5}); visualization_msgs::MarkerArray corr_priors_results = MatrixXd2MarkerArray(priors, result_frame_id, "corr_prior_results", {0.0, 0.0, 0.0, 0.5}, {1.0, 0.0, 0.0, 0.5}); @@ -587,6 +600,7 @@ int main(int argc, char **argv) { init_nodes_sub = nh.subscribe("/trackdlo/init_nodes", 1, update_init_nodes); camera_info_sub = nh.subscribe(camera_info_topic, 1, update_camera_info); + image_transport::Publisher mask_pub = it.advertise("/trackdlo/mask", pub_queue_size); image_transport::Publisher tracking_img_pub = it.advertise("/trackdlo/results_img", pub_queue_size); pc_pub = nh.advertise("/trackdlo/filtered_pointcloud", pub_queue_size); results_pub = nh.advertise("/trackdlo/results_marker", pub_queue_size); From 2aa87a365b54811756e25654b7cb90aebd9ec091 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Sun, 19 Nov 2023 18:12:21 -0600 Subject: [PATCH 13/16] [ADD] Update init depth_threshold, change marker result color, change color_thresholding for green tip, update trackdlo.launch for green tip rope --- launch/trackdlo.launch | 14 ++++++------- trackdlo/src/initialize.py | 31 ++++++++++++----------------- trackdlo/src/trackdlo_node.cpp | 36 +++++++++++----------------------- 3 files changed, 30 insertions(+), 51 deletions(-) diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index 2832f35..1f75776 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -1,20 +1,18 @@ - - + - - - + - + + - + @@ -61,7 +59,7 @@ - + diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index bcdff88..e91a36c 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -26,25 +26,20 @@ def camera_info_callback (info): camera_info_sub.unregister() def color_thresholding (hsv_image, cur_depth): - # --- rope blue --- - lower = (90, 90, 60) - upper = (130, 255, 255) - mask_dlo = cv2.inRange(hsv_image, lower, upper).astype('uint8') - - # --- tape red --- - lower = (130, 60, 40) - upper = (255, 255, 255) - mask_red_1 = cv2.inRange(hsv_image, lower, upper).astype('uint8') - lower = (0, 60, 40) - upper = (10, 255, 255) - mask_red_2 = cv2.inRange(hsv_image, lower, upper).astype('uint8') - mask_marker = cv2.bitwise_or(mask_red_1.copy(), mask_red_2.copy()).astype('uint8') + global lower, upper + + mask_dlo = cv2.inRange(hsv_image.copy(), lower, upper).astype('uint8') + + # tape green + lower_green = (58, 130, 50) + upper_green = (90, 255, 89) + mask_green = cv2.inRange(hsv_image.copy(), lower_green, upper_green).astype('uint8') # combine masks - mask = cv2.bitwise_or(mask_marker.copy(), mask_dlo.copy()) + mask = cv2.bitwise_or(mask_green.copy(), mask_dlo.copy()) # filter mask base on depth values - mask[cur_depth < 0.58*1000] = 0 + mask[cur_depth < 0.57*1000] = 0 return mask @@ -100,7 +95,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.57 # m extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold] # tck, u = interpolate.splprep(extracted_chains_3d.T, s=0.001) @@ -119,7 +114,8 @@ def callback (rgb, depth): init_nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)] - results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) + # results = ndarray2MarkerArray(init_nodes, result_frame_id, [1, 150/255, 0, 0.75], [0, 1, 0, 0.75]) + results = ndarray2MarkerArray(init_nodes, result_frame_id, [0, 149/255, 203/255, 0.75], [0, 149/255, 203/255, 0.75]) results_pub.publish(results) # add color @@ -131,7 +127,6 @@ def callback (rgb, depth): header.stamp = rospy.Time.now() converted_points = pcl2.create_cloud(header, fields, pc_colored) pc_pub.publish(converted_points) - # rospy.signal_shutdown('Finished initial node set computation.') except: rospy.logerr("Failed to extract splines.") rospy.signal_shutdown('Stopping initialization.') diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index f2d2626..cb86eb9 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -86,34 +86,21 @@ double pub_data_total = 0; int frames = 0; Mat color_thresholding (Mat cur_image_hsv) { - std::vector lower_blue = {90, 90, 60}; + std::vector lower_blue = {90, 90, 30}; std::vector upper_blue = {130, 255, 255}; - std::vector lower_red_1 = {130, 60, 50}; - std::vector upper_red_1 = {255, 255, 255}; + std::vector lower_green = {58, 130, 50}; + std::vector upper_green = {90, 255, 255}; - std::vector lower_red_2 = {0, 60, 50}; - std::vector upper_red_2 = {10, 255, 255}; - std::vector lower_yellow = {15, 100, 80}; - std::vector upper_yellow = {40, 255, 255}; + Mat mask_blue, mask_green, mask; - Mat mask_blue, mask_red_1, mask_red_2, mask_red, mask_yellow, mask; // filter blue cv::inRange(cur_image_hsv, cv::Scalar(lower_blue[0], lower_blue[1], lower_blue[2]), cv::Scalar(upper_blue[0], upper_blue[1], upper_blue[2]), mask_blue); - // filter red - cv::inRange(cur_image_hsv, cv::Scalar(lower_red_1[0], lower_red_1[1], lower_red_1[2]), cv::Scalar(upper_red_1[0], upper_red_1[1], upper_red_1[2]), mask_red_1); - cv::inRange(cur_image_hsv, cv::Scalar(lower_red_2[0], lower_red_2[1], lower_red_2[2]), cv::Scalar(upper_red_2[0], upper_red_2[1], upper_red_2[2]), mask_red_2); + cv::inRange(cur_image_hsv, cv::Scalar(lower_green[0], lower_green[1], lower_green[2]), cv::Scalar(upper_green[0], upper_green[1], upper_green[2]), mask_green); - // filter yellow - cv::inRange(cur_image_hsv, cv::Scalar(lower_yellow[0], lower_yellow[1], lower_yellow[2]), cv::Scalar(upper_yellow[0], upper_yellow[1], upper_yellow[2]), mask_yellow); - - // combine red mask - cv::bitwise_or(mask_red_1, mask_red_2, mask_red); - // combine overall mask - cv::bitwise_or(mask_red, mask_blue, mask); - cv::bitwise_or(mask_yellow, mask, mask); + cv::bitwise_or(mask_green, mask_blue, mask); return mask; } @@ -410,8 +397,8 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons cv::Scalar line_color; if (std::find(vis.begin(), vis.end(), idx) != vis.end()) { - point_color = cv::Scalar(0, 150, 255); - line_color = cv::Scalar(0, 255, 0); + point_color = cv::Scalar(203, 149, 0); + line_color = cv::Scalar(203, 149, 0); } else { point_color = cv::Scalar(0, 0, 255); @@ -421,7 +408,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons line_color = cv::Scalar(0, 0, 255); } else { - line_color = cv::Scalar(0, 255, 0); + line_color = cv::Scalar(203, 149, 0); } } @@ -433,7 +420,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons cv::circle(tracking_img, cv::Point(x, y), 7, point_color, -1); if (std::find(vis.begin(), vis.end(), idx+1) != vis.end()) { - point_color = cv::Scalar(0, 150, 255); + point_color = cv::Scalar(203, 149, 0); } else { point_color = cv::Scalar(0, 0, 255); @@ -452,7 +439,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons tracking_img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", tracking_img).toImageMsg(); // publish the results as a marker array - visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {1.0, 150.0/255.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 1.0}, 0.01, 0.005, vis, {1.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0, 1.0}); + visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {0.0, 149.0/255.0, 203.0/255.0, 1.0}, {0.0, 149.0/255.0, 203.0/255.0, 1.0}, 0.01, 0.005, vis, {1.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0, 1.0}); // visualization_msgs::MarkerArray results = MatrixXd2MarkerArray(Y, result_frame_id, "node_results", {1.0, 150.0/255.0, 0.0, 1.0}, {0.0, 1.0, 0.0, 1.0}, 0.01, 0.005); visualization_msgs::MarkerArray guide_nodes_results = MatrixXd2MarkerArray(guide_nodes, result_frame_id, "guide_node_results", {0.0, 0.0, 0.0, 0.5}, {0.0, 0.0, 1.0, 0.5}); visualization_msgs::MarkerArray corr_priors_results = MatrixXd2MarkerArray(priors, result_frame_id, "corr_prior_results", {0.0, 0.0, 0.0, 0.5}, {1.0, 0.0, 0.0, 0.5}); @@ -600,7 +587,6 @@ int main(int argc, char **argv) { init_nodes_sub = nh.subscribe("/trackdlo/init_nodes", 1, update_init_nodes); camera_info_sub = nh.subscribe(camera_info_topic, 1, update_camera_info); - image_transport::Publisher mask_pub = it.advertise("/trackdlo/mask", pub_queue_size); image_transport::Publisher tracking_img_pub = it.advertise("/trackdlo/results_img", pub_queue_size); pc_pub = nh.advertise("/trackdlo/filtered_pointcloud", pub_queue_size); results_pub = nh.advertise("/trackdlo/results_marker", pub_queue_size); From 2982ecda1c970a7ad21828e6fbdc99620d031642 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Mon, 20 Nov 2023 11:00:52 -0600 Subject: [PATCH 14/16] [FEATURE] green tip tracking (working, tested) --- trackdlo/src/initialize.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index e91a36c..2338869 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -41,7 +41,7 @@ def color_thresholding (hsv_image, cur_depth): # filter mask base on depth values mask[cur_depth < 0.57*1000] = 0 - return mask + return mask, mask_green def callback (rgb, depth): global lower, upper @@ -60,8 +60,7 @@ def callback (rgb, depth): mask = cv2.inRange(hsv_image, lower, upper) else: # color thresholding - mask = color_thresholding(hsv_image, cur_depth) - + mask, mask_tip = color_thresholding(hsv_image, cur_depth) try: start_time = time.time() mask = cv2.cvtColor(mask.copy(), cv2.COLOR_GRAY2BGR) @@ -84,6 +83,11 @@ def callback (rgb, depth): cy = proj_matrix[1, 2] pixel_x = all_pixel_coords[:, 1] pixel_y = all_pixel_coords[:, 0] + # if the first mask value is not in the tip mask, reverse the pixel order + if multi_color_dlo: + pixel_value1 = mask_tip[pixel_y[0],pixel_x[0]] + if pixel_value1 == 0: + pixel_x, pixel_y = pixel_x[::-1], pixel_y[::-1] pc_x = (pixel_x - cx) * pc_z / f pc_y = (pixel_y - cy) * pc_z / f @@ -110,7 +114,6 @@ def callback (rgb, depth): u_fine = np.linspace(0, 1, num_true_pts) # <-- num true points x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck) spline_pts = np.vstack((x_fine, y_fine, z_fine)).T - total_spline_len = np.sum(np.sqrt(np.sum(np.square(np.diff(spline_pts, axis=0)), axis=1))) init_nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)] @@ -134,7 +137,7 @@ def callback (rgb, depth): if __name__=='__main__': rospy.init_node('init_tracker', anonymous=True) - global new_messages + global new_messages, lower, upper new_messages=False num_of_nodes = rospy.get_param('/init_tracker/num_of_nodes') From 01935361012f3d97814570d91c165bf95cc8c01b Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Mon, 20 Nov 2023 16:48:05 -0600 Subject: [PATCH 15/16] [FIX] fix re-ordering nodes based on tip color thresholding --- trackdlo/src/initialize.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/trackdlo/src/initialize.py b/trackdlo/src/initialize.py index 2338869..cb76221 100755 --- a/trackdlo/src/initialize.py +++ b/trackdlo/src/initialize.py @@ -85,8 +85,8 @@ def callback (rgb, depth): pixel_y = all_pixel_coords[:, 0] # if the first mask value is not in the tip mask, reverse the pixel order if multi_color_dlo: - pixel_value1 = mask_tip[pixel_y[0],pixel_x[0]] - if pixel_value1 == 0: + pixel_value1 = mask_tip[pixel_y[-1],pixel_x[-1]] + if pixel_value1 == 255: pixel_x, pixel_y = pixel_x[::-1], pixel_y[::-1] pc_x = (pixel_x - cx) * pc_z / f From f81157cfc633349ef52154d7a1c96f99320dfa98 Mon Sep 17 00:00:00 2001 From: hollydinkel Date: Tue, 23 Jan 2024 08:30:44 -0600 Subject: [PATCH 16/16] [FIX] re-add code snippet for publishing self-occluded nodes --- trackdlo/src/trackdlo_node.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index cb86eb9..4142337 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -322,11 +322,14 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons } // add edges for checking overlap with upcoming nodes - double x1 = col_1; - double y1 = row_1; - double x2 = col_2; - double y2 = row_2; - cv::line(projected_edges, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(255, 255, 255), dlo_pixel_width); + cv::line(projected_edges, cv::Point(col_1, row_1), cv::Point(col_2, row_2), cv::Scalar(255, 255, 255), dlo_pixel_width); + } + + // obtain self-occluded nodes + for (int i = 0; i < Y.rows(); i ++) { + if (std::find(not_self_occluded_nodes.begin(), not_self_occluded_nodes.end(), i) == not_self_occluded_nodes.end()) { + self_occluded_nodes.push_back(i); + } } // sort visible nodes to preserve the original connectivity