search

SteveMacenski / slam_toolbox

Slam Toolbox for lifelong mapping and localization in potentially massive maps with ROS
GNU Lesser General Public License v2.1
1.67k stars 525 forks source link

Bug from upstream(karto) #601

Open qlibp opened 1 year ago

qlibp commented 1 year ago

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 commented 1 year ago

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

qlibp commented 1 year ago

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 commented 1 year ago

@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)