slammobile-robot-toolkit

MRPT SLAM MRPT::slam::CMetricMapBuilderICP warning Pose Extrapolation failed


I am using MRPT library's Map Builder ICP to implement a 2D slam on C++ by using Sick LMS151 to get CObservation2DRangeScan. Whenever I am providing the 2D range scan to the map builder, it generates a warning that the Pose Extrapolation has failed. How do I know where the fault is in my codes?

mrpt::slam::CMetricMapBuilderICP mapBuilder;

double RANGE_MAX = 20.0;
double RANGE_MIN = 0.05;

py::list processObservation(double timestamp, py::list scanRanges, py::tuple pose) {
    /* Takes observation and pose and returns the pose that is predicted by SLAM */
    mrpt::obs::CObservation2DRangeScan *rangescan = new mrpt::obs::CObservation2DRangeScan();
    //Set Intensities to false, as our lidar does not send it
    rangescan->setScanHasIntensity(false);
    //Set Tolerance of Scan to +- 0.8radians in pitch and roll
    rangescan->isPlanarScan(0.08);
    rangescan->timestamp = mrpt::system::time_tToTimestamp(timestamp);
    rangescan->aperture = M_PI*1.5;
    rangescan->maxRange = RANGE_MAX;
    mrpt::poses::CPose3D Pose;
    //Sensor Pose for Observation
    Pose.setFromValues(pose[0].cast<double>()+base_to_lidar,pose[1].cast<double>(),0,0,0,pose[2].cast<double>());
    rangescan->setSensorPose(Pose);

    std::vector <float>scanranges;
    std::vector <char>valid(scanranges.size());

    for(auto i: scanRanges) {
        float range = i.cast<float>();
        valid.push_back(range<=RANGE_MAX && range>=RANGE_MIN);
        scanranges.push_back(range);
    }

    rangescan->loadFromVectors(scanranges.size(), &scanranges[0], &valid[0]);

    mrpt::obs::CObservation2DRangeScan::Ptr obs_ptr(rangescan);

    try {
        mapBuilder.processObservation(obs_ptr);
    }
    catch(...) {
        std::cerr<<"Cannot Process Observation. The old pose will be returned\n";
    }

    mrpt::poses::CPose3DPDF::Ptr predicted_pose = mapBuilder.getCurrentPoseEstimation();

    mrpt::math::CMatrixDouble cov;
    mrpt::poses::CPose3D mean;
    predicted_pose->getCovarianceDynAndMean(cov, mean);

    std::vector <double> pos_vector;
    pos_vector.push_back(mean.x());
    pos_vector.push_back(mean.y());
    pos_vector.push_back(mean.yaw());
    pos_vector.insert(pos_vector.end(), cov.begin(), cov.end());
    py::list list_pose = py::cast(pos_vector);
    return list_pose;
}


The expected output was to be the 2D pose as predicted by the ICP-slam algorithm, which is not the case.

However, the output is as follows:

[10:36:40.1430|WARN |CMetricMapBuilderICP] processObservation(): new pose extrapolation failed, using last pose as is.
Cannot Process Observation. The old pose will be returned
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

[x, y, yaw, covariances]


Solution

  • As long as this does not happen all the time, it is just a warning, not an error, so you should not worry too much.

    It means that the initial pose used by ICP will be the last pose, without an additional refining step that consists in extrapolating (guessing) the robot pose at the timestamp of the LiDAR, using its velocity vector.