Go to the documentation of this file.
33 CMetricMapBuilderICP::CMetricMapBuilderICP()
34 : ICP_options(m_min_verbosity_level)
58 : matchAgainstTheGrid(false),
59 insertionLinDistance(1.0),
60 insertionAngDistance(30.0_deg),
61 localizationLinDistance(0.20),
62 localizationAngDistance(30.0_deg),
63 minICPgoodnessToAccept(0.40),
64 verbosity_level(parent_verbosity_level),
93 section,
"verbosity_level", verbosity_level);
97 mapInitializers.loadFromConfigFile(source, section);
101 std::ostream&
out)
const
103 out <<
"\n----------- [CMetricMapBuilderICP::TConfigParams] ------------ "
107 "insertionLinDistance = %f m\n",
108 insertionLinDistance);
110 "insertionAngDistance = %f deg\n",
111 RAD2DEG(insertionAngDistance));
113 "localizationLinDistance = %f m\n",
114 localizationLinDistance);
116 "localizationAngDistance = %f deg\n",
117 RAD2DEG(localizationAngDistance));
119 "verbosity_level = %s\n",
124 out <<
" Now showing 'mapsInitializers':\n";
125 mapInitializers.dumpToTextStream(
out);
142 throw std::runtime_error(
143 "Neither grid maps nor points map: Have you called initialize() "
144 "after setting ICP_options.mapInitializers?");
151 MRPT_LOG_DEBUG(
"processObservation(): obs is CObservationOdometry");
155 std::dynamic_pointer_cast<CObservationOdometry>(obs);
163 odo->odometry.asTPose(), odo->timestamp, odo->hasVelocities,
166 if (pose_before_valid)
173 "processObservation(): obs is CObservationOdometry, new "
183 TPose2D initialEstimatedRobotPose(0, 0, 0);
189 "processObservation(): extrapolating pose from latest pose "
190 "and new observation timestamp...");
192 initialEstimatedRobotPose, robotVelLocal,
193 robotVelGlobal, obs->timestamp))
199 "processObservation(): new pose extrapolation failed, "
200 "using last pose as is.");
206 "processObservation(): invalid observation timestamp.");
212 CPose2D previousKnownRobotPose;
218 previousKnownRobotPose);
224 const bool we_skip_ICP_pose_correction =
234 "processObservation(): skipping ICP pose correction due to small "
235 "odometric displacement? : "
236 << (we_skip_ICP_pose_correction ?
"YES" :
"NO"));
239 bool can_do_icp =
false;
246 matchWith =
static_cast<CMetricMap*
>(pGrid.get());
247 MRPT_LOG_DEBUG(
"processObservation(): matching against gridmap.");
252 ASSERTMSG_(pPts,
"No points map in multi-metric map.");
254 matchWith =
static_cast<CMetricMap*
>(pPts.get());
255 MRPT_LOG_DEBUG(
"processObservation(): matching against point map.");
259 if (!we_skip_ICP_pose_correction)
285 std::dynamic_pointer_cast<CObservation2DRangeScan>(obs);
289 obsLaser->sensorPose.z()) < 0.01)
313 const auto firstGuess =
319 firstGuess, icpReturn);
335 "processObservation: previousPose="
336 << previousKnownRobotPose <<
"-> currentPose="
339 "[CMetricMapBuilderICP] Fit:%.1f%% Itr:%i In "
347 "Ignoring ICP of low quality: "
348 << icpReturn.
goodness * 100 << std::endl);
356 currentKnownRobotPose);
363 "Cannot do ICP: empty pointmap or not suitable "
376 const bool firstTimeForThisSensor =
380 firstTimeForThisSensor ||
397 if (matchWith && matchWith->
isEmpty()) update =
true;
400 "update map: " << (update ?
"YES" :
"NO")
401 <<
" options.enableMapUpdating: "
416 currentKnownRobotPose.
asTPose();
423 "Updating map from pose %s\n",
424 currentKnownRobotPose.
asString().c_str()));
426 CPose3D estimatedPose3D(currentKnownRobotPose);
427 const bool anymap_update =
431 "**No map was updated** after inserting an "
434 << obs->GetRuntimeClass()->className <<
"`");
447 "Map updated OK. Done in "
480 std::make_shared<CObservationOdometry>();
481 obs->timestamp = movEstimation->timestamp;
515 pdf3D->copyFrom(pdf2D);
561 posePDF->getMean(estimatedPose3D);
564 SF->insertObservationsInto(&
metricMap, &estimatedPose3D);
571 std::vector<float>& x, std::vector<float>& y)
578 pPts->getAllPoints(x, y);
606 const std::string& file,
bool formatEMF_BMP)
625 unsigned int imgHeight = img.getHeight();
632 x2 = pGrid->x2idx(0.0f);
633 y2 = pGrid->y2idx(0.0f);
636 for (
size_t j = 0; j < nPoses; j++)
648 x1, imgHeight - 1 - y1, x2, imgHeight - 1 - y2, TColor::black());
672 ang = std::abs(Ap.phi());
677 this->last_update = p.
asTPose();
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
std::shared_ptr< CObservation > Ptr
void updateDistances(const mrpt::poses::CPose2D &p)
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) override
Appends a new action and observations to update this map: See the description of the class at the top...
unsigned int nIterations
The number of executed iterations until convergence.
mrpt::math::TPose2D asTPose() const
TConfigParams options
The options employed by the ICP align.
std::shared_ptr< mrpt::obs ::CObservationOdometry > Ptr
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
~CMetricMapBuilderICP() override
Destructor:
bool contains(const mrpt::rtti::TRuntimeClassId *id) const
Does the list contains this class?
std::map< std::string, TDist > m_distSinceLastInsertion
Indexed by sensor label.
void processUpdateNewPoseLocalization(const mrpt::math::TPose2D &newPose, mrpt::Clock::time_point tim)
Updates the filter with new global-coordinates localization data from a localization or SLAM source.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
#define MRPT_LOG_INFO(_STRING)
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
static time_point now() noexcept
Returns the current time using the currently selected Clock source.
A high-performance stopwatch, with typical resolution of nanoseconds.
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
bool m_there_has_been_an_odometry
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
void loadCurrentMapFromFile(const std::string &fileName)
Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file.
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true.
void getCurrentMapPoints(std::vector< float > &x, std::vector< float > &y)
Returns the 2D points of current local map.
mrpt::math::CMatrixDouble33 m_lastPoseEst_cov
Last pose estimation (covariance)
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
The pose estimation by the alignment algorithm (ICP).
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
mrpt::vision::TStereoCalibResults out
void getAsImage(mrpt::img::CImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if ...
std::string currentMapFile
Current map file.
void leaveCriticalSection()
Leave critical section for map updating.
#define MRPT_LOG_THROTTLE_WARN(_PERIOD_SECONDS, _STRING)
Declares a class for storing a collection of robot actions.
bool enableMapUpdating
Enable map updating, default is true.
#define THROW_EXCEPTION(msg)
bool matchAgainstTheGrid
(default:false) Match against the occupancy grid or the points map? The former is quicker but less pr...
#define ASSERT_(f)
Defines an assertion mechanism.
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
void updatePose(const mrpt::poses::CPose2D &p)
This namespace contains representation of robot actions and observations.
bool getCurrentEstimate(mrpt::math::TPose2D &pose, mrpt::math::TTwist2D &velLocal, mrpt::math::TTwist2D &velGlobal, mrpt::Clock::time_point tim_query=mrpt::Clock::now()) const
Get the estimate for a given timestamp (defaults to now()), obtained as:
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
The ICP algorithm return information.
std::shared_ptr< mrpt::poses ::CPose3DPDFGaussian > Ptr
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
CPose2D mean
The mean value.
double Tac() noexcept
Stops the stopwatch.
An observation of the current (cumulative) odometry for a wheeled robot.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
void enterCriticalSection()
Enter critical section for map updating.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
TConfigParams & operator=(const TConfigParams &other)
Declares a virtual base class for all metric maps storage classes.
bool getLatestRobotPose(mrpt::math::TPose2D &pose) const
Get the latest known robot pose, either from odometry or localization.
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0....
void processUpdateNewOdometry(const mrpt::math::TPose2D &newGlobalOdometry, mrpt::Clock::time_point cur_tim, bool hasVelocities=false, const mrpt::math::TTwist2D &newRobotVelLocal=mrpt::math::TTwist2D())
Updates the filter with new odometry readings.
VerbosityLevel
Enumeration of available verbosity levels.
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map!
This class allows loading and storing values and vectors of different types from a configuration text...
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
This class stores any customizable set of metric maps.
constexpr double RAD2DEG(const double x)
Radians to degrees.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
size_t size() const
Returns the count of pairs (pose,sensory data)
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y yaw]", yaw in degrees)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i'th pair, first one is index '0'.
void resetRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
void Tic() noexcept
Starts the stopwatch.
TConfigParams(mrpt::system::VerbosityLevel &parent_verbosity_level)
Initializer.
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
ENUMTYPE read_enum(const std::string §ion, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
std::string formatTimeInterval(const double timeSeconds)
Returns a formated string with the given time difference (passed as the number of seconds),...
#define MRPT_LOG_WARN(_STRING)
A class for storing images as grayscale or RGB bitmaps.
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
double localizationLinDistance
Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (...
#define IS_DERIVED(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is an instance of the given ...
std::mutex critZoneChangingMap
Critical zones.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
This base provides a set of functions for maths stuff.
std::shared_ptr< CPose3DPDF > Ptr
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
A class for storing an occupancy grid map.
Algorithm configuration params.
#define MRPT_LOG_INFO_STREAM(__CONTENTS)
mrpt::poses::CPose2D m_auxAccumOdometry
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
double insertionAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be i...
TConfigParams ICP_options
Options for the ICP-SLAM application.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
void reset()
Resets all internal state.
void setLoggerName(const std::string &name)
Set the name of the COutputLogger instance.
bool insertObservationPtr(const mrpt::obs::CObservation::Ptr &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
A wrapper for smart pointers, just calls the non-smart pointer version.
std::shared_ptr< CPosePDF > Ptr
void saveCurrentMapToFile(const std::string &fileName, bool compressGZ=true) const
Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.
std::string std::string format(std::string_view fmt, ARGS &&... args)
#define MRPT_LOAD_CONFIG_VAR_DEGREES( variableName, configFileObject, sectionNameStr)
Loads a double variable, stored as radians but entered in the INI-file as degrees.
void setCurrentMapFile(const char *mapFile)
Sets the "current map file", thus that map will be loaded if it exists or a new one will be created i...
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
mrpt::rtti::CListOfClasses alwaysInsertByClass
A list of observation classes (derived from mrpt::obs::CObservation) which will be always inserted in...
bool also_interpolate
If set to true, far points (<1m) are interpolated with samples at "minDistSqrBetweenLaserPoints" inte...
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020 | |