Я использую Map Builder ICP библиотеки MRPT для реализации 2D шлема на C ++, используя 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;
}
Ожидаемый результат должен был быть в 2D-позе, как предсказано алгоритмом ICP-slam, что не соответствует действительности.
Однако вывод выглядит следующим образом:
[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, ковариации]