Go to the documentation of this file.
10 #ifndef CFIXEDINTERVALSNRD_IMPL_H
11 #define CFIXEDINTERVALSNRD_IMPL_H
22 template <
class GRAPH_T>
25 this->initializeLoggers(
"CFixedIntervalsNRD");
28 template <
class GRAPH_T>
36 template <
class GRAPH_T>
52 m_observation_only_rawlog =
true;
57 std::dynamic_pointer_cast<CObservationOdometry>(observation);
59 m_curr_odometry_only_pose =
pose_t(obs_odometry->odometry);
61 "Current odometry-only pose: %s",
62 m_curr_odometry_only_pose.asString().c_str());
66 this->m_since_prev_node_PDF.mean =
67 m_curr_odometry_only_pose - m_last_odometry_only_pose;
72 m_observation_only_rawlog =
false;
75 bool found = action->getFirstMovementEstimation(move_pdf);
82 this->m_since_prev_node_PDF += incr_constraint;
86 bool registered = this->checkRegistrationCondition();
90 if (m_observation_only_rawlog)
94 m_last_odometry_only_pose = m_curr_odometry_only_pose;
103 template <
class GRAPH_T>
110 pose_t last_pose_inserted =
112 ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
116 bool registered =
false;
118 if (this->checkRegistrationCondition(
119 last_pose_inserted, this->getCurrentRobotPosEstimation()))
121 registered = this->registerNewNodeAtEnd();
128 template <
class GRAPH_T>
144 template <
class GRAPH_T>
153 params.registration_max_angle) ||
155 params.registration_max_angle) ||
164 template <
class GRAPH_T>
168 parent_t::loadParams(source_fname);
170 params.loadFromConfigFileName(
171 source_fname,
"NodeRegistrationDeciderParameters");
175 int min_verbosity_level =
source.read_int(
176 "NodeRegistrationDeciderParameters",
"class_verbosity", 1,
false);
183 template <
class GRAPH_T>
187 parent_t::printParams();
193 template <
class GRAPH_T>
204 stringstream class_props_ss;
205 class_props_ss <<
"Strategy: "
206 <<
"Fixed Odometry-based Intervals" << std::endl;
207 class_props_ss << header_sep << std::endl;
210 const std::string time_res = this->m_time_logger.getStatsAsText();
211 const std::string output_res = this->getLogAsString();
215 parent_t::getDescriptiveReport(report_str);
217 *report_str += class_props_ss.str();
218 *report_str += report_sep;
221 *report_str +=
params.getAsString();
222 *report_str += report_sep;
225 *report_str += time_res;
226 *report_str += report_sep;
228 *report_str += output_res;
229 *report_str += report_sep;
236 template <
class GRAPH_T>
240 template <
class GRAPH_T>
244 template <
class GRAPH_T>
246 std::ostream& out)
const
252 template <
class GRAPH_T>
259 registration_max_distance =
source.read_double(
260 section,
"registration_max_distance", 0.5 ,
false);
261 registration_max_angle =
source.read_double(
262 section,
"registration_max_angle", 60 ,
false);
263 registration_max_angle =
DEG2RAD(registration_max_angle);
268 template <
class GRAPH_T>
275 double max_angle_deg =
RAD2DEG(registration_max_angle);
279 "------------------[ Fixed Intervals Node Registration "
280 "]------------------\n";
282 "Max distance for registration = %.2f m\n", registration_max_distance);
284 "Max angle for registration = %.2f deg\n", max_angle_deg);
288 template <
class GRAPH_T>
294 this->getAsString(&str);
std::shared_ptr< CObservation > Ptr
std::string getAsString() const
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
std::shared_ptr< CSensoryFrame > Ptr
std::shared_ptr< CObservationOdometry > Ptr
double pitch() const
Get the PITCH angle (in radians)
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
const double & phi() const
Get the phi angle of the 2D pose (in radians)
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
CFixedIntervalsNRD()
Class constructor.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
This namespace contains representation of robot actions and observations.
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the las...
GLsizei GLsizei GLchar * source
An observation of the current (cumulative) odometry for a wheeled robot.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
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...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
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
#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...
double roll() const
Get the ROLL angle (in radians)
bool checkRegistrationCondition()
If estimated position surpasses the registration max values since the previous registered node,...
~CFixedIntervalsNRD()
Class destructor.
double yaw() const
Get the YAW angle (in radians)
This class allows loading and storing values and vectors of different types from "....
This base provides a set of functions for maths stuff.
GLsizei const GLchar ** string
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.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
std::shared_ptr< CActionCollection > Ptr
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
GLenum const GLfloat * params
double DEG2RAD(const double x)
Degrees to radians.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |