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;
}
}