Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- 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();
- }
Add Comment
Please, Sign In to add comment