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;
}
예제 #3
0
파일: test.cpp 프로젝트: bcsdleweev/mrpt
// ----------------------------------------------------
// 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;
	}
}