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 #if MRPT_HAS_VTK
19 #include <vtkCellArray.h>
20 #include <vtkDoubleArray.h>
21 #include <vtkPointData.h>
22 #include <vtkPoints.h>
23 #include <vtkSmartPointer.h>
24 #include <vtkStructuredGrid.h>
25 #include <vtkVersion.h>
26 #include <vtkXMLStructuredGridWriter.h>
27 #endif
28 
29 using namespace mrpt;
30 using namespace mrpt::maps;
31 using namespace mrpt::system;
32 using namespace std;
33 
35 
36 /*---------------------------------------------------------------
37  Constructor
38  ---------------------------------------------------------------*/
40  double x_min, double x_max, double y_min, double y_max, double z_min,
41  double z_max, double voxel_size, bool call_initialize_now)
42  : CDynamicGrid3D<TRandomFieldVoxel>(
43  x_min, x_max, y_min, y_max, z_min, z_max, voxel_size /*xy*/,
44  voxel_size /*z*/),
46 {
47  if (call_initialize_now) this->internal_initialize();
48 }
49 
50 /** Changes the size of the grid, erasing previous contents */
52  const double x_min, const double x_max, const double y_min,
53  const double y_max, const double z_min, const double z_max,
54  const double resolution_xy, const double resolution_z,
55  const TRandomFieldVoxel* fill_value)
56 {
57  MRPT_START;
58 
60  x_min, x_max, y_min, y_max, z_min, z_max, resolution_xy, resolution_z,
61  fill_value);
62  this->internal_initialize();
63 
64  MRPT_END;
65 }
66 
68  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
69  double new_z_min, double new_z_max,
70  const TRandomFieldVoxel& defaultValueNewCells,
71  double additionalMarginMeters)
72 {
73  MRPT_START;
74 
76  new_x_min, new_x_max, new_y_min, new_y_max, new_z_min, new_z_max,
77  defaultValueNewCells, additionalMarginMeters);
78  this->internal_initialize(false);
79 
80  MRPT_END;
81 }
82 
84 {
86  internal_initialize();
87 }
88 
89 void CRandomFieldGridMap3D::internal_initialize(bool erase_prev_contents)
90 {
91  if (erase_prev_contents)
92  {
93  // Set the gridmap (m_map) to initial values:
94  TRandomFieldVoxel def(0, 0); // mean, std
95  fill(def);
96  }
97 
98  // Reset gmrf:
99  m_gmrf.setVerbosityLevel(this->getMinLoggingLevel());
100  if (erase_prev_contents)
101  {
102  m_gmrf.clear();
103  m_mrf_factors_activeObs.clear();
104  }
105  else
106  {
107  // Only remove priors, leave observations:
108  m_gmrf.clearAllConstraintsByType_Binary();
109  }
110  m_mrf_factors_priors.clear();
111 
112  // Initiating prior:
113  const size_t nodeCount = m_map.size();
114  ASSERT_EQUAL_(nodeCount, m_size_x * m_size_y * m_size_z);
115  ASSERT_EQUAL_(m_size_x_times_y, m_size_x * m_size_y);
116 
118  "[internal_initialize] Creating priors for GMRF with "
119  << nodeCount << " nodes." << std::endl);
120  CTicTac tictac;
121  tictac.Tic();
122 
123  m_mrf_factors_activeObs.resize(nodeCount); // Alloc space for obs
124  m_gmrf.initialize(nodeCount);
125 
126  ConnectivityDescriptor* custom_connectivity =
127  m_gmrf_connectivity
128  .get(); // Use a raw ptr to avoid the cost in the inner loops
129 
130  size_t cx = 0, cy = 0, cz = 0;
131  for (size_t j = 0; j < nodeCount; j++)
132  {
133  // add factors between this node and:
134  // 1) the right node: j +1
135  // 2) the back node: j+m_size_x
136  // 3) the top node: j+m_size_x*m_size*y
137  //-------------------------------------------------
138  const size_t c_idx_to_check[3] = {cx, cy, cz};
139  const size_t c_idx_to_check_limits[3] = {m_size_x - 1, m_size_y - 1,
140  m_size_z - 1};
141  const size_t c_neighbor_idx_incr[3] = {1, m_size_x, m_size_x_times_y};
142 
143  for (int dir = 0; dir < 3; dir++)
144  {
145  if (c_idx_to_check[dir] >= c_idx_to_check_limits[dir]) continue;
146 
147  const size_t i = j + c_neighbor_idx_incr[dir];
148  ASSERT_(i < nodeCount);
149 
150  double edge_lamdba = .0;
151  if (custom_connectivity != nullptr)
152  {
153  const bool is_connected =
154  custom_connectivity->getEdgeInformation(
155  this, cx, cy, cz, cx + (dir == 0 ? 1 : 0),
156  cy + (dir == 1 ? 1 : 0), cz + (dir == 2 ? 1 : 0),
157  edge_lamdba);
158  if (!is_connected) continue;
159  }
160  else
161  {
162  edge_lamdba = insertionOptions.GMRF_lambdaPrior;
163  }
164 
165  TPriorFactorGMRF new_prior(*this);
166  new_prior.node_id_i = i;
167  new_prior.node_id_j = j;
168  new_prior.Lambda = edge_lamdba;
169 
170  m_mrf_factors_priors.push_back(new_prior);
171  m_gmrf.addConstraint(
172  *m_mrf_factors_priors.rbegin()); // add to graph
173  }
174 
175  // Increment coordinates:
176  if (++cx >= m_size_x)
177  {
178  cx = 0;
179  if (++cy >= m_size_y)
180  {
181  cy = 0;
182  cz++;
183  }
184  }
185  } // end for "j"
186 
188  "[internal_initialize] Prior built in " << tictac.Tac() << " s\n"
189  << std::endl);
190 }
191 
192 /*---------------------------------------------------------------
193  TInsertionOptions
194  ---------------------------------------------------------------*/
196 
197  = default;
198 
200  std::ostream& out) const
201 {
202  out << mrpt::format(
203  "GMRF_lambdaPrior = %f\n", GMRF_lambdaPrior);
204  out << mrpt::format(
205  "GMRF_skip_variance = %s\n",
206  GMRF_skip_variance ? "true" : "false");
207 }
208 
210  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
211 {
212  GMRF_lambdaPrior = iniFile.read_double(
213  section.c_str(), "GMRF_lambdaPrior", GMRF_lambdaPrior);
214  GMRF_skip_variance = iniFile.read_bool(
215  section.c_str(), "GMRF_skip_variance", GMRF_skip_variance);
216 }
217 
218 /** Save the current estimated grid to a VTK file (.vts) as a "structured grid".
219  * \sa saveAsCSV */
221  const std::string& fil) const
222 {
223  MRPT_START;
224 #if MRPT_HAS_VTK
225 
226  vtkStructuredGrid* vtkGrid = vtkStructuredGrid::New();
227  this->getAsVtkStructuredGrid(vtkGrid);
228 
229  // Write file
230  vtkSmartPointer<vtkXMLStructuredGridWriter> writer =
231  vtkSmartPointer<vtkXMLStructuredGridWriter>::New();
232  writer->SetFileName(fil.c_str());
233 
234 #if VTK_MAJOR_VERSION <= 5
235  writer->SetInput(vtkGrid);
236 #else
237  writer->SetInputData(vtkGrid);
238 #endif
239 
240  int ret = writer->Write();
241 
242  vtkGrid->Delete();
243 
244  return ret == 0;
245 #else
246  THROW_EXCEPTION("This method requires building MRPT against VTK!");
247 #endif
248  MRPT_END
249 }
250 
252  const std::string& filName_mean, const std::string& filName_stddev) const
253 {
254  std::ofstream f_mean, f_stddev;
255 
256  f_mean.open(filName_mean);
257  if (!f_mean.is_open())
258  {
259  return false;
260  }
261  else
262  {
263  f_mean << "x coord, y coord, z coord, scalar\n";
264  }
265 
266  if (!filName_stddev.empty())
267  {
268  f_stddev.open(filName_stddev);
269  if (!f_stddev.is_open())
270  {
271  return false;
272  }
273  else
274  {
275  f_mean << "x coord, y coord, z coord, scalar\n";
276  }
277  }
278 
279  const size_t nodeCount = m_map.size();
280  size_t cx = 0, cy = 0, cz = 0;
281  for (size_t j = 0; j < nodeCount; j++)
282  {
283  const double x = idx2x(cx), y = idx2y(cy), z = idx2z(cz);
284  const double mean_val = m_map[j].mean_value;
285  const double stddev_val = m_map[j].stddev_value;
286 
287  f_mean << mrpt::format("%f, %f, %f, %e\n", x, y, z, mean_val);
288 
289  if (f_stddev.is_open())
290  f_stddev << mrpt::format("%f, %f, %f, %e\n", x, y, z, stddev_val);
291 
292  // Increment coordinates:
293  if (++cx >= m_size_x)
294  {
295  cx = 0;
296  if (++cy >= m_size_y)
297  {
298  cy = 0;
299  cz++;
300  }
301  }
302  } // end for "j"
303 
304  return true;
305 }
306 
308 {
309  ASSERTMSG_(
310  !m_mrf_factors_activeObs.empty(),
311  "Cannot update a map with no observations!");
312 
313  mrpt::math::CVectorDouble x_incr, x_var;
314  m_gmrf.updateEstimation(
315  x_incr, insertionOptions.GMRF_skip_variance ? nullptr : &x_var);
316 
317  ASSERT_(size_t(m_map.size()) == size_t(x_incr.size()));
318  ASSERT_(
319  insertionOptions.GMRF_skip_variance ||
320  size_t(m_map.size()) == size_t(x_var.size()));
321 
322  // Update Mean-Variance in the base grid class
323  for (size_t j = 0; j < m_map.size(); j++)
324  {
325  m_map[j].mean_value += x_incr[j];
326  m_map[j].stddev_value =
327  insertionOptions.GMRF_skip_variance ? .0 : std::sqrt(x_var[j]);
328  }
329 }
330 
332  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor)
333 {
334  m_gmrf_connectivity = new_connectivity_descriptor;
335 }
336 
338  /** [in] The value observed in the (x,y,z) position */
339  const double sensorReading,
340  /** [in] The variance of the sensor observation */
341  const double sensorVariance,
342  /** [in] The (x,y,z) location */
343  const mrpt::math::TPoint3D& point,
344  /** [in] Voxel interpolation method: how many voxels will be affected by the
345  reading */
346  const TVoxelInterpolationMethod method,
347  /** [in] Run a global map update after inserting this observatin
348  (algorithm-dependant) */
349  const bool update_map)
350 {
351  MRPT_START;
352 
353  ASSERT_ABOVE_(sensorVariance, .0);
354  ASSERTMSG_(
355  m_mrf_factors_activeObs.size() == m_map.size(),
356  "Trying to insert observation in uninitialized map (!)");
357 
358  const size_t cell_idx =
359  cellAbsIndexFromCXCYCZ(x2idx(point.x), y2idx(point.y), z2idx(point.z));
360  if (cell_idx == INVALID_VOXEL_IDX) return false;
361 
362  TObservationGMRF new_obs(*this);
363  new_obs.node_id = cell_idx;
364  new_obs.obsValue = sensorReading;
365  new_obs.Lambda = 1.0 / sensorVariance;
366 
367  m_mrf_factors_activeObs[cell_idx].push_back(new_obs);
368  m_gmrf.addConstraint(
369  *m_mrf_factors_activeObs[cell_idx].rbegin()); // add to graph
370 
371  if (update_map) this->updateMapEstimation();
372 
373  return true;
374 
375  MRPT_END;
376 }
377 
381 {
382  dyngridcommon_writeToStream(out);
383 
384  // To assure compatibility: The size of each cell:
385  auto n = static_cast<uint32_t>(sizeof(TRandomFieldVoxel));
386  out << n;
387 
388  // Save the map contents:
389  n = static_cast<uint32_t>(m_map.size());
390  out << n;
391 
392 // Save the "m_map": This requires special handling for big endian systems:
393 #if MRPT_IS_BIG_ENDIAN
394  for (uint32_t i = 0; i < n; i++)
395  out << m_map[i].mean_value << m_map[i].stddev_value;
396 #else
397  // Little endian: just write all at once:
398  out.WriteBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
399 #endif
400 
401  out << insertionOptions.GMRF_lambdaPrior
402  << insertionOptions.GMRF_skip_variance;
403 }
404 
407 {
408  switch (version)
409  {
410  case 0:
411  {
412  dyngridcommon_readFromStream(in);
413 
414  // To assure compatibility: The size of each cell:
415  uint32_t n;
416  in >> n;
417 
418  ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldVoxel)));
419  // Load the map contents:
420  in >> n;
421  m_map.resize(n);
422 
423 // Read the note in writeToStream()
424 #if MRPT_IS_BIG_ENDIAN
425  for (uint32_t i = 0; i < n; i++)
426  in >> m_map[i].mean_value >> m_map[i].stddev_value;
427 #else
428  // Little endian: just read all at once:
429  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
430 #endif
431  in >> insertionOptions.GMRF_lambdaPrior >>
432  insertionOptions.GMRF_skip_variance;
433  }
434  break;
435  default:
437  };
438 }
439 
441  vtkStructuredGrid* output, const std::string& label_mean,
442  const std::string& label_stddev) const
443 {
444  MRPT_START;
445 #if MRPT_HAS_VTK
446 
447  const size_t nx = this->getSizeX(), ny = this->getSizeY(),
448  nz = this->getSizeZ();
449 
450  const int num_values = nx * ny * nz;
451 
452  vtkPoints* newPoints = vtkPoints::New();
453 
454  vtkDoubleArray* newData = vtkDoubleArray::New();
455  newData->SetNumberOfComponents(3);
456  newData->SetNumberOfTuples(num_values);
457 
458  vtkDoubleArray* mean_arr = vtkDoubleArray::New();
459  mean_arr->SetNumberOfComponents(1);
460  mean_arr->SetNumberOfTuples(num_values);
461 
462  vtkDoubleArray* std_arr = vtkDoubleArray::New();
463  std_arr->SetNumberOfComponents(1);
464  std_arr->SetNumberOfTuples(num_values);
465 
466  vtkIdType numtuples = newData->GetNumberOfTuples();
467 
468  {
469  size_t cx = 0, cy = 0, cz = 0;
470  for (vtkIdType cc = 0; cc < numtuples; cc++)
471  {
472  const double x = idx2x(cx), y = idx2y(cy), z = idx2z(cz);
473 
474  newData->SetComponent(cc, 0, x);
475  newData->SetComponent(cc, 1, y);
476  newData->SetComponent(cc, 2, z);
477 
478  mean_arr->SetComponent(cc, 0, m_map[cc].mean_value);
479  std_arr->SetComponent(cc, 0, m_map[cc].stddev_value);
480 
481  // Increment coordinates:
482  if (++cx >= m_size_x)
483  {
484  cx = 0;
485  if (++cy >= m_size_y)
486  {
487  cy = 0;
488  cz++;
489  }
490  }
491  }
492  ASSERT_(size_t(m_map.size()) == size_t(numtuples));
493  }
494 
495  newPoints->SetData(newData);
496  newData->Delete();
497 
498  output->SetExtent(0, nx - 1, 0, ny - 1, 0, nz - 1);
499  output->SetPoints(newPoints);
500  newPoints->Delete();
501 
502  mean_arr->SetName(label_mean.c_str());
503  std_arr->SetName(label_stddev.c_str());
504  output->GetPointData()->AddArray(mean_arr);
505  output->GetPointData()->AddArray(std_arr);
506 
507  mean_arr->Delete();
508  std_arr->Delete();
509 
510 #else
511  THROW_EXCEPTION("This method requires building MRPT against VTK!");
512 #endif // VTK
513  MRPT_END;
514 }
515 
516 // ============ TObservationGMRF ===========
518 {
519  return m_parent->m_map[this->node_id].mean_value - this->obsValue;
520 }
522 {
523  return this->Lambda;
524 }
526 {
527  dr_dx = 1.0;
528 }
529 // ============ TPriorFactorGMRF ===========
531 {
532  return m_parent->m_map[this->node_id_i].mean_value -
533  m_parent->m_map[this->node_id_j].mean_value;
534 }
536 {
537  return this->Lambda;
538 }
540  double& dr_dx_i, double& dr_dx_j) const
541 {
542  dr_dx_i = +1.0;
543  dr_dx_j = -1.0;
544 }
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)...
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CArchive.cpp:49
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.
GLfloat GLfloat nz
Definition: glext.h:6385
unsigned char uint8_t
Definition: rptypes.h:44
#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
virtual 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 &defaultValueNewCells, double additionalMarginMeters=2)
Changes the size of the grid, maintaining previous contents.
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...
auto dir
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:53
CRandomFieldGridMap3D represents a 3D regular grid where each voxel is associated one real-valued pro...
GLfloat GLfloat GLfloat GLfloat nx
Definition: glext.h:6389
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
void getAsVtkStructuredGrid(vtkStructuredGrid *output, const std::string &label_mean=std::string("mean"), const std::string &label_stddev=std::string("stddev")) const
Returns the 3D grid contents as an VTK grid.
bool saveAsVtkStructuredGrid(const std::string &fil) const
Save the current estimated grid to a VTK file (.vts) as a "structured grid".
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
GLfloat ny
Definition: glext.h:6385
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.
unsigned __int32 uint32_t
Definition: rptypes.h:50
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: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019