void drawCameraStateEllipse(VNL::Vector<double> location, VNL::Matrix<double> Pxx, COpenGLScenePtr & mrpt3dscene){
CEllipsoidPtr objEllip = CEllipsoid::Create();
objEllip->setLocation(location.Get(0), location.Get(1), location.Get(2));
CMatrixDouble33 COV3;
int size=3;
for (int i = 0; i < size; i++) {
for (int j = 0; j < size; j++) {
COV3(i, j) = Pxx.Get(i, j);
}
}
objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
objEllip->setColor(1, 1, 0);
objEllip->enableDrawSolid3D(true);
mrpt3dscene->insert(objEllip);
}
void drawEllipse(Feature* feature, COpenGLScenePtr & mrpt3dscene) {
VNL::Vector<double> estimateY = feature->get_y();//get_feature_measurement_model()->get_yigraphicsRES();
//VNL::Matrix<double> matrixPyy = feature->get_feature_measurement_model()->get_PyiyigraphicsRES();
VNL::Matrix<double> matrixPyy = feature->get_Pyy();
CEllipsoidPtr objEllip = CEllipsoid::Create();
objEllip->setLocation(estimateY.Get(0), estimateY.Get(1), estimateY.Get(2));
// CMatrixDouble matrix;
// CMatrixDouble66 COV;
CMatrixDouble33 COV3;
int size=3;
for (int i = 0; i < size; i++) {
for (int j = 0; j < size; j++) {
COV3(i, j) = matrixPyy.Get(i, j);
//cout << (COV3) (j, i) << " ";
}
//cout << endl;
}
objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
//int size = 3;
//cout << "Size: " << b->Rows() << "-" << b->Cols() << endl;
// matrix.resize(size, size);
// for (int i = 0; i < size; i++) {
// for (int j = 0; j < size; j++) {
// (matrix) (j, i) = feature->get_Pxy().Get(j, i);
// cout << (matrix) (j, i) << " ";
// }
// cout << endl;
// }
// cout << endl;
//convertMatrix(&matrix, &feature->get_Pxy());
// objEllip->setCovMatrix(matrix, 3);
//objEllip->setCovMatrix(COV3, COV3(2, 2) == 0 ? 2 : 3);
objEllip->setColor(0, 1, 1);
objEllip->enableDrawSolid3D(true);
mrpt3dscene->insert(objEllip);
}
/** Returns a representation of a the PDF - this is just an auxiliary function, it's more natural to call
* mrpt::poses::CPointPDF::getAs3DObject */
CSetOfObjectsPtr CSetOfObjects::posePDF2opengl(const CPointPDF &o)
{
CSetOfObjectsPtr outObj = CSetOfObjects::Create();
if (IS_CLASS(&o,CPointPDFSOG))
{
const CPointPDFSOG *p = static_cast<const CPointPDFSOG*>(&o);
// For each gaussian node
for (CPointPDFSOG::CListGaussianModes::const_iterator it = p->begin(); it!= p->end();++it)
{
opengl::CEllipsoidPtr obj = opengl::CEllipsoid::Create();
obj->setPose( it->val.mean);
obj->setCovMatrix(it->val.cov, it->val.cov(2,2)==0 ? 2:3);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
obj->setColor(POINT_COLOR);
outObj->insert( obj );
} // end for each gaussian node
}
else if (IS_CLASS(&o, CPointPDFGaussian))
{
const CPointPDFGaussian *p = static_cast<const CPointPDFGaussian*>(&o);
CEllipsoidPtr obj = CEllipsoid::Create();
obj->setLocation(p->mean);
obj->setCovMatrix(p->cov, p->cov(2,2)==0 ? 2:3);
obj->setColor(POINT_COLOR);
obj->setQuantiles(3);
obj->enableDrawSolid3D(false);
outObj->insert( obj );
}
else if (IS_CLASS(&o, CPointPDFParticles))
{
const CPointPDFParticles *p = static_cast<const CPointPDFParticles*>(&o);
mrpt::opengl::CPointCloudPtr obj = mrpt::opengl::CPointCloud::Create();
const size_t N=p->size();
obj->resize(N);
obj->setColor(POINT_COLOR);
for (size_t i=0;i<N;i++)
obj->setPoint(
i,
p->m_particles[i].d->x,
p->m_particles[i].d->y,
p->m_particles[i].d->z );
outObj->insert( obj );
}
return outObj;
}
void CDifodoDatasets::updateScene()
{
if ( !visualize_results )
return;
CPose3D rel_lenspose(0,-0.022,0,0,0,0);
scene = window->get3DSceneAndLock();
//Reference gt
CSetOfObjectsPtr reference_gt = scene->getByClass<CSetOfObjects>(0);
reference_gt->setPose(global_pose);
//Camera points
for (unsigned int c=0; c<NC; c++)
{
CPointCloudColouredPtr cam_points = scene->getByClass<CPointCloudColoured>(c);
cam_points->clear();
cam_points->setPose(global_pose + cam_pose[c]);
for (unsigned int y=0; y<cols; y++)
for (unsigned int z=0; z<rows; z++)
cam_points->push_back(depth[c][repr_level](z,y), xx[c][repr_level](z,y), yy[c][repr_level](z,y),
// 0.f, 0.f, 1.f);
1.f-sqrt(weights[c](z,y)), sqrt(weights[c](z,y)), 0);
//DifOdo camera
CBoxPtr camera_odo = scene->getByClass<CBox>(c);
camera_odo->setPose(global_pose + cam_pose[c] + rel_lenspose);
//Frustum
CFrustumPtr FOV = scene->getByClass<CFrustum>(c);
FOV->setPose(global_pose + cam_pose[c]);
}
//Global traj lines
CSetOfLinesPtr traj_lines_odo = scene->getByClass<CSetOfLines>(0);
traj_lines_odo->appendLine(global_oldpose.x(), global_oldpose.y(), global_oldpose.z(), global_pose.x(), global_pose.y(), global_pose.z());
//Global traj points
CPointCloudPtr traj_points_odo = scene->getByClass<CPointCloud>(0);
traj_points_odo->insertPoint(global_pose.x(), global_pose.y(), global_pose.z());
////Cam traj lines
//CPose3D new_cam_pose = global_pose + cam_pose[0], old_cam_pose = global_oldpose + cam_pose[0];
// CSetOfLinesPtr traj_lines_cam = scene->getByClass<CSetOfLines>(1);
// traj_lines_cam->appendLine(old_cam_pose.x(), old_cam_pose.y(), old_cam_pose.z(), new_cam_pose.x(), new_cam_pose.y(), new_cam_pose.z());
// //Cam traj points
// CPointCloudPtr traj_points_cam = scene->getByClass<CPointCloud>(1);
// traj_points_cam->insertPoint(new_cam_pose.x(), new_cam_pose.y(), new_cam_pose.z());
//Ellipsoid showing covariance
math::CMatrixFloat33 cov3d = 20.f*est_cov.topLeftCorner(3,3);
CEllipsoidPtr ellip = scene->getByClass<CEllipsoid>(0);
ellip->setCovMatrix(cov3d);
ellip->setPose(global_pose + rel_lenspose);
window->unlockAccess3DScene();
window->repaint();
}
void CDifodoDatasets::initializeScene()
{
if ( !visualize_results )
return;
window = mrpt::gui::CDisplayWindow3DPtr( new mrpt::gui::CDisplayWindow3D("DIFODO") );
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)
CGridPlaneXYPtr ground = CGridPlaneXY::Create();
scene->insert( ground );
//Reference
CSetOfObjectsPtr reference = stock_objects::CornerXYZ();
scene->insert( reference );
// Cameras and points
//------------------------------------------------------
//Cameras
for (unsigned int c=0; c<NC; c++)
{
CBoxPtr camera_odo = CBox::Create(math::TPoint3D(-0.02,-0.1,-0.01),math::TPoint3D(0.02,0.1,0.01));
camera_odo->setPose(cam_pose[c] + rel_lenspose);
camera_odo->setColor(0,1,0);
scene->insert( camera_odo );
//Frustum
opengl::CFrustumPtr FOV = opengl::CFrustum::Create(0.3, 2, 57.3*fovh, 57.3*fovv, 1.f, true, false);
FOV->setColor(0.7,0.7,0.7);
FOV->setPose(cam_pose[c]);
scene->insert( FOV );
//Camera points
CPointCloudColouredPtr cam_points = CPointCloudColoured::Create();
cam_points->setColor(1,0,0);
cam_points->setPointSize(2);
cam_points->enablePointSmooth(1);
cam_points->setPose(cam_pose[c]);
scene->insert( cam_points );
}
// Trajectories and covariance
//-------------------------------------------------------------
//Trajectory of the global reference frame
CSetOfLinesPtr traj_lines_odo = CSetOfLines::Create();
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 );
CPointCloudPtr traj_points_odo = CPointCloud::Create();
traj_points_odo->setColor(0,0.6,0);
traj_points_odo->setPointSize(4);
traj_points_odo->enablePointSmooth(1);
scene->insert( traj_points_odo );
//Trajectory of one camera
CSetOfLinesPtr traj_lines_cam = CSetOfLines::Create();
traj_lines_cam->setLocation(0,0,0);
traj_lines_cam->setColor(0.6,0,0);
traj_lines_cam->setLineWidth(6);
scene->insert( traj_lines_cam );
CPointCloudPtr traj_points_cam = CPointCloud::Create();
traj_points_cam->setColor(0.6,0,0);
traj_points_cam->setPointSize(4);
traj_points_cam->enablePointSmooth(1);
scene->insert( traj_points_cam );
//Ellipsoid showing covariance
math::CMatrixFloat33 cov3d = 20.f*est_cov.topLeftCorner(3,3);
CEllipsoidPtr ellip = CEllipsoid::Create();
ellip->setCovMatrix(cov3d);
ellip->setQuantiles(2.0);
ellip->setColor(1.0, 1.0, 1.0, 0.5);
ellip->enableDrawSolid3D(true);
ellip->setPose(global_pose);
scene->insert( ellip );
//User-interface information
utils::CImage img_legend;
img_legend.loadFromXPM(legend_xpm);
COpenGLViewportPtr legend = scene->createViewport("legend");
legend->setViewportPosition(20, 20, 332, 164);
legend->setImageView(img_legend);
window->unlockAccess3DScene();
window->repaint();
}
