MRPT  1.9.9
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-2019, 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 {
22  MRPT_START;
23 
25  float running_time;
27 
28  // have them initialized prior - and then just clear them
29  m1.insertObservation(&from);
30  m2.insertObservation(&to);
31 
32  // If given, use initial_pose_in as a first guess for the ICP
33  mrpt::poses::CPose2D initial_pose;
34  if (initial_pose_in)
35  {
36  initial_pose = *initial_pose_in;
37  }
38 
40  params.icp.Align(&m1, &m2, initial_pose, &running_time, (void*)&info);
41 
42  // return the edge regardless of the goodness of the alignment
43  rel_edge->copyFrom(*pdf);
44 
45  // if given, fill the TReturnInfo Struct
46  if (icp_info) *icp_info = info;
47 
48  MRPT_END;
49 }
50 template <class GRAPH_T>
54  const mrpt::poses::CPose2D* initial_pose_in,
56 {
57  MRPT_START;
58 
60  from.hasRangeImage,
61  mrpt::format("Laser scan doesn't contain valid range image"));
63  to.hasRangeImage,
64  mrpt::format("Laser scan doesn't contain valid range image"));
65 
66  // TODO - have this as a class member
68  float running_time;
70 
71  m1.insertObservation(&from);
72  m2.insertObservation(&to);
73 
74  // this->decimatePointsMap(&m1, [> keep every = */ 40, /* low_lim = <]
75  // 5000);
76  // this->decimatePointsMap(&m2, [> keep every = */ 40, /* low_lim = <]
77  // 5000);
78 
79  // If given, use initial_pose_in as a first guess for the ICP
80  mrpt::poses::CPose3D initial_pose;
81  if (initial_pose_in)
82  {
83  initial_pose = mrpt::poses::CPose3D(*initial_pose_in);
84  }
85 
87  params.icp.Align3D(&m1, &m2, initial_pose, &running_time, (void*)&info);
88 
89  // return the edge regardless of the goodness of the alignment
90  // copy from the 3D PDF
91  // NULLIFY contraint_t if Z displacement is high (normally this should be 0)
92  // since we are moving in 2D
93  if (fabs(pdf->getMeanVal().z()) < 0.1)
94  {
95  rel_edge->copyFrom(*pdf);
96  }
97  else
98  {
99  rel_edge = nullptr;
100  }
101 
102  // if given, fill the TReturnInfo Struct
103  if (icp_info) *icp_info = info;
104 
105  MRPT_END;
106 }
107 
108 template <class GRAPH_T>
110  mrpt::maps::CPointsMap* m, size_t keep_point_every, size_t low_lim)
111 {
112  MRPT_START;
113 
114  size_t map_size = m->size();
115 
116  if (low_lim)
117  {
118  // check if current keep_point_every variable is too large
119  size_t conservative_keep_point_every = map_size / low_lim;
120  keep_point_every =
121  std::min(keep_point_every, conservative_keep_point_every);
122  }
123 
124  // insert a false every "keep_point_every" points
125  std::vector<bool> deletion_mask(map_size, true);
126  for (size_t i = 0; i != map_size; ++i)
127  {
128  if (i % keep_point_every == 0)
129  {
130  deletion_mask[i] = false;
131  }
132  }
133  m->applyDeletionMask(deletion_mask);
134 
135  // std::cout << "Map size: " << map_size << " => " << m->size() <<
136  // std::endl;
137 
138  MRPT_END;
139 }
140 
141 template <class GRAPH_T>
145 {
146  MRPT_START;
147 
148  bool success = false;
149  // if it doesn't exist, create it
150  if (!*scan2D_out)
151  {
152  *scan2D_out =
153  mrpt::make_aligned_shared<mrpt::obs::CObservation2DRangeScan>();
154  }
155 
156  if (scan3D_in->hasRangeImage)
157  {
158  scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
159  success = true;
160  }
161  else
162  {
163  std::cout << "No valid rangeImage found" << std::endl;
164  success = false;
165  }
166 
167  return success;
168  MRPT_END;
169 }
170 
171 template <class GRAPH_T>
173 {
174  MRPT_START;
175 
176  out << mrpt::format(
177  "3D=>2D LaserScan Conversion Sensor label = %s\n",
179  out << mrpt::format(
180  "3D=>2D LaserScan Conversion angle sup = %.2f deg\n",
182  out << mrpt::format(
183  "3D=>2D LaserScan Conversion angle inf = %.2f deg\n",
185  out << mrpt::format(
186  "3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
188  out << mrpt::format(
189  "3D=>2D LaserScan Conversion Z minimum = %.2f\n",
191 
193 
194  MRPT_END;
195 }
196 template <class GRAPH_T>
198  const mrpt::config::CConfigFileBase& source, const std::string& section)
199 {
200  MRPT_START;
201 
202  conversion_params.sensorLabel = source.read_string(
203  section, "conversion_sensor_label", "KINECT_TO_2D_SCAN", false);
204  conversion_params.angle_sup = mrpt::DEG2RAD(
205  source.read_double(section, "conversion_angle_sup", 10, false));
206  conversion_params.angle_inf = mrpt::DEG2RAD(
207  source.read_double(section, "conversion_angle_inf", 10, false));
208  conversion_params.oversampling_ratio = source.read_double(
209  section, "conversion_oversampling_ratio", 1.1, false);
210  conversion_params.z_min = source.read_double(
211  section, "conversion_z_min", 0, false); // TODO - is this accurate?
212 
213  // load the icp parameters - from "ICP" section explicitly
214  icp.options.loadFromConfigFile(source, "ICP");
215 
216  has_read_config = true;
217 
218  MRPT_END;
219 }
220 } // 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:282
#define min(a, b)
double RAD2DEG(const double x)
Radians to degrees.
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:94
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScan::Ptr &scan3D_in, mrpt::obs::CObservation2DRangeScan::Ptr *scan2D_out)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
double DEG2RAD(const double x)
Degrees to radians.
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...
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. ...
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...
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.
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.
GLsizei const GLchar ** string
Definition: glext.h:4116
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:232
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:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:286
The ICP algorithm return information.
Definition: CICP.h:190
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:422
GLenum const GLfloat * params
Definition: glext.h:3538
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool&#39;s array as "true".



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019