fix parameter passing bug of first link to other robots

[SunZezhou]

LinkScans in Mapper.cpp is
void MapperGraph::LinkScans(LocalizedRangeScan* pFromScan, LocalizedRangeScan* pToScan, const Pose2& rMean, const Matrix3& rCovariance)

but line 1198 in Mapper.cpp reversed the pscan and pSensorManager->GetScan(rCandidateSensorName, 0) parameters.

The correct order should be as shown on line 1171.

[mikeferguson]

How have you tested this? What exactly does it change at runtime? (it seems strange to me that a bug like this could exist so long)

[SunZezhou]

Yes, I have tested it with multi-robot environment.

As shown in Fig.1, pSensorManager->GetScan(rCandidateSensorName, 0) is robot_2 with position (x=3,y=0), pScan is robot_1 with position (x=0, y=0).

When step into LinkScans function (Fig. 2), two same position pFromScan->GetSensorPose()and rMean are passed into LinkInfo function.

And the wrong result after execute CorrectPoses() function is shown in Fig. 3.

The corrected process are shown in Fig. 4-6.