//--------------------------------------------------------------------------------
void ofxCvImage::warpPerspective( const ofPoint& A, const ofPoint& B, const ofPoint& C, const ofPoint& D ) {
if( !bAllocated ){
ofLogError("ofxCvImage") << "warpPerspective(): image not allocated";
return;
}
// compute matrix for perspectival warping (homography)
CvPoint2D32f cvsrc[4];
CvPoint2D32f cvdst[4];
CvMat* translate = cvCreateMat( 3,3, CV_32FC1 );
cvSetZero( translate );
cvdst[0].x = 0;
cvdst[0].y = 0;
cvdst[1].x = width;
cvdst[1].y = 0;
cvdst[2].x = width;
cvdst[2].y = height;
cvdst[3].x = 0;
cvdst[3].y = height;
cvsrc[0].x = A.x;
cvsrc[0].y = A.y;
cvsrc[1].x = B.x;
cvsrc[1].y = B.y;
cvsrc[2].x = C.x;
cvsrc[2].y = C.y;
cvsrc[3].x = D.x;
cvsrc[3].y = D.y;
cvGetPerspectiveTransform( cvsrc, cvdst, translate ); // calculate homography
cvWarpPerspective( cvImage, cvImageTemp, translate );
swapTemp();
flagImageChanged();
cvReleaseMat( &translate );
}
//--------------------------------------------------------------------------------
void ofxCvImage::warpIntoMe( ofxCvImage& mom, const ofPoint src[4], const ofPoint dst[4] ){
if( !bAllocated ){
ofLogError("ofxCvImage") << "warpIntoMe(): image not allocated";
return;
}
if( !mom.bAllocated ){
ofLogError("ofxCvImage") << "warpIntoMe(): source image not allocated";
return;
}
if( mom.getCvImage()->nChannels == cvImage->nChannels &&
mom.getCvImage()->depth == cvImage->depth ) {
// compute matrix for perspectival warping (homography)
CvPoint2D32f cvsrc[4];
CvPoint2D32f cvdst[4];
CvMat* translate = cvCreateMat( 3, 3, CV_32FC1 );
cvSetZero( translate );
for (int i = 0; i < 4; i++ ) {
cvsrc[i].x = src[i].x;
cvsrc[i].y = src[i].y;
cvdst[i].x = dst[i].x;
cvdst[i].y = dst[i].y;
}
cvGetPerspectiveTransform( cvsrc, cvdst, translate ); // calculate homography
cvWarpPerspective( mom.getCvImage(), cvImage, translate);
flagImageChanged();
cvReleaseMat( &translate );
} else {
ofLogError("ofxCvImage") << "warpIntoMe(): image type mismatch";
}
}
//Input image, Edges, outputNameFile
int homography_transformation(CvMat* src, char* out_filename, CvPoint2D32f* srcQuad){
if(src == NULL || out_filename == NULL || srcQuad == NULL)
return -1;
CvPoint2D32f dstQuad[4]; //Destination vertices
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1); //transformation matrix
CvMat* dst = cvCloneMat(src); //clone image
int p[3]={CV_IMWRITE_JPEG_QUALITY,JPEG_QUALITY,0}; //FORMAT, QUALITY
dstQuad[0].x = 0; //dst Top left
dstQuad[0].y = 0;
dstQuad[1].x = src->cols; //dst Top right
dstQuad[1].y = 0;
dstQuad[2].x = 0; //dst Bottom left
dstQuad[2].y = src->rows;
dstQuad[3].x = src->cols; //dst Bot right
dstQuad[3].y = src->rows;
// get transformation matrix that you can use to calculate
cvGetPerspectiveTransform(srcQuad,dstQuad, warp_matrix);
// perspective transformation. Parameters: source, destination, warp_matrix,
//type of interpolation: (CV_INTER_LINEAR, CV_INTER_AREA, CV_INTER_CUBIC, CV_INTER_LANCZOS4)
///Set all scalar with the same value. 0 means the black color of border
cvWarpPerspective(src, dst, warp_matrix, CV_INTER_LINEAR, cvScalarAll(0)); // 1 = CV_INTER_LINEAR
cvSaveImage(out_filename,dst, p);
//close and destroy all stuff
cvReleaseMat(&warp_matrix);
cvReleaseMat(&dst);
return 0;
}
int main(int argc, char** argv)
{
IplImage* image = cvLoadImage(argv[1], CV_LOAD_IMAGE_COLOR); // cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
cvNamedWindow("result", CV_WINDOW_AUTOSIZE);
/*
gsl_qrng* q = gsl_qrng_alloc(gsl_qrng_halton, 2);
for (int i = 0; i < 64; i++)
{
double v[2];
gsl_qrng_get(q, v);
int x = (int)(v[0] * 319), y = (int)(v[1] * 239);
image->imageData[x * 3 + y * image->widthStep] = image->imageData[x * 3 + 1 + y * image->widthStep] = image->imageData[x * 3 + 2 + y * image->widthStep] = 255;
}
gsl_qrng_free(q);
*/
gsl_rng_env_setup();
const gsl_rng_type* T = gsl_rng_default;
gsl_rng* r = gsl_rng_alloc(T);
gsl_rng_set(r, time(NULL));
double sigma = 0.1 * ((image->width < image->height) ? image->width : image->height);
CvPoint2D32f src[4], dst[4], rds[4];
src[0].x = 0; src[0].y = 0;
src[1].x = image->width; src[1].y = 0;
src[2].x = image->width; src[2].y = image->height;
src[3].x = 0; src[3].y = image->height;
dst[0].x = -image->width / 2 + gsl_ran_gaussian(r, sigma); dst[0].y = -image->height / 2 + gsl_ran_gaussian(r, sigma);
dst[1].x = image->width / 2 + gsl_ran_gaussian(r, sigma); dst[1].y = -image->height / 2 + gsl_ran_gaussian(r, sigma);
dst[2].x = image->width / 2 + gsl_ran_gaussian(r, sigma); dst[2].y = image->height / 2 + gsl_ran_gaussian(r, sigma);
dst[3].x = -image->width / 2 + gsl_ran_gaussian(r, sigma); dst[3].y = image->height / 2 + gsl_ran_gaussian(r, sigma);
double radius = gsl_ran_gaussian(r, 3.1415926 / 8.0);
double minx = 0, miny = 0, maxx = 0, maxy = 0;
for (int i = 0; i < 4; i++)
{
rds[i].x = dst[i].x * cos(radius) - dst[i].y * sin(radius);
rds[i].y = dst[i].x * sin(radius) + dst[i].y * cos(radius);
if (rds[i].x < minx) minx = rds[i].x;
if (rds[i].y < miny) miny = rds[i].y;
if (rds[i].x > maxx) maxx = rds[i].x;
if (rds[i].y > maxy) maxy = rds[i].y;
}
for (int i = 0; i < 4; i++)
{
rds[i].x -= minx;
rds[i].y -= miny;
}
gsl_rng_free(r);
float _m[9];
CvMat m = cvMat(3, 3, CV_32FC1, _m);
cvGetPerspectiveTransform(src, rds, &m);
IplImage* transformed = cvCreateImage(cvSize(maxx - minx, maxy - miny), IPL_DEPTH_8U, 3);
cvWarpPerspective(image, transformed, &m);
cvShowImage("result", transformed);
cvWaitKey(0);
cvDestroyWindow("result");
return 0;
}
// A function to calculate the transformation matrix used for perspective transformation
void calculateTransformationMatrix( BoundingBox* from, BoundingBox* to, CvMat* transMat ) {
CvPoint2D32f from_arr[] = {
cvPointTo32f( from->topLeft ),
cvPointTo32f( from->topRight ),
cvPointTo32f( from->bottomRight ),
cvPointTo32f( from->bottomLeft )
};
CvPoint2D32f to_arr[] = {
cvPointTo32f( to->topLeft ),
cvPointTo32f( to->topRight ),
cvPointTo32f( to->bottomRight ),
cvPointTo32f( to->bottomLeft )
};
cvGetPerspectiveTransform( from_arr, to_arr, transMat );
}
void COpenCVMFCView::OnWarpPerspect()
{
// TODO: Add your command handler code here
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1);
IplImage *src=0, *dst=0;
src = cvCloneImage(workImg);
cvFlip(src);
dst = cvCloneImage(src);
dst->origin = src->origin;
cvZero(dst);
srcQuad[0].x = 0; //src Top left
srcQuad[0].y = 0;
srcQuad[1].x = (float) src->width - 1; //src Top right
srcQuad[1].y = 0;
srcQuad[2].x = 0; //src Bottom left
srcQuad[2].y = (float) src->height - 1;
srcQuad[3].x = (float) src->width - 1; //src Bot right
srcQuad[3].y = (float) src->height - 1;
//- - - - - - - - - - - - - -//
dstQuad[0].x = (float)(src->width*0.05); //dst Top left
dstQuad[0].y = (float)(src->height*0.33);
dstQuad[1].x = (float)(src->width*0.9); //dst Top right
dstQuad[1].y = (float)(src->height*0.25);
dstQuad[2].x = (float)(src->width*0.2); //dst Bottom left
dstQuad[2].y = (float)(src->height*0.7);
dstQuad[3].x = (float)(src->width*0.8); //dst Bot right
dstQuad[3].y = (float)(src->height*0.9);
cvGetPerspectiveTransform(srcQuad,dstQuad,warp_matrix);
cvWarpPerspective(src,dst,warp_matrix);
cvNamedWindow( "Perspective_Warp", 1 );
cvShowImage( "Perspective_Warp", dst );
m_ImageType=-3;
cvWaitKey();
cvDestroyWindow( "Perspective_Warp" );
cvReleaseImage(&src);
cvReleaseImage(&dst);
cvReleaseMat(&warp_matrix);
m_ImageType=imageType(workImg);
}
void perspectiveTransform() {
processImage = windowImage;
setVariables(); //设置四个预设的点
setPoints(); //选取图片上的点
cvDestroyWindow("monitor");
windowImage = cvCreateImage(cvSize(squareWindowSize,squareWindowSize), IPL_DEPTH_8U, 3);
cvGetPerspectiveTransform(originalPoints, transPoints, transmat); //计算transmat的值
cvWarpPerspective(processImage , windowImage , transmat); //这一句利用transmat进行变换
cvNamedWindow("control");
cvMoveWindow("control", middlewindowX, middlewindowY);
cvShowImage("control", windowImage);
cvWaitKey();
cvDestroyWindow("control");
}
int main(int argc, const char * argv[]) {
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3, 3, CV_32FC1);
IplImage *src, *dst;
if (argc == 2 && ((src = cvLoadImage(argv[1], 1)) != 0)) {
dst = cvCloneImage( src );
dst->origin = src->origin;
cvZero( dst );
// warp matrix
srcQuad[0].x = 0;
srcQuad[0].y = 0;
srcQuad[1].x = src->width - 1;
srcQuad[1].y = 0;
srcQuad[2].x = 0;
srcQuad[2].y = src->height - 1;
srcQuad[3].x = src->width - 1;
srcQuad[3].y = src->height - 1;
dstQuad[0].x = src->width * 0.005;
dstQuad[0].y = src->height * 0.33;
dstQuad[1].x = src->width * 0.9;
dstQuad[1].y = src->height * 0.25;
dstQuad[2].x = src->width * 0.2;
dstQuad[2].y = src->height * 0.7;
dstQuad[3].x = src->width * 0.8;
dstQuad[3].y = src->height * 0.9;
cvGetPerspectiveTransform( srcQuad, dstQuad, warp_matrix );
cvWarpPerspective( src, dst, warp_matrix );
cvNamedWindow( "Perspective_Transform", 1 );
cvShowImage( "Perspective_Transform" , dst );
cvWaitKey();
cvReleaseImage( &dst );
}
cvReleaseMat( &warp_matrix );
return 0;
}
IplImage *square_puzzle(IplImage *in, const CvPoint2D32f *location) {
int xsize = location[1].x - location[0].x;
int ysize = xsize;
CvPoint2D32f warped_coordinates[4];
warped_coordinates[0] = cvPointTo32f(cvPoint(0, 0));
warped_coordinates[1] = cvPointTo32f(cvPoint(xsize-1, 0));
warped_coordinates[2] = cvPointTo32f(cvPoint(xsize-1, ysize-1));
warped_coordinates[3] = cvPointTo32f(cvPoint(0, ysize-1));
CvMat *map_matrix = cvCreateMat(3, 3, CV_64FC1);
cvGetPerspectiveTransform(location, warped_coordinates, map_matrix);
IplImage *warped_image = cvCreateImage(cvSize(xsize, ysize), 8, in->nChannels);
CvScalar fillval=cvScalarAll(0);
cvWarpPerspective(in, warped_image, map_matrix, CV_WARP_FILL_OUTLIERS, fillval);
return warped_image;
}
void opencv_birds_eye_remap(ProbabilisticMapParams map, const IplImage *src_image, IplImage *dst_image, double camera_height, double camera_pitch, stereo_util stereo_util_instance)
{
// coordinates of 4 quadrangle vertices in the source image.
CvPoint2D32f src_pts[4] = { cvPoint2D32f(180, 400), cvPoint2D32f(180, 320), cvPoint2D32f(520, 320), cvPoint2D32f(520, 400) };
// coordinates of the 4 corresponding quadrangle vertices in the destination image.
CvPoint2D32f dst_pts[4];
for (int i = 0; i < 4; i++)
{
carmen_position_t right_point;
right_point.x = src_pts[i].x;
right_point.y = src_pts[i].y;
carmen_vector_3D_t p3D = camera_to_world(right_point, camera_height, camera_pitch, stereo_util_instance);
int x_map = map_grid_x(map, p3D.x);
int y_map = map_grid_y(map, p3D.y);
dst_pts[i] = cvPoint2D32f(x_map, y_map);
}
// FIND THE HOMOGRAPHY
static CvMat *homography_matrix;
if (homography_matrix == NULL)
homography_matrix = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(dst_pts, src_pts, homography_matrix);
float Z = camera_height;
CV_MAT_ELEM(*homography_matrix, float, 2, 2) = Z;
cvWarpPerspective(src_image,
dst_image,
homography_matrix,
CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS,
cvScalarAll(0));
}
IplImage* cutSign(IplImage* origImg, CvPoint* corners, int numcorners, bool drawcircles)
{
// convert corners to CvPoint2D32f.
CvPoint2D32f cornersf[numcorners];
for (int i=0; i<numcorners; ++i)
cornersf[i] = cvPointTo32f(corners[i]);
if (_debug) printf("Corners: %d,%d %d,%d %d,%d %d,%d\n",corners[0].x,corners[0].y,corners[1].x,corners[1].y,corners[2].x,corners[2].y,corners[3].x,corners[3].y);
// Create target-image with right size.
double xDiffBottom = pointDist(corners[0], corners[1]);
double yDiffLeft = pointDist(corners[0], corners[3]);
IplImage* cut = cvCreateImage(cvSize(xDiffBottom,yDiffLeft), IPL_DEPTH_8U, 3);
// target points for perspective correction.
CvPoint2D32f cornerstarget[numcorners];
cornerstarget[0] = cvPoint2D32f(0,0);
cornerstarget[1] = cvPoint2D32f(cut->width-1,0);
cornerstarget[2]= cvPoint2D32f(cut->width-1,cut->height-1);
cornerstarget[3] = cvPoint2D32f(0,cut->height-1);
if (_debug) printf("Corners: %f,%f %f,%f %f,%f %f,%f\n",cornerstarget[0].x,cornerstarget[0].y,cornerstarget[1].x,cornerstarget[1].y,cornerstarget[2].x,cornerstarget[2].y,cornerstarget[3].x,cornerstarget[3].y);
// Apply perspective correction to the image.
CvMat* transmat = cvCreateMat(3, 3, CV_32FC1); // Colums, rows ?
transmat = cvGetPerspectiveTransform(cornersf,cornerstarget,transmat);
cvWarpPerspective(origImg,cut,transmat);
cvReleaseMat(&transmat);
// Draw yellow circles around the corners.
if (drawcircles)
for (int i=0; i<numcorners; ++i)
cvCircle(origImg, corners[i],5,CV_RGB(255,255,0),2);
return cut;
}
int main(int argc, char *argv[]) {
if (argc < 3){
printf("Usage: %s <image-file-name1> <image-file-name2>\n", argv[0]);
exit(1);
}
IplImage* img1 = cvLoadImage(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
if (!img1) {
printf("Could not load image file: %s\n", argv[1]);
exit(1);
}
IplImage* img1f = cvCreateImage(cvGetSize(img1), IPL_DEPTH_32F, 1);
cvConvertScale(img1, img1f, 1.0 / 255.0);
IplImage* img2 = cvLoadImage(argv[2], CV_LOAD_IMAGE_GRAYSCALE);
if (!img2) {
printf("Could not load image file: %s\n", argv[2]);
exit(1);
}
IplImage* img2f = cvCreateImage(cvGetSize(img1), IPL_DEPTH_32F, 1);
cvConvertScale(img2, img2f, 1.0 / 255.0);
/**
* Aufgabe: Homographien (5 Punkte)
*
* Unter der Annahme, dass Bilder mit einer verzerrungsfreien Lochbildkamera
* aufgenommen werden, kann man Aufnahmen mit verschiedenen Bildebenen und
* gleichem Projektionszentren durch projektive Abbildungen, sogenannte
* Homographien, beschreiben.
*
* - Schreibe Translation als Homographie auf (auf Papier!).
* - Verschiebe die Bildebene eines Testbildes um 20 Pixel nach rechts, ohne
* das Projektionszentrum zu ändern. Benutze dafür \code{cvWarpPerspective}.
* - Wieviele Punktkorrespondenzen benötigt man mindestens, um eine projektive
* Abbildung zwischen zwei Bildern bis auf eine Skalierung eindeutig zu
* bestimmen? Warum? (Schriftlich beantworten!)
*/
/* TODO */
IplImage* img_moved = cvCreateImage(cvGetSize(img1), IPL_DEPTH_32F, 1);
cv::Mat matImg1f_task1 = cv::Mat(img1f);
cv::Mat matImgMoved = cv::Mat(img_moved);
float data[] = { 1, 0, -20, 0, 1, 0, 0, 0, 1 };
cv::Mat trans(3, 3, CV_32FC1, data);;
cv::warpPerspective(matImg1f_task1, matImgMoved, trans, matImgMoved.size());
cv::namedWindow("mainWin", CV_WINDOW_AUTOSIZE);
cv::Mat img_moved_final(img_moved);
cv::imshow("mainWin", img_moved_final);
cvWaitKey(0);
/**
* Aufgabe: Panorama (15 Punkte)
*
* Ziel dieser Aufgabe ist es, aus zwei gegebenen Bildern ein Panorama zu konstruieren.
* \begin{center}
* \includegraphics[width = 0.3\linewidth]{left.png}
* \includegraphics[width = 0.3\linewidth]{right.png}
* \end{center}
*
* Dafür muss zunächst aus den gegeben Punktkorrespondenzen
* \begin{center}
* \begin{tabular}{|c|c|}
* \hline
* linkes Bild & rechtes Bild \\
* $(x, y)$ & $(x, y)$ \\ \hline \hline
* (463, 164) & (225, 179)\\ \hline
* (530, 357) & (294, 370)\\ \hline
* (618, 357) &(379, 367)\\ \hline
* (610, 153) & (369, 168)\\ \hline
* \end{tabular}
* \end{center}
* eine perspektivische Transformation bestimmt werden, mit der die Bilder auf eine gemeinsame Bildebene transformiert werden können.
*
* - Berechne die Transformation aus den gegebenen Punktkorrespondenzen.
* Benutze die Funktion \code{cvGetPerspectiveTransform}. Was ist die
* zentrale Idee des DLT-Algorithmus, wie er in der Vorlesung vorgestellt
* wurde?
*/
/* TODO */
CvMat *P = cvCreateMat(3, 3, CV_32FC1);
CvPoint points1[] = { cvPoint(463, 164), cvPoint(530, 357), cvPoint(618, 357), cvPoint(610, 153) };
CvPoint points2[] = { cvPoint(225, 179), cvPoint(294, 370), cvPoint(379, 367), cvPoint(369, 168) };
CvPoint2D32f pt1[4], pt2[4];
for (int i = 0; i < 4; ++i) {
pt2[i].x = points2[i].x;
pt2[i].y = points2[i].y;
pt1[i].x = points1[i].x;
pt1[i].y = points1[i].y;
}
cvGetPerspectiveTransform(pt1, pt2, P);
/**
* - Bestimme die notwendige Bildgröße für das Panoramabild.
*/
/* TODO */
int h = img1f->height - 1;
int w = img1f->width - 1;
float p1[] = { 0.0, 0.0, 1.0 };
float p2[] = { 0.0, (float)(h), 1.0 };
float p3[] = { (float)(w), (float)(h), 1.0 };
float p4[] = { (float)(w), 0.0, 1.0 };
cv::Mat P1 = P * cv::Mat(3, 1, CV_32FC1, p1);
cv::Mat P2 = P * cv::Mat(3, 1, CV_32FC1, p2);
cv::Mat P3 = P * cv::Mat(3, 1, CV_32FC1, p3);
cv::Mat P4 = P * cv::Mat(3, 1, CV_32FC1, p4);
// mustn't be zero
assert(P1.at<float>(2,0) != 0 && P2.at<float>(2,0) != 0 && P3.at<float>(2,0) != 0 && P4.at<float>(2,0) != 0);
P1 = P1 / P1.at<float>(2,0);
P2 = P2 / P2.at<float>(2,0);
P3 = P3 / P3.at<float>(2,0);
P4 = P4 / P4.at<float>(2,0);
/**
* - Projiziere das linke Bild in die Bildebene des rechten Bildes. Beachte
* dabei, dass auch der linke Bildrand in das Panoramabild projiziert
* wird.
*/
/* TODO */
///////// Hier wird irgendwo ein fehler sein bei der Groesse...
std::vector<cv::Mat*> matrices;
matrices.push_back(&P1);
matrices.push_back(&P2);
matrices.push_back(&P3);
matrices.push_back(&P4);
cv::Point minP(P1.at<float>(0,0), P1.at<float>(1,0)), maxP(P1.at<float>(0,0), P1.at<float>(1,0));
for(int i = 0; i < matrices.size(); ++i) {
minP.x = (int)(min(matrices[i]->at<float>(0,0), (float)minP.x));
minP.y = (int)(min(matrices[i]->at<float>(1,0), (float)minP.y));
maxP.x = (int)(max(matrices[i]->at<float>(0,0), (float)maxP.x)+1.0);
maxP.y = (int)(max(matrices[i]->at<float>(1,0), (float)maxP.y)+1.0);
}
minP.x = min(minP.x, 0); minP.y = min(minP.y, 0);
maxP.x = max(maxP.x, img1f->width-1); maxP.y = max(maxP.y, img1f->height-1);
// create image
cv::Mat Panorama = cv::Mat(cv::Size(maxP-minP), CV_32FC1, cv::Scalar(0.0));
cv::Mat PLeft = cv::Mat(cv::Size(maxP-minP), CV_32FC1, cv::Scalar(0.0));
cv::Mat PRight = cv::Mat(cv::Size(maxP-minP), CV_32FC1, cv::Scalar(0.0));
cv::Mat matImg1f = cv::Mat( img1f);
cv::Mat matImg2f = cv::Mat( img2f);
for(int y=0; y < matImg1f.rows; ++y ) {
for(int x=0; x < matImg1f.cols; ++x ) {
PLeft.at<float>(y,x) = matImg1f.at<float>(y,x);
}
}
for(int y=0; y < matImg2f.rows; ++y ) {
for(int x=0; x < matImg2f.cols; ++x ) {
PRight.at<float>(y,x) = matImg2f.at<float>(y,x);
}
}
cv::imshow("mainWin", PLeft);
cv::waitKey(0);
cv::imshow("mainWin", PRight);
cv::waitKey(0);
float trans2[] = { 1.0, 0.0, -minP.x, 0.0, 1.0, -minP.y, 0.0, 0.0, 1.0};
cv::Mat translation(3,3,CV_32FC1,trans2);
//translate P
cv::Mat Pnew = translation*cv::Mat(P);
cv::warpPerspective(PLeft, Panorama, Pnew, Panorama.size());
cv::warpPerspective(PRight, PLeft, translation, PLeft.size());
PRight = PLeft.clone();
cv::imshow("mainWin", PLeft);
cv::waitKey(0);
cv::imshow("mainWin", Panorama);
cv::waitKey(0);
/**
* - Bilde das Panoramabild, so dass Pixel, für die zwei Werte vorhanden sind,
* den Mittelwert zugeordnet bekommen.
*/
cv::Mat mask = (Panorama > 0.0) & (PLeft > 0.0);
cv::imshow("mainWin", mask);
cv::waitKey(0);
mask.convertTo(mask,CV_32FC1, 0.5/255.);
cv::Mat weighted = cv::Mat(Panorama.size(), CV_32FC1, cv::Scalar(1.0)) - mask;
Panorama = Panorama + PLeft;
cv::multiply(Panorama, weighted, Panorama);
cv::imshow("mainWin", Panorama);
cv::waitKey(0);
/* TODO */
/**
* - Zeige das Panoramabild an.
*/
/* TODO */
}
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);
}
//추후 수정
void FkPaperKeyboard_TypeA::cornerVerification(IplImage* srcImage){
CvSize size = cvGetSize(srcImage);
IplImage* eigImage = cvCreateImage(size, IPL_DEPTH_8U,1);
IplImage* tempImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* grayImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* veriImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* dstImage = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* mask = cvCreateImage(size, IPL_DEPTH_8U, 1);
IplImage* mask2 = cvCreateImage(size, IPL_DEPTH_8U, 1);
CvRect rect = cvRect(10, 10, 640 - 20, 480 - 20);
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3, CV_32FC1);
CvMat* warp_matrix_invert = cvCreateMat(3,3, CV_32FC1);
CvMat* result = cvCreateMat(3, 1, CV_32FC1);
CvMat* dst = cvCreateMat(3, 1,CV_32FC1);
int keyButtonCornerCount = 316;
cvCvtColor(srcImage, grayImage, CV_BGR2GRAY);
cvSetImageROI(grayImage, rect);
cvSetImageROI(mask, rect);
cvSetImageROI(dstImage, rect);
cvSetImageROI(mask2, rect);
// 150~255사이의 값만 추출해서 마스크에 저장
cvInRangeS(grayImage, cvScalar(100, 100, 100), cvScalar(255, 255, 255), mask);
cvCopy(mask, mask2);
//cvShowImage("mask", mask);
//cvShowImage("mask2", mask2);
// 20,20? 150 미만의 값을 제외하기 위해 0인 값(mask)과 추출한 값(mask2)을 XOR 연산 한다.
cvFloodFill(mask, cvPoint(10, 10), cvScalar(0, 0, 0));
cvXor(mask2, mask, dstImage);
//cvShowImage("mask3", mask);
//cvShowImage("mask4", mask2);
//cvShowImage("dstImage", dstImage);
// 최종 연산된 이미지에서 코너 추출(각 키패드의 코너)
cvGoodFeaturesToTrack(dstImage, eigImage, tempImage, keyButtonCorner, &keyButtonCornerCount, 0.01, 7, NULL, 7, 0);
cvFindCornerSubPix (dstImage, keyButtonCorner, keyButtonCornerCount,cvSize (3, 3), cvSize (-1, -1), cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvResetImageROI(dstImage);
for(int i =0 ; i < 316 ; i++){
keyButtonCorner[i].x += rect.x;
keyButtonCorner[i].y += rect.y;
}
initKeyButtonCorner();
srcQuad[CLOCKWISE_1].x = keyButtonCorner[315].x+10;
srcQuad[CLOCKWISE_1].y = keyButtonCorner[315].y-10;
srcQuad[CLOCKWISE_5].x = keyButtonCorner[31].x + 10;
srcQuad[CLOCKWISE_5].y = keyButtonCorner[31].y + 10;
srcQuad[CLOCKWISE_7].x = keyButtonCorner[0].x - 10;
srcQuad[CLOCKWISE_7].y = keyButtonCorner[0].y + 10;
srcQuad[CLOCKWISE_11].x = keyButtonCorner[290].x - 10;
srcQuad[CLOCKWISE_11].y = keyButtonCorner[290].y - 10;
dstQuad[CLOCKWISE_1].x = 640;
dstQuad[CLOCKWISE_1].y = 0;
dstQuad[CLOCKWISE_5].x = 640;
dstQuad[CLOCKWISE_5].y = 480;
dstQuad[CLOCKWISE_7].x = 0;
dstQuad[CLOCKWISE_7].y = 480;
dstQuad[CLOCKWISE_11].x = 0;
dstQuad[CLOCKWISE_11].y = 0;
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix);
cvWarpPerspective(dstImage, veriImage, warp_matrix);
detectKeyButtonCorner(veriImage);
cvInvert(warp_matrix, warp_matrix_invert);
for(int i = 0 ; i < 316 ; i++){
cvmSet(dst, 0, 0, keyButtonCorner[i].x);
cvmSet(dst, 1, 0, keyButtonCorner[i].y);
cvmSet(dst, 2, 0, 1);
cvMatMul(warp_matrix_invert, dst, result);
float t = cvmGet(result, 2,0);
keyButtonCorner[i].x = cvmGet(result, 0,0)/t ;
keyButtonCorner[i].y = cvmGet(result, 1,0)/t ;
}
cvResetImageROI(srcImage);
cvResetImageROI(mask);
cvReleaseImage(&eigImage);
cvReleaseImage(&tempImage);
cvReleaseImage(&grayImage);
cvReleaseImage(&veriImage);
cvReleaseImage(&dstImage);
cvReleaseImage(&mask);
cvReleaseImage(&mask2);
cvReleaseMat(&warp_matrix);
cvReleaseMat(&warp_matrix_invert);
cvReleaseMat(&result);
cvReleaseMat(&dst);
}
void FkPaperKeyboard_TypeA::setKeyButton(IplImage* srcImage){
//꼭지점
CvPoint2D32f srcQuad[4], dstQuad[4];
IplImage* perspectiveTransImage = cvCreateImage(cvSize(640,480), IPL_DEPTH_8U, 3);
CvMat* warp_matrix = cvCreateMat(3,3, CV_32FC1);
CvMat* warp_matrix_invert = cvCreateMat(3,3, CV_32FC1);
CvMat* result = cvCreateMat(3, 1, CV_32FC1);
CvMat* dst = cvCreateMat(3, 1,CV_32FC1);
sortPaperKeyboardCorner();
srcQuad[CLOCKWISE_1] = keyboardCorner[1];
srcQuad[CLOCKWISE_5] = keyboardCorner[2];
srcQuad[CLOCKWISE_7] = keyboardCorner[3];
srcQuad[CLOCKWISE_11] = keyboardCorner[0];
dstQuad[CLOCKWISE_1].x = 640;
dstQuad[CLOCKWISE_1].y = 0;
dstQuad[CLOCKWISE_5].x = 640;
dstQuad[CLOCKWISE_5].y = 480;
dstQuad[CLOCKWISE_7].x = 0;
dstQuad[CLOCKWISE_7].y = 480;
dstQuad[CLOCKWISE_11].x = 0;
dstQuad[CLOCKWISE_11].y = 0;
// 원근변환 후 검사한다.
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix); // 변환 값 계산
cvWarpPerspective(srcImage, perspectiveTransImage, warp_matrix); // 투영 변환 행렬을 구한다
cornerVerification(perspectiveTransImage); // 추출된 행렬의 유효성을 검사한다
cvInvert(warp_matrix, warp_matrix_invert); //
//cvShowImage("srcImage",srcImage);
//cvShowImage("perspectiveTransImage", perspectiveTransImage);
for(int i = 0 ; i < 316 ; i++){
cvmSet(dst, 0, 0, keyButtonCorner[i].x);
cvmSet(dst, 1, 0, keyButtonCorner[i].y);
cvmSet(dst, 2, 0, 1);
cvMatMul(warp_matrix_invert, dst, result);
float t = cvmGet(result, 2,0);
keyButtonCorner[i].x = cvmGet(result, 0,0)/t;
keyButtonCorner[i].y = cvmGet(result, 1,0)/t;
}
setKeyButtonArea(keyButtonCorner, 0, 16);
setKeyButtonArea(keyButtonCorner, 64, 14);
setKeyButtonArea(keyButtonCorner, 120, 14);
setKeyButtonArea(keyButtonCorner, 176, 13);
setKeyButtonArea(keyButtonCorner, 228, 12);
setKeyButtonArea(keyButtonCorner, 276, 7);
setDirectionKeyButtonArea(keyButtonCorner, 306, 2, 77);
setDirectionKeyButtonArea(keyButtonCorner, 304, 4, 76);
setDirectionKeyButtonArea(keyButtonCorner, 308, 3, 78);
cvReleaseImage(&perspectiveTransImage);
cvReleaseMat(&warp_matrix);
cvReleaseMat(&warp_matrix_invert);
cvReleaseMat(&result);
cvReleaseMat(&dst);
}
void the_project::project_getting()
{
cout << "Ready to go?\n";
cin.get();
get_in = cvCreateImage(image_size, IPL_DEPTH_8U, 3);
if(v_c_flag==0){
// get a image of path
///*
IplImage *new_one;
cout << "调整照相机位置并获取图像,Sapce to continue:\n";
cvNamedWindow("Adjust");
while(1){
if(cvWaitKey(10)==' '){
cvCopyImage(new_one,get_in);
cvDestroyWindow("Adjust");
cvReleaseCapture(&for_cam);
break;
}
new_one = cvQueryFrame(for_cam);
cvShowImage("Adjust",new_one);
}
cvSaveImage("image_origin.jpg",get_in);
//*/
}
else if(v_c_flag==1){
//for_test
get_in = cvQueryFrame(for_video);
}
cvNamedWindow("win1");
cvShowImage("win1",get_in);
// transform
get_change = cvCreateImage(image_size, IPL_DEPTH_8U, 3);
transmat = cvCreateMat(3, 3, CV_32FC1);
vector<CvPoint2D32f> points;
void * p_pointer =
reinterpret_cast<void *>(&points);
cout << "choose four points for transform.\n";
cvSetMouseCallback("win1", mouse,p_pointer);
cvWaitKey();
cvDestroyWindow("win1");
for(int i=0;i<4;i++)
originpoints[i] = points[i];
cvGetPerspectiveTransform(originpoints, newpoints, transmat);
cvWarpPerspective(get_in, get_change, transmat);
cvNamedWindow("win1");
cvShowImage("win1",get_in);
cvNamedWindow("win2");
cvShowImage("win2",get_change);
cvSaveImage("image_square.jpg",get_change);
get_unchange = cvCreateImage(image_size, IPL_DEPTH_8U, 3);
get_looking = cvCreateImage(image_size, IPL_DEPTH_8U, 3);
cvCopyImage(get_change,get_unchange);
cout << "Press any key to continue...\n";
cvWaitKey();
}
void BirdsIview(CvMat* intrinsic,CvMat* distortion,IplImage* image,CvPoint2D32f* corners){
//This initializes rectification matrices
IplImage* mapx = cvCreateImage( cvGetSize(image), IPL_DEPTH_32F, 1 );
IplImage* mapy = cvCreateImage( cvGetSize(image), IPL_DEPTH_32F, 1 );
// UNDISTORT OUR IMAGE
cvInitUndistortMap(intrinsic,distortion,mapx,mapy);
IplImage *t = cvCloneImage(image);
// Rectify our image
cvRemap(t, image, mapx,mapy,CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,cvScalarAll(0));
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;
//arrange points in Z orientation
if ( abs(corners[0].x - corners[board_w-1].x) > abs(corners[0].y - corners[board_w-1].y) ){
if ((corners[board_w-1].x-corners[0].x) > 0){
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];
}
if ((corners[board_w-1].x-corners[0].x) < 0){
imgPts[3] = corners[0];
imgPts[2] = corners[board_w-1];
imgPts[1] = corners[(board_h-1)*board_w];
imgPts[0] = corners[(board_h-1)*board_w + board_w-1];
}
}
if ( abs(corners[0].x - corners[board_w-1].x) < abs(corners[0].y - corners[board_w-1].y) ){
if ((corners[board_w-1].y-corners[0].y) > 0){
imgPts[1] = corners[0];
imgPts[3] = corners[board_w-1];
imgPts[0] = corners[(board_h-1)*board_w];
imgPts[2] = corners[(board_h-1)*board_w + board_w-1];
}
if ((corners[board_w-1].y-corners[0].y) < 0){
imgPts[2] = corners[0]; //2
imgPts[0] = corners[board_w-1]; //0
imgPts[3] = corners[(board_h-1)*board_w];
imgPts[1] = corners[(board_h-1)*board_w + board_w-1]; //1
}
}
// DRAW THE POINTS in order: B,G,R,YELLOW
cvCircle(image,cvPointFrom32f(imgPts[0]),9,CV_RGB(0,0,255),3,8,0);
cvCircle(image,cvPointFrom32f(imgPts[1]),9,CV_RGB(0,255,0),3,8,0);
cvCircle(image,cvPointFrom32f(imgPts[2]),9,CV_RGB(255,0,0),3,8,0);
cvCircle(image,cvPointFrom32f(imgPts[3]),9,CV_RGB(255,255,0),3,8,0);
//Find the Homography
H = cvCreateMat(3,3,CV_32F);
cvGetPerspectiveTransform( objPts,imgPts,H );
IplImage *birdsview_image = cvCloneImage(image);
CV_MAT_ELEM(*H,float,2,2) += Z;
cvWarpPerspective(
image,
birdsview_image,
H,
CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS |CV_WARP_INVERSE_MAP ,
cvScalarAll(0) );
cvShowImage(BVwindow,birdsview_image);
}
int match_X(IplImage* binary, CvPoint* points, int pixel_count, CvPoint* r_point, int cent_x, int cent_y, IplImage* debug){
int i,x,y; //useful variable names
CvPoint* corners; //the corners of the image
corners = (CvPoint*)calloc(4, sizeof(CvPoint));
corners[0].x = r_point->x;
corners[0].y = r_point->y;
//find the angle of the first corner
x = corners[0].x - cent_x;
y = corners[0].y - cent_y;
int theta = arctan(y,x);
//find the other 3 corners
int maxr1 = 0;
int maxr2 = 0;
int maxr3 = 0;
//keep track of the pixel sums on either side of angle0
int pxsum1 = 0; //this is bigger --> corner 0 goes in upper left
int pxsum2 = 0; // this is bigger --> corner 0 goes in upper right
for(i = pixel_count -1; i>=0; i--){
x = points[i].x - cent_x;
y = points[i].y - cent_y;
int tempang = arctan(y,x);
//normalize angles
if (tempang < theta) tempang += 360;
//find tempr
int tempr = (int)sqrt((double)(x*x + y*y));
if( tempang - theta > 315 ){
//we are in the same quadrant as the origional angle
pxsum1++;
} else if( tempang - theta > 225){
//call this quadrant 3
if( tempr > maxr3){
maxr3 = tempr;
corners[3].x = points[i].x;
corners[3].y = points[i].y;
}
} else if( tempang - theta > 135) {
//call this quadrant 2
if( tempr > maxr2) {
maxr2 = tempr;
corners[2].x = points[i].x;
corners[2].y = points[i].y;
}
} else if( tempang - theta > 45) {
//call this quadrant 1
if( tempr > maxr1) {
maxr1 = tempr;
corners[1].x = points[i].x;
corners[1].y = points[i].y;
}
} else {
//we are again in the same quadrant as the origional angle
pxsum2++;
}
#ifdef VISUAL_DEBUG_X
//color every quadrant a different color
if( tempang - theta > 315 ){
debug->imageData[3*points[i].y*debug->width+3*points[i].x+2]=0;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+1]=254;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+0]=254;
} else if( tempang - theta > 225){
debug->imageData[3*points[i].y*debug->width+3*points[i].x+2]=254;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+1]=0;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+0]=0;
} else if( tempang - theta > 135) {
debug->imageData[3*points[i].y*debug->width+3*points[i].x+2]=0;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+1]=0;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+0]=254;
} else if( tempang - theta > 45) {
debug->imageData[3*points[i].y*debug->width+3*points[i].x+2]=254;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+1]=0;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+0]=254;
} else {
debug->imageData[3*points[i].y*debug->width+3*points[i].x+2]=0;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+1]=254;
debug->imageData[3*points[i].y*debug->width+3*points[i].x+0]=0;
}
#endif
}
#ifdef VISUAL_DEBUG_X
//draw a line between consecutive corners
CvScalar boxcolor = {{0, 254, 0}};
cvLine(debug, corners[0], corners[1], boxcolor, 1, 8, 0);
cvLine(debug, corners[1], corners[2], boxcolor, 1, 8, 0);
cvLine(debug, corners[2], corners[3], boxcolor, 1, 8, 0);
cvLine(debug, corners[3], corners[0], boxcolor, 1, 8, 0);
//draw a circle at the center of the image
CvPoint center = {cent_x, cent_y};
CvScalar centercolor = {{0, 0, 254}};
cvCircle(debug, center, 5, centercolor, 2, 8, 0);
//mark corner 0
CvPoint corner0 = {corners[0].x, corners[0].y};
CvScalar cornercolor = {{254, 0, 254}};
cvCircle(debug, corner0, 5, cornercolor, 1, 8, 0);
#endif
//load template
IplImage* xtemplate = cvLoadImage("../vision/xtemplate.png",CV_LOAD_IMAGE_GRAYSCALE);
//get transformation matrix that would place corner values
//in the correct location to compare to template
//transform image
//create an array for the 4 src points and dest points
CvPoint2D32f* src;
CvPoint2D32f* dst;
src = (CvPoint2D32f*)calloc(4, sizeof(CvPoint2D32f));
dst = (CvPoint2D32f*)calloc(4, sizeof(CvPoint2D32f));
//populate the src array
if( pxsum1 >= pxsum2 ){
for ( i=0; i<4; i++){
src[i].x = corners[i].x;
src[i].y = corners[i].y;
}
}else{
for ( i=0; i<4; i++){
int j = i-1;
if ( j<0 ) j = 3;
src[i].x = corners[j].x;
src[i].y = corners[j].y;
}
}
//populate the dst array
dst[0].x = 0;
dst[0].y = 0;
dst[1].x = 0;
dst[1].y = 100;
dst[2].x = 100;
dst[2].y = 100;
dst[3].x = 100;
dst[3].y = 0;
//get the transformation matrix
CvMat* tmatrix = cvCreateMat(3,3,CV_32FC1);
tmatrix = cvGetPerspectiveTransform( src, dst, tmatrix);
//transform the image
CvSize warpedsize = {100,100};
IplImage* warped = cvCreateImage(warpedsize, 8, 1);
CvScalar fillcolor = {{0}};
cvWarpPerspective(binary, warped, tmatrix,CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS , fillcolor);
#ifdef VISUAL_DEBUG_X
IplImage* compared = cvCreateImage(cvGetSize(xtemplate),8,3);
#endif
//XOR the template image with our warped image
int xor_sum = 0;
for ( i=xtemplate->width*xtemplate->height -1; i>=0; i--){
int p1 = xtemplate->imageData[i];
int p2 = warped->imageData[i];
if(( p1 != 0 && p2 != 0 ) || ( p1 == 0 && p2 == 0 )){
xor_sum++;
}
#ifdef VISUAL_DEBUG_X
if(p1 != 0 && p2 != 0){
compared->imageData[i*3+0] = 0x00;
compared->imageData[i*3+1] = 0xff;
compared->imageData[i*3+2] = 0x00;
}else if(p1 == 0 && p2 == 0){
compared->imageData[i*3+0] = 0x00;
compared->imageData[i*3+1] = 0xff;
compared->imageData[i*3+2] = 0x00;
}else if(p1 != 0){
compared->imageData[i*3+0] = 0xff;
compared->imageData[i*3+1] = 0x00;
compared->imageData[i*3+2] = 0x00;
}else if(p2 != 0){
compared->imageData[i*3+0] = 0xff;
compared->imageData[i*3+1] = 0x00;
compared->imageData[i*3+2] = 0x00;
}
#endif
}
//compute confidence
int confidence = xor_sum * 100 / (xtemplate->width*xtemplate->height);
#ifdef VISUAL_DEBUG_X
cvNamedWindow("Compared",CV_WINDOW_AUTOSIZE);
cvShowImage("Compared", compared);
cvNamedWindow("Warped", CV_WINDOW_AUTOSIZE);
cvShowImage("Warped", warped);
cvNamedWindow("Xtemplate", CV_WINDOW_AUTOSIZE);
cvShowImage("Xtemplate",xtemplate);
#endif
//free memory
cvReleaseImage(&xtemplate);
cvReleaseImage(&warped);
free(corners);
free(src);
free(dst);
cvReleaseMat(&tmatrix);
#ifdef VISUAL_DEBUG_X
cvReleaseImage(&compared);
#endif
return confidence;
}
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;
}
}
int main( int argc, char** argv )
{
IplImage *current_frame=NULL;
CvSize size;
size.height = 300; size.width = 200;
IplImage *corrected_frame = cvCreateImage( size, IPL_DEPTH_8U, 3 );
IplImage *labelled_image=NULL;
IplImage *vertical_edge_image=NULL;
int user_clicked_key=0;
// Load the video (AVI) file
CvCapture *capture = cvCaptureFromAVI( "./Postboxes.avi" );
// Ensure AVI opened properly
if( !capture )
return 1;
// Get Frames Per Second in order to playback the video at the correct speed
int fps = ( int )cvGetCaptureProperty( capture, CV_CAP_PROP_FPS );
// Explain the User Interface
printf( "Hot keys: \n"
"\tESC - quit the program\n"
"\tSPACE - pause/resume the video\n");
CvPoint2D32f from_points[4] = { {3, 6}, {221, 11}, {206, 368}, {18, 373} };
CvPoint2D32f to_points[4] = { {0, 0}, {200, 0}, {200, 300}, {0, 300} };
CvMat* warp_matrix = cvCreateMat( 3,3,CV_32FC1 );
cvGetPerspectiveTransform( from_points, to_points, warp_matrix );
// Create display windows for images
cvNamedWindow( "Input video", 0 );
cvNamedWindow( "Vertical edges", 0 );
cvNamedWindow( "Results", 0 );
// Setup mouse callback on the original image so that the user can see image values as they move the
// cursor over the image.
cvSetMouseCallback( "Input video", on_mouse_show_values, 0 );
window_name_for_on_mouse_show_values="Input video";
while( user_clicked_key != ESC ) {
// Get current video frame
current_frame = cvQueryFrame( capture );
image_for_on_mouse_show_values=current_frame; // Assign image for mouse callback
if( !current_frame ) // No new frame available
break;
cvWarpPerspective( current_frame, corrected_frame, warp_matrix );
if (labelled_image == NULL)
{ // The first time around the loop create the image for processing
labelled_image = cvCloneImage( corrected_frame );
vertical_edge_image = cvCloneImage( corrected_frame );
}
check_postboxes( corrected_frame, labelled_image, vertical_edge_image );
// Display the current frame and results of processing
cvShowImage( "Input video", current_frame );
cvShowImage( "Vertical edges", vertical_edge_image );
cvShowImage( "Results", labelled_image );
// Wait for the delay between frames
user_clicked_key = cvWaitKey( 1000 / fps );
if (user_clicked_key == ' ')
{
user_clicked_key = cvWaitKey(0);
}
}
/* free memory */
cvReleaseCapture( &capture );
cvDestroyWindow( "video" );
return 0;
}
void main(int argc, char** argv)
{
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1);
float Z=1;
/*cvNamedWindow("img", CV_WINDOW_AUTOSIZE);
cvNamedWindow("warp", CV_WINDOW_AUTOSIZE);*/
dstQuad[0].x = 250; //src Top left
dstQuad[0].y = 100;
dstQuad[1].x = 430; //src Top right
dstQuad[1].y = 115;
dstQuad[2].x = 50; //src Bottom left
dstQuad[2].y = 170;
dstQuad[3].x = 630; //src Bot right
dstQuad[3].y = 250;
int lOff = 50, tOff = 150;
srcQuad[0].x = tOff; //dst Top left
srcQuad[0].y = lOff;
srcQuad[1].x = 640-tOff; //dst Top right
srcQuad[1].y = lOff;
srcQuad[2].x = tOff; //dst Bottom left
srcQuad[2].y = 480-lOff;
srcQuad[3].x = 640-tOff; //dst Bot right
srcQuad[3].y = 480-lOff;
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix);
int ik=0, ni = 0, niX = 22-1;
char names[22][25] = {
"../../Data/6 Dec/009.jpg",
"../../Data/6 Dec/011.jpg",
"../../Data/6 Dec/012.jpg",
"../../Data/6 Dec/016.jpg",
"../../Data/6 Dec/018.jpg",
"../../Data/6 Dec/019.jpg",
"../../Data/6 Dec/020.jpg",
"../../Data/6 Dec/022.jpg",
"../../Data/6 Dec/024.jpg",
"../../Data/6 Dec/064.jpg",
"../../Data/6 Dec/065.jpg",
"../../Data/6 Dec/066.jpg",
"../../Data/6 Dec/067.jpg",
"../../Data/6 Dec/068.jpg",
"../../Data/6 Dec/069.jpg",
"../../Data/6 Dec/070.jpg",
"../../Data/6 Dec/071.jpg",
"../../Data/6 Dec/072.jpg",
"../../Data/6 Dec/073.jpg",
"../../Data/6 Dec/074.jpg",
"../../Data/6 Dec/075.jpg",
"../../Data/6 Dec/076.jpg"
};
int lwSum = 0, nopf = 0;
//CvCapture *capture = cvCaptureFromCAM(0);
/*double fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
IplImage* image = cvRetrieveFrame(capture);
CvSize imgSize;
imgSize.width = image->width;
imgSize.height = image->height;
CvVideoWriter *writer = cvCreateVideoWriter("out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, imgSize);*/
while(1)
{
//IplImage* img = cvQueryFrame(capture);
IplImage* img = cvLoadImage( "../../Data/23 Jan/c.jpg", CV_LOAD_IMAGE_COLOR);
//cvSaveImage(nameGen(ik++), img, 0);
//cvShowImage("img", img);
IplImage* warp_img = cvCloneImage(img);
CV_MAT_ELEM(*warp_matrix, float, 2, 2) = Z;
cvWarpPerspective(img, warp_img, warp_matrix, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
//cvReleaseImage(&img);
//cvWaitKey(0);
IplImage* grayimg = cvCreateImage(cvGetSize(warp_img),IPL_DEPTH_8U,1);
cvCvtColor( warp_img, grayimg, CV_RGB2GRAY );
cvReleaseImage(&warp_img);
cvSmooth(grayimg, grayimg, CV_GAUSSIAN, 3, 3, 0.0, 0.0);
cvEqualizeHist(grayimg, grayimg);
cvThreshold(grayimg, grayimg, PercentileThreshold(grayimg, 10.0), 255, CV_THRESH_BINARY);
IplImage* finalimg = cvCreateImage(cvGetSize(grayimg),IPL_DEPTH_8U,3);
CvMemStorage* line_storage=cvCreateMemStorage(0);
CvSeq* results = cvHoughLines2(grayimg,line_storage,CV_HOUGH_PROBABILISTIC,10,CV_PI/180*5,350,100,10);
cvReleaseImage(&grayimg);
double angle = 0.0, temp;
double lengthSqd, wSum=0;
CvPoint center = cvPoint(0, 0);
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
//lengthSqd = (line[0].x - line[1].x)*(line[0].x - line[1].x) + (line[0].y - line[1].y)*(line[0].y - line[1].y);
wSum += 1;//lengthSqd;
if(line[0].y > line[1].y)
temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x));
else
temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x));
if(temp < 0)
angle += (90 + 180/3.14*temp)/* * lengthSqd*/;
else
angle += (180/3.14*temp - 90)/* * lengthSqd*/;
center.x += (line[0].x + line[1].x)/2;
center.y += (line[0].y + line[1].y)/2;
}
angle /= wSum; // Angle Direction: Left == -ve and Right == +ve
// Angle is calculated w.r.t Vertical
//angle+=10; // Angle Offset (Depends on camera's position)
center.x /= results->total;
center.y /= results->total;
double m = (angle != 0) ? tan(CV_PI*(0.5-angle/180)) : 100000; // 100000 represents a very large slope (near vertical)
//m=-m; // Slope Correction
CvPoint leftCenter = cvPoint(0, 0), rightCenter = cvPoint(0, 0);
double leftSlope = 0, rightSlope = 0, leftCount = 0, rightCount = 0;
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
CvPoint midPoint = cvPoint((line[0].x + line[1].x)/2, (line[0].y + line[1].y)/2);
double L11 = (0-center.y + m*(0-center.x + 0.0))/m;
double L22 = (midPoint.y-center.y + m*(midPoint.x-center.x + 0.0))/m;
if(L11*L22 > 0)
{
leftCenter.x += midPoint.x;
leftCenter.y += midPoint.y;
leftSlope += -(line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
leftCount++;
}
else
{
rightCenter.x += midPoint.x;
rightCenter.y += midPoint.y;
rightSlope += -(line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
rightCount++;
}
}
cvReleaseMemStorage(&line_storage);
leftCenter.x /= leftCount; leftCenter.y /= leftCount; leftSlope /= leftCount;
rightCenter.x /= rightCount; rightCenter.y /= rightCount; rightSlope /= rightCount;
CvPoint botCenter = cvPoint(finalimg->width/2, finalimg->height);
int dL = abs(botCenter.y-leftCenter.y + m * (botCenter.x-leftCenter.x)) / sqrt(m*m + 1);
int dR = abs(botCenter.y-rightCenter.y + m * (botCenter.x-rightCenter.x)) / sqrt(m*m + 1);
int lw = abs((leftCenter.y - rightCenter.y) + m*(leftCenter.x - rightCenter.x)) / sqrt(m*m + 1);
lwSum += lw;
nopf++;
if(lw <= SINGLE_LANE_WIDTH)
{
double L11 = (0-leftCenter.y + m*(0-leftCenter.x + 0.0))/m;
double L22 = (botCenter.y-leftCenter.y + m*(botCenter.x-leftCenter.x + 0.0))/m;
if(L11*L22 < 0)
dR = lwSum/nopf - dL; // Only Left Lane is visible
else
dL = lwSum/nopf - dR; // Only Right Lane is visible
}
//cvSaveImage("test.jpg", finalimg, 0);
printf("Bot:\t(%d, %d, %.3f)\n", dL, (finalimg->height)/10, 90.0-angle);
printf("Target:\t(%d, %d, %.3f)\n", (dL+dR)/2, (finalimg->height)*9/10, 90.0);
location bot, target;
bot.x = dL; bot.y = (finalimg->height)/10; bot.theta = 90.0-angle;
target.x = (dL+dR)/2; target.y = (finalimg->height)*9/10; target.theta = 90.0;
cvReleaseImage(&finalimg);
list *ol = NULL, *cl = NULL;
elem e,vare;
e.l = bot; e.g = 0; e.h = 0; e.id = UNDEFINED;
int n = 15;
elem* np = loadPosData(n);
while(1)
{
cl = append(cl, e);
//printList(cl);
if(isNear(e.l, target))
break;
ol = update(ol, e, target, np, n);
//printList(ol);
e = findMin(ol);
//printf("Min: (%.3f, %.3f, %.3f, %d)\n", e.l.x, e.l.y, e.l.theta, e.id);
ol = detach(ol, e);
//printList(ol);
//getchar();
}
free(np);
vare = e;
printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id);
while(!((abs(vare.l.x-bot.x) < 1.25) && (abs(vare.l.y-bot.y) < 1.25)))
{
vare=search(cl,vare.parent.x,vare.parent.y);
if(vare.id != -1)
{
printf("(%.3f, %.3f, %.3f) : %d\n", vare.l.x, vare.l.y, vare.l.theta, vare.id);
e = vare;
}
}
printf("\n(%.3f, %.3f, %.3f) : %d\n", e.l.x, e.l.y, e.l.theta, e.id);
//navCommand(10-e.id, e.id);
releaseList(ol);
releaseList(cl);
getchar();
int c = cvWaitKey(0);
if(c == '4')
{
if(ni != 0)
ni--;
}
else if(c == '6')
{
if(ni != niX)
ni++;
}
}
}
/* Warps source into destination by a perspective transform */
static void cvWarpPerspective( CvArr* src, CvArr* dst, double quad[4][2] )
{
CV_FUNCNAME( "cvWarpPerspective" );
__BEGIN__;
#ifdef __IPL_H__
IplImage src_stub, dst_stub;
IplImage* src_img;
IplImage* dst_img;
CV_CALL( src_img = cvGetImage( src, &src_stub ) );
CV_CALL( dst_img = cvGetImage( dst, &dst_stub ) );
iplWarpPerspectiveQ( src_img, dst_img, quad, IPL_WARP_R_TO_Q,
IPL_INTER_CUBIC | IPL_SMOOTH_EDGE );
#else
int fill_value = 0;
double c[3][3]; /* transformation coefficients */
double q[4][2]; /* rearranged quad */
int left = 0;
int right = 0;
int next_right = 0;
int next_left = 0;
double y_min = 0;
double y_max = 0;
double k_left, b_left, k_right, b_right;
uchar* src_data;
int src_step;
CvSize src_size;
uchar* dst_data;
int dst_step;
CvSize dst_size;
double d = 0;
int direction = 0;
int i;
if( !src || (!CV_IS_IMAGE( src ) && !CV_IS_MAT( src )) ||
cvGetElemType( src ) != CV_8UC1 ||
cvGetDims( src ) != 2 )
{
CV_ERROR( CV_StsBadArg,
"Source must be two-dimensional array of CV_8UC1 type." );
}
if( !dst || (!CV_IS_IMAGE( dst ) && !CV_IS_MAT( dst )) ||
cvGetElemType( dst ) != CV_8UC1 ||
cvGetDims( dst ) != 2 )
{
CV_ERROR( CV_StsBadArg,
"Destination must be two-dimensional array of CV_8UC1 type." );
}
CV_CALL( cvGetRawData( src, &src_data, &src_step, &src_size ) );
CV_CALL( cvGetRawData( dst, &dst_data, &dst_step, &dst_size ) );
CV_CALL( cvGetPerspectiveTransform( src_size, quad, c ) );
/* if direction > 0 then vertices in quad follow in a CW direction,
otherwise they follow in a CCW direction */
direction = 0;
for( i = 0; i < 4; ++i )
{
int ni = i + 1; if( ni == 4 ) ni = 0;
int pi = i - 1; if( pi == -1 ) pi = 3;
d = (quad[i][0] - quad[pi][0])*(quad[ni][1] - quad[i][1]) -
(quad[i][1] - quad[pi][1])*(quad[ni][0] - quad[i][0]);
int cur_direction = CV_SIGN(d);
if( direction == 0 )
{
direction = cur_direction;
}
else if( direction * cur_direction < 0 )
{
direction = 0;
break;
}
}
if( direction == 0 )
{
CV_ERROR( CV_StsBadArg, "Quadrangle is nonconvex or degenerated." );
}
/* <left> is the index of the topmost quad vertice
if there are two such vertices <left> is the leftmost one */
left = 0;
for( i = 1; i < 4; ++i )
{
if( (quad[i][1] < quad[left][1]) ||
((quad[i][1] == quad[left][1]) && (quad[i][0] < quad[left][0])) )
{
left = i;
}
}
/* rearrange <quad> vertices in such way that they follow in a CW
direction and the first vertice is the topmost one and put them
into <q> */
if( direction > 0 )
{
for( i = left; i < 4; ++i )
{
q[i-left][0] = quad[i][0];
q[i-left][1] = quad[i][1];
}
for( i = 0; i < left; ++i )
{
q[4-left+i][0] = quad[i][0];
q[4-left+i][1] = quad[i][1];
}
}
else
{
for( i = left; i >= 0; --i )
{
q[left-i][0] = quad[i][0];
q[left-i][1] = quad[i][1];
}
for( i = 3; i > left; --i )
{
q[4+left-i][0] = quad[i][0];
q[4+left-i][1] = quad[i][1];
}
}
left = right = 0;
/* if there are two topmost points, <right> is the index of the rightmost one
otherwise <right> */
if( q[left][1] == q[left+1][1] )
{
right = 1;
}
/* <next_left> follows <left> in a CCW direction */
next_left = 3;
/* <next_right> follows <right> in a CW direction */
next_right = right + 1;
/* subtraction of 1 prevents skipping of the first row */
y_min = q[left][1] - 1;
/* left edge equation: y = k_left * x + b_left */
k_left = (q[left][0] - q[next_left][0]) /
(q[left][1] - q[next_left][1]);
b_left = (q[left][1] * q[next_left][0] -
q[left][0] * q[next_left][1]) /
(q[left][1] - q[next_left][1]);
/* right edge equation: y = k_right * x + b_right */
k_right = (q[right][0] - q[next_right][0]) /
(q[right][1] - q[next_right][1]);
b_right = (q[right][1] * q[next_right][0] -
q[right][0] * q[next_right][1]) /
(q[right][1] - q[next_right][1]);
for(;;)
{
int x, y;
y_max = MIN( q[next_left][1], q[next_right][1] );
int iy_min = MAX( cvRound(y_min), 0 ) + 1;
int iy_max = MIN( cvRound(y_max), dst_size.height - 1 );
double x_min = k_left * iy_min + b_left;
double x_max = k_right * iy_min + b_right;
/* walk through the destination quadrangle row by row */
for( y = iy_min; y <= iy_max; ++y )
{
int ix_min = MAX( cvRound( x_min ), 0 );
int ix_max = MIN( cvRound( x_max ), dst_size.width - 1 );
for( x = ix_min; x <= ix_max; ++x )
{
/* calculate coordinates of the corresponding source array point */
double div = (c[2][0] * x + c[2][1] * y + c[2][2]);
double src_x = (c[0][0] * x + c[0][1] * y + c[0][2]) / div;
double src_y = (c[1][0] * x + c[1][1] * y + c[1][2]) / div;
int isrc_x = cvFloor( src_x );
int isrc_y = cvFloor( src_y );
double delta_x = src_x - isrc_x;
double delta_y = src_y - isrc_y;
uchar* s = src_data + isrc_y * src_step + isrc_x;
int i00, i10, i01, i11;
i00 = i10 = i01 = i11 = (int) fill_value;
/* linear interpolation using 2x2 neighborhood */
if( isrc_x >= 0 && isrc_x <= src_size.width &&
isrc_y >= 0 && isrc_y <= src_size.height )
{
i00 = s[0];
}
if( isrc_x >= -1 && isrc_x < src_size.width &&
isrc_y >= 0 && isrc_y <= src_size.height )
{
i10 = s[1];
}
if( isrc_x >= 0 && isrc_x <= src_size.width &&
isrc_y >= -1 && isrc_y < src_size.height )
{
i01 = s[src_step];
}
if( isrc_x >= -1 && isrc_x < src_size.width &&
isrc_y >= -1 && isrc_y < src_size.height )
{
i11 = s[src_step+1];
}
double i0 = i00 + (i10 - i00)*delta_x;
double i1 = i01 + (i11 - i01)*delta_x;
((uchar*)(dst_data + y * dst_step))[x] = (uchar) (i0 + (i1 - i0)*delta_y);
}
x_min += k_left;
x_max += k_right;
}
if( (next_left == next_right) ||
(next_left+1 == next_right && q[next_left][1] == q[next_right][1]) )
{
break;
}
if( y_max == q[next_left][1] )
{
left = next_left;
next_left = left - 1;
k_left = (q[left][0] - q[next_left][0]) /
(q[left][1] - q[next_left][1]);
b_left = (q[left][1] * q[next_left][0] -
q[left][0] * q[next_left][1]) /
(q[left][1] - q[next_left][1]);
}
if( y_max == q[next_right][1] )
{
right = next_right;
next_right = right + 1;
k_right = (q[right][0] - q[next_right][0]) /
(q[right][1] - q[next_right][1]);
b_right = (q[right][1] * q[next_right][0] -
q[right][0] * q[next_right][1]) /
(q[right][1] - q[next_right][1]);
}
y_min = y_max;
}
#endif /* #ifndef __IPL_H__ */
__END__;
}
void main(int argc, char** argv)
{
cvNamedWindow("src",0 );
cvNamedWindow("warp image",0 );
cvNamedWindow("warp image (grey)",0 );
cvNamedWindow("Smoothed warped gray",0 );
cvNamedWindow("threshold image",0 );
cvNamedWindow("canny",0 );
cvNamedWindow("final",1 );
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1);
float Z=1;
dstQuad[0].x = 216; //src Top left
dstQuad[0].y = 15;
dstQuad[1].x = 392; //src Top right
dstQuad[1].y = 6;
dstQuad[2].x = 12; //src Bottom left
dstQuad[2].y = 187;
dstQuad[3].x = 620; //src Bot right
dstQuad[3].y = 159;
srcQuad[0].x = 100; //dst Top left
srcQuad[0].y = 120;
srcQuad[1].x = 540; //dst Top right
srcQuad[1].y = 120;
srcQuad[2].x = 100; //dst Bottom left
srcQuad[2].y = 360;
srcQuad[3].x = 540; //dst Bot right
srcQuad[3].y = 360;
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix);
//CvCapture *capture = cvCaptureFromCAM(0);
/*double fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
IplImage* image = cvRetrieveFrame(capture);
CvSize imgSize;
imgSize.width = image->width;
imgSize.height = image->height;
CvVideoWriter *writer = cvCreateVideoWriter("out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, imgSize);*/
int ik=0;
while(1)
{
//IplImage* img = cvQueryFrame(capture);
IplImage* img = cvLoadImage( "../../Data/6 Dec/009.jpg", CV_LOAD_IMAGE_COLOR);
cvShowImage( "src", img );
//cvWriteFrame(writer, img);
//cvSaveImage(nameGen(ik++), img, 0);
IplImage* warp_img = cvCloneImage(img);
CV_MAT_ELEM(*warp_matrix, float, 2, 2) = Z;
cvWarpPerspective(img, warp_img, warp_matrix, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
cvShowImage( "warp image", warp_img );
IplImage* grayimg = cvCreateImage(cvGetSize(warp_img),IPL_DEPTH_8U,1);
cvCvtColor( warp_img, grayimg, CV_RGB2GRAY );
cvShowImage( "warp image (grey)", grayimg );
cvSmooth(grayimg, grayimg, CV_GAUSSIAN, 3, 3, 0.0, 0.0);
cvShowImage( "Smoothed warped gray", grayimg );
IplImage* thresholded_img=simplethreshold(grayimg, 220);
cvShowImage("threshold image",thresholded_img);
//grayimg = doCanny( thresholded_img, 50, 100, 3 );
grayimg = cvCloneImage(thresholded_img);
cvShowImage("canny",grayimg);
IplImage* finalimg = cvCreateImage(cvGetSize(grayimg),IPL_DEPTH_8U,3);
CvMemStorage* line_storage=cvCreateMemStorage(0);
CvSeq* results = cvHoughLines2(grayimg,line_storage,CV_HOUGH_PROBABILISTIC,10,CV_PI/180*5,350,100,10);
double angle = 0.0, temp;
double lengthSqd, wSum=0;
double xc = 0, yc = 0;
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
cvLine( finalimg, line[0], line[1], CV_RGB(0,0,255), 1, CV_AA, 0 );
//lengthSqd = (line[0].x - line[1].x)*(line[0].x - line[1].x) + (line[0].y - line[1].y)*(line[0].y - line[1].y);
wSum += 1;//lengthSqd;
if(line[0].y > line[1].y)
temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x));
else
temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x));
if(temp < 0)
angle += (90 + 180/3.14*temp)/* * lengthSqd*/;
else
angle += (180/3.14*temp - 90)/* * lengthSqd*/;
xc += line[0].x + line[1].x;
yc += line[0].y + line[1].y;
}
angle=angle/wSum;
//angle+=10;
printf("total: %d, angle: % f\n", results->total, angle);
xc /= 2*results->total;
yc /= 2*results->total;
double m = (angle != 0) ? 1/tan(angle*3.14/180) : 100; // 100 represents a very large slope (near vertical)
m=-m;
double x1, y1, x2, y2; // The Center Line
y1 = 0;
y2 = finalimg->height;
x1 = xc + (y1-yc)/m;
x2 = xc + (y2-yc)/m;
cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(0,255,0), 1, CV_AA, 0);
printf("point: %f\t%f\n", xc, yc);
double lx=0, ly=0, lm=0, lc=0, rx=0, ry=0, rm=0, rc=0;
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
double xm = (line[0].x + line[1].x)/2.0, ym = (line[0].y + line[1].y)/2.0;
if(ym - yc - m*(xm - xc) > 0)
{
lx += xm;
ly += ym;
lm += (line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
lc++;
}
else
{
rx += xm;
ry += ym;
rm += (line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
rc++;
}
}
// The Left Line
lx /= lc; ly /= lc; lm /= lc;
rx /= rc; ry /= rc; rm /= rc;
printf("lins: %f\t%f\t%f\n", lx, ly, lm);
printf("lins: %f\t%f\t%f\n", rx, ry, rm);
y1 = 0;
y2 = finalimg->height-5;
x1 = lx + (y1-ly)/lm;
x2 = lx + (y2-ly)/lm;
cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(255,255,0), 1, CV_AA, 0);
// The Right Line
y1 = 0;
y2 = finalimg->height-5;
x1 = rx + (y1-ry)/rm;
x2 = rx + (y2-ry)/rm;
cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(0,255,255), 1, CV_AA, 0);
// The Center Point
CvPoint vpt = cvPoint(finalimg->width/2, 416);
printf("center point: %d\t%d\n", vpt.x, vpt.y);
// The Dl and Dr
int dl = vpt.x - lx + (ly-vpt.y+0.0)/lm;
int dr = (vpt.y-ry+0.0)/rm + rx - vpt.x;
printf("dl-dr: %d\n", dl-dr);
cvShowImage("final",finalimg);
if(dl-dr < SAFEZONE_LL) // Assume that the bot lies just on the boundary of the safe zone
{
if(angle < -10)
{
navCommand(7, angle);
}
else
{
navCommand(7, angle);
}
}
else if(dl-dr > SAFEZONE_RL)
{
if(angle > 10)
{
navCommand(-7, angle);
}
else
{
navCommand(-7, angle);
}
}
else
{
if((angle < 10) && (angle > -10))
{
navCommand(angle, angle);
}
else
{
navCommand(0, angle);
}
}
cvWaitKey(0);
}
}
int main(int argc, char *argv[])
{
if (argc != 6) {
printf("\nERROR: too few parameters\n");
help();
return -1;
}
help();
//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, *gray_image = 0;
if ((image = cvLoadImage(argv[5])) == 0) {
printf("Error: Couldn't load %s\n", argv[5]);
return -1;
}
gray_image = cvCreateImage(cvGetSize(image), 8, 1);
cvCvtColor(image, gray_image, CV_BGR2GRAY);
//UNDISTORT OUR IMAGE
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);
//GET THE CHECKERBOARD ON THE PLANE
cvNamedWindow("Checkers");
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 checkerboard 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));
//GET THE IMAGE AND OBJECT POINTS:
//Object points are at (r,c): (0,0), (board_w-1,0), (0,board_h-1), (board_w-1,board_h-1)
//That means corners are at: corners[r*board_w + c]
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];
//DRAW THE POINTS in order: B,G,R,YELLOW
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);
//DRAW THE FOUND CHECKERBOARD
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
cvShowImage("Checkers", image);
//FIND THE HOMOGRAPHY
CvMat *H = cvCreateMat(3, 3, CV_32F);
CvMat *H_invt = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(objPts, imgPts, H);
//LET THE USER ADJUST THE Z HEIGHT OF THE VIEW
float Z = 25;
int key = 0;
IplImage *birds_image = cvCloneImage(image);
cvNamedWindow("Birds_Eye");
while (key != 27) { //escape key stops
CV_MAT_ELEM(*H, float, 2, 2) = Z;
// cvInvert(H,H_invt); //If you want to invert the homography directly
// cvWarpPerspective(image,birds_image,H_invt,CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS );
//USE HOMOGRAPHY TO REMAP THE VIEW
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;
}
//SHOW ROTATION AND TRANSLATION VECTORS
CvMat *image_points = cvCreateMat(4, 1, CV_32FC2);
CvMat *object_points = cvCreateMat(4, 1, CV_32FC3);
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);
}
CvMat *RotRodrigues = cvCreateMat(3, 1, CV_32F);
CvMat *Rot = cvCreateMat(3, 3, CV_32F);
CvMat *Trans = cvCreateMat(3, 1, CV_32F);
cvFindExtrinsicCameraParams2(object_points, image_points,
intrinsic, distortion, RotRodrigues, Trans);
cvRodrigues2(RotRodrigues, Rot);
//SAVE AND EXIT
cvSave("Rot.xml", Rot);
cvSave("Trans.xml", Trans);
cvSave("H.xml", H);
cvInvert(H, H_invt);
cvSave("H_invt.xml", H_invt); //Bottom row of H invert is horizon line
return 0;
}
RTC::ReturnCode_t ImageCalibration::onExecute(RTC::UniqueId ec_id)
{
board_sz = cvSize(m_board_w, m_board_h);
//Calibrationパターンを計算する。
if (m_inputImageIn.isNew()) {
m_inputImageIn.read();
if(m_keyIn.isNew()){
m_keyIn.read();
key = (int)m_key.data;
}
if(g_temp_w != m_inputImage.width || g_temp_h != m_inputImage.height){
inputImage_buff = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
outputImage_buff = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
tempImage_buff = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
undistortionImage = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
birds_image = cvCreateImage(cvSize(m_inputImage.width, m_inputImage.height), 8, 3);
intrinsicMatrix = cvCreateMat(3,3,CV_64FC1);
distortionCoefficient = cvCreateMat(4,1,CV_64FC1);
captureCount = 0;
findFlag = 0;
mapx = cvCreateImage( cvSize(m_inputImage.width, m_inputImage.height), IPL_DEPTH_32F, 1);
mapy = cvCreateImage( cvSize(m_inputImage.width, m_inputImage.height), IPL_DEPTH_32F, 1);
corners = new CvPoint2D32f[m_board_w * m_board_h];
g_temp_w = m_inputImage.width;
g_temp_h = m_inputImage.height;
}
//Capture開始する。
memcpy(inputImage_buff->imageData,(void *)&(m_inputImage.pixels[0]), m_inputImage.pixels.length());
// tempImage_buff = cvCloneImage(inputImage_buff);
//OutPortに出力する。
int len = inputImage_buff->nChannels * inputImage_buff->width * inputImage_buff->height;
m_origImage.pixels.length(len);
memcpy((void *)&(m_origImage.pixels[0]), inputImage_buff->imageData, len);
m_origImage.width = inputImage_buff->width;
m_origImage.height = inputImage_buff->height;
m_origImageOut.write();
//Capture確認用のWindowの生成
//cvShowImage("Capture", inputImage_buff);
cvWaitKey(1);
//SpaceBarを押すとサンプル映像5枚を撮る
if (key == ' ') {
tempImage_buff = cvCloneImage(inputImage_buff);
//映像を生成する
IplImage *grayImage = cvCreateImage(cvGetSize(tempImage_buff), 8, 1);
//行列の生成
CvMat *worldCoordinates = cvCreateMat((m_board_w * m_board_h) * NUM_OF_BACKGROUND_FRAMES, 3, CV_64FC1); //世界座標用行列
CvMat *imageCoordinates = cvCreateMat((m_board_w * m_board_h) * NUM_OF_BACKGROUND_FRAMES ,2, CV_64FC1); //画像座標用行列
CvMat *pointCounts = cvCreateMat(NUM_OF_BACKGROUND_FRAMES, 1, CV_32SC1); //コーナー数の行列
CvMat *rotationVectors = cvCreateMat(NUM_OF_BACKGROUND_FRAMES, 3, CV_64FC1); //回転ベクトル
CvMat *translationVectors = cvCreateMat(NUM_OF_BACKGROUND_FRAMES, 3, CV_64FC1);
//世界座標を設定する
for (int i = 0; i < NUM_OF_BACKGROUND_FRAMES; i++){
for ( int j = 0; j < (m_board_w * m_board_h); j++) {
cvSetReal2D(worldCoordinates, i * (m_board_w * m_board_h) + j, 0, (j % m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i * (m_board_w * m_board_h) + j, 1, (j / m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i * (m_board_w * m_board_h) + j, 2, 0.0);
}
}
//コーナー数を設定
for(int i = 0; i < NUM_OF_BACKGROUND_FRAMES; i++){
cvSetReal2D(pointCounts, i, 0, (m_board_w * m_board_h));
}
//コーナーを検出する。
findFlag = findCorners(tempImage_buff, grayImage, corners);
if (findFlag != 0) {
//コーナーをすべて検出した場合
//映像座標を設定する。
for (;;){
for (int i = 0; i < (m_board_w * m_board_h); i++){
cvSetReal2D(imageCoordinates, captureCount * (m_board_w * m_board_h) + i, 0, corners[i].x);
cvSetReal2D(imageCoordinates, captureCount * (m_board_w * m_board_h) + i, 1, corners[i].y);
}
captureCount++;
printf("%d枚目キャプチャしました\n", captureCount);
if (captureCount == NUM_OF_BACKGROUND_FRAMES) {
//設定した回数チェックパターンを撮った場合
//カメラパラメータを推定する。
cvCalibrateCamera2(
worldCoordinates,
imageCoordinates,
pointCounts,
cvGetSize(inputImage_buff),
intrinsicMatrix,
distortionCoefficient,
rotationVectors,
translationVectors,
CALIBRATE_CAMERA_FLAG
);
//情報をTextとして出力
printf("\nレンズ歪み係数\n");
saveRenseMatrix(distortionCoefficient);
printMatrix("%lf", distortionCoefficient);
//m_renseParameter.data = renseParameters;
printf("\n内部パラメータ\n");
saveInternalParameterMatrix(intrinsicMatrix);
printMatrix("%lf ", intrinsicMatrix);
//m_internalParameter.data = internalParameter;
captureCount = 0;
break;
}
}
}
if (findFlag != 0){
InParameter = 1;
}else if (findFlag == 0) {
InParameter = 0;
}
//メモリ解除
cvReleaseMat(&worldCoordinates);
cvReleaseMat(&imageCoordinates);
cvReleaseMat(&pointCounts);
cvReleaseMat(&rotationVectors);
cvReleaseMat(&translationVectors);
cvReleaseImage(&grayImage);
}
g_temp_w = m_inputImage.width;
g_temp_h = m_inputImage.height;
}
//外部パターンを取得
if (key == ' ' && m_inputImageIn.isNew() && InParameter == 1) {
//行列の生成
CvMat *worldCoordinates = cvCreateMat((m_board_w * m_board_h), 3, CV_64FC1); //世界座標用行列
CvMat *imageCoordinates = cvCreateMat((m_board_w * m_board_h), 2, CV_64FC1); //画像座標用行列
CvMat *rotationVectors = cvCreateMat(1, 3, CV_64FC1); //回転ベクトル
CvMat *rotationMatrix = cvCreateMat(3, 3, CV_64FC1); //回転行列
CvMat *translationVectors = cvCreateMat(1, 3, CV_64FC1);
//世界座標を設定する
for (int i = 0; i < (m_board_w * m_board_h); i++){
cvSetReal2D(worldCoordinates, i, 0, (i % m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i, 1, (i / m_board_w) * UNIT);
cvSetReal2D(worldCoordinates, i, 2, 0.0);
}
cvWaitKey( 1 );
// スペースキーが押されたら
if ( findFlag != 0 ) {
// コーナーがすべて検出された場合
// 画像座標を設定する
for ( int i = 0; i < (m_board_w * m_board_h); i++ ){
cvSetReal2D( imageCoordinates, i, 0, corners[i].x);
cvSetReal2D( imageCoordinates, i, 1, corners[i].y);
}
// 外部パラメータを推定する
cvFindExtrinsicCameraParams2(
worldCoordinates,
imageCoordinates,
intrinsicMatrix,
distortionCoefficient,
rotationVectors,
translationVectors
);
// 回転ベクトルを回転行列に変換する
cvRodrigues2( rotationVectors, rotationMatrix, NULL );
printf( "\n外部パラメータ\n" );
printExtrinsicMatrix( rotationMatrix, translationVectors );
saveExternalParameterMatrix(rotationMatrix, translationVectors);
m_externalParameter.data = CORBA::string_dup(externalParameter);
m_renseParameter.data = CORBA::string_dup(renseParameters);
m_internalParameter.data = CORBA::string_dup(internalParameter);
}
//メモリを解放
cvReleaseMat( &worldCoordinates );
cvReleaseMat( &imageCoordinates );
cvReleaseMat( &rotationVectors );
cvReleaseMat( &rotationMatrix );
cvReleaseMat( &translationVectors );
//X,Y初期化
cvInitUndistortMap(
intrinsicMatrix,
distortionCoefficient,
mapx,
mapy
);
//外部パラメータ確認フラグ
outParameter = 1;
key = 0;
}
//内部外部パラメータの出力に成功したら
if (InParameter == 1 && outParameter == 1) {
// レンズ歪みを補正した画像を生成する
cvUndistort2(
inputImage_buff,
undistortionImage,
intrinsicMatrix,
distortionCoefficient
);
//cvShowImage("歪み補正", undistortionImage);
//OutPortに補正映像を出力する。
//int len = undistortionImage->nChannels * undistortionImage->width * undistortionImage->height;
//m_calbImage.pixels.length(len);
//歪み補正映像をOutPortとしてメモリコピーする。
//memcpy((void *)&(m_calbImage.pixels[0]), undistortionImage->imageData, len);
//m_calbImageOut.write();
//鳥瞰図の座標設定
objPts[0].x = 0; objPts[0].y = 0;
objPts[1].x = m_board_w-1; objPts[1].y = 0;
objPts[2].x = 0; objPts[2].y = m_board_h-1;
objPts[3].x = m_board_w-1; objPts[3].y = m_board_h-1;
//取得するCornerを設定
imgPts[0] = corners[0];
imgPts[1] = corners[m_board_w - 1];
imgPts[2] = corners[(m_board_h - 1) * m_board_w];
imgPts[3] = corners[(m_board_h - 1) * m_board_w + m_board_w - 1];
//指定したCornerに○を作成する
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[0]), 9, CV_RGB(0,0,255), 3);
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[1]), 9, CV_RGB(0,255,0), 3);
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[2]), 9, CV_RGB(255,0,0), 3);
cvCircle(tempImage_buff, cvPointFrom32f(imgPts[3]), 9, CV_RGB(255,255,0), 3);
CvMat *H = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(objPts, imgPts, H);
//高さを設定する。
CV_MAT_ELEM(*H, float, 2, 2) = m_camera_Height;
//Warppingを実行
cvWarpPerspective(inputImage_buff, birds_image, H, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
//鳥瞰図をOutPortに出力する。
int len = birds_image->nChannels * birds_image->width * birds_image->height;
m_birdImage.pixels.length(len);
memcpy((void *)&(m_birdImage.pixels[0]), birds_image->imageData, len);
m_birdImage.width = inputImage_buff->width;
m_birdImage.height = inputImage_buff->height;
m_birdImageOut.write();
cvWaitKey(10);
//cvShowImage("Bird_Eye", birds_image);
cvReleaseMat(&H);
g_temp_w = m_inputImage.width;
g_temp_h = m_inputImage.height;
key = 0;
}
