MRPT  2.0.1
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-2020, 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>
16 #include <mrpt/system/CTicTac.h>
17 #include <fstream>
18 
19 using namespace mrpt;
20 using namespace mrpt::maps;
21 using namespace mrpt::system;
22 using namespace std;
23 
25 
26 /*---------------------------------------------------------------
27  Constructor
28  ---------------------------------------------------------------*/
30  double x_min, double x_max, double y_min, double y_max, double z_min,
31  double z_max, double voxel_size, bool call_initialize_now)
32  : CDynamicGrid3D<TRandomFieldVoxel>(
33  x_min, x_max, y_min, y_max, z_min, z_max, voxel_size /*xy*/,
34  voxel_size /*z*/),
36 {
37  if (call_initialize_now) this->internal_initialize();
38 }
39 
40 /** Changes the size of the grid, erasing previous contents */
42  const double x_min, const double x_max, const double y_min,
43  const double y_max, const double z_min, const double z_max,
44  const double resolution_xy, const double resolution_z,
45  const TRandomFieldVoxel* fill_value)
46 {
48 
50  x_min, x_max, y_min, y_max, z_min, z_max, resolution_xy, resolution_z,
51  fill_value);
52  this->internal_initialize();
53 
54  MRPT_END
55 }
56 
58  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
59  double new_z_min, double new_z_max,
60  const TRandomFieldVoxel& defaultValueNewCells,
61  double additionalMarginMeters)
62 {
64 
66  new_x_min, new_x_max, new_y_min, new_y_max, new_z_min, new_z_max,
67  defaultValueNewCells, additionalMarginMeters);
68  this->internal_initialize(false);
69 
70  MRPT_END
71 }
72 
74 {
76  internal_initialize();
77 }
78 
79 void CRandomFieldGridMap3D::internal_initialize(bool erase_prev_contents)
80 {
81  if (erase_prev_contents)
82  {
83  // Set the gridmap (m_map) to initial values:
84  TRandomFieldVoxel def(0, 0); // mean, std
85  fill(def);
86  }
87 
88  // Reset gmrf:
89  m_gmrf.setVerbosityLevel(this->getMinLoggingLevel());
90  if (erase_prev_contents)
91  {
92  m_gmrf.clear();
93  m_mrf_factors_activeObs.clear();
94  }
95  else
96  {
97  // Only remove priors, leave observations:
98  m_gmrf.clearAllConstraintsByType_Binary();
99  }
100  m_mrf_factors_priors.clear();
101 
102  // Initiating prior:
103  const size_t nodeCount = m_map.size();
104  ASSERT_EQUAL_(nodeCount, m_size_x * m_size_y * m_size_z);
105  ASSERT_EQUAL_(m_size_x_times_y, m_size_x * m_size_y);
106 
108  "[internal_initialize] Creating priors for GMRF with "
109  << nodeCount << " nodes." << std::endl);
110  CTicTac tictac;
111  tictac.Tic();
112 
113  m_mrf_factors_activeObs.resize(nodeCount); // Alloc space for obs
114  m_gmrf.initialize(nodeCount);
115 
116  ConnectivityDescriptor* custom_connectivity =
117  m_gmrf_connectivity
118  .get(); // Use a raw ptr to avoid the cost in the inner loops
119 
120  size_t cx = 0, cy = 0, cz = 0;
121  for (size_t j = 0; j < nodeCount; j++)
122  {
123  // add factors between this node and:
124  // 1) the right node: j +1
125  // 2) the back node: j+m_size_x
126  // 3) the top node: j+m_size_x*m_size*y
127  //-------------------------------------------------
128  const size_t c_idx_to_check[3] = {cx, cy, cz};
129  const size_t c_idx_to_check_limits[3] = {m_size_x - 1, m_size_y - 1,
130  m_size_z - 1};
131  const size_t c_neighbor_idx_incr[3] = {1, m_size_x, m_size_x_times_y};
132 
133  for (int dir = 0; dir < 3; dir++)
134  {
135  if (c_idx_to_check[dir] >= c_idx_to_check_limits[dir]) continue;
136 
137  const size_t i = j + c_neighbor_idx_incr[dir];
138  ASSERT_(i < nodeCount);
139 
140  double edge_lamdba = .0;
141  if (custom_connectivity != nullptr)
142  {
143  const bool is_connected =
144  custom_connectivity->getEdgeInformation(
145  this, cx, cy, cz, cx + (dir == 0 ? 1 : 0),
146  cy + (dir == 1 ? 1 : 0), cz + (dir == 2 ? 1 : 0),
147  edge_lamdba);
148  if (!is_connected) continue;
149  }
150  else
151  {
152  edge_lamdba = insertionOptions.GMRF_lambdaPrior;
153  }
154 
155  TPriorFactorGMRF new_prior(*this);
156  new_prior.node_id_i = i;
157  new_prior.node_id_j = j;
158  new_prior.Lambda = edge_lamdba;
159 
160  m_mrf_factors_priors.push_back(new_prior);
161  m_gmrf.addConstraint(
162  *m_mrf_factors_priors.rbegin()); // add to graph
163  }
164 
165  // Increment coordinates:
166  if (++cx >= m_size_x)
167  {
168  cx = 0;
169  if (++cy >= m_size_y)
170  {
171  cy = 0;
172  cz++;
173  }
174  }
175  } // end for "j"
176 
178  "[internal_initialize] Prior built in " << tictac.Tac() << " s\n"
179  << std::endl);
180 }
181 
182 /*---------------------------------------------------------------
183  TInsertionOptions
184  ---------------------------------------------------------------*/
186 
187  = default;
188 
190  std::ostream& out) const
191 {
192  out << mrpt::format(
193  "GMRF_lambdaPrior = %f\n", GMRF_lambdaPrior);
194  out << mrpt::format(
195  "GMRF_skip_variance = %s\n",
196  GMRF_skip_variance ? "true" : "false");
197 }
198 
200  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
201 {
202  GMRF_lambdaPrior = iniFile.read_double(
203  section.c_str(), "GMRF_lambdaPrior", GMRF_lambdaPrior);
204  GMRF_skip_variance = iniFile.read_bool(
205  section.c_str(), "GMRF_skip_variance", GMRF_skip_variance);
206 }
207 
209  const std::string& filName_mean, const std::string& filName_stddev) const
210 {
211  std::ofstream f_mean, f_stddev;
212 
213  f_mean.open(filName_mean);
214  if (!f_mean.is_open())
215  {
216  return false;
217  }
218  else
219  {
220  f_mean << "x coord, y coord, z coord, scalar\n";
221  }
222 
223  if (!filName_stddev.empty())
224  {
225  f_stddev.open(filName_stddev);
226  if (!f_stddev.is_open())
227  {
228  return false;
229  }
230  else
231  {
232  f_mean << "x coord, y coord, z coord, scalar\n";
233  }
234  }
235 
236  const size_t nodeCount = m_map.size();
237  size_t cx = 0, cy = 0, cz = 0;
238  for (size_t j = 0; j < nodeCount; j++)
239  {
240  const double x = idx2x(cx), y = idx2y(cy), z = idx2z(cz);
241  const double mean_val = m_map[j].mean_value;
242  const double stddev_val = m_map[j].stddev_value;
243 
244  f_mean << mrpt::format("%f, %f, %f, %e\n", x, y, z, mean_val);
245 
246  if (f_stddev.is_open())
247  f_stddev << mrpt::format("%f, %f, %f, %e\n", x, y, z, stddev_val);
248 
249  // Increment coordinates:
250  if (++cx >= m_size_x)
251  {
252  cx = 0;
253  if (++cy >= m_size_y)
254  {
255  cy = 0;
256  cz++;
257  }
258  }
259  } // end for "j"
260 
261  return true;
262 }
263 
265 {
266  ASSERTMSG_(
267  !m_mrf_factors_activeObs.empty(),
268  "Cannot update a map with no observations!");
269 
270  mrpt::math::CVectorDouble x_incr, x_var;
271  m_gmrf.updateEstimation(
272  x_incr, insertionOptions.GMRF_skip_variance ? nullptr : &x_var);
273 
274  ASSERT_(size_t(m_map.size()) == size_t(x_incr.size()));
275  ASSERT_(
276  insertionOptions.GMRF_skip_variance ||
277  size_t(m_map.size()) == size_t(x_var.size()));
278 
279  // Update Mean-Variance in the base grid class
280  for (size_t j = 0; j < m_map.size(); j++)
281  {
282  m_map[j].mean_value += x_incr[j];
283  m_map[j].stddev_value =
284  insertionOptions.GMRF_skip_variance ? .0 : std::sqrt(x_var[j]);
285  }
286 }
287 
289  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor)
290 {
291  m_gmrf_connectivity = new_connectivity_descriptor;
292 }
293 
295  /** [in] The value observed in the (x,y,z) position */
296  const double sensorReading,
297  /** [in] The variance of the sensor observation */
298  const double sensorVariance,
299  /** [in] The (x,y,z) location */
300  const mrpt::math::TPoint3D& point,
301  /** [in] Voxel interpolation method: how many voxels will be affected by the
302  reading */
303  const TVoxelInterpolationMethod method,
304  /** [in] Run a global map update after inserting this observatin
305  (algorithm-dependant) */
306  const bool update_map)
307 {
308  MRPT_START
309 
310  ASSERT_ABOVE_(sensorVariance, .0);
311  ASSERTMSG_(
312  m_mrf_factors_activeObs.size() == m_map.size(),
313  "Trying to insert observation in uninitialized map (!)");
314 
315  const size_t cell_idx =
316  cellAbsIndexFromCXCYCZ(x2idx(point.x), y2idx(point.y), z2idx(point.z));
317  if (cell_idx == INVALID_VOXEL_IDX) return false;
318 
319  TObservationGMRF new_obs(*this);
320  new_obs.node_id = cell_idx;
321  new_obs.obsValue = sensorReading;
322  new_obs.Lambda = 1.0 / sensorVariance;
323 
324  m_mrf_factors_activeObs[cell_idx].push_back(new_obs);
325  m_gmrf.addConstraint(
326  *m_mrf_factors_activeObs[cell_idx].rbegin()); // add to graph
327 
328  if (update_map) this->updateMapEstimation();
329 
330  return true;
331 
332  MRPT_END
333 }
334 
335 uint8_t CRandomFieldGridMap3D::serializeGetVersion() const { return 0; }
338 {
339  dyngridcommon_writeToStream(out);
340 
341  // To assure compatibility: The size of each cell:
342  auto n = static_cast<uint32_t>(sizeof(TRandomFieldVoxel));
343  out << n;
344 
345  // Save the map contents:
346  n = static_cast<uint32_t>(m_map.size());
347  out << n;
348 
349 // Save the "m_map": This requires special handling for big endian systems:
350 #if MRPT_IS_BIG_ENDIAN
351  for (uint32_t i = 0; i < n; i++)
352  out << m_map[i].mean_value << m_map[i].stddev_value;
353 #else
354  // Little endian: just write all at once:
355  out.WriteBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
356 #endif
357 
358  out << insertionOptions.GMRF_lambdaPrior
359  << insertionOptions.GMRF_skip_variance;
360 }
361 
363  mrpt::serialization::CArchive& in, uint8_t version)
364 {
365  switch (version)
366  {
367  case 0:
368  {
369  dyngridcommon_readFromStream(in);
370 
371  // To assure compatibility: The size of each cell:
372  uint32_t n;
373  in >> n;
374 
375  ASSERT_EQUAL_(n, static_cast<uint32_t>(sizeof(TRandomFieldVoxel)));
376  // Load the map contents:
377  in >> n;
378  m_map.resize(n);
379 
380 // Read the note in writeToStream()
381 #if MRPT_IS_BIG_ENDIAN
382  for (uint32_t i = 0; i < n; i++)
383  in >> m_map[i].mean_value >> m_map[i].stddev_value;
384 #else
385  // Little endian: just read all at once:
386  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
387 #endif
388  in >> insertionOptions.GMRF_lambdaPrior >>
389  insertionOptions.GMRF_skip_variance;
390  }
391  break;
392  default:
394  };
395 }
396 
397 // ============ TObservationGMRF ===========
399 {
400  return m_parent->m_map[this->node_id].mean_value - this->obsValue;
401 }
403 {
404  return this->Lambda;
405 }
407 {
408  dr_dx = 1.0;
409 }
410 // ============ TPriorFactorGMRF ===========
412 {
413  return m_parent->m_map[this->node_id_i].mean_value -
414  m_parent->m_map[this->node_id_j].mean_value;
415 }
417 {
418  return this->Lambda;
419 }
421  double& dr_dx_i, double& dr_dx_j) const
422 {
423  dr_dx_i = +1.0;
424  dr_dx_j = -1.0;
425 }
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
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.
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);
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...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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
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.
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
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...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020