Skip to content

Commit

Permalink
Merge pull request #73 from jingyi-xiang/master
Browse files Browse the repository at this point in the history 
updated visualization and configurable parameters
  • Loading branch information
@jingyi-xiang
jingyi-xiang committed Jun 26, 2023
2 parents 331f35d + 5f618de commit 220e2ce
Show file tree
Hide file tree
Showing 12 changed files with 158 additions and 225 deletions.
26 changes: 18 additions & 8 deletions launch/trackdlo.launch
View file
Original file line number Diff line number Diff line change
@@ -1,25 +1,32 @@
<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_upper_limit" default="150 150 40" />
<arg name="hsv_threshold_lower_limit" default="0 0 0" /> -->
<arg name="num_of_nodes" default="45" />
<arg name="visualize_initialization_process" default="true" />
<!-- <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="25" />
<arg name="visualize_initialization_process" default="false" />

<!-- modifying the below parameters is usually not needed -->
<!-- kernel: 0 -> Laplacian; 1 -> 1st order; 2nd -> 2nd order; 3 -> Gaussian -->
<arg name="kernel" default="1" />
<arg name="multi_color_dlo" default="false" />

<!-- the below arg is for quick sanity checking with GLTP.
it should ALWAYS be set to FALSE when running TrackDLO -->
<arg name="gltp" default="false" />

<!-- 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)" />
Expand All @@ -44,16 +51,16 @@
<param name="lle_weight" value="10.0" />

<!-- mu: ranges from 0 to 1, large mu indicates the point cloud is noisy -->
<param name="mu" value="0.05" />
<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.00001" />
<param name="tol" value="0.0002" />

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

<!-- set to false in the registration step because we did not have space in the paper to discuss LLE -->
<param name="include_lle" value="false" />
Expand All @@ -71,13 +78,16 @@
<param name="multi_color_dlo" type="bool" value="$(arg multi_color_dlo)" />
<param name="visibility_threshold" type="double" value="0.008" />
<param name="dlo_pixel_width" value="40" />
<param name="gltp" type="bool" value="$(arg gltp)" />
</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)" />
Expand Down
88 changes: 5 additions & 83 deletions rviz/tracking.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,85 +78,7 @@ Visualization Manager:
Marker Topic: /trackdlo/results_marker
Name: MarkerArray
Namespaces:
line_results0: true
line_results1: true
line_results10: true
line_results11: true
line_results12: true
line_results13: true
line_results14: true
line_results15: true
line_results16: true
line_results17: true
line_results18: true
line_results19: true
line_results2: true
line_results20: true
line_results21: true
line_results22: true
line_results23: true
line_results24: true
line_results25: true
line_results26: true
line_results27: true
line_results28: true
line_results29: true
line_results3: true
line_results30: true
line_results31: true
line_results32: true
line_results33: true
line_results34: true
line_results35: true
line_results36: true
line_results37: true
line_results38: true
line_results4: true
line_results5: true
line_results6: true
line_results7: true
line_results8: true
line_results9: true
node_results0: true
node_results1: true
node_results10: true
node_results11: true
node_results12: true
node_results13: true
node_results14: true
node_results15: true
node_results16: true
node_results17: true
node_results18: true
node_results19: true
node_results2: true
node_results20: true
node_results21: true
node_results22: true
node_results23: true
node_results24: true
node_results25: true
node_results26: true
node_results27: true
node_results28: true
node_results29: true
node_results3: true
node_results30: true
node_results31: true
node_results32: true
node_results33: true
node_results34: true
node_results35: true
node_results36: true
node_results37: true
node_results38: true
node_results39: true
node_results4: true
node_results5: true
node_results6: true
node_results7: true
node_results8: true
node_results9: true
{}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Expand All @@ -179,7 +101,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: true
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Expand All @@ -194,7 +116,7 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Expand All @@ -207,7 +129,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Expand All @@ -222,7 +144,7 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
Value: true
Enabled: true
Global Options:
Background Color: 85; 87; 83
Expand Down
6 changes: 0 additions & 6 deletions trackdlo/include/run_evaluation.h
View file

This file was deleted.

6 changes: 0 additions & 6 deletions trackdlo/include/trackdlo_node.h
View file

This file was deleted.

18 changes: 13 additions & 5 deletions trackdlo/src/initialize.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ def camera_info_callback (info):
print(proj_matrix)
camera_info_sub.unregister()

def color_thresholding (hsv_image):
def color_thresholding (hsv_image, cur_depth):
# --- rope blue ---
lower = (90, 90, 60)
upper = (130, 255, 255)
Expand All @@ -44,6 +44,9 @@ def color_thresholding (hsv_image):
# combine masks
mask = cv2.bitwise_or(mask_marker.copy(), mask_dlo.copy())

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

return mask

def callback (rgb, depth):
Expand All @@ -63,7 +66,7 @@ def callback (rgb, depth):
mask = cv2.inRange(hsv_image, lower, upper)
else:
# color thresholding
mask = color_thresholding(hsv_image)
mask = color_thresholding(hsv_image, cur_depth)

start_time = time.time()
mask = cv2.cvtColor(mask.copy(), cv2.COLOR_GRAY2BGR)
Expand Down Expand Up @@ -96,8 +99,12 @@ def callback (rgb, depth):
# 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.002)
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)
Expand All @@ -112,7 +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, "camera_color_optical_frame", [255, 150, 0, 0.75], [0, 255, 0, 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
Expand All @@ -135,6 +142,7 @@ def callback (rgb, depth):
camera_info_topic = rospy.get_param('/init_tracker/camera_info_topic')
rgb_topic = rospy.get_param('/init_tracker/rgb_topic')
depth_topic = rospy.get_param('/init_tracker/depth_topic')
result_frame_id = rospy.get_param('/init_tracker/result_frame_id')
visualize_initialization_process = rospy.get_param('/init_tracker/visualize_initialization_process')

hsv_threshold_upper_limit = rospy.get_param('/init_tracker/hsv_threshold_upper_limit')
Expand All @@ -152,7 +160,7 @@ def callback (rgb, depth):
# header
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'camera_color_optical_frame'
header.frame_id = result_frame_id
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1),
Expand Down
4 changes: 2 additions & 2 deletions trackdlo/src/run_evaluation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons

double time_from_start;
time_from_start = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - tracking_evaluator.start_time()).count();
time_from_start = time_from_start / 1000.0 * tracking_evaluator.rate();
time_from_start = (time_from_start / 1000.0 + 3.0) * tracking_evaluator.rate(); // initialization usually takes 1.5 seconds
std::cout << time_from_start << "; " << tracking_evaluator.exit_time() << std::endl;

if (tracking_evaluator.exit_time() == -1) {
Expand Down Expand Up @@ -454,7 +454,7 @@ int main(int argc, char **argv) {

message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "/camera/color/image_raw", 10);
message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "/camera/depth/color/points", 10);
message_filters::Subscriber<sensor_msgs::PointCloud2> result_sub(nh, "/" + alg + "_results_pc", 10);
message_filters::Subscriber<sensor_msgs::PointCloud2> result_sub(nh, "/" + alg + "/results_pc", 10);
message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::PointCloud2, sensor_msgs::PointCloud2> sync(image_sub, pc_sub, result_sub, 10);

sync.registerCallback<std::function<void(const sensor_msgs::ImageConstPtr&,
Expand Down
61 changes: 4 additions & 57 deletions trackdlo/src/trackdlo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -335,12 +335,6 @@ bool trackdlo::cpd_lle (MatrixXd X,

if (use_geodesic) {
std::vector<int> max_p_nodes(P.cols(), 0);

// // temp test
// for (int i = 0; i < N; i ++) {
// P.col(i).maxCoeff(&max_p_nodes[i]);
// }

MatrixXd pts_dis_sq_geodesic = MatrixXd::Zero(M, N);

// loop through all points
Expand Down Expand Up @@ -405,49 +399,6 @@ bool trackdlo::cpd_lle (MatrixXd X,

for (int i = 0; i < Y.rows(); i ++) {
double shortest_node_pt_dist = shortest_node_pt_dists[i];
// double shortest_node_pt_dist;

// // if this node is visible, the shortest dist is always 0
// if (std::find(visible_nodes.begin(), visible_nodes.end(), i) != visible_nodes.end()){
// shortest_node_pt_dist = 0;
// }
// else {
// // find the closest visible node
// for (int j = 0; j <= Y.rows()-1; j ++) {
// int left_node = i - j;
// int right_node = i + j;
// double left_dist = 100000;
// double right_dist = 100000;
// bool found_visible_node = false;

// // make sure node index doesn't go out of bound
// if (left_node >= 0) {
// // if i - j is visible, record the geodesic distance between the nodes
// if (std::find(visible_nodes.begin(), visible_nodes.end(), left_node) != visible_nodes.end()){
// left_dist = abs(converted_node_coord[left_node] - converted_node_coord[i]);
// found_visible_node = true;
// }
// }
// if (right_node <= Y.rows()-1) {
// // if i + j is visible, record the geodesic distance between the nodes
// if (std::find(visible_nodes.begin(), visible_nodes.end(), right_node) != visible_nodes.end()){
// right_dist = abs(converted_node_coord[right_dist] - converted_node_coord[i]);
// found_visible_node = true;
// }
// }

// // take the shortest distance
// if (found_visible_node) {
// if (left_dist < right_dist) {
// shortest_node_pt_dist = left_dist;
// }
// else {
// shortest_node_pt_dist = right_dist;
// }
// break;
// }
// }
// }

double P_vis_i = exp(-k_vis * shortest_node_pt_dist);
total_P_vis += P_vis_i;
Expand Down Expand Up @@ -502,12 +453,7 @@ bool trackdlo::cpd_lle (MatrixXd X,
}
}

// MatrixXd W = A_matrix.householderQr().solve(B_matrix);
// MatrixXd W = A_matrix.completeOrthogonalDecomposition().solve(B_matrix);
// MatrixXd W = A_matrix.fullPivLu().solve(B_matrix);
// MatrixXd W = A_matrix.partialPivLu().solve(B_matrix);
// MatrixXd W = A_matrix.colPivHouseholderQr().solve(B_matrix);
MatrixXd W = A_matrix.fullPivHouseholderQr().solve(B_matrix);
MatrixXd W = A_matrix.completeOrthogonalDecomposition().solve(B_matrix);

MatrixXd T = Y_0 + G * W;
double trXtdPt1X = (X.transpose() * Pt1.asDiagonal() * X).trace();
Expand All @@ -516,7 +462,7 @@ bool trackdlo::cpd_lle (MatrixXd X,

sigma2 = (trXtdPt1X - 2*trPXtT + trTtdP1T) / (Np * D);

if (pt2pt_dis_sq(Y, Y_0 + G*W) < tol) {
if (pt2pt_dis(Y, Y_0 + G*W) / Y.rows() < tol) {
Y = Y_0 + G*W;
ROS_INFO_STREAM("Iteration until convergence: " + std::to_string(it+1));
break;
Expand Down Expand Up @@ -1018,7 +964,8 @@ void trackdlo::tracking_step (MatrixXd X,
// determine DLO state: heading visible, tail visible, both visible, or both occluded
// priors_vec should be the final output; priors_vec[i] = {index, x, y, z}
double sigma2_pre_proc = sigma2_;
cpd_lle(X, guide_nodes_, sigma2_pre_proc, 3, 1, 1, mu_, 50, 0.00001, true, true, true, false, {}, 0, 1, visible_nodes_extended, 0, visibility_threshold_);
cpd_lle(X, guide_nodes_, sigma2_pre_proc, 3, 1, lle_weight_, mu_, 50, tol_, true, true, true, false, {}, 0, 1);
// cpd_lle(X, guide_nodes_, sigma2_pre_proc, 1, 1, 1, mu_, 50, tol_, true, true, true);

if (visible_nodes_extended.size() == Y_.rows()) {
if (visible_nodes.size() == visible_nodes_extended.size()) {
Expand Down
Loading

0 comments on commit 220e2ce

Please sign in to comment.