From fe6a6d909c9e754409a705330e529303bf855145 Mon Sep 17 00:00:00 2001 From: Jingyi Xiang Date: Fri, 11 Aug 2023 17:45:41 -0500 Subject: [PATCH 1/3] unfinished cleanup --- launch/trackdlo.launch | 40 +------ trackdlo/include/trackdlo.h | 35 ++---- trackdlo/src/trackdlo.cpp | 197 +++++++++++---------------------- trackdlo/src/trackdlo_node.cpp | 38 ++----- 4 files changed, 91 insertions(+), 219 deletions(-) diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index afab431..20e131e 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -9,18 +9,10 @@ - + - - - - - - - @@ -32,24 +24,12 @@ - - - - - - - - - - - + + - - - @@ -62,23 +42,13 @@ - - - - - + + - - - - - diff --git a/trackdlo/include/trackdlo.h b/trackdlo/include/trackdlo.h index 281ca50..a155307 100644 --- a/trackdlo/include/trackdlo.h +++ b/trackdlo/include/trackdlo.h @@ -58,19 +58,15 @@ class trackdlo trackdlo(int num_of_nodes); // fancy constructor trackdlo(int num_of_nodes, - double visibility_threshold, - double beta, - double lambda, - double alpha, - double gamma, - double k_vis, - double mu, - int max_iter, - const double tol, - bool include_lle, - bool use_geodesic, - bool use_prev_sigma2, - int kernel); + double visibility_threshold, + double beta, + double lambda, + double alpha, + double k_vis, + double mu, + int max_iter, + double tol, + double lle_weight); double get_sigma2(); MatrixXd get_tracking_result(); @@ -85,17 +81,13 @@ class trackdlo double& sigma2, double beta, double lambda, - double gamma, + double lle_weight, double mu, int max_iter = 30, double tol = 0.00001, bool include_lle = true, - bool use_geodesic = false, - bool use_prev_sigma2 = false, - bool use_ecpd = false, std::vector correspondence_priors = {}, double alpha = 0, - int kernel = 3, std::vector visible_nodes = {}, double k_vis = 0, double visibility_threshold = 0.01); @@ -114,15 +106,12 @@ class trackdlo double beta_; double lambda_; double alpha_; - double lle_weight_; double k_vis_; double mu_; int max_iter_; double tol_; - bool include_lle_; - bool use_geodesic_; - bool use_prev_sigma2_; - int kernel_; + double lle_weight_; + std::vector geodesic_coord_; std::vector correspondence_priors_; double visibility_threshold_; diff --git a/trackdlo/src/trackdlo.cpp b/trackdlo/src/trackdlo.cpp index 0d139ab..54e1c07 100644 --- a/trackdlo/src/trackdlo.cpp +++ b/trackdlo/src/trackdlo.cpp @@ -20,10 +20,6 @@ trackdlo::trackdlo(int num_of_nodes) { mu_ = 0.05; max_iter_ = 50; tol_ = 0.00001; - include_lle_ = true; - use_geodesic_ = true; - use_prev_sigma2_ = true; - kernel_ = 1; geodesic_coord_ = {}; correspondence_priors_ = {}; visibility_threshold_ = 0.02; @@ -34,15 +30,11 @@ trackdlo::trackdlo(int num_of_nodes, double beta, double lambda, double alpha, - double lle_weight, double k_vis, double mu, int max_iter, double tol, - bool include_lle, - bool use_geodesic, - bool use_prev_sigma2, - int kernel) + double lle_weight) { Y_ = MatrixXd::Zero(num_of_nodes, 3); visibility_threshold_ = visibility_threshold; @@ -56,10 +48,6 @@ trackdlo::trackdlo(int num_of_nodes, mu_ = mu; max_iter_ = max_iter; tol_ = tol; - include_lle_ = include_lle; - use_geodesic_ = use_geodesic; - use_prev_sigma2_ = use_prev_sigma2; - kernel_ = kernel; geodesic_coord_ = {}; correspondence_priors_ = {}; } @@ -174,12 +162,8 @@ bool trackdlo::cpd_lle (MatrixXd X, int max_iter, double tol, bool include_lle, - bool use_geodesic, - bool use_prev_sigma2, - bool use_ecpd, std::vector correspondence_priors, double alpha, - int kernel, std::vector visible_nodes, double k_vis, double visibility_threshold) @@ -187,10 +171,6 @@ bool trackdlo::cpd_lle (MatrixXd X, bool converged = true; - if (correspondence_priors.size() == 0) { - use_ecpd = false; - } - int M = Y.rows(); int N = X.rows(); int D = 3; @@ -211,65 +191,29 @@ bool trackdlo::cpd_lle (MatrixXd X, std::vector converted_node_coord = {0.0}; // this is not squared MatrixXd G = MatrixXd::Zero(M, M); - if (!use_geodesic) { - if (kernel == 3) { - G = (-diff_yy / (2 * beta * beta)).array().exp(); - } - else if (kernel == 0) { - G = (-diff_yy_sqrt / (2 * beta * beta)).array().exp(); - } - else if (kernel == 1) { - // G = 1/(2*beta * 2*beta) * (-sqrt(2)*diff_yy_sqrt/beta).array().exp() * (2*diff_yy_sqrt.array() + sqrt(2)*beta); - G = 1/(2 * pow(beta, 2)) * (-2 *diff_yy_sqrt/beta).array().exp() * (2*diff_yy_sqrt.array() + sqrt(2)*beta); - } - else if (kernel == 2) { - // G = 27 * 1/(72 * pow(beta, 3)) * (-sqrt(3)*diff_yy_sqrt/beta).array().exp() * (sqrt(3)*beta*beta + 3*beta*diff_yy_sqrt.array() + sqrt(3)*diff_yy.array()); - G = 3/(16 * pow(beta, 3)) * (-sqrt(6)*diff_yy_sqrt/beta).array().exp() * (2*sqrt(6)*diff_yy.array() + 6*beta*diff_yy_sqrt.array() + sqrt(6) * pow(beta, 2)); - } - else { // default to gaussian - G = (-diff_yy / (2 * beta * beta)).array().exp(); - } + double cur_sum = 0; + for (int i = 0; i < M-1; i ++) { + cur_sum += pt2pt_dis(Y_0.row(i+1), Y_0.row(i)); + converted_node_coord.push_back(cur_sum); } - else { - double cur_sum = 0; - for (int i = 0; i < M-1; i ++) { - cur_sum += pt2pt_dis(Y_0.row(i+1), Y_0.row(i)); - converted_node_coord.push_back(cur_sum); - } - for (int i = 0; i < converted_node_coord.size(); i ++) { - for (int j = 0; j < converted_node_coord.size(); j ++) { - converted_node_dis_sq(i, j) = pow(converted_node_coord[i] - converted_node_coord[j], 2); - converted_node_dis(i, j) = abs(converted_node_coord[i] - converted_node_coord[j]); - } - } - - if (kernel == 3) { - G = (-converted_node_dis_sq / (2 * beta * beta)).array().exp(); - } - else if (kernel == 0) { - G = (-converted_node_dis / (2 * beta * beta)).array().exp(); - } - else if (kernel == 1) { - G = 1/(2*beta * 2*beta) * (-sqrt(2)*converted_node_dis/beta).array().exp() * (sqrt(2)*converted_node_dis.array() + beta); - } - else if (kernel == 2) { - G = 27 * 1/(72 * pow(beta, 3)) * (-sqrt(3)*converted_node_dis/beta).array().exp() * (sqrt(3)*beta*beta + 3*beta*converted_node_dis.array() + sqrt(3)*converted_node_dis_sq.array()); - } - else { // default to gaussian - G = (-converted_node_dis_sq / (2 * beta * beta)).array().exp(); + for (int i = 0; i < converted_node_coord.size(); i ++) { + for (int j = 0; j < converted_node_coord.size(); j ++) { + converted_node_dis_sq(i, j) = pow(converted_node_coord[i] - converted_node_coord[j], 2); + converted_node_dis(i, j) = abs(converted_node_coord[i] - converted_node_coord[j]); } } + // kernel matrix + G = 1/(2*beta * 2*beta) * (-sqrt(2)*converted_node_dis/beta).array().exp() * (2*converted_node_dis.array() + sqrt(2)*beta); + // get the LLE matrix MatrixXd L = calc_LLE_weights(6, Y_0); MatrixXd H = (MatrixXd::Identity(M, M) - L).transpose() * (MatrixXd::Identity(M, M) - L); - // construct R and J - MatrixXd priors = MatrixXd::Zero(correspondence_priors.size(), 3); + // construct J MatrixXd J = MatrixXd::Zero(M, M); MatrixXd Y_extended = Y_0.replicate(1, 1); - MatrixXd G_masked = MatrixXd::Zero(M, M); if (correspondence_priors.size() != 0) { int num_of_correspondence_priors = correspondence_priors.size(); @@ -280,10 +224,8 @@ bool trackdlo::cpd_lle (MatrixXd X, temp(0, 1) = correspondence_priors[i](0, 2); temp(0, 2) = correspondence_priors[i](0, 3); - priors.row(i) = temp; J.row(index) = MatrixXd::Identity(M, M).row(index); Y_extended.row(index) = temp; - G_masked.row(index) = G.row(index); // // enforce boundaries // if (i == 0 || i == num_of_correspondence_priors-1) { @@ -301,7 +243,7 @@ bool trackdlo::cpd_lle (MatrixXd X, } // initialize sigma2 - if (!use_prev_sigma2 || sigma2 == 0) { + if (sigma2 == 0) { sigma2 = diff_xy.sum() / static_cast(D * M * N); } @@ -333,64 +275,59 @@ bool trackdlo::cpd_lle (MatrixXd X, double c = pow((2 * M_PI * sigma2), static_cast(D)/2) * mu / (1 - mu) * static_cast(M)/N; P = P.array().rowwise() / (P.colwise().sum().array() + c); - if (use_geodesic) { - std::vector max_p_nodes(P.cols(), 0); - MatrixXd pts_dis_sq_geodesic = MatrixXd::Zero(M, N); + // P matrix calculation based on geodesic distance + std::vector max_p_nodes(P.cols(), 0); + MatrixXd pts_dis_sq_geodesic = MatrixXd::Zero(M, N); - // loop through all points - for (int i = 0; i < N; i ++) { - - P.col(i).maxCoeff(&max_p_nodes[i]); - int max_p_node = max_p_nodes[i]; + // loop through all points + for (int i = 0; i < N; i ++) { + + P.col(i).maxCoeff(&max_p_nodes[i]); + int max_p_node = max_p_nodes[i]; - int potential_2nd_max_p_node_1 = max_p_node - 1; - if (potential_2nd_max_p_node_1 == -1) { - potential_2nd_max_p_node_1 = 2; - } + int potential_2nd_max_p_node_1 = max_p_node - 1; + if (potential_2nd_max_p_node_1 == -1) { + potential_2nd_max_p_node_1 = 2; + } - int potential_2nd_max_p_node_2 = max_p_node + 1; - if (potential_2nd_max_p_node_2 == M) { - potential_2nd_max_p_node_2 = M - 3; - } + int potential_2nd_max_p_node_2 = max_p_node + 1; + if (potential_2nd_max_p_node_2 == M) { + potential_2nd_max_p_node_2 = M - 3; + } - int next_max_p_node; - if (pt2pt_dis(Y.row(potential_2nd_max_p_node_1), X.row(i)) < pt2pt_dis(Y.row(potential_2nd_max_p_node_2), X.row(i))) { - next_max_p_node = potential_2nd_max_p_node_1; - } - else { - next_max_p_node = potential_2nd_max_p_node_2; - } + int next_max_p_node; + if (pt2pt_dis(Y.row(potential_2nd_max_p_node_1), X.row(i)) < pt2pt_dis(Y.row(potential_2nd_max_p_node_2), X.row(i))) { + next_max_p_node = potential_2nd_max_p_node_1; + } + else { + next_max_p_node = potential_2nd_max_p_node_2; + } - // fill the current column of pts_dis_sq_geodesic - pts_dis_sq_geodesic(max_p_node, i) = pt2pt_dis_sq(Y.row(max_p_node), X.row(i)); - pts_dis_sq_geodesic(next_max_p_node, i) = pt2pt_dis_sq(Y.row(next_max_p_node), X.row(i)); + // fill the current column of pts_dis_sq_geodesic + pts_dis_sq_geodesic(max_p_node, i) = pt2pt_dis_sq(Y.row(max_p_node), X.row(i)); + pts_dis_sq_geodesic(next_max_p_node, i) = pt2pt_dis_sq(Y.row(next_max_p_node), X.row(i)); - if (max_p_node < next_max_p_node) { - for (int j = 0; j < max_p_node; j ++) { - pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[max_p_node]) + pt2pt_dis(Y.row(max_p_node), X.row(i)), 2); - } - for (int j = next_max_p_node; j < M; j ++) { - pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[next_max_p_node]) + pt2pt_dis(Y.row(next_max_p_node), X.row(i)), 2); - } + if (max_p_node < next_max_p_node) { + for (int j = 0; j < max_p_node; j ++) { + pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[max_p_node]) + pt2pt_dis(Y.row(max_p_node), X.row(i)), 2); } - else { - for (int j = 0; j < next_max_p_node; j ++) { - pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[next_max_p_node]) + pt2pt_dis(Y.row(next_max_p_node), X.row(i)), 2); - } - for (int j = max_p_node; j < M; j ++) { - pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[max_p_node]) + pt2pt_dis(Y.row(max_p_node), X.row(i)), 2); - } + for (int j = next_max_p_node; j < M; j ++) { + pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[next_max_p_node]) + pt2pt_dis(Y.row(next_max_p_node), X.row(i)), 2); + } + } + else { + for (int j = 0; j < next_max_p_node; j ++) { + pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[next_max_p_node]) + pt2pt_dis(Y.row(next_max_p_node), X.row(i)), 2); + } + for (int j = max_p_node; j < M; j ++) { + pts_dis_sq_geodesic(j, i) = pow(abs(converted_node_coord[j] - converted_node_coord[max_p_node]) + pt2pt_dis(Y.row(max_p_node), X.row(i)), 2); } } - - // update P - P = (-0.5 * pts_dis_sq_geodesic / sigma2).array().exp(); - // P = P.array().rowwise() / (P.colwise().sum().array() + c); - } - else { - P = P_stored.replicate(1, 1); } + // update P + P = (-0.5 * pts_dis_sq_geodesic / sigma2).array().exp(); + // modified membership probability (adapted from cdcpd) if (visible_nodes.size() != Y.rows() && !visible_nodes.empty() && k_vis != 0) { @@ -420,11 +357,8 @@ bool trackdlo::cpd_lle (MatrixXd X, P = P.array().rowwise() / (P.colwise().sum().array() + c); } - // test code when not using pvis - // P = P.array().rowwise() / (P.colwise().sum().array() + c); - // std::cout << P.colwise().sum() << std::endl; - MatrixXd Pt1 = P.colwise().sum(); // this should have shape (N,) or (1, N) + MatrixXd Pt1 = P.colwise().sum(); MatrixXd P1 = P.rowwise().sum(); double Np = P1.sum(); MatrixXd PX = P * X; @@ -433,7 +367,7 @@ bool trackdlo::cpd_lle (MatrixXd X, MatrixXd A_matrix; MatrixXd B_matrix; if (include_lle) { - if (use_ecpd) { + if (correspondence_priors.size() != 0) { A_matrix = P1.asDiagonal()*G + lambda*sigma2 * MatrixXd::Identity(M, M) + sigma2*lle_weight * H*G + alpha*J*G; B_matrix = PX - P1.asDiagonal()*Y_0 - sigma2*lle_weight * H*Y_0 + alpha*(Y_extended - Y_0); } @@ -443,7 +377,7 @@ bool trackdlo::cpd_lle (MatrixXd X, } } else { - if (use_ecpd) { + if (correspondence_priors.size() != 0) { A_matrix = P1.asDiagonal() * G + lambda * sigma2 * MatrixXd::Identity(M, M) + alpha*J*G; B_matrix = PX - P1.asDiagonal() * Y_0 + alpha*(Y_extended - Y_0); } @@ -655,7 +589,7 @@ std::vector trackdlo::traverse_euclidean (std::vector geodesic int seg_dist_it = 0; MatrixXd cur_center = guide_nodes.row(0); - // basically pure pursuit lol + // basically pure pursuit while (last_found_index+1 <= consecutive_visible_nodes.size()-1 && seg_dist_it+1 <= geodesic_coord.size()-1) { double look_ahead_dist = fabs(geodesic_coord[seg_dist_it+1] - geodesic_coord[seg_dist_it]); bool found_intersection = false; @@ -730,7 +664,7 @@ std::vector trackdlo::traverse_euclidean (std::vector geodesic int seg_dist_it = geodesic_coord.size()-1; MatrixXd cur_center = guide_nodes.row(guide_nodes.rows()-1); - // basically pure pursuit lol + // basically pure pursuit while (last_found_index-1 >= (guide_nodes.rows() - consecutive_visible_nodes.size()) && seg_dist_it-1 >= 0) { double look_ahead_dist = fabs(geodesic_coord[seg_dist_it] - geodesic_coord[seg_dist_it-1]); @@ -808,7 +742,7 @@ std::vector trackdlo::traverse_euclidean (std::vector geodesic int seg_dist_it = visible_nodes[alignment_node_idx]; MatrixXd cur_center = guide_nodes.row(alignment_node_idx); - // basically pure pursuit lol + // basically pure pursuit while (last_found_index+1 <= alignment_node_idx+consecutive_visible_nodes_2.size()-1 && seg_dist_it+1 <= geodesic_coord.size()-1) { double look_ahead_dist = fabs(geodesic_coord[seg_dist_it+1] - geodesic_coord[seg_dist_it]); bool found_intersection = false; @@ -879,7 +813,7 @@ std::vector trackdlo::traverse_euclidean (std::vector geodesic seg_dist_it = visible_nodes[alignment_node_idx]; cur_center = guide_nodes.row(alignment_node_idx); - // basically pure pursuit lol + // basically pure pursuit while (last_found_index-1 >= alignment_node_idx-consecutive_visible_nodes_1.size() && seg_dist_it-1 >= 0) { double look_ahead_dist = fabs(geodesic_coord[seg_dist_it] - geodesic_coord[seg_dist_it-1]); bool found_intersection = false; @@ -964,8 +898,7 @@ 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, 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); + cpd_lle(X, guide_nodes_, sigma2_pre_proc, 3, 1, lle_weight_, mu_, 50, tol_, true); if (visible_nodes_extended.size() == Y_.rows()) { if (visible_nodes.size() == visible_nodes_extended.size()) { @@ -1038,5 +971,5 @@ void trackdlo::tracking_step (MatrixXd X, } // include_lle == false because we have no space to discuss it in the paper - cpd_lle (X, Y_, sigma2_, beta_, lambda_, lle_weight_, mu_, max_iter_, tol_, include_lle_, use_geodesic_, use_prev_sigma2_, true, correspondence_priors_, alpha_, kernel_, visible_nodes_extended, k_vis_, visibility_threshold_); + cpd_lle (X, Y_, sigma2_, beta_, lambda_, lle_weight_, mu_, max_iter_, tol_, false, correspondence_priors_, alpha_, visible_nodes_extended, k_vis_, visibility_threshold_); } \ No newline at end of file diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index 2b902b8..6e3f3a9 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -25,10 +25,7 @@ Mat occlusion_mask; bool updated_opencv_mask = false; MatrixXd proj_matrix(3, 4); -double total_len = 0; - bool multi_color_dlo; -bool gltp; double visibility_threshold; int dlo_pixel_width; double beta; @@ -39,10 +36,6 @@ double mu; int max_iter; double tol; double k_vis; -bool include_lle; -bool use_geodesic; -bool use_prev_sigma2; -int kernel; double downsample_leaf_size; std::string camera_info_topic; @@ -131,7 +124,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons if (!initialized) { if (received_init_nodes && received_proj_matrix) { - tracker = trackdlo(init_nodes.rows(), visibility_threshold, beta, lambda, alpha, lle_weight, k_vis, mu, max_iter, tol, include_lle, use_geodesic, use_prev_sigma2, kernel); + tracker = trackdlo(init_nodes.rows(), visibility_threshold, beta, lambda, alpha, k_vis, mu, max_iter, tol, lle_weight); sigma2 = 0.001; @@ -347,10 +340,6 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons double y1 = row_1; double x2 = col_2; double y2 = row_2; - // x1 = x1 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (x2 - x1); - // y1 = y1 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (y2 - y1); - // x2 = x2 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (x1 - x2); - // y2 = y2 + (0.25*dlo_pixel_width) / sqrt(pow(x1-x2, 2) + pow(y1-y2, 2)) * (y1 - y2); cv::line(projected_edges, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(255, 255, 255), dlo_pixel_width); } @@ -371,15 +360,11 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons } visible_nodes_extended.push_back(visible_nodes[visible_nodes.size()-1]); - if (!gltp) { - tracker.tracking_step(X_pruned, visible_nodes, visible_nodes_extended, proj_matrix, mask.rows, mask.cols); - Y = tracker.get_tracking_result(); - guide_nodes = tracker.get_guide_nodes(); - priors = tracker.get_correspondence_pairs(); - } - else { - tracker.cpd_lle(X_pruned, Y, sigma2, 1, 1, 1, mu, 50, tol, true, false, true); - } + // step tracker + tracker.tracking_step(X_pruned, visible_nodes, visible_nodes_extended, proj_matrix, mask.rows, mask.cols); + Y = tracker.get_tracking_result(); + guide_nodes = tracker.get_guide_nodes(); + priors = tracker.get_correspondence_pairs(); // log time time_diff = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - cur_time).count() / 1000.0; @@ -530,20 +515,15 @@ int main(int argc, char **argv) { nh.getParam("/trackdlo/beta", beta); nh.getParam("/trackdlo/lambda", lambda); nh.getParam("/trackdlo/alpha", alpha); - nh.getParam("/trackdlo/lle_weight", lle_weight); nh.getParam("/trackdlo/mu", mu); nh.getParam("/trackdlo/max_iter", max_iter); nh.getParam("/trackdlo/tol", tol); nh.getParam("/trackdlo/k_vis", k_vis); - nh.getParam("/trackdlo/include_lle", include_lle); - nh.getParam("/trackdlo/use_geodesic", use_geodesic); - nh.getParam("/trackdlo/use_prev_sigma2", use_prev_sigma2); - nh.getParam("/trackdlo/kernel", kernel); - - nh.getParam("/trackdlo/multi_color_dlo", multi_color_dlo); - nh.getParam("/trackdlo/gltp", gltp); nh.getParam("/trackdlo/visibility_threshold", visibility_threshold); nh.getParam("/trackdlo/dlo_pixel_width", dlo_pixel_width); + nh.getParam("/trackdlo/lle_weight", lle_weight); + + nh.getParam("/trackdlo/multi_color_dlo", multi_color_dlo); nh.getParam("/trackdlo/downsample_leaf_size", downsample_leaf_size); nh.getParam("/trackdlo/camera_info_topic", camera_info_topic); From 3c920c03713c0501f96389bf94ca488eca61faa4 Mon Sep 17 00:00:00 2001 From: Jingyi Xiang Date: Sat, 12 Aug 2023 10:59:00 -0500 Subject: [PATCH 2/3] made all parameters mentioned in the paper configurable --- launch/trackdlo.launch | 15 ++++++++++--- trackdlo/include/trackdlo.h | 10 ++++++--- trackdlo/src/trackdlo.cpp | 40 +++++++++++++++++++++++++++------- trackdlo/src/trackdlo_node.cpp | 30 +++++++++---------------- 4 files changed, 61 insertions(+), 34 deletions(-) diff --git a/launch/trackdlo.launch b/launch/trackdlo.launch index 20e131e..d8dc402 100644 --- a/launch/trackdlo.launch +++ b/launch/trackdlo.launch @@ -42,13 +42,22 @@ - + + + + + + + + + + + + - - diff --git a/trackdlo/include/trackdlo.h b/trackdlo/include/trackdlo.h index a155307..4d0e13e 100644 --- a/trackdlo/include/trackdlo.h +++ b/trackdlo/include/trackdlo.h @@ -66,6 +66,8 @@ class trackdlo double mu, int max_iter, double tol, + double beta_pre_proc, + double lambda_pre_proc, double lle_weight); double get_sigma2(); @@ -76,7 +78,7 @@ class trackdlo void initialize_nodes (MatrixXd Y_init); void set_sigma2 (double sigma2); - bool cpd_lle (MatrixXd X, + bool cpd_lle (MatrixXd X_orig, MatrixXd& Y, double& sigma2, double beta, @@ -84,7 +86,7 @@ class trackdlo double lle_weight, double mu, int max_iter = 30, - double tol = 0.00001, + double tol = 0.0001, bool include_lle = true, std::vector correspondence_priors = {}, double alpha = 0, @@ -92,7 +94,7 @@ class trackdlo double k_vis = 0, double visibility_threshold = 0.01); - void tracking_step (MatrixXd X, + void tracking_step (MatrixXd X_orig, std::vector visible_nodes, std::vector visible_nodes_extended, MatrixXd proj_matrix, @@ -104,7 +106,9 @@ class trackdlo MatrixXd guide_nodes_; double sigma2_; double beta_; + double beta_pre_proc_; double lambda_; + double lambda_pre_proc_; double alpha_; double k_vis_; double mu_; diff --git a/trackdlo/src/trackdlo.cpp b/trackdlo/src/trackdlo.cpp index 54e1c07..70717a5 100644 --- a/trackdlo/src/trackdlo.cpp +++ b/trackdlo/src/trackdlo.cpp @@ -12,8 +12,10 @@ trackdlo::trackdlo(int num_of_nodes) { Y_ = MatrixXd::Zero(num_of_nodes, 3); guide_nodes_ = Y_.replicate(1, 1); sigma2_ = 0.0; - beta_ = 1.0; + beta_ = 5.0; + beta_pre_proc_ = 3.0; lambda_ = 1.0; + lambda_pre_proc_ = 1.0; alpha_ = 0.0; lle_weight_ = 1.0; k_vis_ = 0.0; @@ -34,6 +36,8 @@ trackdlo::trackdlo(int num_of_nodes, double mu, int max_iter, double tol, + double beta_pre_proc, + double lambda_pre_proc, double lle_weight) { Y_ = MatrixXd::Zero(num_of_nodes, 3); @@ -41,7 +45,9 @@ trackdlo::trackdlo(int num_of_nodes, guide_nodes_ = Y_.replicate(1, 1); sigma2_ = 0.0; beta_ = beta; + beta_pre_proc_ = beta_pre_proc; lambda_ = lambda; + lambda_pre_proc_ = lambda_pre_proc; alpha_ = alpha; lle_weight_ = lle_weight; k_vis_ = k_vis; @@ -152,7 +158,7 @@ MatrixXd trackdlo::calc_LLE_weights (int k, MatrixXd X) { return W; } -bool trackdlo::cpd_lle (MatrixXd X, +bool trackdlo::cpd_lle (MatrixXd X_orig, MatrixXd& Y, double& sigma2, double beta, @@ -168,6 +174,25 @@ bool trackdlo::cpd_lle (MatrixXd X, double k_vis, double visibility_threshold) { + // prune X + MatrixXd X_temp = MatrixXd::Zero(X_orig.rows(), 3); + int valid_pt_counter = 0; + for (int i = 0; i < X_orig.rows(); i ++) { + // find shortest distance between this point and any node + double shortest_dist = 100000; + for (int j = 0; j < Y.rows(); j ++) { + double dist = (Y.row(j) - X_orig.row(i)).norm(); + if (dist < shortest_dist) { + shortest_dist = dist; + } + } + // require a point to be sufficiently close to the node set to be valid + if (shortest_dist < 0.1) { + X_temp.row(valid_pt_counter) = X_orig.row(i); + valid_pt_counter += 1; + } + } + MatrixXd X = X_temp.topRows(valid_pt_counter); bool converged = true; @@ -872,7 +897,7 @@ std::vector trackdlo::traverse_euclidean (std::vector geodesic return node_pairs; } -void trackdlo::tracking_step (MatrixXd X, +void trackdlo::tracking_step (MatrixXd X_orig, std::vector visible_nodes, std::vector visible_nodes_extended, MatrixXd proj_matrix, @@ -898,7 +923,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, lle_weight_, mu_, 50, tol_, true); + // pre-processing registration + cpd_lle(X_orig, guide_nodes_, sigma2_pre_proc, beta_pre_proc_, lambda_pre_proc_, lle_weight_, mu_, max_iter_, tol_, true); if (visible_nodes_extended.size() == Y_.rows()) { if (visible_nodes.size() == visible_nodes_extended.size()) { @@ -908,11 +934,9 @@ void trackdlo::tracking_step (MatrixXd X, ROS_INFO("Minor occlusion"); } - // get priors vec + // remap visible node locations std::vector priors_vec_1 = traverse_euclidean(geodesic_coord_, guide_nodes_, visible_nodes_extended, 0); std::vector priors_vec_2 = traverse_euclidean(geodesic_coord_, guide_nodes_, visible_nodes_extended, 1); - // std::vector priors_vec_1 = traverse_geodesic(geodesic_coord, guide_nodes, visible_nodes, 0); - // std::vector priors_vec_2 = traverse_geodesic(geodesic_coord, guide_nodes, visible_nodes, 1); // priors vec 2 goes from last index -> first index std::reverse(priors_vec_2.begin(), priors_vec_2.end()); @@ -971,5 +995,5 @@ void trackdlo::tracking_step (MatrixXd X, } // include_lle == false because we have no space to discuss it in the paper - cpd_lle (X, Y_, sigma2_, beta_, lambda_, lle_weight_, mu_, max_iter_, tol_, false, correspondence_priors_, alpha_, visible_nodes_extended, k_vis_, visibility_threshold_); + cpd_lle (X_orig, Y_, sigma2_, beta_, lambda_, lle_weight_, mu_, max_iter_, tol_, false, correspondence_priors_, alpha_, visible_nodes_extended, k_vis_, visibility_threshold_); } \ No newline at end of file diff --git a/trackdlo/src/trackdlo_node.cpp b/trackdlo/src/trackdlo_node.cpp index 6e3f3a9..2bf240b 100644 --- a/trackdlo/src/trackdlo_node.cpp +++ b/trackdlo/src/trackdlo_node.cpp @@ -29,13 +29,16 @@ bool multi_color_dlo; double visibility_threshold; int dlo_pixel_width; double beta; +double beta_pre_proc; double lambda; +double lambda_pre_proc; double alpha; double lle_weight; double mu; int max_iter; double tol; double k_vis; +double d_vis; double downsample_leaf_size; std::string camera_info_topic; @@ -124,7 +127,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons if (!initialized) { if (received_init_nodes && received_proj_matrix) { - tracker = trackdlo(init_nodes.rows(), visibility_threshold, beta, lambda, alpha, k_vis, mu, max_iter, tol, lle_weight); + tracker = trackdlo(init_nodes.rows(), visibility_threshold, beta, lambda, alpha, k_vis, mu, max_iter, tol, beta_pre_proc, lambda_pre_proc, lle_weight); sigma2 = 0.001; @@ -252,8 +255,6 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons // for each point in X, determine its shortest distance to Y std::map shortest_node_pt_dists; std::vector shortest_pt_node_dists(X.rows(), 100000.0); - MatrixXd X_temp = MatrixXd::Zero(X.rows(), 3); - int valid_pt_counter = 0; for (int m = 0; m < Y.rows(); m ++) { int closest_pt_idx = 0; double shortest_dist = 100000; @@ -270,24 +271,10 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons if (dist < shortest_pt_node_dists[n]) { shortest_pt_node_dists[n] = dist; } - // count valid point in the last iteration - if (m == Y.rows()-1) { - // assumption: DLO movement between frames is small - // get rid of points too far away from the node point set Y^{t-1} - // this is necessary because exp(-dist/sigma2) can become too small to represent in MatrixXd - // if a point x_n in X is too far away from all nodes, all entries in row n of P will be zero, - // even if they technically aren't all zeros - if (shortest_pt_node_dists[n] < 0.05) { - X_temp.row(valid_pt_counter) = X.row(n); - valid_pt_counter += 1; - } - } } shortest_node_pt_dists.insert(std::pair(m, shortest_dist)); } - MatrixXd X_pruned = X_temp.topRows(valid_pt_counter); - // for current nodes and edges in Y, sort them based on how far away they are from the camera std::vector averaged_node_camera_dists = {}; std::vector indices_vec = {}; @@ -351,8 +338,8 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons std::vector visible_nodes_extended = {}; for (int i = 0; i < visible_nodes.size()-1; i ++) { visible_nodes_extended.push_back(visible_nodes[i]); - // extend two nodes - if (visible_nodes[i+1] - visible_nodes[i] <= 3) { + // extend visible nodes + if (fabs(converted_node_coord[visible_nodes[i+1]] - converted_node_coord[visible_nodes[i]]) <= d_vis) { for (int j = 1; j < visible_nodes[i+1] - visible_nodes[i]; j ++) { visible_nodes_extended.push_back(visible_nodes[i] + j); } @@ -361,7 +348,7 @@ sensor_msgs::ImagePtr Callback(const sensor_msgs::ImageConstPtr& image_msg, cons visible_nodes_extended.push_back(visible_nodes[visible_nodes.size()-1]); // step tracker - tracker.tracking_step(X_pruned, visible_nodes, visible_nodes_extended, proj_matrix, mask.rows, mask.cols); + tracker.tracking_step(X, visible_nodes, visible_nodes_extended, proj_matrix, mask.rows, mask.cols); Y = tracker.get_tracking_result(); guide_nodes = tracker.get_guide_nodes(); priors = tracker.get_correspondence_pairs(); @@ -519,8 +506,11 @@ int main(int argc, char **argv) { nh.getParam("/trackdlo/max_iter", max_iter); nh.getParam("/trackdlo/tol", tol); nh.getParam("/trackdlo/k_vis", k_vis); + nh.getParam("/trackdlo/d_vis", d_vis); nh.getParam("/trackdlo/visibility_threshold", visibility_threshold); nh.getParam("/trackdlo/dlo_pixel_width", dlo_pixel_width); + nh.getParam("/trackdlo/beta_pre_proc", beta_pre_proc); + nh.getParam("/trackdlo/lambda_pre_proc", lambda_pre_proc); nh.getParam("/trackdlo/lle_weight", lle_weight); nh.getParam("/trackdlo/multi_color_dlo", multi_color_dlo); From e2ffb0c3ba4e8be90ccc31d0da397e362be96851 Mon Sep 17 00:00:00 2001 From: Jingyi Xiang Date: Sat, 12 Aug 2023 17:47:13 -0500 Subject: [PATCH 3/3] updated readme --- README.md | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index ff18fdd..6809a78 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,11 @@ -

- +

+

-This repository contains the TrackDLO Robot Operating System (ROS) package. The TrackDLO ROS package is an implementation of our paper *TrackDLO: Tracking Deformable Linear Objects Under Occlusion with Motion Coherence* (under review) by Jingyi Xiang, Holly Dinkel, Harry Zhao, Naixiang Gao, Brian Coltin, Trey Smith, and Timothy Bretl. The TrackDLO algorithm is implemented in C++. +## TrackDLO: Tracking Deformable Linear Objects Under Occlusion with Motion Coherence +**IEEE Robotics and Automation Letters (IEEE Xplore: https://ieeexplore.ieee.org/document/10214157)** + +This repository contains the TrackDLO Robot Operating System (ROS) package. The TrackDLO ROS package is an implementation of our paper, *TrackDLO: Tracking Deformable Linear Objects Under Occlusion with Motion Coherence*, by Jingyi Xiang, Holly Dinkel, Harry Zhao, Naixiang Gao, Brian Coltin, Trey Smith, and Timothy Bretl. The TrackDLO algorithm is implemented in C++.

@@ -19,14 +22,17 @@ See the [requirements and run instructions](https://github.com/RMDLO/trackdlo/bl To learn more about TrackDLO, read our [supplemental documentation](https://github.com/RMDLO/trackdlo/blob/master/docs/LEARN_MORE.md). - \ No newline at end of file +``` \ No newline at end of file