From 1c7bc01b712babe0acd05bb946785c8eeac649fb Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Mon, 18 Nov 2024 16:02:28 -0800 Subject: [PATCH] MarkerMultiBeamSearch updates --- dart/biomechanics/MarkerMultiBeamSearch.cpp | 543 +++++++++++++----- dart/biomechanics/MarkerMultiBeamSearch.hpp | 68 ++- dart/external/odelcpsolver/lcp.h | 28 +- .../biomechanics/MarkerMultiBeamSearch.cpp | 67 ++- .../biomechanics/__init__.pyi | 29 +- unittests/unit/test_MarkerMultiBeamSearch.cpp | 177 +++++- 6 files changed, 695 insertions(+), 217 deletions(-) diff --git a/dart/biomechanics/MarkerMultiBeamSearch.cpp b/dart/biomechanics/MarkerMultiBeamSearch.cpp index d5dec9453..4a850cf86 100644 --- a/dart/biomechanics/MarkerMultiBeamSearch.cpp +++ b/dart/biomechanics/MarkerMultiBeamSearch.cpp @@ -3,8 +3,12 @@ #include "dart/biomechanics/MarkerMultiBeamSearch.hpp" #include +#include #include +#include +#include #include +#include #include namespace dart { @@ -16,75 +20,17 @@ TraceHead::TraceHead( bool observed_this_timestep, const Eigen::Vector3d& last_observed_point, double last_observed_timestamp, + int last_observed_index, const Eigen::Vector3d& last_observed_velocity, - Eigen::VectorXd distances_to_other_traces, std::shared_ptr parent) : label(label), observed_this_timestep(observed_this_timestep), last_observed_point(last_observed_point), last_observed_timestamp(last_observed_timestamp), + last_observed_index(last_observed_index), last_observed_velocity(last_observed_velocity), parent(parent) { - if (parent != nullptr) - { - if (parent->num_distance_samples > 0) - { - num_distance_samples = parent->num_distance_samples + 1; - Eigen::VectorXd delta - = distances_to_other_traces - parent->distances_to_other_traces_mean; - distances_to_other_traces_m2 - = parent->distances_to_other_traces_m2 - + delta.cwiseProduct( - distances_to_other_traces - - parent->distances_to_other_traces_mean); - distances_to_other_traces_mean = parent->distances_to_other_traces_mean - + delta / (double)num_distance_samples; - } - else - { - num_distance_samples = 1; - distances_to_other_traces_mean = distances_to_other_traces; - distances_to_other_traces_m2 - = Eigen::VectorXd::Zero(distances_to_other_traces.size()); - } - } - else - { - num_distance_samples = 1; - distances_to_other_traces_mean = distances_to_other_traces; - distances_to_other_traces_m2 - = Eigen::VectorXd::Zero(distances_to_other_traces.size()); - } -} - -//============================================================================== -TraceHead::TraceHead( - const std::string& label, - bool observed_this_timestep, - const Eigen::Vector3d& last_observed_point, - double last_observed_timestamp, - const Eigen::Vector3d& last_observed_velocity, - std::shared_ptr parent) - : label(label), - observed_this_timestep(observed_this_timestep), - last_observed_point(last_observed_point), - last_observed_timestamp(last_observed_timestamp), - last_observed_velocity(last_observed_velocity), - parent(parent) -{ - if (parent != nullptr) - { - num_distance_samples = parent->num_distance_samples; - distances_to_other_traces_mean = parent->distances_to_other_traces_mean; - distances_to_other_traces_m2 = parent->distances_to_other_traces_m2; - } - else - { - num_distance_samples = 0; - distances_to_other_traces_mean = Eigen::VectorXd::Zero(0); - distances_to_other_traces_m2 = Eigen::VectorXd::Zero(0); - } } //============================================================================== @@ -115,12 +61,21 @@ MarkerMultiBeamSearch::MarkerMultiBeamSearch( const std::vector& seed_points, const std::vector& seed_labels, double seed_timestamp, + int seed_index, + Eigen::MatrixXd pairwise_distances, + double pair_weight, + double pair_threshold, + double vel_weight, double vel_threshold, - double acc_threshold, - double acc_scaling) - : vel_threshold(vel_threshold), - acc_threshold(acc_threshold), - acc_scaling(acc_scaling) + double acc_weight, + double acc_threshold) + : pairwise_distances(pairwise_distances), + pair_weight(pair_weight), + pair_threshold(pair_threshold), + vel_weight(vel_weight), + vel_threshold(vel_threshold), + acc_weight(acc_weight), + acc_threshold(acc_threshold) { std::vector> trace_heads; for (size_t i = 0; i < seed_points.size(); ++i) @@ -131,6 +86,7 @@ MarkerMultiBeamSearch::MarkerMultiBeamSearch( true, seed_points[i], seed_timestamp, + seed_index, zero_velocity, nullptr); trace_heads.push_back(trace_head); @@ -143,13 +99,16 @@ MarkerMultiBeamSearch::MarkerMultiBeamSearch( void MarkerMultiBeamSearch::make_next_generation( const std::map& markers, double timestamp, - int trace_head_to_attach) + int index, + int trace_head_to_attach, + int beam_width) { std::vector> new_beams; for (const auto& beam : beams) { const std::shared_ptr& trace_head = beam->trace_heads[trace_head_to_attach]; + const double delta_time = timestamp - trace_head->last_observed_timestamp; std::set timestep_used_markers = beam->timestep_used_markers; if (trace_head_to_attach == 0) @@ -159,19 +118,39 @@ void MarkerMultiBeamSearch::make_next_generation( // Option 1: Skip adding a marker double skip_cost - = beam->cost + vel_threshold + (acc_threshold * acc_scaling); - std::shared_ptr skip_trace_head = std::make_shared( - trace_head->label, - false, - trace_head->last_observed_point, - trace_head->last_observed_timestamp, - trace_head->last_observed_velocity, - trace_head); - std::vector> child_trace_heads - = beam->get_child_trace_heads(skip_trace_head, trace_head_to_attach); - auto new_beam_ptr = std::make_shared( - skip_cost, child_trace_heads, timestep_used_markers); - new_beams.push_back(new_beam_ptr); + = beam->cost + (vel_threshold * vel_weight) + + (acc_threshold * acc_weight) + // Measure your distance to all previous trace_heads, but not yourself + // or subsequent trace_heads that have not been chosen yet. Hence, + // this is `trace_head_to_attach` penalties. + + (pair_threshold * pair_weight * trace_head_to_attach); + if (new_beams.size() < beam_width || skip_cost < new_beams.end()[-1]->cost) + { + std::shared_ptr skip_trace_head = std::make_shared( + trace_head->label, + false, + trace_head->last_observed_point, + trace_head->last_observed_timestamp, + trace_head->last_observed_index, + trace_head->last_observed_velocity, + trace_head); + std::vector> child_trace_heads + = beam->get_child_trace_heads(skip_trace_head, trace_head_to_attach); + auto new_beam_ptr = std::make_shared( + skip_cost, child_trace_heads, timestep_used_markers); + new_beams.push_back(new_beam_ptr); + std::sort( + new_beams.begin(), + new_beams.end(), + [](const std::shared_ptr& a, + const std::shared_ptr& b) { + return a->cost < b->cost; + }); + if (new_beams.size() > beam_width) + { + new_beams.resize(beam_width); + } + } // Option 2: Add each possible marker for (const auto& marker : markers) @@ -184,83 +163,64 @@ void MarkerMultiBeamSearch::make_next_generation( continue; } - double delta_time = timestamp - trace_head->last_observed_timestamp; Eigen::Vector3d velocity = (point - trace_head->last_observed_point) / delta_time; Eigen::Vector3d acc = (velocity - trace_head->last_observed_velocity) / delta_time; double vel_mag = velocity.norm(); - if (vel_mag < 2 * vel_threshold) + double acc_mag = acc.norm(); + double cost + = beam->cost + (vel_mag * vel_weight) + (acc_mag * acc_weight); + if (new_beams.size() == beam_width && cost > new_beams.end()[-1]->cost) { - double acc_mag = acc.norm(); - double cost = beam->cost + vel_mag + (acc_mag * acc_scaling); + continue; + } - Eigen::VectorXd distances_to_other_traces - = Eigen::VectorXd::Zero(beam->trace_heads.size()); - for (size_t i = 0; i < beam->trace_heads.size(); ++i) + // Compare our distances to all previous trace_heads that have already + // (potentially) been attached. + for (size_t i = 0; i < trace_head_to_attach; ++i) + { + // If this trace head was attached this frame, take a penalty on the + // observed distance + if (beam->trace_heads[i]->last_observed_index == index) { - distances_to_other_traces[i] + double distance = (beam->trace_heads[i]->last_observed_point - point).norm(); + cost += pair_weight + * std::abs( + pairwise_distances(i, trace_head_to_attach) - distance); } - if (trace_head->num_distance_samples > 1000) + else { - for (int i = 0; i < distances_to_other_traces.size(); ++i) - { - if (i == trace_head_to_attach) - { - continue; - } - // Only penalize marker pairs that are closer than 10cm on mean - if (trace_head->distances_to_other_traces_mean[i] > 0.1) - { - continue; - } - double stddev = std::sqrt( - trace_head->distances_to_other_traces_m2[i] - / (trace_head->num_distance_samples - 1)); - if (stddev < 0.001) - { - stddev = 0.001; - } - double error = std::abs( - distances_to_other_traces[i] - - trace_head->distances_to_other_traces_mean[i]) - / stddev; - if (error > 5.0 && stddev < 0.007) - { - // std::cout << "Error: " << error << ", stddev: " << stddev - // << ", mean: " - // << trace_head->distances_to_other_traces_mean[i] - // << ", distance: " << distances_to_other_traces[i] - // << " between next marker " << label << " (on trace " - // << trace_head_to_attach << "=" << trace_head->label - // << " with implied velocity " << vel_mag << ")" - // << " and " << beam->trace_heads[i]->label << " (trace - // " - // << i << ")" << std::endl; - // skip_beam = true; - cost += error * 1000.0; - } - } + // Otherwise just take a penalty on the threshold distance + cost += pair_threshold * pair_weight; } + } + if (new_beams.size() == beam_width && cost > new_beams.end()[-1]->cost) + { + continue; + } - std::shared_ptr new_trace_head = std::make_shared( - label, - true, - point, - timestamp, - velocity, - distances_to_other_traces, - trace_head); - std::set new_timestep_used_markers = timestep_used_markers; - new_timestep_used_markers.insert(label); - - std::vector> child_trace_heads - = beam->get_child_trace_heads(new_trace_head, trace_head_to_attach); - auto new_beam_ptr = std::make_shared( - cost, child_trace_heads, new_timestep_used_markers); - new_beams.push_back(new_beam_ptr); + std::shared_ptr new_trace_head = std::make_shared( + label, true, point, timestamp, index, velocity, trace_head); + std::set new_timestep_used_markers = timestep_used_markers; + new_timestep_used_markers.insert(label); + + std::vector> child_trace_heads + = beam->get_child_trace_heads(new_trace_head, trace_head_to_attach); + new_beams.emplace_back(std::make_shared( + cost, child_trace_heads, new_timestep_used_markers)); + std::sort( + new_beams.begin(), + new_beams.end(), + [](const std::shared_ptr& a, + const std::shared_ptr& b) { + return a->cost < b->cost; + }); + if (new_beams.size() > beam_width) + { + new_beams.resize(beam_width); } } } @@ -372,6 +332,74 @@ void MarkerMultiBeamSearch::crystallize_beams(bool include_last) beams.resize(1); } +//============================================================================== +double MarkerMultiBeamSearch::get_median_70_percent_mean_distance( + std::string a_label, + std::string b_label, + const std::vector>& + marker_observations) +{ + // Figure out the distance between the markers + std::vector observed_distances; + for (size_t t = 0; t < marker_observations.size(); ++t) + { + const auto& marker_timestep = marker_observations[t]; + if (marker_timestep.find(a_label) != marker_timestep.end() + && marker_timestep.find(b_label) != marker_timestep.end()) + { + double dist + = (marker_timestep.at(a_label) - marker_timestep.at(b_label)).norm(); + observed_distances.push_back(dist); + } + } + + // Calculate the median + std::vector sorted_distances = observed_distances; + std::sort(sorted_distances.begin(), sorted_distances.end()); + size_t n = sorted_distances.size(); + double median + = (n % 2 == 0) + ? (sorted_distances[n / 2 - 1] + sorted_distances[n / 2]) / 2.0 + : sorted_distances[n / 2]; + + // Calculate absolute distances from the median + std::vector distance_to_median; + for (double dist : observed_distances) + { + distance_to_median.push_back(std::abs(dist - median)); + } + + // Select the 70% of data closest to the median + std::vector indices(distance_to_median.size()); + std::iota(indices.begin(), indices.end(), 0); + std::sort( + indices.begin(), + indices.end(), + [&distance_to_median](size_t i1, size_t i2) { + return distance_to_median[i1] < distance_to_median[i2]; + }); + + size_t threshold_index = static_cast(observed_distances.size() * 0.7); + std::vector closest_70_percent; + for (size_t i = 0; i < threshold_index; ++i) + { + closest_70_percent.push_back(observed_distances[indices[i]]); + } + + if (closest_70_percent.size() == 0) + { + return 0.0; + } + + // Calculate the mean of the 70% of data closest to the median + double mean_70 + = std::accumulate( + closest_70_percent.begin(), closest_70_percent.end(), 0.0) + / closest_70_percent.size(); + + return mean_70; +} + //============================================================================== std::pair< std::vector>, @@ -382,9 +410,12 @@ MarkerMultiBeamSearch::search( marker_observations, const std::vector& timestamps, int beam_width, + double pair_weight, + double pair_threshold, + double vel_weight, double vel_threshold, + double acc_weight, double acc_threshold, - double acc_scaling, int print_interval, int crysatilize_interval) { @@ -411,25 +442,67 @@ MarkerMultiBeamSearch::search( // 2. If not found, return empty trace if (first_observation_index == -1) { + std::cout + << "Could not find first observation of all labels on the same frame" + << std::endl; + std::map label_counts; + for (size_t i = 0; i < marker_observations.size(); ++i) + { + for (const auto& label : labels) + { + if (marker_observations[i].find(label) != marker_observations[i].end()) + { + label_counts[label]++; + } + } + } + for (const auto& lc : label_counts) + { + std::cout << "Label " << lc.first << " found " << lc.second << " times" + << std::endl; + } return std::make_pair( std::vector>(), std::vector()); } - // 3. Start the beam search + // 3. Get the starting points for the beam search std::vector seed_points; for (const auto& label : labels) { seed_points.push_back( marker_observations[first_observation_index].at(label)); } + + // 4. Collect all the pairwise distance statistics for the given labels + Eigen::MatrixXd pairwise_distances(labels.size(), labels.size()); + for (size_t i = 0; i < labels.size(); ++i) + { + for (size_t j = i + 1; j < labels.size(); ++j) + { + double first_timestep_distance = (seed_points[i] - seed_points[j]).norm(); + double median_distance = get_median_70_percent_mean_distance( + labels[i], labels[j], marker_observations); + std::cout << "Distance between " << labels[i] << " and " << labels[j] + << " is " << first_timestep_distance << ", with median " + << median_distance << std::endl; + pairwise_distances(i, j) = first_timestep_distance; + pairwise_distances(j, i) = first_timestep_distance; + } + } + MarkerMultiBeamSearch beam_search( seed_points, labels, timestamps[first_observation_index], + first_observation_index, + pairwise_distances, + pair_weight, + pair_threshold, + vel_weight, vel_threshold, - acc_threshold, - acc_scaling); + acc_weight, + acc_threshold); beam_search.past_beams.reserve(marker_observations.size() * labels.size()); for (size_t i = first_observation_index + 1; i < marker_observations.size(); @@ -445,8 +518,11 @@ MarkerMultiBeamSearch::search( for (size_t j = 0; j < labels.size(); ++j) { beam_search.make_next_generation( - marker_observations[i], timestamps[i], static_cast(j)); - beam_search.prune_beams(beam_width); + marker_observations[i], + timestamps[i], + i, + static_cast(j), + beam_width); } if (i % crysatilize_interval == 0) @@ -462,5 +538,176 @@ MarkerMultiBeamSearch::search( beam_search.marker_observations, beam_search.timestamps); } +std::tuple< + std::vector>, + std::vector> +MarkerMultiBeamSearch::process_markers( + const std::vector>& label_groups, + const std::vector>& + marker_observations, + const std::vector& timestamps, + size_t beam_width, + double pair_weight, + double pair_threshold, + double vel_weight, + double vel_threshold, + double acc_weight, + double acc_threshold, + int print_interval, + int crysatilize_interval, + bool multithread) +{ + std::map marker_observation_counts; + for (const auto& marker_observation : marker_observations) + { + for (const auto& marker : marker_observation) + { + marker_observation_counts[marker.first]++; + } + } + + std::cout << "Building marker traces for " << label_groups.size() << " groups" + << std::endl; + std::vector> filtered_groups; + for (int i = 0; i < label_groups.size(); ++i) + { + std::cout << "Group " << i << ":" << std::endl; + + std::vector filtered_group; + std::vector rejected_group; + + for (const auto& label : label_groups[i]) + { + if (marker_observation_counts[label] > 0) + { + filtered_group.push_back(label); + } + else + { + rejected_group.push_back(label); + } + } + if (filtered_group.size() > 0) + { + filtered_groups.push_back(filtered_group); + std::cout << " Found Markers: "; + for (const auto& label : filtered_group) + { + std::cout << label << " "; + } + std::cout << std::endl; + } + if (rejected_group.size() > 0) + { + std::cout << " Did Not Find Markers: "; + for (const auto& label : rejected_group) + { + std::cout << label << " "; + } + std::cout << std::endl; + } + std::cout << std::endl; + } + + std::map> trace_output_map; + + if (multithread) + { + std::mutex mutex; + std::vector> futures; + for (const std::vector& label_group : filtered_groups) + { + futures.push_back(std::async([&] { + auto result = MarkerMultiBeamSearch::search( + label_group, + marker_observations, + timestamps, + beam_width, + pair_weight, + pair_threshold, + vel_weight, + vel_threshold, + acc_weight, + acc_threshold, + print_interval, + crysatilize_interval); + + { + std::lock_guard lock(mutex); + std::vector> outputTraces + = std::get<0>(result); + std::vector outputTimesteps = std::get<1>(result); + + for (size_t i = 0; i < outputTraces.size(); ++i) + { + for (const auto& label_point : outputTraces[i]) + { + trace_output_map[outputTimesteps[i]][label_point.first] + = label_point.second; + } + } + } + })); + } + + for (auto& future : futures) + { + future.wait(); + } + } + else + { + for (int g = 0; g < filtered_groups.size(); g++) + { + std::cout << "Processing group " << g << "/" << filtered_groups.size() + << ": "; + const std::vector& label_group = filtered_groups.at(g); + for (const auto& label : label_group) + { + std::cout << label << " "; + } + std::cout << std::endl; + + auto result = MarkerMultiBeamSearch::search( + label_group, + marker_observations, + timestamps, + beam_width, + pair_weight, + pair_threshold, + vel_weight, + vel_threshold, + acc_weight, + acc_threshold, + print_interval, + crysatilize_interval); + std::vector> outputTraces + = std::get<0>(result); + std::vector outputTimesteps = std::get<1>(result); + + for (size_t i = 0; i < outputTraces.size(); ++i) + { + for (const auto& label_point : outputTraces[i]) + { + trace_output_map[outputTimesteps[i]][label_point.first] + = label_point.second; + } + } + } + } + + std::cout << "Finished building marker traces" << std::endl; + + std::vector> traces; + std::vector timesteps; + for (const auto& trace : trace_output_map) + { + traces.push_back(trace.second); + timesteps.push_back(trace.first); + } + + return std::make_tuple(traces, timesteps); +} + } // namespace biomechanics } // namespace dart diff --git a/dart/biomechanics/MarkerMultiBeamSearch.hpp b/dart/biomechanics/MarkerMultiBeamSearch.hpp index 3c8934aa6..9d3930790 100644 --- a/dart/biomechanics/MarkerMultiBeamSearch.hpp +++ b/dart/biomechanics/MarkerMultiBeamSearch.hpp @@ -22,10 +22,8 @@ class TraceHead bool observed_this_timestep; Eigen::Vector3d last_observed_point; double last_observed_timestamp; + int last_observed_index; Eigen::Vector3d last_observed_velocity; - int num_distance_samples; - Eigen::VectorXd distances_to_other_traces_mean; - Eigen::VectorXd distances_to_other_traces_m2; std::weak_ptr parent; TraceHead( @@ -33,15 +31,7 @@ class TraceHead bool observed_this_timestep, const Eigen::Vector3d& last_observed_point, double last_observed_timestamp, - const Eigen::Vector3d& last_observed_velocity, - Eigen::VectorXd distances_to_other_traces, - std::shared_ptr parent = nullptr); - - TraceHead( - const std::string& label, - bool observed_this_timestep, - const Eigen::Vector3d& last_observed_point, - double last_observed_timestamp, + int last_observed_index, const Eigen::Vector3d& last_observed_velocity, std::shared_ptr parent = nullptr); }; @@ -66,9 +56,14 @@ class MarkerMultiBeamSearch { public: std::vector> beams; + Eigen::MatrixXd pairwise_distances; + + double pair_weight; + double pair_threshold; + double vel_weight; double vel_threshold; + double acc_weight; double acc_threshold; - double acc_scaling; // We try to prune the beams as we go, and store the finished marker // observations. @@ -83,14 +78,21 @@ class MarkerMultiBeamSearch const std::vector& seed_points, const std::vector& seed_labels, double seed_timestamp, - double vel_threshold = 7.0, - double acc_threshold = 2000.0, - double acc_scaling = 0.025); + int seed_index, + Eigen::MatrixXd pairwise_distances, + double pair_weight = 100.0, + double pair_threshold = 0.01, + double vel_weight = 1.0, + double vel_threshold = 5.0, + double acc_weight = 0.01, + double acc_threshold = 1000.0); void make_next_generation( const std::map& markers, double timestamp, - int trace_head_to_attach); + int index, + int trace_head_to_attach, + int beam_width); void prune_beams(int beam_width); @@ -101,6 +103,12 @@ class MarkerMultiBeamSearch void crystallize_beams(bool include_last = true); + static double get_median_70_percent_mean_distance( + std::string label_1, + std::string label_2, + const std::vector>& + marker_observations); + static std::pair< std::vector>, std::vector> @@ -110,11 +118,33 @@ class MarkerMultiBeamSearch marker_observations, const std::vector& timestamps, int beam_width = 20, - double vel_threshold = 7.0, + double pair_weight = 100.0, + double pair_threshold = 0.01, + double vel_weight = 1.0, + double vel_threshold = 5.0, + double acc_weight = 0.01, double acc_threshold = 1000.0, - double acc_scaling = 0.025, int print_interval = 1000, int crysatilize_interval = 1000); + + static std::tuple< + std::vector>, + std::vector> + process_markers( + const std::vector>& label_groups, + const std::vector>& + marker_observations, + const std::vector& timestamps, + size_t beam_width = 20, + double pair_weight = 100.0, + double pair_threshold = 0.001, + double vel_weight = 0.1, + double vel_threshold = 5.0, + double acc_weight = 0.001, + double acc_threshold = 500.0, + int print_interval = 1000, + int crysatilize_interval = 1000, + bool multithread = true); }; } // namespace biomechanics diff --git a/dart/external/odelcpsolver/lcp.h b/dart/external/odelcpsolver/lcp.h index 6a94127cf..e24fc8d63 100644 --- a/dart/external/odelcpsolver/lcp.h +++ b/dart/external/odelcpsolver/lcp.h @@ -24,9 +24,9 @@ given (A,b,lo,hi), solve the LCP problem: A*x = b+w, where each x(i),w(i) satisfies one of - (1) x = lo, w >= 0 - (2) x = hi, w <= 0 - (3) lo < x < hi, w = 0 + (1) x = lo, w >= 0 + (2) x = hi, w <= 0 + (3) lo < x < hi, w = 0 A is a matrix of dimension n*n, everything else is a vector of size n*1. lo and hi can be +/- dInfinity as needed. the first `nub' variables are unbounded, i.e. hi and lo are assumed to be +/- dInfinity. @@ -46,23 +46,31 @@ to be implemented. the first `nub' variables are assumed to have findex < 0. */ - #ifndef _ODE_LCP_H_ #define _ODE_LCP_H_ -#include -#include #include -#include "dart/external/odelcpsolver/odeconfig.h" +#include +#include + #include "dart/external/odelcpsolver/common.h" +#include "dart/external/odelcpsolver/odeconfig.h" -bool dSolveLCP (int n, dReal *A, dReal *x, dReal *b, dReal *w, - int nub, dReal *lo, dReal *hi, int *findex, bool earlyTermination = false); +bool dSolveLCP( + int n, + dReal* A, + dReal* x, + dReal* b, + dReal* w, + int nub, + dReal* lo, + dReal* hi, + int* findex, + bool earlyTermination = false); size_t dEstimateSolveLCPMemoryReq(int n, bool outer_w_avail); - extern "C" ODE_API int dTestSolveLCP(); #endif diff --git a/python/_nimblephysics/biomechanics/MarkerMultiBeamSearch.cpp b/python/_nimblephysics/biomechanics/MarkerMultiBeamSearch.cpp index 6202cafe1..4e90caa26 100644 --- a/python/_nimblephysics/biomechanics/MarkerMultiBeamSearch.cpp +++ b/python/_nimblephysics/biomechanics/MarkerMultiBeamSearch.cpp @@ -26,12 +26,14 @@ void MarkerMultiBeamSearch(py::module& m) bool, const Eigen::Vector3d&, double, + int, const Eigen::Vector3d&, std::shared_ptr>(), py::arg("label"), py::arg("observed_this_timestep"), py::arg("last_observed_point"), py::arg("last_observed_timestamp"), + py::arg("last_observed_index"), py::arg("last_observed_velocity"), py::arg("parent") = nullptr) .def_readonly("label", &dart::biomechanics::TraceHead::label) @@ -44,6 +46,9 @@ void MarkerMultiBeamSearch(py::module& m) .def_readonly( "last_observed_timestamp", &dart::biomechanics::TraceHead::last_observed_timestamp) + .def_readonly( + "last_observed_index", + &dart::biomechanics::TraceHead::last_observed_index) .def_readonly( "last_observed_velocity", &dart::biomechanics::TraceHead::last_observed_velocity) @@ -81,25 +86,41 @@ void MarkerMultiBeamSearch(py::module& m) const std::vector&, const std::vector&, double, + int, + Eigen::MatrixXd, + double, + double, + double, double, double, double>(), py::arg("seed_points"), py::arg("seed_labels"), py::arg("seed_timestamp"), - py::arg("vel_threshold") = 7.0, - py::arg("acc_threshold") = 2000.0, - py::arg("acc_scaling") = 0.025) + py::arg("seed_index"), + py::arg("pairwise_distances"), + py::arg("pair_weight") = 100.0, + py::arg("pair_threshold") = 0.01, + py::arg("vel_weight") = 1.0, + py::arg("vel_threshold") = 5.0, + py::arg("acc_weight") = 0.01, + py::arg("acc_threshold") = 1000.0) .def( "make_next_generation", &dart::biomechanics::MarkerMultiBeamSearch::make_next_generation, py::arg("markers"), py::arg("timestamp"), - py::arg("trace_head_to_attach")) + py::arg("index"), + py::arg("trace_head_to_attach"), + py::arg("beam_width")) .def( "prune_beams", &dart::biomechanics::MarkerMultiBeamSearch::prune_beams, py::arg("beam_width")) + .def( + "crysatilize_beams", + &dart::biomechanics::MarkerMultiBeamSearch::crystallize_beams, + py::arg("include_last") = true) .def_readonly("beams", &dart::biomechanics::MarkerMultiBeamSearch::beams) .def_readonly( "vel_threshold", @@ -107,10 +128,23 @@ void MarkerMultiBeamSearch(py::module& m) .def_readonly( "acc_threshold", &dart::biomechanics::MarkerMultiBeamSearch::acc_threshold) + .def_readonly( + "pair_weight", + &dart::biomechanics::MarkerMultiBeamSearch::pair_weight) .def_static( "convert_to_traces", &dart::biomechanics::MarkerMultiBeamSearch::convert_to_traces, py::arg("beam")) + .def_static( + "get_median_70_percent_mean_distance", + [](std::string label_1, + std::string label_2, + const std::vector>& + marker_observations) { + return dart::biomechanics::MarkerMultiBeamSearch:: + get_median_70_percent_mean_distance( + label_1, label_2, marker_observations); + }) .def_static( "search", &dart::biomechanics::MarkerMultiBeamSearch::search, @@ -118,11 +152,30 @@ void MarkerMultiBeamSearch(py::module& m) py::arg("marker_observations"), py::arg("timestamps"), py::arg("beam_width") = 20, - py::arg("vel_threshold") = 7.0, + py::arg("pair_weight") = 100.0, + py::arg("pair_threshold") = 0.01, + py::arg("vel_weight") = 1.0, + py::arg("vel_threshold") = 5.0, + py::arg("acc_weight") = 0.01, py::arg("acc_threshold") = 1000.0, - py::arg("acc_scaling") = 0.025, py::arg("print_interval") = 1000, - py::arg("crystalize_interval") = 1000); + py::arg("crysatilize_interval") = 1000) + .def_static( + "process_markers", + &dart::biomechanics::MarkerMultiBeamSearch::process_markers, + py::arg("label_groups"), + py::arg("marker_observations"), + py::arg("timestamps"), + py::arg("beam_width") = 20, + py::arg("pair_weight") = 100.0, + py::arg("pair_threshold") = 0.001, + py::arg("vel_weight") = 0.1, + py::arg("vel_threshold") = 5.0, + py::arg("acc_weight") = 0.001, + py::arg("acc_threshold") = 500.0, + py::arg("print_interval") = 1000, + py::arg("crysatilize_interval") = 1000, + py::arg("multithread") = true); } } // namespace python diff --git a/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi b/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi index 172c48fe2..f312951c3 100644 --- a/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi +++ b/stubs/_nimblephysics-stubs/biomechanics/__init__.pyi @@ -1851,15 +1851,15 @@ class LinkBeam(): """ pass class LinkBeamSearch(): - def __init__(self, seed_a_point: numpy.ndarray[numpy.float64, _Shape[m, 1]], seed_a_label: str, seed_b_point: numpy.ndarray[numpy.float64, _Shape[m, 1]], seed_b_label: str, seed_timestamp: float, pair_dist: float, pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 1.0, vel_threshold: float = 5.0, acc_weight: float = 0.01, acc_threshold: float = 1000.0) -> None: ... + def __init__(self, seed_a_point: numpy.ndarray[numpy.float64, _Shape[m, 1]], seed_a_label: str, seed_b_point: numpy.ndarray[numpy.float64, _Shape[m, 1]], seed_b_label: str, seed_timestamp: float, pair_dist: float, pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 1.0, vel_threshold: float = 5.0, acc_weight: float = 0.001, acc_threshold: float = 1000.0) -> None: ... @staticmethod def convert_to_traces(beam: LinkBeam) -> typing.Tuple[typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], typing.List[float], str, typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], typing.List[float], str]: ... def make_next_generation(self, markers: typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]], timestamp: float, beam_width: int) -> None: ... @staticmethod - def process_markers(label_pairs: typing.List[typing.Tuple[str, str]], marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]]], timestamps: typing.List[float], beam_width: int = 20, pair_weight: float = 100.0, pair_threshold: float = 0.001, vel_weight: float = 0.1, vel_threshold: float = 5.0, acc_weight: float = 0.001, acc_threshold: float = 500.0, print_updates: bool = True) -> typing.Tuple[typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]]], typing.List[float]]: ... + def process_markers(label_pairs: typing.List[typing.Tuple[str, str]], marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]]], timestamps: typing.List[float], beam_width: int = 5, pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 0.1, vel_threshold: float = 5.0, acc_weight: float = 0.001, acc_threshold: float = 1000.0, print_updates: bool = True, multithread: bool = True) -> typing.Tuple[typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]]], typing.List[float]]: ... def prune_beams(self, beam_width: int) -> None: ... @staticmethod - def search(a_label: str, b_label: str, marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]]], timestamps: typing.List[float], beam_width: int = 20, pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 1.0, vel_threshold: float = 5.0, acc_weight: float = 0.01, acc_threshold: float = 1000.0, print_updates: bool = True) -> typing.Tuple[typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], typing.List[float], str, typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], typing.List[float], str]: ... + def search(a_label: str, b_label: str, marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[m, 1]]]], timestamps: typing.List[float], beam_width: int = 5, pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 1.0, vel_threshold: float = 5.0, acc_weight: float = 0.001, acc_threshold: float = 1000.0, print_updates: bool = True) -> typing.Tuple[typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], typing.List[float], str, typing.List[numpy.ndarray[numpy.float64, _Shape[m, 1]]], typing.List[float], str]: ... @property def beams(self) -> typing.List[LinkBeam]: """ @@ -2205,13 +2205,18 @@ class MarkerLabellerMock(MarkerLabeller): def setMockJointLocations(self, jointsOverTime: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]]) -> None: ... pass class MarkerMultiBeamSearch(): - def __init__(self, seed_points: typing.List[numpy.ndarray[numpy.float64, _Shape[3, 1]]], seed_labels: typing.List[str], seed_timestamp: float, vel_threshold: float = 7.0, acc_threshold: float = 2000.0, acc_scaling: float = 0.025) -> None: ... + def __init__(self, seed_points: typing.List[numpy.ndarray[numpy.float64, _Shape[3, 1]]], seed_labels: typing.List[str], seed_timestamp: float, seed_index: int, pairwise_distances: numpy.ndarray[numpy.float64, _Shape[m, n]], pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 1.0, vel_threshold: float = 5.0, acc_weight: float = 0.01, acc_threshold: float = 1000.0) -> None: ... @staticmethod def convert_to_traces(beam: MultiBeam) -> typing.Tuple[typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], typing.List[float]]: ... - def make_next_generation(self, markers: typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]], timestamp: float, trace_head_to_attach: int) -> None: ... + def crysatilize_beams(self, include_last: bool = True) -> None: ... + @staticmethod + def get_median_70_percent_mean_distance(arg0: str, arg1: str, arg2: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]]) -> float: ... + def make_next_generation(self, markers: typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]], timestamp: float, index: int, trace_head_to_attach: int, beam_width: int) -> None: ... + @staticmethod + def process_markers(label_groups: typing.List[typing.List[str]], marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], timestamps: typing.List[float], beam_width: int = 20, pair_weight: float = 100.0, pair_threshold: float = 0.001, vel_weight: float = 0.1, vel_threshold: float = 5.0, acc_weight: float = 0.001, acc_threshold: float = 500.0, print_interval: int = 1000, crysatilize_interval: int = 1000, multithread: bool = True) -> typing.Tuple[typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], typing.List[float]]: ... def prune_beams(self, beam_width: int) -> None: ... @staticmethod - def search(labels: typing.List[str], marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], timestamps: typing.List[float], beam_width: int = 20, vel_threshold: float = 7.0, acc_threshold: float = 1000.0, acc_scaling: float = 0.025, print_interval: int = 1000, crystalize_interval: int = 1000) -> typing.Tuple[typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], typing.List[float]]: ... + def search(labels: typing.List[str], marker_observations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], timestamps: typing.List[float], beam_width: int = 20, pair_weight: float = 100.0, pair_threshold: float = 0.01, vel_weight: float = 1.0, vel_threshold: float = 5.0, acc_weight: float = 0.01, acc_threshold: float = 1000.0, print_interval: int = 1000, crysatilize_interval: int = 1000) -> typing.Tuple[typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], typing.List[float]]: ... @property def acc_threshold(self) -> float: """ @@ -2223,6 +2228,11 @@ class MarkerMultiBeamSearch(): :type: typing.List[MultiBeam] """ @property + def pair_weight(self) -> float: + """ + :type: float + """ + @property def vel_threshold(self) -> float: """ :type: float @@ -3220,13 +3230,18 @@ class SubjectOnDiskTrialPass(): def setVels(self, vels: numpy.ndarray[numpy.float64, _Shape[m, n]]) -> None: ... pass class TraceHead(): - def __init__(self, label: str, observed_this_timestep: bool, last_observed_point: numpy.ndarray[numpy.float64, _Shape[3, 1]], last_observed_timestamp: float, last_observed_velocity: numpy.ndarray[numpy.float64, _Shape[3, 1]], parent: TraceHead = None) -> None: ... + def __init__(self, label: str, observed_this_timestep: bool, last_observed_point: numpy.ndarray[numpy.float64, _Shape[3, 1]], last_observed_timestamp: float, last_observed_index: int, last_observed_velocity: numpy.ndarray[numpy.float64, _Shape[3, 1]], parent: TraceHead = None) -> None: ... @property def label(self) -> str: """ :type: str """ @property + def last_observed_index(self) -> int: + """ + :type: int + """ + @property def last_observed_point(self) -> numpy.ndarray[numpy.float64, _Shape[3, 1]]: """ :type: numpy.ndarray[numpy.float64, _Shape[3, 1]] diff --git a/unittests/unit/test_MarkerMultiBeamSearch.cpp b/unittests/unit/test_MarkerMultiBeamSearch.cpp index e44b6799d..6bab5e2a2 100644 --- a/unittests/unit/test_MarkerMultiBeamSearch.cpp +++ b/unittests/unit/test_MarkerMultiBeamSearch.cpp @@ -1,49 +1,174 @@ -#include // std::sort -#include +#include #include -#include #include -#include -#include "dart/biomechanics/C3DLoader.hpp" -#include "dart/biomechanics/MarkerBeamSearch.hpp" #include "dart/biomechanics/MarkerMultiBeamSearch.hpp" -#include "dart/biomechanics/OpenSimParser.hpp" -#include "dart/common/LocalResourceRetriever.hpp" -#include "dart/common/ResourceRetriever.hpp" -#include "dart/common/Uri.hpp" -#include "dart/math/MathTypes.hpp" -#include "dart/server/GUIWebsocketServer.hpp" -#include "dart/utils/C3D.hpp" -#include "dart/utils/CompositeResourceRetriever.hpp" -#include "dart/utils/DartResourceRetriever.hpp" -#include "dart/utils/PackageResourceRetriever.hpp" +using namespace std; using namespace dart; using namespace biomechanics; #define ALL_TESTS +// Test case for MarkerMultiBeamSearch constructor #ifdef ALL_TESTS -TEST(MULTI_BEAM_SEARCH, DOES_NOT_CRASH) +TEST(MarkerMultiBeamSearchTest, ConstructorInitialization) { - biomechanics::C3D c3d - = biomechanics::C3DLoader::loadC3D("dart://sample/c3d/cmu_02_05.c3d"); + std::vector seed_points + = {Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}; + std::vector seed_labels = {"A", "B"}; + double seed_timestamp = 0.0; + int seed_index = 0; + Eigen::MatrixXd pairwise_distances = Eigen::MatrixXd::Zero(2, 2); - std::vector markerNames; - for (auto& pair : c3d.markerTimesteps[0]) + MarkerMultiBeamSearch beam_search( + seed_points, seed_labels, seed_timestamp, seed_index, pairwise_distances); + + ASSERT_EQ(beam_search.beams.size(), 1); + EXPECT_EQ(beam_search.beams[0]->trace_heads[0]->label, "A"); + EXPECT_EQ(beam_search.beams[0]->trace_heads[1]->label, "B"); + EXPECT_EQ(beam_search.beams[0]->cost, 0.0); +} +#endif + +// Test case for make_next_generation +#ifdef ALL_TESTS +TEST(MarkerMultiBeamSearchTest, MakeNextGeneration) +{ + std::vector seed_points + = {Eigen::Vector3d::Zero(), Eigen::Vector3d::Ones()}; + std::vector seed_labels = {"A", "B"}; + double seed_timestamp = 0.0; + int seed_index = 0; + Eigen::MatrixXd pairwise_distances = Eigen::MatrixXd::Zero(2, 2); + + MarkerMultiBeamSearch beam_search( + seed_points, seed_labels, seed_timestamp, seed_index, pairwise_distances); + + std::map markers; + markers["new"] = 0.5 * Eigen::Vector3d::Ones(); + + beam_search.make_next_generation(markers, 1.0, 1, 0, 10); + beam_search.make_next_generation(markers, 1.0, 1, 1, 10); + + ASSERT_GT(beam_search.beams.size(), 0); + for (const auto& beam : beam_search.beams) + { + EXPECT_TRUE( + (beam->trace_heads[0]->label == "A" + && beam->trace_heads[1]->label == "B") + || (beam->trace_heads[0]->label == "A" + && beam->trace_heads[1]->label == "new") + || (beam->trace_heads[0]->label == "new" + && beam->trace_heads[1]->label == "B")); + } +} +#endif + +// Test case for prune_beams +#ifdef ALL_TESTS +TEST(MarkerMultiBeamSearchTest, PruneBeams) +{ + std::vector seed_points + = {Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}; + std::vector seed_labels = {"A", "B"}; + double seed_timestamp = 0.0; + int seed_index = 0; + Eigen::MatrixXd pairwise_distances = Eigen::MatrixXd::Zero(2, 2); + + MarkerMultiBeamSearch beam_search( + seed_points, seed_labels, seed_timestamp, seed_index, pairwise_distances); + + // Create multiple beams with different costs + for (int i = 0; i < 10; ++i) { - markerNames.push_back(pair.first); + auto trace_head = std::make_shared( + "A", + true, + Eigen::Vector3d::Random(), + seed_timestamp, + seed_index, + Eigen::Vector3d::Zero()); + auto beam = std::make_shared( + i, + std::vector>{trace_head}, + std::set()); + beam_search.beams.push_back(beam); } - std::vector> markerTimesteps; - for (int i = 0; i < std::min(c3d.markerTimesteps.size(), 3000); i++) + // Prune to 5 beams + beam_search.prune_beams(5); + + EXPECT_EQ(beam_search.beams.size(), 5); + + // Ensure beams are sorted by cost + for (size_t i = 0; i < beam_search.beams.size() - 1; ++i) { - markerTimesteps.push_back(c3d.markerTimesteps[i]); + EXPECT_LE(beam_search.beams[i]->cost, beam_search.beams[i + 1]->cost); } +} +#endif + +// Test case for convert_to_traces +#ifdef ALL_TESTS +TEST(MarkerMultiBeamSearchTest, ConvertToTraces) +{ + auto trace_head = std::make_shared( + "A", true, Eigen::Vector3d::Zero(), 0.0, 0, Eigen::Vector3d::Zero()); + + auto beam = std::make_shared( + 0.5, + std::vector>{trace_head}, + std::set()); + + auto result = MarkerMultiBeamSearch::convert_to_traces(beam); + + const auto& marker_observations = result.first; + const auto& timestamps = result.second; + + EXPECT_EQ(marker_observations.size(), 1); + EXPECT_EQ(timestamps.size(), 1); +} +#endif + +// Test case for the full search method +#ifdef ALL_TESTS +TEST(MarkerMultiBeamSearchTest, FullSearch) +{ + std::vector> marker_observations + = {{{"A", Eigen::Vector3d::Zero()}, {"B", Eigen::Vector3d::Ones()}}, + {{"A", Eigen::Vector3d::Ones()}, {"B", 2 * Eigen::Vector3d::Ones()}}}; + + std::vector timestamps = {0.0, 1.0}; auto result = MarkerMultiBeamSearch::search( - markerNames, markerTimesteps, c3d.timestamps, 20, 7.0, 1000.0, 1, 100); + {"A", "B"}, marker_observations, timestamps); + + const auto& marker_traces = result.first; + const auto& trace_timestamps = result.second; + + EXPECT_EQ(marker_traces.size(), 2); + EXPECT_EQ(trace_timestamps.size(), 2); +} +#endif + +#ifdef ALL_TESTS +TEST(MarkerMultiBeamSearchTest, SearchWithMissingLabels) +{ + std::vector> marker_observations + = {{{"A", Eigen::Vector3d::Zero()}, {"B", Eigen::Vector3d::Ones()}}, + {{"A", Eigen::Vector3d::Ones()}}}; + + std::vector timestamps = {0.0, 1.0}; + + auto result = MarkerMultiBeamSearch::search( + {"A", "B"}, marker_observations, timestamps); + + const auto& marker_traces = result.first; + const auto& trace_timestamps = result.second; + + EXPECT_EQ(marker_traces.size(), 2); + EXPECT_EQ(trace_timestamps.size(), 2); } #endif \ No newline at end of file