/* -----------------------------------------------------------------------
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 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::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
}