CheckerboardDetector.cpp

// author Jan Woetzel (c) 2006
#include "CheckerboardDetector.hh"
#include <Base/Common/W32Compat.hh>
#include <math.h>
#include <string>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <sstream>

//BIAS
#include <Base/Debug/Error.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>

//OpenCV
#include <cv.h>
#include <highgui.h>

using namespace std;
using namespace BIAS;


// Global parameters for checkerboard detection algorithm
int CheckerboardDetector::cornerDetectorFlags_ = CV_CALIB_CB_ADAPTIVE_THRESH;
int CheckerboardDetector::subPixelIterations_ = 100;
double CheckerboardDetector::subPixelAccuracy_ = 0.1;


int CheckerboardDetector::
Compute(const BIAS::Image<unsigned char> &img,
        const unsigned int &xCorners, const unsigned int &yCorners,
        t_CoordVecf &coords, const bool &saveDebugImage)
{
  // allocate memory for corners
  const int numCorners = xCorners*yCorners;
  if ((int)coords.size() != numCorners)
    coords.resize(numCorners, std::pair<float,float>(0.0f, 0.0f));

  // conversion required from BIAS image to CV image
  IplImage* gray = NULL;
  BIAS::Image<unsigned char> imgGrey; 
  ImageConvert::ToGrey(img, imgGrey);
  ImageConvert::BIAS2ipl(imgGrey, gray);  
  BIASASSERT(gray != NULL); // loaded?

  // checkerboard detection
  int cornersFound = 0;
  int res = cvFindChessboardCorners(gray, cvSize(xCorners, yCorners),
                                    (CvPoint2D32f*)&(coords[0].first),
                                    &cornersFound, cornerDetectorFlags_);

  if (res == 0) {
    //BIASWARN("Could not find all "<<xCorners<<"x"<<yCorners<<" inner corners!");
    return -3;
  }

  // subpixel refinement
  cvFindCornerSubPix(gray, (CvPoint2D32f*)&coords[0].first,
                     cornersFound, cvSize(5,5), cvSize(-1,-1),
                     cvTermCriteria(CV_TERMCRIT_ITER, //+CV_TERMCRIT_EPS,
                                    subPixelIterations_, subPixelAccuracy_));

  // create and save debug image
  if (saveDebugImage) {
    BIAS::Image<unsigned char> imgRGB;
    IplImage *image=NULL;
    ImageConvert::ToRGB(img, imgRGB);
    //remember to delete image which is created in BIAS2ipl
    ImageConvert::BIAS2ipl(imgRGB, image);
    BIASASSERT(image != NULL); // loaded?
    // confirm correctness of points
    DrawPoints(image, (CvPoint2D32f*)&coords[0].first,
               cornersFound, xCorners, yCorners);
    cvSaveImage("out__findChessBoardCornerGuesses.jpg", image);
    cvReleaseImage(&image);
  }

  // release memory
  cvReleaseImage(&gray);  
  return 0;
}


void CheckerboardDetector::
DrawPoints(void* imgVoid, void* ptArrayVoid, //t_CoordVecf &coords,
           int numPts, const int xCorners, const int yCorners,
           const bool DRAW_IDEAL_OVERLAY, const bool DRAW_AXES,
           const bool DRAW_POINT_LABEL)
{

  CvArr* img = (CvArr*)imgVoid;
  CvPoint2D32f *ptArray = (CvPoint2D32f*)ptArrayVoid;
  CvPoint pt;
  CvPoint ptOld;
  CvFont font;
  if (DRAW_POINT_LABEL) {
    cvInitFont(&font, CV_FONT_HERSHEY_PLAIN, 1.0f, 1.0f);
  }

  // init old as first 
  if (numPts > 0) {
    ptOld.x = (int)ptArray[0].x;
    ptOld.y = (int)ptArray[0].y;
  }
  for (int i = 0; i < numPts; i++) {
    int clr = (int)((float)i / numPts * 255.0);
    pt.x = (int)ptArray[i].x;
    pt.y = (int)ptArray[i].y;
    cvCircle(img, pt, 3, CV_RGB(255-clr,0,clr), CV_FILLED);

    if (DRAW_POINT_LABEL){
      // label the point
      stringstream ssID;
      ssID << i;
      cvPutText(img, ssID.str().c_str(), pt, &font, CV_RGB(0,0,255));
    }

    // next
    ptOld=pt;
  }

  if (DRAW_IDEAL_OVERLAY) {
    // mark ideal pattern
    // horizontal lines
    for (int xx = 0; xx < xCorners*yCorners; xx += xCorners) {
      cvLine(img, cvPointFrom32f(ptArray[xx]),
             cvPointFrom32f(ptArray[xx+xCorners-1]),
             CV_RGB(0,0,255), 1, CV_AA);
    }
    // vertical lines
    for (int yy = 0; yy < xCorners; yy += 1) {
      cvLine(img, cvPointFrom32f(ptArray[yy]),
             cvPointFrom32f(ptArray[yy+xCorners*(yCorners-1)]),
             CV_RGB(0,0,255), 1, CV_AA);
    }
  }

  if (DRAW_AXES) {
    CvFont fontBig;
    cvInitFont(&fontBig, CV_FONT_HERSHEY_PLAIN, 5.0f, 5.0f, 0, 2, CV_AA);
    cvPutText(img, "0", cvPointFrom32f(ptArray[0]), &fontBig, CV_RGB(0,0,255));
    cvLine(img, cvPointFrom32f(ptArray[0]), cvPointFrom32f(ptArray[xCorners-1]),
           CV_RGB(255,0,0), 1, CV_AA);
    cvPutText(img, "X", cvPointFrom32f(ptArray[xCorners-1]),
              &fontBig, CV_RGB(255,0,0));
    cvLine(img, cvPointFrom32f(ptArray[0]),
           cvPointFrom32f(ptArray[xCorners*(yCorners-1)]),
           CV_RGB(0,255,0), 1, CV_AA);
    cvPutText(img, "Y", cvPointFrom32f(ptArray[xCorners*(yCorners-1)]),
              &fontBig, CV_RGB(0,255,0));
  }

  cvNamedWindow("img", 1);
  cvMoveWindow("img", 0, 0);
  cvShowImage("img", img);
  cvWaitKey(1000);
  cvDestroyAllWindows();
}