mrpt::opengl::CSetOfObjects::Ptr framePosesVecVisualize(
const TFramePosesVec& poses, const double len, const double lineWidth)
{
mrpt::opengl::CSetOfObjects::Ptr obj =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
for (size_t i = 0; i < poses.size(); i++)
{
CSetOfObjects::Ptr corner =
opengl::stock_objects::CornerXYZSimple(len, lineWidth);
CPose3D p = poses[i];
p.x(WORLD_SCALE * p.x());
p.y(WORLD_SCALE * p.y());
p.z(WORLD_SCALE * p.z());
corner->setPose(p);
corner->setName(format("%u", (unsigned int)i));
corner->enableShowName();
obj->insert(corner);
}
return obj;
}
/*---------------------------------------------------------------
RobotPioneer
---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotPioneer()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
ret->setName("theRobot");
CSetOfTriangles::Ptr obj = mrpt::make_aligned_shared<CSetOfTriangles>();
// Add triangles:
CSetOfTriangles::TTriangle trian;
trian.r[0] = trian.r[1] = trian.r[2] = 1;
trian.g[0] = trian.g[1] = trian.g[2] = 0;
trian.b[0] = trian.b[1] = trian.b[2] = 0;
trian.a[0] = trian.a[1] = trian.a[2] = 1;
trian.x[0] = 0.10f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = -0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.25f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 0
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.20f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 1
// trian.r = 0.9f; trian.g = 0; trian.b = 0; trian.a = 1;
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = 0.10f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.20f;
trian.z[2] = 0.20f;
obj->insertTriangle(trian); // 2
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = 0.10f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.05f;
trian.z[2] = 0.20f;
obj->insertTriangle(trian); // 3
trian.x[0] = -0.20f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.25f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 2b
trian.x[0] = -0.20f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.05f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 3b
// trian.r = 0.8f; trian.g = 0; trian.b = 0; trian.a = 1;
trian.x[0] = 0.10f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = -0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.25f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 4
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = -0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.05f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 5
trian.x[0] = 0.10f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = 0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.25f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 6
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = -0.20f;
trian.y[0] = 0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.05f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 7
trian.r[0] = trian.r[1] = trian.r[2] = 0.05f;
trian.g[0] = trian.g[1] = trian.g[2] = 0.05f;
trian.b[0] = trian.b[1] = trian.b[2] = 0.05f;
trian.a[0] = trian.a[1] = trian.a[2] = 1;
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = 0.05f;
trian.y[0] = 0.11f;
trian.y[1] = 0.11f;
trian.y[2] = 0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 8
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = -0.05f;
trian.y[0] = 0.11f;
trian.y[1] = 0.11f;
trian.y[2] = 0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 9
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = 0.05f;
trian.y[0] = -0.11f;
trian.y[1] = -0.11f;
trian.y[2] = -0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 10
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = -0.05f;
trian.y[0] = -0.11f;
trian.y[1] = -0.11f;
trian.y[2] = -0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 11
ret->insert(obj);
return ret;
}
