template class mrpt::graphslam::CGraphSlamEngine

Main file for the GraphSlamEngine.

Given a dataset of measurements build a graph of nodes (keyframes) and constraints (edges) and solve it to find an estimation of the actual robot trajectory.

The GRAPH_T resource is accessed after having locked the relevant section m_graph_section. Critical section is also locked prior to the calls to the deciders/optimizers.

• output_dir_fname

  • Section : GeneralConfiguration

  • Default value : 1 (mrpt::system::LVL_INFO)

  • Required : FALSE

• user_decides_about_output_dir

  • Section : GeneralConfiguration

  • Default value : FALSE

  • Required : FALSE

  • Description : If flag true and in case of name conflict with output directory of the previous execution, a command-line is presented to the user to decide what to do about the new output directory. By default output directory from previous run is overwritten by the directory of the current run.

• ground_truth_file_format

  • Section : GeneralConfiguration

  • Default value : NavSimul

  • Required : FALSE

  • Description : Specify the format of the ground-truth file if one is provided. Currently CGraphSlamEngine supports ground truth files generated by the GridMapNavSimul tool or ground truth files corresponding to RGBD-TUM datasets.

  • Available Options: NavSimul, RGBD_TUM

• class_verbosity

  • Section : GeneralConfiguration

  • Default value : 1 (mrpt::system::LVL_INFO)

  • Required : FALSE

• visualize_map

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

• visualize_odometry_poses

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

• visualize_estimated_trajectory

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

• visualize_GT

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

• visualize_SLAM_metric

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

• enable_curr_pos_viewport

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

  • Description : Applicable only when dealing with RGB-D datasets

• enable_range_viewport

  • Section : VisualizationParameters

  • Default value : TRUE

  • Required : FALSE

  • Description : Applicable only when dealing with RGB-D datasets

• enable_intensity_viewport

  • Section : VisualizationParameters

  • Default value : FALSE

  • Required : FALSE

  • Description : Applicable only when dealing with RGB-D datasets

Implementation can be found in the file CGraphSlamEngine_impl.h

Parameters:

GRAPH_T

Graph Representation type - only CPosePDFGaussianInf has been tested at the moment

#include <mrpt/graphslam/CGraphSlamEngine.h>

template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CGraphSlamEngine: public mrpt::system::COutputLogger
{
public:
    // typedefs

    typedef std::map<std::string, mrpt::io::CFileOutputStream> fstreams_out;
    typedef typename fstreams_out::iterator fstreams_out_it;
    typedef typename GRAPH_T::constraint_t constraint_t;
    typedef typename GRAPH_T::constraint_t::type_value pose_t;
    typedef typename GRAPH_T::global_pose_t global_pose_t;
    typedef std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr> nodes_to_scans2D_t;

    // structs

    struct TRGBDInfoFileParams;

    // construction

    CGraphSlamEngine(
        const std::string& config_file,
        const std::string& rawlog_fname = "",
        const std::string& fname_GT = "",
        mrpt::graphslam::CWindowManager* win_manager = nullptr,
        mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>* node_reg = nullptr,
        mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>* edge_reg = nullptr,
        mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>* optimizer = nullptr
        );

    //
methods

    void getMap(
        mrpt::maps::COccupancyGridMap2D::Ptr map,
        mrpt::system::TTimeStamp* acquisition_time = nullptr
        ) const;

    void getMap(
        mrpt::maps::COctoMap::Ptr map,
        mrpt::system::TTimeStamp* acquisition_time = nullptr
        ) const;

    void computeMap() const;

    static void readGTFile(
        const std::string& fname_GT,
        std::vector<mrpt::poses::CPose2D>* gt_poses,
        std::vector<mrpt::system::TTimeStamp>* gt_timestamps = nullptr
        );

    static void readGTFile(
        const std::string& fname_GT,
        std::vector<mrpt::poses::CPose3D>* gt_poses,
        std::vector<mrpt::system::TTimeStamp>* gt_timestamps = nullptr
        );

    static void readGTFileRGBD_TUM(
        const std::string& fname_GT,
        std::vector<mrpt::poses::CPose2D>* gt_poses,
        std::vector<mrpt::system::TTimeStamp>* gt_timestamps = nullptr
        );

    bool isPaused() const;
    void togglePause();
    void resumeExec() const;
    void pauseExec();
    global_pose_t getCurrentRobotPosEstimation() const;
    virtual GRAPH_T::global_poses_t getRobotEstimatedTrajectory() const;
    virtual void getNodeIDsOfEstimatedTrajectory(std::set<mrpt::graphs::TNodeID>* nodes_set) const;
    void saveGraph(const std::string* fname_in = nullptr) const;
    void save3DScene(const std::string* fname_in = nullptr) const;
    void loadParams(const std::string& fname);
    void getParamsAsString(std::string* params_out) const;
    std::string getParamsAsString() const;
    void printParams() const;
    bool execGraphSlamStep(mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry);
    virtual bool _execGraphSlamStep(mrpt::obs::CActionCollection::Ptr& action, mrpt::obs::CSensoryFrame::Ptr& observations, mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry);
    const GRAPH_T& getGraph() const;
    std::string getRawlogFname();
    void generateReportFiles(const std::string& output_dir_fname_in);
    void getDeformationEnergyVector(std::vector<double>* vec_out) const;

    bool getGraphSlamStats(
        std::map<std::string, int>* node_stats,
        std::map<std::string, int>* edge_stats,
        mrpt::system::TTimeStamp* timestamp = nullptr
        );
};

Inherited Members

public:
    // structs

    struct TMsg;

Typedefs

typedef std::map<std::string, mrpt::io::CFileOutputStream> fstreams_out

Map for managing output file streams.

typedef typename fstreams_out::iterator fstreams_out_it

Map for iterating over output file streams.

typedef typename GRAPH_T::constraint_t constraint_t

Type of graph constraints.

typedef typename GRAPH_T::constraint_t::type_value pose_t

Type of underlying poses (2D/3D).

Construction

CGraphSlamEngine(
    const std::string& config_file,
    const std::string& rawlog_fname = "",
    const std::string& fname_GT = "",
    mrpt::graphslam::CWindowManager* win_manager = nullptr,
    mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>* node_reg = nullptr,
    mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>* edge_reg = nullptr,
    mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>* optimizer = nullptr
    )

Constructor of CGraphSlamEngine class template.

// TODO - remove the deprecated arguments If a null win_manager is provided, the application runs on headless mode. In this case, no visual feedback is given but application receives a big boost in performance

Parameters:

config_file	.ini file containing the configuration parameters for the CGraphSlamEngine as well as the deciders/optimizer classes that CGraphSlamEngine is using
win_manager	CwindowManager instance that includes a pointer to a CDisplayWindow3D and a CWindowObserver instance for properly interacting with the display window
rawlog_fname	.rawlog dataset file, containing the robot measurements. CGraphSlamEngine supports both MRPT rawlog formats but in order for graphSLAM to work as expected the rawlog foromat has to be supported by the every decider/optimizer class that CGraphSlamEngine makes use of.
fname_GT	Textfile containing the ground truth path of the robot. Currently the class can read ground truth files corresponding either to RGBD - TUM datasets or to rawlog files generated with the GridMapNavSimul MRPT application.
node_reg	Node Registration Decider to be used
edge_reg	Edge Registration Decider to be used
optimizer	Optimizer class to be used

Methods

void computeMap() const

Compute the map of the environment based on the recorded measurements.

Currently only mrpt::obs ::2DRangeScans are supported

See also:

getMap

static void readGTFile(
    const std::string& fname_GT,
    std::vector<mrpt::poses::CPose2D>* gt_poses,
    std::vector<mrpt::system::TTimeStamp>* gt_timestamps = nullptr
    )

Parse the ground truth .txt file and fill in the corresponding gt_poses vector.

It is assumed that the rawlog, thererfore the groundtruth file has been generated using the GridMapNavSimul MRPT application. If not user should abide the ground-truth file format to that of the files generated by the GridMapNavSimul app.

Parameters:

fname_GT	Ground truth filename from which the measurements are to be read
gt_poses	std::vector which is to contain the 2D ground truth poses.
gt_timestamps	std::vector which is to contain the timestamps for the corresponding ground truth poses. Ignore this argument if timestamps are not needed.

See also:

readGTFileRGBD_TUM

static void readGTFileRGBD_TUM(
    const std::string& fname_GT,
    std::vector<mrpt::poses::CPose2D>* gt_poses,
    std::vector<mrpt::system::TTimeStamp>* gt_timestamps = nullptr
    )

Parse the ground truth .txt file and fill in the corresponding m_GT_poses vector.

The poses returned are given with regards to the MRPT reference frame.

It is assumed that the groundtruth file has been generated using the rgbd_dataset2rawlog MRPT tool.

Parameters:

fname_GT	Ground truth filename from which the measurements are to be read
gt_poses	std::vector which is to contain the 2D ground truth poses.
gt_timestamps	std::vector which is to contain the timestamps for the corresponding ground truth poses. Ignore this argument if timestamps are not needed.

See also:

readGTFile, https://www.mrpt.org/Collection_of_Kinect_RGBD_datasets_with_ground_truth_CVPR_TUM_2011

virtual void getNodeIDsOfEstimatedTrajectory(std::set<mrpt::graphs::TNodeID>* nodes_set) const

Return the list of nodeIDs which make up robot trajectory.

See also:

updateEstimatedTrajectoryVisualization

void saveGraph(const std::string* fname_in = nullptr) const

Wrapper method around the GRAPH_T::saveToTextFile method.

Method saves the graph in the format used by TORO & HoG-man strategies

Parameters:

fname_in

Name of the generated graph file - Defaults to “output_graph” if not set by the user

See also:

save3DScene, https://www.mrpt.org/Robotics_file_formats

void save3DScene(const std::string* fname_in = nullptr) const

Wrapper method around the COpenGLScene::saveToFile method.

Parameters:

Name	of the generated graph file - Defaults to “output_graph” if not set by the user

See also:

saveGraph

void loadParams(const std::string& fname)

Read the configuration variables from the .ini file specified by the user.

Method is automatically called, upon CGraphSlamEngine initialization

void getParamsAsString(std::string* params_out) const

Fill in the provided string with the class configuration parameters.

See also:

printParams

std::string getParamsAsString() const

Wrapper around getParamsAsString.

Returns the generated string instead of passing it as an argument to the call

See also:

printParams

void printParams() const

Print the problem parameters to the console for verification.

Method is a wrapper around CGraphSlamEngine::getParamsAsString method

See also:

getParamsAsString

bool execGraphSlamStep(mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)

Wrapper method around _execGraphSlamStep.

Handy for not having to specify any action/observations objects

Returns:

False if the user has requested to exit the graphslam execution (e.g. pressed ctrl-c), True otherwise

virtual bool _execGraphSlamStep(
    mrpt::obs::CActionCollection::Ptr& action,
    mrpt::obs::CSensoryFrame::Ptr& observations,
    mrpt::obs::CObservation::Ptr& observation,
    size_t& rawlog_entry
    )

Main class method responsible for parsing each measurement and for executing graphSLAM.

Method reads each measurement seperately, so the application that invokes it is responsibe for fetching the measurements (e.g. from a rawlog file).

Returns:

False if the user has requested to exit the graphslam execution (e.g. pressed ctrl-c), True otherwise

const GRAPH_T& getGraph() const

Return a reference to the underlying GRAPH_T instance.

std::string getRawlogFname()

Return the filename of the used rawlog file.

void generateReportFiles(const std::string& output_dir_fname_in)

Generate and write to a corresponding report for each of the respective self/decider/optimizer classes.

Parameters:

output_dir_fname

directory name to generate the files in. Directory must be crated prior to this call

See also:

getDescriptiveReport, CGraphSlamHandler::initOutputDir

void getDeformationEnergyVector(std::vector<double>* vec_out) const

Fill the given vector with the deformation energy values computed for the SLAM evaluation metric.

Parameters:

vec_out

deformation energy vector to be filled

See also:

m_deformation_energy_vec

bool getGraphSlamStats(
    std::map<std::string, int>* node_stats,
    std::map<std::string, int>* edge_stats,
    mrpt::system::TTimeStamp* timestamp = nullptr
    )

Fill the given maps with stats regarding the overall execution of graphslam.