Removed squaring from constrained smoother cost function shape

[videh25]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5072
Primary OS tested on	Ubuntu
Robotic platform tested on	Loopback Sim
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Removed squaring of residuals in ceres cost fucntions of nav2_constrained_smoother.
• Tuned the new nav2_constraind_smoother.

Description of documentation updates required from your changes

• nav2_constrained_smoother's configure guide will have to be updated with the new parameters.

Description of how this change was tested

• Copied the current nav2_constrained_smoother as nav2_constrained_smoother_old so that both the improvements can be compared side by side with the older smoother via benchmarking script.
• Made changes to the nav2_constrained_smoother to remove the squaring and added temporary changes to publish the ceres summary data over a topic for becnchmarking.
• Made changes to the smoother benchmarking scripts to be able to incorporate the ceres summaries in results.
• Copied and modified maps for testing from different packages.
• Ran the smoother benchmarking scripts to measure the performance of both smoothers across different maps and with different planners. Tested for 200 trajectories over different maps and planners.
• More detailed results posted on Constrained smoother cost function shape #5072.

---

Future work that may be required in bullet points

NA

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-*.

[SteveMacenski]

A couple of questions and a request for doc updates! LGTM

[SteveMacenski]

Please add to the migration guide at docs.nav2.org that this improvement has been had which increases path quality + convergence time. Also update the parameter defaults to be more in line with what folks should use in the configuration guide for the constrained smoother :-)

[SteveMacenski]

If we're changing the formulation for the defaults to need adjustment, could the defaults themselves just account on being squared so we don't perform these sqrt operations over the weights? I.e. retune the defaults to be sqrt of it's updated value? That would remove some relatively heavy operations

The cost check points may also be the same?

[videh25]

I too agree with removing the repeated square root functions.
Just listing down ways and concerns in doing so:

1. Square root the weights in configuration and remove sqrt from the code.
   • For anyone considering the mathematical formulation of cost function, it is natural to think of the weights mentioned in the config to be the ones directly being multiplied to individual squared costs, since that is how the formulae are generally mentioned in papers and academia.
   • Weights being squared internally by ceres will be misleading.
   • This can work if we also update the documentation to explicitly mention the mathematical formulation used in the smoother to mention that the yaml weights are squared by ceres.
2. Keep the yaml values and sqrt them when the parameter is fetched/updated
   • Values mentioned in the config will be the same as the ones multiplied to the squared cost terms, despite of ceres squaring them internally.
   • Might have to rename the class variables to something like weight_sqrted since they are not the exact same values as in yaml?

Please let me know what approach sounds best!

[mini-1235]

I think the second option probably makes the most sense

[SteveMacenski]

I agree with the second one :-)

[mini-1235]

Also tests are failing after the changes: https://app.circleci.com/pipelines/github/ros-navigation/navigation2/18155/workflows/b2ce8ae9-f4b4-4915-927a-e6f465eb63d3/jobs/54622/tests

[SteveMacenski]

LGTM!

Only 3 asks:

• There are a number of tests that fail now, can you take a look and address? This includes DCO (which is easy if you click on the job it tells you what to run)
• Can you update the migration guide in docs.nav2.org to mention this fix + updated parameters required?
• Does it make sense to update the defaults in the code / example yaml in the configuration guide for the constrained smoother as well to be what you've found works well now?

[videh25]

• There are a number of tests that fail now, can you take a look and address? This includes DCO (which is easy if you click on the job it tells you what to run)

• Still working on the tests. Need to retune the parameters used in the tests for some of them.
• A bunch of them are failing due to improvements in the performance, where smoother is improving the path more than the tests expected. Example: testingAnchoringToOriginalPath in here. For such test, should I update the values expected by the tests? Or should I update the tests to a greater than comparision rather than the current expect near?

• Can you update the migration guide in docs.nav2.org to mention this fix + updated parameters required?

• Yes, I have those changes ready offline. Will create a PR!

• Does it make sense to update the defaults in the code / example yaml in the configuration guide for the constrained smoother as well to be what you've found works well now?

• Yes, still finalising the parameters. Will get a final set of parameters once I fix all the tests.

[mini-1235]

I would suggest keeping the EXPECT_NEAR and updating the expected value. At the same time, please sanity check the new value to make sure it actually makes sense to you