MRPT SLAM MRPT::slam::CMetricMapBuilderICP警告姿态外推失败

Question

我使用MRPT库的Map在C++上实现了一个2D slam，它使用病态的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，偏航，协方差

Answer 1

只要这不是时时刻刻发生的，它只是一个警告，而不是一个错误，所以你不应该担心太多。

这意味着ICP使用的初始姿态将是最后的姿态，而不需要使用LiDAR的速度矢量在时间戳外推(猜测)机器人的姿态。