10 #ifndef CICPCRITERIANRD_IMPL_H 11 #define CICPCRITERIANRD_IMPL_H 18 template <
class GRAPH_T>
40 template <
class GRAPH_T>
45 template <
class GRAPH_T>
58 bool registered_new_node =
false;
92 if (action->getBestMovementEstimation())
97 action->getBestMovementEstimation();
100 increment_gaussian.
copyFrom(*increment);
120 return registered_new_node;
125 template <
class GRAPH_T>
130 bool registered_new_node =
false;
144 return registered_new_node;
148 template <
class GRAPH_T>
155 bool registered_new_node =
false;
166 MRPT_LOG_DEBUG(
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
168 "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
171 "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
172 rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
174 "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
180 double mahal_distance =
191 double mahal_distance_lim = 0.18;
198 if (mahal_distance < mahal_distance_lim ||
224 MRPT_LOG_DEBUG(
"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
225 return registered_new_node;
229 template <
class GRAPH_T>
237 template <
class GRAPH_T>
244 template <
class GRAPH_T>
255 bool angle_crit =
false;
260 params.registration_max_angle;
262 bool distance_crit =
false;
266 params.registration_max_distance;
270 bool registered =
false;
271 if (distance_crit || angle_crit)
280 template <
class GRAPH_T>
286 params.loadFromConfigFileName(
287 source_fname,
"NodeRegistrationDeciderParameters");
294 int min_verbosity_level =
source.read_int(
295 "NodeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
301 template <
class GRAPH_T>
313 template <
class GRAPH_T>
324 stringstream class_props_ss;
325 class_props_ss <<
"ICP Goodness-based Registration Procedure Summary: " 337 *report_str += class_props_ss.str();
341 *report_str += time_res;
344 *report_str += output_res;
352 template <
class GRAPH_T>
356 template <
class GRAPH_T>
360 template <
class GRAPH_T>
362 std::ostream& out)
const 369 "------------------[ ICP Fixed Intervals Node Registration " 370 "]------------------\n");
372 "Max distance for registration = %.2f m\n", registration_max_distance);
374 "Max Angle for registration = %.2f deg\n",
375 RAD2DEG(registration_max_angle));
377 decider.range_ops_t::params.dumpToTextStream(out);
381 template <
class GRAPH_T>
389 registration_max_distance =
source.read_double(
390 section,
"registration_max_distance", 0.5 ,
false);
391 registration_max_angle =
source.read_double(
392 section,
"registration_max_angle", 10 ,
false);
393 registration_max_angle =
DEG2RAD(registration_max_angle);
396 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.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
#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.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
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.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
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...
bool checkRegistrationCondition()
Check whether a new node should be registered in the graph.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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);
static const std::string report_sep
CICPCriteriaNRD()
Class constructor.
This namespace contains representation of robot actions and observations.
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...
double leave(const char *func_name)
End of a named section.
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...
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
~CICPCriteriaNRD()
Class destructor.
The ICP algorithm return information.
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
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...
An observation of the current (cumulative) odometry for a wheeled robot.
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Update the decider state using the latest dataset measurements.
GLenum const GLfloat * params
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
void enter(const char *func_name)
Start of a named section.
#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...