// Add objects at your will to check results
void generateObjects(CSetOfObjects::Ptr& world)
{
// create object, give it a random pose/color, insert it in the world
CDisk::Ptr dsk = mrpt::make_aligned_shared<CDisk>();
dsk->setDiskRadius(MYRANDG(5, 5), MYRANDG(5));
configRandom(dsk);
world->insert(dsk);
CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(MYRANDG(5, 1));
configRandom(sph);
world->insert(sph);
CTexturedPlane::Ptr pln = mrpt::make_aligned_shared<CTexturedPlane>(
MYRANDG(10, -10), MYRANDG(10), MYRANDG(10, -10), MYRANDG(10));
configRandom(pln);
world->insert(pln);
for (size_t i = 0; i < 5; i++)
{
CPolyhedron::Ptr poly =
CPolyhedron::CreateRandomPolyhedron(MYRANDG(2, 2));
configRandom(poly);
world->insert(poly);
}
CCylinder::Ptr cil = mrpt::make_aligned_shared<CCylinder>(
MYRANDG(3.0, 3.0), MYRANDG(3.0, 1.0), MYRANDG(2.0f, 3.0f), 50, 1);
configRandom(cil);
world->insert(cil);
CEllipsoid::Ptr ell = mrpt::make_aligned_shared<CEllipsoid>();
CMatrixDouble md = CMatrixDouble(3, 3);
for (size_t i = 0; i < 3; i++) md(i, i) = MYRANDG(8.0, 1.0);
for (size_t i = 0; i < 3; i++)
{
size_t ii = (i + 1) % 3;
md(i, ii) = md(ii, i) = MYRANDG(sqrt(md(i, i) * md(ii, ii)));
}
ell->setCovMatrix(md);
configRandom(std::dynamic_pointer_cast<CRenderizable>(ell));
world->insert(ell);
}
void CDifodoDatasets::updateScene()
{
CPose3D rel_lenspose(0, -0.022, 0, 0, 0, 0);
scene = window.get3DSceneAndLock();
// Reference gt
CSetOfObjects::Ptr reference_gt = scene->getByClass<CSetOfObjects>(0);
reference_gt->setPose(gt_pose);
// Camera points
CPointCloudColoured::Ptr cam_points =
scene->getByClass<CPointCloudColoured>(0);
cam_points->clear();
cam_points->setPose(gt_pose);
for (unsigned int y = 0; y < cols; y++)
for (unsigned int z = 0; z < rows; z++)
cam_points->push_back(
depth[repr_level](z, y), xx[repr_level](z, y),
yy[repr_level](z, y), 1.f - sqrt(weights(z, y)),
sqrt(weights(z, y)), 0);
// DifOdo camera
CBox::Ptr camera_odo = scene->getByClass<CBox>(0);
camera_odo->setPose(cam_pose + rel_lenspose);
// Groundtruth camera
CBox::Ptr camera_gt = scene->getByClass<CBox>(1);
camera_gt->setPose(gt_pose + rel_lenspose);
// Frustum
CFrustum::Ptr FOV = scene->getByClass<CFrustum>(0);
FOV->setPose(gt_pose);
if ((first_pose == true) && ((gt_pose - gt_oldpose).norm() < 0.5))
{
// Difodo traj lines
CSetOfLines::Ptr traj_lines_odo = scene->getByClass<CSetOfLines>(0);
traj_lines_odo->appendLine(
cam_oldpose.x(), cam_oldpose.y(), cam_oldpose.z(), cam_pose.x(),
cam_pose.y(), cam_pose.z());
// Difodo traj points
CPointCloud::Ptr traj_points_odo = scene->getByClass<CPointCloud>(0);
traj_points_odo->insertPoint(cam_pose.x(), cam_pose.y(), cam_pose.z());
// Groundtruth traj lines
CSetOfLines::Ptr traj_lines_gt = scene->getByClass<CSetOfLines>(1);
traj_lines_gt->appendLine(
gt_oldpose.x(), gt_oldpose.y(), gt_oldpose.z(), gt_pose.x(),
gt_pose.y(), gt_pose.z());
// Groundtruth traj points
CPointCloud::Ptr traj_points_gt = scene->getByClass<CPointCloud>(1);
traj_points_gt->insertPoint(gt_pose.x(), gt_pose.y(), gt_pose.z());
}
// Ellipsoid showing covariance
math::CMatrixFloat33 cov3d = 20.f * est_cov.topLeftCorner(3, 3);
CEllipsoid::Ptr ellip = scene->getByClass<CEllipsoid>(0);
ellip->setCovMatrix(cov3d);
ellip->setPose(cam_pose + rel_lenspose);
window.unlockAccess3DScene();
window.repaint();
}
void CDifodoDatasets::initializeScene()
{
CPose3D rel_lenspose(0, -0.022, 0, 0, 0, 0);
global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE(1000000);
window.resize(1000, 900);
window.setPos(900, 0);
window.setCameraZoom(16);
window.setCameraAzimuthDeg(0);
window.setCameraElevationDeg(90);
window.setCameraPointingToPoint(0, 0, 0);
window.setCameraPointingToPoint(0, 0, 1);
scene = window.get3DSceneAndLock();
// Lights:
scene->getViewport()->setNumberOfLights(1);
CLight& light0 = scene->getViewport()->getLight(0);
light0.light_ID = 0;
light0.setPosition(0, 0, 1, 1);
// Grid (ground)
CGridPlaneXY::Ptr ground = mrpt::make_aligned_shared<CGridPlaneXY>();
scene->insert(ground);
// Reference
// CSetOfObjects::Ptr reference = stock_objects::CornerXYZ();
// scene->insert( reference );
// Cameras and points
//------------------------------------------------------
// DifOdo camera
CBox::Ptr camera_odo = mrpt::make_aligned_shared<CBox>(
math::TPoint3D(-0.02, -0.1, -0.01), math::TPoint3D(0.02, 0.1, 0.01));
camera_odo->setPose(cam_pose + rel_lenspose);
camera_odo->setColor(0, 1, 0);
scene->insert(camera_odo);
// Groundtruth camera
CBox::Ptr camera_gt = mrpt::make_aligned_shared<CBox>(
math::TPoint3D(-0.02, -0.1, -0.01), math::TPoint3D(0.02, 0.1, 0.01));
camera_gt->setPose(gt_pose + rel_lenspose);
camera_gt->setColor(1, 0, 0);
scene->insert(camera_gt);
// Frustum
opengl::CFrustum::Ptr FOV = mrpt::make_aligned_shared<opengl::CFrustum>(
0.3f, 2, 57.3f * fovh, 57.3f * fovv, 1.f, true, false);
FOV->setColor(0.7, 0.7, 0.7);
FOV->setPose(gt_pose);
scene->insert(FOV);
// Reference gt
CSetOfObjects::Ptr reference_gt = stock_objects::CornerXYZ();
reference_gt->setScale(0.2f);
reference_gt->setPose(gt_pose);
scene->insert(reference_gt);
// Camera points
CPointCloudColoured::Ptr cam_points =
mrpt::make_aligned_shared<CPointCloudColoured>();
cam_points->setColor(1, 0, 0);
cam_points->setPointSize(2);
cam_points->enablePointSmooth(1);
cam_points->setPose(cam_pose);
scene->insert(cam_points);
// Trajectories and covariance
//-------------------------------------------------------------
// Dif Odometry
CSetOfLines::Ptr traj_lines_odo = mrpt::make_aligned_shared<CSetOfLines>();
traj_lines_odo->setLocation(0, 0, 0);
traj_lines_odo->setColor(0, 0.6, 0);
traj_lines_odo->setLineWidth(6);
scene->insert(traj_lines_odo);
CPointCloud::Ptr traj_points_odo = mrpt::make_aligned_shared<CPointCloud>();
traj_points_odo->setColor(0, 0.6, 0);
traj_points_odo->setPointSize(4);
traj_points_odo->enablePointSmooth(1);
scene->insert(traj_points_odo);
// Groundtruth
CSetOfLines::Ptr traj_lines_gt = mrpt::make_aligned_shared<CSetOfLines>();
traj_lines_gt->setLocation(0, 0, 0);
traj_lines_gt->setColor(0.6, 0, 0);
traj_lines_gt->setLineWidth(6);
scene->insert(traj_lines_gt);
CPointCloud::Ptr traj_points_gt = mrpt::make_aligned_shared<CPointCloud>();
traj_points_gt->setColor(0.6, 0, 0);
traj_points_gt->setPointSize(4);
traj_points_gt->enablePointSmooth(1);
scene->insert(traj_points_gt);
// Ellipsoid showing covariance
math::CMatrixFloat33 cov3d = 20.f * est_cov.topLeftCorner(3, 3);
CEllipsoid::Ptr ellip = mrpt::make_aligned_shared<CEllipsoid>();
ellip->setCovMatrix(cov3d);
ellip->setQuantiles(2.0);
ellip->setColor(1.0, 1.0, 1.0, 0.5);
ellip->enableDrawSolid3D(true);
ellip->setPose(cam_pose + rel_lenspose);
scene->insert(ellip);
// User-interface information
utils::CImage img_legend;
img_legend.loadFromXPM(legend_xpm);
COpenGLViewport::Ptr legend = scene->createViewport("legend");
legend->setViewportPosition(20, 20, 332, 164);
legend->setImageView(img_legend);
window.unlockAccess3DScene();
window.repaint();
}
