Go to the documentation of this file.
18 template <
class GRAPH_T>
21 this->initializeLoggers(
"CFixedIntervalsNRD");
27 template <
class GRAPH_T>
42 m_observation_only_rawlog =
true;
47 std::dynamic_pointer_cast<CObservationOdometry>(observation);
49 m_curr_odometry_only_pose =
pose_t(obs_odometry->odometry);
51 "Current odometry-only pose: %s",
52 m_curr_odometry_only_pose.asString().c_str());
56 this->m_since_prev_node_PDF.mean =
57 m_curr_odometry_only_pose - m_last_odometry_only_pose;
62 m_observation_only_rawlog =
false;
65 bool found = action->getFirstMovementEstimation(move_pdf);
72 this->m_since_prev_node_PDF += incr_constraint;
76 bool registered = this->checkRegistrationCondition();
80 if (m_observation_only_rawlog)
84 m_last_odometry_only_pose = m_curr_odometry_only_pose;
93 template <
class GRAPH_T>
100 pose_t last_pose_inserted =
102 ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
106 bool registered =
false;
108 if (this->checkRegistrationCondition(
109 last_pose_inserted, this->getCurrentRobotPosEstimation()))
111 registered = this->registerNewNodeAtEnd();
118 template <
class GRAPH_T>
134 template <
class GRAPH_T>
143 params.registration_max_angle) ||
145 params.registration_max_angle) ||
154 template <
class GRAPH_T>
158 parent_t::loadParams(source_fname);
160 params.loadFromConfigFileName(
161 source_fname,
"NodeRegistrationDeciderParameters");
165 int min_verbosity_level = source.
read_int(
166 "NodeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
173 template <
class GRAPH_T>
177 parent_t::printParams();
183 template <
class GRAPH_T>
185 std::string* report_str)
const
190 const std::string report_sep(2,
'\n');
191 const std::string header_sep(80,
'#');
194 stringstream class_props_ss;
195 class_props_ss <<
"Strategy: "
196 <<
"Fixed Odometry-based Intervals" << std::endl;
197 class_props_ss << header_sep << std::endl;
200 const std::string time_res = this->m_time_logger.getStatsAsText();
201 const std::string output_res = this->getLogAsString();
205 parent_t::getDescriptiveReport(report_str);
207 *report_str += class_props_ss.str();
208 *report_str += report_sep;
211 *report_str +=
params.getAsString();
212 *report_str += report_sep;
215 *report_str += time_res;
216 *report_str += report_sep;
218 *report_str += output_res;
219 *report_str += report_sep;
224 template <
class GRAPH_T>
226 std::ostream&
out)
const
232 template <
class GRAPH_T>
240 section,
"registration_max_distance", 0.5 ,
false);
242 section,
"registration_max_angle", 60 ,
false);
243 registration_max_angle =
DEG2RAD(registration_max_angle);
248 template <
class GRAPH_T>
250 std::string* params_out)
const
255 double max_angle_deg =
RAD2DEG(registration_max_angle);
259 "------------------[ Fixed Intervals Node Registration "
260 "]------------------\n";
262 "Max distance for registration = %.2f m\n", registration_max_distance);
264 "Max angle for registration = %.2f deg\n", max_angle_deg);
268 template <
class GRAPH_T>
274 this->getAsString(&str);
std::shared_ptr< CObservation > Ptr
std::string getAsString() const
std::shared_ptr< mrpt::obs ::CObservationOdometry > Ptr
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
double pitch() const
Get the PITCH angle (in radians)
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
double phi() const
Get the phi angle of the 2D pose (in radians)
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
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.
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
mrpt::vision::TStereoCalibResults out
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
void printParams() const override
This namespace contains representation of robot actions and observations.
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
An observation of the current (cumulative) odometry for a wheeled robot.
int read_int(const std::string §ion, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
VerbosityLevel
Enumeration of available verbosity levels.
This class allows loading and storing values and vectors of different types from a configuration text...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
constexpr double RAD2DEG(const double x)
Radians to degrees.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
double roll() const
Get the ROLL angle (in radians)
constexpr double DEG2RAD(const double x)
Degrees to radians
mrpt::vision::TStereoCalibParams params
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
double yaw() const
Get the YAW angle (in radians)
This class allows loading and storing values and vectors of different types from "....
bool checkRegistrationCondition() override
If estimated position surpasses the registration max values since the previous registered node,...
This base provides a set of functions for maths stuff.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the las...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020 | |