(fix) refine match wrong param passing

[qlibp]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#601
Primary OS tested on	(Ubuntu)
Robotic platform tested on	tested only with rosbag laser data

---

Description of contribution in a few bullet points

• code from upstream open_karto, accidentally passing the wrong params when doing refine matching, which will crash the code when using an extreme set of params

Description of documentation updates required from your changes

• no

---

Future work that may be required in bullet points

• no

[981213]

Hi!
The original code is intentional and this PR shouldn't be applied:
A coarse match has a resolution of 'pCoarseAngleResolution'. A fine match is supposed to refine this, so its angle search space is half the resolution of the coarse match.
The actual mistake is the parameter name: FineSearchAngleOffset should be named FineSearchAngleResolution instead.

[981213]

Here are two mapping sessions from the same rosbag:
Without this PR:

With this PR:

We can see the scan matching is significantly degraded.

[qlibp]

Hi!
The original code is intentional and this PR shouldn't be applied:
A coarse match has a resolution of 'pCoarseAngleResolution'. A fine match is supposed to refine this, so its angle search space is half the resolution of the coarse match.
The actual mistake is the parameter name: FineSearchAngleOffset should be named FineSearchAngleResolution instead.

If I understand correctly, what you mean is that pFineSearchAngleOffset =0.5*pCoarseAngleResolution is intentional? If that’s the case, then the semantics of pFineSearchAngleOffset is not consistent with pCoarseSearchAngleOffset, which means it set the left & right boundary of the search range, how could we just half down the boundary? For example, if pCoarseAngleResolution=1, and we have a gt pose with angle=0.8, with the half down strategy, pFineSearchAngleOffset=0.5, then we have no way to search the gt angle as it’s out of boundary. Correct me if I’m wrong.

[981213]

For example, if pCoarseAngleResolution=1, and we have a gt pose with angle=0.8, with the half down strategy, pFineSearchAngleOffset=0.5, then we have no way to search the gt angle as it’s out of boundary.

Say we have a starting point of angle=0. with gt=0.8 and CoarseAngleResolution=1, the coarse matching result will be 1 instead of 0 because 0.8 is closer to 1. (That's how the scan matching behaves if I understand this paper correctly.) With a fine angle offset of 0.5 the actual searching space is 0.5~1.5, and 0.8 is covered.

[SteveMacenski]

@qlibp I reverted the merge in #605 due to the discussion above.

The original code is intentional and this PR shouldn't be applied: A coarse match has a resolution of 'pCoarseAngleResolution'. A fine match is supposed to refine this, so its angle search space is half the resolution of the coarse match.

The 0.5 isn't dropped, the function arguments are just reordered to be as the function signature expects. If you look at the other instances where the function is called, the arguments appear to be in the correct order. You can see that just a few lines above this one, which is what convinced me: Offset then Resolution

        bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
            newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
            doPenalize, rMean, rCovariance, false);

The actual mistake is the parameter name: FineSearchAngleOffset should be named FineSearchAngleResolution instead.

Isn't that what's just being flipped here? Its simply reordering the arguments so that the Offset vs Resolution params are going into the function in the expected order. Are you suggesting we keep the 0.5 in the same slot but using the corrected parameter? Perhaps we just need to swap the default values / retune for these since they're now being correctly used?

[qlibp]

@SteveMacenski I need some more test to verify my understanding on this call. As in my case #601, my param setting will raise an assert error. I didn’t run a slam test, I just run a pure scan matcher algorithm. I tend to believe the degraded situation mentioned above is the result of wrong param setting

[981213]

Isn't that what's just being flipped here? Its simply reordering the arguments so that the Offset vs Resolution params are going into the function in the expected order.

Not quite. While it does apply the FineSearchAngleOffset to the offset slot, this PR also hard-coded the fine searching resolution as half of the CoarseAngleResolution.
I would argue that 0.5 * CoarseAngleResolution is not an enough resolution for a fine search at all. The resolution should be a parameter for user instead of a hard-coded value.

Meanwhile 0.5 * CoarseAngleResolution is a reasonable search space for the refinement. There's no need to make this a parameter : the error of a coarse match is exactly 0.5 * CoarseAngleResolution. There's no need to search more than that, and we can't search less than that or the ground truth may lie outside of the search space.

Are you suggesting we keep the 0.5 in the same slot but using the corrected parameter?

I'm suggesting we simply change the parameter name from FineSearchAngleOffset to FineSearchAngleResolution, without any parameter slot changes. We don't need an offset parameter and we do need the resolution one.

Perhaps we just need to swap the default values / retune for these since they're now being correctly used?

See above. With this PR we don't have a way to tune the fine search resolution at all, and there's no need to tune this search offset because the purpose of this fine search is to further correct an error (with known range) in the coarse search.

[SteveMacenski]

this PR also hard-coded the fine searching resolution as half of the CoarseAngleResolution

That was already there before, check the diff. If your issue is with what was there before w.r.t. multiplier, that's another issue altogether than the fact that these arguments were inverted to other uses of it in the code + what the function's signature says it expects. These are certainly related in performance, but lets try to decouple this problem into its components so its easier to talk about. First: function order / variables, second: the multiplier.

I do agree though looking at this that I'm not following Kurt's logic as to why there is a 0.5 on that particular parameter. That does feel like it should be its own parameter outright for a fine angle resolution parameter.

I would argue that 0.5 * CoarseAngleResolution is not an enough resolution for a fine search at all
Meanwhile 0.5 * CoarseAngleResolution is a reasonable search space for the refinement.

These statements seem exactly contradictory. What was changed here is under doRefineMatch. This is the fine / refine correlation portion of the code

I'm suggesting we simply change the parameter name from FineSearchAngleOffset to FineSearchAngleResolution

But that would make the inputs now, in order, a resolution followed by a resolution. The function's signature is clear that its an offset than a resolution.

[981213]

this PR also hard-coded the fine searching resolution as half of the CoarseAngleResolution

That was already there before, check the diff.

0.5 * CoarseAngleResolution is an intentionally chosen value for rSearchSpaceOffset during doRefineMatch. It's meant to be used as an offset, rather than a resolution, in the first place. Do let me know if I need to explain this part further :)

I do agree though looking at this that I'm not following Kurt's logic as to why there is a 0.5 on that particular parameter. That does feel like it should be its own parameter outright for a fine angle resolution parameter.

That's where my conclusion come from:

1. The offset value is chosen with an intention.
2. The resolution should be it's own parameter.

I assume they first wrote all the parameter names they needed and incorrectly wrote down FineSearchAngleOffset and omitted FineSearchAngleResolution. When writing the part of code touched by this PR they decided to use FineSearchAngleOffset in place temporarily and never remembered to fix the naming afterwards.

I would argue that 0.5 * CoarseAngleResolution is not an enough resolution for a fine search at all
Meanwhile 0.5 * CoarseAngleResolution is a reasonable search space for the refinement.

These statements seem exactly contradictory. What was changed here is under doRefineMatch. This is the fine / refine correlation portion of the code

I don't quite understand what you mean here. :(

I'm suggesting we simply change the parameter name from FineSearchAngleOffset to FineSearchAngleResolution

But that would make the inputs now, in order, a resolution followed by a resolution. The function's signature is clear that its an offset than a resolution.

It's a hard-coded chosen offset followed by a resolution, which does match the function signature.

[SteveMacenski]

Do let me know if I need to explain this part further :)

Perhaps, also a clip of what your suggested change looks like.

I assume they first wrote all the parameter names they needed and incorrectly wrote down FineSearchAngleOffset and omitted FineSearchAngleResolution. When writing the part of code touched by this PR they decided to use FineSearchAngleOffset in place temporarily and never remembered to fix the naming afterwards.

The other function uses are:

bestResponse = CorrelateScan(pScan, scanPose, coarseSearchOffset, coarseSearchResolution,
            newSearchAngleOffset, m_pMapper->m_pCoarseAngleResolution->GetValue(),
            doPenalize, rMean, rCovariance, false);

Which corresponds to the signature

/**
 * Finds the best pose for the scan centering the search in the correlation grid
 * at the given pose and search in the space by the vector and angular offsets
 * in increments of the given resolutions
 * @param rScan scan to match against correlation grid
 * @param rSearchCenter the center of the search space
 * @param rSearchSpaceOffset searches poses in the area offset by this vector around search center
 * @param rSearchSpaceResolution how fine a granularity to search in the search space
 * @param searchAngleOffset searches poses in the angles offset by this angle around search center
 * @param searchAngleResolution how fine a granularity to search in the angular search space
 * @param doPenalize whether to penalize matches further from the search center
 * @param rMean output parameter of mean (best pose) of match
 * @param rCovariance output parameter of covariance of match
 * @param doingFineMatch whether to do a finer search after coarse search
 * @return strength of response
 */
kt_double ScanMatcher::CorrelateScan(
  LocalizedRangeScan * pScan, const Pose2 & rSearchCenter,
  const Vector2<kt_double> & rSearchSpaceOffset,
  const Vector2<kt_double> & rSearchSpaceResolution,
  kt_double searchAngleOffset, kt_double searchAngleResolution,
  kt_bool doPenalize, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doingFineMatch)

So as far as I'm concerned, its clear as day what should be a resolution, offset, etc for what the method itself asks for. I don't understand your suggestion in this context. Its not in question as far as I can tell from the software's use of this function and the function itself what should be the inputs. What's there now seems weirdly to have a flipped input unlike the others, but what you're suggesting sounds to me like you're asking to give this function inputs its not asking for. I think clarifying your specific changes would be helpful. We might just be missing each other on diction.

[981213]

a clip of what your suggested change looks like.

From ff40e8cb47c5568359f4fdf3381ce0e15cb9cdde Mon Sep 17 00:00:00 2001
From: Chuanhong Guo <[email protected]>
Date: Sat, 17 Jun 2023 19:31:43 +0800
Subject: [PATCH] karto_sdk: Drop FineSearchAngleOffset and add
 FineSearchAngleResolution

The angle error of a coarse search is less than
0.5 * CoarseSearchAngleResolution. In other words, we know that the
ground truth is in this range:
CoarseSearchResult - 0.5 * CoarseSearchAngleResolution < ground truth <
CoarseSearchResult + 0.5 * CoarseSearchAngleResolution
Since a FineSearch is meant to refine the coarse search (i.e.
further reduce the angle error), it's search space can be set to the
known range above. An angle offset param isn't necessary.
Meanwhile, the search angle resolution depends on the accuracy end
users wants, and should be a parameter.

The original karto code uses the angle search offset described above
instead of the parameter it defines, and use the FineSearchAngleOffset
as the search resolution. This is likely a typo that never got
corrected.

Drop the FineSearchAngleOffset parameter and add the
FineSearchAngleResolution to correct the parameter name.
---
 lib/karto_sdk/include/karto_sdk/Mapper.h |  8 ++++----
 lib/karto_sdk/src/Mapper.cpp             | 20 ++++++++++----------
 src/slam_mapper.cpp                      | 10 +++++-----
 3 files changed, 19 insertions(+), 19 deletions(-)

diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
index d8256b2..757884a 100644
--- a/lib/karto_sdk/include/karto_sdk/Mapper.h
+++ b/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -2343,11 +2343,11 @@ protected:
   Parameter<kt_double> * m_pAngleVariancePenalty;
 
   // The range of angles to search during a coarse search and a finer search
-  Parameter<kt_double> * m_pFineSearchAngleOffset;
   Parameter<kt_double> * m_pCoarseSearchAngleOffset;
 
   // Resolution of angles to search during a coarse search
   Parameter<kt_double> * m_pCoarseAngleResolution;
+  Parameter<kt_double> * m_pFineSearchAngleResolution;
 
   // Minimum value of the penalty multiplier so scores do not
   // become too small
@@ -2395,9 +2395,9 @@ protected:
     ar & BOOST_SERIALIZATION_NVP(m_pLoopSearchSpaceSmearDeviation);
     ar & BOOST_SERIALIZATION_NVP(m_pDistanceVariancePenalty);
     ar & BOOST_SERIALIZATION_NVP(m_pAngleVariancePenalty);
-    ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleOffset);
     ar & BOOST_SERIALIZATION_NVP(m_pCoarseSearchAngleOffset);
     ar & BOOST_SERIALIZATION_NVP(m_pCoarseAngleResolution);
+    ar & BOOST_SERIALIZATION_NVP(m_pFineSearchAngleResolution);
     ar & BOOST_SERIALIZATION_NVP(m_pMinimumAnglePenalty);
     ar & BOOST_SERIALIZATION_NVP(m_pMinimumDistancePenalty);
     ar & BOOST_SERIALIZATION_NVP(m_pUseResponseExpansion);
@@ -2438,7 +2438,7 @@ public:
   // Scan Matcher Parameters
   double getParamDistanceVariancePenalty();
   double getParamAngleVariancePenalty();
-  double getParamFineSearchAngleOffset();
+  double getParamFineSearchAngleResolution();
   double getParamCoarseSearchAngleOffset();
   double getParamCoarseAngleResolution();
   double getParamMinimumAnglePenalty();
@@ -2476,7 +2476,7 @@ public:
   // Scan Matcher Parameters
   void setParamDistanceVariancePenalty(double d);
   void setParamAngleVariancePenalty(double d);
-  void setParamFineSearchAngleOffset(double d);
+  void setParamFineSearchAngleResolution(double d);
   void setParamCoarseSearchAngleOffset(double d);
   void setParamCoarseAngleResolution(double d);
   void setParamMinimumAnglePenalty(double d);
diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
index 83e2af2..f020c7d 100644
--- a/lib/karto_sdk/src/Mapper.cpp
+++ b/lib/karto_sdk/src/Mapper.cpp
@@ -624,7 +624,7 @@ kt_double ScanMatcher::MatchScan(
       m_pCorrelationGrid->GetResolution());
     bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
         0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
-        m_pMapper->m_pFineSearchAngleOffset->GetValue(),
+        m_pMapper->m_pFineSearchAngleResolution->GetValue(),
         doPenalize, rMean, rCovariance, true);
   }
 
@@ -2259,11 +2259,6 @@ void Mapper::InitializeParameters()
     "See DistanceVariancePenalty.",
     math::Square(math::DegreesToRadians(20)), GetParameterManager());
 
-  m_pFineSearchAngleOffset = new Parameter<kt_double>(
-    "FineSearchAngleOffset",
-    "The range of angles to search during a fine search.",
-    math::DegreesToRadians(0.2), GetParameterManager());
-
   m_pCoarseSearchAngleOffset = new Parameter<kt_double>(
     "CoarseSearchAngleOffset",
     "The range of angles to search during a coarse search.",
@@ -2274,6 +2269,11 @@ void Mapper::InitializeParameters()
     "Resolution of angles to search during a coarse search.",
     math::DegreesToRadians(2), GetParameterManager());
 
+  m_pFineSearchAngleResolution = new Parameter<kt_double>(
+    "FineSearchAngleResolution",
+    "Resolution of angles to search during a fine search.",
+    math::DegreesToRadians(0.2), GetParameterManager());
+
   m_pMinimumAnglePenalty = new Parameter<kt_double>(
     "MinimumAnglePenalty",
     "Minimum value of the angle penalty multiplier so scores do not become "
@@ -2417,9 +2417,9 @@ double Mapper::getParamAngleVariancePenalty()
   return std::sqrt(static_cast<double>(m_pAngleVariancePenalty->GetValue()));
 }
 
-double Mapper::getParamFineSearchAngleOffset()
+double Mapper::getParamFineSearchAngleResolution()
 {
-  return static_cast<double>(m_pFineSearchAngleOffset->GetValue());
+  return static_cast<double>(m_pFineSearchAngleResolution->GetValue());
 }
 
 double Mapper::getParamCoarseSearchAngleOffset()
@@ -2569,9 +2569,9 @@ void Mapper::setParamAngleVariancePenalty(double d)
   m_pAngleVariancePenalty->SetValue((kt_double)math::Square(d));
 }
 
-void Mapper::setParamFineSearchAngleOffset(double d)
+void Mapper::setParamFineSearchAngleResolution(double d)
 {
-  m_pFineSearchAngleOffset->SetValue((kt_double)d);
+  m_pFineSearchAngleResolution->SetValue((kt_double)d);
 }
 
 void Mapper::setParamCoarseSearchAngleOffset(double d)
diff --git a/src/slam_mapper.cpp b/src/slam_mapper.cpp
index 79dfa2f..c4eb9e4 100644
--- a/src/slam_mapper.cpp
+++ b/src/slam_mapper.cpp
@@ -313,12 +313,12 @@ void SMapper::configure(const rclcpp::Node::SharedPtr & node)
   node->get_parameter("angle_variance_penalty", angle_variance_penalty);
   mapper_->setParamAngleVariancePenalty(angle_variance_penalty);
 
-  double fine_search_angle_offset = 0.00349;
-  if (!node->has_parameter("fine_search_angle_offset")) {
-    node->declare_parameter("fine_search_angle_offset", fine_search_angle_offset);
+  double fine_search_angle_resolution = 0.00349;
+  if (!node->has_parameter("fine_search_angle_resolution")) {
+    node->declare_parameter("fine_search_angle_resolution", fine_search_angle_resolution);
   }
-  node->get_parameter("fine_search_angle_offset", fine_search_angle_offset);
-  mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);
+  node->get_parameter("fine_search_angle_resolution", fine_search_angle_resolution);
+  mapper_->setParamFineSearchAngleResolution(fine_search_angle_resolution);
 
   double coarse_search_angle_offset = 0.349;
   if (!node->has_parameter("coarse_search_angle_offset")) {
-- 
2.40.1

[SteveMacenski]

I believe this is what I mentioned is definitely incorrect:

     bestResponse = CorrelateScan(pScan, rMean, fineSearchOffset, fineSearchResolution,
         0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue(),
-        m_pMapper->m_pFineSearchAngleOffset->GetValue(),
+        m_pMapper->m_pFineSearchAngleResolution->GetValue(),
         doPenalize, rMean, rCovariance, true);
   }

/**
 * Finds the best pose for the scan centering the search in the correlation grid
 * at the given pose and search in the space by the vector and angular offsets
 * in increments of the given resolutions
 * @param rScan scan to match against correlation grid
 * @param rSearchCenter the center of the search space
 * @param rSearchSpaceOffset searches poses in the area offset by this vector around search center
 * @param rSearchSpaceResolution how fine a granularity to search in the search space
 * @param searchAngleOffset searches poses in the angles offset by this angle around search center
 * @param searchAngleResolution how fine a granularity to search in the angular search space
 * @param doPenalize whether to penalize matches further from the search center
 * @param rMean output parameter of mean (best pose) of match
 * @param rCovariance output parameter of covariance of match
 * @param doingFineMatch whether to do a finer search after coarse search
 * @return strength of response
 */
kt_double ScanMatcher::CorrelateScan(
  LocalizedRangeScan * pScan, const Pose2 & rSearchCenter,
  const Vector2<kt_double> & rSearchSpaceOffset,
  const Vector2<kt_double> & rSearchSpaceResolution,
  kt_double searchAngleOffset, kt_double searchAngleResolution,
  kt_bool doPenalize, Pose2 & rMean, Matrix3 & rCovariance, kt_bool doingFineMatch)

It is clear that the 6th argument is a resolution from the doxygen and from the param argument naming. This also does not change any of the other calls to CorrelateScan to update that logic similarly -- why? This is simply 1 of several calls to the function, the others don't need to be causing issues and the only difference between them is the corrected ordering of the params the original PR tried to swap.

[981213]

@SteveMacenski

It is clear that the 6th argument is a resolution from the doxygen and from the param argument naming.

I have no doubt about this one. It is indeed a resolution.

This also does not change any of the other calls to CorrelateScan to update that logic similarly -- why?

My patch doesn't change any other calls to CorrelateScan. All I did is renaming a variable.

This is simply 1 of several calls to the function, the others don't need to be causing issues and the only difference between them is the corrected ordering of the params the original PR tried to swap.

By swapping them as this PR did, you put a wrong hard-coded value at the resolution spot.
As you've said yourself, 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue() as a fine angle resolution doesn't make sense:

do agree though looking at this that I'm not following Kurt's logic as to why there is a 0.5 on that particular parameter. That does feel like it should be its own parameter outright for a fine angle resolution parameter.

Did you miss that the value in question is m_pCoarseAngleResolution, not m_pFineAngleResolution? m_pCoarseAngleResolution is a resolution for coarse search, not for a fine search we discussed here.
It's clear to me that 0.5 * m_pMapper->m_pCoarseAngleResolution->GetValue() is intended to be a value for offset and it seems that I failed to explain this specific point. Maybe someone else could try.

If you insist on using the existing m_pFineSearchAngleOffset as the offset please also add a separated parameter for m_pFineSearchAngleResolution as the fine search resolution so that people don't have to live with degraded scan matching caused by incorrectly using hard-coded 0.5 * m_pCoarseAngleResolution as the fine resolution.