31 double resolution_XYZ,
double resolution_YPR)
35 uniformDistribution();
40 if (
this == &o)
return;
45 void CPose3DPDFGrid::getMean(
CPose3D& p)
const
50 for (
size_t cR = 0; cR < m_sizeRoll; cR++)
51 for (
size_t cP = 0; cP < m_sizePitch; cP++)
52 for (
size_t cY = 0; cY < m_sizeYaw; cY++)
53 for (
size_t cz = 0; cz < m_sizeZ; cz++)
54 for (
size_t cy = 0; cy < m_sizeY; cy++)
55 for (
size_t cx = 0; cx < m_sizeX; cx++)
58 *getByIndex(cx, cy, cz, cY, cP, cR);
61 idx2x(cx), idx2y(cy), idx2z(cz),
62 idx2yaw(cY), idx2pitch(cP), idx2roll(cR)),
68 std::tuple<CMatrixDouble66, CPose3D> CPose3DPDFGrid::getCovarianceAndMean()
75 for (
size_t cR = 0; cR < m_sizeRoll; cR++)
76 for (
size_t cP = 0; cP < m_sizePitch; cP++)
77 for (
size_t cY = 0; cY < m_sizeYaw; cY++)
78 for (
size_t cz = 0; cz < m_sizeZ; cz++)
79 for (
size_t cy = 0; cy < m_sizeY; cy++)
80 for (
size_t cx = 0; cx < m_sizeX; cx++)
83 *getByIndex(cx, cy, cz, cY, cP, cR);
86 idx2x(cx), idx2y(cy), idx2z(cz), idx2yaw(cY),
87 idx2pitch(cP), idx2roll(cR));
94 uint8_t CPose3DPDFGrid::serializeGetVersion()
const {
return 0; }
98 out << m_bb_min << m_bb_max << m_resolutionXYZ << m_resolutionYPR;
99 out.WriteAs<int32_t>(m_sizeX);
100 out.WriteAs<int32_t>(m_sizeY);
101 out.WriteAs<int32_t>(m_sizeZ);
102 out.WriteAs<int32_t>(m_sizeYaw);
103 out.WriteAs<int32_t>(m_sizePitch);
104 out.WriteAs<int32_t>(m_sizeRoll);
105 out << m_sizeX << m_sizeY << m_sizeZ << m_sizeYaw << m_sizePitch
107 out << m_min_cidX << m_min_cidY << m_min_cidZ << m_min_cidYaw
108 << m_min_cidPitch << m_min_cidRoll;
113 void CPose3DPDFGrid::serializeFrom(
120 in >> m_bb_min >> m_bb_max >> m_resolutionXYZ >> m_resolutionYPR;
121 m_sizeX = in.
ReadAs<int32_t>();
122 m_sizeY = in.
ReadAs<int32_t>();
123 m_sizeZ = in.
ReadAs<int32_t>();
124 m_sizeYaw = in.
ReadAs<int32_t>();
125 m_sizePitch = in.
ReadAs<int32_t>();
126 m_sizeRoll = in.
ReadAs<int32_t>();
127 in >> m_sizeX >> m_sizeY >> m_sizeZ >> m_sizeYaw >> m_sizePitch >>
129 in >> m_min_cidX >> m_min_cidY >> m_min_cidZ >> m_min_cidYaw >>
130 m_min_cidPitch >> m_min_cidRoll;
135 update_cached_size_products();
145 bool CPose3DPDFGrid::saveToTextFile(
const std::string& dataFile)
const
152 void CPose3DPDFGrid::changeCoordinatesReference([
153 [maybe_unused]]
const CPose3D& newReferenceBase)
158 void CPose3DPDFGrid::bayesianFusion(
165 void CPose3DPDFGrid::inverse([[maybe_unused]]
CPose3DPDF& o)
const
170 void CPose3DPDFGrid::drawSingleSample([[maybe_unused]]
CPose3D& outPart)
const
175 void CPose3DPDFGrid::drawManySamples(
176 [[maybe_unused]]
size_t N,
177 [[maybe_unused]] std::vector<CVectorDouble>& outSamples)
const
187 for (
auto it = m_data.begin(); it != m_data.end(); ++it) SUM += *it;
191 const auto f = 1.0 / SUM;
192 for (
double& it : m_data) it *= f;
196 void CPose3DPDFGrid::uniformDistribution()
198 const double val = 1.0 / m_data.size();
200 for (
double& it : m_data) it =
val;