MRPT  2.0.2
CRangeScanOps_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
12 
14 {
15 template <class GRAPH_T>
19  const mrpt::poses::CPose2D* initial_pose_in,
21 {
23 
26 
27  // have them initialized prior - and then just clear them
28  m1.insertObservation(from);
29  m2.insertObservation(to);
30 
31  // If given, use initial_pose_in as a first guess for the ICP
32  mrpt::poses::CPose2D initial_pose;
33  if (initial_pose_in)
34  {
35  initial_pose = *initial_pose_in;
36  }
37 
39  params.icp.Align(&m1, &m2, initial_pose, info);
40 
41  // return the edge regardless of the goodness of the alignment
42  rel_edge->copyFrom(*pdf);
43 
44  // if given, fill the TReturnInfo Struct
45  if (icp_info) *icp_info = info;
46 
47  MRPT_END
48 }
49 template <class GRAPH_T>
53  const mrpt::poses::CPose2D* initial_pose_in,
55 {
57 
59  from.hasRangeImage, "Laser scan doesn't contain valid range image");
61  to.hasRangeImage, "Laser scan doesn't contain valid range image");
62 
63  // TODO - have this as a class member
66 
67  m1.insertObservation(from);
68  m2.insertObservation(to);
69 
70  // If given, use initial_pose_in as a first guess for the ICP
71  mrpt::poses::CPose3D initial_pose;
72  if (initial_pose_in)
73  {
74  initial_pose = mrpt::poses::CPose3D(*initial_pose_in);
75  }
76 
78  params.icp.Align3D(&m1, &m2, initial_pose, info);
79 
80  // return the edge regardless of the goodness of the alignment
81  // copy from the 3D PDF
82  // NULLIFY contraint_t if Z displacement is high (normally this should be 0)
83  // since we are moving in 2D
84  if (fabs(pdf->getMeanVal().z()) < 0.1)
85  {
86  rel_edge->copyFrom(*pdf);
87  }
88  else
89  {
90  rel_edge = nullptr;
91  }
92 
93  // if given, fill the TReturnInfo Struct
94  if (icp_info) *icp_info = info;
95 
96  MRPT_END
97 }
98 
99 template <class GRAPH_T>
101  mrpt::maps::CPointsMap* m, size_t keep_point_every, size_t low_lim)
102 {
103  MRPT_START
104 
105  size_t map_size = m->size();
106 
107  if (low_lim)
108  {
109  // check if current keep_point_every variable is too large
110  size_t conservative_keep_point_every = map_size / low_lim;
111  keep_point_every =
112  std::min(keep_point_every, conservative_keep_point_every);
113  }
114 
115  // insert a false every "keep_point_every" points
116  std::vector<bool> deletion_mask(map_size, true);
117  for (size_t i = 0; i != map_size; ++i)
118  {
119  if (i % keep_point_every == 0)
120  {
121  deletion_mask[i] = false;
122  }
123  }
124  m->applyDeletionMask(deletion_mask);
125 
126  // std::cout << "Map size: " << map_size << " => " << m->size() <<
127  // std::endl;
128 
129  MRPT_END
130 }
131 
132 template <class GRAPH_T>
136 {
137  MRPT_START
138 
139  bool success = false;
140  // if it doesn't exist, create it
141  if (!*scan2D_out)
142  {
144  }
145 
146  if (scan3D_in->hasRangeImage)
147  {
148  scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
149  success = true;
150  }
151  else
152  {
153  std::cout << "No valid rangeImage found" << std::endl;
154  success = false;
155  }
156 
157  return success;
158  MRPT_END
159 }
160 
161 template <class GRAPH_T>
163 {
164  MRPT_START
165 
166  out << mrpt::format(
167  "3D=>2D LaserScan Conversion Sensor label = %s\n",
169  out << mrpt::format(
170  "3D=>2D LaserScan Conversion angle sup = %.2f deg\n",
172  out << mrpt::format(
173  "3D=>2D LaserScan Conversion angle inf = %.2f deg\n",
175  out << mrpt::format(
176  "3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
178  out << mrpt::format(
179  "3D=>2D LaserScan Conversion Z minimum = %.2f\n",
181 
183 
184  MRPT_END
185 }
186 template <class GRAPH_T>
188  const mrpt::config::CConfigFileBase& source, const std::string& section)
189 {
190  MRPT_START
191 
192  conversion_params.sensorLabel = source.read_string(
193  section, "conversion_sensor_label", "KINECT_TO_2D_SCAN", false);
194  conversion_params.angle_sup = mrpt::DEG2RAD(
195  source.read_double(section, "conversion_angle_sup", 10, false));
196  conversion_params.angle_inf = mrpt::DEG2RAD(
197  source.read_double(section, "conversion_angle_inf", 10, false));
198  conversion_params.oversampling_ratio = source.read_double(
199  section, "conversion_oversampling_ratio", 1.1, false);
200  conversion_params.z_min = source.read_double(
201  section, "conversion_z_min", 0, false); // TODO - is this accurate?
202 
203  // load the icp parameters - from "ICP" section explicitly
204  icp.options.loadFromConfigFile(source, "ICP");
205 
206  has_read_config = true;
207 
208  MRPT_END
209 }
210 } // namespace mrpt::graphslam::deciders
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
#define MRPT_START
Definition: exceptions.h:241
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScan::Ptr &scan3D_in, mrpt::obs::CObservation2DRangeScan::Ptr *scan2D_out)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
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...
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...
double angle_sup
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
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.
mrpt::vision::TStereoCalibParams params
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
double z_min
(Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle points with Z coordinates wi...
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
typename GRAPH_T::constraint_t constraint_t
Definition: CRangeScanOps.h:73
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
constexpr double DEG2RAD(const double x)
Degrees to radians.
bool hasRangeImage
true means the field rangeImage contains valid data
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
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...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
The ICP algorithm return information.
Definition: CICP.h:190
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:440
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool&#39;s array as "true".
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93



Page generated by Doxygen 1.8.14 for MRPT 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020