// The processing loop where images are retrieved, tags detected,
// and information about detections generated
void loop() {
cv::Mat image;
cv::Mat image_gray;
int frame = 0;
double last_t = tic();
while (true) {
// capture frame
Camera.grab();
Camera.retrieve ( image);
processImage(image, image_gray);
// print out the frame rate at which image frames are being processed
frame++;
if (frame % 10 == 0) {
double t = tic();
cout << " " << 10./(t-last_t) << " fps" << endl;
last_t = t;
}
// exit if any key is pressed
if (cv::waitKey(1) >= 0) break;
}
}
void setup() {
m_tagDetector = new AprilTags::TagDetector(m_tagCodes);
Camera.set( CV_CAP_PROP_FORMAT, CV_8UC1);
Camera.set( CV_CAP_PROP_FRAME_WIDTH, m_width);
Camera.set( CV_CAP_PROP_FRAME_HEIGHT, m_height);
// prepare window for drawing the camera images
if (m_draw) {
cv::namedWindow(windowName, 1);
}
}
void setupVideo() {
#ifdef EXPOSURE_CONTROL
// manually setting camera exposure settings; OpenCV/v4l1 doesn't
// support exposure control; so here we manually use v4l2 before
// opening the device via OpenCV; confirmed to work with Logitech
// C270; try exposure=20, gain=100, brightness=150
string video_str = video_string;
cout << "video string " << video_str << endl;
int device = v4l2_open(video_str.c_str(), O_RDWR | O_NONBLOCK);
if (m_exposure >= 0) {
// not sure why, but v4l2_set_control() does not work for
// V4L2_CID_EXPOSURE_AUTO...
struct v4l2_control c;
c.id = V4L2_CID_EXPOSURE_AUTO;
c.value = 1; // 1=manual, 3=auto; V4L2_EXPOSURE_AUTO fails...
if (v4l2_ioctl(device, VIDIOC_S_CTRL, &c) != 0) {
cout << "Failed to set... " << strerror(errno) << endl;
}
cout << "exposure: " << m_exposure << endl;
v4l2_set_control(device, V4L2_CID_EXPOSURE_ABSOLUTE, m_exposure*6);
}
if (m_gain >= 0) {
cout << "gain: " << m_gain << endl;
v4l2_set_control(device, V4L2_CID_GAIN, m_gain*256);
}
if (m_brightness >= 0) {
cout << "brightness: " << m_brightness << endl;
v4l2_set_control(device, V4L2_CID_BRIGHTNESS, m_brightness*256);
}
v4l2_close(device);
#endif
// find and open a USB camera (built in laptop camera, web cam etc)
/*m_cap = cv::VideoCapture(video_string);
if(!m_cap.isOpened()) {
cerr << "ERROR: Can't find video device " << video_string << "\n";
exit(1);
}*/
if (!Camera.open()) {cerr<<"Error opening the camera"<<endl; exit(1);}
cout << "Camera successfully opened (ignore error messages above...)" << endl;
}
void processCommandLine ( int argc,char **argv,raspicam::RaspiCam_Cv &Camera ) {
Camera.set ( CV_CAP_PROP_FRAME_WIDTH, getParamVal ( "-w",argc,argv,320 ) );
Camera.set ( CV_CAP_PROP_FRAME_HEIGHT, getParamVal ( "-h",argc,argv,180 ) );
Camera.set ( CV_CAP_PROP_BRIGHTNESS,getParamVal ( "-br",argc,argv,50 ) );
Camera.set ( CV_CAP_PROP_CONTRAST ,getParamVal ( "-co",argc,argv,50 ) );
Camera.set ( CV_CAP_PROP_SATURATION, getParamVal ( "-sa",argc,argv,50 ) );
Camera.set ( CV_CAP_PROP_GAIN, getParamVal ( "-g",argc,argv ,50 ) );
if ( findParam ( "-gr",argc,argv ) !=-1 )
Camera.set ( CV_CAP_PROP_FORMAT, CV_8UC1 );
if ( findParam ( "-test_speed",argc,argv ) !=-1 )
doTestSpeedOnly=true;
if ( findParam ( "-ss",argc,argv ) !=-1 )
Camera.set ( CV_CAP_PROP_EXPOSURE, getParamVal ( "-ss",argc,argv ) );
// Camera.setSharpness ( getParamVal ( "-sh",argc,argv,0 ) );
// if ( findParam ( "-vs",argc,argv ) !=-1 )
// Camera.setVideoStabilization ( true );
// Camera.setExposureCompensation ( getParamVal ( "-ev",argc,argv ,0 ) );
}
int32_t FindLineCenter(raspicam::RaspiCam_Cv &Camera, int32_t nTick, int32_t &nCenterX, int32_t &nCenterY)
{
cv::Mat image;
char szFilename[100] = "";
char szOriFilename[100];
#if PRESERVEROI
char szOriROIFilename[100];
cv::Mat roiImgPreserve;
#endif
cv::Rect roi(0, 190, CAMERA_IMG_WIDTH, 100);
cv::Mat roiImg, erodeElmt, dilateElmt;
int thVal = IMGTHRESHOLD;
vector<vector<cv::Point> > contours;
vector<cv::Vec4i> hierarchy;
int nRet = -1;
nCenterX = CAMERA_IMG_WIDTH / 2;
nCenterY = CAMERA_IMG_HEIGHT / 2;
{
try {
Camera.grab();
Camera.retrieve(image);
image(roi).copyTo(roiImg);
#if PRESERVEROI
roiImg.copyTo(roiImgPreserve);
#endif
} catch (cv::Exception& e)
{
const char* err_msg = e.what();
std::cout << "exception caught: " << err_msg << std::endl;
}
#if THRESHOLDIMAGE
//printf("threshold image\n");
cv::threshold(roiImg, roiImg, thVal, 255, cv::THRESH_BINARY);
//printf("bitwise_not image\n");
cv::bitwise_not(roiImg, roiImg); // negative image
erodeElmt = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
dilateElmt = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
//printf("erode image\n");
cv::erode(roiImg, roiImg, erodeElmt);
//printf("dilate image\n");
cv::dilate(roiImg, roiImg, dilateElmt);
#endif
#if TESTROI
{
sprintf(szFilename, "roi_image%d.png", nTick);
sprintf(szOriFilename, "image%d.png", nTick);
try {
//cv::drawContours(roiImg, contours, s, cv::Scalar(255, 255, 255), 2, 8, hierarchy, 0, cv::Point() );
cv::imwrite(szFilename, roiImg);
cv::imwrite(szOriFilename, image);
} catch (cv::Exception& e)
{
const char* err_msg = e.what();
std::cout << "exception caught: " << err_msg << std::endl;
}
}
#endif
#if !TESTROI
//printf("findContours image\n");
cv::findContours(roiImg, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0,0));
int nMaxAreaContourIndex = -1;
float fMaxArea = 0.f;
for (size_t s = 0; s < contours.size(); s++)
{
float fArea = cv::contourArea(contours[s]);
if (fArea > fMaxArea)
{
nMaxAreaContourIndex = s;
fMaxArea = fArea;
}
}
{
if (fMaxArea > MAXAREATHRESHOLD && nMaxAreaContourIndex >= 0)
{
cv::Moments mu;
mu = cv::moments(contours[nMaxAreaContourIndex], false);
cv::Point2f center(mu.m10 / mu.m00, mu.m01 / mu.m00); // point in center (x only)
printf("Find a region: Area=%f, center(%f, %f)\n", fMaxArea, center.x, center.y);
nCenterX = (int32_t)center.x;
nCenterY = (int32_t)center.y;
nRet = 1;
// here is the center pixel
#if 0
{
try {
cv::circle(roiImg, center, 5, cv::Scalar(255, 255, 255), -1, 8, 0);
} catch (cv::Exception& e)
{
const char* err_msg = e.what();
std::cout << "exception caught: " << err_msg << std::endl;
}
sprintf(szFilename, "roi_image%d.png", nTick);
sprintf(szOriFilename, "image%d.png", nTick);
try {
cv::drawContours(roiImg, contours, s, cv::Scalar(255, 255, 255), 2, 8, hierarchy, 0, cv::Point());
cv::imwrite(szFilename, roiImg);
//cv::imwrite(szOriFilename, image);
#if PRESERVEROI
sprintf(szOriROIFilename, "ori_roi_image%d.png", nTick);
cv::imwrite(szOriROIFilename, roiImgPreserve);
#endif
} catch (cv::Exception& e)
{
const char* err_msg = e.what();
std::cout << "exception caught: " << err_msg << std::endl;
}
}
#endif
}
}
#endif
}
return nRet;
}