16 #include <mrpt/config.h> 19 #include <vtkStructuredGrid.h> 20 #include <vtkDoubleArray.h> 21 #include <vtkPointData.h> 22 #include <vtkVersion.h> 23 #include <vtkCellArray.h> 24 #include <vtkPoints.h> 25 #include <vtkXMLStructuredGridWriter.h> 26 #include <vtkSmartPointer.h> 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)
43 x_min, x_max, y_min, y_max, z_min, z_max, voxel_size ,
47 if (call_initialize_now) this->internal_initialize();
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,
60 x_min, x_max, y_min, y_max, z_min, z_max, resolution_xy, resolution_z,
62 this->internal_initialize();
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,
71 double additionalMarginMeters)
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);
86 internal_initialize();
91 if (erase_prev_contents)
99 m_gmrf.setVerbosityLevel(this->getMinLoggingLevel());
100 if (erase_prev_contents)
103 m_mrf_factors_activeObs.clear();
108 m_gmrf.clearAllConstraintsByType_Binary();
110 m_mrf_factors_priors.clear();
113 const size_t nodeCount = m_map.size();
118 "[internal_initialize] Creating priors for GMRF with " 119 << nodeCount <<
" nodes." << std::endl);
123 m_mrf_factors_activeObs.resize(nodeCount);
124 m_gmrf.initialize(nodeCount);
130 size_t cx = 0, cy = 0, cz = 0;
131 for (
size_t j = 0; j < nodeCount; j++)
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,
141 const size_t c_neighbor_idx_incr[3] = {1, m_size_x, m_size_x_times_y};
145 if (c_idx_to_check[
dir] >= c_idx_to_check_limits[
dir])
continue;
147 const size_t i = j + c_neighbor_idx_incr[
dir];
150 double edge_lamdba = .0;
151 if (custom_connectivity !=
nullptr)
153 const bool is_connected =
155 this, cx, cy, cz, cx + (
dir == 0 ? 1 : 0),
156 cy + (
dir == 1 ? 1 : 0), cz + (
dir == 2 ? 1 : 0),
158 if (!is_connected)
continue;
162 edge_lamdba = insertionOptions.GMRF_lambdaPrior;
168 new_prior.
Lambda = edge_lamdba;
170 m_mrf_factors_priors.push_back(new_prior);
171 m_gmrf.addConstraint(
172 *m_mrf_factors_priors.rbegin());
176 if (++cx >= m_size_x)
179 if (++cy >= m_size_y)
188 "[internal_initialize] Prior built in " << tictac.
Tac() <<
" s\n" 196 : GMRF_lambdaPrior(0.01f),
198 GMRF_skip_variance(false)
203 std::ostream& out)
const 206 "GMRF_lambdaPrior = %f\n", GMRF_lambdaPrior);
208 "GMRF_skip_variance = %s\n",
209 GMRF_skip_variance ?
"true" :
"false");
215 GMRF_lambdaPrior =
iniFile.read_double(
216 section.c_str(),
"GMRF_lambdaPrior", GMRF_lambdaPrior);
217 GMRF_skip_variance =
iniFile.read_bool(
218 section.c_str(),
"GMRF_skip_variance", GMRF_skip_variance);
229 vtkStructuredGrid* vtkGrid = vtkStructuredGrid::New();
233 vtkSmartPointer<vtkXMLStructuredGridWriter> writer =
234 vtkSmartPointer<vtkXMLStructuredGridWriter>::New();
235 writer->SetFileName(fil.c_str());
237 #if VTK_MAJOR_VERSION <= 5 238 writer->SetInput(vtkGrid);
240 writer->SetInputData(vtkGrid);
243 int ret = writer->Write();
257 std::ofstream f_mean, f_stddev;
259 f_mean.open(filName_mean);
260 if (!f_mean.is_open())
266 f_mean <<
"x coord, y coord, z coord, scalar\n";
269 if (!filName_stddev.empty())
271 f_stddev.open(filName_stddev);
272 if (!f_stddev.is_open())
278 f_mean <<
"x coord, y coord, z coord, scalar\n";
282 const size_t nodeCount =
m_map.size();
283 size_t cx = 0, cy = 0, cz = 0;
284 for (
size_t j = 0; j < nodeCount; j++)
287 const double mean_val =
m_map[j].mean_value;
288 const double stddev_val =
m_map[j].stddev_value;
292 if (f_stddev.is_open())
314 "Cannot update a map with no observations!");
316 Eigen::VectorXd x_incr, x_var;
323 size_t(
m_map.size()) ==
size_t(x_var.size()));
326 for (
size_t j = 0; j <
m_map.size(); j++)
328 m_map[j].mean_value += x_incr[j];
329 m_map[j].stddev_value =
342 const double sensorReading,
344 const double sensorVariance,
352 const bool update_map)
359 "Trying to insert observation in uninitialized map (!)");
361 const size_t cell_idx =
368 new_obs.
Lambda = 1.0 / sensorVariance;
396 #if MRPT_IS_BIG_ENDIAN 398 out <<
m_map[i].mean_value <<
m_map[i].stddev_value;
427 #if MRPT_IS_BIG_ENDIAN 444 vtkStructuredGrid* output,
const std::string& label_mean,
453 const int num_values =
nx *
ny *
nz;
455 vtkPoints* newPoints = vtkPoints::New();
457 vtkDoubleArray* newData = vtkDoubleArray::New();
458 newData->SetNumberOfComponents(3);
459 newData->SetNumberOfTuples(num_values);
461 vtkDoubleArray* mean_arr = vtkDoubleArray::New();
462 mean_arr->SetNumberOfComponents(1);
463 mean_arr->SetNumberOfTuples(num_values);
465 vtkDoubleArray* std_arr = vtkDoubleArray::New();
466 std_arr->SetNumberOfComponents(1);
467 std_arr->SetNumberOfTuples(num_values);
469 vtkIdType numtuples = newData->GetNumberOfTuples();
472 size_t cx = 0, cy = 0, cz = 0;
473 for (vtkIdType cc = 0; cc < numtuples; cc++)
477 newData->SetComponent(cc, 0,
x);
478 newData->SetComponent(cc, 1,
y);
479 newData->SetComponent(cc, 2,
z);
481 mean_arr->SetComponent(cc, 0,
m_map[cc].mean_value);
482 std_arr->SetComponent(cc, 0,
m_map[cc].stddev_value);
498 newPoints->SetData(newData);
501 output->SetExtent(0,
nx - 1, 0,
ny - 1, 0,
nz - 1);
502 output->SetPoints(newPoints);
505 mean_arr->SetName(label_mean.c_str());
506 std_arr->SetName(label_stddev.c_str());
507 output->GetPointData()->AddArray(mean_arr);
508 output->GetPointData()->AddArray(std_arr);
522 return m_parent->m_map[this->node_id].mean_value - this->obsValue;
535 return m_parent->m_map[this->node_id_i].mean_value -
536 m_parent->m_map[this->node_id_j].mean_value;
543 double& dr_dx_i,
double& dr_dx_j)
const bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
double Tac() noexcept
Stops the stopwatch.
EIGEN_STRONG_INLINE void fill(const Scalar v)
mrpt::graphs::ScalarFactorGraph m_gmrf
#define THROW_EXCEPTION(msg)
The contents of each voxel in a CRandomFieldGridMap3D map.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in 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).
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
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.0)
Changes the size of the grid, maintaining previous contents.
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.
size_t cellAbsIndexFromCXCYCZ(const int cx, const int cy, const int cz) const
Gets the absolute index of a voxel in the linear container m_map[] from its cx,cy,cz indices, or -1 if out of map bounds (in any dimension).
void dumpToTextStream(std::ostream &out) const
See utils::CLoadableOptions.
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
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.
double idx2z(int cz) const
double idx2y(int cy) const
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define ASSERT_(f)
Defines an assertion mechanism.
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.
double Lambda
"Information" of the observation (=inverse of the variance)
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 &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.
std::vector< std::deque< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information, for each map cell...
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"))
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
ConnectivityDescriptor::Ptr m_gmrf_connectivity
Empty: default.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
int x2idx(double x) const
Transform a coordinate values into voxel indexes.
GLsizei const GLchar ** string
int z2idx(double z) const
TInsertionOptions()
Default values loader.
void dyngridcommon_readFromStream(ARCHIVE &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
double obsValue
Observation value.
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
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.
int y2idx(double y) const
CRandomFieldGridMap3D represents a 3D regular grid where each voxel is associated one real-valued pro...
GLfloat GLfloat GLfloat GLfloat nx
std::vector< TRandomFieldVoxel > m_map
The cells.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define ASSERT_ABOVE_(__A, __B)
TVoxelInterpolationMethod
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".
void dyngridcommon_writeToStream(ARCHIVE &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
virtual void clear()
Erase the contents of all the cells, setting them to their default values (default ctor)...
double idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
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 loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion)
See utils::CLoadableOptions.
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.
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
static const size_t INVALID_VOXEL_IDX
unsigned __int32 uint32_t
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) override
Changes the size of the grid, erasing previous contents.If resolution_z<0, the same resolution will b...
void updateEstimation(Eigen::VectorXd &solved_x_inc, Eigen::VectorXd *solved_variances=nullptr)
TInsertionOptions insertionOptions
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...