14 template <
class GRAPH_T>
37 template <
class GRAPH_T>
50 bool registered_new_node =
false;
84 if (action->getBestMovementEstimation())
89 action->getBestMovementEstimation();
92 increment_gaussian.
copyFrom(*increment);
112 return registered_new_node;
117 template <
class GRAPH_T>
122 bool registered_new_node =
false;
136 return registered_new_node;
140 template <
class GRAPH_T>
147 bool registered_new_node =
false;
158 MRPT_LOG_DEBUG(
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
160 "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
163 "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
164 rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
166 "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
172 double mahal_distance =
183 double mahal_distance_lim = 0.18;
190 if (mahal_distance < mahal_distance_lim ||
216 MRPT_LOG_DEBUG(
"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
217 return registered_new_node;
221 template <
class GRAPH_T>
229 template <
class GRAPH_T>
236 template <
class GRAPH_T>
247 bool angle_crit =
false;
252 params.registration_max_angle;
254 bool distance_crit =
false;
258 params.registration_max_distance;
262 bool registered =
false;
263 if (distance_crit || angle_crit)
272 template <
class GRAPH_T>
278 params.loadFromConfigFileName(
279 source_fname,
"NodeRegistrationDeciderParameters");
286 int min_verbosity_level =
source.read_int(
287 "NodeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
293 template <
class GRAPH_T>
300 template <
class GRAPH_T>
311 stringstream class_props_ss;
312 class_props_ss <<
"ICP Goodness-based Registration Procedure Summary: " 324 *report_str += class_props_ss.str();
328 *report_str += time_res;
331 *report_str += output_res;
337 template <
class GRAPH_T>
342 template <
class GRAPH_T>
344 std::ostream& out)
const 351 "------------------[ ICP Fixed Intervals Node Registration " 352 "]------------------\n");
354 "Max distance for registration = %.2f m\n", registration_max_distance);
356 "Max Angle for registration = %.2f deg\n",
357 RAD2DEG(registration_max_angle));
359 decider.range_ops_t::params.dumpToTextStream(out);
363 template <
class GRAPH_T>
371 registration_max_distance =
source.read_double(
372 section,
"registration_max_distance", 0.5 ,
false);
373 registration_max_angle =
source.read_double(
374 section,
"registration_max_angle", 10 ,
false);
375 registration_max_angle =
DEG2RAD(registration_max_angle);
378 decider.range_ops_t::params.loadFromConfigFile(
source,
"ICP");
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
handy laser scans to use in the class methods
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
#define THROW_EXCEPTION(msg)
bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
double DEG2RAD(const double x)
Degrees to radians.
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=nullptr, mrpt::slam::CICP::TReturnInfo *icp_info=nullptr)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
pose_t m_curr_odometry_only_pose
pose_t estimation using only odometry information.
pose_t m_last_odometry_only_pose
pose_t estimation using only odometry information.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
static const std::string header_sep
Separator string to be used in debugging messages.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
mrpt::obs::CObservation2DRangeScan::Ptr m_curr_laser_scan2D
Current LaserScan.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
ICP-based Fixed Intervals Node Registration.
bool checkRegistrationCondition() override
Check whether a new node should be registered in the graph.
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
bool registerNewNodeAtEnd()
Same goal as the previous method - uses the m_since_prev_node_PDF as the constraint at the end...
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
static const std::string report_sep
This namespace contains representation of robot actions and observations.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
bool m_use_distance_node_reg
constraint_t m_latest_odometry_PDF
Odometry rigid-body transformation since the last accepted LaserScan.
void resetPDF(constraint_t *c)
Reset the given PDF method and assign a fixed high-certainty Covariance/Information matrix...
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
unsigned short nIterations
The number of executed iterations until convergence.
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
bool m_use_angle_difference_node_reg
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
Update the decider state using the latest dataset measurements.
std::string getLogAsString() const
Get the history of COutputLogger instance in a string representation.
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information...
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
The ICP algorithm return information.
GLsizei GLsizei GLchar * source
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
An observation of the current (cumulative) odometry for a wheeled robot.
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
GLenum const GLfloat * params
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
void enter(const std::string_view &func_name)
Start of a named section.
double leave(const std::string_view &func_name)
End of a named section.
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
constraint_t m_since_prev_node_PDF
Tracking the PDF of the current position of the robot with regards to the <b previous registered node...