MRPT SLAM MRPT::slam::CMetricMapBuilderICP 警告姿势外推失败答案

【问题标题】：MRPT SLAM MRPT::slam::CMetricMapBuilderICP warning Pose Extrapolation failedMRPT SLAM MRPT::slam::CMetricMapBuilderICP 警告姿势外推失败
【发布时间】：2019-05-10 05:17:00
【问题描述】：

我正在使用 MRPT 库的 Map Builder ICP 在 C++ 上实现 2D slam，方法是使用 Sick LMS151 获取 CObservation2DRangeScan。每当我向地图构建器提供 2D 范围扫描时，它都会生成姿势外推失败的警告。我怎么知道我的代码哪里出了问题？

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;
}

预期的输出是 ICP-slam 算法预测的 2D 姿势，但事实并非如此。

但是，输出如下：

[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, 偏航, 协方差]

【问题讨论】：

标签： slam mobile-robot-toolkit

【解决方案1】：

只要不是一直发生这种情况，它只是一个警告，而不是错误，所以你不必太担心。

这意味着 ICP 使用的初始位姿将是最后一个位姿，无需额外的精炼步骤，即使用其速度向量在 LiDAR 的时间戳外推（猜测）机器人位姿。

【讨论】：