Bug from upstream(karto)

[qlibp]

https://github.com/SteveMacenski/slam_toolbox/blob/184f7278926eb365b41006805906468bb568af92/lib/karto_sdk/src/Mapper.cpp#LL627C15-L627C57

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

I believe the correct way to call should be

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

[SteveMacenski]

That does indeed look like a bug - do you mind opening a PR? I'll backport across the distributions that fix. Good catch!

[qlibp]

Yeah, though this seems like a bug, but I don't really test it out. Any idea on how to verify the fix is correct?

[qlibp]

@SteveMacenski
OK, I guess I know how to prove it a bug. Here's the failure mode: Set some extreme param

karto::Mapper* pMapper = new karto::Mapper();
pMapper->Reset();
pMapper->setParamCoarseSearchAngleOffset(0.255);
pMapper->setParamCoarseAngleResolution(0.017);
pMapper->setParamFineSearchAngleOffset(0.051);
pMapper->setParamCorrelationSearchSpaceDimension(1);
pMapper->setParamCorrelationSearchSpaceResolution(0.01);
pMapper->setParamCorrelationSearchSpaceSmearDeviation(0.01);

Then do the process as usual, we will encounter an assertion fail:
math::DoubleEqual(angle, rSearchCenter.GetHeading() + searchAngleOffset)