uvsim: /home/jgoppert/Projects/uvsim/uvsim/src/uvsim/sensing/DepthEngine.cc Source File

00001 /*
00002  * DepthEngine.cc
00003  * Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
00004  *
00005  * DepthEngine.cc is free software: you can redistribute it and/or modify it
00006  * under the terms of the GNU General Public License as published by the
00007  * Free Software Foundation, either version 3 of the License, or
00008  * (at your option) any later version.
00009  *
00010  * DepthEngine.cc is distributed in the hope that it will be useful, but
00011  * WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00013  * See the GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License along
00016  * with this program.  If not, see <http://www.gnu.org/licenses/>.
00017  */
00018 
00019 #include "DepthEngine.h"
00020 #include <iostream>
00021 
00022 namespace uvsim
00023 {
00024 
00025 DepthEngine::DepthEngine(IplImage * frame): image(), grey(), prevGrey(), pyramid(), prevPyramid(), swapTemp(), eig(), temp(), frame(frame)
00026 {
00027 
00028 
00029     image = cvCreateImage(cvGetSize(frame), 8, 3);
00030     prevImage = cvCreateImage(cvGetSize(frame), 8, 3);
00031     rectifiedImage = cvCreateImage(cvGetSize(frame), 8, 1);
00032     prevRectifiedImage = cvCreateImage(cvGetSize(frame), 8, 1);
00033 
00034     image->origin = frame->origin;
00035     grey = cvCreateImage(cvGetSize(frame), 8, 1);
00036     prevGrey = cvCreateImage(cvGetSize(frame), 8, 1);
00037     pyramid = cvCreateImage(cvGetSize(frame), 8, 1 );
00038     prevPyramid = cvCreateImage( cvGetSize(frame), 8, 1 );
00039     points[0] = (CvPoint2D32f*)cvAlloc(maxCount*sizeof(points[0][0]));
00040     points[1] = (CvPoint2D32f*)cvAlloc(maxCount*sizeof(points[0][0]));
00041     status = (char*)cvAlloc(maxCount);
00042     flags = 0;
00043     eig = cvCreateImage( cvGetSize(grey), 32, 1 );
00044     temp = cvCreateImage( cvGetSize(grey), 32, 1 );
00045     firstRun = true;
00046     fundamentalMatrix = cvCreateMat(3,3,CV_32FC1);
00047     H1 = cvCreateMat(3,3, CV_32FC1);
00048     H2 = cvCreateMat(3,3, CV_32FC1);
00049 
00050 //Single camera so intrinsics are the same
00051 
00052     M1 = (CvMat*)cvLoad("Intrinsics.xml");
00053     M2 = (CvMat*)cvLoad("Intrinsics.xml");
00054 //Single camera so distCoeffs are the same
00055     D1 = (CvMat*)cvLoad("Distortion.xml");
00056     D2 = (CvMat*)cvLoad("Distortion.xml");
00057 
00058     iM = cvCreateMat(3,3, CV_32FC1);
00059 
00060     R1 = cvCreateMat(3,3, CV_32FC1);
00061     R2 = cvCreateMat(3,3, CV_32FC1);
00062 
00063     mx1 = cvCreateMat(frame->height,frame->width, CV_32FC1);
00064     my1 = cvCreateMat(frame->height,frame->width, CV_32FC1);
00065     mx2 = cvCreateMat(frame->height,frame->width, CV_32FC1);
00066     my2 = cvCreateMat(frame->height,frame->width, CV_32FC1);
00067     pair = cvCreateMat( frame->height, frame->width*2, CV_8UC3 );
00068 
00069 //Initalize matrices
00070     //cvSetIdentity(M1);
00071     //cvSetIdentity(M2);
00072     //cvZero(D1);
00073     //cvZero(D2);
00074 
00075     imageSize = cvGetSize(image);
00076 }
00077 
00078 DepthEngine::~DepthEngine()
00079 {
00080     // release images
00081     if (image) cvReleaseImage(&image);
00082     if (prevImage) cvReleaseImage(&prevImage);
00083     if (rectifiedImage) cvReleaseImage(&rectifiedImage);
00084     if (prevRectifiedImage) cvReleaseImage(&prevRectifiedImage);
00085     if (grey) cvReleaseImage(&grey);
00086     if (prevGrey) cvReleaseImage(&prevGrey);
00087     if (pyramid) cvReleaseImage(&pyramid);
00088     if (prevPyramid) cvReleaseImage(&prevPyramid);
00089     if (eig) cvReleaseImage(&eig);
00090     if (temp) cvReleaseImage(&temp);
00091 
00092     // release arrays
00093     if (points[0]) cvFree(&points[0]);
00094     if (points[1]) cvFree(&points[1]);
00095     if (status) cvFree(&status);
00096 
00097     //Release Matrices
00098 
00099     if (H1) cvReleaseMat(&H1);
00100     if (H2) cvReleaseMat(&H2);
00101     if (fundamentalMatrix) cvReleaseMat(&fundamentalMatrix);
00102     if (M1) cvReleaseMat(&M1);
00103     if (M2) cvReleaseMat(&M2);
00104     if (D1) cvReleaseMat(&D1);
00105     if (D2) cvReleaseMat(&D2);
00106     if (iM) cvReleaseMat(&iM);
00107     if (R1) cvReleaseMat(&R1);
00108     if (R2) cvReleaseMat(&R2);
00109     if (mx1) cvReleaseMat(&mx1);
00110     if (my1) cvReleaseMat(&my1);
00111     if (mx2) cvReleaseMat(&mx2);
00112     if (my2) cvReleaseMat(&my2);
00113     if (pair) cvReleaseMat(&pair);
00114 
00115 }
00116 
00117 void DepthEngine::init()
00118 {
00119     std::cout << "intializing features" << std::endl;
00120     count = maxCount;
00121     cvGoodFeaturesToTrack( grey, eig, temp, points[1], &count,
00122                            quality, minDistance, 0, 3, 0, 0.04 );
00123     cvFindCornerSubPix( grey, points[1], count,
00124                         cvSize(winSize,winSize), cvSize(-1,-1),
00125                         cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
00126     std::cout << count << " features found." << std::endl;
00127 
00128 
00129 }
00130 
00131 void DepthEngine::calculateOpticalFlow()
00132 {
00133     // optical flow
00134 
00135     cvCalcOpticalFlowPyrLK(prevGrey, grey, prevPyramid, pyramid, points[0],
00136                            points[1], count, cvSize(winSize, winSize), 3, status, 0,
00137                            cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03), flags);
00138     flags |= CV_LKFLOW_PYR_A_READY;
00139 
00140     // rearrange points array
00141     int i,k;
00142     for (i = k = 0; i<count; i++)
00143     {
00144         if (!status[i]) continue; // ignore unmatched features
00145         points[1][k++] = points[1][i]; // moves points to start of array
00146     }
00147     count = k;
00148 }
00149 
00150 void DepthEngine::calculateDepth()
00151 {
00152     CvMat imagePoints1 = cvMat(1,count,CV_32FC2, &points[0][0]);
00153     CvMat imagePoints2 = cvMat(1,count,CV_32FC2, &points[1][0]);
00154 
00155 
00156     //Undistort Image points, does this need to be here?????????????
00157     cvUndistortPoints(&imagePoints1, &imagePoints1, M1, D1, 0, 0 );
00158     cvUndistortPoints(&imagePoints2, &imagePoints2, M2, D2, 0, 0 );
00159 
00160 
00161     CvMat* statusFundamental = cvCreateMat(1,count,CV_8UC1);
00162     int fm_count = cvFindFundamentalMat(&imagePoints1, &imagePoints2, fundamentalMatrix, CV_FM_RANSAC,1.0,0.99,statusFundamental);
00163     cvStereoRectifyUncalibrated(&imagePoints1, &imagePoints2, fundamentalMatrix,
00164                                 imageSize, H1, H2, 3);
00165 
00166     std::cout<<"Fundamental Matrix count- "<<fm_count<<std::endl;
00167 
00168     cvInvert(M1, iM);
00169     cvMatMul(H1, M1, R1);
00170     cvMatMul(iM, R1, R1);
00171     cvInvert(M2, iM);
00172     cvMatMul(H2, M2, R2);
00173     cvMatMul(iM, R2, R2);
00174 
00175     //Precompute map for cvRemap()
00176     cvInitUndistortRectifyMap(M1,D1,R1,M1,mx1,my1);
00177     cvInitUndistortRectifyMap(M2,D2,R2,M2,mx2,my2);
00178     cvRemap( grey, rectifiedImage, mx1, my1 );
00179     cvRemap( prevGrey, prevRectifiedImage, mx2, my2 );
00180     CvMat part;
00181 
00182     cvGetCols( pair, &part, 0, frame->width );
00183     cvCvtColor( prevRectifiedImage, &part, CV_GRAY2BGR );
00184     cvGetCols( pair, &part, frame->width, frame->width*2 );
00185     cvCvtColor( rectifiedImage, &part, CV_GRAY2BGR );
00186     for ( int j = 0; j < frame->height; j += 16 )
00187         cvLine( pair, cvPoint(0,j),
00188                 cvPoint(frame->width*2,j),
00189                 CV_RGB(0,255,0));
00190 
00191 
00192 
00193 //for (int i = 0; i< H1->rows; i++)
00194     //{
00195     //for (int j = 0; j< H1.cols; j++)
00196     //{
00197     //std::cout << i << " " << j << " " << H1.data.fl[i+H1.rows*j] << std::endl;
00198     //}
00199     //}
00200 
00201     cvSave("StatusFundamental.xml",statusFundamental);
00202 
00203     cvSave("M1.xml",M1);
00204     cvSave("M2.xml",M2);
00205     cvSave("H1.xml",H1);
00206     cvSave("H2.xml",H2);
00207     cvSave("fundamentalMatrix.xml",fundamentalMatrix);
00208     cvReleaseMat(&statusFundamental);
00209 }
00210 
00211 
00212 void DepthEngine::update(bool initialize)
00213 {
00214     // std::cout << "updating depth" << std::endl;
00215 
00216     // update data for next step
00217     CV_SWAP( prevImage, image, swapTemp );
00218     CV_SWAP( prevGrey, grey, swapTemp );
00219     CV_SWAP( prevPyramid, pyramid, swapTemp );
00220     CV_SWAP( points[0], points[1], swapPoints );
00221 
00222     // copy image
00223     cvCopy(frame, image, 0);
00224 
00225     cvCvtColor(image, grey, CV_BGR2GRAY);
00226 
00227 
00228 
00229     // initialize if requested
00230     if (initialize || firstRun)
00231     {
00232         //image = cvLoadImage("left01.jpg", 0);
00233         //prevImage = cvLoadImage("right01.jpg", 0);
00234 
00235         //grey = cvLoadImage("left01.jpg", 0);
00236         //prevGrey = cvLoadImage("right01.jpg", 0);
00237         init();
00238         firstRun = false;
00239     }
00240     else
00241     {
00242         calculateOpticalFlow();
00243 
00244     }
00245     drawPoints();
00246 }
00247 
00248 void DepthEngine::drawPoints()
00249 {
00250 
00251     for (int i = 0; i<count; i++)
00252     {
00253         //std::cout << "feature " << i << " :  " << points[1][i].x << ", " << points[1][i].y << std::endl;
00254         cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
00255         cvCircle( prevImage, cvPointFrom32f(points[0][i]), 3, CV_RGB(0,0,255), -1, 8,0);
00256     }
00257 
00258 }
00259 
00260 }
00261 
00262 // vim:ts=4:sw=4