CameraFrame CameraPointGrey::getFrame(){ FlyCapture2::Error error; if(triggerMode == triggerModeSoftware){ // Fire software trigger // broadcasting not supported on some platforms cam.FireSoftwareTrigger(false); } // Retrieve the image FlyCapture2::Image rawImage; error = cam.RetrieveBuffer(&rawImage); if (error != FlyCapture2::PGRERROR_OK) PrintError(error); CameraFrame frame; frame.timeStamp = rawImage.GetTimeStamp().cycleCount; frame.height = rawImage.GetRows(); frame.width = rawImage.GetCols(); frame.memory = rawImage.GetData(); return frame; }
static bool frameToImage (FlyCapture2::Image * frame, sensor_msgs::Image & image) { // Get the raw image dimensions FlyCapture2::PixelFormat pixFormat; uint32_t rows, cols, stride; FlyCapture2::Image convertedImage; FlyCapture2::Error error; error = frame->Convert(FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage ); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return false; } // Get the raw image dimensions convertedImage.GetDimensions( &rows, &cols, &stride, &pixFormat ); return sensor_msgs::fillImage (image, sensor_msgs::image_encodings::RGB8, rows, cols, stride, convertedImage.GetData ()); }
/*! Acquire a color image from the active camera. \param I : Image data structure (RGBa image). \param timestamp : The acquisition timestamp. */ void vpFlyCaptureGrabber::acquire(vpImage<vpRGBa> &I, FlyCapture2::TimeStamp ×tamp) { this->open(); FlyCapture2::Error error; // Retrieve an image error = m_camera.RetrieveBuffer( &m_rawImage ); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); throw (vpException(vpException::fatalError, "Cannot retrieve image for camera with guid 0x%lx", m_guid) ); } timestamp = m_rawImage.GetTimeStamp(); // Create a converted image FlyCapture2::Image convertedImage; // Convert the raw image error = m_rawImage.Convert( FlyCapture2::PIXEL_FORMAT_RGBU, &convertedImage ); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); throw (vpException(vpException::fatalError, "Cannot convert image for camera with guid 0x%lx", m_guid) ); } height = convertedImage.GetRows(); width = convertedImage.GetCols(); unsigned char *data = convertedImage.GetData(); I.resize(height, width); unsigned int bps = convertedImage.GetBitsPerPixel(); memcpy(I.bitmap, data, width*height*bps/8); }
void VideoSource::GetAndFillFrameBWandRGB(CVD::Image<CVD::byte> &imBW, CVD::Image<CVD::Rgb<CVD::byte> > &imRGB) { Error error; imRGB.resize(mirSize); imBW.resize(mirSize); static FlyCapture2::Image imCap; error = cam.RetrieveBuffer(&imCap); if (error != PGRERROR_OK) { PrintErrorAndExit(error); } unsigned char *pImage = imCap.GetData(); for (int y = 0; y<mirSize.y; y++) { for (int x = 0; x<mirSize.x; x++) { imRGB[y][x].blue = *pImage; pImage++; imRGB[y][x].green =*pImage; imBW[y][x] = *pImage; pImage++; imRGB[y][x].red = *pImage; pImage++; } } }
void CCaptureGUIView::setFlyImage(const FlyCapture2::Image& image) { unsigned int rows, cols, stride; FlyCapture2::PixelFormat format; image.GetDimensions(&rows, &cols, &stride, &format); m_bmpmutex.lock(); auto error = image.Convert(FlyCapture2::PIXEL_FORMAT_BGR, &fly_image); m_bmpmutex.unlock(); assert(error != FlyCapture2::PGRERROR_OK); // error check }
void CCflycap_grab_next_frame_blocking_with_stride( CCflycap *ccntxt, unsigned char *out_bytes, intptr_t stride0, float timeout ) { CHECK_CC(ccntxt); FlyCapture2::Camera *cam = (FlyCapture2::Camera *)ccntxt->inherited.cam; cam_iface_backend_extras* backend_extras = (cam_iface_backend_extras*)(ccntxt->inherited.backend_extras); // The main frame grabbing code goes here. FlyCapture2::Image rawImage; CIPGRCHK(cam->RetrieveBuffer( &rawImage )); if (stride0 < (intptr_t)rawImage.GetStride()) { CAM_IFACE_THROW_ERROR("stride too small for image"); } if (stride0==(intptr_t)rawImage.GetStride()) { // same stride memcpy((void*)out_bytes, /*dest*/ (const void*)rawImage.GetData(),/*src*/ rawImage.GetDataSize());/*src*/ } else { // different strides for (int row=0; row<(int)rawImage.GetRows(); row++) { memcpy((void*)(out_bytes+row*stride0), /*dest*/ (const void*)(rawImage.GetData() + row*rawImage.GetStride()),/*src*/ rawImage.GetStride());/*size*/ } } }
bool Camera::saveImageToFile(FlyCapture2::Image * rawImage) { static int imageCount=0; FlyCapture2::Error error; FlyCapture2::CameraInfo camInfo; error = camPGR_.GetCameraInfo(&camInfo); // Get the raw image dimensions FlyCapture2::PixelFormat pixFormat; unsigned int rows, cols, stride; rawImage->GetDimensions( &rows, &cols, &stride, &pixFormat ); // Create a converted image FlyCapture2::Image convertedImage; // Convert the raw image error = rawImage->Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &convertedImage ); if (error != PGRERROR_OK) { PrintError( error ); return false; } // Create a unique filename char filename[512]; sprintf( filename, "/tmp/%u-%d.bmp", camInfo.serialNumber, imageCount ); ROS_INFO("Filename: %s", filename); // Save the image. If a file format is not passed in, then the file // extension is parsed to attempt to determine the file format. error = convertedImage.Save( filename ); if (error != PGRERROR_OK) { PrintError( error ); } imageCount++; return true; }
void CCflycap_CCflycap( CCflycap * ccntxt, int device_number, int NumImageBuffers, int mode_number) { // call parent CamContext_CamContext((CamContext*)ccntxt,device_number,NumImageBuffers,mode_number); // XXX cast error? ccntxt->inherited.vmt = (CamContext_functable*)&CCflycap_vmt; CAM_IFACE_CHECK_DEVICE_NUMBER(device_number); std::vector<CamMode> result; int myerr = get_mode_list(device_number, result); ccntxt->inherited.device_number = device_number; ccntxt->inherited.backend_extras = new cam_iface_backend_extras; memset(ccntxt->inherited.backend_extras,0,sizeof(cam_iface_backend_extras)); FlyCapture2::Camera *cam = new FlyCapture2::Camera; FlyCapture2::PGRGuid guid; CIPGRCHK( BACKEND_GLOBAL(busMgr_ptr)->GetCameraFromIndex(device_number, &guid)); CIPGRCHK(cam->Connect(&guid)); FlyCapture2::FC2Config cfg; CIPGRCHK(cam->GetConfiguration(&cfg)); cfg.numBuffers = NumImageBuffers; CIPGRCHK(cam->SetConfiguration(&cfg)); // Set the settings to the camera CamMode target_mode = result[mode_number]; if (target_mode.videomode == FlyCapture2::VIDEOMODE_FORMAT7) { CIPGRCHK(cam->SetFormat7Configuration(&target_mode.fmt7ImageSettings, target_mode.fmt7PacketInfo.recommendedBytesPerPacket )); } else { CIPGRCHK(cam->SetVideoModeAndFrameRate(target_mode.videomode, target_mode.framerate)); } ccntxt->inherited.cam = (void*)cam; // XXX move this to start camera and query camera for settings CIPGRCHK(cam->StartCapture()); // Retrieve an image to get width, height. XXX change to query later. FlyCapture2::Image rawImage; CIPGRCHK( cam->RetrieveBuffer( &rawImage )); cam_iface_backend_extras *extras = (cam_iface_backend_extras *)ccntxt->inherited.backend_extras; ccntxt->inherited.depth = rawImage.GetBitsPerPixel(); extras->buf_size = rawImage.GetDataSize(); extras->current_height = rawImage.GetRows(); extras->current_width = rawImage.GetCols(); extras->max_height = rawImage.GetRows(); extras->max_width = rawImage.GetCols(); CIPGRCHK( cam->GetTriggerModeInfo( &extras->trigger_mode_info )); switch (rawImage.GetPixelFormat()) { case FlyCapture2::PIXEL_FORMAT_MONO8: ccntxt->inherited.coding = CAM_IFACE_MONO8; if (rawImage.GetBayerTileFormat()!=FlyCapture2::NONE) { NOT_IMPLEMENTED; } break; case FlyCapture2::PIXEL_FORMAT_RAW8: switch (rawImage.GetBayerTileFormat()) { case FlyCapture2::NONE: ccntxt->inherited.coding = CAM_IFACE_RAW8; break; case FlyCapture2::RGGB: ccntxt->inherited.coding = CAM_IFACE_MONO8_BAYER_RGGB; break; case FlyCapture2::GRBG: ccntxt->inherited.coding = CAM_IFACE_MONO8_BAYER_GRBG; break; case FlyCapture2::GBRG: ccntxt->inherited.coding = CAM_IFACE_MONO8_BAYER_GBRG; break; case FlyCapture2::BGGR: ccntxt->inherited.coding = CAM_IFACE_MONO8_BAYER_BGGR; break; default: NOT_IMPLEMENTED; } } CIPGRCHK(cam->StopCapture()); }
static bool frameToImage (FlyCapture2::Image * frame, sensor_msgs::Image & image) { std::string encoding; // Get the raw image dimensions FlyCapture2::PixelFormat pixFormat; uint32_t rows, cols, stride; // NOTE: 16-bit and Yuv formats not supported static const char *BAYER_ENCODINGS[] = { "none", "bayer_rggb8", "bayer_grbg8", "bayer_gbrg8", "bayer_bggr8", "unknown" }; //unsigned int bpp = frame->GetBitsPerPixel(); FlyCapture2::BayerTileFormat bayerFmt; bayerFmt = frame->GetBayerTileFormat (); //ROS_INFO("bayer is %u", bayerFmt); frame->GetDimensions( &rows, &cols, &stride, &pixFormat ); //ROS_INFO("Frame WxH = %d x %d", cols, rows); // FIXME: Dynamic resizing of camera is now working //ROS_INFO("Pixel Format (from Raw Image): 0x%08x", pixFormat); //ROS_INFO("Stride of Raw Image (The number of bytes between rows of the image): %d", stride); if (bayerFmt == FlyCapture2::NONE) { //ROS_INFO("Pixel Format (from Raw Image): 0x%08x", pixFormat); encoding = sensor_msgs::image_encodings::MONO8; return sensor_msgs::fillImage (image, encoding, rows, cols, stride, frame->GetData ()); } else { // Create a converted image FlyCapture2::Image convertedImage; FlyCapture2::Error error; //encoding = BAYER_ENCODINGS[bayerFmt]; switch ((FlyCapture2::PixelFormat) pixFormat) { case FlyCapture2::PIXEL_FORMAT_RAW8: // Convert the raw image error = frame->Convert(FlyCapture2::PIXEL_FORMAT_RGB, &convertedImage ); encoding = sensor_msgs::image_encodings::RGB8; // Arbitrary encoding based on pixel format if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return false; } // Get the raw image dimensions convertedImage.GetDimensions( &rows, &cols, &stride, &pixFormat ); break; default: return false; // FIXME: Now, it's arbitrarily converting Raw images to RGB8 encoding // TODO: Handle encoding directly from Bayer break; } // switch //ROS_INFO("Stride of Converted Image (The number of bytes between rows of the image): %d", stride); return sensor_msgs::fillImage (image, encoding, rows, cols, stride, convertedImage.GetData ()); } //else }
/*! @brief open camera @param[out] w image width @param[out] h image height @param[out] c num of channels @retval successed or not */ bool CCam::Open( int &w, int &h, int &c ) { #ifdef USEPGR if (!mPGRCap.IsConnected()){ FlyCapture2::Error error; // connect to a camera error = mPGRCap.Connect(0); if (error != FlyCapture2::PGRERROR_OK){ return false; } FlyCapture2::CameraInfo camInfo; error = mPGRCap.GetCameraInfo(&camInfo); if (error != FlyCapture2::PGRERROR_OK) { return false; } // start capturing error = mPGRCap.StartCapture(); if (error != FlyCapture2::PGRERROR_OK){ return false; } // get image FlyCapture2::Image raw; error = mPGRCap.RetrieveBuffer(&raw); if (error != FlyCapture2::PGRERROR_OK){ return false; } // VGA w = mWidth = raw.GetCols() / 2; // image width h= mHeight = raw.GetRows() / 2; // image height c= mChannel = 3; // color camera mSize = mWidth*mHeight*mChannel; mImg = cv::Mat(cv::Size(mWidth, mHeight), CV_8UC3); return true; } else{ return false; } #else if (!mCap.isOpened()){ if (!mCap.open(mDeviceID)){ // if opening failed printf("Cam ID %d not found\n", mDeviceID); return false; } // VGA mCap.set(cv::CAP_PROP_FRAME_WIDTH, 640); mCap.set(cv::CAP_PROP_FRAME_HEIGHT, 480); // check image int count = 0; while (mImg.data == NULL){ mCap >> mImg; ++count; if (count > 10){ // if retrieval failed printf("Cannot retrieve images\n"); return false; } } mWidth = w = mImg.cols; mHeight = h = mImg.rows; mChannel = c = mImg.channels(); mSize = mHeight*(int)mImg.step; return true; } else{ return false;
int main() { FlyCapture2::Error error; FlyCapture2::PGRGuid guid; FlyCapture2::BusManager busMgr; FlyCapture2::Camera cam; FlyCapture2::VideoMode vm; FlyCapture2::FrameRate fr; FlyCapture2::Image rawImage; //Initializing camera error = busMgr.GetCameraFromIndex(0, &guid); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return CAM_FAILURE; } vm = FlyCapture2::VIDEOMODE_640x480Y8; fr = FlyCapture2::FRAMERATE_60; error = cam.Connect(&guid); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return CAM_FAILURE; } cam.SetVideoModeAndFrameRate(vm, fr); //Starting the capture error = cam.StartCapture(); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return CAM_FAILURE; } while(1) { Mat frame (Size(640,480),CV_8UC1); // Mat img_scene (Size(640,480),CV_8UC3); Mat img_object = imread( "bitslogo.png", CV_LOAD_IMAGE_GRAYSCALE ); cam.RetrieveBuffer(&rawImage); memcpy(frame.data, rawImage.GetData(), rawImage.GetDataSize()); cvtColor(frame,frame,CV_BayerBG2RGB); imwrite("temp.bmp",frame); // cvtColor(img_scene,img_scene,CV_RGB2GRAY); Mat img_scene = imread( "temp.bmp", CV_LOAD_IMAGE_GRAYSCALE ); if( !img_object.data || !img_scene.data ) { std::cout<< " --(!) Error reading images " << std::endl; return -1; } //-- Step 1: Detect the keypoints using SURF Detector int minHessian = 400; SurfFeatureDetector detector( minHessian ); std::vector<KeyPoint> keypoints_object, keypoints_scene; detector.detect( img_object, keypoints_object ); detector.detect( img_scene, keypoints_scene ); //-- Step 2: Calculate descriptors (feature vectors) SurfDescriptorExtractor extractor; Mat descriptors_object, descriptors_scene; extractor.compute( img_object, keypoints_object, descriptors_object ); extractor.compute( img_scene, keypoints_scene, descriptors_scene ); //-- Step 3: Matching descriptor vectors using FLANN matcher FlannBasedMatcher matcher; std::vector< DMatch > matches; matcher.match( descriptors_object, descriptors_scene, matches ); double max_dist = 0; double min_dist = 100; //-- Quick calculation of max and min distances between keypoints for( int i = 0; i < descriptors_object.rows; i++ ) { double dist = matches[i].distance; if( dist < min_dist ) min_dist = dist; if( dist > max_dist ) max_dist = dist; } printf("-- Max dist : %f \n", max_dist ); printf("-- Min dist : %f \n", min_dist ); //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist ) std::vector< DMatch > good_matches; for( int i = 0; i < descriptors_object.rows; i++ ) { if( matches[i].distance < 3*min_dist ) { good_matches.push_back( matches[i]); } } Mat img_matches; drawMatches( img_object, keypoints_object, img_scene, keypoints_scene, good_matches, img_matches, Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS ); //-- Localize the object std::vector<Point2f> obj; std::vector<Point2f> scene; for( int i = 0; i < good_matches.size(); i++ ) { //-- Get the keypoints from the good matches obj.push_back( keypoints_object[ good_matches[i].queryIdx ].pt ); scene.push_back( keypoints_scene[ good_matches[i].trainIdx ].pt ); } Mat H = findHomography( obj, scene, CV_RANSAC ); //-- Get the corners from the image_1 ( the object to be "detected" ) std::vector<Point2f> obj_corners(4); obj_corners[0] = cvPoint(0,0); obj_corners[1] = cvPoint( img_object.cols, 0 ); obj_corners[2] = cvPoint( img_object.cols, img_object.rows ); obj_corners[3] = cvPoint( 0, img_object.rows ); std::vector<Point2f> scene_corners(4); perspectiveTransform( obj_corners, scene_corners, H); //-- Draw lines between the corners (the mapped object in the scene - image_2 ) line( img_matches, scene_corners[0] + Point2f( img_object.cols, 0), scene_corners[1] + Point2f( img_object.cols, 0), Scalar(0, 255, 0), 4 ); line( img_matches, scene_corners[1] + Point2f( img_object.cols, 0), scene_corners[2] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 ); line( img_matches, scene_corners[2] + Point2f( img_object.cols, 0), scene_corners[3] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 ); line( img_matches, scene_corners[3] + Point2f( img_object.cols, 0), scene_corners[0] + Point2f( img_object.cols, 0), Scalar( 0, 255, 0), 4 ); //-- Show detected matches imshow( "Good Matches & Object detection", img_matches ); int c = cvWaitKey(100); if(c == 27) break; } return 0; }
int main() { FlyCapture2::Error error; FlyCapture2::PGRGuid guid; FlyCapture2::BusManager busMgr; FlyCapture2::Camera cam; FlyCapture2::VideoMode vm; FlyCapture2::FrameRate fr; FlyCapture2::Image rawImage; //Initializing camera error = busMgr.GetCameraFromIndex(0, &guid); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return CAM_FAILURE; } vm = FlyCapture2::VIDEOMODE_640x480Y8; fr = FlyCapture2::FRAMERATE_60; error = cam.Connect(&guid); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return CAM_FAILURE; } cam.SetVideoModeAndFrameRate(vm, fr); //Starting the capture error = cam.StartCapture(); if (error != FlyCapture2::PGRERROR_OK) { error.PrintErrorTrace(); return CAM_FAILURE; } //Image Variables Mat frame (Size(640,480),CV_8UC1); Mat img_bgr (Size(640,480),CV_8UC3); Mat img_hsv (Size(640,480),CV_8UC3); //Image in HSV color space Mat threshy (Size(640,480),CV_8UC1); //Threshed Image // Mat labelImg (Size(640,480),CV_8UC1); //Image Variable for blobs IplImage* histogram= cvCreateImage(cvSize(640,480),8,3); //Image histograms Mat histograms (Size(640,480),CV_8UC1); //greyscale image for histogram Mat empty (Size(640,480),CV_8UC3); // CvBlobs blobs; while(1) { cam.RetrieveBuffer(&rawImage); memcpy(frame.data, rawImage.GetData(), rawImage.GetDataSize()); // histogram = empty.clone(); cvZero(histogram); // cvAddS(frame, cvScalar(70,70,70), frame); cvtColor(frame,img_bgr,CV_BayerBG2BGR); // cout<<"\n1"; cvtColor(img_bgr,img_hsv,CV_BGR2HSV); // cout<<"\n2"; //Thresholding the frame for yellow inRange(img_hsv, Scalar(20, 100, 20), Scalar(70, 255, 255), threshy); // cvInRangeS(img_hsv, cvScalar(0, 120, 100), cvScalar(255, 255, 255), threshy); //Filtering the frame - subsampling?? // smooth(threshy,threshy,CV_MEDIAN,7,7); //Finding the blobs // unsigned int result=cvLabel(threshy,labelImg,blobs); //Filtering the blobs // cvFilterByArea(blobs,500,1000); //Rendering the blobs // cvRenderBlobs(labelImg,blobs,img_bgr,img_bgr); CvPoint prev = cvPoint(0,0); for(int x=0;x<640;++x) { Mat col = threshy.col(x); int y = 480 - countNonZero(col); for(int i=0 ; i<480 ; ++i) histograms.at<uchar>(x,i) = y; CvPoint next = cvPoint(x,y); cvLine(histogram,prev,next,cColor); // cvCircle(histogram,next,2,cColor,3); prev=next; } //Showing the images imshow("Live",img_bgr); imshow("Threshed",threshy); cvShowImage("Histogram",histogram); int c= cvWaitKey(10); if(c == 27) break; } //Cleanup // cvReleaseImage(&frame); // cvReleaseImage(&threshy); // cvReleaseImage(&img_hsv); // cvReleaseImage(&labelImg); // cvReleaseImage(&histogram); cvDestroyAllWindows(); return 0; }
int main( int /*argc*/, char** /*argv*/ ) { //!< load camera calibration info cv::Mat mx1, my1, mx2, my2, Q; loadCalibrationInfo(mx1, my1, mx2, my2, Q); //!< connect to cameras unsigned int numCameras; FlyCapture2::Camera** ppCameras; prepareCameras( numCameras, ppCameras ); //!< Allocate images //!< an image par camera: a vector of pointers to images (cv::Mat) std::vector< std::shared_ptr< cv::Mat > > pimageCamera; for ( unsigned int i = 0; i < numCameras; i++ ) pimageCamera.push_back( std::shared_ptr< cv::Mat > ( new cv::Mat(480, 640, CV_8UC1) ) ); //!< create cv::Mat*, cast to shared_ptr, then push_back //!< disparity maps cv::Mat imageDisparity(480, 640, CV_32F), disp8U; //!< OpenCV control panel: block stereo matching parameters cv::namedWindow("control panel", cv::WINDOW_NORMAL ); int ndisparities = 5; cv::createTrackbar( "(n+1)x16=ndisparities", "control panel", &ndisparities, 20, NULL ); //!< ndisparities % 16 == 0, positive, ndisparities < width int blockSize = 5; cv::createTrackbar( "(s+2)x2+1=blockSize", "control panel", &blockSize, 30, NULL ); //!< 5 <= blocksize <= 255, odd #define ADDBAR( name1 , name2 , init, maxval ) \ int name1 = 5; \ cv::createTrackbar( #name1, "control panel", &name1, maxval, NULL ); \ sbm->name2( name1 ); #ifdef USE_OPENCV_SBM //!< sample code from https://github.com/Itseez/opencv/blob/52a785e95a30d9336bfbac97a0a0d0089ffaa7de/samples/cpp/stereo_match.cpp //!< stereo matching object cv::Ptr< cv::StereoBM > sbm = cv::createStereoBM( ndisparities, blockSize ); //sbm->setPreFilterType( cv::StereoBM::PREFILTER_NORMALIZED_RESPONSE ); //sbm->setPreFilterType( cv::StereoBM::PREFILTER_XSOBEL ); ADDBAR( textureThreshold, setTextureThreshold, 5, 30 ); ADDBAR( smallerBlockSize, setSmallerBlockSize, 5, 30 ); #endif #ifdef USE_OPENCV_SGBM //!< sample code from https://github.com/Itseez/opencv/blob/52a785e95a30d9336bfbac97a0a0d0089ffaa7de/samples/cpp/stereo_match.cpp cv::Ptr< cv::StereoSGBM > sbm = cv::createStereoSGBM( 0, ndisparities, blockSize ); //!< parameters from http://www.jayrambhia.com/blog/disparity-maps/ ADDBAR( P1, setP1, 500, 1000 ); ADDBAR( P2, setP2, 2000, 3000 ); #endif #if defined(USE_OPENCV_SBM) || defined(USE_OPENCV_SGBM) ADDBAR( preFilterCap, setPreFilterCap, 4, 20 ); //sbm->setMinDisparity( 0 ); //sbm->setNumDisparities( ndisparities ); ADDBAR( uniquenessRatio, setUniquenessRatio, 10, 20 ); ADDBAR( speckleWindowSize, setSpeckleWindowSize, 150, 200 ); ADDBAR( speckleRange, setSpeckleRange, 2, 10 ); ADDBAR( disp12MaxDiff, setDisp12MaxDiff, 5, 20 ); #endif #ifdef USE_ELAS Elas::parameters param; param.postprocess_only_left = true; param.disp_min = 0; param.disp_max = (ndisparities + 1) * 16; const int32_t dims[3] = {640, 480, 640}; Elas elas( param ); //float* D1_data = (float*)malloc(640*480*sizeof(float)); //float* D2_data = (float*)malloc(width*height*sizeof(float)); #endif pcl::PointCloud< pcl::PointXYZ >::Ptr pointCloudFromDepth_ptr ( new pcl::PointCloud< pcl::PointXYZ > ); { pcl::PointXYZ initPoint; initPoint.x = 0; initPoint.y = 0; initPoint.z = 0; pointCloudFromDepth_ptr->points.push_back( initPoint ); pointCloudFromDepth_ptr->width = (int) pointCloudFromDepth_ptr->points.size (); pointCloudFromDepth_ptr->height = 1; } boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0, 0, 0); viewer->addPointCloud<pcl::PointXYZ> (pointCloudFromDepth_ptr, "my points"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "my points"); viewer->addCoordinateSystem (1.0, "global"); viewer->initCameraParameters (); while ( 1 ) { //!< Display the timestamps for all cameras to show that the image //!< capture is synchronized for each image #if defined(USE_OPENCV_SBM) sbm->setTextureThreshold( textureThreshold ); sbm->setSmallerBlockSize( smallerBlockSize ); #endif #if defined(USE_OPENCV_SGBM) sbm->setP1( P1 ); sbm->setP2( P2 ); #endif #if defined(USE_OPENCV_SBM) || defined(USE_OPENCV_SGBM) sbm->setBlockSize( (blockSize + 2) * 2 + 1 ); sbm->setNumDisparities( (ndisparities + 1) * 16 ); sbm->setUniquenessRatio( uniquenessRatio ); sbm->setSpeckleWindowSize( speckleWindowSize ); sbm->setSpeckleRange( speckleRange ); sbm->setDisp12MaxDiff( disp12MaxDiff ); #endif #ifdef USE_ELAS param.disp_max = (ndisparities + 1) * 16; elas.setParameters( param ); #endif for ( unsigned int i = 0; i < numCameras; i++ ) { FlyCapture2::Image rawImage; //!< buffer: Must be here in the loop, otherwise cameras do not synchronize. FlyCapture2::Error error; error = ppCameras[i]->RetrieveBuffer( &rawImage ); PRINT_ERROR( error ); // FlyCapture2::TimeStamp timestamp = rawImage.GetTimeStamp(); // fprintf(stderr, // "Cam %d TimeStamp [%d %d]\n", // i, // timestamp.cycleSeconds, // timestamp.cycleCount); memcpy( pimageCamera[i]->data, rawImage.GetData(), rawImage.GetDataSize() ); } //!< rectifying images (and un-distorting?) cv::remap( *pimageCamera[1], *pimageCamera[1], mx1, my1, cv::INTER_LINEAR ); cv::remap( *pimageCamera[0], *pimageCamera[0], mx2, my2, cv::INTER_LINEAR ); #if defined(USE_OPENCV_SBM) || defined(USE_OPENCV_SGBM) sbm->compute( *pimageCamera[1], *pimageCamera[0], imageDisparity ); #endif #ifdef USE_ELAS // int static skip = 0; // if (skip % 15 ==0) { // elas.process(imageCamera1.data, imageCamera0.data, (float*)imageDisparity.data,(float*)imageDisparity.data, dims); elas.process(pimageCamera[1]->data, pimageCamera[0]->data, (float*)imageDisparity.data,(float*)imageDisparity.data, dims); } // skip++; #endif //!< normalize disparity map for imshow double minVal, maxVal; minMaxLoc( imageDisparity, &minVal, &maxVal ); imageDisparity.convertTo( disp8U, CV_8UC1, 255/(maxVal - minVal) ); #define DRAWTXT(img, str, x, y, s) \ cv::putText( img, str, cv::Point(x,y), cv::FONT_HERSHEY_SIMPLEX, s, cv::Scalar::all(255) ) DRAWTXT( disp8U, "disparity", 10, 20, 0.5 ); cv::imshow( "disparity", disp8U ); //!< disparity map --> depth map --> point cloud cv::Mat depth; cv::reprojectImageTo3D( imageDisparity, depth, Q ); cv::Mat pointCloudFromDepth = depth.reshape( 3, depth.size().area() ); pointCloudFromDepth_ptr->clear(); for (int i = 0; i < pointCloudFromDepth.rows; i++) { float *pt = pointCloudFromDepth.ptr < float > ( i ); pcl::PointXYZ basic_point; basic_point.x = pt[0]; basic_point.y = pt[1]; basic_point.z = pt[2]; #if defined(USE_OPENCV_SBM) || defined(USE_OPENCV_SGBM) basic_point.z /= 8.0; //!< simple hack, but unknown reason..... //!< still wrong. Depth maybe strange. #endif if ( pt[2] < 129 ) //!< simple hack pointCloudFromDepth_ptr->points.push_back ( basic_point ); } pointCloudFromDepth_ptr->width = (int) pointCloudFromDepth_ptr->points.size (); pointCloudFromDepth_ptr->height = 1; viewer->updatePointCloud< pcl::PointXYZ > ( pointCloudFromDepth_ptr, "my points" ); viewer->spinOnce (); for ( unsigned int i = 0; i < numCameras; i++ ) { // drawCameraInfo( ppCameras[i], *pimageCamera[i] ); cv::imshow( std::string("cam") + std::to_string(i), *pimageCamera[i] ); } cv::imshow( "sub", cv::abs( *pimageCamera[0] - *pimageCamera[1] ) ); char keyValue = cv::waitKey( 10 ); if (keyValue == 'q' || keyValue == 27 /* ESC */ ) { break; } } exit(0); //!< hack; code below may freeze. for ( unsigned int i = 0; i < numCameras; i++ ) { ppCameras[i]->StopCapture(); ppCameras[i]->Disconnect(); delete ppCameras[i]; } delete [] ppCameras; return 0; }
void VideoSaverFlyCapture::captureThread() { if (!isFlyCapture()) { VideoSaver::captureThread(); } else { m_GrabbingFinished = false; m_KeepThreadAlive = true; m_frameNumber = 0; m_newFrameAvailable = false; FlyCapture2::Image rgbImage; FlyCapture2::Image rawImage; FlyCapture2::Image rawImage2; m_timer= std::clock(); while (m_KeepThreadAlive) { FlyCapture2::Error error = m_Camera.RetrieveBuffer( &rawImage ); if ( error != FlyCapture2::PGRERROR_OK ) { error.PrintErrorTrace(); } //get the time stamp const double localtimestamp = M_TIMER_ELAPSED; // convert to bgr rawImage2.DeepCopy(&rawImage); // not sure if really needed since we convert below... rawImage2.Convert(FlyCapture2::PIXEL_FORMAT_RGB, &rgbImage ); // convert to Mat unsigned int rowBytes = (double) rgbImage.GetReceivedDataSize()/(double)rgbImage.GetRows(); // copy to frame variable and update times { std::unique_lock<std::mutex> lock(VideoSaver::m_FrameMutex); m_Frame.release(); m_Frame = cv::Mat(rgbImage.GetRows(), rgbImage.GetCols(), CV_8UC3, rgbImage.GetData(),rowBytes); // could this happen ? if (m_Frame.size().width==0) m_Frame = cv::Mat::zeros(m_FrameSize,CV_8UC3); m_TimeStamp = localtimestamp; m_frameNumber++; m_newFrameAvailable = true; VideoSaver::m_newFrameAvailableCond.notify_one(); } } rawImage.ReleaseBuffer(); rawImage2.ReleaseBuffer(); rgbImage.ReleaseBuffer(); m_newFrameAvailableCond.notify_one(); std::cout << "Finished grabbing." << std::endl; m_GrabbingFinished = true; } }