/*---------------------------------------------------------------
Set the scene camera
---------------------------------------------------------------*/
void CFBORender::setCamera( const COpenGLScene& scene, const CCamera& camera )
{
MRPT_START
scene.getViewport("main")->getCamera() = camera;
MRPT_END
}
// COpenGLScene
void COpenGLScene_insert(COpenGLScene &self, const CRenderizablePtr &newObject, const std::string &viewportName=std::string("main"))
{
self.insert(newObject, viewportName);
}
/*---------------------------------------------------------------
CHMTSLAM_LOG
Implements a 2D local SLAM method based on scan matching
between near observations and an EKF. A part of HMT-SLAM
\param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
\param act The action to process (or NULL).
\param sf The observations to process (or NULL).
--------------------------------------------------------------- */
void CHMTSLAM::generateLogFiles(unsigned int nIteration)
{
MRPT_START
// Speed up the storage of images (in opengl::CTexturedPlane's):
//CImage::DISABLE_ZIP_COMPRESSION = true;
static CTicTac tictac;
tictac.Tic();
THypothesisID bestHypoID;
CLocalMetricHypothesis *bestLMH = NULL;
{
CCriticalSectionLocker locker( &m_LMHs_cs );
printf_debug("[LOG] Number of LMHs: %u\n", (unsigned int)m_LMHs.size() );
// Generate 3D view of local areas:
{
string filLocalAreas = format("%s/LSLAM_3D/mostLikelyLMH_LSLAM_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
COpenGLScenePtr sceneLSLAM = COpenGLScene::Create();
// Look for the most likely LMH:
aligned_containers<THypothesisID, CLocalMetricHypothesis>::map_t::iterator it;
for ( it = m_LMHs.begin();it!=m_LMHs.end();it++)
{
if (!bestLMH)
{
bestLMH = & it->second;
}
else if ( it->second.m_log_w > bestLMH->m_log_w)
{
bestLMH = & it->second;
}
}
ASSERT_(bestLMH!=NULL)
bestHypoID = bestLMH->m_ID;
{
CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
{
// Generate the metric maps 3D view...
opengl::CSetOfObjectsPtr maps3D = opengl::CSetOfObjects::Create();
maps3D->setName("metric-maps");
bestLMH->getMostLikelyParticle()->d->metricMaps.getAs3DObject( maps3D );
sceneLSLAM->insert( maps3D );
// ...and the robot poses, areas, etc:
opengl::CSetOfObjectsPtr LSLAM_3D = opengl::CSetOfObjects::Create();
LSLAM_3D->setName("LSLAM_3D");
bestLMH->getAs3DScene( LSLAM_3D );
sceneLSLAM->insert( LSLAM_3D );
sceneLSLAM->enableFollowCamera(true);
printf_debug("[LOG] Saving %s\n", filLocalAreas.c_str());
CFileGZOutputStream(filLocalAreas) << *sceneLSLAM;
}
// Save the SSO matrix:
#if 0
{
CCriticalSectionLocker locker( &bestLMH->m_robotPosesGraph.lock );
string filSSO = format("%s/ASSO/mostLikelyLMH_ASSO_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration );
COpenGLScene sceneSSO;
opengl::CSetOfObjectsPtr sso3D = opengl::CSetOfObjects::Create();
bestLMH->m_robotPosesGraph.partitioner.getAs3DScene( sso3D, &bestLMH->m_robotPosesGraph.idx2pose );
sceneSSO.insert(sso3D);
CFileGZOutputStream(filSSO) << sceneSSO;
if (1)
{
CMatrix A;
bestLMH->m_robotPosesGraph.partitioner.getAdjacencyMatrix( A );
if (A.getColCount()>0)
{
A.adjustRange();
A.saveToTextFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.txt", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
CImage(A,true).saveToFile( format("%s/ASSO/mostLikelyLMH_ASSO_%05u.png", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
}
}
} // end lock partitioner's CS
#endif
} // end LMH's lock
}
} // end of lock on LMHs_cs
#if 1
{
// Save the whole HMT-SLAM state to a dump file
static int CNT = 0;
if ((CNT++ % 20) == 0)
{
string hmtmap_file( format("%s/HMTSLAM_state/state_%05u.hmtslam", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
printf_debug("[LOG] Saving %s\n", hmtmap_file.c_str());
CFileGZOutputStream(hmtmap_file) << *this;
}
}
#endif
#if 1
{
// Update the poses-graph in the HMT-map from the LMH to draw it:
static int CNT = 0;
if ((CNT++ % 5) == 0)
{
CCriticalSectionLocker lockerLMH( &bestLMH->m_lock );
for (TNodeIDSet::const_iterator n = bestLMH->m_neighbors.begin();n!=bestLMH->m_neighbors.end();++n)
bestLMH->updateAreaFromLMH( *n );
// Save global map for most likely hypothesis:
COpenGLScene sceneGlobalHMTMAP;
{
CCriticalSectionLocker locker( &m_map_cs );
printf_debug("[LOG] HMT-map: %u nodes/ %u arcs\n", (unsigned int)m_map.nodeCount(), (unsigned int)m_map.arcCount() );
m_map.getAs3DScene(
sceneGlobalHMTMAP, // Scene
m_map.getFirstNode()->getID(), // Reference node
bestHypoID, // Hypothesis to get
3 // iterations
);
}
string hmtmap_file( format("%s/HMAP_3D/mostLikelyHMT_MAP_%05u.3Dscene", m_options.LOG_OUTPUT_DIR.c_str(), nIteration ) );
printf_debug("[LOG] Saving %s\n", hmtmap_file.c_str());
CFileGZOutputStream(hmtmap_file) << sceneGlobalHMTMAP;
}
}
#endif
// Save the memory usage:
unsigned long memUsage = mrpt::system::getMemoryUsage();
FILE *f=os::fopen( format("%s/log_MemoryUsage.txt",m_options.LOG_OUTPUT_DIR.c_str()).c_str() ,"at");
if (f)
{
os::fprintf(f,"%u\t%f\n",nIteration,memUsage/(1024.0*1024.0));
os::fclose(f);
}
double t_log = tictac.Tac();
printf_debug("[LOG] Time for logging: %f ms\n", 1000*t_log);
MRPT_END
}
// ------------------------------------------------------
// TestDisplay3D
// ------------------------------------------------------
void TestDisplay3D()
{
COpenGLScene scene;
// Modify the scene:
// ------------------------------------------------------
{
opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-20,20,-20,20,0,1);
obj->setColor(0.4,0.4,0.4);
scene.insert( obj );
}
{
opengl::CAxisPtr obj = opengl::CAxis::Create();
obj->setFrequency(5);
obj->enableTickMarks();
obj->setAxisLimits(-10,-10,-10, 10,10,10);
scene.insert( obj );
}
{
opengl::CSpherePtr obj = opengl::CSphere::Create();
obj->setColor(0,0,1);
obj->setRadius(0.3);
obj->setLocation(0,0,1);
obj->setName( "ball_1" );
scene.insert( obj );
}
{
opengl::CSpherePtr obj = opengl::CSphere::Create();
obj->setColor(1,0,0);
obj->setRadius(0.3);
obj->setLocation(-1,-1,1);
obj->setName( "ball_2");
scene.insert( obj );
}
CDisplayWindow win("output");
int c = 0, width = 640, height = 480;
CFBORender render(width, height);
CImage frame(width, height, 3, false);
{
CCamera& camera = render.getCamera(scene);
camera.setZoomDistance(50);
}
while (!mrpt::system::os::kbhit())
{
CRenderizablePtr obj = scene.getByName("ball_1");
obj->setLocation(
obj->getPoseX() + cos(obj->getPoseY()/2)*0.05,
obj->getPoseY() - sin(obj->getPoseX()/2)*0.09,
obj->getPoseZ() - sin(obj->getPoseX()/2)*0.08 );
obj = scene.getByName("ball_2");
obj->setLocation(
obj->getPoseX() + cos(obj->getPoseY()/2)*0.05,
obj->getPoseY() - sin(obj->getPoseX()/2)*0.09,
obj->getPoseZ() - sin(obj->getPoseX()/2)*0.08 );
// change the size
if(++c > 100)
{
width = 800, height = 600;
frame.resize(width, height, 3, false);
render.resize(width, height);
}
// render the scene
render.getFrame2(scene, frame);
// show the redered image
win.showImage(frame);
mrpt::system::sleep(50);
}
}
// Show GPS path in a window:
void xRawLogViewerFrame::OnMenuDrawGPSPath(wxCommandEvent& event)
{
WX_START_TRY
string the_label = AskForObservationByLabel("Choose the GPS to use:");
size_t i, M = 0, n = rawlog.size();
TGeodeticCoords ref;
bool ref_valid = false;
// Ask the user for the reference?
if (wxYES!=wxMessageBox(_("Do you want to take the GPS reference automatically from the first found entry?"),_("GPS path"),wxYES_NO ))
{
wxString s = wxGetTextFromUser(
_("Reference Latitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.lat.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Longitude (degrees):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.lon.decimal_value)) { wxMessageBox(_("Invalid number")); return; }
s = wxGetTextFromUser(
_("Reference Height (meters):"),
_("GPS reference"),
_("0.0"), this );
if (s.IsEmpty()) return;
if (!s.ToDouble(&ref.height)) { wxMessageBox(_("Invalid number")); return; }
ref_valid=true;
}
// Only RTK fixed?
bool only_rtk = wxYES==wxMessageBox(_("Take into account 'rtk' (modes 4-5) readings only?"),_("GPS path"),wxYES_NO );
vector<float> xs,ys,zs;
double overall_distance = 0;
for (i=0;i<n;i++)
{
CObservationGPSPtr obs;
switch ( rawlog.getType(i) )
{
case CRawlog::etSensoryFrame:
{
CSensoryFramePtr sf = rawlog.getAsObservations(i);
CObservationPtr o= sf->getObservationBySensorLabel(the_label);
if (o && IS_CLASS(o,CObservationGPS))
{
obs = CObservationGPSPtr(o);
}
}
break;
case CRawlog::etObservation:
{
CObservationPtr o = rawlog.getAsObservation(i);
if ( !os::_strcmpi(o->sensorLabel.c_str(), the_label.c_str()) && IS_CLASS(o,CObservationGPS))
{
obs = CObservationGPSPtr(o);
}
}
break;
default: break;
}
// If we had a GPS obs, process it:
if (obs && obs->has_GGA_datum && (!only_rtk || obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5) )
{
TPoint3D X_ENU; // Transformed coordinates
const TGeodeticCoords obsCoords = obs->GGA_datum.getAsStruct<TGeodeticCoords>();
// The first gps datum?
if (!ref_valid)
{
ref_valid=true;
ref = obsCoords;
}
// Local XYZ coordinates transform:
geodeticToENU_WGS84( obsCoords, X_ENU, ref );
// Geocentric XYZ:
TPoint3D X_geo;
geodeticToGeocentric_WGS84( obsCoords, X_geo);
if (!xs.empty())
overall_distance+=sqrt( square(X_ENU.x-*xs.rbegin())+square(X_ENU.y-*ys.rbegin())+square(X_ENU.z-*zs.rbegin()) );
xs.push_back(X_ENU.x);
ys.push_back(X_ENU.y);
zs.push_back(X_ENU.z);
M++;
}
}
// Window 3d:
winGPSPath = CDisplayWindow3D::Create(format("GPS path, %i points (%s) %.03f meters length", int(M), the_label.c_str(), overall_distance ) );
COpenGLScene scene;
CPointCloudPtr gl_path = CPointCloud::Create();
gl_path->setAllPoints(xs,ys,zs);
gl_path->setColor(0,0,1);
gl_path->setPointSize(3);
scene.insert( gl_path );
scene.insert( CGridPlaneXYPtr( CGridPlaneXY::Create(-300,300,-300,300,0,10)));
scene.insert( CAxisPtr( CAxis::Create(-300,-300,-50, 300,300,50, 1.0, 3, true ) ) );
COpenGLScenePtr the_scene = winGPSPath->get3DSceneAndLock();
*the_scene = scene;
winGPSPath->unlockAccess3DScene();
winGPSPath->repaint();
// 2D wins:
winGPSPath2D_xy = CDisplayWindowPlots::Create( format("GPS path - XY (%s)", the_label.c_str() ) );
winGPSPath2D_xy->plot(xs,ys,"b");
winGPSPath2D_xy->axis_fit(true);
winGPSPath2D_xz = CDisplayWindowPlots::Create( format("GPS path - XZ (%s)", the_label.c_str() ) );
winGPSPath2D_xz->plot(xs,zs,"b");
winGPSPath2D_xz->axis_fit(true);
WX_END_TRY
}