HeliPose.C

00001 /*!@file Devices/HeliPose.C read HeliPose data  */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/HeliBot/HeliPose.C $
00035 // $Id: HeliPose.C 12962 2010-03-06 02:13:53Z irock $
00036 //
00037 
00038 #include "Robots/HeliBot/HeliPose.H"
00039 
00040 #include "Component/ParamMap.H"
00041 #include "Util/Assert.H"
00042 #include "Util/Types.H"
00043 #include "Util/log.H"
00044 #include "Image/ColorOps.H"
00045 #include "Image/DrawOps.H"
00046 
00047 #include <cmath>
00048 #include <unistd.h>
00049 
00050 #ifdef HAVE_OPENCV
00051 namespace
00052 {
00053   const int thresh = 50;
00054 
00055   // helper function:
00056   // finds a cosine of angle between vectors
00057   // from pt0->pt1 and from pt0->pt2
00058   double angle( CvPoint* pt1, CvPoint* pt2, CvPoint* pt0 )
00059   {
00060     double dx1 = pt1->x - pt0->x;
00061     double dy1 = pt1->y - pt0->y;
00062     double dx2 = pt2->x - pt0->x;
00063     double dy2 = pt2->y - pt0->y;
00064     return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
00065   }
00066 
00067   CvSeq* findSquares(const Image<PixRGB<byte> >& in, CvMemStorage* storage,
00068                   const int minarea, const int maxarea, const double mincos)
00069   {
00070     const int N = 11;
00071 
00072     IplImage* img = img2ipl(in);
00073 
00074     CvSize sz = cvSize( img->width & -2, img->height & -2 );
00075     IplImage* timg = cvCloneImage( img ); // make a copy of input image
00076     IplImage* gray = cvCreateImage( sz, 8, 1 );
00077     IplImage* pyr = cvCreateImage( cvSize(sz.width/2, sz.height/2), 8, 3 );
00078     // create empty sequence that will contain points -
00079     // 4 points per square (the square's vertices)
00080     CvSeq* squares = cvCreateSeq( 0, sizeof(CvSeq), sizeof(CvPoint), storage );
00081 
00082     // select the maximum ROI in the image
00083     // with the width and height divisible by 2
00084     cvSetImageROI( timg, cvRect( 0, 0, sz.width, sz.height ));
00085 
00086     // down-scale and upscale the image to filter out the noise
00087     cvPyrDown( timg, pyr, 7 );
00088     cvPyrUp( pyr, timg, 7 );
00089     IplImage* tgray = cvCreateImage( sz, 8, 1 );
00090 
00091     // find squares in every color plane of the image
00092     for (int c = 0; c < 3; ++c)
00093       {
00094         // extract the c-th color plane
00095         cvSetImageCOI( timg, c+1 );
00096         cvCopy( timg, tgray, 0 );
00097 
00098         // try several threshold levels
00099         for (int l = 0; l < N; ++l)
00100           {
00101             // hack: use Canny instead of zero threshold level.
00102             // Canny helps to catch squares with gradient shading
00103             if( l == 0 )
00104               {
00105                 // apply Canny. Take the upper threshold from slider
00106                 // and set the lower to 0 (which forces edges merging)
00107                 cvCanny( tgray, gray, 0, thresh, 5 );
00108                 // dilate canny output to remove potential
00109                 // holes between edge segments
00110                 cvDilate( gray, gray, 0, 1 );
00111               }
00112             else
00113               {
00114                 // apply threshold if l!=0:
00115                 //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
00116                 cvThreshold( tgray, gray, (l+1)*255/N, 255, CV_THRESH_BINARY );
00117               }
00118 
00119             // find contours and store them all as a list
00120             CvSeq* contours = 0;
00121             cvFindContours( gray, storage, &contours, sizeof(CvContour),
00122                             CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
00123 
00124             // test each contour
00125             while( contours )
00126               {
00127                 // approximate contour with accuracy proportional
00128                 // to the contour perimeter
00129                 CvSeq* result =
00130                   cvApproxPoly( contours, sizeof(CvContour), storage,
00131                                 CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 );
00132                 // square contours should have 4 vertices after approximation
00133                 // relatively large area (to filter out noisy contours)
00134                 // and be convex.
00135                 // Note: absolute value of an area is used because
00136                 // area may be positive or negative - in accordance with the
00137                 // contour orientation
00138                 const double area = fabs(cvContourArea(result,CV_WHOLE_SEQ));
00139                 if (result->total == 4 &&
00140                     area >= minarea && area <= maxarea &&
00141                     cvCheckContourConvexity(result))
00142                   {
00143                     double s = 0;
00144 
00145                     for (int i = 0; i < 4; ++i)
00146                       {
00147                         // find minimum angle between joint
00148                         // edges (maximum of cosine)
00149                         const double t =
00150                           fabs(angle((CvPoint*)cvGetSeqElem( result, i % 4 ),
00151                                      (CvPoint*)cvGetSeqElem( result, (i-2) % 4 ),
00152                                      (CvPoint*)cvGetSeqElem( result, (i-1) % 4 )));
00153                         s = s > t ? s : t;
00154                       }
00155 
00156 
00157                     // if cosines of all angles are small
00158                     // (all angles are ~90 degree) then write quandrangle
00159                     // vertices to resultant sequence
00160                     if (s < mincos)
00161                     {
00162                       for (int i = 0; i < 4; ++i)
00163                         cvSeqPush(squares,
00164                                   (CvPoint*)cvGetSeqElem( result, i ));
00165                       //LINFO("area=%f, mincos=%f", area, s);
00166                     }
00167                   }
00168 
00169                 // take the next contour
00170                 contours = contours->h_next;
00171               }
00172           }
00173       }
00174 
00175     // release all the temporary images
00176     cvReleaseImage( &gray );
00177     cvReleaseImage( &pyr );
00178     cvReleaseImage( &tgray );
00179     cvReleaseImage( &timg );
00180     cvReleaseImageHeader( &img );
00181 
00182     return squares;
00183   }
00184 
00185   // the function draws all the squares in the image
00186   Image<PixRGB<byte> > drawSquares(const Image<PixRGB<byte> >&in, CvSeq* squares)
00187   {
00188     Image<PixRGB<byte> > out(in);
00189 
00190     CvSeqReader reader;
00191 
00192     // initialize reader of the sequence
00193     cvStartReadSeq(squares, &reader, 0);
00194 
00195     // read 4 sequence elements at a time (all vertices of a square)
00196     for (int i = 0; i < squares->total; i += 4)
00197       {
00198         CvPoint pt[4];
00199 
00200         // read 4 vertices
00201         CV_READ_SEQ_ELEM( pt[0], reader );
00202         CV_READ_SEQ_ELEM( pt[1], reader );
00203         CV_READ_SEQ_ELEM( pt[2], reader );
00204         CV_READ_SEQ_ELEM( pt[3], reader );
00205 
00206         for (int j = 0; j < 4; ++j)
00207           drawLine(out,
00208                    Point2D<int>(pt[j].x, pt[j].y),
00209                    Point2D<int>(pt[(j+1)%4].x, pt[(j+1)%4].y),
00210                    PixRGB<byte>(0, 255, 0),
00211                    2);
00212       }
00213 
00214     return out;
00215   }
00216 
00217   std::vector<Point2D<int> > getRectangle(CvSeq* cards)
00218   {
00219     CvSeqReader reader;
00220     // initialize reader of the sequence
00221     cvStartReadSeq( cards, &reader, 0 );
00222 
00223     std::vector<Point2D<int> > corners;
00224 
00225     if (cards->total > 0)
00226     {
00227       CvPoint pt[4];
00228       // read 4 vertices
00229       CV_READ_SEQ_ELEM( pt[0], reader );
00230       CV_READ_SEQ_ELEM( pt[1], reader );
00231       CV_READ_SEQ_ELEM( pt[2], reader );
00232       CV_READ_SEQ_ELEM( pt[3], reader );
00233 
00234       corners.push_back(Point2D<int>(pt[0].x, pt[0].y));
00235       corners.push_back(Point2D<int>(pt[1].x, pt[1].y));
00236       corners.push_back(Point2D<int>(pt[2].x, pt[2].y));
00237       corners.push_back(Point2D<int>(pt[3].x, pt[3].y));
00238 
00239     }
00240 
00241     return corners;
00242   }
00243 }
00244 
00245 #endif //HAVE_OPENCV
00246 
00247 
00248 void* HeliPose_run(void *r0); // will live in a separate thread
00249 
00250 // ######################################################################
00251 void* HeliPose_run(void *r0)
00252 {
00253   HeliPose *r = (HeliPose *)r0;
00254   //r->run_cameraPose(); return NULL;
00255   r->run_imuPose(); return NULL;
00256 }
00257 
00258 // ######################################################################
00259 HeliPose::HeliPose(OptionManager& mgr,
00260     nub::ref<InputFrameSeries>& ifs,
00261     nub::ref<OutputFrameSeries>& ofs,
00262     const std::string& descrName,
00263     const std::string& tagName) :
00264   ModelComponent(mgr, descrName, tagName),
00265   itsIfs(ifs),
00266   itsOfs(ofs),
00267   itsIMU(new IMU_SFE_Atomic(mgr))
00268 {
00269   addSubComponent(itsIMU);
00270 
00271   running = false;
00272   itsDebug = true;
00273 
00274   pthread_mutex_init(&itsPoseLock, NULL);
00275 
00276   //pthread_mutex_init(&itsImgLock, NULL);
00277 
00278   //itsStorage = cvCreateMemStorage(0);
00279   //itsIntrinsicMatrix = cvCreateMat( 3, 3, CV_32FC1);
00280   //itsDistortionCoeffs = cvCreateMat( 4, 1, CV_32FC1);
00281   //itsCameraRotation = cvCreateMat( 1, 3, CV_64FC1);
00282   //itsCameraTranslation = cvCreateMat( 1, 3, CV_64FC1);
00283 
00284 
00285   //cvmSet(itsDistortionCoeffs, 0, 0, -0.1033503  );
00286   //cvmSet(itsDistortionCoeffs, 1, 0, 0.0095519 );
00287   //cvmSet(itsDistortionCoeffs, 2, 0, -0.0163438  );
00288   //cvmSet(itsDistortionCoeffs, 3, 0, -0.0204184  );
00289 
00290   //cvmSet(itsIntrinsicMatrix, 0, 0, 64.10168); cvmSet(itsIntrinsicMatrix, 0, 1, 0); cvmSet(itsIntrinsicMatrix, 0, 2, 159.50000);
00291   //cvmSet(itsIntrinsicMatrix, 1, 0, 0); cvmSet(itsIntrinsicMatrix, 1, 1, 71.64519 ); cvmSet(itsIntrinsicMatrix, 1, 2, 119.5);
00292   //cvmSet(itsIntrinsicMatrix, 2, 0, 0); cvmSet(itsIntrinsicMatrix, 2, 1, 0); cvmSet(itsIntrinsicMatrix, 2, 2, 1);
00293 
00294   //cvmSet(itsDistortionCoeffs, 0, 0, -0.4240642  );
00295   //cvmSet(itsDistortionCoeffs, 1, 0, -1.9482374 );
00296   //cvmSet(itsDistortionCoeffs, 2, 0, -0.0267558  );
00297   //cvmSet(itsDistortionCoeffs, 3, 0, -0.0132413  );
00298 
00299   //cvmSet(itsIntrinsicMatrix, 0, 0, 841.03479); cvmSet(itsIntrinsicMatrix, 0, 1, 0); cvmSet(itsIntrinsicMatrix, 0, 2, 159.50000);
00300   //cvmSet(itsIntrinsicMatrix, 1, 0, 0); cvmSet(itsIntrinsicMatrix, 1, 1, 861.99316 ); cvmSet(itsIntrinsicMatrix, 1, 2, 119.5);
00301   //cvmSet(itsIntrinsicMatrix, 2, 0, 0); cvmSet(itsIntrinsicMatrix, 2, 1, 0); cvmSet(itsIntrinsicMatrix, 2, 2, 1);
00302 
00303   itsCurrentPose.translation = Point3D<float>(0,0,0);
00304   itsCurrentPose.velocity = Point3D<float>(0,0,0);
00305   itsCurrentPose.rotation = Point3D<float>(0,0,0);
00306 
00307   itsVelocityScale.x = 0.1;
00308   itsVelocityScale.y = 1;
00309   itsVelocityScale.z = 1;
00310   itsRotationScale.x = 1;
00311   itsRotationScale.y = 1;
00312   itsRotationScale.z = M_PI/((17870-12360)*2); //units per 90 degrees
00313 
00314   itsVelocityBias = Point3D<float>(0,0,0);
00315   itsRotationBias = Point3D<float>(0,0,0);
00316   itsVelocitySigma = Point3D<float>(0,0,0);
00317   itsVelocitySigma = Point3D<float>(0,0,0);
00318 
00319 
00320 }
00321 
00322 // ######################################################################
00323 void HeliPose::start1()
00324 {
00325 }
00326 
00327 // ######################################################################
00328 void HeliPose::start2()
00329 {
00330   // start thread for run():
00331   pthread_create(&runner, NULL, &HeliPose_run, (void *)this);
00332 }
00333 
00334 // ######################################################################
00335 void HeliPose::stop1()
00336 {
00337   // stop our thread:
00338   running = false; while(running == false) usleep(5);
00339   usleep(50); running = false;
00340 }
00341 
00342 // ######################################################################
00343 HeliPose::~HeliPose()
00344 {
00345   cvReleaseMemStorage(&itsStorage);
00346   pthread_mutex_destroy(&itsPoseLock);
00347   //pthread_mutex_destroy(&itsImgLock);
00348 }
00349 
00350 // ######################################################################
00351 std::vector<Point2D<int> > HeliPose::getExtrinsic(Image<byte>& img)
00352 {
00353   int rows = 3;
00354   int cols = 4;
00355   std::vector<CvPoint2D32f> corners(rows*cols);
00356 
00357   int count = 0;
00358   int result = cvFindChessboardCorners(img2ipl(img),
00359       cvSize(rows,cols),
00360       &corners[0], &count,
00361       CV_CALIB_CB_ADAPTIVE_THRESH |
00362       CV_CALIB_CB_NORMALIZE_IMAGE |
00363       CV_CALIB_CB_FILTER_QUADS);
00364 
00365   // result = 0 if not all corners were found
00366   // Find corners to an accuracy of 0.1 pixel
00367   if(result != 0)
00368   {
00369     cvFindCornerSubPix(img2ipl(img),
00370         &corners[0],
00371         count,
00372         cvSize(10,10), //win
00373         cvSize(-1,-1), //zero_zone
00374         cvTermCriteria(CV_TERMCRIT_ITER,1000,0.01) );
00375     //cvDrawChessboardCorners(img2ipl(img), cvSize(rows,cols), &corners[0], count, result);
00376 
00377     //Get
00378     CvMat *image_points_ex = cvCreateMat( corners.size(), 2, CV_64FC1);
00379 
00380     for (uint j = 0; j < corners.size(); j++){
00381       cvSetReal2D( image_points_ex, j, 0, corners[j].x);
00382       cvSetReal2D( image_points_ex, j, 1, corners[j].y);
00383     }
00384 
00385     CvMat *object_points_ex = cvCreateMat( corners.size(), 3, CV_64FC1);
00386     for (uint j = 0; j < corners.size(); j++){
00387       cvSetReal2D( object_points_ex, j, 0, ( j % rows) * 50 ); //0.4m
00388       cvSetReal2D( object_points_ex, j, 1, ( j / rows) * 50 );
00389       cvSetReal2D( object_points_ex, j, 2, 0.0 );
00390     }
00391 
00392     cvFindExtrinsicCameraParams2( object_points_ex,
00393         image_points_ex,
00394         itsIntrinsicMatrix,
00395         itsDistortionCoeffs,
00396         itsCameraRotation,
00397         itsCameraTranslation);
00398 
00399     cvReleaseMat( &image_points_ex);
00400     cvReleaseMat( &object_points_ex);
00401     //return corners;
00402   }
00403 
00404   //No corners found
00405   return std::vector<Point2D<int> >();
00406 }
00407 
00408 // ######################################################################
00409 std::vector<Point2D<int> > HeliPose::getExtrinsic(Image<PixRGB<byte> >& img)
00410 {
00411   CvSeq* square = findSquares(itsCurrentImg, itsStorage,
00412       1500, //minArea
00413       15000, //maxArea
00414       0.3 //minCos
00415       );
00416   std::vector<Point2D<int> > corners = getRectangle(square);
00417   if (corners.size() > 0)
00418   {
00419 
00420     CvMat *image_points_ex = cvCreateMat( corners.size(), 2, CV_64FC1);
00421 
00422     for (uint j = 0; j < corners.size(); j++){
00423       cvSetReal2D( image_points_ex, j, 0, (float)corners[j].i);
00424       cvSetReal2D( image_points_ex, j, 1, (float)corners[j].j);
00425     }
00426 
00427     CvMat *object_points_ex = cvCreateMat( corners.size(), 3, CV_64FC1);
00428 
00429     cvSetReal2D( object_points_ex, 0, 0, 0 ); //0.4m
00430     cvSetReal2D( object_points_ex, 0, 1, 0 );
00431     cvSetReal2D( object_points_ex, 0, 2, 0.0 );
00432 
00433     cvSetReal2D( object_points_ex, 1, 0, 0 );
00434     cvSetReal2D( object_points_ex, 1, 1, 203 );
00435     cvSetReal2D( object_points_ex, 1, 2, 0.0 );
00436 
00437     cvSetReal2D( object_points_ex, 2, 0, 127) ; //0.4m
00438     cvSetReal2D( object_points_ex, 2, 1, 203 );
00439     cvSetReal2D( object_points_ex, 2, 2, 0.0 );
00440 
00441     cvSetReal2D( object_points_ex, 3, 0, 127); //0.4m
00442     cvSetReal2D( object_points_ex, 3, 1, 0 );
00443     cvSetReal2D( object_points_ex, 3, 2, 0.0 );
00444 
00445     cvFindExtrinsicCameraParams2( object_points_ex,
00446         image_points_ex,
00447         itsIntrinsicMatrix,
00448         itsDistortionCoeffs,
00449         itsCameraRotation,
00450         itsCameraTranslation);
00451 
00452     cvReleaseMat( &image_points_ex);
00453     cvReleaseMat( &object_points_ex);
00454     //   return corners;
00455 
00456   }
00457   return corners;
00458 
00459 }
00460 
00461 // ######################################################################
00462 void HeliPose::displayExtrinsic(Image<byte>& img)
00463 {
00464   int  NUM_GRID         = 12; //21
00465   CvMat *my_3d_point = cvCreateMat( 3, NUM_GRID * NUM_GRID + 1, CV_64FC1);
00466   CvMat *my_image_point = cvCreateMat( 2, NUM_GRID * NUM_GRID + 1, CV_64FC1);
00467 
00468   for ( int i = 0; i < NUM_GRID; i++){
00469     for ( int j = 0; j < NUM_GRID; j++){
00470       cvSetReal2D( my_3d_point, 0, i * NUM_GRID + j, (i * 50));
00471       cvSetReal2D( my_3d_point, 1, i * NUM_GRID + j, (j * 50));
00472       cvSetReal2D( my_3d_point, 2, i * NUM_GRID + j, 0.0);
00473     }
00474   }
00475 
00476   cvSetReal2D( my_3d_point, 0, NUM_GRID*NUM_GRID, 0.0);
00477   cvSetReal2D( my_3d_point, 1, NUM_GRID*NUM_GRID, 0.0);
00478   cvSetReal2D( my_3d_point, 2, NUM_GRID*NUM_GRID, 15);
00479 
00480 
00481   cvProjectPoints2( my_3d_point,
00482       itsCameraRotation,
00483       itsCameraTranslation,
00484       itsIntrinsicMatrix,
00485       itsDistortionCoeffs,
00486       my_image_point);
00487 
00488   //printf( "Rotation2: %f %f %f\n",
00489   //    cvGetReal2D(itsCameraRotation, 0, 0),
00490   //    cvGetReal2D(itsCameraRotation, 0, 1),
00491   //    cvGetReal2D(itsCameraRotation, 0, 2));
00492   //printf("%f %f %f\n",
00493   //    cvGetReal2D(itsCameraTranslation, 0, 0),
00494   //    cvGetReal2D(itsCameraTranslation, 0, 1),
00495   //    cvGetReal2D(itsCameraTranslation, 0, 2));
00496 
00497   for ( int i = 0; i < NUM_GRID; i++){
00498     for ( int j = 0; j < NUM_GRID-1; j++){
00499       int im_x1 = (int)cvGetReal2D( my_image_point, 0, i * NUM_GRID + j);
00500       int im_y1 = (int)cvGetReal2D( my_image_point, 1, i * NUM_GRID + j);
00501       int im_x2 = (int)cvGetReal2D( my_image_point, 0, i * NUM_GRID + j+1);
00502       int im_y2 = (int)cvGetReal2D( my_image_point, 1, i * NUM_GRID + j+1);
00503 
00504       cvLine( img2ipl(img), cvPoint( im_x1, im_y1), cvPoint( im_x2, im_y2), CV_RGB( 0, 255, 0), 1);
00505     }
00506   }
00507   for ( int j = 0; j < NUM_GRID; j++){
00508     for ( int i = 0; i < NUM_GRID-1; i++){
00509       int im_x1 = (int)cvGetReal2D( my_image_point, 0, i * NUM_GRID + j);
00510       int im_y1 = (int)cvGetReal2D( my_image_point, 1, i * NUM_GRID + j);
00511       int im_x2 = (int)cvGetReal2D( my_image_point, 0, (i+1) * NUM_GRID + j);
00512       int im_y2 = (int)cvGetReal2D( my_image_point, 1, (i+1) * NUM_GRID + j);
00513 
00514       cvLine( img2ipl(img), cvPoint( im_x1, im_y1), cvPoint( im_x2, im_y2), CV_RGB( 0, 255, 0), 1);
00515     }
00516   }
00517 
00518   int im_x0 = (int)cvGetReal2D( my_image_point, 0, 0);
00519   int im_y0 = (int)cvGetReal2D( my_image_point, 1, 0);
00520   int im_x = (int)cvGetReal2D( my_image_point, 0, NUM_GRID*NUM_GRID);
00521   int im_y = (int)cvGetReal2D( my_image_point, 1, NUM_GRID*NUM_GRID);
00522   cvLine( img2ipl(img), cvPoint( im_x0, im_y0), cvPoint( im_x, im_y), CV_RGB( 255, 0, 0), 2); //Z axis
00523 
00524   cvReleaseMat( &my_3d_point);
00525   cvReleaseMat( &my_image_point);
00526 
00527 }
00528 
00529 HeliPose::Pose HeliPose::getPose()
00530 {
00531   pthread_mutex_lock(&itsPoseLock);
00532   Pose pose = itsCurrentPose;
00533   pthread_mutex_unlock(&itsPoseLock);
00534   return pose;
00535 }
00536 
00537 // ######################################################################
00538 Image<PixRGB<byte> > HeliPose::getImg()
00539 {
00540   pthread_mutex_lock(&itsImgLock);
00541   Image<PixRGB<byte> > img = itsCurrentImg;
00542   pthread_mutex_unlock(&itsImgLock);
00543 
00544   return img;
00545 }
00546 
00547 // ######################################################################
00548 void HeliPose::run_cameraPose()
00549 {
00550   running = true;
00551 
00552   while(running)
00553   {
00554     GenericFrame input = itsIfs->readFrame();
00555 
00556     pthread_mutex_lock(&itsImgLock);
00557     itsCurrentImg = input.asRgb();
00558     pthread_mutex_unlock(&itsImgLock);
00559 
00560     //Image<byte> grey = luminance(itsCurrentImg);
00561     CvSeq* square = findSquares(itsCurrentImg, itsStorage,
00562         1500, //minArea
00563         15000, //maxArea
00564         0.3 //minCos
00565         );
00566     std::vector<Point2D<int> > corners = getRectangle(square);
00567 
00568     //std::vector<CvPoint2D32f> corners = getExtrinsic(grey);
00569 
00570     //std::vector<Point2D<int> > corners = getExtrinsic(itsCurrentImg);
00571 
00572     Point2D<float> center(0,0);
00573     for(uint i=0; i<corners.size(); i++)
00574     {
00575       center.i += corners[i].i;
00576       center.j += corners[i].j;
00577     }
00578     center /= corners.size();
00579 
00580 
00581     if (itsDebug)
00582     {
00583       if (corners.size() > 0)
00584       {
00585         for(uint i=0; i<corners.size(); i++)
00586           drawCircle(itsCurrentImg, corners[i], 6, PixRGB<byte>(255,0,0), 4);
00587         drawCircle(itsCurrentImg, Point2D<int>(center), 6, PixRGB<byte>(0,255,0), 4);
00588       }
00589 
00590       //displayExtrinsic(grey);
00591       itsOfs->writeRGB(itsCurrentImg, "HeliPose", FrameInfo("heliPose", SRC_POS));
00592     }
00593 
00594 
00595     pthread_mutex_lock(&itsPoseLock);
00596 
00597     if (corners.size() > 0)
00598     {
00599       itsCurrentPose.translation = Point3D<float>(center.i - (320/2), center.j - (240/2), 0);
00600       itsCurrentPose.valid = true;
00601     } else {
00602       itsCurrentPose.valid = false;
00603     }
00604 
00605 
00606 
00607 
00608     //float x = cvGetReal2D(itsCameraTranslation, 0, 0);
00609     //float y = cvGetReal2D(itsCameraTranslation, 0, 1);
00610     //float z  = cvGetReal2D(itsCameraTranslation, 0, 2);
00611 
00612     //float rotx =  cvGetReal2D(itsCameraRotation, 0, 0);
00613     //float roty =  cvGetReal2D(itsCameraRotation, 0, 1);
00614     //float rotz =  cvGetReal2D(itsCameraRotation, 0, 2);
00615 
00616     //itsCurrentPose.translation = Point3D<float>(x,y,z);
00617     //itsCurrentPose.rotation = Point3D<float>(rotx, roty, rotz);
00618     //if (corners.size() > 0)
00619     //  itsCurrentPose.valid = true;
00620     //else
00621     //  itsCurrentPose.valid = false;
00622 
00623     pthread_mutex_unlock(&itsPoseLock);
00624 
00625     cvClearMemStorage(itsStorage);
00626 
00627 
00628     //// sleep a little:
00629     usleep(10000);
00630   }
00631 
00632   // we got an order to stop:
00633   //running = f;  // FIXME: bogus! other thread may unlock too soon
00634   pthread_exit(0);
00635 }
00636 
00637 void HeliPose::run_imuPose()
00638 {
00639   running = true;
00640 
00641   while(running)
00642   {
00643     IMU_SFE_Atomic::IMUData imuData = itsIMU->readIMUData();
00644 
00645     pthread_mutex_lock(&itsPoseLock);
00646     Pose currentPose = itsCurrentPose; //get the current pose
00647     pthread_mutex_unlock(&itsPoseLock);
00648 
00649 
00650     currentPose.rotation.z += (imuData.yaw*itsRotationScale.z) - itsRotationBias.z;
00651     currentPose.velocity.x += (imuData.accelX*itsVelocityScale.x) - itsVelocityBias.x;
00652     currentPose.translation.x += currentPose.velocity.x*itsVelocityScale.x;
00653     //Update the pose position from the imu
00654 
00655 
00656     pthread_mutex_lock(&itsPoseLock);
00657 
00658     itsCurrentPose.valid = true;
00659     itsCurrentPose.translation = currentPose.translation;
00660     itsCurrentPose.velocity = currentPose.velocity;
00661     itsCurrentPose.rotation = currentPose.rotation;
00662     itsCurrentPose.accelX = imuData.accelX*itsVelocityScale.x;
00663     itsCurrentPose.accelY = imuData.accelY;
00664     itsCurrentPose.accelZ = imuData.accelZ;
00665     itsCurrentPose.roll = imuData.roll;
00666     itsCurrentPose.pitch = imuData.pitch;
00667     itsCurrentPose.yaw = imuData.yaw*itsRotationScale.z;
00668 
00669     pthread_mutex_unlock(&itsPoseLock);
00670 
00671     //// sleep a little:
00672     usleep(10000);
00673   }
00674 
00675   // we got an order to stop:
00676   //running = f;  // FIXME: bogus! other thread may unlock too soon
00677   pthread_exit(0);
00678 }
00679 
00680 void HeliPose::getIMUBias()
00681 {
00682 
00683   //Claculate the mean and variance of the imu data
00684 
00685   pthread_mutex_lock(&itsPoseLock);
00686   for(int i=0; i<50; i++)
00687   {
00688     IMU_SFE_Atomic::IMUData imuData = itsIMU->readIMUData();
00689 
00690     float yawVal = imuData.yaw*itsRotationScale.z;
00691     float xAcc = imuData.accelX*itsVelocityScale.x;
00692 
00693     itsRotationBias.z = onlineMean(itsRotationBias.z, yawVal, i+1);
00694     itsVelocityBias.x = onlineMean(itsVelocityBias.x, xAcc, i+1);
00695     LINFO("%f %f",
00696         itsRotationBias.z, itsVelocityBias.x);
00697   }
00698 
00699   itsCurrentPose.translation = Point3D<float>(0,0,0);
00700   itsCurrentPose.rotation = Point3D<float>(0,0,0);
00701   itsCurrentPose.velocity = Point3D<float>(0,0,0);
00702   pthread_mutex_unlock(&itsPoseLock);
00703 
00704 }
00705 
00706 
00707 // ######################################################################
00708 /* So things look consistent in everyone's emacs... */
00709 /* Local Variables: */
00710 /* indent-tabs-mode: nil */
00711 /* End: */
Generated on Sun May 8 08:41:22 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3