Fix and optimize findPathFurthestReachedPoint

[tonynajjar]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

---

Future work that may be required in bullet points

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[mergify]

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

I would like some computational analysis here before merging. But as you can imagine, I have no obvious complaints about the code changes in it of itself

[SteveMacenski]

@tonynajjar was there a ticket / discussion for this that we should link / close when merged?

@mini-1235 is going to do the perf testing shortly

[tonynajjar]

I don't think so no

[mini-1235]

@claude can you write a unit test that demonstrates the original implementation is incorrect?

[SteveMacenski]

@claude can you write a unit test that demonstrates the original implementation is incorrect?

[claude]

Claude encountered an error —— View job

Failed with exit code 128

---

I'll analyze this and get back to you.

[SteveMacenski]

Oh I see, this doesn't work with PRs on forks. I mostly use Claude in tickets. You could just locally give it this PR URL and it should be able to provide you something using MCP

[mini-1235]

I tested this today over 100 iterations. The original implementation takes ~45 ms, while this PR takes ~65 ms

[SteveMacenski]

Yeah, that is very non-trivial. Maybe this can be vectorized better with Eigen functions? Or perhaps we can set a maximum it can look backwards from that is 'reasonable' for any single point to need to go back to (i.e. we know the dt/trajectories, we should be able to find the maximum in reverse a given sample can go to bound the lower check)

Not tested or really reviewed in detail, but this is something that Claude Code is pretty good at in general. I asked it to vectorize using Eigen APIs and it seems plausible. The minCoeff method would be vectorized with SIMD so it'd be faster than the looping. I assume that's where the compute time is from those metric differentials.

inline size_t findPathFurthestReachedPoint(const CriticData & data)
{
  const int traj_cols = data.trajectories.x.cols();
  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
  const auto traj_y = data.trajectories.y.col(traj_cols - 1);

  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;

  const auto dists = (dx.array().square() + dy.array().square()).matrix();
  
  int max_id = 0;
  const int last_idx = dists.cols() - 1;
  for (int i = 0; i < dists.rows(); ++i) {
    Eigen::Index min_idx;
    dists.row(i).minCoeff(&min_idx);
    if (min_idx > max_id) {
      max_id = min_idx;
      if (max_id == last_idx) break;
    }
  }
  
  return max_id;
}

[mini-1235]

Not tested or really reviewed in detail, but this is something that Claude Code is pretty good at in general. I asked it to vectorize using Eigen APIs and it seems plausible. The minCoeff method would be vectorized with SIMD so it'd be faster than the looping. I assume that's where the compute time is from those metric differentials.

inline size_t findPathFurthestReachedPoint(const CriticData & data)
{
  const int traj_cols = data.trajectories.x.cols();
  const auto traj_x = data.trajectories.x.col(traj_cols - 1);
  const auto traj_y = data.trajectories.y.col(traj_cols - 1);

  const auto dx = (data.path.x.transpose()).replicate(traj_x.rows(), 1).colwise() - traj_x;
  const auto dy = (data.path.y.transpose()).replicate(traj_y.rows(), 1).colwise() - traj_y;

  const auto dists = (dx.array().square() + dy.array().square()).matrix();
  
  int max_id = 0;
  const int last_idx = dists.cols() - 1;
  for (int i = 0; i < dists.rows(); ++i) {
    Eigen::Index min_idx;
    dists.row(i).minCoeff(&min_idx);
    if (min_idx > max_id) {
      max_id = min_idx;
      if (max_id == last_idx) break;
    }
  }
  
  return max_id;
}

This actually makes it even slower, around 130ms :(

Actually, I try computing dx, dy, and the distances on the fly instead, and it only takes ~40ms. I will open a PR, let me know if that sounds good to you

[SteveMacenski]

#5792 supersedes