/*!
Connect the active camera.
\sa disconnect()
*/
void vpFlyCaptureGrabber::connect()
{
if (m_connected == false) {
FlyCapture2::Error error;
m_numCameras = this->getNumCameras();
if (m_numCameras == 0) {
throw (vpException(vpException::fatalError, "No camera found on the bus"));
}
FlyCapture2::BusManager busMgr;
error = busMgr.GetCameraFromIndex(m_index, &m_guid);
if (error != FlyCapture2::PGRERROR_OK) {
error.PrintErrorTrace();
throw (vpException(vpException::fatalError,
"Cannot retrieve guid of camera with index %u.",
m_index) );
}
// Connect to a camera
error = m_camera.Connect(&m_guid);
if (error != FlyCapture2::PGRERROR_OK) {
error.PrintErrorTrace();
throw (vpException(vpException::fatalError,
"Cannot connect to camera with guid 0x%lx", m_guid));
}
m_connected = true;
}
if (m_connected && m_capture)
init = true;
else
init = false;
}
bool Camera::setup()
{
///////////////////////////////////////////////////////
// Get a camera:
FlyCapture2::Error error;
FlyCapture2::BusManager busMgr;
unsigned int N;
if ((error = busMgr.GetNumOfCameras(&N)) != PGRERROR_OK)
ROS_ERROR (error.GetDescription ());
if (camSerNo == 0)
{
if ((error = busMgr.GetCameraFromIndex(0, &guid_)) != PGRERROR_OK)
PRINT_ERROR_AND_RETURN_FALSE;
ROS_INFO ("did busMgr.GetCameraFromIndex(0, &guid)");
}
else
{
if ((error = busMgr.GetCameraFromSerialNumber(camSerNo, &guid_)) != PGRERROR_OK)
PRINT_ERROR_AND_RETURN_FALSE;
}
ROS_INFO ("Setup: Camera GUID = %u %u %u %u", guid_.value[0], guid_.value[1], guid_.value[2], guid_.value[3]);
ROS_INFO ("Setup successful");
return true;
}
oneCame::oneCame(int id, FlyCapture2::BusManager &busMgr, cv::Mat* img):_cameraId(id), _imgOPENCV(img), _readyFlag(false)
{
PGR_SAFE_CALL(busMgr.GetCameraFromIndex(_cameraId, &_guid));
PGR_SAFE_CALL(_cam.Connect(&_guid));
// restart cam:
//restartCam();
int centerX = 1328/2;
int centerY = 1024/2;
//_allImgs = new FlyCapture2::Image[_numOfCams];
_fmt7ImageSettings.mode = FlyCapture2::MODE_7;;
_fmt7ImageSettings.width = 800;
_fmt7ImageSettings.height = 600;
_fmt7ImageSettings.offsetX = centerX - _fmt7ImageSettings.width /2;
_fmt7ImageSettings.offsetY = centerY - _fmt7ImageSettings.height /2;
_fmt7ImageSettings.pixelFormat = FlyCapture2::PIXEL_FORMAT_RGB8;
// Validate the settings to make sure that they are valid
bool valid;
PGR_SAFE_CALL( _cam.ValidateFormat7Settings(&_fmt7ImageSettings,&valid, &_fmt7PacketInfo ));
PGR_SAFE_CALL( _cam.SetFormat7Configuration( &_fmt7ImageSettings, _fmt7PacketInfo.recommendedBytesPerPacket ));
FlyCapture2::Property prop;
// set gain
prop.type = FlyCapture2::GAIN; prop.autoManualMode = false; prop.absControl = true; prop.absValue = 10;
PGR_SAFE_CALL(_cam.SetProperty( &prop ));
// set shutter
prop.type = FlyCapture2::SHUTTER; prop.autoManualMode = false; prop.onOff = true;
prop.absControl = true; prop.absValue = 45; PGR_SAFE_CALL(_cam.SetProperty( &prop ));
// set frame rate
prop.type = FlyCapture2::FRAME_RATE; prop.autoManualMode = false; prop.onOff = true;
prop.absControl = true; prop.absValue = 20; PGR_SAFE_CALL(_cam.SetProperty( &prop ));
// set auto white balance
FlyCapture2::Property prop1;
prop1.type = FlyCapture2::WHITE_BALANCE; prop1.onOff = true; prop1.autoManualMode = false;
prop1.valueA = 490; prop1.valueB = 790; PGR_SAFE_CALL(_cam.SetProperty( &prop1 ));
// disable sharpness
FlyCapture2::Property sharpnessProp;
sharpnessProp.type = FlyCapture2::SHARPNESS; sharpnessProp.onOff = false; PGR_SAFE_CALL(_cam.SetProperty( &sharpnessProp ));
// diable gamma
FlyCapture2::Property gammaProp;
gammaProp.type = FlyCapture2::GAMMA; gammaProp.onOff = false; PGR_SAFE_CALL(_cam.SetProperty( &gammaProp ));
// set trigger mode
_triggerMode.onOff = true;
_triggerMode.mode = 15;
_triggerMode.parameter = 10;
_triggerMode.source = 0;
PGR_SAFE_CALL(_cam.SetTriggerMode( &_triggerMode));
std::cout<< getFrameRate() << std::endl;
}
int VideoHandler::start() {
if (m_stopped) {
if ( m_camera) {
FlyCapture2::BusManager busMgr;
FlyCapture2::Error error;
FlyCapture2::PGRGuid guid;
cout << m_camIdx << endl;
error = busMgr.GetCameraFromIndex( m_camIdx, &guid );
if (error != PGRERROR_OK)
{
error.PrintErrorTrace();
return -1;
}
cout << "got camera from index" << endl;
pVideoSaver = new VideoSaver();
cout << "created VideoSaver" << endl;
if (pVideoSaver->init(guid)!=0){
cout << "Warning: Error in initializing the camera\n" ;
return -1;
}
if (fname.empty()) {
cout << "start capture thread" << endl;
if (pVideoSaver->startCapture()!=0) {
cout << "Warning: Error in starting the capture thread the camera \n";
return -1;
}
}
else {
cout << "start capture and write threads" << endl;
if (pVideoSaver->startCaptureAndWrite(fname,string("X264"))!=0) {
cout << "Warning: Error in starting the writing thread the camera \n";
return -1;
}
}
}
else {
pVideoCapture = new VideoCapture(fname);
//pVideoCapture->set(cv::CAP_PROP_CONVERT_RGB,true); // output RGB
}
if (knnMethod)
pBackgroundSubtractor = cv::createBackgroundSubtractorKNN(250,400,false);
else
pBackgroundThresholder = new BackgroundThresholder();
m_stopped=false;
startThreads();
}
return 0;
}
int VideoSaverFlyCapture::init(int camIdx){
m_isFlyCapture = false;
// check whether FlyCap camera exists
FlyCapture2::PGRGuid guid;
FlyCapture2::BusManager busMgr;
FlyCapture2::Error error;
error = busMgr.GetCameraFromIndex(camIdx, &guid );
if (error == FlyCapture2::PGRERROR_OK) {
return init(guid);
}
else { // try OpenCV
cout << "Cannot find FlyCapture camera. Use OpenCV instead." << endl;
return VideoSaver::init(camIdx);
}
}
vector<CameraInfo> CameraPointGrey::getCameraList(){
FlyCapture2::Error error;
FlyCapture2::BusManager busManager;
unsigned int numCameras;
error = busManager.GetNumOfCameras(&numCameras);
vector<CameraInfo> ret;
if (error != FlyCapture2::PGRERROR_OK){
PrintError(error);
return ret;
}
for (unsigned int i=0; i < numCameras; i++){
FlyCapture2::PGRGuid guid;
error = busManager.GetCameraFromIndex(i, &guid);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// Connect to camera
FlyCapture2::Camera cam;
error = cam.Connect(&guid);
if (error != FlyCapture2::PGRERROR_OK)
PrintError( error );
// Get the camera information
FlyCapture2::CameraInfo camInfo;
error = cam.GetCameraInfo(&camInfo);
if (error != FlyCapture2::PGRERROR_OK)
PrintError( error );
CameraInfo camera;
camera.busID = camInfo.nodeNumber;
camera.model = camInfo.modelName;
camera.vendor = "Point Grey Research";
ret.push_back(camera);
}
return ret;
}
void TrackerDataSource::list_serial_numbers(){
std::cout << "Serial numbers of cameras on the system: \n";
FlyCapture2::BusManager busMgr;
FlyCapture2::PGRGuid guid;
unsigned int n_cams;
FlyCapture2::CameraInfo camInfo;
busMgr.GetNumOfCameras(&n_cams);
FlyCapture2::Camera *this_camera = new FlyCapture2::Camera();
for(int i = 0; i < n_cams; i++){
busMgr.GetCameraFromIndex(i, &guid);
this_camera->Connect( &guid );
this_camera->GetCameraInfo( &camInfo );
std::cout << "Camera " << i
<< " serial number: " << camInfo.serialNumber
<< std::endl;
this_camera->Disconnect();
}
delete this_camera;
std::cout << "Serial numbers requested in config file" << std::endl;
for(int g = 0; g < opt.group_size(); g++){
for(int c = 0; c < opt.group(g).cam_size(); c++){
std::cout << "Group " << g << " camera " << c
<< " serial number: " << opt.group(g).cam(c).serial_number()
<< std::endl;
}
}
}
CameraPointGrey::CameraPointGrey(unsigned int camNum, CameraTriggerMode triggerMode) : Camera(triggerMode){
FlyCapture2::Error error;
// Connect to camera
FlyCapture2::BusManager busManager;
FlyCapture2::PGRGuid camGuid;
busManager.GetCameraFromIndex(camNum, &camGuid);
error = cam.Connect(&camGuid);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// // Configure video mode and frame rate (legacy modes)
// FlyCapture2::VideoMode videoMode = FlyCapture2::VIDEOMODE_640x480Y8;
// FlyCapture2::FrameRate frameRate = FlyCapture2::FRAMERATE_30;
// cam.SetVideoModeAndFrameRate(videoMode, frameRate);
// Configure Format7 mode (2x2 binning)
FlyCapture2::Format7ImageSettings format7Settings;
format7Settings.mode = FlyCapture2::MODE_0;
format7Settings.pixelFormat = FlyCapture2::PIXEL_FORMAT_MONO8;
format7Settings.width = 1280;
format7Settings.height = 960;
format7Settings.offsetX = 0;
format7Settings.offsetY = 0;
// Validate and set mode
FlyCapture2::Format7PacketInfo packetInfo;
bool valid;
error = cam.ValidateFormat7Settings(&format7Settings, &valid, &packetInfo);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// packetsize configures maximum frame rate
error = cam.SetFormat7Configuration(&format7Settings, packetInfo.recommendedBytesPerPacket);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// Turn off gamma
FlyCapture2::Property property;
property.type = FlyCapture2::AUTO_EXPOSURE;
property.onOff = false;
error = cam.SetProperty(&property);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
property.type = FlyCapture2::GAMMA;
property.onOff = true;
property.absControl = true;
property.absValue = 1.0;
error = cam.SetProperty(&property);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
// Get the camera information
FlyCapture2::CameraInfo camInfo;
error = cam.GetCameraInfo(&camInfo);
if (error != FlyCapture2::PGRERROR_OK)
PrintError(error);
std::cout << camInfo.vendorName << " " << camInfo.modelName << " " << camInfo.serialNumber << std::endl;
// Set reasonable default settings
CameraSettings settings;
//settings.shutter = 8.33;
settings.shutter = 33.33;
settings.gain = 0.0;
this->setCameraSettings(settings);
return;
}
Camera::Camera() : frameRate(FlyCapture2::FRAMERATE_30)
{
FlyCapture2::BusManager busMgr;
throw_if_error(busMgr.GetCameraFromIndex(0, &guid));
}
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;
}
//! \brief Connect cameras and return camera objects
//!
//! \return unsigned int &numCameras
//! \return FlyCapture2::Camera** &ppCameras
//!
//!
void prepareCameras(unsigned int &numCameras,
FlyCapture2::Camera** &ppCameras)
{
FlyCapture2::Error error;
//!<
//!< Get camera bus
//!<
FlyCapture2::BusManager busMgr;
error = busMgr.GetNumOfCameras( &numCameras );
PRINT_ERROR( error );
std::cout << "Number of cameras detected: " << numCameras << std::endl;
if ( numCameras < 1 )
{
std::cerr << "Insufficient number of cameras... press Enter to exit." << std::endl;
getchar();
exit(-1);
}
ppCameras = new FlyCapture2::Camera* [ numCameras ];
//!<
//!< Connect to all detected cameras and attempt to set them to a common video mode and frame rate
//!<
for ( unsigned int i = 0; i < numCameras; i++)
{
ppCameras[i] = new FlyCapture2::Camera();
std::cout << "setting camera " << i << std::endl;
FlyCapture2::PGRGuid guid;
error = busMgr.GetCameraFromIndex( i, &guid );
PRINT_ERROR( error );
//!< Connect to a camera
error = ppCameras[i]->Connect( &guid );
PRINT_ERROR( error );
PrintCameraInfo( ppCameras[i] );
PrintCameraPropertyInfo( ppCameras[i] );
//!< Set all cameras to a specific mode and frame rate so they can be synchronized
error = ppCameras[i]->SetVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_640x480Y8, //!< size of 640x480, format Mono8bit (Y8)
FlyCapture2::FRAMERATE_30 );
PRINT_ERROR_MSG( error,
"Error starting cameras. \n"
"This example requires cameras to be able to set to 640x480 Y8 at 30fps. \n"
"If your camera does not support this mode, please edit the source code and recompile the application. \n"
"Press Enter to exit. \n");
}
std::cout << "all camera set to specific mode." << std::endl;
error = FlyCapture2::Camera::StartSyncCapture( numCameras, (const FlyCapture2::Camera**)ppCameras );
PRINT_ERROR_MSG( error,
"Error starting cameras. \n"
"This example requires cameras to be able to set to 640x480 Y8 at 30fps. \n"
"If your camera does not support this mode, please edit the source code and recompile the application. \n"
"Press Enter to exit. \n");
std::cout << "StartSyncCapture." << std::endl;
}
