Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 { namespace graphslam { namespace deciders {
14 
15 template<class GRAPH_T>
19  constraint_t* rel_edge,
20  const mrpt::poses::CPose2D* initial_pose_in/* = NULL */,
21  mrpt::slam::CICP::TReturnInfo* icp_info/* = NULL */) {
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  initial_pose = *initial_pose_in;
36  }
37 
38  mrpt::poses::CPosePDFPtr pdf = params.icp.Align(
39  &m1,
40  &m2,
41  initial_pose,
42  &running_time,
43  (void*)&info);
44 
45  // return the edge regardless of the goodness of the alignment
46  rel_edge->copyFrom(*pdf);
47 
48  // if given, fill the TReturnInfo Struct
49  if (icp_info) *icp_info = info;
50 
51  MRPT_END;
52 }
53 template<class GRAPH_T>
57  constraint_t* rel_edge,
58  const mrpt::poses::CPose2D* initial_pose_in /* =NULL */,
59  mrpt::slam::CICP::TReturnInfo* icp_info /* =NULL */) {
60  MRPT_START;
61 
63  mrpt::format("Laser scan doesn't contain valid range image"));
65  mrpt::format("Laser scan doesn't contain valid range image"));
66 
67  // TODO - have this as a class member
69  float running_time;
71 
72  m1.insertObservation(&from);
73  m2.insertObservation(&to);
74 
75  //this->decimatePointsMap(&m1, [> keep every = */ 40, /* low_lim = <] 5000);
76  //this->decimatePointsMap(&m2, [> keep every = */ 40, /* low_lim = <] 5000);
77 
78  // If given, use initial_pose_in as a first guess for the ICP
79  mrpt::poses::CPose3D initial_pose;
80  if (initial_pose_in) {
81  initial_pose = mrpt::poses::CPose3D(*initial_pose_in);
82  }
83 
84  mrpt::poses::CPose3DPDFPtr pdf = params.icp.Align3D(
85  &m1,
86  &m2,
87  initial_pose,
88  &running_time,
89  (void*)&info);
90 
91  // return the edge regardless of the goodness of the alignment
92  // copy from the 3D PDF
93  // NULLIFY contraint_t if Z displacement is high (normally this should be 0)
94  // since we are moving in 2D
95  if (fabs(pdf->getMeanVal().z()) < 0.1) {
96  rel_edge->copyFrom(*pdf);
97  }
98  else {
99  rel_edge = NULL;
100  }
101 
102  // if given, fill the TReturnInfo Struct
103  if (icp_info) *icp_info = info;
104 
105 
106  MRPT_END;
107 }
108 
109 template<class GRAPH_T>
112  size_t keep_point_every, /* = 4 */
113  size_t low_lim /* = 8000 */) {
114  MRPT_START;
115 
116  size_t map_size = m->size();
117 
118  if (low_lim) {
119  // check if current keep_point_every variable is too large
120  size_t conservative_keep_point_every = map_size / low_lim;
121  keep_point_every = 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  if (i % keep_point_every == 0) {
128  deletion_mask[i] = false;
129  }
130  }
131  m->applyDeletionMask(deletion_mask);
132 
133  //std::cout << "Map size: " << map_size << " => " << m->size() << std::endl;
134 
135  MRPT_END;
136 }
137 
138 template<class GRAPH_T>
140  mrpt::obs::CObservation3DRangeScanPtr& scan3D_in,
141  mrpt::obs::CObservation2DRangeScanPtr* scan2D_out /*= NULL*/) {
142  MRPT_START;
143 
144  bool success = false;
145  // if it doesn't exist, create it
146  if ( !(*scan2D_out) ) {
148  }
149 
150  if (scan3D_in->hasRangeImage) {
151  scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
152  success = true;
153  }
154  else {
155  std::cout << "No valid rangeImage found" << std::endl;
156  success = false;
157  }
158 
159  return success;
160  MRPT_END;
161 }
162 
163 // TParameter
164 // //////////////////////////////////
165 
166 template<class GRAPH_T>
168  has_read_config(false)
169 { }
170 
171 template<class GRAPH_T>
173 }
174 
175 template<class GRAPH_T>
177  mrpt::utils::CStream &out) const {
178  MRPT_START;
179 
180  out.printf("3D=>2D LaserScan Conversion Sensor label = %s\n",
181  conversion_params.sensorLabel.c_str());
182  out.printf("3D=>2D LaserScan Conversion angle sup = %.2f deg\n",
183  mrpt::utils::RAD2DEG(conversion_params.angle_sup));
184  out.printf("3D=>2D LaserScan Conversion angle inf = %.2f deg\n",
185  mrpt::utils::RAD2DEG(conversion_params.angle_inf));
186  out.printf("3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
187  conversion_params.oversampling_ratio);
188  out.printf("3D=>2D LaserScan Conversion Z minimum = %.2f\n",
189  conversion_params.z_min);
190 
191  icp.options.dumpToTextStream(out);
192 
193  MRPT_END;
194 }
195 template<class GRAPH_T>
198  const std::string& section) {
199  MRPT_START;
200 
201  conversion_params.sensorLabel = source.read_string(
202  section,
203  "conversion_sensor_label",
204  "KINECT_TO_2D_SCAN", false);
205  conversion_params.angle_sup = mrpt::utils::DEG2RAD(source.read_double(
206  section,
207  "conversion_angle_sup",
208  10, false));
209  conversion_params.angle_inf = mrpt::utils::DEG2RAD(source.read_double(
210  section,
211  "conversion_angle_inf",
212  10, false));
213  conversion_params.oversampling_ratio = source.read_double(
214  section,
215  "conversion_oversampling_ratio",
216  1.1, false);
217  conversion_params.z_min = source.read_double(
218  section,
219  "conversion_z_min",
220  0, false); // TODO - is this accurate?
221 
222 
223  // load the icp parameters - from "ICP" section explicitly
224  icp.options.loadFromConfigFile(source, "ICP");
225 
226  has_read_config = true;
227 
228  MRPT_END;
229 }
230 
231 } } } // end of namespaces
232 
233 #endif /* end of include guard: CRANGESCANOPS_IMPL_H */
#define min(a, b)
double DEG2RAD(const double x)
Degrees to radians.
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.
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 &section)
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...
Definition: CStream.h:38
GRAPH_T::constraint_t constraint_t
Handy typedefs.
Definition: CRangeScanOps.h:84
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
bool hasRangeImage
true means the field rangeImage contains valid data
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
static CObservation2DRangeScanPtr Create()
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...
#define MRPT_START
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:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
backing_store_ptr info
Definition: jmemsys.h:170
The ICP algorithm return information.
Definition: CICP.h:128
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
size_t size() const
Returns the number of stored points in the map.
#define ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
Definition: glext.h:3514
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.cpp:102
bool convert3DTo2DRangeScan(mrpt::obs::CObservation3DRangeScanPtr &scan3D_in, mrpt::obs::CObservation2DRangeScanPtr *scan2D_out=NULL)
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. ...
void applyDeletionMask(const std::vector< bool > &mask)
Remove from the map the points marked in a bool&#39;s array as "true".
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019