Add path similarity metric

moveit_core/metrics/include/moveit/metrics/robot_trajectory_metrics.hpp

@@ -48,8 +48,9 @@ namespace core
 /// \brief Calculate the path length of a given trajectory based on the accumulated robot state distances. The distance
 /// between two robot states is calculated based on the sum of active joint distances between the two states (L1 norm).
 /// This function is adapted from
-/// https://github.com/ros-planning/moveit2/blob/main/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L872 \param[in]
-/// trajectory Given robot trajectory \return Length of the robot trajectory [rad]
+/// https://github.com/ros-planning/moveit2/blob/main/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L872
+/// \param[in] trajectory Given robot trajectory
+/// \return Length of the robot trajectory [rad]
 [[nodiscard]] double calculateTrajectoryLength(robot_trajectory::RobotTrajectory const& trajectory);
 
 /// \brief Calculate the smoothness of a given trajectory
@@ -65,7 +66,14 @@ namespace core
 [[nodiscard]] std::optional<double>
 calculateTrajectoryWaypointDensity(robot_trajectory::RobotTrajectory const& trajectory);
 
-/// Correctness and clearance
 /// Similarity
+/// \brief Calculate the waypoint density of a trajectory
+/// This functionality is adapted from
+/// https://github.com/ros-planning/moveit2/blob/33b075d15f8d9ec64ca8fea98cf1573b7c581c47/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp#L970
+/// \param[in] trajectories Vector of trajectories to be compared
+/// \return Waypoint density of the given trajectory or nullopt if it is not possible to calculate the density
+//[[nodiscard]] std::optional<double>
+// calculatePathSimilarity(std::<robot_trajectory::RobotTrajectory> const& trajectories);
+
 }  // namespace core
 }  // namespace moveit

moveit_core/metrics/src/robot_trajectory_metrics.cpp

@@ -115,5 +115,72 @@ std::optional<double> calculateTrajectoryWaypointDensity(robot_trajectory::Robot
   return std::nullopt;
 }
 
+/* std::optional<double>
+calculatePathSimilarity(robot_trajectory::RobotTrajectory const& trajectories){
+  // Abort if trajectories are empty
+  if (traj_first.empty() || traj_second.empty())
+    return false;
+
+  // Waypoint counter
+  size_t pos_first = 0;
+  size_t pos_second = 0;
+  const size_t max_pos_first = traj_first.getWayPointCount() - 1;
+  const size_t max_pos_second = traj_second.getWayPointCount() - 1;
+
+  // Compute total distance between pairwise waypoints of both trajectories.
+  // The selection of waypoint pairs is based on what steps results in the minimal distance between the next pair of
+  // waypoints. We first check what steps are still possible or if we reached the end of the trajectories. Then we
+  // compute the pairwise waypoint distances of the pairs from increasing both, the first, or the second trajectory.
+  // Finally we select the pair that results in the minimal distance, summarize the total distance and iterate
+  // accordingly. After that we compute the average trajectory distance by normalizing over the number of steps.
+  double total_distance = 0;
+  size_t steps = 0;
+  double current_distance = traj_first.getWayPoint(pos_first).distance(traj_second.getWayPoint(pos_second));
+  while (true)
+  {
+    // Keep track of total distance and number of comparisons
+    total_distance += current_distance;
+    ++steps;
+    if (pos_first == max_pos_first && pos_second == max_pos_second)  // end reached
+      break;
+
+    // Determine what steps are still possible
+    bool can_up_first = pos_first < max_pos_first;
+    bool can_up_second = pos_second < max_pos_second;
+    bool can_up_both = can_up_first && can_up_second;
+
+    // Compute pair-wise waypoint distances (increasing both, first, or second trajectories).
+    double up_both = std::numeric_limits<double>::max();
+    double up_first = std::numeric_limits<double>::max();
+    double up_second = std::numeric_limits<double>::max();
+    if (can_up_both)
+      up_both = traj_first.getWayPoint(pos_first + 1).distance(traj_second.getWayPoint(pos_second + 1));
+    if (can_up_first)
+      up_first = traj_first.getWayPoint(pos_first + 1).distance(traj_second.getWayPoint(pos_second));
+    if (can_up_second)
+      up_second = traj_first.getWayPoint(pos_first).distance(traj_second.getWayPoint(pos_second + 1));
+
+    // Select actual step, store new distance value and iterate trajectory positions
+    if (can_up_both && up_both < up_first && up_both < up_second)
+    {
+      ++pos_first;
+      ++pos_second;
+      current_distance = up_both;
+    }
+    else if ((can_up_first && up_first < up_second) || !can_up_second)
+    {
+      ++pos_first;
+      current_distance = up_first;
+    }
+    else if (can_up_second)
+    {
+      ++pos_second;
+      current_distance = up_second;
+    }
+  }
+  // Normalize trajectory distance by number of comparison steps
+  result_distance = total_distance / static_cast<double>(steps);
+  return true;*/
+
 }  // namespace core
 }  // namespace moveit