33 double xMin,
double xMax,
double yMin,
double yMax,
double resolutionXY,
34 double resolutionPhi,
double phiMin,
double phiMax)
36 xMin, xMax, yMin, yMax, resolutionXY, resolutionPhi, phiMin, phiMax)
38 uniformDistribution();
43 if (
this == &o)
return;
51 CPosePDFGrid::~CPosePDFGrid() =
default;
58 void CPosePDFGrid::getMean(
CPose2D& p)
const
62 for (
size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
63 for (
size_t y = 0; y < m_sizeY; y++)
64 for (
size_t x = 0; x < m_sizeX; x++)
66 const double w = *getByIndex(x, y, phiInd);
68 CPose2D(idx2x(x), idx2y(y), idx2phi(phiInd)), w);
73 std::tuple<CMatrixDouble33, CPose2D> CPosePDFGrid::getCovarianceAndMean()
const
77 TPose2D(0, 0, 0), m_sizePhi * m_sizeY * m_sizeX);
79 for (
size_t phiInd = 0; phiInd < m_sizePhi; phiInd++)
81 for (
size_t y = 0; y < m_sizeY; y++)
82 for (
size_t x = 0; x < m_sizeX; x++)
85 log(*getByIndex(x, y, phiInd));
87 TPose2D(idx2x(x), idx2y(y), idx2phi(phiInd));
93 uint8_t CPosePDFGrid::serializeGetVersion()
const {
return 0; }
97 out << m_xMin << m_xMax << m_yMin << m_yMax << m_phiMin << m_phiMax
98 << m_resolutionXY << m_resolutionPhi << static_cast<int32_t>(m_sizeX)
99 <<
static_cast<int32_t
>(m_sizeY) <<
static_cast<int32_t
>(m_sizePhi)
100 <<
static_cast<int32_t
>(m_sizeXY) <<
static_cast<int32_t
>(m_idxLeftX)
101 <<
static_cast<int32_t
>(m_idxLeftY)
102 <<
static_cast<int32_t
>(m_idxLeftPhi);
107 void CPosePDFGrid::serializeFrom(
115 in >> m_xMin >> m_xMax >> m_yMin >> m_yMax >> m_phiMin >>
116 m_phiMax >> m_resolutionXY >> m_resolutionPhi;
118 int32_t sizeX, sizeY, sizePhi, sizeXY, idxLeftX, idxLeftY,
121 in >> sizeX >> sizeY >> sizePhi >> sizeXY >> idxLeftX >> idxLeftY >>
128 m_idxLeftX = idxLeftX;
129 m_idxLeftY = idxLeftY;
130 m_idxLeftPhi = idxLeftPhi;
141 bool CPosePDFGrid::saveToTextFile(
const std::string& dataFile)
const
143 const auto dimsFile = dataFile + std::string(
"_dims.txt");
145 std::ofstream f_d(dataFile), f_s(dimsFile);
146 if (!f_d.is_open() || !f_s.is_open())
return false;
150 "%u %u %u %f %f %f %f %f %f\n", (
unsigned)m_sizeX, (
unsigned)m_sizeY,
151 (
unsigned)m_sizePhi, m_xMin, m_xMax, m_yMin, m_yMax, m_phiMin,
155 for (
unsigned int phiInd = 0; phiInd < m_sizePhi; phiInd++)
157 for (
unsigned int y = 0; y < m_sizeY; y++)
159 for (
unsigned int x = 0; x < m_sizeX; x++)
160 f_d <<
mrpt::format(
"%.5e ", *getByIndex(x, y, phiInd));
171 void CPosePDFGrid::changeCoordinatesReference([
172 [maybe_unused]]
const CPose3D& newReferenceBase)
180 void CPosePDFGrid::bayesianFusion(
182 [[maybe_unused]]
const double minMahalanobisDistToDrop)
190 void CPosePDFGrid::inverse([[maybe_unused]]
CPosePDF& o)
const
198 void CPosePDFGrid::drawSingleSample([[maybe_unused]]
CPose2D& outPart)
const
206 void CPosePDFGrid::drawManySamples(
207 [[maybe_unused]]
size_t N,
208 [[maybe_unused]] std::vector<CVectorDouble>& outSamples)
const
221 for (
auto it = m_data.begin(); it != m_data.end(); ++it) SUM += *it;
226 for (
double& it : m_data) it /= SUM;
233 void CPosePDFGrid::uniformDistribution()
235 double val = 1.0f / m_data.size();
237 for (
double& it : m_data) it =
val;