Go to the documentation of this file.
14 template <
class GRAPH_T>
37 template <
class GRAPH_T>
49 bool registered_new_node =
false;
57 std::dynamic_pointer_cast<CObservation2DRangeScan>(observation);
63 std::dynamic_pointer_cast<CObservation3DRangeScan>(observation);
71 std::dynamic_pointer_cast<CObservationOdometry>(observation);
83 if (action->getBestMovementEstimation())
88 action->getBestMovementEstimation();
91 increment_gaussian.
copyFrom(*increment);
111 return registered_new_node;
116 template <
class GRAPH_T>
121 bool registered_new_node =
false;
135 return registered_new_node;
139 template <
class GRAPH_T>
146 bool registered_new_node =
false;
157 MRPT_LOG_DEBUG(
">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
159 "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
162 "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
163 rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
165 "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
171 double mahal_distance =
182 double mahal_distance_lim = 0.18;
189 if (mahal_distance < mahal_distance_lim ||
215 MRPT_LOG_DEBUG(
"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
216 return registered_new_node;
220 template <
class GRAPH_T>
228 template <
class GRAPH_T>
235 template <
class GRAPH_T>
246 bool angle_crit =
false;
253 bool distance_crit =
false;
261 bool registered =
false;
262 if (distance_crit || angle_crit)
271 template <
class GRAPH_T>
278 source_fname,
"NodeRegistrationDeciderParameters");
285 int min_verbosity_level = source.
read_int(
286 "NodeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
292 template <
class GRAPH_T>
299 template <
class GRAPH_T>
301 std::string* report_str)
const
310 stringstream class_props_ss;
311 class_props_ss <<
"ICP Goodness-based Registration Procedure Summary: "
323 *report_str += class_props_ss.str();
327 *report_str += time_res;
330 *report_str += output_res;
336 template <
class GRAPH_T>
341 template <
class GRAPH_T>
343 std::ostream&
out)
const
349 out <<
"------------------[ ICP Fixed Intervals Node Registration "
350 "]------------------\n";
352 "Max distance for registration = %.2f m\n", registration_max_distance);
354 "Max Angle for registration = %.2f deg\n",
355 RAD2DEG(registration_max_angle));
357 decider.range_ops_t::params.dumpToTextStream(
out);
361 template <
class GRAPH_T>
370 section,
"registration_max_distance", 0.5 ,
false);
372 section,
"registration_max_angle", 10 ,
false);
373 registration_max_angle =
DEG2RAD(registration_max_angle);
376 decider.range_ops_t::params.loadFromConfigFile(source,
"ICP");
std::shared_ptr< CObservation > Ptr
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
unsigned int nIterations
The number of executed iterations until convergence.
std::shared_ptr< mrpt::obs ::CObservationOdometry > Ptr
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
static const std::string header_sep
Separator string to be used in debugging messages.
double registration_max_distance
Maximum distance for new node registration.
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information.
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
pose_t m_curr_odometry_only_pose
pose_t estimation using only odometry information.
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
bool m_use_angle_difference_node_reg
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
bool checkRegistrationCondition() override
Check whether a new node should be registered in the graph.
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...
void loadFromConfigFileName(const std::string &config_file, const std::string §ion)
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile ob...
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
mrpt::vision::TStereoCalibResults out
void resetPDF(constraint_t *c)
Reset the given PDF method and assign a fixed high-certainty Covariance/Information matrix.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
#define THROW_EXCEPTION(msg)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
This namespace contains representation of robot actions and observations.
The ICP algorithm return information.
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
An observation of the current (cumulative) odometry for a wheeled robot.
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
std::string getLogAsString() const
Get the history of COutputLogger instance in a string representation.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
double leave(const std::string_view &func_name) noexcept
End of a named section.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
VerbosityLevel
Enumeration of available verbosity levels.
This class allows loading and storing values and vectors of different types from a configuration text...
constexpr double RAD2DEG(const double x)
Radians to degrees.
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
handy laser scans to use in the class methods
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account.
void enter(const std::string_view &func_name) noexcept
Start of a named section.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
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.
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.
constexpr double DEG2RAD(const double x)
Degrees to radians
mrpt::vision::TStereoCalibParams params
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...
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
constraint_t m_latest_odometry_PDF
Odometry rigid-body transformation since the last accepted LaserScan.
virtual 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 "....
mrpt::obs::CObservation2DRangeScan::Ptr m_curr_laser_scan2D
Current LaserScan.
static const std::string report_sep
double registration_max_angle
Maximum angle difference for new node registration.
bool m_use_distance_node_reg
This base provides a set of functions for maths stuff.
bool registerNewNodeAtEnd()
Same goal as the previous method - uses the m_since_prev_node_PDF as the constraint at the end.
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
ICP-based Fixed Intervals Node Registration.
pose_t m_last_odometry_only_pose
pose_t estimation using only odometry information.
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
std::shared_ptr< CPosePDF > Ptr
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
std::shared_ptr< mrpt::obs ::CObservation3DRangeScan > Ptr
std::string std::string format(std::string_view fmt, ARGS &&... args)
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020 | |