c++ - MRPT图满贯最小示例

我试图提出一种使用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();

}

我在正确的方向吗？我错过了什么？

最佳答案

嗯，乍一看脚本似乎还不错，您将以两个不同的步骤以“观察”形式提供里程计和激光扫描。


小音符

自动node_reg = mrpt :: graphslam :: deciders :: CICPCriteriaNRD {};


如果要使用Odometry +激光扫描来运行，请改用CFixedIntervalsNRD。它经过了更好的测试，并且实际上利用了这些度量。

MRPT中目前没有最小的graphslam-engine示例，但这是使用数据集运行graph-slam的主要方法：

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.");
}

您基本上可以获取数据，然后通过execGraphSlamStep或_execGraphSlamStep方法将其连续地馈送到CGraphSlamEngine。

这也是在相应的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++;
  }
}

关于c++ - MRPT图满贯最小示例，我们在Stack Overflow上找到一个类似的问题：https://stackoverflow.com/questions/51421935/