MRPT Graph Slam Минимальный пример

Я пытаюсь придумать «минимальный» способ запуска приложения Slam Graph с использованием MRPT. Данные датчика (LaserScan / Odometry) будут предоставляться пользовательским промежуточным программным обеспечением, аналогичным ROS. После тщательного чтения документов и исходных кодов (как для MRPT, так и для моста ROS) я придумал следующий фрагмент:

std::string config_file = "../../../laser_odometry.ini";
std::string rawlog_fname = "";
std::string fname_GT = "";
auto node_reg = mrpt::graphslam::deciders::CICPCriteriaNRD<mrpt::graphs::CNetworkOfPoses2DInf>{};
auto edge_reg = mrpt::graphslam::deciders::CICPCriteriaERD<mrpt::graphs::CNetworkOfPoses2DInf>{};
auto optimizer = mrpt::graphslam::optimizers::CLevMarqGSO<mrpt::graphs::CNetworkOfPoses2DInf>{};

auto win3d = mrpt::gui::CDisplayWindow3D{"Slam", 800, 600};
auto win_observer = mrpt::graphslam::CWindowObserver{};
auto win_manager = mrpt::graphslam::CWindowManager{&win3d, &win_observer};

auto engine = mrpt::graphslam::CGraphSlamEngine<mrpt::graphs::CNetworkOfPoses2DInf>{
config_file, rawlog_fname, fname_GT, &win_manager, &node_reg, &edge_reg, &optimizer};

for (size_t measurement_count = 0;;) {
// grab laser scan from the network, then fill it (hardcoded values for now), e.g:
auto scan_ptr = mrpt::obs::CObservation2DRangeScan::Create();
scan_ptr->timestamp = std::chrono::system_clock::now().time_since_epoch().count();
scan_ptr->rightToLeft = true;
scan_ptr->sensorLabel = "";
scan_ptr->aperture = 3.14;  // rad (max-min)
scan_ptr->maxRange = 3.0;   // m
scan_ptr->sensorPose = mrpt::poses::CPose3D{};

scan_ptr->resizeScan(30);
for (int i = 0; i < 30; ++i) {
scan_ptr->setScanRange(i, 0.5);
scan_ptr->setScanRangeValidity(i, true);
}

{ // Send LaserScan measurement to the slam engine
auto obs_ptr = std::dynamic_pointer_cast<mrpt::obs::CObservation>(scan_ptr);
engine.execGraphSlamStep(obs_ptr, measurement_count);
++measurement_count;
}// grab odometry from the network, then fill it (hardcoded values for now), e.g:
auto odometry_ptr = mrpt::obs::CObservationOdometry::Create();
odometry_ptr->timestamp = std::chrono::system_clock::now().time_since_epoch().count();
odometry_ptr->hasVelocities = false;
odometry_ptr->odometry.x(0);
odometry_ptr->odometry.y(0);
odometry_ptr->odometry.phi(0);

{ // Send Odometry measurement to the slam engine
auto obs_ptr = std::dynamic_pointer_cast<mrpt::obs::CObservation>(odometry_ptr);
engine.execGraphSlamStep(obs_ptr, measurement_count);
++measurement_count;
}

// Get pose estimation from the engine
auto pose = engine.getCurrentRobotPosEstimation();

}

Я в правильном направлении здесь? Я что-то пропустил?

0

Решение

Хм, на первый взгляд сценарий выглядит нормально, вы предоставляете одометрию и лазерное сканирование в два этапа и в форме наблюдения.

  • Незначительная нота

    auto node_reg = mrpt :: graphslam :: решить — :: CICPCriteriaNRD {};

Если вы хотите работать с лазерным сканированием Odometry +, используйте взамен CFixedIntervalsNRD. Это намного лучше проверено и фактически использует те измерения.

В настоящее время в MRPT нет минимального примера движка графического шлема, но вот основной метод запуска графического шлема с наборами данных:

https://github.com/MRPT/mrpt/blob/26ee0f2d3a9366c50faa5f78d0388476ae886808/libs/graphslam/include/mrpt/graphslam/apps_related/CGraphSlamHandler_impl.h#L395

template <class GRAPH_T>
void CGraphSlamHandler<GRAPH_T>::execute()
{
using namespace mrpt::obs;
ASSERTDEB_(m_engine);

// Variables initialization
mrpt::io::CFileGZInputStream rawlog_stream(m_rawlog_fname);
CActionCollection::Ptr action;
CSensoryFrame::Ptr observations;
CObservation::Ptr observation;
size_t curr_rawlog_entry;
auto arch = mrpt::serialization::archiveFrom(rawlog_stream);

// Read the dataset and pass the measurements to CGraphSlamEngine
bool cont_exec = true;
while (CRawlog::getActionObservationPairOrObservation(
arch, action, observations, observation, curr_rawlog_entry) &&
cont_exec)
{
// actual call to the graphSLAM execution method
// Exit if user pressed C-c
cont_exec = m_engine->_execGraphSlamStep(
action, observations, observation, curr_rawlog_entry);
}
m_logger->logFmt(mrpt::system::LVL_WARN, "Finished graphslam execution.");
}

Вы в основном захватываете данные и затем непрерывно передаете их в CGraphSlamEngine с помощью методов execGraphSlamStep или _execGraphSlamStep.

Вот также соответствующий фрагмент для обработки измерений в соответствующей оболочке ROS, которая работает с измерениями из тем ROS:

https://github.com/mrpt-ros-pkg/mrpt_slam/blob/8b32136e2a381b1759eb12458b4adba65e2335da/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h#L719

template<class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::processObservation(
mrpt::obs::CObservation::Ptr& observ) {
this->_process(observ);
}

template<class GRAPH_T>
void CGraphSlamHandler_ROS<GRAPH_T>::_process(
mrpt::obs::CObservation::Ptr& observ) {
using namespace mrpt::utils;
if (!this->m_engine->isPaused()) {
this->m_engine->execGraphSlamStep(observ, m_measurement_cnt);
m_measurement_cnt++;
}
}
1

Другие решения

Других решений пока нет …

По вопросам рекламы [email protected]