Question

我正在使用MRPT库的Map Builder ICP通过使用Sick LMS151获取CObservation2DRangeScan在C ++上实现2D猛击。每当我向地图构建器提供2D范围扫描时，它都会生成一个警告，表明“姿势推断”失败。我怎么知道我的代码在哪里？

>> getopt_example -v -n 3 --some_parameter=7  % Sample inputs
    'v'                 [1]
    'n'                 [3]
    'some_parameter'    [7]

>> getopt_example -n 3 --some_parameter=7  % Omit -v option
    'v'                 [0]
    'n'                 [3]
    'some_parameter'    [7]

>> getopt_example -v -n --some_parameter=7  % Omit argument for -n option
Error using getopt_example (line 38)
Invalid input option format!

>> getopt_example -v -b --some_parameter=7  % Pass invalid -b option
Error using getopt_example (line 20)
Invalid input option!

>> getopt_example -v -n 3 --some_parameter=7 a1  % Pass additional argument a1
    'v'                 [1]
    'n'                 [3]
    'some_parameter'    [7]

    'a1'

预期输出为ICP-slam算法所预测的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;
}

[x，y，偏航，协方差]

Answer 1

只要这种情况并非始终存在，就只是警告，而不是错误，因此您不必担心太多。

这意味着ICP使用的初始姿势将是最后一个姿势，而无需执行额外的优化步骤，该步骤包括使用其速度矢量外推（猜测）LiDAR时间戳下的机器人姿势。

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

1 个答案: