search

MRPT / mrpt

:zap: The Mobile Robot Programming Toolkit (MRPT)
https://docs.mrpt.org/reference/latest/
BSD 3-Clause "New" or "Revised" License
1.89k stars 627 forks source link

There might be a little mistake in CICP::ICP_Method_LM #1264

Closed ysklab closed 1 year ago

ysklab commented 1 year ago

In function "CICP::ICP_Method_LM", branch "develop", line 597:

matchParams.angularDistPivotPoint =
        TPoint3D(q.x(), q.y(), 0);  // Pivot point for angular measurements

The value "q" is not initialized (so it's zero?). I guess the "angularDistPivotPoint" should be the scan center's position in map, which should not be zero.

Compared with "CICP::ICP_Method_Classic", the "angularDistPivotPoint" is set to

matchParams.angularDistPivotPoint = TPoint3D(
                gaussPdf->mean.x(), gaussPdf->mean.y(),
                0);  // Pivot point for angular measurements

and the "guessPdf" is the initial pose given.

jlblancoc commented 1 year ago

@ysklab thanks, indeed that was the case. Fixed via c774d0521ea20fabeaf5976086d97078b96e036e