std::vector<CvPoint2D32f> findCorners(Image<PixRGB<byte> > &img, int rows, int cols)
{
int count = 0;
std::vector<CvPoint2D32f> corners(rows*cols);
Image<byte> in = luminance(img);
int result = cvFindChessboardCorners(img2ipl(in), cvSize(rows,cols),
&corners[0], &count,
CV_CALIB_CB_ADAPTIVE_THRESH |
CV_CALIB_CB_NORMALIZE_IMAGE |
CV_CALIB_CB_FILTER_QUADS);
// result = 0 if not all corners were found
// Find corners to an accuracy of 0.1 pixel
if(result != 0)
{
cvFindCornerSubPix(img2ipl(in),
&corners[0],
count,
cvSize(10,10), //win
cvSize(-1,-1), //zero_zone
cvTermCriteria(CV_TERMCRIT_ITER,1000,0.01) );
return corners;
} else {
return std::vector<CvPoint2D32f>();
}
}
int cvFindChessBoardCornerGuesses( const void* arr, void*,
CvMemStorage*, CvSize pattern_size,
CvPoint2D32f* corners, int* corner_count )
{
return cvFindChessboardCorners( arr, pattern_size, corners,
corner_count, CV_CALIB_CB_ADAPTIVE_THRESH );
}
bool CybCamCalibration::generateDataToCalibration(IplImage *rgb_image) {
IplImage *view_gray;
int found = 0, count = 0;
img_size = cvGetSize(rgb_image);
found = cvFindChessboardCorners( rgb_image, board_size,
image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
// improve the found corners' coordinate accuracy
view_gray = cvCreateImage( cvGetSize(rgb_image), 8, 1 );
cvCvtColor( rgb_image, view_gray, CV_BGR2GRAY );
cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),
cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
cvReleaseImage( &view_gray );
if( found )
{
cvSeqPush( image_points_seq, image_points_buf );
sprintf( imagename, "image%03d.png", image_points_seq->total - 1 );
cvSaveImage( imagename, rgb_image );
}
cvDrawChessboardCorners( rgb_image, board_size, image_points_buf, count, found );
return ((unsigned)image_points_seq->total >= (unsigned)image_count);
}
int main(int argc, char* argv[])
{
board_w = 5; // Board width in squares
board_h = 8; // Board height
n_boards = 8; // Number of boards
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
CvCapture* capture = cvCreateCameraCapture( 0 );
assert( capture );
cvNamedWindow( "Calibration" );
// Allocate Sotrage
CvMat* image_points = cvCreateMat( n_boards*board_n, 2, CV_32FC1 );
CvMat* object_points = cvCreateMat( n_boards*board_n, 3, CV_32FC1 );
CvMat* point_counts = cvCreateMat( n_boards, 1, CV_32SC1 );
CvMat* intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1 );
CvMat* distortion_coeffs = cvCreateMat( 5, 1, CV_32FC1 );
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count;
int successes = 0;
int step, frame = 0;
IplImage *image = cvQueryFrame( capture );
IplImage *gray_image = cvCreateImage( cvGetSize( image ), 8, 1 );
// Capture Corner views loop until we've got n_boards
// succesful captures (all corners on the board are found)
while( successes < n_boards ){
// Skp every board_dt frames to allow user to move chessboard
if( frame++ % board_dt == 0 ){
// Find chessboard corners:
int found = cvFindChessboardCorners( image, board_sz, corners,
&corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
// Get subpixel accuracy on those corners
cvCvtColor( image, gray_image, CV_BGR2GRAY );
cvFindCornerSubPix( gray_image, corners, corner_count, cvSize( 11, 11 ),
cvSize( -1, -1 ), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
// Draw it
cvDrawChessboardCorners( image, board_sz, corners, corner_count, found );
cvShowImage( "Calibration", image );
// If we got a good board, add it to our data
if( corner_count == board_n ){
step = successes*board_n;
for( int i=step, j=0; j < board_n; ++i, ++j ){
CV_MAT_ELEM( *image_points, float, i, 0 ) = corners[j].x;
CV_MAT_ELEM( *image_points, float, i, 1 ) = corners[j].y;
CV_MAT_ELEM( *object_points, float, i, 0 ) = j/board_w;
CV_MAT_ELEM( *object_points, float, i, 1 ) = j%board_w;
CV_MAT_ELEM( *object_points, float, i, 2 ) = 0.0f;
}
CV_MAT_ELEM( *point_counts, int, successes, 0 ) = board_n;
successes++;
}
}
//
// コーナーを検出する
//
// 引数:
// frameImage : キャプチャ画像用IplImage
// grayImage : グレースケール画像用IplImage
// corners : コーナーの位置を格納する変数
//
// 戻り値:
// 0 : コーナーがすべて検出できなかった場合
// 非0 : コーナーがすべて検出された場合
//
int findCorners( IplImage *frameImage, IplImage *grayImage, CvPoint2D32f *corners ) {
int cornerCount; // 検出したコーナーの数
int findChessboardCornersFlag; // cvFindChessboardCorners用フラグ
int findFlag; // コーナーがすべて検出できたかのフラグ
IplImage* m_image_binary;
IplImage* m_set_image;
m_image_binary = cvCreateImage(cvSize(frameImage->width, frameImage->height), IPL_DEPTH_8U, 1);
m_set_image = cvCreateImage(cvSize(frameImage->width, frameImage->height), IPL_DEPTH_8U, 3);
// cvChessboardCorners用フラグを生成する
findChessboardCornersFlag = createFindChessboardCornersFlag();
// 画像をBinaryImageとして変換する。
// コーナーを検出する
cvCvtColor( frameImage, grayImage, CV_BGR2GRAY );
// グレースケールから2値に変換する
cvThreshold( grayImage, m_image_binary, 128, 255, CV_THRESH_BINARY );
// Convert to 3channel image
cvMerge(m_image_binary, m_image_binary, m_image_binary, NULL, m_set_image);
findFlag=cvFindChessboardCorners(
m_set_image,
//m_set_image,
//cvSize( CORNER_WIDTH, CORNER_HEIGHT ),
board_sz,
corners,
&cornerCount,
findChessboardCornersFlag
);
if( findFlag != 0 ) {
// コーナーがすべて検出された場合
// 検出されたコーナーの位置をサブピクセル単位にする
CvTermCriteria criteria={ CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, MAX_ITERATIONS, EPSILON };
cvFindCornerSubPix(
grayImage,
corners,
cornerCount,
cvSize( SEARCH_WINDOW_HALF_WIDTH, SEARCH_WINDOW_HALF_HEIGHT ),
cvSize( DEAD_REGION_HALF_WIDTH, DEAD_REGION_HALF_HEIGHT ),
cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, MAX_ITERATIONS, EPSILON )
);
}
// コーナーの位置を描く
cvDrawChessboardCorners( frameImage, board_sz, corners, cornerCount, findFlag );
cvReleaseImage(&m_set_image);
cvReleaseImage(&m_image_binary);
return findFlag;
}
//borrowed from Patrick...
int do_calibration() {
IplImage* image = NULL;
int xc = 12;
int yc = 12;
int nc = xc*yc;
float side_length = 15;
CvSize board_sz = cvSize(xc, yc);
int ntrials = img_cnt;
char filename[128];
CvMat* object_points = cvCreateMat( ntrials * nc, 3, CV_32FC1 );
CvMat* image_points = cvCreateMat( ntrials * nc, 2, CV_32FC1 );
CvMat* point_counts = cvCreateMat( ntrials, 1, CV_32SC1 );
CvPoint2D32f* corners = new CvPoint2D32f[nc];
for(int t=1; t<=ntrials; t++) {
// load an image
sprintf(filename, "%s/Image%d.jpg", dir_name, t);
image = cvLoadImage(filename);
if(!image){
printf("Could not load image file: %s\n",filename);
exit(0);
}
int corner_count;
int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
IplImage* gray = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
cvCvtColor(image, gray, CV_BGR2GRAY);
cvFindCornerSubPix(gray, corners, corner_count,
cvSize(5, 5), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f ));
printf("Image: %d, %d / %d\n", t, corner_count, nc);
// get data points
if(corner_count == nc && found != 0) {
for(int c=0; c<nc; c++) {
CV_MAT_ELEM( *image_points, float, (t-1)*nc+c, 0 ) = corners[c].x;
CV_MAT_ELEM( *image_points, float, (t-1)*nc+c, 1 ) = corners[c].y;
CV_MAT_ELEM( *object_points, float, (t-1)*nc+c, 0 ) = (float)(c/xc)*side_length;
CV_MAT_ELEM( *object_points, float, (t-1)*nc+c, 1 ) = (float)(c%xc)*side_length;
CV_MAT_ELEM( *object_points, float, (t-1)*nc+c, 2 ) = 0.0f;
}
CV_MAT_ELEM( *point_counts, int, t-1, 0 ) = nc;
}
else printf("Bad board! How did this happen?\n");
}
void Calibrate(){
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count;
int successes = 0;
int step, frame = 0;
IplImage *image = cvQueryFrame( capture );
/*(IplImage *image = cvQueryFrame( capture );
cvSetImageROI(image, cvRect(300, 300, 600, 600));
tmp = cvCreateImage(cvGetSize(image), image->depth, image->nChannels);
cvCopy(image, tmp, NULL);
cvResetImageROI(image);
image = cvCloneImage(tmp);//*/
IplImage *gray_image = cvCreateImage( cvGetSize( image ), 8, 1 );
// Capture Corner views loop until we've got n_boards
// successful captures (all corners on the board are found)
while( successes < n_boards ){
// Skp every board_dt frames to allow user to move chessboard
if( frame++ % board_dt == 0 ){
// Find chessboard corners:
int found = cvFindChessboardCorners( image, board_sz, corners,
&corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
// Get subpixel accuracy on those corners
cvCvtColor( image, gray_image, CV_BGR2GRAY );
cvFindCornerSubPix( gray_image, corners, corner_count, cvSize( 11, 11 ),
cvSize( -1, -1 ), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
// Draw it
cvDrawChessboardCorners( image, board_sz, corners, corner_count, found );
//cvShowImage( "Calibration", image );
// If we got a good board, add it to our data
if( corner_count == board_n ){
step = successes*board_n;
for( int i=step, j=0; j < board_n; ++i, ++j ){
CV_MAT_ELEM( *image_points, float, i, 0 ) = corners[j].x;
CV_MAT_ELEM( *image_points, float, i, 1 ) = corners[j].y;
CV_MAT_ELEM( *object_points, float, i, 0 ) = j/board_w;
CV_MAT_ELEM( *object_points, float, i, 1 ) = j%board_w;
CV_MAT_ELEM( *object_points, float, i, 2 ) = 0.0f;
}
CV_MAT_ELEM( *point_counts, int, successes, 0 ) = board_n;
successes++;
}
}
void get_calib(CvCapture* capture) {
int board_n = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
cvNamedWindow("Calibration");
CvMat* image_points = cvCreateMat(n_boards * board_n, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards * board_n, 3, CV_32FC1);
CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32FC1);
CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int corner_count;
int successes = 0;
int step, frame = 0;
IplImage *image = cvQueryFrame(capture);
IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1);
while(successes < n_boards) {
if(frame++ % board_dt == 0) {
int found = cvFindChessboardCorners(
image, board_sz, corners, &corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
cvCvtColor(image, gray_image, CV_RGB2GRAY);
cvFindCornerSubPix(gray_image, corners, corner_count,
cvSize(11,11), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
cvShowImage("Calibration", image);
//get a good board, add to data
if(corner_count == board_n) {
printf("here\n");
step = successes * board_n;
for(int i = step, j = 0; j < board_n; ++i, ++j) {
CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;
CV_MAT_ELEM(*object_points, float, i, 0) = j / board_w;
CV_MAT_ELEM(*object_points, float, i, 1) = j % board_w;
CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;
}
CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;
successes++;
printf("Nice shot!\n");
int k = cvWaitKey(-1);
while(k != 'n') k = cvWaitKey(-1);
}
}
/* search for chessboard */
void chessboard_finder(IplImage *frame) {
CvPoint2D32f points[70];
int cornercount;
if (cvFindChessboardCorners(frame,
cvSize(10,7),
points,
&cornercount,
0)) {
/* found chessboard */
cvDrawChessboardCorners(frame,
cvSize(10,7),
points,
cornercount,
1);
}
}
int StereoVision::calibrationAddSample(IplImage* imageLeft,IplImage* imageRight) {
if(!calibrationStarted) return RESULT_FAIL;
IplImage* image[2] = {imageLeft,imageRight};
int succeses = 0;
for(int lr=0; lr<2; lr++) {
CvSize imageSize = cvGetSize(image[lr]);
if(imageSize.width != this->imageSize.width || imageSize.height != this->imageSize.height)
return RESULT_FAIL;
int cornersDetected = 0;
//FIND CHESSBOARDS AND CORNERS THEREIN:
int result = cvFindChessboardCorners(
image[lr], cvSize(cornersX, cornersY),
&ponintsTemp[lr][0], &cornersDetected,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE
);
if(result && cornersDetected == cornersN) {
//Calibration will suffer without subpixel interpolation
cvFindCornerSubPix(
image[lr], &ponintsTemp[lr][0], cornersDetected,
cvSize(11, 11), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,30, 0.01)
);
succeses++;
}
}
if(2==succeses) {
for(int lr=0; lr<2; lr++) {
points[lr].resize((sampleCount+1)*cornersN);
copy( ponintsTemp[lr].begin(), ponintsTemp[lr].end(), points[lr].begin() + sampleCount*cornersN);
}
sampleCount++;
return RESULT_OK;
} else {
return RESULT_FAIL;
}
}
const CheckerboardDetector::Checkerboard &
CheckerboardDetector::detect(const Img8u &image){
const Img8u *useImage = ℑ
if(image.getFormat() != formatGray && image.getChannels() != 1){
m_data->grayBuf.setFormat(formatGray);
m_data->grayBuf.setSize(image.getSize());
cc(&image, &m_data->grayBuf);
useImage = &m_data->grayBuf;
}
img_to_ipl(useImage, &m_data->ipl);
std::vector<CvPoint2D32f> corners(m_data->cb.size.getDim());
CvSize s = {m_data->cb.size.width, m_data->cb.size.height };
int n = corners.size();
m_data->cb.found = cvFindChessboardCorners(m_data->ipl, s, corners.data(), &n,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
bool optSubPix = getPropertyValue("subpixel opt.enabled");
int radius = getPropertyValue("subpixel opt.radius");
int innerR = getPropertyValue("subpixel opt.inner radius");
if(innerR >= radius) innerR = radius -1;
if(innerR == 0) innerR = -1;
int maxIter = getPropertyValue("subpixel opt.max iterations");
float minErr = getPropertyValue("subpixel opt.min error");
if(m_data->cb.found && optSubPix){
cvFindCornerSubPix(m_data->ipl, corners.data(), corners.size(), cvSize(radius,radius),
cvSize(innerR, innerR),
cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, maxIter, minErr));
}
if(m_data->cb.found){
m_data->cb.corners.resize(corners.size());
for(size_t i=0;i<corners.size();++i){
m_data->cb.corners[i] = Point32f(corners[i].x, corners[i].y);
}
}else{
m_data->cb.corners.clear();
}
return m_data->cb;
}
static void mainLoop(void)
{
ARUint8 *dataPtr;
int cornerCount;
char buf[256];
int i;
if ((dataPtr = arVideoGetImage()) == NULL)
{
arUtilSleep(2);
return;
}
glClear(GL_COLOR_BUFFER_BIT);
argDrawMode2D(vp);
argDrawImage(dataPtr);
// Convert to grayscale, results will go to arIPI->image, which also provides the backing for calibImage.
arImageProcLuma(arIPI, dataPtr);
cornerFlag = cvFindChessboardCorners(calibImage, cvSize(chessboardCornerNumY, chessboardCornerNumX),
corners, &cornerCount, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if (cornerFlag)
glColor4ub(255, 0, 0, 255);
else
glColor4ub(0, 255, 0, 255);
glLineWidth(2.0f);
// ARLOG("Detected corners = %d\n", cornerCount);
for (i = 0; i < cornerCount; i++)
{
argDrawLineByObservedPos(corners[i].x - 5, corners[i].y - 5, corners[i].x + 5, corners[i].y + 5);
argDrawLineByObservedPos(corners[i].x - 5, corners[i].y + 5, corners[i].x + 5, corners[i].y - 5);
// ARLOG(" %f, %f\n", corners[i].x, corners[i].y);
sprintf(buf, "%d\n", i);
argDrawStringsByObservedPos(buf, corners[i].x, corners[i].y + 20);
}
sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum);
argDrawStringsByObservedPos(buf, 10, 30);
argSwapBuffers();
}
void compute_and_display_image_corners(char * imageName, CvSize * imageSize, CvSize chessboardSize, CvPoint2D32f * cornersArrayToFillIn)
{
IplImage * img = 0;
int cornersCount = 0;
int patternWasFound = 0;
int i;
img = cvLoadImage(imageName, 1);
*imageSize = cvGetSize(img); // useful only for calibration function
//initialisation of the given array
for(i=0;i<chessboardSize.height*chessboardSize.width;i++){ cornersArrayToFillIn[i].x= 0; cornersArrayToFillIn[i].y= 0; }
printf("OK1\n");
// core algorithm
patternWasFound = cvFindChessboardCorners(img, chessboardSize, cornersArrayToFillIn, &cornersCount, 0);
printf("OK2\n");
// display_array_values(cornersArrayToFillIn,chessboardSize.height*chessboardSize.width);
improve_precision(img, cornersArrayToFillIn, cornersCount);
// display_array_values(cornersArrayToFillIn,chessboardSize.height*chessboardSize.width);
// visual only part
cvDrawChessboardCorners(img, chessboardSize, cornersArrayToFillIn, cornersCount, patternWasFound);
cvNamedWindow(imageName, CV_WINDOW_AUTOSIZE);
cvMoveWindow(imageName, 100, 100);
cvShowImage(imageName, img );
cvWaitKey(200);
cvDestroyWindow(imageName);
// end
cvReleaseImage(&img );
}
bool CamCalib::FindChessboard(IplImage *src, IplImage *dst)
{
IplImage *gray = cvCreateImage (cvGetSize(src), IPL_DEPTH_8U, 1);
cvCvtColor(src, gray, CV_BGR2GRAY);
// 체스판 코너 찾기
CvPoint2D32f* corners = new CvPoint2D32f[_board_n];
int corner_count = 0;
int found = cvFindChessboardCorners(src, cvSize(_board_w, _board_h), corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
// 검출된 코너로부터 서브픽셀 정확도로 코너 좌표를 구한다.
cvFindCornerSubPix (gray, corners, corner_count, cvSize(11,11), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
// 코너를 dst 이미지에 그린다.
cvDrawChessboardCorners (dst, cvSize(_board_w, _board_h), corners, corner_count, found);
// 코너를 정상적으로 찾았다면, 코너 데이터를 저장한다.
bool ret = false;
if (found && corner_count == _board_n) {
for( int i=_successes*_board_n, j=0; j<_board_n; ++i, ++j ) {
CV_MAT_ELEM(*_image_points, float, i, 0) = corners[j].x;
CV_MAT_ELEM(*_image_points, float, i, 1) = corners[j].y;
CV_MAT_ELEM(*_object_points,float, i, 0) = (float)(j%_board_w)*_cell_w;
CV_MAT_ELEM(*_object_points,float, i, 1) = (float)(_board_h - j/_board_w - 1)*_cell_h;
CV_MAT_ELEM(*_object_points,float, i, 2) = 0.0f;
}
CV_MAT_ELEM(*_point_counts, int, _successes, 0) = _board_n;
ret = true;
}
delete [] corners;
cvReleaseImage(&gray);
return ret;
}
int main(int argc, char * argv[])
{
int corner_count;
int successes = 0;
int step, frame = 0;
const char* intrinsics_path = argv[1];
const char* distortions_path = argv[2];
int total = argc - 3 ;
int start = 3;
const char* loc = argv[start] ;
board_w = 7; // Board width in squares
board_h = 4; // Board height
n_boards = total; // Number of boards
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
// Allocate Sotrage
CvMat* image_points = cvCreateMat( n_boards*board_n, 2, CV_32FC1 );
CvMat* object_points = cvCreateMat( n_boards*board_n, 3, CV_32FC1 );
CvMat* point_counts = cvCreateMat( n_boards, 1, CV_32SC1 );
CvMat* intrinsic_matrix = cvCreateMat( 3, 3, CV_32FC1 );
CvMat* distortion_coeffs = cvCreateMat( 5, 1, CV_32FC1 );
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
IplImage *image = cvLoadImage( loc );
//IplImage *image = cvQueryFrame(capture);
IplImage *gray_image = cvCreateImage( cvGetSize( image ), 8, 1 );
// Capture Corner views loop until we've got n_boards
// succesful captures (all corners on the board are found)
while( start < total ){
// Skp every board_dt frames to allow user to move chessboard
// if( frame++ % board_dt == 0 ){
// Find chessboard corners:
int found = cvFindChessboardCorners( image, board_sz, corners,
&corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
// Get subpixel accuracy on those corners
cvCvtColor( image, gray_image, CV_BGR2GRAY );
cvFindCornerSubPix( gray_image, corners, corner_count, cvSize( 11, 11 ),
cvSize( -1, -1 ), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
// Draw it
cvDrawChessboardCorners( image, board_sz, corners, corner_count, found );
if( found )
{
cvSaveImage( "/tmp/grid_save.png", image);
}
// If we got a good board, add it to our data
if( corner_count == board_n ){
step = successes*board_n;
for( int i=step, j=0; j < board_n; ++i, ++j ){
CV_MAT_ELEM( *image_points, float, i, 0 ) = corners[j].x;
CV_MAT_ELEM( *image_points, float, i, 1 ) = corners[j].y;
CV_MAT_ELEM( *object_points, float, i, 0 ) = j/board_w;
CV_MAT_ELEM( *object_points, float, i, 1 ) = j%board_w;
CV_MAT_ELEM( *object_points, float, i, 2 ) = 0.0f;
}
CV_MAT_ELEM( *point_counts, int, successes, 0 ) = board_n;
successes++;
}
// }
if( start < total )
{
start++;
}
else if ( start == total )
{
start = 1;
// return -1;
}
loc = argv[start] ;
image = cvLoadImage( loc );
} // End collection while loop
void get_bird_eye(CvCapture* capture) {
printf("haha\n");
//get bird_eye picture
cvNamedWindow("Get_birdeye");
CvMat *intrinsic = (CvMat*) cvLoad("Intrinsics.xml");
CvMat *distortion = (CvMat*) cvLoad("Distortion.xml");
IplImage *image = cvQueryFrame(capture);
IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1);
int board_n = board_w * board_h;
IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
cvInitUndistortMap(
intrinsic,
distortion,
mapx,
mapy
);
CvSize board_sz = cvSize(board_w, board_h);
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int frame = 0;
int corner_count;
bool catch_bird = false;
while(!catch_bird) {
IplImage *t = cvCloneImage(image);
if(frame++ % board_dt == 0) {
IplImage* t = cvCloneImage(image);
cvRemap(t, image, mapx, mapy);
int found = cvFindChessboardCorners(
image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
cvCvtColor(
image,
gray_image,
CV_RGB2GRAY
);
cvFindCornerSubPix(
gray_image,
corners,
corner_count,
cvSize(11,11),
cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)
);
cvDrawChessboardCorners(
image,
board_sz,
corners,
corner_count,
found
);
cvShowImage("Get_birdeye", image);
//get a good board, add to data
if(corner_count == board_n) {
catch_bird = true;
printf("That's it!\n");
}
}
int c;
if(catch_bird) c = cvWaitKey(-1);
else c = cvWaitKey(15);
if(catch_bird && c == 's') {
cvSaveImage("./Resource/bird-eye.jpg", t, 0);
printf("save at ./Resource/bird-eye.jpg\n");
}
else catch_bird = false;
if(c == 'p') {
c = 0;
while(c!='p' && c!= 27){
c = cvWaitKey(250);
}
}
image = cvQueryFrame(capture);
}
}
int CCalibration::CalibrateFromCAMorAVI( int nImages, int nChessRow, int nChessCol, int nSquareSize, int nSkip, CvMat* intrinsic, CvMat* distortion, int fromCamera, char* fName)
{
int nChessSize = nChessRow*nChessCol;
int nAllPoints = nImages*nChessSize;
int i, j, k;
int corner_count, found;
int* p_count = new int[nImages];
IplImage **src_img = new IplImage*[nImages];
CvSize pattern_size = cvSize (nChessCol, nChessRow);
CvPoint3D32f* objects = new CvPoint3D32f[nAllPoints];
CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * nAllPoints);
CvMat object_points;
CvMat image_points;
CvMat point_counts;
CvMat *rotation = cvCreateMat (1, 3, CV_32FC1);
CvMat *translation = cvCreateMat (1, 3, CV_32FC1);
// (1)
for (i = 0; i < nImages; i++) {
for (j = 0; j < nChessRow; j++) {
for (k = 0; k < nChessCol; k++) {
objects[i * nChessSize + j * nChessCol + k].x = (float)j * nSquareSize;
objects[i * nChessSize + j * nChessCol + k].y = (float)k * nSquareSize;
objects[i * nChessSize + j * nChessCol + k].z = 0.0;
}
}
}
cvInitMatHeader (&object_points, nAllPoints, 3, CV_32FC1, objects);
// (2)
CvCapture *capture = NULL;
if (fromCamera)
capture = cvCaptureFromCAM(0);
else
capture = cvCaptureFromAVI(fName);
assert(capture);
int found_num = 0;
cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE);
cvNamedWindow ("Webcam", CV_WINDOW_AUTOSIZE);
int c = 0;
for (i = 0; i < nImages; i++)
{
IplImage * frame;
while (true)
{
frame = cvQueryFrame(capture);
cvShowImage("Webcam", frame);
if (c++ % nSkip == 0)
{
found = cvFindChessboardCorners (frame, pattern_size, &corners[i * nChessSize], &corner_count);
if (found)
{
char s[100];
sprintf(s, "%d.png", i);
cvSaveImage(s, frame);
src_img[i] = cvCloneImage(frame);
fprintf (stderr, "ok\n");
found_num++;
break;
}
}
cvWaitKey(5);
}
fprintf (stderr, "%02d...", i);
// (4)
IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1);
cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY);
cvFindCornerSubPix (src_gray, &corners[i * nChessSize], corner_count,
cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvDrawChessboardCorners (src_img[i], pattern_size, &corners[i * nChessSize], corner_count, found);
p_count[i] = corner_count;
cvShowImage ("Calibration", src_img[i]);
//cvWaitKey (0);
}
cvDestroyWindow ("Calibration");
if (found_num != nImages)
return -1;
cvInitMatHeader (&image_points, nAllPoints, 1, CV_32FC2, corners);
cvInitMatHeader (&point_counts, nImages, 1, CV_32SC1, p_count);
// (5)
cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion);
// (6)
/*
CvMat sub_image_points, sub_object_points;
int base = 0;
cvGetRows (&image_points, &sub_image_points, base * nChessSize, (base + 1) * nChessSize);
cvGetRows (&object_points, &sub_object_points, base * nChessSize, (base + 1) * nChessSize);
cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);
*/
// (7)ToXML
/*
CvFileStorage *fs;
fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite (fs, "intrinsic", intrinsic);
cvWrite (fs, "rotation", rotation);
cvWrite (fs, "translation", translation);
cvWrite (fs, "distortion", distortion);
cvReleaseFileStorage (&fs);*/
for (i = 0; i < nImages; i++)
cvReleaseImage (&src_img[i]);
delete p_count;
delete src_img;
delete objects;
return 1;
}
int CCalibration::CalibrateFromFiles(int nImages, int nChessRow, int nChessCol, float nSquareSize, CvMat* intrinsic, CvMat* distortion)
{
int nChessSize = nChessRow*nChessCol;
int nAllPoints = nImages*nChessSize;
int i, j, k;
int corner_count, found;
int* p_count = new int[nImages];
IplImage **src_img = new IplImage*[nImages];
CvSize pattern_size = cvSize (nChessCol, nChessRow);
CvPoint3D32f* objects = new CvPoint3D32f[nAllPoints];
CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * nAllPoints);
CvMat object_points;
CvMat image_points;
CvMat point_counts;
//CvMat *rotation = cvCreateMat (1, 3, CV_32FC1);
//CvMat *translation = cvCreateMat (1, 3, CV_32FC1);
CvMat *PMatrix = cvCreateMat (3, 4, CV_32FC1);
CvMat *tmp33 = cvCreateMat(3,3,CV_32FC1);
CvMat *tmp31 = cvCreateMat(3,1,CV_32FC1);
CvMat *roTrans = cvCreateMat(3,3,CV_32FC1);
CvMat *translationTrans = cvCreateMat(3,1,CV_32FC1);
// (1)
for (i = 0; i < nImages; i++) {
char buf[32];
sprintf (buf, "%d.png", i);
if ((src_img[i] = cvLoadImage (buf, CV_LOAD_IMAGE_COLOR)) == NULL) {
fprintf (stderr, "cannot load image file : %s\n", buf);
}
}
// (2)
for (i = 0; i < nImages; i++) {
for (j = 0; j < nChessRow; j++) {
for (k = 0; k < nChessCol; k++) {
objects[i * nChessSize + j * nChessCol + k].x = j * nSquareSize;
objects[i * nChessSize + j * nChessCol + k].y = k * nSquareSize;
objects[i * nChessSize + j * nChessCol + k].z = 0.0;
}
}
}
cvInitMatHeader (&object_points, nAllPoints, 3, CV_32FC1, objects);
// (3)
int found_num = 0;
//cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE);
for (i = 0; i < nImages; i++) {
found = cvFindChessboardCorners (src_img[i], pattern_size, &corners[i * nChessSize], &corner_count);
fprintf (stderr, "%02d...", i);
if (found) {
fprintf (stderr, "ok\n");
found_num++;
}
else {
fprintf (stderr, "fail\n");
}
// (4)
IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1);
cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY);
cvFindCornerSubPix (src_gray, &corners[i * nChessSize], corner_count,
cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
p_count[i] = corner_count;
cvWaitKey (0);
}
if (found_num != nImages)
return -1;
cvInitMatHeader (&image_points, nAllPoints, 1, CV_32FC2, corners);
cvInitMatHeader (&point_counts, nImages, 1, CV_32SC1, p_count);
// (5)
cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion);
/* (6) ***************** TEST EXTRINSIC PARAMETERS ******************
CvMat sub_image_points, sub_object_points;
int base = 0;
cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE);
CModel myBox("box.txt");
for (i=0; i<nImages; i++)
{
cvGetRows (&image_points, &sub_image_points, base * nChessSize, (base + 1) * nChessSize);
cvGetRows (&object_points, &sub_object_points, base * nChessSize, (base + 1) * nChessSize);
cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);
CvMat *rotMat = cvCreateMat(3, 3, CV_32FC1);
cvRodrigues2(rotation, rotMat);
//cvTranspose(rotMat,roTrans);
cvTranspose(translation,translationTrans);
CvMat* tmp34 = cvCreateMat(3,4,CV_32FC1);
for (int i = 0; i < 3; i++)
for (int j=0; j<3; j++)
cvmSet(tmp34,i,j,cvmGet(rotMat,i,j));
for (int i = 0; i < 3; i++)
cvmSet(tmp34,i,3,cvmGet(translationTrans,i,0));
cvMatMul(intrinsic,tmp34,PMatrix);
myBox.Draw(src_img[i], PMatrix, cvScalar(0, 255, 0), 2);
cvShowImage ("Calibration", src_img[i]);
cvWaitKey(0);
base++;
}*/
cvDestroyWindow("Calibration");
// Luu ket qua
/* CvFileStorage *fs;
fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite (fs, "intrinsic", intrinsic);
cvWrite (fs, "rotation", rotation);
cvWrite (fs, "translation", translation);
cvWrite (fs, "distortion", distortion);
cvReleaseFileStorage (&fs);*/
for (i = 0; i < nImages; i++) {
cvReleaseImage (&src_img[i]);
}
delete p_count;
delete src_img;
delete objects;
return 1;
}
int
main (int argc, char *argv[])
{
int i, j, k;
int corner_count, found;
int p_count[IMAGE_NUM];
IplImage *src_img[IMAGE_NUM];
CvSize pattern_size = cvSize (PAT_COL, PAT_ROW);
CvPoint3D32f objects[ALL_POINTS];
CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * ALL_POINTS);
CvMat object_points;
CvMat image_points;
CvMat point_counts;
CvMat *intrinsic = cvCreateMat (3, 3, CV_32FC1);
CvMat *rotation = cvCreateMat (1, 3, CV_32FC1);
CvMat *translation = cvCreateMat (1, 3, CV_32FC1);
CvMat *distortion = cvCreateMat (1, 4, CV_32FC1);
// (1)キャリブレーション画像の読み込み
for (i = 0; i < IMAGE_NUM; i++) {
char buf[32];
sprintf (buf, "calib_img/%02d.png", i);
if ((src_img[i] = cvLoadImage (buf, CV_LOAD_IMAGE_COLOR)) == NULL) {
fprintf (stderr, "cannot load image file : %s\n", buf);
}
}
// (2)3次元空間座標の設定
for (i = 0; i < IMAGE_NUM; i++) {
for (j = 0; j < PAT_ROW; j++) {
for (k = 0; k < PAT_COL; k++) {
objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;
}
}
}
cvInitMatHeader (&object_points, ALL_POINTS, 3, CV_32FC1, objects);
// (3)チェスボード（キャリブレーションパターン）のコーナー検出
int found_num = 0;
cvNamedWindow ("Calibration", CV_WINDOW_AUTOSIZE);
for (i = 0; i < IMAGE_NUM; i++) {
found = cvFindChessboardCorners (src_img[i], pattern_size, &corners[i * PAT_SIZE], &corner_count);
fprintf (stderr, "%02d...", i);
if (found) {
fprintf (stderr, "ok\n");
found_num++;
}
else {
fprintf (stderr, "fail\n");
}
// (4)コーナー位置をサブピクセル精度に修正，描画
IplImage *src_gray = cvCreateImage (cvGetSize (src_img[i]), IPL_DEPTH_8U, 1);
cvCvtColor (src_img[i], src_gray, CV_BGR2GRAY);
cvFindCornerSubPix (src_gray, &corners[i * PAT_SIZE], corner_count,
cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvDrawChessboardCorners (src_img[i], pattern_size, &corners[i * PAT_SIZE], corner_count, found);
p_count[i] = corner_count;
cvShowImage ("Calibration", src_img[i]);
cvWaitKey (0);
}
cvDestroyWindow ("Calibration");
if (found_num != IMAGE_NUM)
return -1;
cvInitMatHeader (&image_points, ALL_POINTS, 1, CV_32FC2, corners);
cvInitMatHeader (&point_counts, IMAGE_NUM, 1, CV_32SC1, p_count);
// (5)内部パラメータ，歪み係数の推定
cvCalibrateCamera2 (&object_points, &image_points, &point_counts, cvSize (640, 480), intrinsic, distortion);
// (6)外部パラメータの推定
CvMat sub_image_points, sub_object_points;
int base = 0;
cvGetRows (&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);
cvGetRows (&object_points, &sub_object_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);
cvFindExtrinsicCameraParams2 (&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);
// (7)XMLファイルへの書き出し
CvFileStorage *fs;
fs = cvOpenFileStorage ("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite (fs, "intrinsic", intrinsic);
cvWrite (fs, "rotation", rotation);
cvWrite (fs, "translation", translation);
cvWrite (fs, "distortion", distortion);
cvReleaseFileStorage (&fs);
for (i = 0; i < IMAGE_NUM; i++) {
cvReleaseImage (&src_img[i]);
}
return 0;
}
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description : This is the entry point of the program.
// Return value : SUCCESS:0 ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char **argv)
{
// AR.Drone class
ARDrone ardrone;
// Initialize
if (!ardrone.open()) {
printf("Failed to initialize.\n");
return -1;
}
// Images
std::vector<IplImage*> images;
printf("Press space key to take a sample picture !\n");
// Main loop
while (1) {
// Key input
int key = cvWaitKey(1);
if (key == 0x1b) break;
// Update
if (!ardrone.update()) break;
// Get an image
IplImage *image = ardrone.getImage();
// Convert the camera image to grayscale
IplImage *gray = cvCreateImage(cvGetSize(image), IPL_DEPTH_8U, 1);
cvCvtColor(image, gray, CV_BGR2GRAY);
// Detect the chessboard
int corner_count = 0;
CvSize size = cvSize(PAT_COL, PAT_ROW);
CvPoint2D32f corners[PAT_SIZE];
int found = cvFindChessboardCorners(gray, size, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH+CV_CALIB_CB_NORMALIZE_IMAGE|CV_CALIB_CB_FAST_CHECK);
// Chessboard detected
if (found) {
// Draw corners
cvDrawChessboardCorners(image, size, corners, corner_count, found);
// If you push Space key
if (key == ' ') {
// Add to buffer
images.push_back(gray);
}
else {
// Release the image
cvReleaseImage(&gray);
}
}
// Failed to detect
else {
// Release the image
cvReleaseImage(&gray);
}
// Display the image
cvDrawText(image, cvPoint(15, 20), "NUM = %d", (int)images.size());
cvShowImage("camera", image);
}
// Destroy the window
cvDestroyWindow("camera");
// At least one image was taken
if (!images.empty()) {
// Total number of images
const int num = (int)images.size();
//// For debug
//for (int i = 0; i < num; i++) {
// char name[256];
// sprintf(name, "images[%d/%d]", i+1, num);
// cvShowImage(name, images[i]);
// cvWaitKey(0);
// cvDestroyWindow(name);
//}
// Ask save parameters or not
if (cvAsk("Do you save the camera parameters ? (y/n)\n")) {
// Detect coners
int *p_count = (int*)malloc(sizeof(int) * num);
CvPoint2D32f *corners = (CvPoint2D32f*)cvAlloc(sizeof(CvPoint2D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
// Detect chessboard
int corner_count = 0;
CvSize size = cvSize(PAT_COL, PAT_ROW);
int found = cvFindChessboardCorners(images[i], size, &corners[i * PAT_SIZE], &corner_count);
// Convert the corners to sub-pixel
cvFindCornerSubPix(images[i], &corners[i * PAT_SIZE], corner_count, cvSize(3, 3), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
p_count[i] = corner_count;
}
// Set the 3D position of patterns
CvPoint3D32f *objects = (CvPoint3D32f*)cvAlloc(sizeof(CvPoint3D32f) * num * PAT_SIZE);
for (int i = 0; i < num; i++) {
for (int j = 0; j < PAT_ROW; j++) {
for (int k = 0; k < PAT_COL; k++) {
objects[i * PAT_SIZE + j * PAT_COL + k].x = j * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].y = k * CHESS_SIZE;
objects[i * PAT_SIZE + j * PAT_COL + k].z = 0.0;
}
}
}
// Create matrices
CvMat object_points, image_points, point_counts;
cvInitMatHeader(&object_points, num * PAT_SIZE, 3, CV_32FC1, objects);
cvInitMatHeader(&image_points, num * PAT_SIZE, 1, CV_32FC2, corners);
cvInitMatHeader(&point_counts, num, 1, CV_32SC1, p_count);
// Estimate intrinsic parameters and distortion coefficients
printf("Calicurating parameters...");
CvMat *intrinsic = cvCreateMat(3, 3, CV_32FC1);
CvMat *distortion = cvCreateMat(1, 4, CV_32FC1);
cvCalibrateCamera2(&object_points, &image_points, &point_counts, cvGetSize(images[0]), intrinsic, distortion);
printf("Finished !\n");
// Output a file
printf("Generating a XML file...");
CvFileStorage *fs = cvOpenFileStorage("camera.xml", 0, CV_STORAGE_WRITE);
cvWrite(fs, "intrinsic", intrinsic);
cvWrite(fs, "distortion", distortion);
cvReleaseFileStorage(&fs);
printf("Finished !\n");
// Release the matrices
free(p_count);
cvFree(&corners);
cvFree(&objects);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
}
// Release the images
for (int i = 0; i < num; i++) cvReleaseImage(&images[i]);
}
// See you
ardrone.close();
return 0;
}
int
main (int argc, char *argv[])
{
// IMAGE_NUM, PAT_ROW, PAT_COL,PAT_SIZE, ALL_POINTS, CHESS_SIZE
/*
if (argc < 6)
{
std::cout<< "ERROR : augment is incorrect" << std::endl;
return -1;
}
*/
//int PAT_ROW = atoi(argv[3]);
//int PAT_COL = atoi(argv[4]);
//int CHESS_SIZE = atoi(argv[5]);
//int PAT_SIZE = PAT_ROW*PAT_COL;
char* NAME_IMG_IN = argv[1];
//char* NAME_XML_OUT = argv[2];
int i,j;
int corner_count, found;
IplImage *src_img;
CvSize pattern_size = cvSize(PAT_COL, PAT_ROW);
CvMat image_points;
CvMat object_points;
CvMat *intrinsic, *distortion;
CvMat *rotation = cvCreateMat(1, 3, CV_32FC1);
CvMat *rotationConv = cvCreateMat(3, 3, CV_32FC1);
CvMat *translation = cvCreateMat(1, 3, CV_32FC1);
CvPoint3D32f objects[PAT_SIZE];
CvFileStorage *fs;
CvFileNode *param;
CvPoint2D32f *corners = (CvPoint2D32f *) cvAlloc (sizeof (CvPoint2D32f) * PAT_SIZE);
// (1)�����оݤȤʤ������ɤ߹���
if ( ( src_img = cvLoadImage(NAME_IMG_IN, CV_LOAD_IMAGE_COLOR) ) == 0)
//if (argc < 2 || (src_img = cvLoadImage (argv[1], CV_LOAD_IMAGE_COLOR)) == 0)
{
std::cout<< "ERROR : input image is not exist or augment is incorrect" << std::endl;
return -1;
}
// 3�������ֺ�ɸ������
for (i = 0; i < PAT_ROW; i++) {
for (j = 0; j < PAT_COL; j++) {
objects[i * PAT_COL + j].x = i * CHESS_SIZE;
objects[i * PAT_COL + j].y = j * CHESS_SIZE;
objects[i * PAT_COL + j].z = 0.0;
}
}
cvInitMatHeader(&object_points, PAT_SIZE, 3, CV_32FC1, objects);
// �������ܡ��ɡʥ����֥졼�����ѥ�����ˤΥ����ʡ�����
int found_num = 0;
// cvNamedWindow("Calibration", CV_WINDOW_AUTOSIZE);
found = cvFindChessboardCorners(src_img, pattern_size, &corners[0], &corner_count);
fprintf(stderr, "corner:%02d...\n", corner_count);
if (found) {
fprintf(stderr, "ok\n");
} else {
fprintf(stderr, "fail\n");
}
// (4)�����ʡ����֤֥ԥ��������٤˽���������
IplImage *src_gray = cvCreateImage (cvGetSize (src_img), IPL_DEPTH_8U, 1);
cvCvtColor (src_img, src_gray, CV_BGR2GRAY);
cvFindCornerSubPix (src_gray, &corners[0], corner_count,
cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvDrawChessboardCorners (src_img, pattern_size, &corners[0], corner_count, found);
// cvShowImage ("Calibration", src_img);
// cvWaitKey (0);
// cvDestroyWindow("Calibration");
cvShowImage ("Calibration", src_img);
cvInitMatHeader(&image_points, PAT_SIZE, 1, CV_32FC2, corners);
// (2)�ѥ����ե�������ɤ߹���
fs = cvOpenFileStorage ("xml/rgb.xml", 0, CV_STORAGE_READ);
param = cvGetFileNodeByName (fs, NULL, "intrinsic");
intrinsic = (CvMat *) cvRead (fs, param);
param = cvGetFileNodeByName (fs, NULL, "distortion");
distortion = (CvMat *) cvRead (fs, param);
cvReleaseFileStorage (&fs);
// (3) �����ѥ����ο���
CvMat sub_image_points, sub_object_points;
int base = 0;
cvGetRows(&image_points, &sub_image_points, base * PAT_SIZE, (base + 1) * PAT_SIZE);
cvGetRows(&object_points, &sub_object_points, base * PAT_SIZE, (base + 1)* PAT_SIZE);
cvFindExtrinsicCameraParams2(&sub_object_points, &sub_image_points, intrinsic, distortion, rotation, translation);
int ret = cvRodrigues2(rotation, rotationConv);
// int cols = sub_object_points.rows;
// printf("cols = %d\n", cols);
// printf("%f\n",sub_object_points.data.fl[0]);
// mm -> m
for (i = 0; i < translation->cols; i++) { translation->data.fl[i] = translation->data.fl[i] / 1000;}
// (4)XML�ե�����ؤνФ�
//fs = cvOpenFileStorage(argv[2], 0, CV_STORAGE_WRITE);
fs = cvOpenFileStorage(NAME_XML_OUT, 0, CV_STORAGE_WRITE);
cvWrite(fs, "rotation", rotationConv);
cvWrite(fs, "translation", translation);
cvReleaseFileStorage(&fs);
/////////////////////////////////////////////////
// write out py
if(1)
{
cv::Mat ttt(translation);
cv::Mat rrr(rotationConv);
char data2Write[1024];
char textFileName[256];
sprintf( textFileName , "cbCoord/cbOneShot.py");
std::ofstream outPy(textFileName);
outPy << "import sys" <<std::endl;
outPy << "sys.path.append('../')" <<std::endl;
outPy << "from numpy import *" <<std::endl;
outPy << "from numpy.linalg import svd" <<std::endl;
outPy << "from numpy.linalg import inv" <<std::endl;
outPy << "from chessboard_points import *"<<std::endl;
outPy << "sys.path.append('../geo')" <<std::endl;
outPy << "from geo import *" <<std::endl;
/*
///////////////////////////////////////////////////////////////////////////////////
// out translation and rotation as xyzabc list
outPy << "xyzabc = []" <<std::endl;
sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(0) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(1) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", ttt.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(0) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(1) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "xyzabc.append(%f)", rrr.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
// out translation and rotation as xyzabc list
///////////////////////////////////////////////////////////////////////////////////
*/
///////////////////////////////////////////////////////////////////////////////////
// out translation
outPy << "ttt = []" <<std::endl;
sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(0) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(1) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "ttt.append(%f)", ttt.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
// out translation
//////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
// out rotation
outPy << "rrr = []" <<std::endl;
sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(0), rrr.at<float>(1), rrr.at<float>(2) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(3), rrr.at<float>(4), rrr.at<float>(5) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
sprintf( data2Write, "rrr.append([%f,%f,%f])", rrr.at<float>(6), rrr.at<float>(7), rrr.at<float>(8) );
outPy << data2Write << std::endl;
std::cout << data2Write << std::endl;
// out rotation
//////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////
outPy<< "_T = FRAME( vec=ttt, mat=rrr )" << std::endl;
/////////////////////////////////////////////////////////////////
}
// write out py
/////////////////////////////////////////////////
std::cout<< "press any key..."<< std::endl;
cvWaitKey (0);
cvDestroyWindow("Calibration");
cvReleaseImage(&src_img);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
return 0;
}
void* captureThread(void* args)
{
CvCapture* capture = NULL;
IplImage* frame;
IplImage* frame_copy;
IplImage* gray;
ChessBoard chess;
int pointsPerScene;
CvPoint2D32f* points;
int pointsNum[1];
CvMat* cornerPoints;
CvMat* objectPoints;
CvMat pointsNumMat;
CvMat* intrinsic = cvCreateMat( 3, 3, CV_64F );
CvMat* distortion = cvCreateMat( 4, 1, CV_64F );
CvMat* rotation = cvCreateMat( 3, 3, CV_64F );
CvMat* translation = cvCreateMat( 3, 1, CV_64F );
loadIntrinsicParams("intrinsic_param_ref.txt", intrinsic, distortion );
capture = cvCreateCameraCapture(0);
if(capture == NULL){
fprintf(stderr, "ERROR: Could not open Camera Device\n");
exit(1);
}
frame = cvQueryFrame(capture);
if(frame == NULL){
fprintf(stderr, "ERROR: Could not query frame\n");
exit(1);
}
frame_copy = cvCreateImage(cvGetSize(frame),
frame->depth, 3);
gray = cvCreateImage(cvGetSize(frame_copy), frame_copy->depth, 1);
cvNamedWindow(captureWinName, CV_WINDOW_AUTOSIZE);
cvMoveWindow(captureWinName, graphicsWinWidth + 10, 0);
chess.dx = CHESS_ROW_DX;
chess.dy = CHESS_COL_DY;
chess.patternSize.width = CHESS_ROW_NUM;
chess.patternSize.height = CHESS_COL_NUM;
pointsPerScene = chess.patternSize.width * chess.patternSize.height;
cornerPoints = cvCreateMat(pointsPerScene, 2, CV_32F);
objectPoints = cvCreateMat(pointsPerScene, 3, CV_32F);
pointsNum[0] = pointsPerScene;
pointsNumMat = cvMat(1, 1, CV_32S, pointsNum);
points = (CvPoint2D32f*)malloc( sizeof(CvPoint2D32f) * pointsPerScene );
while(1){
int allPointsFound = 0;
int detectedPointsNum;
frame = cvQueryFrame( capture );
if( !frame ) {
fprintf(stderr, "could not query frame\n");
exit(1);
}
cvResize(frame, frame_copy, CV_INTER_NN);
cvCvtColor(frame_copy, gray, CV_BGR2GRAY);
if( cvFindChessboardCorners( gray, chess.patternSize, points,
&detectedPointsNum,
CV_CALIB_CB_ADAPTIVE_THRESH ) ){
cvFindCornerSubPix(gray, points, detectedPointsNum,
cvSize(5, 5), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1));
allPointsFound = 1;
} else {
allPointsFound = 0;
}
cvDrawChessboardCorners( frame_copy, chess.patternSize, points,
detectedPointsNum, allPointsFound );
if( allPointsFound ){
double cameraPosition[3];
double cameraOriVec[4];
store2DCoordinates( cornerPoints, points, chess, 0 );
store3DCoordinates( objectPoints, chess, 0 );
calibrate( cornerPoints, objectPoints,
intrinsic, distortion, rotation, translation );
getCameraPosition(rotation, translation, cameraPosition);
printf("cam pos relative to chess board: %.1f, %.1f, %.1f\n",
cameraPosition[0],
cameraPosition[1],
cameraPosition[2]);
convertCv2Gl(cameraPosition, transGL);
getCameraOriVec(rotation, rotGL);
}
cvShowImage( captureWinName, frame_copy);
if(cvWaitKey(10) == KEY_ESC){
exit(1);
}
}
free(points);
cvReleaseMat(&intrinsic);
cvReleaseMat(&distortion);
cvReleaseMat(&rotation);
cvReleaseMat(&translation);
cvReleaseMat(&cornerPoints);
cvReleaseMat(&objectPoints);
cvDestroyWindow(captureWinName);
cvReleaseImage(&frame_copy);
cvReleaseImage(&gray);
cvReleaseCapture(&capture);
}
int main(int argc, char* argv[]) {
if(argc != 6){
printf("too few args\n");
return -1;
}
// INPUT PARAMETERS:
//
int board_w = atoi(argv[1]);
int board_h = atoi(argv[2]);
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
CvMat* intrinsic = (CvMat*)cvLoad(argv[3]);
CvMat* distortion = (CvMat*)cvLoad(argv[4]);
IplImage* image = 0;
IplImage* gray_image = 0;
if( (image = cvLoadImage(argv[5])) == 0 ) {
printf("Error: Couldn't load %s\n",argv[5]);
return -1;
}
CvMat* image_points = cvCreateMat(1*board_n,2,CV_32FC1);
CvMat* object_points = cvCreateMat(1*board_n,3,CV_32FC1);
CvMat* objdrawpoints = cvCreateMat(1,1,CV_32FC3);
CvMat* imgdrawpoints = cvCreateMat(1,1,CV_32FC2);
float x=0;
float y=0;
float z=0;
double grid_width=2.85;
gray_image = cvCreateImage( cvGetSize(image), 8, 1 );
cvCvtColor(image, gray_image, CV_BGR2GRAY );
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count = 0;
int found = cvFindChessboardCorners(
gray_image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
if(!found){
printf("Couldn't aquire chessboard on %s, "
"only found %d of %d corners\n",
argv[5],corner_count,board_n
);
return -1;
}
//Get Subpixel accuracy on those corners:
cvFindCornerSubPix(
gray_image,
corners,
corner_count,
cvSize(11,11),
cvSize(-1,-1),
cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1 )
);
// If we got a good board, add it to our data
for( int i=0, j=0; j<board_n; ++i,++j ) {
CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
CV_MAT_ELEM(*object_points,float,i,0) =grid_width*( j/board_w);
// cout<<j/board_w<<" "<<j%board_w<<endl;
CV_MAT_ELEM(*object_points,float,i,1) = grid_width*(j%board_w);
CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
}
// DRAW THE FOUND CHESSBOARD
//
cvDrawChessboardCorners(
image,
board_sz,
corners,
corner_count,
found
);
// FIND THE HOMOGRAPHY
//
CvMat *trans = cvCreateMat( 1, 3, CV_32F);
CvMat *rot = cvCreateMat( 1, 3, CV_32F);
// LET THE USER ADJUST THE Z HEIGHT OF THE VIEW
//
cvFindExtrinsicCameraParams2(object_points,image_points,intrinsic,distortion,rot,trans);
// cvSave("trans.xml",trans);
// cvSave("rot.xml",rot);
int key = 0;
IplImage *drawn_image = cvCloneImage(image);
cvNamedWindow("translation");
// LOOP TO ALLOW USER TO PLAY WITH HEIGHT:
//
// escape key stops
//
// cvSetZero(trans);
// cvSetZero(rot);
while(key != 27) {
cvCopy(image,drawn_image);
if(key==97)x--;
else if(key==113)x++;
else if(key==115)y--;
else if(key==119)y++;
else if(key==100)z--;
else if(key==101)z++;
((float*)(objdrawpoints->data.ptr))[0]=x;
((float*)(objdrawpoints->data.ptr))[1]=y;
((float*)(objdrawpoints->data.ptr))[2]=z;
printf("%f %f %f\n",x,y,z);
cvProjectPoints2(objdrawpoints,rot,trans,intrinsic,distortion,imgdrawpoints);
cvCircle(drawn_image,cvPoint(((float*)(imgdrawpoints->data.ptr))[0],((float*)(imgdrawpoints->data.ptr))[1]),5,cvScalar(255,0,0),-1);
printf("%f %f\n",((float*)(imgdrawpoints->data.ptr))[0],((float*)(imgdrawpoints->data.ptr))[1]);
cvShowImage( "translation", drawn_image );
key = cvWaitKey(3);
}
cvDestroyWindow( "translation" );
//must add a lot of memory releasing here
return 0;
}
static void mainLoop( void )
{
AR2VideoBufferT *videoBuffL;
AR2VideoBufferT *videoBuffR;
ARUint8 *dataPtrL;
ARUint8 *dataPtrR;
int cornerFlagL;
int cornerFlagR;
int cornerCountL;
int cornerCountR;
char buf[256];
int i;
if ((videoBuffL = ar2VideoGetImage(vidL))) {
gVideoBuffL = videoBuffL;
}
if ((videoBuffR = ar2VideoGetImage(vidR))) {
gVideoBuffR = videoBuffR;
}
if (gVideoBuffL && gVideoBuffR) {
// Warn about significant time differences.
i = ((int)gVideoBuffR->time_sec - (int)gVideoBuffL->time_sec) * 1000
+ ((int)gVideoBuffR->time_usec - (int)gVideoBuffL->time_usec) / 1000;
if( i > 20 ) {
ARLOG("Time diff = %d[msec]\n", i);
} else if( i < -20 ) {
ARLOG("Time diff = %d[msec]\n", i);
}
dataPtrL = gVideoBuffL->buff;
dataPtrR = gVideoBuffR->buff;
glClear(GL_COLOR_BUFFER_BIT);
argDrawMode2D( vpL );
argDrawImage( dataPtrL );
argDrawMode2D( vpR );
argDrawImage( dataPtrR );
copyImage( dataPtrL, (ARUint8 *)calibImageL->imageData, xsizeL*ysizeL, pixFormatL );
cornerFlagL = cvFindChessboardCorners(calibImageL, cvSize(chessboardCornerNumY,chessboardCornerNumX),
cornersL, &cornerCountL, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS );
copyImage( dataPtrR, (ARUint8 *)calibImageR->imageData, xsizeR*ysizeR, pixFormatR );
cornerFlagR = cvFindChessboardCorners(calibImageR, cvSize(chessboardCornerNumY,chessboardCornerNumX),
cornersR, &cornerCountR, CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS );
argDrawMode2D( vpL );
if(cornerFlagL) glColor3f(1.0f, 0.0f, 0.0f);
else glColor3f(0.0f, 1.0f, 0.0f);
glLineWidth(2.0f);
//ARLOG("Detected corners = %d\n", cornerCount);
for( i = 0; i < cornerCountL; i++ ) {
argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y-5, cornersL[i].x+5, cornersL[i].y+5);
argDrawLineByObservedPos(cornersL[i].x-5, cornersL[i].y+5, cornersL[i].x+5, cornersL[i].y-5);
//ARLOG(" %f, %f\n", cornersL[i].x, cornersL[i].y);
sprintf(buf, "%d\n", i);
argDrawStringsByObservedPos(buf, cornersL[i].x, cornersL[i].y+20);
}
argDrawMode2D( vpR );
if(cornerFlagR) glColor3f(1.0f, 0.0f, 0.0f);
else glColor3f(0.0f, 1.0f, 0.0f);
glLineWidth(2.0f);
//ARLOG("Detected corners = %d\n", cornerCount);
for( i = 0; i < cornerCountR; i++ ) {
argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y-5, cornersR[i].x+5, cornersR[i].y+5);
argDrawLineByObservedPos(cornersR[i].x-5, cornersR[i].y+5, cornersR[i].x+5, cornersR[i].y-5);
//ARLOG(" %f, %f\n", cornersR[i].x, cornersR[i].y);
sprintf(buf, "%d\n", i);
argDrawStringsByObservedPos(buf, cornersR[i].x, cornersR[i].y+20);
}
if( cornerFlagL && cornerFlagR ) {
cornerFlag = 1;
glColor3f(1.0f, 0.0f, 0.0f);
}
else {
cornerFlag = 0;
glColor3f(0.0f, 1.0f, 0.0f);
}
argDrawMode2D( vpL );
sprintf(buf, "Captured Image: %2d/%2d\n", capturedImageNum, calibImageNum);
argDrawStringsByIdealPos(buf, 10, 30);
argSwapBuffers();
gVideoBuffL = gVideoBuffR = NULL;
} else arUtilSleep(2);
}
int main( int argc, char** argv )
{
CvSize board_size = {0,0};
float square_size = 1.f, aspect_ratio = 1.f;
const char* out_filename = "out_camera_data.yml";
const char* input_filename = 0;
int i, image_count = 10;
int write_extrinsics = 0, write_points = 0;
int flags = 0;
CvCapture* capture = 0;
FILE* f = 0;
char imagename[1024];
CvMemStorage* storage;
CvSeq* image_points_seq = 0;
int elem_size, flip_vertical = 0;
int delay = 1000;
clock_t prev_timestamp = 0;
CvPoint2D32f* image_points_buf = 0;
CvFont font = cvFont( 1, 1 );
double _camera[9], _dist_coeffs[4];
CvMat camera = cvMat( 3, 3, CV_64F, _camera );
CvMat dist_coeffs = cvMat( 1, 4, CV_64F, _dist_coeffs );
CvMat *extr_params = 0, *reproj_errs = 0;
double avg_reproj_err = 0;
int mode = DETECTION;
int undistort_image = 0;
CvSize img_size = {0,0};
const char* live_capture_help =
"When the live video from camera is used as input, the following hot-keys may be used:\n"
" <ESC>, 'q' - quit the program\n"
" 'g' - start capturing images\n"
" 'u' - switch undistortion on/off\n";
if( argc < 2 )
{
// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [<list_of_views.txt>]
printf( "This is a camera calibration sample.\n"
"Usage: calibration\n"
" -w <board_width> # the number of inner corners per one of board dimension\n"
" -h <board_height> # the number of inner corners per another board dimension\n"
" [-n <number_of_frames>] # the number of frames to use for calibration\n"
" # (if not specified, it will be set to the number\n"
" # of board views actually available)\n"
" [-di <disk_images> # Number of disk images before triggering undistortion\n"
" [-d <delay>] # a minimum delay in ms between subsequent attempts to capture a next view\n"
" # (used only for video capturing)\n"
" [-s <square_size>] # square size in some user-defined units (1 by default)\n"
" [-o <out_camera_params>] # the output filename for intrinsic [and extrinsic] parameters\n"
" [-op] # write detected feature points\n"
" [-oe] # write extrinsic parameters\n"
" [-zt] # assume zero tangential distortion\n"
" [-a <aspect_ratio>] # fix aspect ratio (fx/fy)\n"
" [-p] # fix the principal point at the center\n"
" [-v] # flip the captured images around the horizontal axis\n"
" [input_data] # input data, one of the following:\n"
" # - text file with a list of the images of the board\n"
" # - name of video file with a video of the board\n"
" # if input_data not specified, a live view from the camera is used\n"
"\n" );
printf( "%s", live_capture_help );
return 0;
}
for( i = 1; i < argc; i++ )
{
const char* s = argv[i];
if( strcmp( s, "-w" ) == 0 )
{
if( sscanf( argv[++i], "%u", &board_size.width ) != 1 || board_size.width <= 0 )
return fprintf( stderr, "Invalid board width\n" ), -1;
}
else if( strcmp( s, "-h" ) == 0 )
{
if( sscanf( argv[++i], "%u", &board_size.height ) != 1 || board_size.height <= 0 )
return fprintf( stderr, "Invalid board height\n" ), -1;
}
else if( strcmp( s, "-s" ) == 0 )
{
if( sscanf( argv[++i], "%f", &square_size ) != 1 || square_size <= 0 )
return fprintf( stderr, "Invalid board square width\n" ), -1;
}
else if( strcmp( s, "-n" ) == 0 )
{
if( sscanf( argv[++i], "%u", &image_count ) != 1 || image_count <= 3 )
return printf("Invalid number of images\n" ), -1;
}
else if( strcmp( s, "-di") == 0)
{
if( sscanf( argv[++i], "%d", &images_from_file) != 1 || images_from_file < 3)
return printf("Invalid di, must be >= 3\n"), -1;
}
else if( strcmp( s, "-a" ) == 0 )
{
if( sscanf( argv[++i], "%f", &aspect_ratio ) != 1 || aspect_ratio <= 0 )
return printf("Invalid aspect ratio\n" ), -1;
}
else if( strcmp( s, "-d" ) == 0 )
{
if( sscanf( argv[++i], "%u", &delay ) != 1 || delay <= 0 )
return printf("Invalid delay\n" ), -1;
}
else if( strcmp( s, "-op" ) == 0 )
{
write_points = 1;
}
else if( strcmp( s, "-oe" ) == 0 )
{
write_extrinsics = 1;
}
else if( strcmp( s, "-zt" ) == 0 )
{
flags |= CV_CALIB_ZERO_TANGENT_DIST;
}
else if( strcmp( s, "-p" ) == 0 )
{
flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
}
else if( strcmp( s, "-v" ) == 0 )
{
flip_vertical = 1;
}
else if( strcmp( s, "-o" ) == 0 )
{
out_filename = argv[++i];
}
else if( s[0] != '-' )
input_filename = s;
else
return fprintf( stderr, "Unknown option %s", s ), -1;
}
if( input_filename )
{
capture = cvCreateFileCapture( input_filename );
if( !capture )
{
f = fopen( input_filename, "rt" );
if( !f )
return fprintf( stderr, "The input file could not be opened\n" ), -1;
image_count = -1;
}
mode = CAPTURING;
}
else
capture = cvCreateCameraCapture(0);
if( !capture && !f )
return fprintf( stderr, "Could not initialize video capture\n" ), -2;
if( capture )
printf( "%s", live_capture_help );
elem_size = board_size.width*board_size.height*sizeof(image_points_buf[0]);
storage = cvCreateMemStorage( MAX( elem_size*4, 1 << 16 ));
image_points_buf = (CvPoint2D32f*)cvAlloc( elem_size );
image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
cvNamedWindow( "Image View", 1 );
cvNamedWindow( "Undistort",1);
int disk_image_cnt = 0;
for(;;)
{
IplImage *view = 0, *view_gray = 0;
int count = 0, found, blink = 0;
CvPoint text_origin;
CvSize text_size = {0,0};
int base_line = 0;
char s[100];
int key;
if( f && fgets( imagename, sizeof(imagename)-2, f ))
{
int l = strlen(imagename);
if( l > 0 && imagename[l-1] == '\n' )
imagename[--l] = '\0';
if( l > 0 )
{
if( imagename[0] == '#' )
continue;
view = cvLoadImage( imagename, 1 );
disk_image_cnt++;
}
}
else if( capture )
{
IplImage* view0 = cvQueryFrame( capture );
if( view0 )
{
view = cvCreateImage( cvGetSize(view0), IPL_DEPTH_8U, view0->nChannels );
if( view0->origin == IPL_ORIGIN_BL )
cvFlip( view0, view, 0 );
else
cvCopy( view0, view );
}
}
if( !view || (disk_image_cnt == images_from_file))
{
if( image_points_seq->total > 0 )
{
image_count = image_points_seq->total;
goto calibrate;
}
break;
}
if( flip_vertical )
cvFlip( view, view, 0 );
img_size = cvGetSize(view);
found = cvFindChessboardCorners( view, board_size,
image_points_buf, &count, CV_CALIB_CB_ADAPTIVE_THRESH );
#if 1
// improve the found corners' coordinate accuracy
view_gray = cvCreateImage( cvGetSize(view), 8, 1 );
cvCvtColor( view, view_gray, CV_BGR2GRAY );
cvFindCornerSubPix( view_gray, image_points_buf, count, cvSize(11,11),
cvSize(-1,-1), cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
cvReleaseImage( &view_gray );
#endif
if( mode == CAPTURING && found && (f || clock() - prev_timestamp > delay*1e-3*CLOCKS_PER_SEC) )
{
cvSeqPush( image_points_seq, image_points_buf );
prev_timestamp = clock();
blink = !f;
#if 1
if( capture )
{
sprintf( imagename, "view%03d.png", image_points_seq->total - 1 );
cvSaveImage( imagename, view );
}
#endif
}
cvDrawChessboardCorners( view, board_size, image_points_buf, count, found );
cvGetTextSize( "100/100", &font, &text_size, &base_line );
text_origin.x = view->width - text_size.width - 10;
text_origin.y = view->height - base_line - 10;
if( mode == CAPTURING )
{
if( image_count > 0 )
sprintf( s, "%d/%d", image_points_seq ? image_points_seq->total : 0, image_count );
else
sprintf( s, "%d/?", image_points_seq ? image_points_seq->total : 0 );
}
else if( mode == CALIBRATED )
sprintf( s, "Calibrated" );
else
sprintf( s, "Press 'g' to start" );
cvPutText( view, s, text_origin, &font, mode != CALIBRATED ?
CV_RGB(255,0,0) : CV_RGB(0,255,0));
if( blink )
cvNot( view, view );
//Rectify or Undistort the image
if( mode == CALIBRATED && undistort_image )
{
IplImage* t = cvCloneImage( view );
cvShowImage("Image View", view);
cvUndistort2( t, view, &camera, &dist_coeffs );
cvReleaseImage( &t );
cvShowImage( "Undistort", view );
cvWaitKey(0);
}
else{
cvShowImage( "Image View", view );
key = cvWaitKey(capture ? 50 : 500);
}
if( key == 27 )
break;
if( key == 'u' && mode == CALIBRATED ){
undistort_image = !undistort_image;
}
if( capture && key == 'g' )
{
mode = CAPTURING;
cvClearMemStorage( storage );
image_points_seq = cvCreateSeq( 0, sizeof(CvSeq), elem_size, storage );
}
if( mode == CAPTURING && (unsigned)image_points_seq->total >= (unsigned)image_count )
{
calibrate:
if(disk_image_cnt == images_from_file)
undistort_image = !undistort_image;
cvReleaseMat( &extr_params );
cvReleaseMat( &reproj_errs );
int code = run_calibration( image_points_seq, img_size, board_size,
square_size, aspect_ratio, flags, &camera, &dist_coeffs, &extr_params,
&reproj_errs, &avg_reproj_err );
// save camera parameters in any case, to catch Inf's/NaN's
save_camera_params( out_filename, image_count, img_size,
board_size, square_size, aspect_ratio, flags,
&camera, &dist_coeffs, write_extrinsics ? extr_params : 0,
write_points ? image_points_seq : 0, reproj_errs, avg_reproj_err );
if( code )
mode = CALIBRATED;
else
mode = DETECTION;
}
if( !view )
break;
cvReleaseImage( &view );
}
if( capture )
cvReleaseCapture( &capture );
return 0;
}
void bird_eye() {
int board_n = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
CvMat *intrinsic = (CvMat*) cvLoad("Intrinsics.xml");
CvMat *distortion = (CvMat*) cvLoad("Distortion.xml");
IplImage* image = cvLoadImage("./Resource/bird-eye.jpg", 1);
IplImage* gray_image = cvCreateImage(cvGetSize(image), 8, 1);
cvCvtColor(image, gray_image, CV_BGR2GRAY);
IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
cvInitUndistortMap(
intrinsic,
distortion,
mapx,
mapy
);
IplImage* t = cvCloneImage(image);
cvRemap(t, image, mapx, mapy);
cvNamedWindow("Chessboard");
cvShowImage("Chessboard", image);
int c = cvWaitKey(-1);
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int corner_count = 0;
int found = cvFindChessboardCorners(
image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
if(!found){
printf("couldn't aquire chessboard!\n");
return;
}
cvFindCornerSubPix(
gray_image,
corners,
corner_count,
cvSize(11, 11),
cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1)
);
CvPoint2D32f objPts[4], imgPts[4];
objPts[0].x = 0; objPts[0].y = 0;
objPts[1].x = board_w - 1; objPts[1].y = 0;
objPts[2].x = 0; objPts[2].y = board_h - 1;
objPts[3].x = board_w - 1; objPts[3].y = board_h - 1;
imgPts[0] = corners[0];
imgPts[1] = corners[board_w - 1];
imgPts[2] = corners[(board_h - 1) * board_w];
imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1];
cvCircle(image, cvPointFrom32f(imgPts[0]), 9, CV_RGB(0, 0, 255), 3);
cvCircle(image, cvPointFrom32f(imgPts[1]), 9, CV_RGB(0, 255, 0), 3);
cvCircle(image, cvPointFrom32f(imgPts[2]), 9, CV_RGB(255, 0, 0), 3);
cvCircle(image, cvPointFrom32f(imgPts[3]), 9, CV_RGB(255, 255, 0), 3);
cvDrawChessboardCorners(
image,
board_sz,
corners,
corner_count,
found
);
cvShowImage("Chessboard", image);
CvMat *H = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(objPts, imgPts, H);
float z = 25;
int key = 0;
IplImage * birds_image = cvCloneImage(image);
cvNamedWindow("Birds_Eye");
while(key != 27) {
CV_MAT_ELEM(*H, float, 2, 2) = z;
cvWarpPerspective(
image,
birds_image,
H,
CV_INTER_LINEAR| CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS
);
cvShowImage("Birds_Eye", birds_image);
key = cvWaitKey();
if(key == 'u') z += 0.5;
if(key == 'd') z -= 0.5;
}
cvSave("H.xml", H);
}
int main (int argc, char **argv)
{
if(argc < 8) {
printf("usage: Compute <skip> <out_dir> <num_cameras> <corner_rows> <corner_cols> <int_dir1> <rgb_images1> <ir_images1> <int_dir2> ...\n");
return 1;
}
int skip = atoi(argv[1]);
char * outDir = argv[2];
int numCameras = atoi(argv[3]);
int corner_rows = atoi(argv[4]);
int corner_cols = atoi(argv[5]);
int argBase = 6;
if(argc != argBase+3*numCameras) {
printf("Expected %d intrinsics entries for %d cameras.\n", numCameras, numCameras);
return -1;
}
char temp[1024];
int i, j, k, type;
CvMat* cam_mat = cvCreateMat( 3, 3, CV_32FC1 );
CvMat* dist_coeff = cvCreateMat( 5, 1, CV_32FC1 );
CvPoint2D32f* corners = (CvPoint2D32f*) malloc(sizeof(CvPoint2D32f)*corner_rows*corner_cols);
for(type = 0; type <= 1; type++) {
for(i = 0; i < numCameras; i++) {
int numSkipped = 0;
int numImages = atoi(argv[argBase+i*3+(type==0?1:2)]);
char * intDir = argv[argBase+i*3];
CvMat* image_points = cvCreateMat(numImages*corner_rows*corner_cols, 2, CV_32FC1 );
CvMat* object_points = cvCreateMat(numImages*corner_rows*corner_cols, 3, CV_32FC1 );
CvMat* point_counts = cvCreateMat(numImages, 1, CV_32SC1 );
for(j = 0; j < numImages; j++) {
//load image
sprintf(temp, "%s/img_%s_%d.png", intDir, type==0?"rgb":"ir", j*skip);
printf("loading %s...\n",temp);
IplImage *img = cvLoadImage(temp, CV_LOAD_IMAGE_GRAYSCALE);
if(img != NULL) {
int corner_count = 0;
if(cvFindChessboardCorners(img, cvSize(corner_cols, corner_rows), corners, &corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_NORMALIZE_IMAGE)) {
cvFindCornerSubPix(img, corners, corner_count, cvSize(11, 11), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, REFINE_MAX_ITER, REFINE_EPSILON));
int offset = (j-numSkipped)*corner_rows*corner_cols;
for(k = 0; k < corner_rows*corner_cols; k++) {
CV_MAT_ELEM(*image_points, float, offset+k, 0 ) = corners[k].x;
CV_MAT_ELEM(*image_points, float, offset+k, 1 ) = corners[k].y;
CV_MAT_ELEM(*object_points, float, offset+k, 0 ) = SQUARE_SIZE*(k%corner_cols);
CV_MAT_ELEM(*object_points, float, offset+k, 1 ) = SQUARE_SIZE*(k/corner_cols);
CV_MAT_ELEM(*object_points, float, offset+k, 2 ) = 0.0f;
}
CV_MAT_ELEM(*point_counts, int, j-numSkipped, 0 ) = corner_rows*corner_cols;
} else {
printf("Cannot find corners in image %s, skipping\n", temp);
numSkipped++;
//return -1;
}
cvReleaseImage(&img);
} else {
void ProjectionModel::calculateProjection()
{
if(intrinsic_matrix != 0 && distortion_coeffs != 0)
{
int corner_count = Chessboard::board_total;
cvCvtColor(sourceImg, gray_image, CV_RGB2GRAY);
int found = cvFindChessboardCorners(gray_image,
board_sz,
corners,
&corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if(!found)
{
return;
}
cvFindCornerSubPix(gray_image,
corners,
corner_count,
cvSize(11,11),
cvSize(-1,-1),
cvTermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
objPts[0].x = 0;
objPts[0].y = 0;
objPts[1].x = Chessboard::board_w - 1;
objPts[1].y = 0;
objPts[2].x = 0;
objPts[2].y = Chessboard::board_h - 1;
objPts[3].x = Chessboard::board_w - 1;
objPts[3].y = Chessboard::board_h - 1;
imgPts[3] = corners[0];
imgPts[2] = corners[Chessboard::board_w - 1];
imgPts[1] = corners[(Chessboard::board_h - 1) * Chessboard::board_w];
imgPts[0] = corners[(Chessboard::board_h - 1) * Chessboard::board_w + Chessboard::board_w - 1];
cvGetPerspectiveTransform(objPts, imgPts, H);
for(int i = 0; i < 4; ++i)
{
CV_MAT_ELEM(*image_points, CvPoint2D32f, i, 0) = imgPts[i];
CV_MAT_ELEM(*object_points, CvPoint3D32f, i, 0) = cvPoint3D32f(objPts[i].x, objPts[i].y, 0);
}
cvFindExtrinsicCameraParams2(object_points,
image_points,
intrinsic_matrix,
distortion_coeffs,
rotation_vector,
translation_vector);
cvRodrigues2(rotation_vector, rotation_matrix);
for(int f = 0; f < 3; f++)
{
for(int c = 0; c < 3; c++)
{
fullMatrix[c * 4 + f] = rotation_matrix->data.fl[f * 3 + c]; //transposed
}
}
fullMatrix[3] = 0.0;
fullMatrix[7] = 0.0;
fullMatrix[11] = 0.0;
fullMatrix[12] = translation_vector->data.fl[0];
fullMatrix[13] = translation_vector->data.fl[1];
fullMatrix[14] = translation_vector->data.fl[2];
fullMatrix[15] = 1.0;
}
}
void cameraCalibration() {
board_w = 5; // Board width in squares
board_h = 8; // Board height
n_boards = 8; // Number of boards
int board_n = board_w * board_h;
CvSize board_sz = cvSize(board_w, board_h);
CameraControl* cc = camera_control_new(0);
//cvNamedWindow("Calibration", 0);
// Allocate Sotrage
CvMat* image_points = cvCreateMat(n_boards * board_n, 2, CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards * board_n, 3, CV_32FC1);
CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);
CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(5, 1, CV_32FC1);
IplImage *image;
CvPoint2D32f corners[board_n];
int i = 0;
int j = 0;
for (i = 0; i < board_n; i++)
corners[i] = cvPoint2D32f(0, 0);
int corner_count;
int successes = 0;
int step = 0;
while (1) {
cvWaitKey(10);
image = camera_control_query_frame(cc);
if (image)
break;
}
IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1);
// Capture Corner views loop until we've got n_boards
// succesful captures (all corners on the board are found)
while (successes < n_boards) {
// Skp every board_dt frames to allow user to move chessboard
// skip a second to allow user to move the chessboard
image = camera_control_query_frame(cc); // Get next image
//if (frame++ % board_dt == 0)
{
// Find chessboard corners:
int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
cvWaitKey(1);
// Get subpixel accuracy on those corners
cvCvtColor(image, gray_image, CV_BGR2GRAY);
cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11, 11), cvSize(-1, -1), cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
// Draw it
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
char text[222];
sprintf(text, "calibration image %d/%d", successes, n_boards);
th_put_text(image, text, cvPoint(20, 20), th_white, 1.0);
cvShowImage("Calibration", image);
// If we got a good board, add it to our data
if (corner_count == board_n ) {
step = successes * board_n;
for (i = step, j = 0; j < board_n; ++i, ++j) {
CV_MAT_ELEM( *image_points, float, i, 0 ) = corners[j].x;
CV_MAT_ELEM( *image_points, float, i, 1 ) = corners[j].y;
CV_MAT_ELEM( *object_points, float, i, 0 ) = j / board_w;
CV_MAT_ELEM( *object_points, float, i, 1 ) = j % board_w;
CV_MAT_ELEM( *object_points, float, i, 2 ) = 0.0f;
}
CV_MAT_ELEM( *point_counts, int, successes, 0 ) = board_n;
successes++;
}
}
}
/* this handles computation of the distortion matrices of the camera */
void calibration(int board_w, int board_h, int n_boards, float square_width,
float square_height, CvSize resolution, int device_id) {
int board_n = board_w * board_h;
CvSize board_sz = cvSize( board_w, board_h );
VideoCapture capture(device_id,resolution.width,resolution.height);
IplImage *image = capture.CreateCaptureImage();
cvNamedWindow( "Calibration" );
//ALLOCATE STORAGE
CvMat* image_points = cvCreateMat(n_boards*board_n,2,CV_32FC1);
CvMat* object_points = cvCreateMat(n_boards*board_n,3,CV_32FC1);
CvMat* point_counts = cvCreateMat(n_boards,1,CV_32SC1);
CvMat* intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);
CvMat* distortion_coeffs = cvCreateMat(5,1,CV_32FC1);
CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
int corner_count;
int successes = 0;
int step, frame = 0;
IplImage *gray_image = cvCreateImage(cvGetSize(image),8,1);//subpixel
// CAPTURE CORNER VIEWS LOOP UNTIL WE'VE GOT n_boards
// SUCCESSFUL CAPTURES (ALL CORNERS ON THE BOARD ARE FOUND)
//
cvNamedWindow("Live View");
while(successes < n_boards) {
char c=0;
while(c!='c'){
capture.waitFrame(image);
cvShowImage("Live View",image);
c=cvWaitKey(1);
}
//==============
//Find chessboard corners:
int found = cvFindChessboardCorners(
image, board_sz, corners, &corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
);
//Get Subpixel accuracy on those corners
cvCvtColor(image, gray_image, CV_BGR2GRAY);
cvFindCornerSubPix(gray_image, corners, corner_count,
cvSize(11,11),cvSize(-1,-1), cvTermCriteria(
CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
//Draw it
cvDrawChessboardCorners(image, board_sz, corners,
corner_count, found);
cvShowImage( "Calibration", image );
// If we got a good board, add it to our data
if( corner_count == board_n ) {
step = successes*board_n;
for( int i=step, j=0; j<board_n; ++i,++j ) {
CV_MAT_ELEM(*image_points, float,i,0) = corners[j].x;
CV_MAT_ELEM(*image_points, float,i,1) = corners[j].y;
CV_MAT_ELEM(*object_points,float,i,0) = square_height*(j/board_w);
CV_MAT_ELEM(*object_points,float,i,1) = square_width*(j%board_w);
CV_MAT_ELEM(*object_points,float,i,2) = 0.0f;
}
CV_MAT_ELEM(*point_counts, int,successes,0) = board_n;
successes++;
} else {
