MRPT  1.9.9
CRandomFieldGridMap3D.cpp
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 #include "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/config.h>
15 #include <mrpt/system/CTicTac.h>
16 #include <fstream>
17 
18 using namespace mrpt;
19 using namespace mrpt::maps;
20 using namespace mrpt::system;
21 using namespace std;
22 
24 
25 /*---------------------------------------------------------------
26  Constructor
27  ---------------------------------------------------------------*/
29  double x_min, double x_max, double y_min, double y_max, double z_min,
30  double z_max, double voxel_size, bool call_initialize_now)
31  : CDynamicGrid3D<TRandomFieldVoxel>(
32  x_min, x_max, y_min, y_max, z_min, z_max, voxel_size /*xy*/,
33  voxel_size /*z*/),
35 {
36  if (call_initialize_now) this->internal_initialize();
37 }
38 
39 /** Changes the size of the grid, erasing previous contents */
41  const double x_min, const double x_max, const double y_min,
42  const double y_max, const double z_min, const double z_max,
43  const double resolution_xy, const double resolution_z,
44  const TRandomFieldVoxel* fill_value)
45 {
46  MRPT_START;
47 
49  x_min, x_max, y_min, y_max, z_min, z_max, resolution_xy, resolution_z,
50  fill_value);
51  this->internal_initialize();
52 
53  MRPT_END;
54 }
55 
57  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
58  double new_z_min, double new_z_max,
59  const TRandomFieldVoxel& defaultValueNewCells,
60  double additionalMarginMeters)
61 {
62  MRPT_START;
63 
65  new_x_min, new_x_max, new_y_min, new_y_max, new_z_min, new_z_max,
66  defaultValueNewCells, additionalMarginMeters);
67  this->internal_initialize(false);
68 
69  MRPT_END;
70 }
71 
73 {
75  internal_initialize();
76 }
77 
78 void CRandomFieldGridMap3D::internal_initialize(bool erase_prev_contents)
79 {
80  if (erase_prev_contents)
81  {
82  // Set the gridmap (m_map) to initial values:
83  TRandomFieldVoxel def(0, 0); // mean, std
84  fill(def);
85  }
86 
87  // Reset gmrf:
88  m_gmrf.setVerbosityLevel(this->getMinLoggingLevel());
89  if (erase_prev_contents)
90  {
91  m_gmrf.clear();
92  m_mrf_factors_activeObs.clear();
93  }
94  else
95  {
96  // Only remove priors, leave observations:
97  m_gmrf.clearAllConstraintsByType_Binary();
98  }
99  m_mrf_factors_priors.clear();
100 
101  // Initiating prior:
102  const size_t nodeCount = m_map.size();
103  ASSERT_EQUAL_(nodeCount, m_size_x * m_size_y * m_size_z);
104  ASSERT_EQUAL_(m_size_x_times_y, m_size_x * m_size_y);
105 
107  "[internal_initialize] Creating priors for GMRF with "
108  << nodeCount << " nodes." << std::endl);
109  CTicTac tictac;
110  tictac.Tic();
111 
112  m_mrf_factors_activeObs.resize(nodeCount); // Alloc space for obs
113  m_gmrf.initialize(nodeCount);
114 
115  ConnectivityDescriptor* custom_connectivity =
116  m_gmrf_connectivity
117  .get(); // Use a raw ptr to avoid the cost in the inner loops
118 
119  size_t cx = 0, cy = 0, cz = 0;
120  for (size_t j = 0; j < nodeCount; j++)
121  {
122  // add factors between this node and:
123  // 1) the right node: j +1
124  // 2) the back node: j+m_size_x
125  // 3) the top node: j+m_size_x*m_size*y
126  //-------------------------------------------------
127  const size_t c_idx_to_check[3] = {cx, cy, cz};
128  const size_t c_idx_to_check_limits[3] = {m_size_x - 1, m_size_y - 1,
129  m_size_z - 1};
130  const size_t c_neighbor_idx_incr[3] = {1, m_size_x, m_size_x_times_y};
131 
132  for (int dir = 0; dir < 3; dir++)
133  {
134  if (c_idx_to_check[dir] >= c_idx_to_check_limits[dir]) continue;
135 
136  const size_t i = j + c_neighbor_idx_incr[dir];
137  ASSERT_(i < nodeCount);
138 
139  double edge_lamdba = .0;
140  if (custom_connectivity != nullptr)
141  {
142  const bool is_connected =
143  custom_connectivity->getEdgeInformation(
144  this, cx, cy, cz, cx + (dir == 0 ? 1 : 0),
145  cy + (dir == 1 ? 1 : 0), cz + (dir == 2 ? 1 : 0),
146  edge_lamdba);
147  if (!is_connected) continue;
148  }
149  else
150  {
151  edge_lamdba = insertionOptions.GMRF_lambdaPrior;
152  }
153 
154  TPriorFactorGMRF new_prior(*this);
155  new_prior.node_id_i = i;
156  new_prior.node_id_j = j;
157  new_prior.Lambda = edge_lamdba;
158 
159  m_mrf_factors_priors.push_back(new_prior);
160  m_gmrf.addConstraint(
161  *m_mrf_factors_priors.rbegin()); // add to graph
162  }
163 
164  // Increment coordinates:
165  if (++cx >= m_size_x)
166  {
167  cx = 0;
168  if (++cy >= m_size_y)
169  {
170  cy = 0;
171  cz++;
172  }
173  }
174  } // end for "j"
175 
177  "[internal_initialize] Prior built in " << tictac.Tac() << " s\n"
178  << std::endl);
179 }
180 
181 /*---------------------------------------------------------------
182  TInsertionOptions
183  ---------------------------------------------------------------*/
185 
186  = default;
187 
189  std::ostream& out) const
190 {
191  out << mrpt::format(
192  "GMRF_lambdaPrior = %f\n", GMRF_lambdaPrior);
193  out << mrpt::format(
194  "GMRF_skip_variance = %s\n",
195  GMRF_skip_variance ? "true" : "false");
196 }
197 
199  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
200 {
201  GMRF_lambdaPrior = iniFile.read_double(
202  section.c_str(), "GMRF_lambdaPrior", GMRF_lambdaPrior);
203  GMRF_skip_variance = iniFile.read_bool(
204  section.c_str(), "GMRF_skip_variance", GMRF_skip_variance);
205 }
206 
208  const std::string& filName_mean, const std::string& filName_stddev) const
209 {
210  std::ofstream f_mean, f_stddev;
211 
212  f_mean.open(filName_mean);
213  if (!f_mean.is_open())
214  {
215  return false;
216  }
217  else
218  {
219  f_mean << "x coord, y coord, z coord, scalar\n";
220  }
221 
222  if (!filName_stddev.empty())
223  {
224  f_stddev.open(filName_stddev);
225  if (!f_stddev.is_open())
226  {
227  return false;
228  }
229  else
230  {
231  f_mean << "x coord, y coord, z coord, scalar\n";
232  }
233  }
234 
235  const size_t nodeCount = m_map.size();
236  size_t cx = 0, cy = 0, cz = 0;
237  for (size_t j = 0; j < nodeCount; j++)
238  {
239  const double x = idx2x(cx), y = idx2y(cy), z = idx2z(cz);
240  const double mean_val = m_map[j].mean_value;
241  const double stddev_val = m_map[j].stddev_value;
242 
243  f_mean << mrpt::format("%f, %f, %f, %e\n", x, y, z, mean_val);
244 
245  if (f_stddev.is_open())
246  f_stddev << mrpt::format("%f, %f, %f, %e\n", x, y, z, stddev_val);
247 
248  // Increment coordinates:
249  if (++cx >= m_size_x)
250  {
251  cx = 0;
252  if (++cy >= m_size_y)
253  {
254  cy = 0;
255  cz++;
256  }
257  }
258  } // end for "j"
259 
260  return true;
261 }
262 
264 {
265  ASSERTMSG_(
266  !m_mrf_factors_activeObs.empty(),
267  "Cannot update a map with no observations!");
268 
269  mrpt::math::CVectorDouble x_incr, x_var;
270  m_gmrf.updateEstimation(
271  x_incr, insertionOptions.GMRF_skip_variance ? nullptr : &x_var);
272 
273  ASSERT_(size_t(m_map.size()) == size_t(x_incr.size()));
274  ASSERT_(
275  insertionOptions.GMRF_skip_variance ||
276  size_t(m_map.size()) == size_t(x_var.size()));
277 
278  // Update Mean-Variance in the base grid class
279  for (size_t j = 0; j < m_map.size(); j++)
280  {
281  m_map[j].mean_value += x_incr[j];
282  m_map[j].stddev_value =
283  insertionOptions.GMRF_skip_variance ? .0 : std::sqrt(x_var[j]);
284  }
285 }
286 
288  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor)
289 {
290  m_gmrf_connectivity = new_connectivity_descriptor;
291 }
292 
294  /** [in] The value observed in the (x,y,z) position */
295  const double sensorReading,
296  /** [in] The variance of the sensor observation */
297  const double sensorVariance,
298  /** [in] The (x,y,z) location */
299  const mrpt::math::TPoint3D& point,
300  /** [in] Voxel interpolation method: how many voxels will be affected by the
301  reading */
302  const TVoxelInterpolationMethod method,
303  /** [in] Run a global map update after inserting this observatin
304  (algorithm-dependant) */
305  const bool update_map)
306 {
307  MRPT_START;
308 
309  ASSERT_ABOVE_(sensorVariance, .0);
310  ASSERTMSG_(
311  m_mrf_factors_activeObs.size() == m_map.size(),
312  "Trying to insert observation in uninitialized map (!)");
313 
314  const size_t cell_idx =
315  cellAbsIndexFromCXCYCZ(x2idx(point.x), y2idx(point.y), z2idx(point.z));
316  if (cell_idx == INVALID_VOXEL_IDX) return false;
317 
318  TObservationGMRF new_obs(*this);
319  new_obs.node_id = cell_idx;
320  new_obs.obsValue = sensorReading;
321  new_obs.Lambda = 1.0 / sensorVariance;
322 
323  m_mrf_factors_activeObs[cell_idx].push_back(new_obs);
324  m_gmrf.addConstraint(
325  *m_mrf_factors_activeObs[cell_idx].rbegin()); // add to graph
326 
327  if (update_map) this->updateMapEstimation();
328 
329  return true;
330 
331  MRPT_END;
332 }
333 
334 uint8_t CRandomFieldGridMap3D::serializeGetVersion() const { return 0; }
337 {
338  dyngridcommon_writeToStream(out);
339 
340  // To assure compatibility: The size of each cell:
341  auto n = static_cast<uint32_t>(sizeof(TRandomFieldVoxel));
342  out << n;
343 
344  // Save the map contents:
345  n = static_cast<uint32_t>(m_map.size());
346  out << n;
347 
348 // Save the "m_map": This requires special handling for big endian systems:
349 #if MRPT_IS_BIG_ENDIAN
350  for (uint32_t i = 0; i < n; i++)
351  out << m_map[i].mean_value << m_map[i].stddev_value;
352 #else
353  // Little endian: just write all at once:
354  out.WriteBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
355 #endif
356 
357  out << insertionOptions.GMRF_lambdaPrior
358  << insertionOptions.GMRF_skip_variance;
359 }
360 
362  mrpt::serialization::CArchive& in, uint8_t version)
363 {
364  switch (version)
365  {
366  case 0:
367  {
368  dyngridcommon_readFromStream(in);
369 
370  // To assure compatibility: The size of each cell:
371  uint32_t n;
372  in >> n;
373 
374  ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldVoxel)));
375  // Load the map contents:
376  in >> n;
377  m_map.resize(n);
378 
379 // Read the note in writeToStream()
380 #if MRPT_IS_BIG_ENDIAN
381  for (uint32_t i = 0; i < n; i++)
382  in >> m_map[i].mean_value >> m_map[i].stddev_value;
383 #else
384  // Little endian: just read all at once:
385  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
386 #endif
387  in >> insertionOptions.GMRF_lambdaPrior >>
388  insertionOptions.GMRF_skip_variance;
389  }
390  break;
391  default:
393  };
394 }
395 
396 // ============ TObservationGMRF ===========
398 {
399  return m_parent->m_map[this->node_id].mean_value - this->obsValue;
400 }
402 {
403  return this->Lambda;
404 }
406 {
407  dr_dx = 1.0;
408 }
409 // ============ TPriorFactorGMRF ===========
411 {
412  return m_parent->m_map[this->node_id_i].mean_value -
413  m_parent->m_map[this->node_id_j].mean_value;
414 }
416 {
417  return this->Lambda;
418 }
420  double& dr_dx_i, double& dr_dx_j) const
421 {
422  dr_dx_i = +1.0;
423  dr_dx_j = -1.0;
424 }
void dumpToTextStream(std::ostream &out) const override
See utils::CLoadableOptions.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
virtual void clear()
Erase the contents of all the cells, setting them to their default values (default ctor)...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
The contents of each voxel in a CRandomFieldGridMap3D map.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
virtual bool getEdgeInformation(const CRandomFieldGridMap3D *parent, size_t icx, size_t icy, size_t icz, size_t jcx, size_t jcy, size_t jcz, double &out_edge_information)=0
Implement the check of whether node i=(icx,icy,icz) is connected with node j=(jcx,jcy,jcy).
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
GLenum GLsizei n
Definition: glext.h:5136
void internal_initialize(bool erase_prev_contents=true)
Internal: called called after each change of resolution, size, etc.
A high-performance stopwatch, with typical resolution of nanoseconds.
double getInformation() const override
Return the inverse of the variance of this constraint.
double evaluateResidual() const override
Return the residual/error of this observation.
STL namespace.
double getInformation() const override
Return the inverse of the variance of this constraint.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void setVoxelsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between voxels.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double evaluateResidual() const override
Return the residual/error of this observation.
This class allows loading and storing values and vectors of different types from a configuration text...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
double Lambda
"Information" of the observation (=inverse of the variance)
void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, double new_z_min, double new_z_max, const TRandomFieldVoxel &defaultValueNewvoxels, double additionalMarginMeters=2.0) override
Changes the size of the grid, maintaining previous contents.
Versatile class for consistent logging and management of output messages.
double Lambda
"Information" of the observation (=inverse of the variance)
void clear() override
Erases all added observations and start again with an empty gridmap.
string iniFile(myDataDir+string("benchmark-options.ini"))
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
GLsizei const GLchar ** string
Definition: glext.h:4116
bool saveAsCSV(const std::string &filName_mean, const std::string &filName_stddev=std::string()) const
Save the current estimated mean values to a CSV file (compatible with Paraview) with fields x y z mea...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
CRandomFieldGridMap3D represents a 3D regular grid where each voxel is associated one real-valued pro...
mrpt::vision::TStereoCalibResults out
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
See utils::CLoadableOptions.
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double z_min, const double z_max, const double resolution_xy, const double resolution_z_=-1.0, const TRandomFieldVoxel *fill_value=nullptr)
Changes the size of the grid, ERASING all previous contents.
GLenum GLint GLint y
Definition: glext.h:3542
bool insertIndividualReading(const double sensorReading, const double sensorVariance, const mrpt::math::TPoint3D &point, const TVoxelInterpolationMethod method, const bool update_map)
Direct update of the map with a reading in a given position of the map.
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
GLenum GLint x
Definition: glext.h:3542
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
Lightweight 3D point.
Definition: TPoint3D.h:90
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
images resize(NUM_IMGS)
void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double z_min, const double z_max, const double resolution_xy, const double resolution_z=-1.0, const TRandomFieldVoxel *fill_value=nullptr) override
Changes the size of the grid, erasing previous contents.If resolution_z<0, the same resolution will b...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Base class for user-supplied objects capable of describing voxels connectivity, used to build prior f...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019