v4r_ellipses.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2014 by Markus Bader                                    *
00003  *   markus.bader@tuwien.ac.at                                             *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 
00021 #include "opencv2/imgproc/imgproc.hpp"
00022 #include <opencv/cv.h>
00023 #include <numeric>      // std::inner_product
00024 #include <v4r_ellipses/v4r_ellipses.h>
00025 #include <v4r_ellipses/v4r_ellipses_defaults.h>
00026 #include <boost/foreach.hpp>
00027 #include "v4r_utils/canny.h"
00028 #include "v4r_utils/ellipse_refinement.h"
00029 
00030 using namespace V4R;
00031 
00032 
00033 EllipsesDetection::~EllipsesDetection()
00034 {
00035     if(param_ != NULL) delete param_;
00036 }
00037 
00038 EllipsesDetection::EllipsesDetection(Parameters *param)
00039     :param_(param), loop_count(0) {
00040 }
00041 
00042 void EllipsesDetection::createEllipseCanditates () {
00043     ellipses_.clear();
00044     ellipses_.resize(contours_.size());
00045     cv::Mat cameraMatrix, distCoeffs;
00046     for(unsigned int i = 0; i < contours_.size(); i++) {
00047         unsigned int count = contours_[i].size();
00048         Ellipse &ellipse = ellipses_[i];
00049         ellipse.init();
00050         ellipse.id = i;
00051         if(param_->filter_convex) {
00052             cv::approxPolyDP( cv::Mat (contours_[i]), *ellipse.polygon, param_->threshold_polygon, true );
00053             ellipse.boxContour = cv::boundingRect( cv::Mat(*ellipse.polygon) );
00054             //cv::minEnclosingCircle( (cv::Mat)*ellipse.polygon, ellipse.centerContour, ellipse.radiusContour );
00055         }
00056         ellipse.contourDistort->resize(count);
00057         ellipse.contourUndistort->resize(count);
00058         for(unsigned int j = 0; j < count; j++) {
00059             (*ellipse.contourDistort)[j] = contours_[i][j];
00060             (*ellipse.contourUndistort)[j] = contours_[i][j];
00061         }
00062         if(param_->distorted_input) {
00063             cv::undistortPoints(*ellipse.contourDistort, *ellipse.contourUndistort, camera_.cameraMatrix, camera_.distCoeffs, cv::Mat(), camera_.projectionMatrix);
00064         }
00065     }
00066 }
00067 
00068 
00069 EllipsesDetection::DetectionState EllipsesDetection::filterContour (Ellipse &ellipse) {
00070     if(ellipse.contourDistort->size() < param_->threshold_contour_min_points) {
00071         ellipse.detection = INVALID_CONTOUR_POINTS;
00072         return ellipse.detection;
00073     }
00074     if(param_->filter_convex && !cv::isContourConvex(*ellipse.polygon)) {
00075         ellipse.detection = INVALID_CONTOUR_CONVEX;
00076         return ellipse.detection;
00077     }
00078     return ellipse.detection;
00079 }
00080 
00081 void EllipsesDetection::edge_detection(const cv::Mat &m) {
00082     imgGray_ = m;
00083     switch(param_->edge_detection) {
00084     case EDGE_DETECTION_THRESHOLD:
00085         imgEdges_ = m >= param_->threshold_edge_detection1;
00086         break;
00087     case EDGE_DETECTION_CANNY:
00088         cv::blur(imgGray_, imgBlured_, cv::Size(3,3) );
00089         V4R::Canny(imgBlured_, imgEdges_, imgGradient_, imgDirection_, imgSobelDx_, imgSobelDy_, param_->threshold_edge_detection1, param_->threshold_edge_detection2, param_->kernel_size_edge_detection);
00090         break;
00091     }
00092 }
00093 
00094 void EllipsesDetection::contour_detection() {
00095     switch(param_->edge_linking) {
00096     case EDGE_LINKING_OPENCV_APPROX_NONE:
00097         cv::findContours ( imgEdges_, contours_, CV_RETR_LIST, CV_CHAIN_APPROX_NONE );
00098         break;
00099     case EDGE_LINKING_OPENCV_APPROX_SIMPLE:
00100         cv::findContours( imgEdges_, contours_, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE );
00101         break;
00102     case EDGE_LINKING_V4R_SIMPLE:
00103         if(param_->edge_detection != EDGE_DETECTION_CANNY) break;
00104         contour_detector_.Init(imgGray_.cols, imgGray_.rows);
00105         contour_detector_.Perform(imgEdges_.data, V4R::Contour::MODE_SIMPLE, imgGradient_.data);
00106         contour_detector_.getContours(contours_);
00107         break;
00108     case EDGE_LINKING_V4R_COMPLEX:
00109         if(param_->edge_detection != EDGE_DETECTION_CANNY) break;
00110         contour_detector_.Init(imgGray_.cols, imgGray_.rows);
00111         contour_detector_.Perform(imgEdges_.data, V4R::Contour::MODE_COMPLEX, imgGradient_.data);
00112         contour_detector_.getContours(contours_);
00113         break;
00114     case EDGE_LINKING_V4R_CONTOUR:
00115         if(param_->edge_detection != EDGE_DETECTION_CANNY) break;
00116         contour_detector_.Init(imgGray_.cols, imgGray_.rows);
00117         contour_detector_.Perform(imgEdges_.data, V4R::Contour::MODE_CONTOUR, imgGradient_.data);
00118         contour_detector_.getContours(contours_);
00119         break;
00120     }
00121 }
00122 
00123 void EllipsesDetection::fit_ellipses_opencv (const cv::Mat &m, const cv::Mat cameraMatrix, const cv::Mat distCoeffs, const cv::Mat projectionMatrix, const boost::posix_time::ptime &tstamp) {
00124     loop_count++;
00125     tstamp_ = tstamp;
00126     if(param_->distorted_input) {
00127         cameraMatrix.convertTo(camera_.cameraMatrix, CV_64F) ;
00128         distCoeffs.convertTo(camera_.distCoeffs, CV_64F) ;
00129         projectionMatrix(cv::Rect(0,0,3,3)).convertTo(camera_.projectionMatrix, CV_64F) ;
00130         camera_.projectionMatrix(1,1) = camera_.projectionMatrix(0,0);
00131     } else {
00132         projectionMatrix(cv::Rect(0,0,3,3)).convertTo(camera_.cameraMatrix, CV_64F) ;
00133         camera_.distCoeffs = (cv::Mat_<double>(1,5) << 0, 0, 0, 0, 0);
00134         projectionMatrix(cv::Rect(0,0,3,3)).convertTo(camera_.projectionMatrix, CV_64F) ;
00135     }
00136 
00137     edge_detection(m);
00138     contour_detection();
00139     createEllipseCanditates();
00140 
00141     double min_radius = m.rows * param_->threshold_min_radius;
00142     double max_radius = m.rows * param_->threshold_max_radius;
00143 
00144     for(unsigned int i = 0; i < ellipses_.size(); i++) {
00145         Ellipse &ellipse = ellipses_[i];
00146         if(filterContour (ellipse) != VALID) continue;
00147         cv::Mat pointsf(*ellipse.contourUndistort);
00148         ellipse.boxEllipse = fitEllipse ( pointsf );
00149         ellipse.radiusEllipseMax = MAX( ellipse.boxEllipse.size.width , ellipse.boxEllipse.size.height) / 2.0;
00150         ellipse.radiusEllipseMin = MIN( ellipse.boxEllipse.size.width , ellipse.boxEllipse.size.height) / 2.0;
00151         if(ellipse.radiusEllipseMax < min_radius)  {
00152             ellipse.detection = INVALID_MIN_RAIDUS;
00153         }
00154         if(ellipse.radiusEllipseMax > max_radius)  {
00155             ellipse.detection = INVALID_MAX_RAIDUS;
00156         }
00157         if(filterEllipse (ellipse) != VALID) continue;
00158         if(filterContourMean(ellipse)  != VALID) continue;
00159     }
00160     if(param_->ellipse_redefinement &&!imgSobelDx_.empty() && !imgSobelDy_.empty()) {
00161         for(unsigned int i = 0; i < ellipses_.size(); i++) {
00162             EllipseRedefinement(ellipses_[i]);
00163         }
00164     }
00165 }
00166 
00167 V4R::EllipseRefinement ellipseRefinementDual;
00168 
00169 EllipsesDetection::DetectionState EllipsesDetection::EllipseRedefinement(Ellipse &ellipse) {
00170     if(ellipse.detection != VALID) return ellipse.detection;
00171     
00172     V4R::EllipseRefinement::Ellipse e;
00173     e.setEllipse(ellipse.boxEllipse);
00174     ellipseRefinementDual.refine(imgSobelDx_, imgSobelDy_, *ellipse.contourUndistort, e);
00175     e.get(ellipse.boxEllipse);  
00176     return ellipse.detection;
00177 }
00178 
00179 
00180 EllipsesDetection::DetectionState EllipsesDetection::filterEllipse(Ellipse &ellipse) {
00181     if(ellipse.detection != VALID) return ellipse.detection;
00182     double boxEllipseRatio = ellipse.radiusEllipseMin / ellipse.radiusEllipseMax;
00183     if ( boxEllipseRatio < param_->threshold_ring_ratio ) {
00184         ellipse.detection = INVALID_ROTATED_RECT_RATIO;
00185         return ellipse.detection;
00186     }
00187     return ellipse.detection;
00188 }
00189 
00190 void EllipsesDetection::createRings() {
00191     if(!param_->filter_rings) return;
00192     for(std::vector<Ellipse>::iterator a = ellipses_.begin(); a != ellipses_.end(); a++) {
00193         if((a->innerRing >= 0)  || (a->outerRing >= 0) || (a->detection != VALID)) continue;
00194         double threshold_center = a->radiusEllipseMax * param_->threshold_ring_center;
00195         for(std::vector<Ellipse>::iterator b = ellipses_.begin(); b != ellipses_.end(); b++) {
00196             if((a->id == b->id) || (b->innerRing != -1)  || (b->outerRing != -1) || (b->detection != VALID) ) {
00197                 continue;
00198             }
00199             double d = cv::norm(a->boxEllipse.center - b->boxEllipse.center);
00200             if(d < threshold_center) {
00201                 Ellipse *outer = &(*a), *inner = &(*b);
00202                 if(a->radiusEllipseMax < b->radiusEllipseMax)  outer = &(*b), inner =  &(*a);
00203                 double ratioRadiusRingMax = fabs(inner->radiusEllipseMax / outer->radiusEllipseMax-0.5);
00204                 double ratioRadiusRingMin = fabs(inner->radiusEllipseMax / outer->radiusEllipseMax-0.5);
00205                 if((ratioRadiusRingMax > param_->threshold_ring_ratio) || (ratioRadiusRingMin > param_->threshold_ring_ratio)) {
00206                     continue;
00207                 }
00208                 outer->innerRing = inner->id;
00209                 inner->outerRing = outer->id;
00210                 inner->detection = INVALID_IS_INNER_RING;
00211                 break;
00212             }
00213         }
00214         if((a->innerRing == -1)  && (a->outerRing == -1)) a->detection = INVALID_NO_RING;
00215     }
00216 }
00217 
00218 void EllipsesDetection::next() {
00219     contours_.clear();
00220     ellipses_.clear();
00221     tstampLast_ = tstamp_;
00222 }
00223 
00224 EllipsesDetection::DetectionState  EllipsesDetection::filterContourMean(Ellipse &ellipse) {
00225     if(!param_->filter_contour_mean || (ellipse.detection != VALID)) return ellipse.detection;
00226     const std::vector<cv::Point2f> &contour = *(ellipse.contourUndistort.get());
00227     ellipse.distances = boost::shared_ptr<std::vector<double> >(new std::vector<double> );
00228     std::vector<double > &distances = *(ellipse.distances.get());
00229     const cv::Point2f &pc = ellipse.boxEllipse.center;
00230     double angle = M_PI/180.0 * (double) ellipse.boxEllipse.angle;
00231     double ca = cos(angle), sa = sin(angle);
00232     double a = ellipse.boxEllipse.size.width/2., b = ellipse.boxEllipse.size.height/2.;
00233     double sum = 0;
00234     distances.reserve(contour.size());
00235     for(unsigned int i = 0; i < contour.size(); i++) {
00236         double dx = contour[i].x - pc.x, dy = contour[i].y - pc.y;
00237         double u = ca*dx + sa*dy, v = -sa*dx + ca*dy;
00238         cv::Point2d p(contour[i].x - pc.x, contour[i].y - pc.y);
00239         double d = (u*u)/(a*a) + (v*v)/(b*b);
00240         distances.push_back(d);
00241         sum += d;
00242         // http://stackoverflow.com/questions/11041547/finding-the-distance-of-a-point-to-an-ellipse-wether-its-inside-or-outside-of-e
00243     }
00244     double mean = sum / distances.size();
00245     double diff = fabs((mean - 1.));
00246     if(diff > param_->threshold_contour_mean) {
00247         ellipse.detection = INVALID_CONTOUR_MEAN;
00248     }
00249     return ellipse.detection;
00250 }
00251 
00252 
00253 void EllipsesDetection::estimatePoses() {
00254     if(param_->pose_estimation == POSE_ESTIMATION_OFF) return;
00255     cv::Point2f pi[4];
00256     if(param_->circle_diameter <= 0) return;
00257     cv::Mat_<double> rvec, tvec;
00258     double radius = param_->circle_diameter / 2.0;
00259     cv::Mat objectPoints  = (cv::Mat_<double>(5,3) << 0, 0, 0, -radius, +radius, 0, +radius, +radius, 0, +radius, -radius, 0, -radius, -radius, 0);
00260     for(std::vector<Ellipse>::iterator it = ellipses_.begin(); it != ellipses_.end(); it++) {
00261         Ellipse &ellipse = *it;
00262         if(ellipse.detection != VALID) continue;
00263 
00264         ellipse.boxEllipse.points(pi);
00265         cv::Point2f& pc = ellipse.boxEllipse.center;
00266         cv::Mat imagePoints = (cv::Mat_<double>(5,2) << pc.x, pc.y, pi[0].x, pi[0].y, pi[1].x, pi[1].y, pi[2].x, pi[2].y, pi[3].x, pi[3].y);
00267         switch(param_->pose_estimation) {
00268         case POSE_ESTIMATION_SOLVEPNP:
00269             cv::solvePnP(objectPoints, imagePoints, camera_.cameraMatrix, camera_.distCoeffs, rvec, tvec);
00270             cv::Rodrigues(rvec, ellipse.cone.R[0]);
00271             ellipse.cone.translations[0].x = tvec(0), ellipse.cone.translations[0].y = tvec(1), ellipse.cone.translations[0].z = tvec(2);
00272             ellipse.cone.rotation2Normal(0);
00273             ellipse.cone.R[1] = ellipse.cone.R[0].clone();
00274             ellipse.cone.rotation2Normal(1);
00275             break;
00276         case POSE_ESTIMATION_FROM_ELLIPSE:
00277             ellipse.cone.set(ellipse.boxEllipse, camera_.projectionMatrix);
00278             ellipse.cone.pose(camera_.projectionMatrix, camera_.distCoeffs, param_->circle_diameter/2.);
00279             ellipse.cone.normal2Roation(0);
00280             ellipse.cone.normal2Roation(1);
00281             break;
00282         default:
00283             continue;
00284         }
00285     }
00286     markers_.clear();
00287     for(std::vector<Ellipse>::iterator ellipse = ellipses_.begin(); ellipse != ellipses_.end(); ellipse++) {
00288         if(ellipse->detection != VALID) continue;
00289         markers_.push_back(ellipse->cone);
00290         markers_.back().id = markers_.size()-1;
00291         markers_.back().tstamp = tstamp_;
00292     }
00293 
00294 }
00295 
00296 
00297 void EllipsesDetection::ObliqueCone::rotation2Normal(int i) {
00298     cv::Mat_<double> tvec(translations[i]);
00299     cv::Mat_<double> nvec = R[i] * (cv::Mat_<double>(3,1) << 0, 0, 1);
00300     double d0 = cv::norm(tvec - nvec);
00301     double d1 = cv::norm(tvec + nvec);
00302     if(d1 < d0) {
00303         nvec = -nvec;
00304     }
00305     normals[i][0] = nvec(0), normals[i][1] = nvec(1),   normals[i][2] = nvec(2);
00306 }
00307 void EllipsesDetection::ObliqueCone::normal2Roation(int i) {
00308     cv::Mat_<double> unitZ = (cv::Mat_<double>(3,1) << 0, 0, 1);
00309     cv::Mat_<double> nvec(normals[i]);
00310     nvec = nvec/cv::norm(nvec);
00311     cv::Mat_<double> c2 = nvec;
00312     cv::Mat_<double> c1 = unitZ.cross(c2);
00313     cv::Mat_<double> c0 = c1.cross(c2);
00314     c1 = c1/cv::norm(c1);
00315     c0 = c0/cv::norm(c0);
00316     R[i] = (cv::Mat_<double>(3,3) << c0(0), c1(0), c2(0), c0(1), c1(1), c2(1), c0(2), c1(2), c2(2));
00317 }
00318 
00319 
00320 double EllipsesDetection::ObliqueCone::distance(const cv::Point2d p) {
00321     cv::Mat_<double> P =  (cv::Mat_<double>(3,1) << p.x, p.y, 1);
00322     cv::Mat_<double> D = P.t() * C * P;
00323     double d = D(0,0);
00324     return d;
00325 }
00326 void EllipsesDetection::ObliqueCone::set(cv::RotatedRect box, cv::Mat_<double> intrinsic) {
00327     double a = box.size.width/2., b = box.size.height/2.;
00328     double angle = M_PI/180.0 * (float) box.angle;
00329     double ca = cos(angle), sa = sin(angle);
00330     double cx = intrinsic(0,2), cy = intrinsic(1,2);
00331     cv::Mat_<double>  Re = (cv::Mat_<double>(2,2) << ca, -sa, sa, ca);
00332     cv::Mat_<double> ABInvTAB = (cv::Mat_<double>(2,2) << 1./(a*a), 0., 0., 1./(b*b));
00333     cv::Mat_<double>  X0 = (cv::Mat_<double>(2,1) << box.center.x-cx, box.center.y-cy);
00334     cv::Mat_<double>  M = Re * ABInvTAB * Re.t();
00335     cv::Mat_<double>  Mf = X0.t() * M * X0;
00336     double A = M(0,0);
00337     double B = M(0,1);
00338     double C = M(1,1);
00339     double D = - A * X0(0) - B * X0(1);
00340     double E = - B * X0(0) - C * X0(1);
00341     double F = Mf(0,0) - 1.0;
00342     this->C =  (cv::Mat_<double>(3,3) << A, B, D,
00343                 B, C, E,
00344                 D, E, F);
00345 }
00346 
00347 
00348 
00349 
00350 void EllipsesDetection::ObliqueCone::pose(cv::Mat_<double> intrinsic, cv::Mat_<double> distCoeffs, double radius) {
00351     cv::Mat_<double> Q, V, E;
00352 
00353     double fx = intrinsic(0,0), fy = intrinsic(1,1);
00354     double focalLength = (fx+fy)/2.0;
00355     if(focalLength == 0) return;
00356     Q = this->C.clone();
00357     Q(0,0) = this->C(0,0);
00358     Q(0,1) = this->C(0,1);
00359     Q(0,2) = this->C(0,2) / (focalLength);
00360     Q(1,0) = this->C(1,0);
00361     Q(1,1) = this->C(1,1);
00362     Q(1,2) = this->C(1,2) / (focalLength);
00363     Q(2,0) = this->C(2,0) / (focalLength);
00364     Q(2,1) = this->C(2,1) / (focalLength);
00365     Q(2,2) = this->C(2,2) / (focalLength*focalLength);
00366 
00367     cv::eigen(Q, E, V);
00368     V = V.t();
00369     double e1 = E.at<double>(0);
00370     double e2 = E.at<double>(1);
00371     double e3 = E.at<double>(2);
00372     double S1[] = {+1,+1,+1,+1,-1,-1,-1,-1};
00373     double S2[] = {+1,+1,-1,-1,+1,+1,-1,-1};
00374     double S3[] = {+1,-1,+1,-1,+1,-1,+1,-1};
00375 
00376     double g = sqrt((e2-e3)/(e1-e3));
00377     double h = sqrt((e1-e2)/(e1-e3));
00378     translations[0] = translations[1] = cv::Point3d(0,0,0);
00379     normals[0] = normals[1] = cv::Vec3d(0,0,0);
00380     projections[0] = projections[1] = cv::Point2d(0,0);
00381     unsigned int k = 0;
00382     for(int i = 0; i < 8; i++) {
00383 
00384         double z0 =  S3[i] *    (e2 * radius) / sqrt(-e1*e3);
00385         double Tx =  S2[i] * e3/e2 * h;
00386         double Ty =  0.;
00387         double Tz = -S1[i] * e1/e2 * g;
00388         double Nx =  S2[i] * h;
00389         double Ny =  0.;
00390         double Nz = -S1[i] * g;
00391 
00392         cv::Mat_<double> t = (z0 * V * (cv::Mat_<double>(3,1) << Tx, Ty, Tz));
00393         cv::Mat_<double> n = (V *  (cv::Mat_<double>(3,1) << Nx, Ny, Nz));
00394 
00395         // identify the two possible solutions
00396         if((t(2) > 0) && (n(2)<0)) {
00397             if(k > 1) continue;
00398             translations[k] = cv::Point3d(t(0), t(1), t(2));
00399             normals[k] = cv::Vec3d(n(0), n(1), n(2));
00400             // Projection
00401             cv::Mat_<double> Pc = intrinsic * t;
00402             projections[k].x = Pc(0)/Pc(2);
00403             projections[k].y = Pc(1)/Pc(2);
00404             k++;
00405         }
00406     }
00407 }
00408 


v4r_ellipses
Author(s):
autogenerated on Wed Aug 26 2015 16:41:40