void navlog_viewer_GUI_designDialog::loadLogfile(const std::string &filName)
{
WX_START_TRY
this->edLogFile->SetValue(_U(filName.c_str()));
CFileInputStream f(filName);
m_logdata.clear();
wxBusyCursor busy;
set<string> validClasses;
validClasses.insert("CLogFileRecord");
m_log_first_tim = INVALID_TIMESTAMP;
m_log_last_tim = INVALID_TIMESTAMP;
for (;;)
{
try
{
CSerializablePtr obj = f.ReadObject();
if (validClasses.find(string(obj->GetRuntimeClass()->className))==validClasses.end())
{
wxMessageBox(_U(format("Unexpected class found: %s",obj->GetRuntimeClass()->className).c_str()),_("Error loading log:"));
break;
}
m_logdata.push_back(obj);
// generate time stats:
if (IS_CLASS(obj,CLogFileRecord))
{
const CLogFileRecordPtr logptr = CLogFileRecordPtr(obj);
if (logptr->timestamp!=INVALID_TIMESTAMP)
m_log_last_tim = logptr->timestamp;
}
if (m_log_first_tim == INVALID_TIMESTAMP && m_log_last_tim!=INVALID_TIMESTAMP)
m_log_first_tim = m_log_last_tim;
}
catch (CExceptionEOF &)
{
break;
}
catch (std::exception &)
{
// EOF in the middle of an object... It may be usual if the logger is shut down not cleanly.
break;
}
}
// Update stats, etc...
UpdateInfoFromLoadedLog();
WX_END_TRY
}
/* -----------------------------------------------------------------------
Used to pass CORBA-like object into a MRPT object.
See doc about "Integration with BABEL".
----------------------------------------------------------------------- */
void utils::StringToObject(const std::string &str, CSerializablePtr &obj)
{
MRPT_START
obj.clear_unique();
if (str.empty()) return;
CMemoryStream tmp;
size_t n;
size_t i,lastIdx;
obj.clear_unique();
n = str.size();
// Scan the string to decode it:
// ----------------------------------
lastIdx = 0;
const char *data = str.c_str();
unsigned char c;
for (i=0;i<n && (c=data[i])!=0;i++)
{
// Search for first "0x01" byte:
if ( c == 0x01 )
{
// Copy all till now:
tmp.WriteBuffer( &data[lastIdx], i - lastIdx + 1 );
i+=1; // +1 from "for" loop
lastIdx = i+1;
// And decode:
// 0x01 0x01 --> 0x01
// 0x01 0x02 --> 0x00
if (data[i]==0x01)
((unsigned char*)tmp.getRawBufferData())[tmp.getTotalBytesCount()-1] = (unsigned char)0x01;
else ((unsigned char*)tmp.getRawBufferData())[tmp.getTotalBytesCount()-1] = (unsigned char)0x00;
}
} // end for i
// Copy the rest:
if ( (n-lastIdx) > 0)
tmp.WriteBuffer( &data[lastIdx], n - lastIdx );
// And the '\0' char:
char dummy = '\0';
tmp.WriteBuffer( &dummy, sizeof(char) );
tmp.Seek(0,CStream::sFromBeginning);
obj = tmp.ReadObject();
MRPT_END
}
/*---------------------------------------------------------------
deserializeIntoNewObject
---------------------------------------------------------------*/
void CMessage::deserializeIntoNewObject( CSerializablePtr &obj )
{
MRPT_START
CMemoryStream auxStream;
// Copy data into the stream:
if (!content.empty())
{
auxStream.WriteBuffer( &content[0], content.size() );
auxStream.Seek(0);
// Try to parse data into a new object:
obj = auxStream.ReadObject( );
}
else obj.clear_unique();
MRPT_END
}
/* -----------------------------------------------------------------------
RawStringToObject
----------------------------------------------------------------------- */
void utils::RawStringToObject(const std::string & in_data, CSerializablePtr &obj)
{
try
{
obj.clear_unique();
if (in_data.empty()) return;
CMemoryStream tmp( &in_data[0], in_data.size());
obj = tmp.ReadObject();
}
catch (std::bad_alloc &e)
{
throw e;
}
catch(std::exception &e)
{
std::cerr << "[OctetVectorToObject] Exception: " << e.what() << std::endl;
}
catch(...)
{
std::cerr << "[OctetVectorToObject] Unknown exception" << std::endl;
}
}
void hmt_slam_guiFrame::updateLocalMapView()
{
WX_START_TRY
m_glLocalArea->m_openGLScene->clear();
// Get the hypothesis ID:
THypothesisID hypID = (THypothesisID )atoi( cbHypos->GetStringSelection().mb_str() );
if ( m_hmtslam->m_LMHs.find(hypID)==m_hmtslam->m_LMHs.end() )
{
wxMessageBox(_U( format("No LMH has hypothesis ID %i!", (int)hypID).c_str() ), _("Error with topological hypotesis"));
return;
}
// Get the selected area or LMH in the tree view:
wxArrayTreeItemIds lstSelect;
size_t nSel = treeView->GetSelections(lstSelect);
if (!nSel) return;
CItemData *data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(0) ) );
if (!data1) return;
if (!data1->m_ptr) return;
CSerializablePtr obj = data1->m_ptr;
if (obj->GetRuntimeClass()==CLASS_ID(CHMHMapNode))
{
// The 3D view:
opengl::CSetOfObjectsPtr objs = opengl::CSetOfObjects::Create();
// -------------------------------------------
// Draw a grid on the ground:
// -------------------------------------------
{
opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-100,100,-100,100,0,5);
obj->setColor(0.4,0.4,0.4);
objs->insert(obj); // it will free the memory
}
// Two passes: 1st draw the map on the ground, then the rest.
for (int nRound=0;nRound<2;nRound++)
{
CHMHMapNodePtr firstArea;
CPose3DPDFGaussian refPoseThisArea;
for (size_t nSelItem = 0; nSelItem<nSel;nSelItem++)
{
CItemData *data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(nSelItem) ) );
if (!data1) continue;
if (!data1->m_ptr) continue;
CHMHMapNodePtr area= CHMHMapNodePtr(data1->m_ptr);
if (!area) continue;
// Is this the first rendered area??
if ( !firstArea )
{
firstArea = area;
}
else
{
// Compute the translation btw. ref. and current area:
CPose3DPDFParticles pdf;
m_hmtslam->m_map.computeCoordinatesTransformationBetweenNodes(
firstArea->getID(),
area->getID(),
pdf,
hypID,
200 );
/*0.15f,
DEG2RAD(5.0f) );*/
refPoseThisArea.copyFrom( pdf );
cout << "Pose " << firstArea->getID() << " - " << area->getID() << refPoseThisArea << endl;
}
CMultiMetricMapPtr obj_mmap = area->m_annotations.getAs<CMultiMetricMap>( NODE_ANNOTATION_METRIC_MAPS, hypID, false );
CRobotPosesGraphPtr obj_robposes = area->m_annotations.getAs<CRobotPosesGraph>( NODE_ANNOTATION_POSES_GRAPH, hypID, false );
TPoseID refPoseID;
area->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, refPoseID, hypID, true);
// ---------------------------------------------------------
// The metric map:
// ---------------------------------------------------------
if (nRound==0)
{
opengl::CSetOfObjectsPtr objMap= opengl::CSetOfObjects::Create();
obj_mmap->getAs3DObject(objMap);
objMap->setPose( refPoseThisArea.mean );
objs->insert(objMap);
}
if (nRound==1)
{
// ---------------------------------------------------------
// Bounding boxes for grid maps:
// ---------------------------------------------------------
if (obj_mmap->m_gridMaps.size())
{
float x_min = obj_mmap->m_gridMaps[0]->getXMin();
float x_max = obj_mmap->m_gridMaps[0]->getXMax();
float y_min = obj_mmap->m_gridMaps[0]->getYMin();
float y_max = obj_mmap->m_gridMaps[0]->getYMax();
opengl::CSetOfLinesPtr objBB = opengl::CSetOfLines::Create();
objBB->setColor(0,0,1);
objBB->setLineWidth( 4.0f );
objBB->appendLine(x_min,y_min,0, x_max,y_min,0);
objBB->appendLine(x_max,y_min,0, x_max,y_max,0);
objBB->appendLine(x_max,y_max,0, x_min,y_max,0);
objBB->appendLine(x_min,y_max,0, x_min,y_min,0);
objBB->setPose( refPoseThisArea.mean );
objs->insert(objBB);
}
// -----------------------------------------------
// Draw a 3D coordinates corner for the ref. pose
// -----------------------------------------------
{
CPose3D p;
(*obj_robposes)[refPoseID].pdf.getMean(p);
opengl::CSetOfObjectsPtr corner = stock_objects::CornerXYZ();
corner->setPose( refPoseThisArea.mean + p);
corner->setName(format("AREA %i",(int)area->getID() ));
corner->enableShowName();
objs->insert( corner );
}
// -----------------------------------------------
// Draw ellipsoid with uncertainty of pose transformation
// -----------------------------------------------
if (refPoseThisArea.cov(0,0)!=0 || refPoseThisArea.cov(1,1)!=0)
{
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
ellip->setPose( refPoseThisArea.mean );
ellip->enableDrawSolid3D(false);
CMatrixDouble C = CMatrixDouble(refPoseThisArea.cov);
if (C(2,2)<1e6)
C.setSize(2,2);
else C.setSize(3,3);
ellip->setCovMatrix(C);
ellip->setQuantiles(3);
ellip->setLocation( ellip->getPoseX(), ellip->getPoseY(), ellip->getPoseZ()+0.5);
ellip->setColor(1,0,0);
ellip->setLineWidth(3);
objs->insert( ellip );
}
// ---------------------------------------------------------
// Draw each of the robot poses as 2D/3D ellipsoids
// ---------------------------------------------------------
for (CRobotPosesGraph::iterator it=obj_robposes->begin();it!=obj_robposes->end();++it)
{
}
}
} // end for nSelItem
} // two pass
// Add to the scene:
m_glLocalArea->m_openGLScene->insert(objs);
}
else
if (obj->GetRuntimeClass()==CLASS_ID( CLocalMetricHypothesis ))
{
//CLocalMetricHypothesis *lmh = static_cast<CLocalMetricHypothesis*>( obj );
}
m_glLocalArea->Refresh();
WX_END_TRY
}
/** This is the common function for all operations over a rawlog file ("filter" a rawlog file into a new one) or over the loaded rawlog (depending on the user selection in the GUI).
*/
void CFormChangeSensorPositions::executeOperationOnRawlog( TRawlogFilter operation, const char *endMsg )
{
WX_START_TRY
int processMax;
bool isInMemory;
CStream *in_fil=NULL,*out_fil=NULL;
sensorPoseReadOK = false;
camReadIsOk = false;
if (rbLoaded->GetValue())
{
// APPLY TO rawlog in memory:
isInMemory = true;
processMax = (int)rawlog.size();
}
else
{
// APPLY TO rawlog files:
isInMemory = false;
if ( !txtInputFile->GetValue().size() )
THROW_EXCEPTION("An input rawlog file must be selected")
if ( !txtOutputFile->GetValue().size() )
THROW_EXCEPTION("An output rawlog file must be selected")
string fileName_IN( txtInputFile->GetValue().mbc_str() );
if (!mrpt::system::fileExists(fileName_IN) )
THROW_EXCEPTION("Input file does not exist!")
string fileName_OUT( txtOutputFile->GetValue().mbc_str() );
if (!fileName_OUT.compare(fileName_IN))
THROW_EXCEPTION("Input and output files must be different!")
in_fil = new CFileGZInputStream(fileName_IN);
out_fil = new CFileGZOutputStream(fileName_OUT);
processMax = (int)in_fil->getTotalBytesCount();
}
wxProgressDialog progDia(
wxT("Modifying rawlog"),
wxT("Processing..."),
processMax, // range
this, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
unsigned int countLoop = 0;
bool keepLoading=true;
string errorMsg;
wxString auxStr;
// Apply changes:
int changes = 0;
wxBusyCursor cursor;
while ((( !isInMemory && keepLoading ) ||
( isInMemory && countLoop < rawlog.size() ))&& !sensorPoseReadOK && !camReadIsOk )
{
CSerializablePtr newObj;
try
{
if (isInMemory)
{
newObj = rawlog.getAsGeneric(countLoop);
}
else
{
(*in_fil) >> newObj;
}
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
{
// A sensory frame:
CSensoryFramePtr sf(newObj);
// Process & save:
operation(NULL,sf.pointer(),changes );
if (!isInMemory) (*out_fil) << *sf.pointer();
}
else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
{
// This is an action:
CActionCollectionPtr acts =CActionCollectionPtr( newObj );
// Process & save:
operation( (CActionCollection*)acts.pointer(),NULL,changes);
if (!isInMemory) (*out_fil) << *acts;
}
else if ( newObj->GetRuntimeClass()->derivedFrom(CLASS_ID(CObservation)))
{
// A sensory frame:
CObservationPtr o(newObj);
static CSensoryFrame sf;
sf.clear();
sf.insert(o);
// Process & save:
operation(NULL,&sf,changes );
if (!isInMemory) (*out_fil) << *o;
}
else
{ // Unknown class:
THROW_EXCEPTION(format("Unexpected class found in the file: '%s'",newObj->GetRuntimeClass()->className) );
}
}
catch (exception &e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
// Step counter & update progress dialog
if (countLoop++ % 300 == 0)
{
auxStr.sprintf(wxT("Processing... (%u objects processed)"),countLoop);
int curProgr = isInMemory ? countLoop : (int)in_fil->getPosition();
if (!progDia.Update( curProgr , auxStr ))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
// Delete only if processing file
if (newObj && !isInMemory)
{
newObj.clear();
}
} // end while keep loading
progDia.Update( processMax ); // Close dialog.
if (strlen(endMsg))
{
char tmpStr[1000];
os::sprintf(tmpStr,sizeof(tmpStr),"%s %i\n\nEnd message:\n%s", endMsg, changes, errorMsg.c_str() );
wxMessageBox( _U(tmpStr), _("Result:"), wxOK,this);
}
if (in_fil) delete in_fil;
if (out_fil) delete out_fil;
WX_END_TRY
}
void CStream_WriteObject(CStream& self, CSerializablePtr& obj)
{
self.WriteObject(obj.pointer());
}
// CStream
void CStream_ReadObject(CStream& self, CSerializablePtr& obj)
{
self.ReadObject(obj.pointer());
}
void xRawLogViewerFrame::OnMenuConvertObservationOnly(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox( _("Select the rawlog file to convert...") );
string str;
if ( !AskForOpenRawlog( str ) ) return;
wxMessageBox( _("Select the target file where to save the new rawlog.") );
string filToSave;
if ( !AskForSaveRawlog( filToSave ) ) return;
wxBusyCursor waitCursor;
CFileGZInputStream fil(str);
unsigned int filSize = (unsigned int)fil.getTotalBytesCount();
CFileGZOutputStream f_out(filToSave);
wxString auxStr;
wxProgressDialog progDia(
wxT("Progress"),
wxT("Parsing file..."),
filSize, // range
this, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
unsigned int countLoop = 0;
bool keepLoading=true;
string errorMsg;
CPose2D odometry_accum;
// We'll save here all the individual observations ordered in time:
TListTimeAndObservations time_ordered_list_observation;
mrpt::system::TTimeStamp lastValidObsTime = INVALID_TIMESTAMP;
while (keepLoading)
{
if (countLoop++ % 5 == 0)
{
auxStr.sprintf(wxT("Parsing file... %u objects"),countLoop );
if (!progDia.Update( (int)fil.getPosition(), auxStr ))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
try
{
CSerializablePtr newObj;
fil >> newObj;
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
{
CSensoryFramePtr SF(newObj);
for (CSensoryFrame::iterator it=SF->begin();it!=SF->end();++it)
{
time_ordered_list_observation.insert( TTimeObservationPair( (*it)->timestamp, (*it) ));
lastValidObsTime = (*it)->timestamp;
}
}
else
if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) )
{
// Replace "odometry action" with "odometry observation":
CActionCollectionPtr acts = CActionCollectionPtr( newObj );
// Get odometry:
CActionRobotMovement2DPtr actOdom = acts->getBestMovementEstimation();
if (actOdom)
{
odometry_accum = odometry_accum + actOdom->poseChange->getMeanVal();
// Generate "odometry obs":
CObservationOdometryPtr newO = CObservationOdometry::Create();
newO->sensorLabel = "odometry";
newO->timestamp = actOdom->timestamp!=INVALID_TIMESTAMP ? actOdom->timestamp : lastValidObsTime;
newO->odometry = odometry_accum;
time_ordered_list_observation.insert( TTimeObservationPair( newO->timestamp, newO ));
}
}
else
if ( newObj->GetRuntimeClass()->derivedFrom( CLASS_ID(CObservation) ) )
{
CObservationPtr o = CObservationPtr( newObj );
time_ordered_list_observation.insert( TTimeObservationPair( o->timestamp, o ));
}
// Dump to the new file: Only the oldest one:
// --------------------------------------------------
if (time_ordered_list_observation.size()>30)
{
// Save a few of the oldest and continue:
for (unsigned i=0;i<15;i++)
{
f_out << *(time_ordered_list_observation.begin()->second);
time_ordered_list_observation.erase( time_ordered_list_observation.begin() );
}
}
// Free memory:
newObj.clear_unique();
}
catch (exception &e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
} // end while keep loading
// Save the rest to the out file:
while (!time_ordered_list_observation.empty())
{
f_out << *(time_ordered_list_observation.begin()->second);
time_ordered_list_observation.erase( time_ordered_list_observation.begin() );
}
progDia.Update( filSize );
// Set error msg:
wxMessageBox(_("Done"));
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuConvertExternallyStored(wxCommandEvent& event)
{
WX_START_TRY
wxMessageBox( _("Select the rawlog file with embedded images.") );
string str;
if ( !AskForOpenRawlog( str ) ) return;
wxMessageBox( _("Select the target file where to save the new rawlog.") );
string filToSave;
if ( !AskForSaveRawlog( filToSave ) ) return;
// Create the default "/Images" directory.
string outDir = extractFileDirectory(filToSave) + string("/") + extractFileName(filToSave) + string("_Images");
if (fileExists(outDir))
{
wxMessageBox(_U(format("*ABORTING*: Output directory for images already exists. Select a different output path or remove the directory:\n%s",outDir.c_str()).c_str()));
return;
}
createDirectory( outDir );
if (!fileExists(outDir))
{
wxMessageBox(_U(format("*ABORTING*: Cannot create directory:\n%s",outDir.c_str()).c_str()));
return;
}
// Add the final /
outDir+="/";
// Let the user choose the image format:
string imgFileExtension = AskForImageFileFormat();
if (imgFileExtension.empty()) return;
wxBusyCursor waitCursor;
CFileGZInputStream fil(str);
unsigned int filSize = (unsigned int)fil.getTotalBytesCount();
CFileGZOutputStream f_out(filToSave);
wxString auxStr;
wxProgressDialog progDia(
wxT("Progress"),
wxT("Parsing file..."),
filSize, // range
this, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
unsigned int countLoop = 0;
int imgSaved = 0;
bool keepLoading=true;
string errorMsg;
while (keepLoading)
{
if (countLoop++ % 5 == 0)
{
auxStr.sprintf(wxT("Parsing file... %u objects"),countLoop );
if (!progDia.Update( (int)fil.getPosition(), auxStr ))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
try
{
CSerializablePtr newObj;
fil >> newObj;
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
{
CSensoryFramePtr SF(newObj);
for (unsigned k=0;k<SF->size();k++)
{
if (SF->getObservationByIndex(k)->GetRuntimeClass()==CLASS_ID(CObservationStereoImages ) )
{
CObservationStereoImagesPtr obsSt = SF->getObservationByIndexAs<CObservationStereoImagesPtr>(k);
// save image to file & convert into external storage:
string fileName = format("img_stereo_%u_left_%05u.%s",k,imgSaved,imgFileExtension.c_str() );
obsSt->imageLeft.saveToFile( outDir + fileName );
obsSt->imageLeft.setExternalStorage( fileName );
imgSaved++;
// save image to file & convert into external storage:
fileName = format("img_stereo_%u_right_%05u.%s",k,imgSaved,imgFileExtension.c_str() );
obsSt->imageRight.saveToFile( outDir + fileName );
obsSt->imageRight.setExternalStorage( fileName );
imgSaved++;
}
if (SF->getObservationByIndex(k)->GetRuntimeClass()==CLASS_ID(CObservationImage ) )
{
CObservationImagePtr obsIm = SF->getObservationByIndexAs<CObservationImagePtr>(k);
// save image to file & convert into external storage:
string fileName = format("img_monocular_%u_%05u.%s",k,imgSaved,imgFileExtension.c_str() );
obsIm->image.saveToFile( outDir + fileName );
obsIm->image.setExternalStorage( fileName );
imgSaved++;
}
}
}
else
if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) ||
newObj->GetRuntimeClass() == CLASS_ID(CPose2D) )
{
}
else
{
// Unknown class:
THROW_EXCEPTION("Unknown class found in the file!");
}
// Dump to the new file:
f_out << *newObj;
// Free memory:
newObj.clear_unique();
}
catch (exception &e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
} // end while keep loading
progDia.Update( filSize );
// Set error msg:
wxMessageBox(_U( format("Images saved: %i",imgSaved).c_str() ),_("Done"),wxOK,this);
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuLossLessDecFILE(wxCommandEvent& event)
{
WX_START_TRY
string filToOpen;
if ( !AskForOpenRawlog( filToOpen ) ) return;
wxString strDecimation = wxGetTextFromUser(
_("The number of observations will be decimated (only 1 out of M will be kept). Enter the decimation ratio M:"),
_("Decimation"),
_("1") );
long DECIMATE_RATIO;
strDecimation.ToLong( &DECIMATE_RATIO );
ASSERT_(DECIMATE_RATIO>=1)
string filToSave;
AskForSaveRawlog(filToSave);
CFileGZInputStream fil(filToOpen);
CFileGZOutputStream f_out(filToSave);
wxBusyCursor waitCursor;
unsigned int filSize = (unsigned int)fil.getTotalBytesCount();
wxString auxStr;
wxProgressDialog progDia(
wxT("Progress"),
wxT("Parsing file..."),
filSize, // range
this, // parent
wxPD_CAN_ABORT |
wxPD_APP_MODAL |
wxPD_SMOOTH |
wxPD_AUTO_HIDE |
wxPD_ELAPSED_TIME |
wxPD_ESTIMATED_TIME |
wxPD_REMAINING_TIME);
wxTheApp->Yield(); // Let the app. process messages
unsigned int countLoop = 0;
int entryIndex = 0;
bool keepLoading=true;
string errorMsg;
// ------------------------------------------------------------------------------
// METHOD TO BE MEMORY EFFICIENT:
// To free the memory of the current rawlog entries as we create the new one,
// then call "clearWithoutDelete" at the end.
// ------------------------------------------------------------------------------
CSensoryFramePtr accum_sf;
CActionRobotMovement2D::TMotionModelOptions odometryOptions;
bool cummMovementInit = false;
long SF_counter = 0;
// Reset cummulative pose change:
CPose2D accumMovement(0,0,0);
TTimeStamp timestamp_lastAction = INVALID_TIMESTAMP;
while (keepLoading)
{
if (countLoop++ % 100 == 0)
{
auxStr.sprintf(wxT("Parsing file... %u objects"),entryIndex );
if (!progDia.Update( (int)fil.getPosition(), auxStr ))
keepLoading = false;
wxTheApp->Yield(); // Let the app. process messages
}
CSerializablePtr newObj;
try
{
fil >> newObj;
entryIndex++;
// Check type:
if ( newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame) )
{
// Decimate Observations
// ---------------------------
// Add observations to the accum. SF:
if (!accum_sf)
accum_sf = CSensoryFrame::Create();
// Copy pointers to observations only (fast):
accum_sf->moveFrom( *CSensoryFramePtr(newObj) );
if ( ++SF_counter >= DECIMATE_RATIO )
{
SF_counter = 0;
// INSERT OBSERVATIONS:
f_out << *accum_sf;
accum_sf.clear_unique();
// INSERT ACTIONS:
CActionCollectionPtr actsCol = CActionCollection::Create();
if (cummMovementInit)
{
CActionRobotMovement2D cummMovement;
cummMovement.computeFromOdometry(accumMovement, odometryOptions );
cummMovement.timestamp = timestamp_lastAction;
actsCol->insert( cummMovement );
// Reset odometry accumulation:
accumMovement = CPose2D(0,0,0);
}
f_out << *actsCol;
actsCol.clear_unique();
}
}
else if ( newObj->GetRuntimeClass() == CLASS_ID(CActionCollection) )
{
// Accumulate Actions
// ----------------------
CActionCollectionPtr curActs = CActionCollectionPtr( newObj );
CActionRobotMovement2DPtr mov = curActs->getBestMovementEstimation();
if (mov)
{
timestamp_lastAction = mov->timestamp;
CPose2D incrPose = mov->poseChange->getMeanVal();
// If we have previous observations, shift them according to this new increment:
if (accum_sf)
{
CPose3D inv_incrPose3D( CPose3D(0,0,0) - CPose3D(incrPose) );
for (CSensoryFrame::iterator it=accum_sf->begin();it!=accum_sf->end();++it)
{
CPose3D tmpPose;
(*it)->getSensorPose(tmpPose);
tmpPose = inv_incrPose3D + tmpPose;
(*it)->setSensorPose(tmpPose);
}
}
// Accumulate from odometry:
accumMovement = accumMovement + incrPose;
// Copy the probabilistic options from the first entry we find:
if (!cummMovementInit)
{
odometryOptions = mov->motionModelConfiguration;
cummMovementInit = true;
}
}
}
else
{
// Unknown class:
THROW_EXCEPTION("Unknown class found in the file!");
}
}
catch (exception &e)
{
errorMsg = e.what();
keepLoading = false;
}
catch (...)
{
keepLoading = false;
}
} // end while keep loading
progDia.Update( filSize );
wxTheApp->Yield(); // Let the app. process messages
WX_END_TRY
}
void xRawLogViewerFrame::OnMenuLossLessDecimate(wxCommandEvent& event)
{
WX_START_TRY
CRawlog newRawLog;
newRawLog.setCommentText( rawlog.getCommentText() );
wxString strDecimation = wxGetTextFromUser(
_("The number of observations will be decimated (only 1 out of M will be kept). Enter the decimation ratio M:"),
_("Decimation"),
_("1") );
long DECIMATE_RATIO;
strDecimation.ToLong( &DECIMATE_RATIO );
ASSERT_(DECIMATE_RATIO>=1)
wxBusyCursor busyCursor;
wxTheApp->Yield(); // Let the app. process messages
size_t i, N = rawlog.size();
// ------------------------------------------------------------------------------
// METHOD TO BE MEMORY EFFICIENT:
// To free the memory of the current rawlog entries as we create the new one,
// then call "clearWithoutDelete" at the end.
// ------------------------------------------------------------------------------
CSensoryFramePtr accum_sf;
CActionRobotMovement2D::TMotionModelOptions odometryOptions;
bool cummMovementInit = false;
long SF_counter = 0;
// Reset cummulative pose change:
CPose2D accumMovement(0,0,0);
// For each entry:
for (i=0;i<N;i++)
{
CSerializablePtr obj = rawlog.getAsGeneric(i);
if (rawlog.getType(i)==CRawlog::etActionCollection)
{
// Accumulate Actions
// ----------------------
CActionCollectionPtr curActs = CActionCollectionPtr ( obj );
CActionRobotMovement2DPtr mov = curActs->getBestMovementEstimation();
if (mov)
{
CPose2D incrPose = mov->poseChange->getMeanVal();
// If we have previous observations, shift them according to this new increment:
if (accum_sf)
{
CPose3D inv_incrPose3D( CPose3D(0,0,0) - CPose3D(incrPose) );
for (CSensoryFrame::iterator it=accum_sf->begin();it!=accum_sf->end();++it)
{
CPose3D tmpPose;
(*it)->getSensorPose(tmpPose);
tmpPose = inv_incrPose3D + tmpPose;
(*it)->setSensorPose(tmpPose);
}
}
// Accumulate from odometry:
accumMovement = accumMovement + incrPose;
// Copy the probabilistic options from the first entry we find:
if (!cummMovementInit)
{
odometryOptions = mov->motionModelConfiguration;
cummMovementInit = true;
}
}
}
else if (rawlog.getType(i)==CRawlog::etSensoryFrame)
{
// Decimate Observations
// ---------------------------
// Add observations to the accum. SF:
if (!accum_sf)
accum_sf = CSensoryFrame::Create();
// Copy pointers to observations only (fast):
accum_sf->moveFrom( *CSensoryFramePtr(obj) );
if ( ++SF_counter >= DECIMATE_RATIO )
{
SF_counter = 0;
// INSERT OBSERVATIONS:
newRawLog.addObservationsMemoryReference( accum_sf );
accum_sf.clear_unique();
// INSERT ACTIONS:
CActionCollection actsCol;
if (cummMovementInit)
{
CActionRobotMovement2D cummMovement;
cummMovement.computeFromOdometry(accumMovement, odometryOptions );
actsCol.insert( cummMovement );
// Reset odometry accumulation:
accumMovement = CPose2D(0,0,0);
}
newRawLog.addActions( actsCol );
}
} else
THROW_EXCEPTION("This operation implemented for SF-based rawlogs only.");
// Delete object
obj.clear_unique();
} // end for i each entry
// Clear the list only (objects already deleted)
rawlog.clearWithoutDelete();
// Copy as new log:
rawlog = newRawLog;
rebuildTreeView();
WX_END_TRY
}
