38 #define POSE_COLOR 0, 0, 1
39 #define POINT_COLOR 1, 0, 0
46 auto outObj = CSetOfObjects::Create();
50 const auto* p =
dynamic_cast<const CPosePDFSOG*
>(&o);
54 lins->setColor(0.0f, 0.0f, 1.0f, 0.6f);
57 for (
const auto& it : *p)
61 ellip->setPose(
CPose3D(it.mean.x(), it.mean.y(), 0));
62 ellip->setCovMatrix(it.cov.blockCopy<2, 2>());
64 ellip->setQuantiles(3);
65 ellip->enableDrawSolid3D(
false);
67 outObj->insert(ellip);
70 it.mean.x(), it.mean.y(), 0,
87 ellip->setPose(
CPose3D(p->mean.x(), p->mean.y(), 0));
88 ellip->setCovMatrix(p->cov.blockCopy<2, 2>());
91 ellip->setQuantiles(3);
92 ellip->enableDrawSolid3D(
false);
94 outObj->insert(ellip);
97 p->mean.x(), p->mean.y(), 0,
101 outObj->insert(lins);
116 for (
size_t i = 0; i < p->size(); ++i)
118 const auto po = p->m_particles[i].d;
119 pnts->insertPoint(
d2f(po.x),
d2f(po.y), 0);
124 outObj->insert(pnts);
125 outObj->insert(lins);
136 auto outObj = std::make_shared<CSetOfObjects>();
144 for (
const auto& it : *p)
147 if (it.val.cov(2, 2) == 0)
150 ellip->setCovMatrix(it.val.cov.blockCopy<2, 2>());
151 ellip->setQuantiles(3);
152 ellip->enableDrawSolid3D(
false);
158 ellip->setCovMatrix(it.val.cov);
159 ellip->setQuantiles(3);
160 ellip->enableDrawSolid3D(
false);
163 obj->setPose(it.val.mean);
175 if (p->cov(2, 2) == 0)
178 ellip->setCovMatrix(p->cov.blockCopy<2, 2>());
179 ellip->setQuantiles(3);
180 ellip->enableDrawSolid3D(
false);
186 ellip->setCovMatrix(p->cov);
187 ellip->setQuantiles(3);
188 ellip->enableDrawSolid3D(
false);
191 obj->setLocation(p->mean.x(), p->mean.y(), p->mean.z());
202 const size_t N = p->size();
206 for (
size_t i = 0; i < N; i++)
208 i, p->m_particles[i].d->x, p->m_particles[i].d->y,
209 p->m_particles[i].d->z);
229 for (
const auto& it : *p)
232 std::make_shared<opengl::CEllipsoid3D>();
234 obj->setPose(it.val.mean);
235 obj->setCovMatrix(it.val.cov.blockCopy<3, 3>());
237 obj->setQuantiles(3);
238 obj->enableDrawSolid3D(
false);
245 axes->setPose(it.val.mean);
247 outObj->insert(axes);
255 auto obj = std::make_shared<opengl::CEllipsoid3D>();
257 obj->setPose(p->mean);
258 obj->setCovMatrix(p->cov.blockCopy<3, 3>());
260 obj->setQuantiles(3);
261 obj->enableDrawSolid3D(
false);
267 axes->setPose(p->mean);
269 outObj->insert(axes);
276 for (
size_t i = 0; i < p->size(); i++)
280 axes->setPose(p->m_particles[i].d);
281 outObj->insert(axes);
302 obj->setPose(
CPose3D(p->mean));
303 obj->setCovMatrix(p->cov.blockCopy<3, 3>());
305 obj->setQuantiles(3);
306 obj->enableDrawSolid3D(
false);
312 axes->setPose(
CPose3D(p->mean));
314 outObj->insert(axes);