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
diff --git a/docs/RUN.md b/docs/RUN.md
index f874bc4..3560ce0 100644
--- a/docs/RUN.md
+++ b/docs/RUN.md
@@ -124,4 +124,4 @@ 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 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`.
+* 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
diff --git a/launch/realsense_node.launch b/launch/realsense_node.launch
index 88dd846..4ecd716 100644
--- a/launch/realsense_node.launch
+++ b/launch/realsense_node.launch
@@ -2,25 +2,23 @@
-
-
+
-
+
-
-
-
-
-
+
+
+
+
-
+
diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch
index d8dc402..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 15325d2..197d62c 100755
--- a/trackdlo/src/initialize.py
+++ b/trackdlo/src/initialize.py
@@ -47,6 +47,7 @@ def remove_duplicate_rows(array):
_, idx = np.unique(array, axis=0, return_index=True)
data = array[np.sort(idx)]
rospy.set_param('/init_tracker/num_of_nodes', len(data))
+
return data
def callback (rgb, depth):
@@ -124,6 +125,7 @@ def callback (rgb, depth):
nodes = spline_pts[np.linspace(0, num_true_pts-1, num_of_nodes).astype(int)]
init_nodes = remove_duplicate_rows(nodes)
+
# 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)
diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp
index f2d2626..4142337 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;
}
@@ -335,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
@@ -410,8 +400,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 +411,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 +423,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 +442,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 +590,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);
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)