Main MRPT website > C++ reference for MRPT 1.9.9
CRangeScanOps_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #ifndef CRANGESCANOPS_IMPL_H
11 #define CRANGESCANOPS_IMPL_H
12 
13 namespace mrpt
14 {
15 namespace graphslam
16 {
17 namespace deciders
18 {
19 template <class GRAPH_T>
23  const mrpt::poses::CPose2D* initial_pose_in /* = nullptr */,
24  mrpt::slam::CICP::TReturnInfo* icp_info /* = nullptr */)
25 {
26  MRPT_START;
27 
29  float running_time;
31 
32  // have them initialized prior - and then just clear them
33  m1.insertObservation(&from);
34  m2.insertObservation(&to);
35 
36  // If given, use initial_pose_in as a first guess for the ICP
37  mrpt::poses::CPose2D initial_pose;
38  if (initial_pose_in)
39  {
40  initial_pose = *initial_pose_in;
41  }
42 
44  params.icp.Align(&m1, &m2, initial_pose, &running_time, (void*)&info);
45 
46  // return the edge regardless of the goodness of the alignment
47  rel_edge->copyFrom(*pdf);
48 
49  // if given, fill the TReturnInfo Struct
50  if (icp_info) *icp_info = info;
51 
52  MRPT_END;
53 }
54 template <class GRAPH_T>
58  const mrpt::poses::CPose2D* initial_pose_in /* =nullptr */,
59  mrpt::slam::CICP::TReturnInfo* icp_info /* =nullptr */)
60 {
61  MRPT_START;
62 
64  from.hasRangeImage,
65  mrpt::format("Laser scan doesn't contain valid range image"));
67  to.hasRangeImage,
68  mrpt::format("Laser scan doesn't contain valid range image"));
69 
70  // TODO - have this as a class member
72  float running_time;
74 
75  m1.insertObservation(&from);
76  m2.insertObservation(&to);
77 
78  // this->decimatePointsMap(&m1, [> keep every = */ 40, /* low_lim = <]
79  // 5000);
80  // this->decimatePointsMap(&m2, [> keep every = */ 40, /* low_lim = <]
81  // 5000);
82 
83  // If given, use initial_pose_in as a first guess for the ICP
84  mrpt::poses::CPose3D initial_pose;
85  if (initial_pose_in)
86  {
87  initial_pose = mrpt::poses::CPose3D(*initial_pose_in);
88  }
89 
91  params.icp.Align3D(&m1, &m2, initial_pose, &running_time, (void*)&info);
92 
93  // return the edge regardless of the goodness of the alignment
94  // copy from the 3D PDF
95  // NULLIFY contraint_t if Z displacement is high (normally this should be 0)
96  // since we are moving in 2D
97  if (fabs(pdf->getMeanVal().z()) < 0.1)
98  {
99  rel_edge->copyFrom(*pdf);
100  }
101  else
102  {
103  rel_edge = nullptr;
104  }
105 
106  // if given, fill the TReturnInfo Struct
107  if (icp_info) *icp_info = info;
108 
109  MRPT_END;
110 }
111 
112 template <class GRAPH_T>
114  mrpt::maps::CPointsMap* m, size_t keep_point_every, /* = 4 */
115  size_t low_lim /* = 8000 */)
116 {
117  MRPT_START;
118 
119  size_t map_size = m->size();
120 
121  if (low_lim)
122  {
123  // check if current keep_point_every variable is too large
124  size_t conservative_keep_point_every = map_size / low_lim;
125  keep_point_every =
126  std::min(keep_point_every, conservative_keep_point_every);
127  }
128 
129  // insert a false every "keep_point_every" points
130  std::vector<bool> deletion_mask(map_size, true);
131  for (size_t i = 0; i != map_size; ++i)
132  {
133  if (i % keep_point_every == 0)
134  {
135  deletion_mask[i] = false;
136  }
137  }
138  m->applyDeletionMask(deletion_mask);
139 
140  // std::cout << "Map size: " << map_size << " => " << m->size() <<
141  // std::endl;
142 
143  MRPT_END;
144 }
145 
146 template <class GRAPH_T>
149  mrpt::obs::CObservation2DRangeScan::Ptr* scan2D_out /*= NULL*/)
150 {
151  MRPT_START;
152 
153  bool success = false;
154  // if it doesn't exist, create it
155  if (!*scan2D_out)
156  {
157  *scan2D_out =
158  mrpt::make_aligned_shared<mrpt::obs::CObservation2DRangeScan>();
159  }
160 
161  if (scan3D_in->hasRangeImage)
162  {
163  scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
164  success = true;
165  }
166  else
167  {
168  std::cout << "No valid rangeImage found" << std::endl;
169  success = false;
170  }
171 
172  return success;
173  MRPT_END;
174 }
175 
176 // TParameter
177 // //////////////////////////////////
178 
179 template <class GRAPH_T>
181 {
182 }
183 
184 template <class GRAPH_T>
186 {
187 }
188 
189 template <class GRAPH_T>
191 {
192  MRPT_START;
193 
194  out << mrpt::format(
195  "3D=>2D LaserScan Conversion Sensor label = %s\n",
196  conversion_params.sensorLabel.c_str());
197  out << mrpt::format(
198  "3D=>2D LaserScan Conversion angle sup = %.2f deg\n",
199  mrpt::RAD2DEG(conversion_params.angle_sup));
200  out << mrpt::format(
201  "3D=>2D LaserScan Conversion angle inf = %.2f deg\n",
202  mrpt::RAD2DEG(conversion_params.angle_inf));
203  out << mrpt::format(
204  "3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
205  conversion_params.oversampling_ratio);
206  out << mrpt::format(
207  "3D=>2D LaserScan Conversion Z minimum = %.2f\n",
208  conversion_params.z_min);
209 
210  icp.options.dumpToTextStream(out);
211 
212  MRPT_END;
213 }
214 template <class GRAPH_T>
216  const mrpt::config::CConfigFileBase& source, const std::string& section)
217 {
218  MRPT_START;
219 
220  conversion_params.sensorLabel = source.read_string(
221  section, "conversion_sensor_label", "KINECT_TO_2D_SCAN", false);
222  conversion_params.angle_sup = mrpt::DEG2RAD(
223  source.read_double(section, "conversion_angle_sup", 10, false));
224  conversion_params.angle_inf = mrpt::DEG2RAD(
225  source.read_double(section, "conversion_angle_inf", 10, false));
226  conversion_params.oversampling_ratio = source.read_double(
227  section, "conversion_oversampling_ratio", 1.1, false);
228  conversion_params.z_min = source.read_double(
229  section, "conversion_z_min", 0, false); // TODO - is this accurate?
230 
231  // load the icp parameters - from "ICP" section explicitly
232  icp.options.loadFromConfigFile(source, "ICP");
233 
234  has_read_config = true;
235 
236  MRPT_END;
237 }
238 }
239 }
240 } // end of namespaces
241 
242 #endif /* end of include guard: CRANGESCANOPS_IMPL_H */
#define MRPT_START
Definition: exceptions.h:262
#define min(a, b)
double RAD2DEG(const double x)
Radians to degrees.
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.
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...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
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. ...
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.
Definition: CRangeScanOps.h:80
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:64
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.
Definition: format.cpp:16
bool hasRangeImage
true means the field rangeImage contains valid data
GLsizei const GLchar ** string
Definition: glext.h:4101
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:208
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...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScan::Ptr &scan3D_in, mrpt::obs::CObservation2DRangeScan::Ptr *scan2D_out=nullptr)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
#define MRPT_END
Definition: exceptions.h:266
The ICP algorithm return information.
Definition: CICP.h:192
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:408
GLenum const GLfloat * params
Definition: glext.h:3534
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:95
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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019