There might be a little mistake in CICP::ICP_Method_LM

[ysklab]

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]

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