Go to the documentation of this file.
139 template <
class GRAPH_T =
typename mrpt::graphs::CNetworkOfPoses2DInf>
144 using fstreams_out = std::map<std::string, mrpt::io::CFileOutputStream>;
151 using pose_t =
typename GRAPH_T::constraint_t::type_value;
184 const std::string& config_file,
const std::string& rawlog_fname =
"",
185 const std::string& fname_GT =
"",
205 std::set<mrpt::graphs::TNodeID>* nodes_set)
const;
215 void saveGraph(
const std::string* fname_in =
nullptr)
const;
224 void save3DScene(
const std::string* fname_in =
nullptr)
const;
327 const std::string& fname_GT,
328 std::vector<mrpt::poses::CPose2D>* gt_poses,
329 std::vector<mrpt::system::TTimeStamp>* gt_timestamps =
nullptr);
331 const std::string& fname_GT,
332 std::vector<mrpt::poses::CPose3D>* gt_poses,
333 std::vector<mrpt::system::TTimeStamp>* gt_timestamps =
nullptr);
353 const std::string& fname_GT,
354 std::vector<mrpt::poses::CPose2D>* gt_poses,
355 std::vector<mrpt::system::TTimeStamp>* gt_timestamps =
nullptr);
379 std::map<std::string, int>* node_stats,
380 std::map<std::string, int>* edge_stats,
420 "Program is paused. "
423 <<
"\" in the dipslay window to resume");
433 std::this_thread::sleep_for(1s);
551 nodes_to_laser_scans2D,
552 bool full_update =
false);
601 const int keep_every_n_entries = 2);
627 std::string viz_flag,
int sleep_time = 500 );
674 const std::string& model_name,
676 const size_t model_size = 1,
const pose_t& init_pose =
pose_t());
687 bool registered =
false, std::string class_name =
"Class");
746 std::unique_ptr<mrpt::gui::CDisplayWindowPlots>
m_win_plot =
nullptr;
908 std::map<std::string, std::string>
fields;
void updateAllVisuals()
Wrapper around the deciders/optimizer updateVisuals methods.
std::shared_ptr< CObservation > Ptr
mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T > * m_edge_reg
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
Last laser scan that the current class instance received.
int m_text_index_estimated_traj
double m_offset_y_odometry
Interface for implementing node registration classes.
pose_t m_curr_odometry_only_pose
Current robot position based solely on odometry.
void getMap(mrpt::maps::COccupancyGridMap2D::Ptr map, mrpt::system::TTimeStamp *acquisition_time=nullptr) const
mrpt::img::TColor m_estimated_traj_color
~CGraphSlamEngine() override
void initIntensityImageViewport()
void toggleMapVisualization()
void updateEstimatedTrajectoryVisualization(bool full_update=false)
Update the Esstimated robot trajectory with the latest estimated robot position.
void printParams() const
Print the problem parameters to the console for verification.
mrpt::graphslam::deciders::CNodeRegistrationDecider< GRAPH_T > * m_node_reg
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.
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.
void loadParams(const std::string &fname)
Read the configuration variables from the .ini file specified by the user.
const std::string m_paused_message
Message to be displayed while paused.
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
static const std::string header_sep
Separator string to be used in debugging messages.
GRAPH_T m_graph
The graph object to be built and optimized.
typename mrpt::graphs::CNetworkOfPoses2DInf ::constraint_t constraint_t
Type of graph constraints.
virtual mrpt::poses::CPose3D getLSPoseForGridMapVisualization(const mrpt::graphs::TNodeID nodeID) const
return the 3D Pose of a LaserScan that is to be visualized.
mrpt::obs::CObservation2DRangeScan::Ptr m_first_laser_scan2D
First recorded laser scan - assigned to the root.
virtual void updateCurrPosViewport()
Update the viewport responsible for displaying the graph-building procedure in the estimated position...
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void toggleOdometryVisualization()
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
mrpt::maps::COctoMap::Ptr m_octomap_cached
void alignOpticalWithMRPTFrame()
Interface for implementing edge registration classes.
bool m_is_paused
Indicated if program is temporarily paused by the user.
mrpt::graphs::TNodeID m_nodeID_max
Internal counter for querying for the number of nodeIDs.
typename mrpt::graphs::CNetworkOfPoses2DInf ::constraint_t::type_value pose_t
Type of underlying poses (2D/3D).
double m_offset_y_estimated_traj
Interface for implementing graphSLAM optimizer classes.
std::vector< pose_t > m_GT_poses
std::mutex m_graph_section
Mark graph modification/accessing explicitly for multithreaded implementation.
int m_text_index_odometry
size_t m_robot_model_size
How big are the robots going to be in the scene.
const GRAPH_T & getGraph() const
Return a reference to the underlying GRAPH_T instance.
bool m_enable_range_viewport
mrpt::img::TColor m_current_constraint_type_color
std::shared_ptr< mrpt::maps ::COctoMap > Ptr
std::map< mrpt::graphs::TNodeID, size_t > m_nodeID_to_gt_indices
Map from nodeIDs to their corresponding closest GT pose index.
static double accumulateAngleDiffs(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
bool execGraphSlamStep(mrpt::obs::CObservation::Ptr &observation, size_t &rawlog_entry)
Wrapper method around _execGraphSlamStep.
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
std::string m_keystroke_pause_exec
int m_text_index_paused_message
mrpt::math::CMatrixDouble33 m_rot_TUM_to_MRPT
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualization()
void toggleGTVisualization()
double m_offset_y_paused_message
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.
bool m_map_is_cached
Indicates if the map is cached.
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
void queryObserverForEvents()
Query the observer instance for any user events.
std::string m_keystroke_map
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the class state.
std::string m_GT_file_format
void addTextMessage(const double x_frac, const double y_frac, const std::string &text, const size_t unique_index=0, const mrpt::opengl::TFontParams &fontParams=mrpt::opengl::TFontParams())
A shortcut for calling mrpt::opengl::COpenGLViewport::addTextMessage() in the "main" viewport of the ...
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Monitor events in the visualization window.
void toggleEstimatedTrajectoryVisualization()
Struct responsible for keeping the parameters of the .info file in RGBD related datasets.
std::string getRawlogFname()
Return the filename of the used rawlog file.
static const std::string report_sep
void initTRGBDInfoFileParams()
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
mrpt::opengl::CSetOfObjects::Ptr initRobotModelVisualizationInternal(const mrpt::poses::CPose2D &p_unused)
Method to help overcome the partial template specialization restriction of C++.
mrpt::img::TColor m_optimized_map_color
void initSlamMetricVisualization()
std::string getParamsAsString() const
Wrapper around getParamsAsString.
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
std::string m_img_prev_path_base
double m_offset_x_left
Offset from the left side of the canvas.
virtual void getNodeIDsOfEstimatedTrajectory(std::set< mrpt::graphs::TNodeID > *nodes_set) const
Return the list of nodeIDs which make up robot trajectory.
std::string m_config_fname
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.
void initOdometryVisualization()
std::string m_current_constraint_type
Type of constraint currently in use.
void updateOdometryVisualization()
Update odometry-only cloud with latest odometry estimation.
SLAM methods related to graphs of pose constraints.
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.
nodes_to_scans2D_t m_nodes_to_laser_scans2D
Map of NodeIDs to their corresponding LaserScans.
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.
mrpt::maps::CSimpleMap m_simple_map_cached
Acquired map in CSimpleMap representation.
void computeMap() const
Compute the map of the environment based on the recorded measurements.
void initMapVisualization()
std::string m_keystroke_odometry
double m_offset_y_current_constraint_type
mrpt::graphslam::optimizers::CGraphSlamOptimizer< GRAPH_T > * m_optimizer
double m_dataset_grab_time
Time it took to record the dataset.
size_t m_GT_poses_step
Rate at which to read the GT poses.
void generateReportFiles(const std::string &output_dir_fname_in)
Generate and write to a corresponding report for each of the respective self/decider/optimizer classe...
void initEstimatedTrajectoryVisualization()
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.
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
void initGTVisualization()
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
mrpt::gui::CDisplayWindow3D * m_win
void parseFile()
Parse the RGBD information file to gain information about the rawlog file contents.
std::string m_keystroke_GT
mrpt::obs::CObservation3DRangeScan::Ptr m_last_laser_scan3D
Last laser scan that the current class instance received.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
bool m_visualize_SLAM_metric
bool m_enable_curr_pos_viewport
bool m_request_to_exit
Indicate whether the user wants to exit the application (e.g.
std::vector< std::string > m_supported_constraint_types
MRPT CNetworkOfPoses constraint classes that are currently supported.
virtual void setObjectPropsFromNodeID(const mrpt::graphs::TNodeID nodeID, mrpt::opengl::CSetOfObjects::Ptr &viz_object)
Set the properties of the map visual object based on the nodeID that it was produced by.
void getDeformationEnergyVector(std::vector< double > *vec_out) const
Fill the given vector with the deformation energy values computed for the SLAM evaluation metric.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
fstreams_out m_out_streams
keeps track of the out fstreams so that they can be closed (if still open) in the class Dtor.
double m_curr_deformation_energy
void setRawlogFile(const std::string &rawlog_fname)
struct mrpt::graphslam::CGraphSlamEngine::TRGBDInfoFileParams m_info_params
bool m_visualize_odometry_poses
void saveGraph(const std::string *fname_in=nullptr) const
Wrapper method around the GRAPH_T::saveToTextFile method.
bool m_use_GT
Flag for specifying if we are going to use ground truth data at all.
bool m_observation_only_dataset
std::map< std::string, std::string > fields
Format for the parameters in the info file: string literal - related value (kept in a string represen...
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.
~TRGBDInfoFileParams()=default
void initCurrPosViewport()
mrpt::maps::COccupancyGridMap2D::Ptr m_gridmap_cached
void updateGTVisualization()
Display the next ground truth position in the visualization window.
Versatile class for consistent logging and management of output messages.
typename mrpt::graphs::CNetworkOfPoses2DInf ::global_pose_t global_pose_t
std::string m_keystroke_estimated_trajectory
void initClass()
General initialization method to call from the Class Constructors.
std::vector< double > m_deformation_energy_vec
void execDijkstraNodesEstimation()
Wrapper around the GRAPH_T::dijkstra_nodes_estimate.
void updateSlamMetricVisualization()
Update the displayPlots window with the new information with regards to the metric.
bool m_is_first_time_node_reg
Track the first node registration occurance.
void updateRangeImageViewport()
In RGB-D TUM Datasets update the Range image displayed in a seperate viewport.
std::string upperCase(const std::string &str)
Returns a upper-case version of a string.
void computeSlamMetric(mrpt::graphs::TNodeID nodeID, size_t gt_index)
Compare the SLAM result (estimated trajectory) with the GT path.
mrpt::graphslam::detail::CEdgeCounter m_edge_counter
Instance to keep track of all the edges + visualization related operations.
bool m_user_decides_about_output_dir
int m_text_index_timestamp
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
const bool m_enable_visuals
Determine if we are to enable visualization support or not.
void initResultsFile(const std::string &fname)
Automate the creation and initialization of a results file relevant to the application.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
std::string m_img_external_storage_dir
mrpt::graphslam::CWindowObserver * m_win_observer
std::shared_ptr< mrpt::maps ::COccupancyGridMap2D > Ptr
size_t m_GT_poses_index
Counter for reading back the GT_poses.
mrpt::opengl::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.
typename fstreams_out::iterator fstreams_out_it
Map for iterating over output file streams.
bool m_enable_intensity_viewport
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.
std::unique_ptr< mrpt::gui::CDisplayWindowPlots > m_win_plot
DisplayPlots instance for visualizing the evolution of the SLAM metric.
std::string m_rawlog_fname
Rawlog file from which to read the measurements.
double m_offset_y_timestamp
mrpt::system::TTimeStamp m_init_timestamp
First recorded timestamp in the dataset.
virtual GRAPH_T::global_poses_t getRobotEstimatedTrajectory() const
void save3DScene(const std::string *fname_in=nullptr) const
Wrapper method around the COpenGLScene::saveToFile method.
std::map< std::string, mrpt::io::CFileOutputStream > fstreams_out
Map for managing output file streams.
bool m_visualize_estimated_trajectory
std::shared_ptr< mrpt::obs ::CObservation3DRangeScan > Ptr
int m_text_index_current_constraint_type
std::vector< pose_t > m_odometry_poses
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
mrpt::system::TTimeStamp m_curr_timestamp
Current dataset timestamp.
mrpt::img::TColor m_odometry_color
void initRangeImageViewport()
Main file for the GraphSlamEngine.
mrpt::system::TTimeStamp m_map_acq_time
Timestamp at which the map was computed.
global_pose_t getCurrentRobotPosEstimation() const
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Wrapper method that used for printing error messages in a consistent manner.
mrpt::img::TColor m_GT_color
void updateIntensityImageViewport()
In RGB-D TUM Datasets update the Intensity image displayed in a seperate viewport.
Generic class for tracking the total number of edges for different tpes of edges and for storing visu...
mrpt::graphslam::CWindowManager * m_win_manager
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |