示例#1
0
文件: test.cpp 项目: Jarlene/mrpt
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;
}
示例#2
0
/*---------------------------------------------------------------
					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;
}