10 #ifndef CRANGESCANOPS_IMPL_H 11 #define CRANGESCANOPS_IMPL_H 13 namespace mrpt {
namespace graphslam {
namespace deciders {
15 template<
class GRAPH_T>
34 if (initial_pose_in) {
35 initial_pose = *initial_pose_in;
38 mrpt::poses::CPosePDFPtr pdf = params.icp.Align(
46 rel_edge->copyFrom(*pdf);
49 if (icp_info) *icp_info = info;
53 template<
class GRAPH_T>
63 mrpt::format(
"Laser scan doesn't contain valid range image"));
65 mrpt::format(
"Laser scan doesn't contain valid range image"));
80 if (initial_pose_in) {
84 mrpt::poses::CPose3DPDFPtr pdf = params.icp.Align3D(
95 if (fabs(pdf->getMeanVal().z()) < 0.1) {
96 rel_edge->copyFrom(*pdf);
103 if (icp_info) *icp_info = info;
109 template<
class GRAPH_T>
112 size_t keep_point_every,
116 size_t map_size = m->
size();
120 size_t conservative_keep_point_every = map_size / low_lim;
121 keep_point_every =
std::min(keep_point_every, conservative_keep_point_every);
125 std::vector<bool> deletion_mask(map_size,
true);
126 for (
size_t i = 0; i != map_size; ++i) {
127 if (i % keep_point_every == 0) {
128 deletion_mask[i] =
false;
138 template<
class GRAPH_T>
140 mrpt::obs::CObservation3DRangeScanPtr& scan3D_in,
141 mrpt::obs::CObservation2DRangeScanPtr* scan2D_out ) {
144 bool success =
false;
146 if ( (*scan2D_out).null() ) {
150 if (scan3D_in->hasRangeImage) {
151 scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
155 std::cout <<
"No valid rangeImage found" << std::endl;
166 template<
class GRAPH_T>
168 has_read_config(false)
171 template<
class GRAPH_T>
175 template<
class GRAPH_T>
180 out.
printf(
"3D=>2D LaserScan Conversion Sensor label = %s\n",
182 out.
printf(
"3D=>2D LaserScan Conversion angle sup = %.2f deg\n",
184 out.
printf(
"3D=>2D LaserScan Conversion angle inf = %.2f deg\n",
186 out.
printf(
"3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
188 out.
printf(
"3D=>2D LaserScan Conversion Z minimum = %.2f\n",
195 template<
class GRAPH_T>
198 const std::string& section) {
203 "conversion_sensor_label",
204 "KINECT_TO_2D_SCAN",
false);
207 "conversion_angle_sup",
211 "conversion_angle_inf",
215 "conversion_oversampling_ratio",
double DEG2RAD(const double x)
Degrees to radians.
std::string sensorLabel
The sensor label that will have the newly created observation.
mrpt::obs::T3DPointsTo2DScanParams conversion_params
Struct holding the parameters of 3D to the corresponding 2D range scan conversion.
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
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.
void decimatePointsMap(mrpt::maps::CPointsMap *m, size_t keep_point_every=4, size_t low_lim=0)
Reduce the size of the given CPointsMap by keeping one out of "keep_point_every" points.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
TConfigParams options
The options employed by the ICP align.
This class allows loading and storing values and vectors of different types from a configuration text...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion)
This method load the options from a ".ini"-like file or memory-stored string list.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
GRAPH_T::constraint_t constraint_t
Handy typedefs.
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
double RAD2DEG(const double x)
Radians to degrees.
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool's array as "true".
bool hasRangeImage
true means the field rangeImage contains valid data
double angle_inf
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
static CObservation2DRangeScanPtr Create()
T min(const T v0, const T v1)
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
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).
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
The ICP algorithm return information.
double read_double(const std::string §ion, const std::string &name, double defaultValue, bool failIfNotFound=false) const
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
std::string read_string(const std::string §ion, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
size_t size() const
Returns the number of stored points in the map.
#define ASSERTMSG_(f, __ERROR_MSG)
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScanPtr &scan3D_in, mrpt::obs::CObservation2DRangeScanPtr *scan2D_out=NULL)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.