std::vector<cv::Point2f> getChessboardCornersFileStream(
        cv::VideoCapture &capture,
        const std::vector<cv::Mat> &rectificationMaps,
        const int &horizon, const int &deadZone,
        const cv::Size &boardSize, const cv::Size &imageSize,
        const cv::Size &winSize, const cv::Size &zeroZone,
        const cv::TermCriteria &termCriteria) {
    cv::Mat newIn;
    cv::Mat undistorted;
    cv::Mat greyNewIn;
    cv::Mat ground;

    cv::Rect lowerRoi(cv::Point2f(0, horizon + deadZone),
            cv::Size(imageSize.width,
                    imageSize.height - horizon - deadZone));
    bool found = false;
    std::vector<cv::Point2f> corners;
    cv::namedWindow("ground", CV_WINDOW_NORMAL);
    char control = ' ';
    cv::Mat resizing;
    do {
        capture >> newIn;
        cv::resize(newIn, resizing, imageSize);
        cv::remap(resizing, undistorted, rectificationMaps[0],
                rectificationMaps[1], cv::INTER_LINEAR);
        cv::cvtColor(undistorted, greyNewIn, CV_RGB2GRAY);
        ground = greyNewIn(lowerRoi);
        found = cv::findChessboardCorners(ground, boardSize, corners,
                CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_ADAPTIVE_THRESH
                        | CV_CALIB_CB_NORMALIZE_IMAGE);
        if (corners.size() > 0) {
            drawChessboardCorners(undistorted(lowerRoi), boardSize,
                    corners, found);
            drawChessboardCorners(ground, boardSize, corners, found);
            std::cerr << corners.size() << std::endl;
        }
        drawDeadZoneHorizon(undistorted, horizon, deadZone);
        imshow("ground", ground);
        imshow("main", undistorted);
        control = cv::waitKey(1);
    } while (!found && 'q' != control);

    if ('q' == control) {
        cv::Exception ex(USER_TRIGGERED_EXIT, "user requested exit",
                __func__, __FILE__, __LINE__);
        throw ex;
    }
    cv::cornerSubPix(ground, corners, winSize, zeroZone,
            termCriteria);
    cv::imwrite("chessboard.jpeg", undistorted);
    cv::drawChessboardCorners(undistorted(lowerRoi), boardSize,
            corners, found);
    cv::imshow("main", undistorted);

    cv::destroyWindow("ground");
    cv::destroyWindow("main");

    return corners;
}
void calibrateParameters(cv::VideoCapture &capture,
        std::vector<cv::Mat> rectifyMaps, int &horizon, int &deadZone,
        const cv::Size &imageSize) {
    cv::Mat newIn;
    cv::Mat undistorted;
    cv::Mat far;
    cv::Mat ground;
    char control = ' ';
    cv::Rect lowerRoi;
    cv::Rect upperRoi;

    cv::namedWindow("ground", CV_WINDOW_NORMAL);
    cv::namedWindow("far", CV_WINDOW_NORMAL);
    cv::namedWindow("main", CV_WINDOW_NORMAL);
    cv::namedWindow("trackbars", CV_WINDOW_NORMAL);

    cv::createTrackbar("horizon", "trackbars", &horizon,
            imageSize.height);
    cv::createTrackbar("dead zone", "trackbars", &deadZone,
            imageSize.height / 2);
    cv::Mat resizing;
    std::cerr << imageSize.width << " " << imageSize.height
            << std::endl;
    do {
        capture >> newIn;
        cv::remap(newIn, undistorted, rectifyMaps[0], rectifyMaps[1],
                cv::INTER_LINEAR);
        lowerRoi = cv::Rect(cv::Point2f(0, horizon + deadZone),
                cv::Size(undistorted.cols,
                        undistorted.rows - horizon - deadZone));
        upperRoi = cv::Rect(cv::Point2f(0, 0),
                cv::Size(undistorted.cols, horizon - deadZone));
        ground = undistorted(lowerRoi);
        far = undistorted(upperRoi);
        drawDeadZoneHorizon(undistorted, horizon, deadZone);
        cv::line(undistorted, cv::Point(400, 0), cv::Point(400, 600),
                CV_RGB(255,0,0), 1, 8, 0);
        imshow("ground", ground);
        imshow("far", far);
        imshow("main", undistorted);

        control = cv::waitKey(1);
    } while ('s' != control && 'q' != control);

    if ('q' == control) {
        cv::Exception ex(USER_TRIGGERED_EXIT, "user commanded exit",
                __func__, __FILE__, __LINE__);
        throw ex;
    }
    cv::destroyWindow("ground");
    cv::destroyWindow("far");
    cv::destroyWindow("main");
}
예제 #3
0
    void Kinect::run() {
        while(!stop_) {
            libfreenect2::FrameMap frames;
            listener_.waitForNewFrame(frames);
            libfreenect2::Frame *rgb_frame = frames[libfreenect2::Frame::Color];
            libfreenect2::Frame *depth_frame = frames[libfreenect2::Frame::Depth];
            libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);

            registration_->apply(rgb_frame, depth_frame, &undistorted, &registered);
            listener_.release(frames);

            cv::Mat rgba((int)registered.height, (int)registered.width, CV_8UC4, registered.data);
            cv::Mat depth((int)undistorted.height, (int)undistorted.width , CV_32FC1, undistorted.data);

            cv::Mat rgb;
            //Should flip horizontally to get correct directions.
            cv::Mat rgb_flipped;
            cv::Mat depth_flipped;
            cv::cvtColor(rgba, rgb, CV_BGRA2BGR);
            //1 stands for the horizontal flip
            cv::flip(rgb, rgb_flipped, 1);
            cv::flip(depth, depth_flipped, 1);

            for(std::vector<boost::shared_ptr<KinectFrameListener> >::iterator iter = frame_listeners_.begin();
                iter!=frame_listeners_.end() ; iter++) {
                (*iter)->onFrame(rgb_flipped, depth_flipped);
            }
        }
    }
예제 #4
0
	void ImageCallback( const sensor_msgs::ImageConstPtr& msg,
	                    const sensor_msgs::CameraInfoConstPtr& info )
	{
		cv_bridge::CvImageConstPtr frame;
		try
		{
			frame = cv_bridge::toCvShare( msg, msg->encoding );
		}
		catch( cv_bridge::Exception& e )
		{
			ROS_ERROR( "cv_bridge exception: %s", e.what() );
			return;
		}
		
		camplex::CameraCalibration calib( "", *info );
		
		if( _useMaps && !_mapsInited )
		{
			_distMap1 = cv::Mat( frame->image.size(), CV_16SC2 );
			_distMap2 = cv::Mat( frame->image.size(), CV_16UC1 );
			cv::initUndistortRectifyMap( calib.GetIntrinsicMatrix(),
			                             calib.GetDistortionCoeffs(),
			                             cv::noArray(),
			                             calib.GetIntrinsicMatrix(),
			                             frame->image.size(),
			                             CV_16SC2,
			                             _distMap1,
			                             _distMap2 );
			_mapsInited = true;
		}

		cv::Mat undistorted( frame->image.size(), frame->image.type() );

		if( !_useMaps )
		{
			cv::undistort( frame->image, 
			               undistorted, 
			               calib.GetIntrinsicMatrix(), 
			               calib.GetDistortionCoeffs() );
		}
		else
		{
			cv::remap( frame->image, 
			           undistorted,
			           _distMap1, 
			           _distMap2, 
			           cv::INTER_LINEAR );
		}

		sensor_msgs::ImagePtr outImage = cv_bridge::CvImage( msg->header, 
		                                                     msg->encoding, 
		                                                     undistorted ).toImageMsg();
		sensor_msgs::CameraInfoPtr outInfo = boost::make_shared<sensor_msgs::CameraInfo>( *info );
		outInfo->D = std::vector<double>( 5, 0.0 );
		_imagePub.publish( outImage, outInfo );
	}
예제 #5
0
/**
 * Main application entry point.
 *
 * Accepted argumemnts:
 * - cpu Perform depth processing with the CPU.
 * - gl  Perform depth processing with OpenGL.
 * - cl  Perform depth processing with OpenCL.
 * - <number> Serial number of the device to open.
 * - -noviewer Disable viewer window.
 */
int main(int argc, char *argv[])
{
  std::string program_path(argv[0]);
  size_t executable_name_idx = program_path.rfind("Protonect");

  std::string binpath = "/";

  if(executable_name_idx != std::string::npos)
  {
    binpath = program_path.substr(0, executable_name_idx);
  }

  libfreenect2::Freenect2 freenect2;
  // create a console logger with debug level (default is console logger with info level)
  libfreenect2::setGlobalLogger(libfreenect2::createConsoleLogger(libfreenect2::Logger::Debug));
  MyFileLogger *filelogger = new MyFileLogger(getenv("LOGFILE"));
  if (filelogger->good())
    libfreenect2::setGlobalLogger(filelogger);

  libfreenect2::Freenect2Device *dev = 0;
  libfreenect2::PacketPipeline *pipeline = 0;

  if(freenect2.enumerateDevices() == 0)
  {
    std::cout << "no device connected!" << std::endl;
    return -1;
  }

  std::string serial = freenect2.getDefaultDeviceSerialNumber();

  bool viewer_enabled = true;

  for(int argI = 1; argI < argc; ++argI)
  {
    const std::string arg(argv[argI]);

    if(arg == "cpu")
    {
      if(!pipeline)
        pipeline = new libfreenect2::CpuPacketPipeline();
    }
    else if(arg == "gl")
    {
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenGLPacketPipeline();
#else
      std::cout << "OpenGL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "cl")
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenCLPacketPipeline();
#else
      std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg.find_first_not_of("0123456789") == std::string::npos) //check if parameter could be a serial number
    {
      serial = arg;
    }
    else if(arg == "-noviewer")
    {
      viewer_enabled = false;
    }
    else
    {
      std::cout << "Unknown argument: " << arg << std::endl;
    }
  }

  if(pipeline)
  {
    dev = freenect2.openDevice(serial, pipeline);
  }
  else
  {
    dev = freenect2.openDevice(serial);
  }

  if(dev == 0)
  {
    std::cout << "failure opening device!" << std::endl;
    return -1;
  }

  signal(SIGINT,sigint_handler);
  protonect_shutdown = false;

  libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
  libfreenect2::FrameMap frames;
  libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);

  dev->setColorFrameListener(&listener);
  dev->setIrAndDepthFrameListener(&listener);
  dev->start();

  std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
  std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;

  libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams());

  size_t framecount = 0;
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
  Viewer viewer;
  if (viewer_enabled)
    viewer.initialize();
#else
  viewer_enabled = false;
#endif

  while(!protonect_shutdown)
  {
    listener.waitForNewFrame(frames);
    libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
    libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
    libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];

    registration->apply(rgb, depth, &undistorted, &registered);

    framecount++;
    if (!viewer_enabled)
    {
      if (framecount % 100 == 0)
        std::cout << "The viewer is turned off. Received " << framecount << " frames. Ctrl-C to stop." << std::endl;
      listener.release(frames);
      continue;
    }

#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
    viewer.addFrame("RGB", rgb);
    viewer.addFrame("ir", ir);
    viewer.addFrame("depth", depth);
    viewer.addFrame("registered", &registered);

    protonect_shutdown = protonect_shutdown || viewer.render();
#endif

    listener.release(frames);
    //libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(100));
  }

  // TODO: restarting ir stream doesn't work!
  // TODO: bad things will happen, if frame listeners are freed before dev->stop() :(
  dev->stop();
  dev->close();

  delete registration;

  return 0;
}
예제 #6
0
/**
 * Main application entry point.
 *
 * Accepted argumemnts:
 * - cpu Perform depth processing with the CPU.
 * - gl  Perform depth processing with OpenGL.
 * - cl  Perform depth processing with OpenCL.
 * - <number> Serial number of the device to open.
 * - -noviewer Disable viewer window.
 * - -streamer Enable UDP Streaming of captured images.
 * - -recorder Enable recording of captured images.
 * - -replay Enable replay of captured images.
 */
int main(int argc, char *argv[])
/// [main]
{
  std::string program_path(argv[0]);
  std::cerr << "Version: " << LIBFREENECT2_VERSION << std::endl;
  std::cerr << "Environment variables: LOGFILE=<protonect.log>" << std::endl;
  std::cerr << "Usage: " << program_path << " [-gpu=<id>] [gl | cl | clkde | cuda | cudakde | cpu] [<device serial>]" << std::endl;
  std::cerr << "        [-noviewer] [-norgb | -nodepth] [-help] [-version]" << std::endl;
  std::cerr << "        [-recorder] [-streamer] [-replay]" << std::endl;
  std::cerr << "        [-frames <number of frames to process>]" << std::endl;
  std::cerr << "To pause and unpause: pkill -USR1 ProtonectSR" << std::endl;
  size_t executable_name_idx = program_path.rfind("ProtonectSR");

  const std::string prog(argv[0]);
  
  std::string binpath = "/";

  if(executable_name_idx != std::string::npos)
  {
    binpath = program_path.substr(0, executable_name_idx);
  }

#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__)
  // avoid flooing the very slow Windows console with debug messages
  libfreenect2::setGlobalLogger(libfreenect2::createConsoleLogger(libfreenect2::Logger::Info));
#else
  // create a console logger with debug level (default is console logger with info level)
/// [logging]
  libfreenect2::setGlobalLogger(libfreenect2::createConsoleLogger(libfreenect2::Logger::Debug));
/// [logging]
#endif
/// [file logging]
  MyFileLogger *filelogger = new MyFileLogger(getenv("LOGFILE"));
  if (filelogger->good())
    libfreenect2::setGlobalLogger(filelogger);
  else
    delete filelogger;
/// [file logging]

/// [context]
  libfreenect2::Freenect2 freenect2;
  // TODO: enable on merge
  //libfreenect2::Freenect2Replay freenect2replay;
  libfreenect2::Freenect2Device *dev = 0;
  libfreenect2::PacketPipeline *pipeline = 0;
/// [context]

  std::string serial = "";

  bool viewer_enabled = true;
  bool streamer_enabled = false;
  bool recorder_enabled = false;
  bool replay_enabled = false;
  bool enable_rgb = true;
  bool enable_depth = true;
  int deviceId = -1;
  size_t framemax = -1;

  for(int argI = 1; argI < argc; ++argI)
  {
    const std::string arg(argv[argI]);

    if(arg == "-help" || arg == "--help" || arg == "-h" || arg == "-v" || arg == "--version" || arg == "-version")
    {
      // Just let the initial lines display at the beginning of main
      return 0;
    }
    else if(arg.find("-gpu=") == 0)
    {
      if (pipeline)
      {
        std::cerr << "-gpu must be specified before pipeline argument" << std::endl;
        return -1;
      }
      deviceId = atoi(argv[argI] + 5);
    }
    else if(arg == "cpu")
    {
      if(!pipeline)
/// [pipeline]
        pipeline = new libfreenect2::CpuPacketPipeline();
/// [pipeline]
    }
    else if(arg == "gl")
    {
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenGLPacketPipeline();
#else
      std::cout << "OpenGL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "cl")
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenCLPacketPipeline(deviceId);
#else
      std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "clkde")
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenCLKdePacketPipeline(deviceId);
#else
      std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "cuda")
    {
#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::CudaPacketPipeline(deviceId);
#else
      std::cout << "CUDA pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "cudakde")
    {
#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::CudaKdePacketPipeline(deviceId);
#else
      std::cout << "CUDA pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg.find_first_not_of("0123456789") == std::string::npos) //check if parameter could be a serial number
    {
      serial = arg;
    }
    else if(arg == "-noviewer" || arg == "--noviewer")
    {
      viewer_enabled = false;
    }
    else if(arg == "-norgb" || arg == "--norgb")
    {
      enable_rgb = false;
    }
    else if(arg == "-nodepth" || arg == "--nodepth")
    {
      enable_depth = false;
    }
    else if(arg == "-frames")
    {
      ++argI;
      framemax = strtol(argv[argI], NULL, 0);
      if (framemax == 0) {
        std::cerr << "invalid frame count '" << argv[argI] << "'" << std::endl;
        return -1;
      }
    }
    else if(arg == "-streamer" || arg == "--streamer" || prog == "freenect2-stream")
    {
      streamer_enabled = true;
    }
    else if(arg == "-recorder" || arg == "--recorder" || prog == "freenect2-record")
    {
      recorder_enabled = true;
    }
    else if(arg == "-replay" || arg == "--replay" || prog == "freenect2-replay")
    {
      replay_enabled = true;
    }
    else
    {
      std::cout << "Unknown argument: " << arg << std::endl;
    }
  }

  if (!enable_rgb && !enable_depth)
  {
    std::cerr << "Disabling both streams is not allowed!" << std::endl;
    return -1;
  }

/// [discovery]
  if(replay_enabled == false)
  {
    if(freenect2.enumerateDevices() == 0)
    {
      std::cout << "no device connected!" << std::endl;
      return -1;
    }

    if(serial == "")
    {
      serial = freenect2.getDefaultDeviceSerialNumber();
    }
  }
/// [discovery]

  if(replay_enabled == false)
  {
    if(pipeline)
    {
/// [open]
      dev = freenect2.openDevice(serial, pipeline);
/// [open]
    }
    else
    {
      dev = freenect2.openDevice(serial);
    }
  }
  else
  {
    DIR *d;
    struct dirent *dir;

    std::vector<std::string> frame_filenames;

    d = opendir("recordings/depth");
    
    if(!d)
    {
      std::cerr << "Could not open directory " << dir << " for replay." << std::endl;
      exit(1);
    }
    
    while((dir = readdir(d)) != NULL)
    {
      std::string name = dir->d_name;
      
      if(hasSuffix(name, ".depth"))
      {
        frame_filenames.push_back(name);
      }
      else
      {
        std::cerr << "Skipping currently unsupported frame filename: " << name << std::endl;
      }
    }
    // TODO: enable on merge
/*    
    if(pipeline)
    {
/// [open]
      dev = freenect2replay.openDevice(frame_filenames, pipeline);
/// [open]
    }
    else
    {
      dev = freenect2replay.openDevice(frame_filenames);
    }
*/
  }

  if(dev == 0)
  {
    std::cout << "failure opening device!" << std::endl;
    return -1;
  }

  devtopause = dev;

  signal(SIGINT,sigint_handler);
#ifdef SIGUSR1
  signal(SIGUSR1, sigusr1_handler);
#endif
  protonect_shutdown = false;

/// [listeners]
  int types = 0;
  if (enable_rgb)
    types |= libfreenect2::Frame::Color;
  if (enable_depth)
    types |= libfreenect2::Frame::Ir | libfreenect2::Frame::Depth;
  libfreenect2::SyncMultiFrameListener listener(types);
  libfreenect2::FrameMap frames;

  dev->setColorFrameListener(&listener);
  dev->setIrAndDepthFrameListener(&listener);
/// [listeners]

/// [start]
  if (enable_rgb && enable_depth)
  {
    if (!dev->start())
      return -1;
  }
  else
  {
    if (!dev->startStreams(enable_rgb, enable_depth))
      return -1;
  }

  std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
  std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;
/// [start]

/// [registration setup]
  libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams());
  libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
/// [registration setup]

  size_t framecount = 0;
#ifdef EXAMPLES_WITH_OPENGL_SUPPORT
  Viewer viewer;
  if (viewer_enabled)
    viewer.initialize();
#else
  viewer_enabled = false;
#endif

  Streamer streamer; // have to declare it outside statements to be accessible everywhere
  Recorder recorder;

  if(streamer_enabled)
  {
    streamer.initialize();
  }

  if(recorder_enabled)
  {
    recorder.initialize();
  }

/// [loop start]
  while(!protonect_shutdown && (framemax == (size_t)-1 || framecount < framemax))
  {
    if (!listener.waitForNewFrame(frames, 10*1000)) // 10 sconds
    {
      std::cout << "timeout!" << std::endl;
      return -1;
    }
    libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
    libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
    libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
/// [loop start]

    if (enable_rgb && enable_depth)
    {
/// [registration]
      registration->apply(rgb, depth, &undistorted, &registered);
/// [registration]
    }

    framecount++;

    if (streamer_enabled)
    {
      streamer.stream(depth);
    }

    if (recorder_enabled)
    {
      // TODO: add recording timestamp if max frame number reached
      // + avoid recording new ones
      recorder.record(depth, "depth");
      recorder.record(&registered, "registered");
      // recorder.record(rgb,"rgb");

      recorder.registTimeStamp();
    }

    if (!viewer_enabled)
    {
      if (framecount % 100 == 0)
        std::cout << "The viewer is turned off. Received " << framecount << " frames. Ctrl-C to stop." << std::endl;
      listener.release(frames);
      continue;
    }

#ifdef EXAMPLES_WITH_OPENGL_SUPPORT
    if (enable_rgb)
    {
      viewer.addFrame("RGB", rgb);
    }
    if (enable_depth)
    {
      viewer.addFrame("ir", ir);
      viewer.addFrame("depth", depth);
    }
    if (enable_rgb && enable_depth)
    {
      viewer.addFrame("registered", &registered);
    }

    protonect_shutdown = protonect_shutdown || viewer.render();
#endif

/// [loop end]
    listener.release(frames);
    /** libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(100)); */
  }
/// [loop end]

  if (recorder_enabled)
  {
    recorder.saveTimeStamp();
  }

  // TODO: restarting ir stream doesn't work!
  // TODO: bad things will happen, if frame listeners are freed before dev->stop() :(
/// [stop]
  dev->stop();
  dev->close();
/// [stop]

  delete registration;

  return 0;
}
예제 #7
0
void Projector::reproject(bool gpuView)
{
	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	libfreenect2::FrameMap frames;
	SimpleViewer viewer;
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
	if (!gpuView) {
		cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
		cv::moveWindow("reprojection", 1200, 0);
		cv::setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	}
	else {
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	while (!shutdown)
	{
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
		std::vector<cv::Point3f> wrldSrc;
		if (!gpuView) cv::imshow("reprojection", board);
		(_listener)->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		for (int i = 0; i<512; i++)
		{
			for (int j = 0; j<424; j++)
			{
				float x = 0, y = 0, z = 0, color = 0;
				registration->getPointXYZRGB(&undistorted, &registered,
					i, j,
					x, y, z, color);

				if (z>0.5 && z<1.7)
				{
					x = static_cast<float>(x + right / ((double)640.0)); //////////TO-DO fix that
					y = static_cast<float>(y + up / ((double)480.0));

					x -= 0.5;
					y -= 0.5;
					double PI = 3.14159265;
					x = static_cast<float>(std::cos(rotX * PI / 180) * x - std::sin(rotX * PI / 180) * y);
					y = static_cast<float>(std::sin(rotX * PI / 180) * x + std::cos(rotX * PI / 180) * y);

					x += 0.5;
					y += 0.5;

					wrldSrc.push_back(cv::Point3f(x * 100,
						y * 100,
						z * 100));
				}
			}
		}
		
		if (wrldSrc.size() > 0) {
			std::vector<cv::Point2f> projected = projectPoints(wrldSrc);
			for (int i = 0; i < projected.size(); i++)
			{
				if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {

					cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
					ROI.setTo(cv::Scalar(100, 100, 150, 100));
				}
			}
			if (!gpuView) imshow("reprojection", board);
			else {
				libfreenect2::Frame b(640, 480, 4);
				b.data = board.data;
				viewer.addFrame("RGB", &b);
				shutdown = shutdown || viewer.render();
			}
		}
		(_listener)->release(frames);
		if (!gpuView) {
			int op = cv::waitKey(50);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;

			if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
			if (op == 102 || (char)(op) == 'f') rotX += 0.5;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				break;
			}
		}
		else {
			right = viewer.offsetX;
			up = viewer.offsetY;
			rotX = viewer.rot;
		}
	}
	if (!gpuView) cv::destroyWindow("reprojection");
	else {
		viewer.stopWindow();
	}
}
예제 #8
0
void Projector::showRectangle(bool gpuView)
{
	indices.resize(480);
	for (int i = 0; i < 480; i++) indices[i].resize(640);

	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	libfreenect2::FrameMap frames;
	SimpleViewer viewer;
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
	if (!gpuView) {
		cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
		cv::moveWindow("reprojection", 0, 0);
		//setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	}
	else {
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	while (!shutdown)
	{
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
		std::vector<cv::Point3f> wrldSrc;
		std::vector<cv::Point3f> plnSrc;
		if (!gpuView) cv::imshow("reprojection", board);
		(_listener)->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		
		for (int i = 0; i<512; i++)
		{
			for (int j = 0; j<424; j++)
			{
				float x = 0, y = 0, z = 0, color = 0;
				registration->getPointXYZRGB(&undistorted, &registered,
					i, j,
					x, y, z, color);

				if (z>0.5 && z<2.1)
				{
					x = static_cast<float>(x + right / ((double)640.0)); //////////TO-DO fix that
					y = static_cast<float>(y + up / ((double)480.0));

					x -= 0.5;
					y -= 0.5;
					double PI = 3.14159265;
					x = static_cast<float>(std::cos(rotX * PI / 180) * x - std::sin(rotX * PI / 180) * y);
					y = static_cast<float>(std::sin(rotX * PI / 180) * x + std::cos(rotX * PI / 180) * y);

					x += 0.5;
					y += 0.5;

					wrldSrc.push_back(cv::Point3f(x * 100,
						y * 100,
						z * 100));
				}
			}
		}
		
		PlaneData pln = findRectangle(registration, &undistorted, &registered);
		if (wrldSrc.size() > 0) {
			std::vector<cv::Point2f> projected = projectPoints(wrldSrc);
			for (int i = 0; i < projected.size(); i++)
			{
				if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {

					cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
					ROI.setTo(cv::Scalar(100, 100, 150, 100));
				}
			}
			if (pln.points.size() > 0) {
				projected = projectPoints(pln.points);
				cv::Mat cont = cv::Mat(480, 640, CV_8UC1, cv::Scalar::all(0));

				for (int i = 0; i < projected.size(); i++)
				{
					if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {
						
						cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
						ROI.setTo(cv::Scalar(250, 100, 100, 100));
						cont.at<uchar>(static_cast<int>(480 - projected[i].x), static_cast<int>(projected[i].y), 0) = 255;
						indices[static_cast<int>(480 - projected[i].x)][static_cast<int>(projected[i].y)] = i;
					}
				}
				vector<vector<cv::Point> > contours;
				vector<cv::Vec4i> hierarchy;
				cv::GaussianBlur(cont, cont, cv::Size(7, 7), 5, 11);
				findContours(cont, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));

				vector<vector<cv::Point> > contours_poly(contours.size());
				vector<cv::Rect> boundRect(contours.size());
				vector<cv::Point2f>center(contours.size());
				vector<float>radius(contours.size());
				int nPoly;
				for (int i = 0; i < contours.size(); i++)
				{
					cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 10, true);
					nPoly = contours_poly[i].size();
					
				}

				for (int i = 0; i< contours.size(); i++)
				{
					drawContours(board, contours_poly, 0, cv::Scalar(0, 255, 0), 5);
				}

			}
			if (!gpuView) imshow("reprojection", board);
			else {
				libfreenect2::Frame b(640, 480, 4);
				b.data = board.data;
				viewer.addFrame("RGB", &b);
				shutdown = shutdown || viewer.render();
			}
		}
		(_listener)->release(frames);
		if (!gpuView) {
			int op = cv::waitKey(50);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;
			if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
			if (op == 102 || (char)(op) == 'f') rotX += 0.5;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				break;
			}
		}
		else {
			right = 0;
			up = 0;
			rotX = 0;
			right = viewer.offsetX;
			up = viewer.offsetY;
			rotX = viewer.rot;
		}
	}
	if (!gpuView) cv::destroyWindow("reprojection");
	else {
		viewer.stopWindow();
	}
}
예제 #9
0
void Projector::objProjectionOffline(std::string objPath, std::string objName, bool gpuView)
{
	std::cout << "Camera init: ";
	objObject obj(objPath, objName);
	obj.loadData();
	cout << "DONE\n";
	cv::namedWindow("objTest", CV_WINDOW_NORMAL);
	cv::moveWindow("objTest", 0, 0);
	indices.resize(480);
	for (int i = 0; i < 480; i++) indices[i].resize(640);

	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	libfreenect2::FrameMap frames;
	SimpleViewer viewer;
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(255));
	cv::Vec3f prevNormal(-1, -1, -1);
	if (!gpuView) {
		cv::namedWindow("reprojection", CV_WINDOW_NORMAL);
		cv::moveWindow("reprojection", 200, 200);
		//setWindowProperty("reprojection", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	}
	else {
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	while (!shutdown)
	{
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(255));
		std::vector<cv::Point3f> plnSrc;
		if (!gpuView) cv::imshow("reprojection", board);
		(_listener)->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		PlaneData pln = findRectangle(registration, &undistorted, &registered);

		if (pln.points.size() > 0) {
			std::vector<cv::Point2f> projected = projectPoints(pln.points);
			cv::Mat cont = cv::Mat(480, 640, CV_8UC1, cv::Scalar::all(0));

			for (int i = 0; i < projected.size(); i++)
			{
				if (480 - projected[i].x >0 && projected[i].y > 0 && 480 - projected[i].x < 475 && projected[i].y < 630) {

					cv::Mat ROI = board(cv::Rect(static_cast<int>(projected[i].y), static_cast<int>(480 - projected[i].x), 2, 2));
					ROI.setTo(cv::Scalar(250, 100, 100, 100));
					cont.at<uchar>(static_cast<int>(480 - projected[i].x), static_cast<int>(projected[i].y), 0) = 255;
					plnSrc.push_back(pln.points[i]);
				}
			}
			vector<vector<cv::Point> > contours;
			vector<cv::Vec4i> hierarchy;
			cv::GaussianBlur(cont, cont, cv::Size(7, 7), 5, 11);
			findContours(cont, contours, hierarchy, cv::RETR_CCOMP, cv::CHAIN_APPROX_NONE, cv::Point(0, 0));

			vector<vector<cv::Point> > contours_poly(contours.size());
			vector<cv::Rect> boundRect(contours.size());
			vector<cv::Point2f>center(contours.size());
			vector<float>radius(contours.size());

			for (int i = 0; i < contours.size(); i++)
			{
				cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 10, true);
			}

			for (int i = 0; i < contours.size(); i++)
			{
				drawContours(board, contours_poly, 0, cv::Scalar(0, 255, 0), 5);
			}


			cv::Mat data_pts = cv::Mat(300, 3, CV_64FC1);
			cv::Vec3f normal(0, 0, 0);
			int jump = plnSrc.size() / 300;
			for (int i = 0; i < 100; i++) {
				data_pts.at<double>(i, 0) = plnSrc[i*jump].x;
				data_pts.at<double>(i, 1) = plnSrc[i*jump].y;
				data_pts.at<double>(i, 2) = plnSrc[i*jump].z;
				data_pts.at<double>(i + 100, 0) = plnSrc[(i + 100)*jump].x;
				data_pts.at<double>(i + 100, 1) = plnSrc[(i + 100)*jump].y;
				data_pts.at<double>(i + 100, 2) = plnSrc[(i + 100)*jump].z;
				data_pts.at<double>(i + 200, 0) = plnSrc[(i + 200) *jump].x;
				data_pts.at<double>(i + 200, 1) = plnSrc[(i + 200)*jump].y;
				data_pts.at<double>(i + 200, 2) = plnSrc[(i + 200)*jump].z;
			}

			cv::PCA pca_analysis(data_pts, cv::Mat(), CV_PCA_DATA_AS_ROW);
			cv::Vec3f cntr = cv::Vec3f((pca_analysis.mean.at<double>(0, 0)),
				(pca_analysis.mean.at<double>(0, 1)),
				(pca_analysis.mean.at<double>(0, 2)));

			vector<cv::Point3f> eigen_vecs(2);
			vector<double> eigen_val(2);
			for (int i = 0; i < 2; ++i)
			{
				eigen_vecs[i] = cv::Point3f(pca_analysis.eigenvectors.at<double>(i, 0),
					pca_analysis.eigenvectors.at<double>(i, 1),
					pca_analysis.eigenvectors.at<double>(i, 2));
				eigen_val[i] = pca_analysis.eigenvalues.at<double>(0, i);
			}
			cv::Vec3f p1 = cv::Vec3f((eigen_vecs[0].x * eigen_val[0]), (eigen_vecs[0].y * eigen_val[0]), (eigen_vecs[0].z * eigen_val[0]));
			cv::Vec3f p2 = cv::Vec3f((eigen_vecs[1].x * eigen_val[1]), (eigen_vecs[1].y * eigen_val[1]), (eigen_vecs[1].z * eigen_val[1]));
			normal = p1.cross(p2);
			normal = cv::normalize(normal);
			//pln.center = cntr;

			pln.normal = normal;
			obj.setCamera(cv::Point3f(pln.center.x, -pln.center.y, -pln.center.z + 150),
				cv::Vec3f(pln.normal[0], pln.normal[1], pln.normal[2]));

			if (!gpuView) imshow("reprojection", board);
			else {
				libfreenect2::Frame b(640, 480, 4);
				b.data = board.data;
				viewer.addFrame("RGB", &b);
				shutdown = shutdown || viewer.render();
			}
		}
		cv::Mat im = obj.render();
		cv::imshow("objTest", im);
		//}
		(_listener)->release(frames);
		if (!gpuView) {
			int op = cv::waitKey(50);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;

			if (op == 114 || (char)(op) == 'r') rotX -= 0.5;
			if (op == 102 || (char)(op) == 'f') rotX += 0.5;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				break;
			}
		}
		else {
			//right = 0;
			//up = 0;
			//rotX = 0;
			right = viewer.offsetX;
			up = viewer.offsetY;
			rotX = viewer.rot;
		}
	}
	if (!gpuView) cv::destroyWindow("reprojection");
	else {
		viewer.stopWindow();
	}
	cv::destroyWindow("objTest");
}
예제 #10
0
void Projector::ctProjection(std::string ctFilePath, int startPoint, int xDim, int yDim, int zDim, bool PNG)
{
	SimpleViewer viewer;
	CTObject ctObject(ctFilePath, xDim, yDim, zDim, startPoint);
	if(PNG)
		ctObject.readDataPNG();
	else
		ctObject.readData();
	libfreenect2::FrameMap frames;
	libfreenect2::Registration* registration = new libfreenect2::Registration(_dev->getIrCameraParams(), _dev->getColorCameraParams());
	libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
	bool shutdown = false;
	cv::Mat board(480, 640, CV_8UC4, cv::Scalar::all(0));
	//cv::namedWindow("CTviewer", CV_WINDOW_NORMAL);
	//cv::moveWindow("CTviewer", 00, 0);
	//cv::setWindowProperty("CTviewer", CV_WND_PROP_FULLSCREEN, CV_WINDOW_FULLSCREEN);
	{
		viewer.setSize(480, 640); // TO-DO change resolution
		viewer.initialize();
		libfreenect2::Frame b(640, 480, 4);
		b.data = board.data;
		viewer.addFrame("RGB", &b);
		shutdown = shutdown || viewer.render();
	}
	libfreenect2::Frame *rgb;
	libfreenect2::Frame *depth;
	
	(_listener)->waitForNewFrame(frames);
	rgb = frames[libfreenect2::Frame::Color];
	depth = frames[libfreenect2::Frame::Depth];
	registration->undistortDepth(depth, &undistorted);

	while (!shutdown)
	{
		std::thread *nextFrame = new std::thread[1];
		/*(_listener)->waitForNewFrame(frames);
		rgb = frames[libfreenect2::Frame::Color];
		depth = frames[libfreenect2::Frame::Depth];

		registration->undistortDepth(depth, &undistorted);*/
		//registration->apply(rgb, depth, &undistorted, &registered, true, NULL, NULL);

		//cv::Mat frame = frameToMat("registered", &registered);
		cv::Mat depthFrame;// = frameToMat("depth", depth);
		//cv::Mat board(424, 512, CV_8UC4, cv::Scalar::all(0));
		board = cv::Mat(480, 640, CV_8UC4, cv::Scalar::all(0));

		//depthFrame *= 0.001;
		cv::Mat temp_board(480, 640, CV_16UC3, cv::Scalar::all(0));

		int parts = 128;
		int jump = 512 / parts;
		
		int minX = 999;
		int maxX = -1;
		int minY = 999;
		int maxY = -1;
		std::thread *tt = new std::thread[parts];

		for (int i = 0; i < parts; ++i) {
			tt[i] = std::thread(tst, registration, &undistorted, &registered, jump * i, jump + jump * i, &board, &ctObject,
				right, up, _mr, _mt, _cam, _pro, &depthFrame, &temp_board, &minX, &maxX, &minY, &maxY);
		}
		
		for (int i = 0; i < parts; ++i)
			tt[i].join();
		//_listener->release(frames);
		nextFrame[0] = std::thread(loadNextFrame, _listener, &frames, rgb, depth, registration, &undistorted);

		int parts2 = 8;
		int jump2 = 512 / parts2;
		for (int ii = 0; ii < 3; ii++) {
			for (int i = 0; i < parts2; ++i) {
				tt[i] = std::thread(tt2, &temp_board, jump2 * i, jump2 + jump2 * i, cv::Size(3, 3), minX, maxX, minY, maxY);
			}

			for (int i = 0; i < parts2; ++i)
				tt[i].join();
		}

		
		//for (int i = 0; i < 3; i++)
			//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(3, 3));

		//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(3, 3));
		//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(7, 7));
		//temp_board = pseudoBoxFilter(temp_board, temp_board, cv::Size(7, 7));
		
		//cv::medianBlur(temp_board, temp_board, 5); // TO-DO check if needed
		//cv::GaussianBlur(temp_board, temp_board, cv::Size(3, 3), 0, 1);
		//int parts3 = 16;
		//int jump3 = 480 / parts3;
		//std::thread *tt2 = new std::thread[parts];
		//for (int i = 0; i < parts3; ++i) {
		//	tt2[i] = std::thread(tt3, &board, &temp_board, &ctObject, jump3 * i, jump3 + jump3 * i);
		//	//std::cout << jump3 * i << " " << jump3 + jump3 * i << "\n";
		//}

		//for (int i = 0; i < parts; ++i)
		//	tt2[i].join();
		//std::cout << "zuoooooo\n";
		/*for (int i = 0; i < 480; i++)
		{
			for (int j = 0; j < 640; j++)
			{
				cv::Mat ROI = board(cv::Rect(j, i, 1, 1));
				cv::Vec3s v = temp_board.at<cv::Vec3s>(i, j);
				unsigned char value = ctObject.at(v[0], v[1], v[2]);
				ROI.setTo(cv::Scalar(value, value, value, 100));
			}
		}*/

		
		//cv::imshow("CTviewer", board);
		
		{
			libfreenect2::Frame b(640, 480, 4);
			b.data = board.data;
			viewer.addFrame("RGB", &b);
			shutdown = shutdown || viewer.render();
		}

		//_listener->release(frames);
		nextFrame[0].join();
		delete[] tt;
		delete[] nextFrame;
		
		//delete[] tt2;
		/*{
			int op = cv::waitKey(1);
			if (op == 100 || (char)(op) == 'd') right -= 1;
			if (op == 115 || (char)(op) == 's') up += 1;
			if (op == 97 || (char)(op) == 'a') right += 1;
			if (op == 119 || (char)(op) == 'w') up -= 1;

			if (op == 1113997 || op == 1048586 || op == 1048608 || op == 10 || op == 32)
			{
				std::cout << "right = " << right << ";\nup = " << up << ";\nrotX = " << rotX << ";\n";
				shutdown = true;
				cv::destroyWindow("CTviewer");
			}
		}*/
	}


}
예제 #11
0
/**
 * Main application entry point.
 *
 * Accepted argumemnts:
 * - cpu Perform depth processing with the CPU.
 * - gl  Perform depth processing with OpenGL.
 * - cl  Perform depth processing with OpenCL.
 * - <number> Serial number of the device to open.
 * - -noviewer Disable viewer window.
 */
int main(int argc, char *argv[])
/// [main]
{
  std::string program_path(argv[0]);
  std::cerr << "Version: " << LIBFREENECT2_VERSION << std::endl;
  std::cerr << "Environment variables: LOGFILE=<protonect.log>" << std::endl;
  std::cerr << "Usage: " << program_path << " [gl | cl | cuda | cpu] [<device serial>]" << std::endl;
  std::cerr << "        [-noviewer] [-norgb | -nodepth] [-help] [-version]" << std::endl;
  std::cerr << "To pause and unpause: pkill -USR1 Protonect" << std::endl;
  size_t executable_name_idx = program_path.rfind("Protonect");

  std::string binpath = "/";

  if(executable_name_idx != std::string::npos)
  {
    binpath = program_path.substr(0, executable_name_idx);
  }

#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__)
  // avoid flooing the very slow Windows console with debug messages
  libfreenect2::setGlobalLogger(libfreenect2::createConsoleLogger(libfreenect2::Logger::Info));
#else
  // create a console logger with debug level (default is console logger with info level)
/// [logging]
  libfreenect2::setGlobalLogger(libfreenect2::createConsoleLogger(libfreenect2::Logger::Debug));
/// [logging]
#endif
/// [file logging]
  MyFileLogger *filelogger = new MyFileLogger(getenv("LOGFILE"));
  if (filelogger->good())
    libfreenect2::setGlobalLogger(filelogger);
  else
    delete filelogger;
/// [file logging]

/// [context]
  libfreenect2::Freenect2 freenect2;
  libfreenect2::Freenect2Device *dev = 0;
  libfreenect2::PacketPipeline *pipeline = 0;
/// [context]

  std::string serial = "";

  bool viewer_enabled = true;
  bool enable_rgb = true;
  bool enable_depth = true;

  for(int argI = 1; argI < argc; ++argI)
  {
    const std::string arg(argv[argI]);

    if(arg == "-help" || arg == "--help" || arg == "-h" || arg == "-v" || arg == "--version" || arg == "-version")
    {
      // Just let the initial lines display at the beginning of main
      return 0;
    }
    else if(arg == "cpu")
    {
      if(!pipeline)
/// [pipeline]
        pipeline = new libfreenect2::CpuPacketPipeline();
/// [pipeline]
    }
    else if(arg == "gl")
    {
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenGLPacketPipeline();
#else
      std::cout << "OpenGL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "cl")
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenCLPacketPipeline();
#else
      std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg == "cuda")
    {
#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::CudaPacketPipeline();
#else
      std::cout << "CUDA pipeline is not supported!" << std::endl;
#endif
    }
    else if(arg.find_first_not_of("0123456789") == std::string::npos) //check if parameter could be a serial number
    {
      serial = arg;
    }
    else if(arg == "-noviewer" || arg == "--noviewer")
    {
      viewer_enabled = false;
    }
    else if(arg == "-norgb" || arg == "--norgb")
    {
      enable_rgb = false;
    }
    else if(arg == "-nodepth" || arg == "--nodepth")
    {
      enable_depth = false;
    }
    else
    {
      std::cout << "Unknown argument: " << arg << std::endl;
    }
  }

  if (!enable_rgb && !enable_depth)
  {
    std::cerr << "Disabling both streams is not allowed!" << std::endl;
    return -1;
  }

/// [discovery]
  if(freenect2.enumerateDevices() == 0)
  {
    std::cout << "no device connected!" << std::endl;
    return -1;
  }

  if (serial == "")
  {
    serial = freenect2.getDefaultDeviceSerialNumber();
  }
/// [discovery]

  if(pipeline)
  {
/// [open]
    dev = freenect2.openDevice(serial, pipeline);
/// [open]
  }
  else
  {
    dev = freenect2.openDevice(serial);
  }

  if(dev == 0)
  {
    std::cout << "failure opening device!" << std::endl;
    return -1;
  }

  devtopause = dev;

  signal(SIGINT,sigint_handler);
#ifdef SIGUSR1
  signal(SIGUSR1, sigusr1_handler);
#endif
  protonect_shutdown = false;

/// [listeners]
  int types = 0;
  if (enable_rgb)
    types |= libfreenect2::Frame::Color;
  if (enable_depth)
    types |= libfreenect2::Frame::Ir | libfreenect2::Frame::Depth;
  libfreenect2::SyncMultiFrameListener listener(types);
  libfreenect2::FrameMap frames;

  dev->setColorFrameListener(&listener);
  dev->setIrAndDepthFrameListener(&listener);
/// [listeners]

/// [start]
  if (enable_rgb && enable_depth)
  {
    if (!dev->start())
      return -1;
  }
  else
  {
    if (!dev->startStreams(enable_rgb, enable_depth))
      return -1;
  }

  std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
  std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;
/// [start]

/// [registration setup]
  libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams());
  libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
/// [registration setup]

  size_t framecount = 0;
#ifdef EXAMPLES_WITH_OPENGL_SUPPORT
  Viewer viewer;
  if (viewer_enabled)
    viewer.initialize();
#else
  viewer_enabled = false;
#endif

/// [loop start]
  while(!protonect_shutdown)
  {
    listener.waitForNewFrame(frames);
    libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
    libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
    libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
/// [loop start]

    if (enable_rgb && enable_depth)
    {
/// [registration]
      registration->apply(rgb, depth, &undistorted, &registered);
/// [registration]
    }

    framecount++;
    if (!viewer_enabled)
    {
      if (framecount % 100 == 0)
        std::cout << "The viewer is turned off. Received " << framecount << " frames. Ctrl-C to stop." << std::endl;
      listener.release(frames);
      continue;
    }

#ifdef EXAMPLES_WITH_OPENGL_SUPPORT
    if (enable_rgb)
    {
      viewer.addFrame("RGB", rgb);
    }
    if (enable_depth)
    {
      viewer.addFrame("ir", ir);
      viewer.addFrame("depth", depth);
    }
    if (enable_rgb && enable_depth)
    {
      viewer.addFrame("registered", &registered);
    }

    protonect_shutdown = protonect_shutdown || viewer.render();
#endif

/// [loop end]
    listener.release(frames);
    /** libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(100)); */
  }
/// [loop end]

  // TODO: restarting ir stream doesn't work!
  // TODO: bad things will happen, if frame listeners are freed before dev->stop() :(
/// [stop]
  dev->stop();
  dev->close();
/// [stop]

  delete registration;

  return 0;
}
    //update the kinect
    void Device::updateKinect()
    {
        libfreenect2::FrameMap frames;
        

        //Temporary arrays
        float * newDepth = new float[FRAME_SIZE_DEPTH];
        float * newIr    = new float[FRAME_SIZE_DEPTH];
        float * newUndisorted =  new float[FRAME_SIZE_DEPTH];
        
        libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4);
                                                                 
        //MAIN THREAD
        while(initialized_device){
            listener->waitForNewFrame(frames);
            
            if(enableRegistered){
                
                libfreenect2::Frame *  rgb   = frames[libfreenect2::Frame::Color];
                memcpy(colorData, reinterpret_cast<const uint32_t *>(rgb->data), 1920 * 1080 * 4);
                
                libfreenect2::Frame *  depth = frames[libfreenect2::Frame::Depth];
                memcpy(newDepth, reinterpret_cast<const float * >(depth->data), FRAME_BYTE_SIZE_DEPTH);
                
                 //Mappers RGB + Depth
                registration->apply(rgb, depth, &undistorted, &registered);
                memcpy(newUndisorted, reinterpret_cast<const float * >(undistorted.data), FRAME_BYTE_SIZE_DEPTH);
                memcpy(registeredData, reinterpret_cast<const uint32_t * >(registered.data), FRAME_BYTE_SIZE_DEPTH);
            }else if(enableVideo && !enableDepth){
                
                libfreenect2::Frame *  rgb   = frames[libfreenect2::Frame::Color];
                memcpy(colorData, reinterpret_cast<const uint32_t *>(rgb->data), 1920 * 1080 * 4);
            }else if( !enableVideo && enableDepth ){
                
                libfreenect2::Frame *  depth = frames[libfreenect2::Frame::Depth];
                memcpy(newDepth, reinterpret_cast<const float * >(depth->data), FRAME_BYTE_SIZE_DEPTH);
            }else if(enableVideo && enableDepth && !enableRegistered){
                
                libfreenect2::Frame *  rgb   = frames[libfreenect2::Frame::Color];
                memcpy(colorData, reinterpret_cast<const uint32_t *>(rgb->data), 1920 * 1080 * 4);
                
                libfreenect2::Frame *  depth = frames[libfreenect2::Frame::Depth];
                memcpy(newDepth, reinterpret_cast<const float * >(depth->data), FRAME_BYTE_SIZE_DEPTH);
            }
        

            if(enableIR){
                libfreenect2::Frame *  ir    = frames[libfreenect2::Frame::Ir];
                memcpy(newIr, reinterpret_cast<const float * >(ir->data), FRAME_BYTE_SIZE_DEPTH);
            }
       
                int indexFD = 0;
                int pIndexEnd = (FRAME_SIZE_DEPTH);
                
                int indexX = 0;
                int indexY = 0;
                int cameraXYZ = 0;
                while(indexFD < pIndexEnd){
                    float depth = newDepth[indexFD];
                    
                    //Depth
                    //0.0566666f -> (value/45000)* 255
                    rawDepthData[indexFD] = uint32_t(depth);
                   
                    //IR
                    irData[indexFD]  = colorByte2Int((uint32_t(newIr[indexFD]*0.0566666f)>>2));
      
                    //undisorted
                    undisortedData[indexFD]  = colorByte2Int(uint32_t(newUndisorted[indexFD]*0.0566666f));
                    
                    
                    depthData[indexFD]  = colorByte2Int(uint32_t(depth*0.0566666f));
                    
                    //evaluates the depth XYZ position;
                   
                    depthCameraData[cameraXYZ++] = (indexX - dev->getIrCameraParams().cx) * depth / dev->getIrCameraParams().fx;//x
                    depthCameraData[cameraXYZ++] = (indexY - dev->getIrCameraParams().cy) * depth / dev->getIrCameraParams().fy; //y
                    depthCameraData[cameraXYZ++] = depth; //z
                    
                    indexX++;
                    if(indexX >= 512){ indexX=0; indexY++;}

                    indexFD++;
              //  }
            }
            
            
            //framw listener
            listener->release(frames);
        }
        
        //clean up
        if(newDepth != NULL) delete newDepth;
        if(newIr != NULL) delete newIr;
        if(newUndisorted != NULL) delete newUndisorted;
        
    }
예제 #13
0
//--------------------------------------------------------------------------------
void ofxKinectV2::threadedFunction() 
{
	libfreenect2::Frame undistorted(DEPTH_WIDTH, DEPTH_HEIGHT, 4), registered(DEPTH_WIDTH, DEPTH_HEIGHT, 4);

	while (isThreadRunning()) 
	{
		if (!bOpened) continue;

		listener->waitForNewFrame(frames);
		libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
		libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
		libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
		registration->apply(rgb, depth, &undistorted, &registered);

		frameColor[indexBack].setFromPixels(rgb->data, rgb->width, rgb->height, 4);
		frameIr[indexBack].setFromPixels((float *)ir->data, ir->width, ir->height, 1);
		frameRawDepth[indexBack].setFromPixels((float *)depth->data, depth->width, depth->height, 1);
		frameAligned[indexBack].setFromPixels(registered.data, registered.width, registered.height, 4);
		
		listener->release(frames);

		for (auto pixel : frameColor[indexBack].getPixelsIter()) // swap rgb
			std::swap(pixel[0], pixel[2]);
		for (auto pixel : frameIr[indexBack].getPixelsIter()) // downscale to 0-1
			pixel[0] /= 65535.0f;
		for (auto pixel : frameAligned[indexBack].getPixelsIter()) // swap rgb
			std::swap(pixel[0], pixel[2]);

		if(!bUseRawDepth) 
		{
			auto& depth = frameDepth[indexBack];
			auto& raw_depth = frameRawDepth[indexBack];
			if ((depth.getWidth() != raw_depth.getWidth()) || (depth.getHeight() != raw_depth.getHeight()))
			{
				depth.allocate(raw_depth.getWidth(), raw_depth.getHeight(), 3);
			}
			std::pair<float, float> minmax(0.0, 0.8);
			for (int i = 0; i < raw_depth.size(); i++)
			{
				float hue = ofMap(raw_depth[i], minDistance, maxDistance, minmax.first, minmax.second, true);
				if (hue == minmax.first || hue == minmax.second) depth.setColor(i * 3, ofColor(0));
				else depth.setColor(i * 3, ofFloatColor::fromHsb(hue, 0.9, 0.9));
			}
		}
		
		// get point cloud
		{
			float rgbPix = 0;
			size_t i = 0;
			for (int y = 0; y < DEPTH_HEIGHT; y++)
			{
				for (int x = 0; x < DEPTH_WIDTH; x++)
				{
					auto& pt = pcVertices[indexBack][i];
					registration->getPointXYZRGB(&undistorted, &registered, y, x, pt.x, pt.y, pt.z, rgbPix);
					pt.z *= -1.0f;
					pt.w = 1.0f;
					const uint8_t *p = reinterpret_cast<uint8_t*>(&rgbPix);
					pcColors[indexBack][i] = ofColor(p[2], p[1], p[0]);
					i++;
				}
			}
		}
		
		//while (bNewFrame)
		{
			// wait for main thread
		}

		std::lock_guard<std::mutex> guard(mutex);
		std::swap(indexFront, indexBack);
		bNewFrame = true;
	}
}