// On play:
void CFormPlayVideo::OnbtnPlayClick(wxCommandEvent& event)
{
btnPlay->Enable(false);
btnStop->Enable(true);
m_nowPlaying = true;
try
{
long delay_ms = 0;
edDelay->GetValue().ToLong(&delay_ms);
CFileGZInputStream* fil;
if (rbFile->GetValue())
{
// Load from file:
fil = new CFileGZInputStream(
std::string(edFile->GetValue().mb_str()));
}
else
{
// Use the loaded rawlog:
fil = nullptr;
}
size_t nImgs = 0, count = 0;
// If we are playing from memory, continue:
if (!fil)
{
count = edIndex->GetValue();
}
progressBar->SetRange(
0, fil ? (int)fil->getTotalBytesCount() : (int)rawlog.size());
progressBar->SetValue(0);
// Repeat until EOF exception or cancel.
while (m_nowPlaying)
{
wxTheApp->Yield();
CSerializable::Ptr obj;
if (fil)
{
(*fil) >> obj;
}
else
{
obj = rawlog.getAsGeneric(count);
m_idxInRawlog = count;
}
bool doDelay = false;
if (IS_CLASS(obj, CSensoryFrame))
{
doDelay = showSensoryFrame(obj.get(), nImgs);
}
else if (IS_DERIVED(obj, CObservation))
{
CSensoryFrame sf;
sf.insert(std::dynamic_pointer_cast<CObservation>(obj));
doDelay = showSensoryFrame(&sf, nImgs);
}
// Free the loaded object!
if (fil) obj.reset();
// Update UI
if ((count++) % 100 == 0)
{
progressBar->SetValue(
fil ? (int)fil->getPosition() : (int)count);
wxString str;
str.sprintf(_("Processed: %d images"), nImgs);
lbProgress->SetLabel(str);
if (!doDelay) wxTheApp->Yield();
}
// if (doDelay || (count % 100)==0)
edIndex->SetValue(count);
// End?
if (!fil && count >= rawlog.size()) m_nowPlaying = false;
// Time to process stop button press and redraw image.
if (doDelay)
{
wxTheApp->Yield();
wxMilliSleep(delay_ms);
}
}
}
catch (utils::CExceptionExternalImageNotFound& e)
{
wxMessageBox(
_U(e.what()), _("Error with a delayed load image"), wxOK, this);
if (wxYES ==
wxMessageBox(
_U(format(
"The current directory for relative images "
"is:\n%s\n\nDo you want to set it to a different one?",
CImage::getImagesPathBase().c_str())
.c_str()),
_("Error with delayed loading image"), wxYES_NO, this))
{
// Change CImage::getImagesPathBase()
wxDirDialog dirDialog(
this, _("Choose the base directory for relative image paths"),
_U(CImage::getImagesPathBase().c_str()), 0, wxDefaultPosition);
if (dirDialog.ShowModal() == wxID_OK)
{
CImage::setImagesPathBase(string(dirDialog.GetPath().mb_str()));
}
}
}
catch (std::exception& e)
{
wxMessageBox(_U(e.what()), _("Exception"), wxOK, this);
}
btnPlay->Enable(true);
btnStop->Enable(false);
}
void benchmark()
{
//
// Check rawlogs
//
if (!mrpt::system::fileExists(gtRawlogFilename))
{
cerr << " [ERROR] A rawlog file with name " << gtRawlogFilename;
cerr << " doesn't exist." << endl;
return;
}
gtRawlog.open(gtRawlogFilename);
if (!mrpt::system::fileExists(labeledRawlogFilename))
{
cerr << " [ERROR] A rawlog file with name " << labeledRawlogFilename;
cerr << " doesn't exist." << endl;
return;
}
labeledRawlog.open(labeledRawlogFilename);
cout << " [INFO] Working with ground truth rawlog " << gtRawlogFilename << endl;
cout << " and labeled rawlog " << labeledRawlogFilename << endl;
// TODO: Add tests to ensure that they are the same rawlog sequence
vector<double> v_success;
// Get pairs of observations and compare their labels
CActionCollectionPtr action;
CSensoryFramePtr observations;
CObservationPtr gtObs,labeledObs;
size_t gtObsIndex = 0, labeledObsIndex = 0;
cout << " Process: ";
cout.flush();
while ( ( CRawlog::getActionObservationPairOrObservation(gtRawlog,action,observations,gtObs,gtObsIndex) )&&
( CRawlog::getActionObservationPairOrObservation(labeledRawlog,action,observations,labeledObs,labeledObsIndex) ) )
{
// TODO: Check that the obss are 3D scans
CObservation3DRangeScanPtr gt3DObs = CObservation3DRangeScanPtr(gtObs);
CObservation3DRangeScanPtr labeled3DObs = CObservation3DRangeScanPtr(labeledObs);
if ( !(gtObsIndex % 200) )
{
if ( !(gtObsIndex % 1000) ) cout << "+ "; else cout << ". ";
cout.flush();
}
// Check that both observations have labels
if ( !gt3DObs->hasPixelLabels() || !labeled3DObs->hasPixelLabels() )
continue;
std::map<uint32_t,std::string>::iterator labelsIt;
size_t labelsAppearing = 0;
for ( labelsIt = gt3DObs->pixelLabels->pixelLabelNames.begin();
labelsIt != gt3DObs->pixelLabels->pixelLabelNames.end();
labelsIt++ )
{
// Get label name from the ground truth obs
string label = labelsIt->second;
// Check if it appears in the labeled obs
if ( labeled3DObs->pixelLabels->checkLabelNameExistence(label) >= 0 )
labelsAppearing++;
}
size_t N_labelsGt = gt3DObs->pixelLabels->pixelLabelNames.size();
size_t N_labelsLabeled = labeled3DObs->pixelLabels->pixelLabelNames.size();
size_t maxNumOfLabels = ( (N_labelsGt > N_labelsLabeled) ? N_labelsGt : N_labelsLabeled );
if (!maxNumOfLabels) // Are there labels?
continue;
double success = labelsAppearing / (double)maxNumOfLabels;
v_success.push_back(success);
}
double sumSuccess = std::accumulate(v_success.begin(), v_success.end(), 0.0);
double meanSuccess = sumSuccess / (double)v_success.size();
cout << endl;
cout << " [INFO] Results: " << endl;
cout << " - Mean success: " << meanSuccess*100 << "%";
cout << endl << endl;
}
void labelRawlog()
{
if ( sensors_to_use.empty() )
cout << " [INFO] Considering observations from any sensor." << endl;
else
{
cout << " [INFO] Considering observations from: ";
for ( size_t i_sensor = 0; i_sensor < sensors_to_use.size(); i_sensor++ )
cout << sensors_to_use[i_sensor] << " ";
cout << endl;
}
//
// Check input rawlog file
if (!mrpt::system::fileExists(configuration.rawlogFile))
{
cerr << " [ERROR] A rawlog file with name " << configuration.rawlogFile;
cerr << " doesn't exist." << endl;
// return;
}
i_rawlog.open(configuration.rawlogFile);
cout << " [INFO] Rawlog file : " << configuration.rawlogFile << endl;
cout << " [INFO] Labeled scene : " << configuration.labelledScene << endl;
loadLabelledScene();
//
// Set output rawlog file
string o_rawlogFile;
o_rawlogFile = configuration.rawlogFile.substr(0,configuration.rawlogFile.size()-7);
o_rawlogFile += "_labelled.rawlog";
o_rawlog.open(o_rawlogFile);
if ( configuration.visualizeLabels )
window = mrpt::gui::CDisplayWindowPtr( new mrpt::gui::CDisplayWindow("Labeled depth img"));
//
// Process rawlog
CActionCollectionPtr action;
CSensoryFramePtr observations;
CObservationPtr obs;
size_t obsIndex = 0;
cout.flush();
while ( CRawlog::getActionObservationPairOrObservation(i_rawlog,action,observations,obs,obsIndex) )
{
// Check that it is a 3D observation
if ( !IS_CLASS(obs, CObservation3DRangeScan) )
continue;
cout << "\r" << " Process: " << obsIndex;
cout.flush();
// Check if the sensor is being used
if ( !sensors_to_use.empty()
&& find(sensors_to_use.begin(), sensors_to_use.end(),obs->sensorLabel)
== sensors_to_use.end() )
continue;
// Get obs pose
CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs);
obs3D->load();
CPose3D pose;
obs3D->getSensorPose( pose );
//cout << "Pose [" << obs_index << "]: " << pose << endl;
size_t rows = obs3D->cameraParams.nrows;
size_t cols = obs3D->cameraParams.ncols;
// Create per pixel labeling
// Label size (0=8 bits, 1=16 bits, 2=32 bits, 3=32 bits, 8=64 bits
const unsigned int LABEL_SIZE = 2; // 32 bits
obs3D->pixelLabels = CObservation3DRangeScan::TPixelLabelInfoPtr( new CObservation3DRangeScan::TPixelLabelInfo< LABEL_SIZE >() );
obs3D->pixelLabels->setSize(rows,cols);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud( new pcl::PointCloud<pcl::PointXYZ>() );
obs3D->project3DPointsFromDepthImageInto( *pcl_cloud, true );
Eigen::Matrix4f transMat;
transMat(0,0)=0; transMat(0,1)=-1; transMat(0,2)=0; transMat(0,3)=0;
transMat(1,0)=0; transMat(1,1)=0; transMat(1,2)=+1; transMat(1,3)=0;
transMat(2,0)=1; transMat(2,1)=0; transMat(2,2)=0; transMat(2,3)=0;
transMat(3,0)=0; transMat(3,1)=0; transMat(3,2)=0; transMat(3,3)=1;
pcl::transformPointCloud( *pcl_cloud, *pcl_cloud, transMat );
pcl_cloud->height = 240;
pcl_cloud->width = 320;
//
// Label observation
labelObs( obs3D, pcl_cloud, rows, cols );
//
// Save to output file
o_rawlog << obs3D;
}
i_rawlog.close();
o_rawlog.close();
cout << endl << " [INFO] Done!" << endl << endl;
}
// ----------------------------------------------------
// The online/offline grabbing thread
// ----------------------------------------------------
void thread_grabbing(TThreadParam &p)
{
try
{
#if MRPT_HAS_CXX11
typedef std::unique_ptr<CKinect> CKinectPtr; // This assures automatic destruction
#else
typedef std::auto_ptr<CKinect> CKinectPtr; // This assures automatic destruction
#endif
// Only one of these will be actually used:
CKinectPtr kinect;
CFileGZInputStream dataset;
mrpt::system::TTimeStamp dataset_prev_tim = INVALID_TIMESTAMP;
mrpt::system::TTimeStamp my_last_read_obs_tim = INVALID_TIMESTAMP;
if (p.is_online)
{
// Online:
// ---------------------
kinect = CKinectPtr(new CKinect());
// Set params:
kinect->enableGrabRGB(true);
kinect->enableGrabDepth(true);
kinect->enableGrabAccelerometers(false);
kinect->enableGrab3DPoints(p.generate_3D_pointcloud_in_this_thread);
// Open:
cout << "Calling CKinect::initialize()...";
kinect->initialize();
cout << "OK\n";
}
else
{
// Offline:
// ---------------------
if (!dataset.open(p.rawlog_file))
throw std::runtime_error("Couldn't open rawlog dataset file for input...");
// Set external images directory:
CImage::IMAGES_PATH_BASE = CRawlog::detectImagesDirectory(p.rawlog_file);
}
CTicTac tictac;
int nImgs = 0;
while (!p.quit)
{
if (p.is_online)
{
// Grab new observation from the camera:
bool there_is_obs=true, hard_error=false;
CObservation3DRangeScanPtr obs = CObservation3DRangeScan::Create(); // Smart pointers to observations. Memory pooling is used internally
kinect->getNextObservation(*obs,there_is_obs,hard_error);
if(hard_error)
throw std::runtime_error("Sensor returned 'hardware error'");
else if (there_is_obs)
{
// Send object to the main thread:
p.new_obs.set(obs);
}
}
else
{
// Offline:
CObservationPtr obs;
do
{
try {
dataset >> obs;
}
catch (std::exception &e) {
throw std::runtime_error( string("\nError reading from dataset file (EOF?):\n")+string(e.what()) );
}
ASSERT_(obs.present())
} while (!IS_CLASS(obs,CObservation3DRangeScan));
// We have one observation:
CObservation3DRangeScanPtr obs3D = CObservation3DRangeScanPtr(obs);
obs3D->load(); // *Important* This is needed to load the range image if stored as a separate file.
// Do we have to wait to emulate real-time behavior?
const mrpt::system::TTimeStamp cur_tim = obs3D->timestamp;
const mrpt::system::TTimeStamp now_tim = mrpt::system::now();
if (dataset_prev_tim!=INVALID_TIMESTAMP && my_last_read_obs_tim!=INVALID_TIMESTAMP)
{
#ifndef FAKE_KINECT_FPS_RATE
const double At_dataset = mrpt::system::timeDifference( dataset_prev_tim, cur_tim );
#else
const double At_dataset = 1.0/FAKE_KINECT_FPS_RATE;
#endif
const double At_actual = mrpt::system::timeDifference( my_last_read_obs_tim, now_tim );
const double need_to_wait_ms = 1000.*( At_dataset-At_actual );
//cout << "[Kinect grab thread] Need to wait (ms): " << need_to_wait_ms << endl;
if (need_to_wait_ms>0)
mrpt::system::sleep( need_to_wait_ms );
}
// Send observation to main thread:
p.new_obs.set(obs3D);
dataset_prev_tim = cur_tim;
my_last_read_obs_tim = mrpt::system::now();
}
// Update Hz rate estimate
nImgs++;
if (nImgs>10)
{
p.Hz = nImgs / tictac.Tac();
nImgs=0;
tictac.Tic();
}
}
}
catch(std::exception &e)
{
cout << "Exception in Kinect thread: " << e.what() << endl;
p.quit = true;
}
}
