Пример #1
0
int
FFmpegTagger::IORead(void *opaque, uint8_t *buf, int buf_size)
{
    URLContext* pUrlContext = (URLContext*)opaque;

    std::istream* pInputStream = (std::istream*)pUrlContext->priv_data;
    if (!pInputStream) {
        LOGNS(Omm::AvStream, avstream, error, "IORead failed, std::istream not set");
        return -1;
    }

    pInputStream->read((char*)buf, buf_size);
    Poco::Timestamp::TimeDiff time = _timestamp.elapsed();
    _timestamp.update();
    Poco::Timestamp::TimeDiff startTime = _startTimestamp.elapsed();
    int bytes = pInputStream->gcount();
    _totalBytes += bytes;
    if (!pInputStream->good()) {
        LOGNS(Omm::AvStream, avstream, error, "IORead failed to read from std::istream");
        return -1;
    }
    LOGNS(Omm::AvStream, avstream, trace, "IORead() bytes read: " + Poco::NumberFormatter::format(bytes) + " in " + Poco::NumberFormatter::format(time/1000.0, 3) + " msec (" +  Poco::NumberFormatter::format(bytes*1000/time) + " kB/s), total : " +\
    Poco::NumberFormatter::format(_totalBytes/1000) + "kB in " + Poco::NumberFormatter::format(startTime/(Poco::Timestamp::TimeDiff)1000000) + " sec (" + Poco::NumberFormatter::format((Poco::Timestamp::TimeDiff)(_totalBytes*1000)/startTime) + "kB/s)");
    return bytes;
}
Пример #2
0
bool Debounce(void) {
    if (fpsTimeLast.isElapsed(debounceTimeout.totalMicroseconds())) {
        fpsTimeLast.update();
        return true;
    } else {
        return false;
    }
}
Пример #3
0
bool Debounce(int dtime) {
    const Poco::Timespan TimeOut(0, 1000*dtime);
    if (fpsTimeLast.isElapsed(TimeOut.totalMicroseconds())) {
        fpsTimeLast.update();
        return true;
    } else {
        return false;
    }
}
Пример #4
0
void Timer::run()
{
	Poco::Timestamp now;
	long interval(0);
	do
	{
		long sleep(0);
		do
		{
			now.update();
			sleep = static_cast<long>((_nextInvocation - now)/1000);
			if (sleep < 0)
			{
				if (interval == 0)
				{
					sleep = 0;
					break;
				}
				_nextInvocation += static_cast<Timestamp::TimeVal>(interval)*1000;
				++_skipped;
			}
		}
		while (sleep < 0);

		if (_wakeUp.tryWait(sleep))
		{
			Poco::FastMutex::ScopedLock lock(_mutex);
			_nextInvocation.update();
			interval = _periodicInterval;
		}
		else
		{
			try
			{
				_pCallback->invoke(*this);
			}
			catch (Poco::Exception& exc)
			{
				Poco::ErrorHandler::handle(exc);
			}
			catch (std::exception& exc)
			{
				Poco::ErrorHandler::handle(exc);
			}
			catch (...)
			{
				Poco::ErrorHandler::handle();
			}
			interval = _periodicInterval;
		}
		_nextInvocation += static_cast<Timestamp::TimeVal>(interval)*1000;
		_skipped = 0;
	}
	while (interval > 0);
	_done.set();
}
Пример #5
0
int main(int argc, char**argv) {
  dsosg::DSOSG *dsosg;
  Poco::Timestamp time;
  bool done;

  osg::ArgumentParser arguments(&argc, argv);
  arguments.getApplicationUsage()->setApplicationName(arguments.getApplicationName());
  arguments.getApplicationUsage()->setDescription("Manual display/camera calibration utility");
  arguments.getApplicationUsage()->addCommandLineOption("--config <filename>","Display server config JSON file");
  arguments.getApplicationUsage()->addCommandLineOption("--display-mode <name>","Display mode");

  osg::ApplicationUsage::Type help = arguments.readHelpType();
  if (help != osg::ApplicationUsage::NO_HELP) {
      arguments.getApplicationUsage()->write(std::cout);
      exit(0);
  }

  std::string config_filename = "~/flyvr-devel/flyvr/flyvr/config/config.json";
  while(arguments.read("--config", config_filename));

  std::string display_mode = "vr_display";
  while(arguments.read("--display-mode", display_mode));

  std::string flyvr_basepath = "/home/john/Programming/flyvr.git/flyvr/";
  float observer_radius = 0.01;
  bool two_pass = false;
  bool show_geom_coords = false;

  osg::Vec3 observer_position(0,0,0);
  osg::Quat observer_orientation(0,0,0,1);

  dsosg = new dsosg::DSOSG(
                        flyvr_basepath,
                        display_mode,
                        observer_radius,
                        config_filename,
                        two_pass,
                        show_geom_coords);

  dsosg->setup_viewer("display_server","{}");
  dsosg->set_stimulus_plugin("Stimulus3DDemo");

  done = false;
  while (!done) {
    float now;

    time.update();
    now = time.epochMicroseconds() * 1e6; /* microseconds to seconds */

    dsosg->update(now, observer_position, observer_orientation);
    dsosg->frame();
    done = dsosg->done();
  }

	return 0;
}
Пример #6
0
ByteIOContext*
FFmpegTagger::initIo(std::istream& istr, bool isSeekable, unsigned char* pIoBuffer)
{
    static char streamName[] = "std::istream";

    // FIXME: deallocate URLContext after stream has finished.
    URLContext* pUrlContext = new URLContext;
    pUrlContext->is_streamed = 1;
//     pUrlContext->priv_data = 0;
    pUrlContext->priv_data = &istr;
    pUrlContext->filename = streamName;
    pUrlContext->prot = new URLProtocol;
    pUrlContext->prot->name = "OMM avio wrapper for std::iostream";
    pUrlContext->prot->next = 0;
    pUrlContext->prot->url_open = 0;
//     pUrlContext->prot->url_open = (int (*)(URLContext *, const char *, int))IOOpen;
    pUrlContext->prot->url_read = (int (*) (URLContext *, unsigned char *, int))FFmpegTagger::IORead;
    pUrlContext->prot->url_write = 0;
    if (isSeekable) {
        pUrlContext->prot->url_seek = (int64_t (*) (URLContext *, int64_t, int))FFmpegTagger::IOSeek;
    }
    else {
        pUrlContext->prot->url_seek = 0;
    }
    pUrlContext->prot->url_close = 0;
//     pUrlContext->prot->url_close = (int (*)(URLContext *))IOClose;

    LOG(ffmpeg, trace, "ffmpeg::av_alloc_put_byte() ...");
    ByteIOContext* pIoContext;
    if (isSeekable) {
        pIoContext = av_alloc_put_byte(pIoBuffer, _IoBufferSize, 0, pUrlContext, FFmpegTagger::IORead, 0, FFmpegTagger::IOSeek);
    }
    else {
        pIoContext = av_alloc_put_byte(pIoBuffer, _IoBufferSize, 0, pUrlContext, FFmpegTagger::IORead, 0, 0);
        pIoContext->is_streamed = 1;
    }
    pIoContext->max_packet_size = _IoBufferSize;

    _timestamp.update();
    _startTimestamp.update();

    return pIoContext;
}
Пример #7
0
void XplDevice::run ( void )
{
    if ( !m_bInitialised )
    {
        assert ( 0 );
        return;
    }

    while ( !m_bExitThread )
    {
        // Deal with heartbeats
        int64_t currentTime;
        Poco::Timestamp tst;
        tst.update();
        currentTime = tst.epochMicroseconds();
        //GetSystemTimeAsFileTime( (FILETIME*)&currentTime );
        
        if ( m_nextHeartbeat <= currentTime )
        {
            poco_debug( devLog, "Sending heartbeat" );
            // It is time to send a heartbeat
            if ( m_bConfigRequired )
            {
                // Send a config heartbeat, then calculate the time of the next one
                m_pComms->SendConfigHeartbeat ( m_completeId, m_heartbeatInterval, m_version );
            }
            else
            {
                // Send a heartbeat, then calculate the time of the next one
                m_pComms->SendHeartbeat ( m_completeId, m_heartbeatInterval, m_version );
            }

            SetNextHeartbeatTime();
        }
        // Calculate the time (in milliseconds) until the next heartbeat
        int32 heartbeatTimeout = ( int32 ) ( ( m_nextHeartbeat - currentTime ) ); // Divide by 10000 to convert 100 nanosecond intervals to milliseconds.
        poco_debug ( devLog, "Sleeping " + NumberFormatter::format ( heartbeatTimeout/1000 ) + " seconds till next hbeat" );
        m_hRxInterrupt->tryWait ( heartbeatTimeout );
        //Thread::sleep();
        poco_debug ( devLog, "Woken up for hbeat or interrupt" );

    }
// 	cout << "exiting dev thread (ret)\n";
    return;
}
Пример #8
0
void XplDevice::SetNextHeartbeatTime()
{
    // Set the new heartbeat time
    int64_t currentTime;
    Poco::Timestamp tst;
    tst.update();
    currentTime = tst.epochMicroseconds();
    //GetSystemTimeAsFileTime( (FILETIME*)&currentTime );

    // If we're waiting for a hub, we have to send at more
    // rapid intervals - every 3 seconds for the first two
    // minutes, then once every 30 seconds after that.
    if ( m_bWaitingForHub )
    {
        if ( m_rapidHeartbeatCounter )
        {
            // This counter starts at 40 for 2 minutes of
            // heartbeats at 3 second intervals.
            --m_rapidHeartbeatCounter;


            m_nextHeartbeat = currentTime + ( ( int64_t ) c_rapidHeartbeatFastInterval * 1000l );
        }
        else
        {
            // one second
            m_nextHeartbeat = currentTime + ( ( int64_t ) c_rapidHeartbeatSlowInterval * 1000l );
        }
    }
    else
    {
        // It is time to send a heartbeat
        if ( m_bConfigRequired )
        {
            // one minute
            m_nextHeartbeat = currentTime + 60*1000l;
        }
        else
        {
            // 60000000 is one minute in the 100 nanosecond intervals
            // that the system time is measured in.
            m_nextHeartbeat = currentTime + ( ( int64_t ) m_heartbeatInterval * 60*1000l );
        }
    }
}
Пример #9
0
	void run()
	{
		Poco::Timestamp lastScan;
		ItemInfoMap entries;
		scan(entries);

		while (!_stopped)
		{
			struct timespec timeout;
			timeout.tv_sec = 0;
			timeout.tv_nsec = 200000000;
			unsigned eventFilter = NOTE_WRITE;
			struct kevent event;
			struct kevent eventData;
			EV_SET(&event, _dirFD, EVFILT_VNODE, EV_ADD | EV_CLEAR, eventFilter, 0, 0);
			int nEvents = kevent(_queueFD, &event, 1, &eventData, 1, &timeout);
			if (nEvents < 0 || eventData.flags == EV_ERROR)
			{
				try
				{
					FileImpl::handleLastErrorImpl(owner().directory().path());
				}
				catch (Poco::Exception& exc)
				{
					owner().scanError(&owner(), exc);
				}
			}
			else if (nEvents > 0 || ((owner().eventMask() & DirectoryWatcher::DW_ITEM_MODIFIED) && lastScan.isElapsed(owner().scanInterval()*1000000)))
			{
				ItemInfoMap newEntries;
				scan(newEntries);
				compare(entries, newEntries);
				std::swap(entries, newEntries);
				lastScan.update();
			}
		}
	}
Пример #10
0
void RealSenseSensorImpl::faceDetect(bool depthMask)
{
	Poco::Int64 maskTime = 0;
	Poco::Int64 detectTime = 0;

	Poco::Timestamp now;
	ImageGrayscale mask(grayscalePreview.clone());
	if (depthMask) {
		PXCCapture::Sample *sample = senseManager->QuerySample();
		mask = RealSenseProcessor::depthMask(sample->depth, sample->color, projection, grayscalePreview);
		//cv::imshow("Preview mask", mask);
	}
	maskTime = now.elapsed();

	now.update();
	cv::resize(mask, resizedPreview, cv::Size(320, 240), 0, 0, CV_INTER_NN);
	roiPreview = resizedPreview(detectRoi);
	tracker->detect(roiPreview);
	detectTime = now.elapsed();

	if (depthMask && tracker->isLastDetect()) {
		cv::Rect faceRoi = toRealSizeRoi(tracker->getLastRegion());
		mask = RealSenseProcessor::fillHoles(faceRoi, mask, grayscalePreview, 0.0);
		//cv::imshow("grayscalePreview mask 2", mask);
	}

	//std::cout << std::dec << "Mask time: " << maskTime << "; Face detect time: " << detectTime << std::endl;

	/*
	if (Face::Settings::instance().debug()) {
		if (tracker->isLastDetect()) {
			cv::rectangle(colorPreview, toRealSizeRoi(tracker->getLastRegion()), 255);
		}
		//cv::imshow("detection", roiPreview);
	}*/
}
Пример #11
0
int main (int argc, char** argv) {

    const bool USE_OPENCV_DISPLAY = false; // Use OpenCV or CImg for display?

    pI::BaseApplication app;
    pI::Runtime runtime (app.GetRuntime());
    LoadPlugins (app);

    // Creates and initializes all the necessary plugins.
    const int DeviceIndex = 0;
    const bool ShowRGB = true;

    // Freenect/GetVideoAndDepth returns both the RGB image and the depth map captured by a Kinect.
    boost::shared_ptr<pI::pIn> getImageAndDepth
        = app.SpawnAndInitialize (runtime,
                                  "Freenect/GetVideoAndDepth",
                                  DeviceIndex,
                                  ShowRGB ? 0 : 2,   // Video format: FREENECT_VIDEO_RGB or FREENECT_VIDEO_IR_8BIT
                                  0                  // Depth format: FREENECT_DEPTH_11BIT
                                 );
    pI::Arguments getImageAndDepth_in = getImageAndDepth->GetInputSignature(),
                  getImageAndDepth_out = getImageAndDepth->GetOutputSignature();

    // Freenect/SetTiltAndLED sets the tilt angle and LED status of a Kinect.
    boost::shared_ptr<pI::pIn> setTiltAndLED = app.SpawnAndInitialize (runtime, "Freenect/SetTiltAndLED", DeviceIndex);
    pI::Arguments setTiltAndLED_in = setTiltAndLED->GetInputSignature(),
                  setTiltAndLED_out = setTiltAndLED->GetOutputSignature();

    // Color/ApplyRGBPalette applies an arbitrary RGB palette onto a matrix
    boost::shared_ptr<pI::pIn> applyRGB (runtime.SpawnPlugin ("pIn/Color/ApplyRGBPalette"));
    pI::Arguments applyRGB_params = applyRGB->GetParameterSignature();

    pI::RGBPalette rgb (applyRGB_params[0]);
    rgb.SetCount (2048);

    std::map<size_t, uint32_t> freenect_glview_colors; // Coloring as used in Freenect's glview example
    freenect_glview_colors[0]    = (0xFFFFFF); // white
    freenect_glview_colors[256]  = (0xFF0000); // red
    freenect_glview_colors[512]  = (0xFFFF00); // yellow
    freenect_glview_colors[768]  = (0x00FF00); // green
    freenect_glview_colors[1024] = (0x00FFFF); // cyan
    freenect_glview_colors[1280] = (0x0000FF); // blue
    freenect_glview_colors[1536] = (0x000000); // black

    std::map<size_t, uint32_t> terrain; // Alternative coloring; names taken from http://en.wikipedia.org/wiki/List_of_colors
    terrain[0]     = (0xFFFFFF); // White
    terrain[256]   = (0x964B00); // Brown (traditional)
    terrain[512]   = (0xFBEC5D); // Maize
    terrain[768]   = (0x66FF00); // Bright green
    terrain[1024]  = (0x014421); // Forest green (traditional)
    terrain[1025]  = (0x87CEFA); // Light sky blue
    terrain[1280]  = (0x00008B); // Dark blue
    terrain[2047]  = (0x000000); // Black
    terrain[2048]  = (0x696969); // Dim gray

    ColorRamp ramp (terrain);

    for (int i = 0; i < 2048; i++) {
        float v = powf ( (i / 2048.), 3);
        uint16_t gamma = 9216 * v;

        uint32_t c = ramp.GetColor (gamma);
        rgb.SetR (i, red (c));
        rgb.SetG (i, green (c));
        rgb.SetB (i, blue (c));
    }

    applyRGB->Initialize (applyRGB_params);

    pI::Arguments applyRGB_in = applyRGB->GetInputSignature(),
                  applyRGB_out = applyRGB->GetOutputSignature();

    // OpenCV/Draw/Rectangle draws the inner rectangle.
    boost::shared_ptr<pI::pIn> drawRectImage = app.SpawnAndInitialize (runtime, "OpenCV/Draw/Rectangle");
    pI::Arguments drawRectImage_in = drawRectImage->GetInputSignature(),
                  drawRectImage_out = drawRectImage->GetOutputSignature();

    boost::shared_ptr<pI::pIn> drawRectDepth = app.SpawnAndInitialize (runtime, "OpenCV/Draw/Rectangle");
    pI::Arguments drawRectDepth_in = drawRectDepth->GetInputSignature(),
                  drawRectDepth_out = drawRectDepth->GetOutputSignature();

    // OpenCV/Draw/PutText draws a text string into an image.
    boost::shared_ptr<pI::pIn> puttext
        = app.SpawnAndInitialize (runtime,
                                  "OpenCV/Draw/PutText",
                                  0,                // font type: CV_FONT_HERSHEY_SIMPLEX
                                  pI_FALSE,         // render font italic: no
                                  0.75, 0.75,       // horizontal and vertical scale
                                  0.0,              // shear: no shear
                                  1,                // thickness of the text strokes: 1
                                  2                 // type of the line: CV_AA
                                 );
    pI::Arguments puttext_in = puttext->GetInputSignature(),
                  puttext_out = puttext->GetOutputSignature();

    // OpenCV/IO/SaveImage saves images to disk.
    boost::shared_ptr<pI::pIn> saveImage (runtime.SpawnPlugin ("OpenCV/IO/SaveImage"));
    saveImage->Initialize (saveImage->GetParameterSignature());
    pI::Arguments saveImage_in = saveImage->GetInputSignature(),
                  saveImage_out = saveImage->GetOutputSignature();

    // OpenCV/IO/ShowImage or CImg/Display displays the image and the depth map.
    boost::shared_ptr<pI::pIn> displayImage, displayDepth;

    if (USE_OPENCV_DISPLAY) {
        displayImage = app.SpawnAndInitialize (runtime, "OpenCV/IO/ShowImage",
                                               "RGB image window", false, 640, 480, 0, 0);
        displayDepth = app.SpawnAndInitialize (runtime, "OpenCV/IO/ShowImage",
                                               "Depth map window", false, 640, 480, 650, 0);
    } else {
        displayImage   = app.SpawnAndInitialize (runtime, "CImg/Display", pI_TRUE);
        displayDepth = app.SpawnAndInitialize (runtime, "CImg/Display", pI_TRUE);
    }

    pI::Arguments displayImage_in = displayImage->GetInputSignature(),
                  displayImage_out = displayImage->GetOutputSignature();

    pI::Arguments displayDepth_in = displayDepth->GetInputSignature(),
                  displayDepth_out = displayDepth->GetOutputSignature();


    // Now that all plugins are initialized, connect them.

    drawRectImage_in[0] = getImageAndDepth_out[0];
    pI::IntValue (drawRectImage_in[1]).SetData (310);
    pI::IntValue (drawRectImage_in[2]).SetData (230);
    pI::IntValue (drawRectImage_in[3]).SetData (330);
    pI::IntValue (drawRectImage_in[4]).SetData (250);
    pI::IntValue (drawRectImage_in[5]).SetData (255);
    pI::IntValue (drawRectImage_in[6]).SetData (255);
    pI::IntValue (drawRectImage_in[7]).SetData (255);
    pI::IntValue (drawRectImage_in[8]).SetData (1);
    pI::StringSelection (drawRectImage_in[9]).SetIndex (0);
    pI::IntValue (drawRectImage_in[10]).SetData (0);

    displayImage_in[0] = drawRectImage_in[0];

    pI::StringValue fileName (saveImage_in[0]);
    saveImage_in[1] = getImageAndDepth_out[0];

    applyRGB_in[0] =  getImageAndDepth_out[1];
    drawRectDepth_in[0] = applyRGB_out[0];
    pI::IntValue (drawRectDepth_in[1]).SetData (310);
    pI::IntValue (drawRectDepth_in[2]).SetData (230);
    pI::IntValue (drawRectDepth_in[3]).SetData (330);
    pI::IntValue (drawRectDepth_in[4]).SetData (250);
    pI::IntValue (drawRectDepth_in[5]).SetData (255);
    pI::IntValue (drawRectDepth_in[6]).SetData (255);
    pI::IntValue (drawRectDepth_in[7]).SetData (255);
    pI::IntValue (drawRectDepth_in[8]).SetData (1);
    pI::StringSelection (drawRectDepth_in[9]).SetIndex (0);
    pI::IntValue (drawRectDepth_in[10]).SetData (0);
    displayDepth_in[0] = drawRectDepth_in[0];

    pI::StringValue puttext_text (puttext_in[1]);
    pI::IntValue puttext_x (puttext_in[2]);
    pI::IntValue puttext_y (puttext_in[3]);
    pI::IntValue puttext_R (puttext_in[4]);
    pI::IntValue puttext_G (puttext_in[5]);
    pI::IntValue puttext_B (puttext_in[6]);
    pI::BoolValue puttext_do_draw (puttext_in[7]);

    puttext_in[0] = drawRectDepth_in[0];
    puttext_R.SetData (255);
    puttext_G.SetData (255);
    puttext_B.SetData (255);
    puttext_do_draw.SetData (pI_TRUE);

    puttext_x.SetData (285);
    puttext_y.SetData (272);

    // Executes getImageAndDepth and setTiltAndLED a first time to initialize Kinect.

    getImageAndDepth->Execute (getImageAndDepth_in, getImageAndDepth_out);

    pI::IntValue newTilt (setTiltAndLED_in[0]);
    pI::IntValue newLED (setTiltAndLED_in[1]);

    newTilt.SetData (0); // Tilt angle 0 degree
    newLED.SetData (1); // LED status Green

    setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);

    Poco::Timestamp now;

    bool doLoop = true;
    bool showDistance = false;

    try {
        while (doLoop) {
            while (!kbhit()) {

                // Get Depth and RGB data, ...
                getImageAndDepth->Execute (getImageAndDepth_in, getImageAndDepth_out);

                // ... apply depth to color image conversion, ...
                applyRGB->Execute (applyRGB_in, applyRGB_out);

                // ... draw a white rectangle into the center of both image and map, ...
                if (showDistance) {
                    drawRectImage->Execute (drawRectImage_in, drawRectImage_out);
                    drawRectDepth->Execute (drawRectDepth_in, drawRectDepth_out);
                }

                // ... and perhaps the result of distance measurement, ...
                double avgDistance = 0.;

                if (showDistance) {
                    pI::ShortMatrix depth (getImageAndDepth_out[1]);
                    double sumDistance = 0.;
                    int sumCount = 0;

                    for (int j = 230; j <= 250; ++j) {
                        for (int i = 310; i <= 330; ++i) {
                            int dep = depth.GetData (j, i);

                            if (dep < 2047) {
                                sumDistance += RawDepthToMeters (dep);
                                ++sumCount;
                            }
                        }
                    }

                    if (sumCount > 0) {
                        avgDistance = sumDistance / (double) sumCount;
                    }

                    char str[32];
                    sprintf (str, "%.2f m", avgDistance);
                    puttext_text.SetData (str);

                    puttext->Execute (puttext_in, puttext_out);
                }

                // ... and display both the image and the depth map.
                displayImage->Execute (displayImage_in, displayImage_out);
                displayDepth->Execute (displayDepth_in, displayDepth_out);

                std::cout << "Tilt " << (int) newTilt.GetData();
                std::cout << "; " << (1000000. / now.elapsed()) << " frames/sec          \r";

                now.update();
            }

            int ch = getch();

            switch (ch) {
            case 'q':
            case 'Q':
            case 27 /* ESC */:
                // Terminates
                newTilt.SetData (0); // Tilt angle 0 degree
                newLED.SetData (0); // LED status off

                setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);

                doLoop = false;
                break;

            case 'm':
            case 'M': // Measurement mode
                showDistance = !showDistance;
                break;

            case 's':
            case 'S': { // Takes snapshot

                std::string dateTime = Poco::DateTimeFormatter::format (Poco::LocalDateTime(), "%Y-%m-%d_%H-%M-%S");

                std::string name = std::string ("KinectImage_") + dateTime + std::string (".png");
                fileName.SetData (const_cast<char*> (name.c_str()));

                saveImage->Execute (saveImage_in);

                std::cout << "Saved " << name << std::endl;

                // HACK Experimental: saves PCL point cloud

                pI::ShortMatrix depth (getImageAndDepth_out[1]);
                int width = depth.GetCols();
                int height = depth.GetRows();

                pcl::PointCloud<pcl::PointXYZ> cloud;

                for (int j = 0; j < height; ++j) {
                    for (int i = 0; i < width; ++i) {
                        int dep = depth.GetData (j, i);

                        if (dep < 2048) {
                            cloud.push_back (DepthToWorld (i, j, dep));
                        }
                    }
                }

                name = std::string ("PointCloud_") + dateTime + std::string (".pcd");
                pcl::io::savePCDFileASCII (name, cloud);

                std::cout << "Saved " << name << std::endl;
            }
            break;
            case '+':
            case 'u':
            case 'U':
                //std::cout << std::endl << "Tilt: " << newTilt.GetData();
                newTilt.SetData (newTilt.GetData() + 1);
                //std::cout << " -> " << newTilt.GetData() << std::endl;
                setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);
                break;
            case '-':
            case 'd':
            case 'D':
                //std::cout << std::endl << "Tilt: " << newTilt.GetData();
                newTilt.SetData (newTilt.GetData() - 1);
                //std::cout << " -> " << newTilt.GetData() << std::endl;
                setTiltAndLED->Execute (setTiltAndLED_in, setTiltAndLED_out);
                break;
            default:
                break;
            }
        }
    } catch (pI::exception::ExecutionException& e) {
        std::cerr << e.what() << std::endl;
    }
}
Пример #12
0
 void UnGhost() { 
     unghost = true; 
     unghost_request_time.update(); 
 }
Пример #13
0
void RealSenseSensorImpl::singleCapture(bool calcUvMap, bool calcLandmarks, bool releaseFrame)
{
	Poco::Int64 acquireTime = 0;
	Poco::Int64 getBuffersTime = 0;
	
	Poco::Timestamp now;
	sts = senseManager->AcquireFrame(true);

	if (sts != PXC_STATUS_NO_ERROR) {
		senseManager->ReleaseFrame();
		std::string msg("Can't acquire frame from camera (" + std::to_string(sts) + ")");
		std::cerr << msg << std::endl;
		throw FACELIB_EXCEPTION(msg);
	}
	PXCCapture::Sample *sample = senseManager->QuerySample();
	acquireTime = now.elapsed();
	
	now.update();
	getBuffers(sample->depth, sample->color, calcUvMap, calcLandmarks);
	getBuffersTime = now.elapsed();

	try {
		brightnessToSet = 0;
		Poco::Int64 detectFaceRoiTime = 0;
		Poco::Int64 meanCalcTime = 0;
		Poco::Int64 queryColorBrightnessTime = 0;
		Poco::Int64 queryColorBrightnessInfoTime = 0;
		Poco::Int64 setColorBrightnessTime = 0;

		now.update();
		cv::Rect croi = realsenseDepthFaceTracker->detectFaceRoi(depthMatrix, sample->depth, RealSenseDepthFaceTracker::Domain::Color, false /*debug*/);
		detectFaceRoiTime = now.elapsed();

		// brightness control
		if ((settings.brightnessControl == static_cast<int>(RealSenseSensor::BrightnessControlType::HeadRoiBased) && croi.area() && 
			(state == RealSenseSensor::STATE_POSITIONING || state == RealSenseSensor::STATE_IDLE))) {
			now.update();
			cv::Mat mask = cv::Mat::zeros(grayscalePreview.size(), CV_8U);
			cv::rectangle(mask, croi, cv::Scalar(255), CV_FILLED);
			int mean = cv::mean(grayscalePreview, mask)[0];
			meanCalcTime = now.elapsed();

//			if (Face::Settings::instance().debug()) {
//				cv::Mat maskDrawing = grayscalePreview.clone();
//				cv::rectangle(maskDrawing, croi, cv::Scalar(255));
//				cv::imshow("Depth roi for gain projected to color", maskDrawing);
//			}

			now.update();
			pxcI32 brightness = device->QueryColorBrightness();
			queryColorBrightnessTime = now.elapsed();
			diff = targetColorMean - mean;

			auto newValue = [=](const PXCCapture::Device::PropertyInfo& info, pxcI32 val, pxcI32 newVal) -> pxcI32 {
				pxcI32 newV = std::max(info.range.min, std::min(static_cast<pxcF32>(newVal), info.range.max));
				if (Face::Settings::instance().debug()) {
					std::stringstream sstr;
					sstr << "Val: " << val << "; adjusted val: " << newV << "; mean: " << mean << "/" << targetColorMean;
					shadowText(grayscalePreview, sstr.str(), cv::Point(150, 30), 0.5);
				}
				return newV;
			};

			int _diff = diff == 0 ? 0 : diff / abs(diff);
			pxcI32 newBrightness = newValue(device->QueryColorBrightnessInfo(), brightness, brightness + _diff);
			if (abs(diff) > 15) {
				brightnessToSet = newBrightness;
			}
		}
	} catch (std::exception& e) {
		std::cout << " --- Brightness control exception: " << e.what() << std::endl;
	}

	//std::cout << std::dec << "Acquire time: " << acquireTime << "; getBuffers time: " << getBuffersTime << std::endl;

	if (releaseFrame) {
		senseManager->ReleaseFrame();
	}
}
Пример #14
0
void RealSenseSensorImpl::doPositioning()
{
	Poco::Int64 singleCaptureTime = 0;
	Poco::Int64 faceDetectTime = 0;
	Poco::Int64 pose1Time = 0;
	Poco::Int64 pose2Time = 0;
	Poco::Int64 frameReleaseTime = 0;
	Poco::Int64 frameTime = 0;

	static Poco::Timestamp frameTimer;

	RealSenseSensor::PositioningOutput prevPosOutput = positioningOutput;
	positioningOutput = RealSenseSensor::POS_NONE;

	Poco::Timestamp now;
	singleCapture(false, false, false);
	singleCaptureTime = now.elapsed();
	
	now.update();
	faceDetect(settings.hasPosFeature(RealSenseSensor::PositioningFeature::DepthMask));
	faceDetectTime = now.elapsed();

	frameTime = frameTimer.elapsed();
	frameTimer.update();

	calcMinZ();

	if (tracker->getConsecutiveNonDetects() >= settings.consecutiveNonDetectsToStopPositioning ||
			(tracker->isLastDetect() && nearestZpoint > settings.maximalPositioningDistance)) {
		setState(RealSenseSensor::STATE_IDLE);
		senseManager->ReleaseFrame();
		return;
	}

	if (tracker->getConsecutiveNonDetects() > 10) {
		positioningOutput = RealSenseSensor::POS_NONE;
		senseManager->ReleaseFrame();
		return;
	}

	if (!tracker->isLastDetect()) {
		senseManager->ReleaseFrame();
		return;
	}

	cv::Rect reg = toRealSizeRoi(tracker->getLastRegion());
	double distance = calcDistance(reg);

	now.update();

	FacePoseEstimator::Pose pose = FacePoseEstimator::Pose::UnknownPose();

	if (settings.hasPosFeature(RealSenseSensor::PositioningFeature::PoseEstimation) && tracker->isLastDetect()) {
		landmarks = lmDetector.getLandmarks(grayscalePreview, toRealSizeRoi4LMDetector(tracker->getLastRegion()), false);
		std::map<std::string, std::vector<cv::Point2d> > selectedLMPoints = lmDetector.lmGroupPoints(landmarks);
		pose1Time = now.elapsed();
		
		now.update();
		pose = facePoseEstimator->facePose(selectedLMPoints, realsenseProjection);
		pose2Time = now.elapsed();

		if (Face::Settings::instance().debug() && pose != FacePoseEstimator::Pose::UnknownPose()) {
			cv::Scalar red(0, 0, 255);
			cv::Scalar green(0, 255, 0);
			cv::Scalar white(255, 255, 255);
			cv::Scalar black(0, 0, 0);

			{
				positioningFrameStats.update();
				std::stringstream sstr;
				sstr.precision(1);
				sstr.setf(std::ios::fixed, std::ios::floatfield);
				sstr << "Pitch: " << pose.pitch - pose.relativePitch << "/" << facePoseEstimator->getSettings().pitchZeroAngle <<
					", Yaw: " << pose.yaw - pose.relativeYaw << "/" << facePoseEstimator->getSettings().yawZeroAngle <<
					", FPS: " << positioningFrameStats.fps();
				sstr.flush();
				shadowText(grayscalePreview, sstr.str(), cv::Point(150, 15), 0.5);

				{
					std::stringstream sstrDebug;
					sstrDebug << std::dec << "Capture time: " << singleCaptureTime / 1000 << "; FaceDetect time: " << faceDetectTime / 1000;
					shadowText(grayscalePreview, sstrDebug.str(), cv::Point(150, 45), 0.5);
				}

				{
					std::stringstream sstrDebug;
					sstrDebug << std::dec << "pose time: " << pose1Time / 1000 << "/" << pose2Time / 1000 << "; frame time: " << frameTime / 1000;
					shadowText(grayscalePreview, sstrDebug.str(), cv::Point(150, 60), 0.5);
				}

				auto mirror = [](const cv::Mat& img, const cv::Point& p) -> cv::Point {
					return cv::Point(img.cols - p.x, p.y);
				};

				cv::line(grayscalePreview, mirror(grayscalePreview, pose.imageAxisX.from), mirror(grayscalePreview, pose.imageAxisX.to), black);
				cv::line(grayscalePreview, mirror(grayscalePreview, pose.imageAxisY.from), mirror(grayscalePreview, pose.imageAxisY.to), black);
			}

			cv::rectangle(grayscalePreview, toRealSizeRoi4LMDetector(tracker->getLastRegion()), 255);

			for (const auto& lm : landmarks) {
				cv::circle(grayscalePreview, lm, 2, black);
			}
		}

		landmarks.clear();
	}

	// add hysteresis for distance
	double targetMaxDistance = settings.maximalDistance;
	if (prevPosOutput == RealSenseSensor::POS_MOVE_CLOSER) {
		targetMaxDistance *= 0.96;
	}
	double targetMinDistance = settings.minimalDistance;
	if (prevPosOutput == RealSenseSensor::POS_MOVE_FAR) {
		targetMinDistance *= 1.04;
	}

	if (distance > targetMaxDistance) {
		positioningOutput = RealSenseSensor::POS_MOVE_CLOSER;
	}
	else if (distance < targetMinDistance) {
		positioningOutput = RealSenseSensor::POS_MOVE_FAR;
	}
	else {
		FacePoseEstimator::AlignInstruction instruction = facePoseEstimator->giveFaceAlignInstruction(pose);
		convert(instruction, positioningOutput);
	}

	if (tracker->positionUnsteady() || positioningOutput != RealSenseSensor::POS_DONTMOVE) {
		tracker->init();
	}

	if (positioningOutput == RealSenseSensor::POS_DONTMOVE && tracker->getConsecutiveDetects() >= settings.consecutiveDetectsToStartCapturing) {
		setState(RealSenseSensor::STATE_CAPTURING);
	}

	senseManager->ReleaseFrame();
//	std::cout << std::dec << "SingleCapture time: " << singleCaptureTime << "; FaceDetect time: " << faceDetectTime;
//	std::cout << std::dec << "; pose time: " << pose1Time << "/" << pose2Time << "; frame time: " << frameTime << std::endl;
}
Пример #15
0
int main(int argc, char** argv) {

	int maxManSpeed = 30000;

	try {

		TBS::initLogs("cvclient", 8);

		std::cout << "HAL Client Starts" << std::endl;
		Mat image(200, 200, CV_8UC3);
		Mat imageRadar(50, 200, CV_8UC3);

		cv::line(image, cv::Point(0, 100), cv::Point(200, 100), cv::Scalar(255, 0, 0), 1);
		cv::line(image, cv::Point(100, 0), cv::Point(100, 200), cv::Scalar(255, 0, 0), 1);

		namedWindow("movement", 0);
		setMouseCallback("movement", onMouse, 0);

		namedWindow("bioradar", 0);
		setMouseCallback("bioradar", onMouse2, 0);

		TBS::Services::JsonClientParams bp("localhost", HAL::API::Communication::BioRadarPort, TBS::Services::JsonClientParams::JsonHttp);
		bioRadarClient = new HAL::API::BioRadar::Json::Client(bp);

		TBS::Services::JsonClientParams cp("localhost", HAL::API::Communication::CameraPort, TBS::Services::JsonClientParams::JsonHttp);
		cameraClient = new HAL::API::Camera::Json::Client(cp);

		TBS::Services::JsonClientParams mc("localhost", HAL::API::Communication::ManipulatorPort, TBS::Services::JsonClientParams::JsonHttp);
		manipulatorClient = new HAL::API::Manipulator::Json::Client(mc);

		client = new HAL::API::MovementClient();
		client->Movement().StatusChanged += Poco::delegate(&updateSpeed);
		for (;;) {

			cv::Mat cpy = image.clone();
			drawSpeed(cpy);
			imshow("movement", cpy);
			imshow("bioradar", cpy);

			int c = waitKey(100);
			if ((c & 255) == 27) {
				cout << "Exiting ...\n";
				break;
			}

			try {
				static int speed = 0;
				static Poco::Timestamp tst;
				static int lastInRow = 0;
				if ((c & 255) == 'q') {
					lastInRow++;
					speed = 50;
					bioRadarClient->BioRadar().GoRelBase(speed);
				} else if ((c & 255) == 'a') {
					speed = -50;
					lastInRow++;
					bioRadarClient->BioRadar().GoRelBase(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						bioRadarClient->BioRadar().GoRelBase(speed);
					}
					lastInRow = 0;
				}
				//std::cout << "tst " << tst.elapsed() / 1000 << "ms - speed: " << speed << std::endl;
				tst.update();
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static Poco::Timestamp tst;
				static int lastInRow = 0;
				if ((c & 255) == 'w') {
					lastInRow++;
					speed = 20;
					bioRadarClient->BioRadar().GoRelAntenna(speed);
				} else if ((c & 255) == 's') {
					speed = -20;
					lastInRow++;
					bioRadarClient->BioRadar().GoRelAntenna(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						bioRadarClient->BioRadar().GoRelAntenna(speed);
					}
					lastInRow = 0;
				}
				//std::cout << "tst " << tst.elapsed() / 1000 << "ms - speed: " << speed << std::endl;
				tst.update();
			} catch (Poco::Exception & e) {

			}

			try {
				if ((c & 255) == 'e') {
					bioRadarClient->BioRadar().Enable();
				} else if ((c & 255) == 'd') {
					bioRadarClient->BioRadar().Disable();
				}
			} catch (Poco::Exception & e) {

			}

			try {
				if ((c & 255) == 'r') {

					HAL::API::BioRadar::MotorInfo base;
					HAL::API::BioRadar::MotorInfo antena;
					std::vector<HAL::API::BioRadar::TouchInfo> touchInfo;

					bioRadarClient->BioRadar().GetStatus(base, antena, touchInfo);

					std::cout << "Antena touch min: " << (antena.touchMin ? 1 : 0) << std::endl;
					std::cout << "Antena touch max: " << (antena.touchMax ? 1 : 0) << std::endl;
					std::cout << "Antena position: " << (antena.position) << std::endl;
					std::cout << "Antena error: " << (antena.positionError ? 1 : 0) << std::endl;
				}
			} catch (Poco::Exception & e) {

			}

			try {
				if ((c & 255) == '{') {
					cameraClient->Camera().Enable();
				} else if ((c & 255) == '}') {
					cameraClient->Camera().Disable();
				}
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == '+') {
					lastInRow++;
					speed = 100;
					cameraClient->Camera().GoRelEngine(speed);
				} else if ((c & 255) == '-') {
					speed = -100;
					lastInRow++;
					cameraClient->Camera().GoRelEngine(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						cameraClient->Camera().GoRelEngine(0);
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == '4') {
					lastInRow++;
					speed = 100;
					cameraClient->Camera().GoRelServo1(speed);
				} else if ((c & 255) == '6') {
					speed = -100;
					lastInRow++;
					cameraClient->Camera().GoRelServo1(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						cameraClient->Camera().GoRelServo1(0);
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == '8') {
					lastInRow++;
					speed = 100;
					cameraClient->Camera().GoRelServo2(speed);
				} else if ((c & 255) == '2') {
					speed = -100;
					lastInRow++;
					cameraClient->Camera().GoRelServo2(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						cameraClient->Camera().GoRelServo2(0);
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}

			try {
				if ((c & 255) == 'x') {
					manipulatorClient->Manipulator().Enable();
				} else if ((c & 255) == 'c') {
					manipulatorClient->Manipulator().Disable();
				}
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == 'v') {
					lastInRow++;
					speed = 50;
					manipulatorClient->Manipulator().StartRotation(speed);
				} else if ((c & 255) == 'b') {
					speed = -50;
					lastInRow++;
					manipulatorClient->Manipulator().StartRotation(speed);
				} else if ((c & 255) == 'n') {
					lastInRow++;
					speed = 50;
					manipulatorClient->Manipulator().StartRotationTo(speed, 1450);
				} else if ((c & 255) == 'm') {
					speed = -50;
					lastInRow++;
					manipulatorClient->Manipulator().StartRotationTo(speed, 1450);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						manipulatorClient->Manipulator().StopRotation();
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}
/*
			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == 'n') {
					lastInRow++;
					speed = 50;
					manipulatorClient->Manipulator().StartHolder(speed);
				} else if ((c & 255) == 'm') {
					speed = -50;
					lastInRow++;
					manipulatorClient->Manipulator().StartHolder(speed);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						manipulatorClient->Manipulator().StopHolder();
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}
*/
			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == 'j') {
					lastInRow++;
					speed = maxManSpeed;
					manipulatorClient->Manipulator().StartJoint1(speed);
				} else if ((c & 255) == 'k') {
					speed = -maxManSpeed;
					lastInRow++;
					manipulatorClient->Manipulator().StartJoint1(speed);
				} else if ((c & 255) == 'g') {
					lastInRow++;
					speed = maxManSpeed;
					manipulatorClient->Manipulator().StartJoint1To(speed, 3900);
				} else if ((c & 255) == 'h') {
					speed = -maxManSpeed;
					lastInRow++;
					manipulatorClient->Manipulator().StartJoint1To(speed, 3900);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						manipulatorClient->Manipulator().StopJoint1();
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}

			try {
				static int speed = 0;
				static int lastInRow = 0;
				if ((c & 255) == 'u') {
					lastInRow++;
					speed = maxManSpeed;
					manipulatorClient->Manipulator().StartJoint2(speed);
				} else if ((c & 255) == 'i') {
					speed = -maxManSpeed;
					lastInRow++;
					manipulatorClient->Manipulator().StartJoint2(speed);
				} else if ((c & 255) == 'o') {
					lastInRow++;
					speed = maxManSpeed;
					manipulatorClient->Manipulator().StartJoint2To(speed, 1100);
				} else if ((c & 255) == 'p') {
					speed = -maxManSpeed;
					lastInRow++;
					manipulatorClient->Manipulator().StartJoint2To(speed, 1100);
				} else {
					speed = 0;
					if (lastInRow > 1) {
						manipulatorClient->Manipulator().StopJoint2();
					}
					lastInRow = 0;
				}
			} catch (Poco::Exception & e) {

			}



			try {
				if ((c & 255) == ',') {
					manipulatorClient->Manipulator().LightOn();
				} else if ((c & 255) == '.') {
					manipulatorClient->Manipulator().LightOff();
				}
			} catch (Poco::Exception & e) {

			}

		}
		client->Movement().StatusChanged -= Poco::delegate(&updateSpeed);
	} catch (Poco::Exception & e) {
		std::cerr << "Exception: " << e.displayText() << std::endl;
	} catch (std::exception & e) {
		std::cerr << "Exception: " << e.what() << std::endl;
	}
	return 0;
}