Template Class CGraphSlamEngine

These are set from the .ini configuration file. The actual visualization of these objects can be overridden if the user issues the corresponding keystrokes in the CDisplayWindow3D. In order for them to have any effect, a pointer to CDisplayWindow3D has to be given first.

bool m_visualize_odometry_poses

bool m_visualize_GT

bool m_visualize_map

bool m_visualize_estimated_trajectory

bool m_visualize_SLAM_metric

bool m_enable_curr_pos_viewport

bool m_enable_intensity_viewport

bool m_enable_range_viewport

Parameters relevant to the textMessages appearing in the visualization window. These are divided into

Y offsets: vertical position of the textMessage, starting from the top side.
Indices: Unique ID number of each textMessage, used for updating it

double m_offset_x_left: Offset from the left side of the canvas. Common for all the messages that are displayed on that side.

double m_offset_y_odometry

double m_offset_y_GT

double m_offset_y_estimated_traj

double m_offset_y_timestamp

double m_offset_y_current_constraint_type

double m_offset_y_paused_message

int m_text_index_odometry

int m_text_index_GT

int m_text_index_estimated_traj

int m_text_index_timestamp

int m_text_index_current_constraint_type

int m_text_index_paused_message

User available keystrokes

Keystrokes for toggling the corresponding objects in the CDisplayWindow upon user press

std::string m_keystroke_pause_exec

std::string m_keystroke_odometry

std::string m_keystroke_GT

std::string m_keystroke_estimated_trajectory

std::string m_keystroke_map

Trajectories colors

mrpt::img::TColor m_odometry_color

mrpt::img::TColor m_GT_color

mrpt::img::TColor m_estimated_traj_color

mrpt::img::TColor m_optimized_map_color

mrpt::img::TColor m_current_constraint_type_color

std::map<mrpt::graphs::TNodeID, size_t> m_nodeID_to_gt_indices: Map from nodeIDs to their corresponding closest GT pose index. Keep track of the nodeIDs instead of the node positions as the latter are about to change in the Edge Registration / Loop closing procedures.

double m_curr_deformation_energy

std::vector<double> m_deformation_energy_vec

Cached version and corresponding flag of map

mutable mrpt::maps::COccupancyGridMap2D::Ptr m_gridmap_cached

mutable mrpt::maps::CSimpleMap m_simple_map_cached: Acquired map in CSimpleMap representation.

mutable mrpt::maps::COctoMap::Ptr m_octomap_cached

mutable bool m_map_is_cached: Indicates if the map is cached.

Note

Common var for both 2D and 3D maps.

mutable mrpt::system::TTimeStamp m_map_acq_time: Timestamp at which the map was computed.

Note

Common var for both 2D and 3D maps.

Map computation and acquisition methods

Fill the given map based on the observations that have been recorded so far.

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

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

See also

getMap

Warning

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

pause/resume execution

inline bool isPaused() const

inline void togglePause()

inline void resumeExec() const

inline void pauseExec()

ground-truth parsing methods

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.

See also

readGTFileRGBD_TUM

Parameters:

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

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)

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 – [in] Ground truth filename from which the measurements are to be read
gt_poses – [out] std::vector which is to contain the 2D ground truth poses.
gt_timestamps – [out] std::vector which is to contain the timestamps for the corresponding ground truth poses. Ignore this argument if timestamps are not needed.

Initialization of Visuals

Methods used for initializing various visualization features relevant to the application at hand. If the visual feature is specified by the user (via the .ini file) and if it is relevant to the application then the corresponding method is called in the initClass class method

void initVisualization()

void initRangeImageViewport()

void initIntensityImageViewport()

mrpt::viz::CSetOfObjects::Ptr initRobotModelVisualization()

mrpt::viz::CSetOfObjects::Ptr initRobotModelVisualizationInternal(const mrpt::poses::CPose2D &p_unused): Method to help overcome the partial template specialization restriction of C++. Apply polymorphism by overloading function arguments instead.

mrpt::viz::CSetOfObjects::Ptr initRobotModelVisualizationInternal(const mrpt::poses::CPose3D &p_unused)

Update of Visuals

Methods used for updating various visualization features relevant to the application at hand. If relevant to the application at hand update is periodically scheduled inside the execGraphSlam method

void updateAllVisuals(): Wrapper around the deciders/optimizer updateVisuals methods.

void updateRangeImageViewport(): In RGB-D TUM Datasets update the Range image displayed in a seperate viewport.

void updateIntensityImageViewport(): In RGB-D TUM Datasets update the Intensity image displayed in a seperate viewport.

virtual void updateCurrPosViewport(): Update the viewport responsible for displaying the graph-building procedure in the estimated position of the robot.

virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::graphs::TNodeID nodeID) const

return the 3D Pose of a LaserScan that is to be visualized.

Used during the computeMap call for the occupancy gridmap

virtual void setObjectPropsFromNodeID(const mrpt::graphs::TNodeID nodeID, mrpt::viz::CSetOfObjects::Ptr &viz_object): Set the properties of the map visual object based on the nodeID that it was produced by. Derived classes may override this method if they want to have different visual properties (color, shape etc.) for different nodeIDs.

Note

Base class method sets only the color of the object

void initMapVisualization()

void updateMapVisualization(const std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr> &nodes_to_laser_scans2D, bool full_update = false)

Update the map visualization based on the current graphSLAM state.

Map is produced by arranging the range scans based on the estimated robot trajectory.

See also

updateEstimatedTrajectoryVisualization

void updateGTVisualization()

Display the next ground truth position in the visualization window.

See also

updateOdometryVisualization

void updateOdometryVisualization()

Update odometry-only cloud with latest odometry estimation.

See also

updateGTVisualization

void updateEstimatedTrajectoryVisualization(bool full_update = false)

Update the Esstimated robot trajectory with the latest estimated robot position.

Update CSetOfLines visualization object with the latest graph node position. If full update is asked, method clears the CSetOfLines object and redraws all the lines based on the updated (optimized) positions of the nodes

void updateSlamMetricVisualization(): Update the displayPlots window with the new information with regards to the metric.

Toggling of Visuals

Methods used for toggling various visualization features relevant to the application at hand.

void toggleOdometryVisualization()

void toggleGTVisualization()

void toggleMapVisualization()

void toggleEstimatedTrajectoryVisualization()

Class specific supplementary functions.

static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)

static double accumulateAngleDiffs(const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)

Public Types

using fstreams_out = std::map<std::string, mrpt::io::CFileOutputStream>: Map for managing output file streams.

using fstreams_out_it = typename fstreams_out::iterator: Map for iterating over output file streams.

using constraint_t = typename GRAPH_T::constraint_t: Type of graph constraints.

using pose_t = typename GRAPH_T::constraint_t::type_value: Type of underlying poses (2D/3D).

using global_pose_t = typename GRAPH_T::global_pose_t

using nodes_to_scans2D_t = std::map<mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr>

Public Functions

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

Note

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 – [in] .ini file containing the configuration parameters for the CGraphSlamEngine as well as the deciders/optimizer classes that CGraphSlamEngine is using
win_manager – [in] CwindowManager instance that includes a pointer to a CDisplayWindow3D and a CWindowObserver instance for properly interacting with the display window
rawlog_fname – [in] .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 – [in] 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 – [in] Node Registration Decider to be used
edge_reg – [in] Edge Registration Decider to be used
optimizer – [in] Optimizer class to be used

~CGraphSlamEngine() override

global_pose_t getCurrentRobotPosEstimation() const

virtual GRAPH_T::global_poses_t getRobotEstimatedTrajectory() const

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 – [in] Name of the generated graph file - Defaults to “output_graph” if not set by the user

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

Wrapper method around the Scene::saveToFile method.

See also

saveGraph

Parameters:: Name – [in] of the generated graph file - Defaults to “output_graph” if not set by the user

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.

Note

Method reads each measurement separately, 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

inline const GRAPH_T &getGraph() const: Return a reference to the underlying GRAPH_T instance.

inline 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 – [in] directory name to generate the files in. Directory must be crated prior to this call

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

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

See also

m_deformation_energy_vec

Parameters:: vec_out – [out] deformation energy vector to be filled

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.

Protected Functions

void initClass(): General initialization method to call from the Class Constructors.

Note

Method is automatically called in the class constructor

void initResultsFile(const std::string &fname)

Automate the creation and initialization of a results file relevant to the application.

Open the file (corresponding to the provided filename) and write an introductory message.

See also

CGraphSlamHandler::initOutputDir

void getDescriptiveReport(std::string *report_str) const

Fill the provided string with a detailed report of the class state.

Report includes the following:

Timing of important methods
Properties fo class at the current time
Logging of commands until current time

Note

Decider/Optimizer classes should also implement a getDescriptiveReport method for printing information on their part of the execution.

void initCurrPosViewport()

void initGTVisualization()

void initOdometryVisualization()

void initEstimatedTrajectoryVisualization()

void initSlamMetricVisualization()

void decimateLaserScan(mrpt::obs::CObservation2DRangeScan &laser_scan_in, mrpt::obs::CObservation2DRangeScan *laser_scan_out, const int keep_every_n_entries = 2)

Cut down on the size of the given laser scan.

Handy for reducing the size of the resulting mrpt::viz::CSetOfObjects that would be inserted in the visualization scene. Increase the decimation rate - keep-every_n_entries - to reduce the computational cost of updating the map visualization

See also

updateMapVisualization

void alignOpticalWithMRPTFrame()

void queryObserverForEvents()

Query the observer instance for any user events.

Query the given observer for any events (keystrokes, mouse clicks, that may have occurred in the CDisplayWindow3D and fill in the corresponding class variables

void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index)

Compare the SLAM result (estimated trajectory) with the GT path.

See A Comparison of SLAM Algorithms Based on a Graph of Relations for more details on this.

void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time = 500)

Wrapper method that used for printing error messages in a consistent manner.

Makes use of the COutputLogger instance. Prints error message when toggling illegal visual features in the display window

mrpt::viz::CSetOfObjects::Ptr setCurrentPositionModel(const std::string &model_name, const mrpt::img::TColor &model_color = mrpt::img::TColor(0, 0, 0), const size_t model_size = 1, const pose_t &init_pose = pose_t())

Set the opengl model that indicates the latest position of the trajectory at hand.

Todo:: Use an airplane/quad model for 3D operations

Note

Different model is used depending on whether we are running 2D or 3D SLAM.

Parameters:

model_name – [in] Name of the resulting opengl object.
model_color – [in] Color of the object.
model_size – [in] Scaling of the object.
init_pose – [in] Initial position of the object.

Returns:

CSetOfObjects::Ptr instance.

virtual void monitorNodeRegistration(bool registered = false, std::string class_name = "Class")

Assert that the given nodes number matches the registered graph nodes, otherwise throw exception.

\raise logic_error if the expected node count mismatches with the graph current node count.

Note

Method locks the graph internally.

void execDijkstraNodesEstimation()

Wrapper around the GRAPH_T::dijkstra_nodes_estimate.

Update the global position of the nodes

Protected Attributes

mrpt::system::CTimeLogger m_time_logger: Time logger instance

GRAPH_T m_graph: The graph object to be built and optimized.

const bool m_enable_visuals: Determine if we are to enable visualization support or not.

std::string m_config_fname

std::string m_rawlog_fname

Rawlog file from which to read the measurements.

If string is empty, process is to be run online

std::string m_fname_GT

size_t m_GT_poses_index: Counter for reading back the GT_poses.

size_t m_GT_poses_step: Rate at which to read the GT poses.

bool m_user_decides_about_output_dir

bool m_has_read_config

bool m_observation_only_dataset

fstreams_out m_out_streams: keeps track of the out fstreams so that they can be closed (if still open) in the class Dtor.

mutable bool m_is_paused: Indicated if program is temporarily paused by the user.

const std::string m_paused_message: Message to be displayed while paused.

mrpt::graphslam::detail::CEdgeCounter m_edge_counter: Instance to keep track of all the edges + visualization related operations.

bool m_use_GT

Flag for specifying if we are going to use ground truth data at all.

This is set to true either if the evolution of the SLAM metric or the ground truth visualization is set to true.

std::vector<pose_t> m_odometry_poses

std::vector<pose_t> m_GT_poses

std::string m_GT_file_format

nodes_to_scans2D_t m_nodes_to_laser_scans2D: Map of NodeIDs to their corresponding LaserScans.

mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D: Last laser scan that the current class instance received.

mrpt::obs::CObservation2DRangeScan::Ptr m_first_laser_scan2D: First recorded laser scan - assigned to the root.

mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D: Last laser scan that the current class instance received.

mrpt::math::CMatrixDouble33 m_rot_TUM_to_MRPT

size_t m_robot_model_size: How big are the robots going to be in the scene

mrpt::graphs::TNodeID m_nodeID_max

Internal counter for querying for the number of nodeIDs.

Handy for not locking the m_graph resource

mutable std::mutex m_graph_section: Mark graph modification/accessing explicitly for multithreaded implementation

std::string m_img_external_storage_dir

std::string m_img_prev_path_base

struct mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams m_info_params

double m_dataset_grab_time: Time it took to record the dataset. Processing time should (at least) be equal to the grab time for the algorithm to run in real-time.

mrpt::system::TTimeStamp m_init_timestamp: First recorded timestamp in the dataset.

mrpt::system::TTimeStamp m_curr_timestamp: Current dataset timestamp.

pose_t m_curr_odometry_only_pose: Current robot position based solely on odometry.

bool m_request_to_exit: Indicate whether the user wants to exit the application (e.g. pressed by pressign ctrl-c)

std::string m_class_name

bool m_is_first_time_node_reg

Track the first node registration occurance.

Handy so that we can assign a measurement to the root node as well.

std::vector<std::string> m_supported_constraint_types: MRPT CNetworkOfPoses constraint classes that are currently supported.

std::string m_current_constraint_type: Type of constraint currently in use.

Protected Static Functions

static mrpt::system::TTimeStamp getTimeStamp(const mrpt::obs::CActionCollection::Ptr action, const mrpt::obs::CSensoryFrame::Ptr observations, const mrpt::obs::CObservation::Ptr observation)

Fill the TTimestamp in a consistent manner.

Method can be used in both MRPT Rawlog formats

Note

if both action_ptr and observation_ptr contains valid timestamps, the action is preferred.

Parameters:

action_ptr – [in] Pointer to the action (action-observations format)
observations – [in] Pointer to list of observations (action-observations format)
observation – [in] Pointer to single observation (observation-only format)

Returns:

mrpt::system::TTimeStamp

struct TRGBDInfoFileParams

Struct responsible for keeping the parameters of the .info file in RGBD related datasets.

Public Functions

TRGBDInfoFileParams()

TRGBDInfoFileParams(const std::string &rawlog_fname)

~TRGBDInfoFileParams() = default

void initTRGBDInfoFileParams()

void parseFile(): Parse the RGBD information file to gain information about the rawlog file contents.

void setRawlogFile(const std::string &rawlog_fname)

Public Members

std::map<std::string, std::string> fields: Format for the parameters in the info file: string literal - related value (kept in a string representation)

std::string info_fname