static void keyEvent( unsigned char key, int x, int y)
{
int i;
if( key == 0x1b || key == 'q' || key == 'Q' ) {
cleanup();
}
if( cornerFlag && key==' ' ) {
cvFindCornerSubPix( calibImageL, cornersL, chessboardCornerNumX*chessboardCornerNumY, cvSize(5,5),
cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1) );
for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {
arParamObserv2Ideal(paramL.dist_factor, (double)cornersL[i].x, (double)cornersL[i].y,
&calibData[capturedImageNum].screenCoordL[i].x, &calibData[capturedImageNum].screenCoordL[i].y, paramL.dist_function_version);
}
cvFindCornerSubPix( calibImageR, cornersR, chessboardCornerNumX*chessboardCornerNumY, cvSize(5,5),
cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1) );
for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {
arParamObserv2Ideal(paramR.dist_factor, (double)cornersR[i].x, (double)cornersR[i].y,
&calibData[capturedImageNum].screenCoordR[i].x, &calibData[capturedImageNum].screenCoordR[i].y, paramR.dist_function_version);
}
ARLOG("---------- %2d/%2d -----------\n", capturedImageNum+1, calibImageNum);
for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {
ARLOG(" %f, %f ---- %f, %f\n", calibData[capturedImageNum].screenCoordL[i].x, calibData[capturedImageNum].screenCoordL[i].y,
calibData[capturedImageNum].screenCoordR[i].x, calibData[capturedImageNum].screenCoordR[i].y);
}
ARLOG("---------- %2d/%2d -----------\n", capturedImageNum+1, calibImageNum);
capturedImageNum++;
if( capturedImageNum == calibImageNum ) {
calib();
cleanup();
}
}
}
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>();
}
}
static void keyEvent( unsigned char key, int x, int y)
{
CvPoint2D32f *p1, *p2;
int i;
if( key == 0x1b || key == 'q' || key == 'Q' ) {
cleanup();
}
if( cornerFlag && key==' ' ) {
cvFindCornerSubPix( calibImage, corners, chessboardCornerNumX*chessboardCornerNumY, cvSize(5,5),
cvSize(-1,-1), cvTermCriteria (CV_TERMCRIT_ITER, 100, 0.1) );
p1 = &corners[0];
p2 = &cornerSet[capturedImageNum*chessboardCornerNumX*chessboardCornerNumY];
for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {
*(p2++) = *(p1++);
}
capturedImageNum++;
ARLOG("---------- %2d/%2d -----------\n", capturedImageNum, calibImageNum);
for( i = 0; i < chessboardCornerNumX*chessboardCornerNumY; i++ ) {
ARLOG(" %f, %f\n", corners[i].x, corners[i].y);
}
ARLOG("---------- %2d/%2d -----------\n", capturedImageNum, calibImageNum);
if( capturedImageNum == calibImageNum ) {
calib();
cleanup();
}
}
}
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);
}
public:bool analizarFlujo(IplImage *img, IplImage *imgAnterior, CvRect *rect) {
cvSetImageROI(img, *rect);
IplImage *imgA1 = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
cvCopy(img, imgA1);
cvSetImageROI(imgAnterior, *rect);
IplImage *imgB1 = cvCreateImage(cvGetSize(imgAnterior), imgAnterior->depth, imgAnterior->nChannels);
cvCopy(imgAnterior, imgB1);
cvResetImageROI(img);
cvResetImageROI(imgAnterior);
cvNamedWindow( "img", 1);
cvNamedWindow( "imgA", 1);
cvShowImage( "img", imgA1);
cvShowImage( "imgA", imgB1);
int py = imgA1->height;
int px = imgA1->width;
IplImage *imgA=cvCreateImage( cvSize(px,py),IPL_DEPTH_8U, 1);
IplImage *imgB=cvCreateImage( cvSize(px,py),IPL_DEPTH_8U, 1);
cvCvtColor( imgA1, imgA, CV_BGR2GRAY ); //
cvCvtColor( imgB1, imgB, CV_BGR2GRAY ); //
CvSize img_sz = cvGetSize( imgA );
/////////////////////////////
IplImage *eig_image = cvCreateImage( img_sz, IPL_DEPTH_32F, 1 );
IplImage *tmp_image = cvCreateImage( img_sz, IPL_DEPTH_32F, 1 );
int corner_count = MAX_CORNERS;
CvSize pyr_sz;
int win_size = 5;
cvGoodFeaturesToTrack(imgA,eig_image,tmp_image,cornersA,&corner_count,0.01,5.0,0,3,0,0.04);
cvFindCornerSubPix(imgA,cornersA,corner_count,cvSize(win_size,win_size),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,1));
// Call the Lucas Kanade algorithm
pyr_sz = cvSize( imgA->width+8, imgB->height/3 );
pyrA = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
pyrB = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
cvCalcOpticalFlowPyrLK(imgA,imgB,pyrA,pyrB,cornersA,cornersB,corner_count,cvSize( win_size,win_size ),5,features_found,feature_errors,cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .03 ),0);
// Now make some image of what we are looking at:
float dx=0.0;
for (int i=0; i<corner_count; i++) {
if( features_found[i]==0|| feature_errors[i]>100 ) {continue;}
dx=sqrt((cornersA[i].x-cornersB[i].x)*(cornersA[i].x-cornersB[i].x)+(cornersA[i].y-cornersB[i].y)*(cornersA[i].y-cornersB[i].y));
if(dx>1 && dx<50){
return true;
} else {
return false;
}
}
return false;
}
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++;
}
}
void cv::cornerSubPix( const Mat& image, vector<Point2f>& corners,
Size winSize, Size zeroZone,
TermCriteria criteria )
{
CvMat _image = image;
cvFindCornerSubPix(&_image, (CvPoint2D32f*)&corners[0], (int)corners.size(),
winSize, zeroZone, criteria );
}
//
// コーナーを検出する
//
// 引数:
// 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;
}
int opticalflow( char * im1fname, char * im2fname, CvPoint2D32f * &source_points, CvPoint2D32f * &dest_points, char * &status )
{
int count = MAX_COUNT;
double quality = 0.15;
// double min_distance = 2;
double min_distance = 3;
int block_size = 7;
int use_harris = 0;
int win_size = 10;
int flags = 0;
source_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
dest_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_GRAYSCALE);
IplImage * eigenvalues = cvCreateImage(cvGetSize(image1), 32, 1);
IplImage * temp = cvCreateImage(cvGetSize(image1), 32, 1);
cvGoodFeaturesToTrack( image1, eigenvalues, temp, source_points, &count,
quality, min_distance, 0, block_size, use_harris, 0.04 );
printf("%d features\n",count);
setbuf(stdout, NULL);
printf("Finding corner subpix...");
cvFindCornerSubPix( image1, source_points, count,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
printf("done.\n");
cvReleaseImage(&eigenvalues);
cvReleaseImage(&temp);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_GRAYSCALE);
status = (char*)cvAlloc(sizeof(char)*MAX_COUNT);
IplImage * pyramid = cvCreateImage( cvGetSize(image1), IPL_DEPTH_8U, 1 );
IplImage * second_pyramid = cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 1 );
printf("Computing optical flow...");
cvCalcOpticalFlowPyrLK(image1, image2, pyramid, second_pyramid, source_points,
dest_points, count, cvSize(win_size,win_size), 4, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03),
flags);
printf("done.\n");
cvReleaseImage( &image1 );
cvReleaseImage( &image2 );
cvReleaseImage( &pyramid );
cvReleaseImage( &second_pyramid );
return count;
}
void improve_precision(IplImage *image, CvPoint2D32f * cornersArray, int cornersCount)
{
// subPix tool requires a gray scale:
IplImage * gray_image = cvCreateImage(cvSize(image->width,image->height), 8, 1);
cvCvtColor(image, gray_image, CV_BGR2GRAY);
// we use 2 different terminal criteria: nomber of iterations and/or precision (first to be reached)
cvFindCornerSubPix(gray_image, cornersArray, cornersCount,cvSize(11,11),cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS +CV_TERMCRIT_ITER, 30, 0.1 ));
}
void OpticalFlow::PrepareTracking(IplImage* rgbImage,
IplImage* currGrayImage, int curr_indx,
ProbDistrProvider* pProbProv,
const CuScanMatch& match,
ConstMaskIt mask,
int target_num_features,
int winsize_width,
int winsize_height,
double min_distance,
double max_feature_error)
{
m_pProbDistrProvider = pProbProv;
m_target_num_features = target_num_features;
m_num_features_tracked = 0;
m_prev_buf_meaningful = false;
m_winsize_width = winsize_width;
m_winsize_height = winsize_height;
m_min_distance = min_distance;
m_max_feature_error = max_feature_error;
// first find a big set of features that sits on corners
int num_corners = m_target_num_features*3;
CPointVector corners;
corners.resize(num_corners);
CRect bbox(match);
FindGoodFeatures(currGrayImage, bbox, corners);
// then play with the color probability distribution to pick
// the ones that are on skin color, or if those aren't enough,
// pick some additional ones on skin colored pixels
m_features[0].resize(m_target_num_features);
m_features[1].resize(m_target_num_features);
m_feature_status.resize(m_target_num_features);
m_errors.resize(m_target_num_features);
PickSkinColoredFeatures(rgbImage, corners, m_features[curr_indx], match, mask);
// fine-tune feature locations
cvFindCornerSubPix(currGrayImage,
(CvPoint2D32f*) &m_features[curr_indx][0],
m_target_num_features,
cvSize(5,5), cvSize(-1,-1),
cvTermCriteria( CV_TERMCRIT_ITER, 10, 0.1f ));
// set status right for these features
for (int i=0; i<m_target_num_features; i++) {
m_feature_status[i] = 1;
}
GetAverage(m_features[curr_indx], m_mean_feature_pos);
m_condens_is_tracking = false;
m_condens_init_rect = CRect(match);
m_prepared = true;
}
//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 cv::cornerSubPix( InputArray _image, InputOutputArray _corners,
Size winSize, Size zeroZone,
TermCriteria criteria )
{
Mat corners = _corners.getMat();
int ncorners = corners.checkVector(2);
CV_Assert( ncorners >= 0 && corners.depth() == CV_32F );
Mat image = _image.getMat();
CvMat c_image = image;
cvFindCornerSubPix( &c_image, (CvPoint2D32f*)corners.data, ncorners,
winSize, zeroZone, criteria );
}
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 NeoBrain::setTarget(const Point2D<int> loc, const Image<byte>& grey,
const int saliencyval, bool changeState)
{
if (!itsAllowTracking.getVal())
return;
#ifdef HAVE_OPENCV
count = MAX_COUNT;
IplImage* tmp = img2ipl(grey);
if (count > 1)
{
IplImage* eig = cvCreateImage(cvGetSize(tmp), 32, 1);
IplImage* temp = cvCreateImage(cvGetSize(tmp), 32, 1);
double quality = 0.01;
double min_distance = 5;
cvGoodFeaturesToTrack(tmp, eig, temp, points[1], &count,
quality, min_distance, 0, 3, 0, 0.04);
cvReleaseImage(&eig);
cvReleaseImage(&temp);
} else {
//get from the saliency map
points[1][0].x = loc.i;
points[1][0].y = loc.j;
}
cvFindCornerSubPix(tmp, points[1], count,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,
20,0.03));
cvReleaseImageHeader(&tmp);
IplImage *swap_temp;
prev_grey = grey;
CV_SWAP( prev_pyramid, pyramid, swap_temp );
CV_SWAP( points[0], points[1], swap_points );
//LINFO("Init %i point (%f,%f)\n", count, points[1][0].x, points[1][0].y);
if (changeState)
itsState = CHECK_TARGET;
itsTracking = true;
#endif
if (saliencyval >= 0)
this->saySaliencyVal(byte(saliencyval));
}
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);
}
}
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 = cv::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;
}
bool CalibrationFilter::findTarget()
{
int numExpectedPoints = chessBoardCalibObject.numPoints;
CvPoint2D32f* points = new CvPoint2D32f[numExpectedPoints];
int numFoundPoints;
#ifdef USE_CHESSBOARD
CvSize boardSize = cvSize(CHESSBOARD_CORNERS_X, CHESSBOARD_CORNERS_Y);
int found_target = cvFindChessBoardCorners(input.getCvImage(),
boardSize,
points,
&numFoundCorners
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
#endif
if (found_target)
{
cvFindCornerSubPix(grayInput.getCvImage(),
points,
numFoundPoints,
cvSize(11,11), cvSize(-1,-1),
cvTermCriteria(
CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,
30, 0.1));
#ifdef USE_CHESSBOARD_ONLY
cvDrawChessboardCorners(input, boardSize, points, numFoundPoints, found_target);
#endif
CalibrationCapture* capture = new CalibrationCapture(chessBoardPoints);
capture.image = input;
capture.setPoints(points, numFoundPoints);
captures.push_back(capture);
}
delete points;
return found_target;
}
void FkPaperKeyboard_TypeA::detectKeyButtonCorner(IplImage* srcImage){
//500,250
CvSize transSize = cvSize(640,480);
IplImage* eigImage = cvCreateImage(transSize, IPL_DEPTH_8U,1);
IplImage* tempImage = cvCreateImage(transSize, IPL_DEPTH_8U, 1);
IplImage* grayImage = cvCreateImage(transSize, IPL_DEPTH_8U, 1);
IplImage* dstImage = cvCreateImage(transSize, IPL_DEPTH_8U, 1);
int keyButtonCornerCount = 316;
cvCopy(srcImage, grayImage);
//cvShowImage("ssssssssssssssss",srcImage);
//cvShowImage("gggggggggggggggg",grayImage);
cvGoodFeaturesToTrack(grayImage, eigImage, tempImage, keyButtonCorner, &keyButtonCornerCount, 0.03, 7, NULL, 10, 0);
cvFindCornerSubPix (grayImage, keyButtonCorner, keyButtonCornerCount,cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
initKeyButtonCorner();
cvReleaseImage(&eigImage);
cvReleaseImage(&tempImage);
cvReleaseImage(&grayImage);
cvReleaseImage(&dstImage);
}
void ofxOpticalFlow::calc(ofxCvGrayscaleImage& prevFrame, ofxCvGrayscaleImage& currentFrame){
int cornerCount = MAX_CORNERS;
cvGoodFeaturesToTrack(prevFrame.getCvImage(), eigImg, tempImg, cornersPrev, &cornerCount, 0.01, 5.0, 0, 3, 0, 0.04);
cvFindCornerSubPix(prevFrame.getCvImage(), cornersPrev, cornerCount,
windowSize, cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
cvCalcOpticalFlowPyrLK(prevFrame.getCvImage(), currentFrame.getCvImage(), pyrPrev,
pyrCurr, cornersPrev, cornersCurr, cornerCount, windowSize,
5, featuresFound, NULL,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03), 0);
flowPoints.clear();
for(int i=0; i<cornerCount; i++){
if(featuresFound[i] == 0){
continue;
}
flowPoints.push_back(ofxCvFlowPoint(cornersPrev[i].x, cornersPrev[i].y, cornersCurr[i].x, cornersCurr[i].y));
}
}
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;
}
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++;
}
}
}
bool CvCalibFilter::FindEtalon( CvMat** mats )
{
bool result = true;
if( !mats || etalonPointCount == 0 )
{
assert(0);
result = false;
}
if( result )
{
int i, tempPointCount0 = etalonPointCount*2;
for( i = 0; i < cameraCount; i++ )
{
if( !latestPoints[i] )
latestPoints[i] = (CvPoint2D32f*)
cvAlloc( tempPointCount0*2*sizeof(latestPoints[0]));
}
for( i = 0; i < cameraCount; i++ )
{
CvSize size;
int tempPointCount = tempPointCount0;
bool found = false;
if( !CV_IS_MAT(mats[i]) && !CV_IS_IMAGE(mats[i]))
{
assert(0);
break;
}
size = cvGetSize(mats[i]);
if( size.width != imgSize.width || size.height != imgSize.height )
{
imgSize = size;
}
if( !grayImg || grayImg->width != imgSize.width ||
grayImg->height != imgSize.height )
{
cvReleaseMat( &grayImg );
cvReleaseMat( &tempImg );
grayImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
tempImg = cvCreateMat( imgSize.height, imgSize.width, CV_8UC1 );
}
if( !storage )
storage = cvCreateMemStorage();
switch( etalonType )
{
case CV_CALIB_ETALON_CHESSBOARD:
if( CV_MAT_CN(cvGetElemType(mats[i])) == 1 )
cvCopy( mats[i], grayImg );
else
cvCvtColor( mats[i], grayImg, CV_BGR2GRAY );
found = cvFindChessBoardCornerGuesses( grayImg, tempImg, storage,
cvSize( cvRound(etalonParams[0]),
cvRound(etalonParams[1])),
latestPoints[i], &tempPointCount ) != 0;
if( found )
cvFindCornerSubPix( grayImg, latestPoints[i], tempPointCount,
cvSize(5,5), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,10,0.1));
break;
default:
assert(0);
result = false;
break;
}
latestCounts[i] = found ? tempPointCount : -tempPointCount;
result = result && found;
}
}
if( storage )
cvClearMemStorage( storage );
return result;
}
/*!
Initialise the tracking by extracting KLT keypoints on the provided image.
\param I : Grey level image used as input. This image should have only 1 channel.
\param mask : Image mask used to restrict the keypoint detection area.
If mask is NULL, all the image will be considered.
\exception vpTrackingException::initializationError : If the image I is not
initialized, or if the image or the mask have bad coding format.
*/
void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
{
if (!I) {
throw(vpException(vpTrackingException::initializationError, "Image Not initialized")) ;
}
if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
}
if (mask) {
if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
}
}
//Creation des buffers
CvSize Sizeim, SizeI;
SizeI = cvGetSize(I);
bool b_imOK = true;
if(image != NULL){
Sizeim = cvGetSize(image);
if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
}
if(image == NULL || prev_image == NULL || pyramid==NULL || prev_pyramid ==NULL || !b_imOK){
reset();
image = cvCreateImage(cvGetSize(I), 8, 1);image->origin = I->origin;
prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
}else{
swap_temp = 0;
countFeatures = 0;
countPrevFeatures = 0;
flags = 0;
initialized = 0;
globalcountFeatures = 0;
}
initialized = 1;
//Import
cvCopy(I, image, 0);
//Recherche de points d'interets
countFeatures = maxFeatures;
countPrevFeatures = 0;
IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
cvGoodFeaturesToTrack(image, eig, temp, features,
&countFeatures, quality, min_distance,
mask, block_size, use_harris, harris_free_parameter);
cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
CV_TERMCRIT_EPS,20,0.03));
cvReleaseImage(&eig);
cvReleaseImage(&temp);
if (OnInitialize)
OnInitialize(_tid);
//printf("Number of features at init: %d\n", countFeatures);
for (int boucle=0; boucle<countFeatures;boucle++) {
featuresid[boucle] = globalcountFeatures;
globalcountFeatures++;
if (OnNewFeature){
OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
features[boucle].y);
}
}
}
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 main( int argc, char* argv[] ) {
//IplImage* img = cvCreateImage(imSize,IPL_DEPTH_8U,3);
IplImage* img = cvLoadImage(imcd0,CV_LOAD_IMAGE_UNCHANGED);
IplImage* imgA = cvLoadImage(imcd0,CV_LOAD_IMAGE_GRAYSCALE);
IplImage* imgB = cvLoadImage(imcd1,CV_LOAD_IMAGE_GRAYSCALE);
imSize = cvSize(img->width,img->height);
rmax=0.8*((imSize.width>imSize.height)?imSize.height/2:imSize.width/2);
rmin=0.2*((imSize.width>imSize.height)?imSize.height/2:imSize.width/2);
lx=0.5*imSize.width;
ly=0.5*imSize.height;
int win_siz = 7;
int arr_siz = NUMX*NUMY;
CvPoint2D32f p0 = cvPoint2D32f(imSize.width/2,imSize.height/2);
IplImage* pyr = cvCreateImage(imSize,8,1);
IplImage* pyr_old = cvCreateImage(imSize,8,1);
char* status =0;
status = (char*)cvAlloc(arr_siz);
cvNamedWindow("testWindow");
cvNamedWindow("ImgA");
cvShowImage("ImgA", imgA);
cvNamedWindow("ImgB");
cvShowImage("ImgB", imgB);
CvPoint2D32f* arrg = new CvPoint2D32f[arr_siz];
CvPoint2D32f* arrg_old = new CvPoint2D32f[arr_siz];
int counter=0;
for(int x=0; x<NUMX; x++) {
for(int y=0; y<NUMY; y++) {
arrg_old[counter].x = p0.x + (-lx/2) + lx*x/NUMX;
arrg_old[counter].y = p0.y + (-ly/2) + lx*y/NUMY;
counter++;
}
}
cout << "f**k-0" << endl;
for(int i=0; i<arr_siz; i++) {
cvLine(img,cvPointFrom32f(arrg_old[i]),cvPointFrom32f(arrg_old[i]),CV_RGB(0,0,0),4);
}
cvShowImage("testWindow",img);
cvWaitKey(100);
cout << "f**k-1" << endl;
cvFindCornerSubPix(imgA,
arrg_old,
arr_siz,
cvSize(win_siz,win_siz),
cvSize(2,2),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
//cvReleaseImage(&img);
//img = cvLoadImage(imcd0,CV_LOAD_IMAGE_UNCHANGED);
cout << "f**k-2" << endl;
for(int i=0; i<arr_siz; i++) {
cvLine(img,cvPointFrom32f(arrg_old[i]),cvPointFrom32f(arrg_old[i]),CV_RGB(255,0,255),4);
}
cvShowImage("testWindow",img);
cvWaitKey(100);
cout << "f**k-3" << endl;
float errors[arr_siz];
cvCalcOpticalFlowPyrLK(imgA,imgB,
pyr_old, pyr,
arrg_old,
arrg,
arr_siz,
cvSize(win_siz,win_siz),
5,
status,
errors,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.3),
0);
CvPoint2D32f dp, dp2;
CvPoint2D32f center = cvPoint2D32f(0., 0.);
bool arr_draw[arr_siz];
int count = 0;
for(int i=0; i<arr_siz; i++) {
cvLine(img,cvPointFrom32f(arrg[i]),cvPointFrom32f(arrg[i]),CV_RGB(0,255,0),4);
CvScalar color = CV_RGB(255,0,0);
dp = getDp(arrg[i],arrg_old[i]);
double len = getLength(dp);
// if(errors[i]<50) {
if(getLength(dp)>3) {
color = CV_RGB(255,0,0);
} else {
color = CV_RGB(100,255,100);
}
int nc = i+1;
arr_draw[i] = false;
if((nc>-1) && (nc<arr_siz) && len>3) {
dp2=getDp(arrg[nc],arrg_old[nc]);
if(getLength(dp2)>2) {
CvPoint2D32f ctmp = getCrossPoint(arrg_old[i],getOrtoVec(dp), arrg_old[nc],getOrtoVec(dp2));
// cvLine(img,cvPointFrom32f(arrg_old[i]),cvPointFrom32f(ctmp),CV_RGB(0,0,0),1);
// cvLine(img,cvPointFrom32f(arrg[i]),cvPointFrom32f(ctmp),CV_RGB(0,0,0),1);
center = getSum(center,ctmp);
count++;
arr_draw[i] = true;
}
}
drawArrow(img,arrg_old[i],arrg[i],color,2,15.);
cout << "status=[" << (int)status[i] << "], error=[" << errors[i] << "]" << endl;
// cout << "[" << arrg[i].x << "," << arrg[i].y << "]" << endl;
}
center=getDiv(center,count);
cvCircle(img,cvPointFrom32f(center),10,CV_RGB(0,200,0),1);
double df = 0;
for(int i=0; i<arr_siz; i++) {
if(arr_draw[i]) {
cvLine(img, cvPointFrom32f(center), cvPointFrom32f(arrg_old[i]),CV_RGB(0,0,0),1);
cvLine(img, cvPointFrom32f(center), cvPointFrom32f(arrg[i]),CV_RGB(0,0,0),1);
df += 180.0*(getLength(getDel(arrg[i],arrg_old[i])))
/(CV_PI*getLength(getDel(arrg_old[i],center)));
}
}
CvFont font, fontbg;
cvInitFont(&font,CV_FONT_HERSHEY_PLAIN, 2, 2, 0.0, 2, CV_AA);
cvInitFont(&fontbg,CV_FONT_HERSHEY_PLAIN, 2, 2, 0.0, 8, CV_AA);
char buff[100];
bzero(buff,sizeof(buff));
sprintf(buff,"angle=%0.1f degres",(df/count));
cvPutText(img,buff,cvPoint(10,25),&fontbg,CV_RGB(0,0,0));
cvPutText(img,buff,cvPoint(10,25),&font,CV_RGB(255,0,0));
/*
for(int r=0; r<NUMR; r++) {
for(int f=0; f<NUMF; f++) {
double pfi = 2*CV_PI*f/NUMF;
double ro = rmin + (rmax-rmin)*r/NUMR;
p1.x = p0.x + ro*cos(pfi);
p1.y = p0.y + ro*sin(pfi);
//cvLine(img,cvPointFrom32f(p1),cvPointFrom32f(p1),CV_RGB(0,0,255),2);
drawArrow(img,p0,p1,CV_RGB(255,0,0));
}
}
*/
cvShowImage("testWindow",img);
cvWaitKey(0);
cvDestroyWindow("testWindow");
cvReleaseImage(&img);
cout << "Shutdown" << endl;
return 0;
}
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 FeatureTracker::track_features(geometry_msgs::PoseStamped mapPose){
//set the initial number of features to the max number we want to find
int feature_count=num_features;
printf("pose %f %f %f\n",mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation));
int edge_pixels=5;
//check if there were features from the last image to keep tracking
if(last_feature_count>0){
//if there were call cvCalcOpticalFlowPyrLK();
//find matches between last good features and current image features
// store matches in featuresB
cvCalcOpticalFlowPyrLK(last_image,image_rect,pyrA,pyrB,features,featuresB, last_feature_count,cvSize(win_size,win_size) ,4,last_features_status,track_error, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,.3),0);
}
printf("got image flow\n");
// assign last_feature_id values for matched features and set the non matched spots to -1
//find new features and subpixel them
//I SHOULD ADD THE IMAGE FLOW VALUES AS FEATURES NOW BEFORE FINDING NEW FEATURES
//find all good features
cvGoodFeaturesToTrack(image_rect, eigImage, tempImage, features, &feature_count, quality_level, min_distance, NULL, block_size);
//subpixel good features
cvFindCornerSubPix(image_rect,features,feature_count,cvSize(win_size,win_size),cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
printf("subpixeled image\n");
//for all the features in features B, find their matches in the newly found features
//add all the matches to their correct featuremanager, for the non matching, make a new
//feature manager and add them to it
//for all features by now we need their ray and the robot pose at that location
//draw dots on image where features are
//set the feature ids to a control value
for(int i=0;i<num_features;i++){
current_feature_id[i]=-1;
}
for(int i=0;i<last_feature_count;i++){
//for the previously found features in list b
if(last_features_status[i]>0){
for(int j=0;j<feature_count;j++){
//for every feature found in this image
//determine if the two overlap in a meaningful way
int xdiff=featuresB[i].x-features[j].x;
int ydiff=featuresB[i].y-features[j].y;
//if the pixels are within some margin of eachother
if(sqrt(xdiff*xdiff + ydiff*ydiff)<pixel_tracking_margin){
//if they do set the current id for j to the id of i
current_feature_id[j]=last_feature_id[i];
printf("feature found %d %d",last_feature_id[i],i);
}
}
}
}
printf("assigned IDs image\n");
for(int i=0;i<feature_count;i++){
printf("looping\n");
if(current_feature_id[i]>=0){
printf("prev feature match\n");
//if we matched a previous feature
//add our new feature to the previous features list
cv::Point3d tempRay;
cv::Point2d tempPoint=cv::Point2d(features[i]);
cam_model.projectPixelTo3dRay(tempPoint,tempRay);
if(tempPoint.x> edge_pixels && tempPoint.x < last_image->width- edge_pixels &&
tempPoint.y> edge_pixels && tempPoint.y<last_image->height- edge_pixels){
featureList[current_feature_id[i]].add(RawFeature(mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation), tempPoint,tempRay));
}else{
current_feature_id[i]=-1;
}
}else{
printf("new feature \n");
cv::Point3d tempRay;
cv::Point2d tempPoint=cv::Point2d(features[i]);
cam_model.projectPixelTo3dRay(tempPoint,tempRay);
if(tempPoint.x> edge_pixels && tempPoint.x < last_image->width- edge_pixels &&
tempPoint.y> edge_pixels && tempPoint.y<last_image->height- edge_pixels){
printf("new good feature \n");
//if we didn't
//create a new feature group in the list
current_feature_id[i]=feature_number;
//add the new feature to the feature list
featureList.push_back(FeatureManager());
featureList[feature_number].add(RawFeature(mapPose.pose.position.x, mapPose.pose.position.y, tf::getYaw(mapPose.pose.orientation), tempPoint,tempRay));
++feature_number;
}
}
}
// printf("features: ");
for(int i=0;i<num_features;i++){
if(i<feature_count){
last_feature_id[i]=current_feature_id[i];
}
else{
last_feature_id[i]=-1;
}
// printf(" %d ",current_feature_id[i]);
}
printf("\n");
last_feature_count=feature_count;
}
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);
}
