Closed happygaoxiao closed 6 months ago
Hi @happygaoxiao,
Thanks for raising this issue! It looks like your object is monochrome, is that correct? We use the multi-color functionality to enable tip tracking (to break the object's visual symmetry) or ground truth marker tracking. Objects we use multi-color for are shown in the images below.
Since you were able to get a segmentation mask, it seems your color thresholding is actually working. How close is your camera to the scene? The below depth_threshold
parameter is in initialize.py
and is not configured in the launch file. If the DLO is closer to the camera than depth_threshold
, all the points in the point cloud corresponding to it will be filtered out.
if multi_color_dlo:
depth_threshold = 0.57 # m
extracted_chains_3d = extracted_chains_3d[extracted_chains_3d[:, 2] > depth_threshold]
Try changing this and let me know if it works! If it works, consider opening a pull request for this repository which sets depth_threshold
to a configurable parameter in the trackdlo.launch
file.
Hello, thank you for your quick response. I think my rope is monochrome. And I only modified the trackdlo.launch
file based on the HSV limits from COLOR_THRESHOLD.md
When I set multi_color_dlo
as true
, the init frame is totally black. depth_threshold = 0.3
, The actual distance between the camera and the rope is about 0.37m. The problem is that I didn't change anything in color_thresholding
in initialize.py and trackdlo_node.cpp. Because I only have the HSV limits but it seems to need blue, yellow and red bounds. I have no data for these.
When I set multi_color_dlo
as false
, the init frame looks correct as above screenshot. depth_threshold
should take no effect. But it got stuck here and I cannot get data from /trackdlo/filtered_pointcloud
.
My environment:
Steps:
catkin_make
the package, source thedevel/.bashrc
I followed the instructions in COLOR_THRESHOLD.md
to get HSV limits. Then run the two commands:
roslaunch trackdlo realsense_node.launch roslaunch trackdlo trackdlo.launch
Based on the picture you show, your DLO is one single color. It seems like you should be using multi_color_dlo:=false
, so my previous comment may not apply. You should not need to modify anything in the color_thresholding
function in initialize.py
, since this logic exists in initialize.py
:
if not multi_color_dlo:
# color thresholding
mask = cv2.inRange(hsv_image, lower, upper)
else:
# color thresholding
mask, mask_tip = color_thresholding(hsv_image, cur_depth)
You should also not need to modify the color_thresholding
function in trackdlo_node.cpp
since this logic exists in that script:
if (!multi_color_dlo) {
// color_thresholding
cv::inRange(cur_image_hsv, cv::Scalar(lower[0], lower[1], lower[2]), cv::Scalar(upper[0], upper[1], upper[2]), mask_without_occlusion_block);
}
else {
mask_without_occlusion_block = color_thresholding(cur_image_hsv);
}
Are you able to get to the after skeletonization
screen as you initialize tracking? This is the second screen that appears after visualizing the segmentation mask:
Make sure to press the escape
key on the init frame
window to advance through the initialization process. You can also just try setting visualize_initialization_process:=false
to make sure the lag in advancing through these frames is not affecting anything (it is mainly important to do this if the DLO is moving, for example from playing back .bag
data).
Hello, thank you for your reply.
I just fixed the issue by debugging the initialize.py
. There is a bug for skeletonization in this line. Method lee
or zhang
only works for 2D images from skeletonize.py
in skimage
package (version 0.21.0). So, when I use method=None
it works pretty well. There seems to be another bug in the while loop
of visualize_process
. It cannot break the loop.
Here is a demo with single color. https://github.com/RMDLO/trackdlo/assets/24514908/caa90d11-a2c8-4bc2-9ea5-cd5ad8a2abef
Could you please let me know what I should do for multi_color_dlo
? I used the HSV limits but when I set multi_color_dlo
as true
, the init frame is totally black.
Hi @happygaoxiao,
Nice work! For your single color demo, one thing I noticed is several nodes are marked as self-occluded (colored red). Try changing the num_of_nodes
parameter in trackdlo.launch
and see how this affects tracking for your setup. Tracking is sensitive to this parameter.
Tracking with a multi-color dlo is very sensitive to the HSV values. As you previously noticed, some of these are actually hard-coded and setup-specific in initialize.py
and trackdlo_node.cpp
. For my rope object above (dark blue rope with green tip), I use this as my color_thresholding
function in trackdlo_node.cpp
Mat color_thresholding (Mat cur_image_hsv) {
std::vector<int> lower_blue = {90, 90, 30};
std::vector<int> upper_blue = {130, 255, 255};
std::vector<int> lower_green = {58, 130, 50};
std::vector<int> upper_green = {90, 255, 255};
Mat mask_blue, mask_green, 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);
cv::bitwise_or(mask_green, mask_blue, mask);
return mask;
}
I use this for my color_thresholding
function in initialize.py
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')
# combine masks
mask = cv2.bitwise_or(mask_green.copy(), mask_dlo.copy())
# filter mask base on depth values
mask[cur_depth < 0.57*1000] = 0
return mask, mask_green
When using the color_picker.py
script and following the COLOR_THRESHOLD.md
instructions, make sure that you are actually able to segment two distinct colors. In my case, the color thresholding values which work for the green tip totally filter out the dark blue rope in color_picker.py
and vice-versa.
Hopefully some of that helps! Please let me know how it goes! 😃
Hi @happygaoxiao - were you able to resolve any issues encountered with using the multi_color_dlo option?
Hi @happygaoxiao - closing this issue, but feel free to open a new one if you have any more trouble!
Hello, I followed the instructions in
COLOR_THRESHOLD.md
and got the HSV limits. Then put it into the launch file. However, for Multi-Colored DLO, how could I modify the files in initialize.py and trackdlo_node.cpp? Ininitialize.py
:If I set the
multi_color_dlo
asfalse
, I got an error as below:If I set
visualize_initialization_process
astrue
, it got stuck atInitializing...
.No data are shown in
/trackdlo/filtered_pointcloud
.Could you please tell me what I should do? Thank you very much!