geometric_hash.cpp
Go to the documentation of this file.
00001 /*
00002  *  geometric_hash.cpp
00003  *  edge_matcher
00004  *
00005  *  Created by Victor  Eruhimov on 11/1/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 #include <stdio.h>
00011 #include "outlet_pose_estimation/detail/geometric_hash.h"
00012 
00013 cv::Point2f AffineBasis::getCoords(cv::Point2f point) const
00014 {
00015     point = point - origin;
00016 
00017     cv::Point2f coords;
00018     float d = basis[0].x*basis[1].y - basis[1].x*basis[0].y;
00019     coords.x = (point.x*basis[1].y - point.y*basis[1].x)/d;
00020     coords.y = (point.y*basis[0].x - point.x*basis[0].y)/d;
00021 
00022     return coords;
00023 }
00024 
00025 cv::Point2f AffineBasis::getPoint(cv::Point2f coords) const
00026 {
00027     return origin + basis[0]*coords.x + basis[1]*coords.y;
00028 }
00029 
00030 
00031 GeometricHash::GeometricHash(cv::Size _size, cv::Point2f range_min, cv::Point2f range_max)
00032 {
00033     size = _size;
00034     hash.resize(size.width*size.height);
00035 
00036     range[0] = range_min;
00037     range[1] = range_max;
00038 }
00039 
00040 int GeometricHash::getBin(cv::Point2f coords) const
00041 {
00042     int idx[2];
00043     idx[0] = (int)floor((coords.x - range[0].x)/(range[1].x - range[0].x)*size.width);
00044     idx[1] = (int)floor((coords.y - range[0].y)/(range[1].y - range[0].y)*size.height);
00045     if(idx[0] >= size.width || idx[1] >= size.height)
00046     {
00047         return -1;
00048     }
00049 
00050     return idx[1]*size.width + idx[0];
00051 }
00052 
00053 ModelBasisID GeometricHash::addBasis(const AffineBasis& basis)
00054 {
00055     bases.push_back(basis);
00056     return bases.size() - 1;
00057 }
00058 
00059 void GeometricHash::addEntry(const ModelBasisID& basisID, cv::Point2f point)
00060 {
00061     const AffineBasis& basis = bases[basisID];
00062     cv::Point2f coords = basis.getCoords(point);
00063     int hashIdx = getBin(coords);
00064     if(hashIdx >= 0 && hashIdx < hash.size())
00065     {
00066         hash[hashIdx].push_back(basisID);
00067 /*        if(basisID == 90)
00068         {
00069             printf("Basis 90, coords %f,%f, added to hash bin %d\n", coords.x, coords.y, hashIdx);
00070         }*/
00071     }
00072 }
00073 
00074 const std::list<ModelBasisID>& GeometricHash::getEntries(cv::Point2f coords) const
00075 {
00076     int hashIdx = getBin(coords);
00077     if(hashIdx >= 0)
00078     {
00079         return hash[hashIdx];
00080     }
00081     else
00082     {
00083         return empty_list;
00084     }
00085 }
00086 
00087 const std::vector<AffineBasis>& GeometricHash::getBases() const
00088 {
00089     return bases;
00090 }
00091 
00092 
00093 // GeometricHash3D implementation
00094 GeometricHash3D::GeometricHash3D(cv::Point3i _size, const cv::Point3f* _range)
00095 {
00096     size = _size;
00097     hash.resize(size.x*size.y*size.z);
00098 
00099     range[0] = _range[0];
00100     range[1] = _range[1];
00101 }
00102 
00103 int GeometricHash3D::getBin(cv::Point3f coords) const
00104 {
00105     int idx[3];
00106     idx[0] = (int)round((coords.x - range[0].x)/(range[1].x - range[0].x)*size.x);
00107     idx[1] = (int)round((coords.y - range[0].y)/(range[1].y - range[0].y)*size.y);
00108     idx[2] = (int)round((coords.z - range[0].z)/(range[1].z - range[0].z)*size.z);
00109     if(idx[0] >= size.x || idx[1] >= size.y || idx[2] >= size.z)
00110     {
00111         return -1;
00112     }
00113 
00114     return idx[2]*size.x*size.y + idx[1]*size.x + idx[0];
00115 }
00116 
00117 ModelBasisID GeometricHash3D::addBasis(const AffineBasis& basis)
00118 {
00119     bases.push_back(basis);
00120     return bases.size() - 1;
00121 }
00122 
00123 float calcAffineSeqDist(const AffineBasis& basis, CvSeq* seq, int idx1, int idx2, int is_mapped)
00124 {
00125     if(!is_mapped)
00126     {
00127         cv::Point2f unit_vec[2] = {cv::Point2f(1.0, 0.0), cv::Point2f(0.0, 1.0)};
00128         seq = mapContour(seq, basis, AffineBasis(cv::Point2f(0.0, 0.0), unit_vec, -1), seq->storage);
00129     }
00130     float dist1 = fabs(cvArcLength(seq, cvSlice(idx1, idx2), 1));
00131     float dist2 = fabs(cvArcLength(seq, cvSlice(idx2, idx1), 1));
00132     float dist = MIN(dist1, dist2);
00133 //    if(seq == (void*)0xc460f8 && dist > 20) printf("%f\n", dist);
00134     return dist;
00135 }
00136 
00137 void GeometricHash3D::addEntry(const ModelBasisID& basisID, CvSeq* seq, int idx_origin, int idx_point)
00138 {
00139     const AffineBasis& basis = bases[basisID];
00140     cv::Point2f point2 = cv::Point2f(*(CvPoint*)cvGetSeqElem(seq, idx_point));
00141     cv::Point2f coords = basis.getCoords(point2);
00142     float dist = calcAffineSeqDist(basis, seq, idx_origin, idx_point);
00143     cv::Point3f point3 = cv::Point3f(coords.x, coords.y, dist);
00144 
00145     int hashIdx = getBin(point3);
00146     if(hashIdx >= 0 && hashIdx < hash.size())
00147     {
00148         hash[hashIdx].push_back(basisID);
00149         /*        if(basisID == 90)
00150          {
00151          printf("Basis 90, coords %f,%f, added to hash bin %d\n", coords.x, coords.y, hashIdx);
00152          }*/
00153     }
00154 }
00155 
00156 const std::list<ModelBasisID>& GeometricHash3D::getEntries(cv::Point3f coords) const
00157 {
00158     int hashIdx = getBin(coords);
00159     if(hashIdx >= 0)
00160     {
00161         return hash[hashIdx];
00162     }
00163     else
00164     {
00165         return empty_list;
00166     }
00167 }
00168 
00169 const std::vector<AffineBasis>& GeometricHash3D::getBases() const
00170 {
00171     return bases;
00172 }
00173 
00174 // Edge matcher implementation
00175 
00176 AffineBasis getEdgeBasis(CvSeq* edge, int i, int j, int modelID)
00177 {
00178     cv::Point2f pibegin = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, i));
00179     cv::Point2f piend = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, i+1));
00180     cv::Point2f pjbegin = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, j));
00181     cv::Point2f pjend = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, j+1));
00182 
00183     cv::Point2f iorigin = pibegin;
00184     cv::Point2f idir = piend - pibegin;
00185 
00186     cv::Point2f jorigin = (pjbegin + pjend)*0.5f;
00187     cv::Point2f jdir = pjend - pjbegin;
00188 
00189     cv::Point2f basis[2];
00190     // construct a basis on (iorigin, jorigin, idir)
00191     basis[0] = jorigin - iorigin;
00192     basis[1] = idir;
00193 
00194     AffineBasis edge_basis(iorigin, basis, modelID);
00195 
00196     return edge_basis;
00197 }
00198 
00199 AffineBasis getPointEdgeBasis(cv::Point2f point, CvSeq* edge, int i, int modelID)
00200 {
00201     cv::Point2f pibegin = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, i));
00202     cv::Point2f piend = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, i+1));
00203 
00204     cv::Point2f basis[2];
00205     // construct a basis on (iorigin, jorigin, idir)
00206     basis[0] = point - pibegin;
00207     basis[1] = piend - pibegin;
00208 
00209     AffineBasis edge_basis(pibegin, basis, modelID);
00210 
00211     return edge_basis;
00212 }
00213 
00214 
00215 int EdgeMatcher::addModel(CvSeq* edge)
00216 {
00217     edges.push_back(edge);
00218     int model_id = edges.size() - 1;
00219 
00220     // iterate throuhg all possible bases (pairs of points)
00221     for(int i = 0; i < edge->total; i++)
00222     {
00223         for(int j = 0; j < edge->total; j++)
00224         {
00225             if(i <= j) continue;
00226 
00227             AffineBasis basis = getEdgeBasis(edge, i, j, model_id);
00228             if(basis.getAngle() < min_angle) continue;
00229 
00230             addModelBasis(edge, i, basis);
00231 
00232         }
00233     }
00234 
00235     return model_id;
00236 }
00237 
00238 
00239 // ****************** Edge matcher implementation ***********************
00240 const float EdgeMatcher::min_angle = 0.15f;
00241 
00242 void EdgeMatcher::addModelBasis(CvSeq* edge, int idx_origin, const AffineBasis& basis)
00243 {
00244     ModelBasisID basisID = hash.addBasis(basis);
00245     CvRect bbox = cvBoundingRect(edge);
00246     for(int i = 0; i < edge->total; i++)
00247     {
00248 //        cv::Point2f point = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, i));
00249         hash.addEntry(basisID, edge, idx_origin, i);
00250     }
00251 }
00252 
00253 AffineBasis EdgeMatcher::match(CvSeq* edge, std::map<int,std::pair<int, int> >& votes) const
00254 {
00255     std::vector<int> basis_votes;
00256     basis_votes.assign(hash.getBases().size(), 0);
00257 
00258     // choose edge points for basis
00259     int i = rand()%edge->total;
00260     int j = rand()%edge->total;
00261     AffineBasis basis = getEdgeBasis(edge, i, j, -1);
00262     matchBasis(edge, basis, i, basis_votes);
00263 
00264     aggregateVotes(basis_votes, votes);
00265 
00266     return basis;
00267 }
00268 
00269 void EdgeMatcher::aggregateVotes(const std::vector<int>& basis_votes, std::map<int,std::pair<int,int> >& agg_votes) const
00270 {
00271     for(size_t i = 0; i < basis_votes.size(); i++)
00272     {
00273         int modelID = hash.getBases()[i].getModelID();
00274         if(agg_votes.find(modelID) == agg_votes.end())
00275         {
00276             agg_votes[modelID] = std::pair<int,int>(i, basis_votes[i]);
00277         }
00278         else
00279         {
00280             if(basis_votes[i] > agg_votes[modelID].second)
00281             {
00282                 agg_votes[modelID] = std::pair<int,int>(i,basis_votes[i]);
00283             }
00284         }
00285     }
00286 }
00287 
00288 void EdgeMatcher::matchBasis(CvSeq* edge, const AffineBasis& basis, int idx_origin, std::vector<int>& votes) const
00289 {
00290     cv::Point2f unit_vec[] = {cv::Point2f(1.0,0.0), cv::Point2f(0.0,1.0)};
00291     CvSeq* mapped_edge = mapContour(edge, basis, AffineBasis(cv::Point2f(0.0), unit_vec, -1), edge->storage);
00292 
00293     for(int i = 0; i < edge->total; i++)
00294     {
00295 #if 0
00296         cv::Point2f point = cv::Point2f(*(CvPoint*)cvGetSeqElem(edge, i));
00297         cv::Point2f coords = basis.getCoords(point);
00298 #else
00299         cv::Point2f coords = cv::Point2f(*(CvPoint*)cvGetSeqElem(mapped_edge, i));
00300 #endif
00301         float dist = calcAffineSeqDist(basis, mapped_edge, idx_origin, i, 1);
00302         if(length(coords) < 2.0) continue;
00303         const std::list<ModelBasisID>& bases_list = hash.getEntries(cv::Point3f(coords.x,coords.y,dist));
00304         for(std::list<ModelBasisID>::const_iterator it = bases_list.begin(); it != bases_list.end(); it++)
00305         {
00306 //            AffineBasis basis = hash.getBases()[*it];
00307             votes[*it]++;
00308 /*            if(*it == 90)
00309             {
00310                 printf("Coords %f,%f: added vote for point %d\n", coords.x, coords.y, i);
00311             }*/
00312         }
00313     }
00314 }
00315 
00316 float fitEdges(CvSeq* model_seq, const AffineBasis& model_basis, CvSeq* test_seq, const AffineBasis& test_basis)
00317 {
00318     float dist = 0;
00319     for(int i = 0; i < test_seq->total; i++)
00320     {
00321         cv::Point2f test_point = cv::Point2f(*(CvPoint*)cvGetSeqElem(test_seq, i));
00322         cv::Point2f test_coords = test_basis.getCoords(test_point);
00323 
00324         cv::Point2f test_point1 = cv::Point2f(*(CvPoint*)cvGetSeqElem(test_seq, i + 1));
00325         cv::Point2f test_coords1 = test_basis.getCoords(test_point1);
00326 
00327         float min_dist = 1e10;
00328         for(int j = 0; j < model_seq->total; j++)
00329         {
00330             cv::Point2f model_point = cv::Point2f(*(CvPoint*)cvGetSeqElem(model_seq, j));
00331             cv::Point2f model_coords = model_basis.getCoords(model_point);
00332             float _dist = length(model_coords - test_coords);
00333             min_dist = MIN(min_dist, _dist*_dist);
00334         }
00335 
00336         dist += min_dist;//*length(test_coords1 - test_coords);
00337     }
00338 
00339     cv::Point2f basis[2];
00340     model_basis.getBasis(basis);
00341     dist = sqrt(dist/length(basis[0])*length(basis[1]));
00342 
00343     dist /= sqrt(test_seq->total);
00344     return dist;
00345 }
00346 
00347 float fitEdgesSym(CvSeq* model_seq, const AffineBasis& model_basis, CvSeq* test_seq, const AffineBasis& test_basis)
00348 {
00349     float error1 = fitEdges(model_seq, model_basis, test_seq, test_basis);
00350     float error2 = fitEdges(test_seq, test_basis, model_seq, model_basis);
00351 
00352     return error1 + error2;
00353 }
00354 
00355 CvSeq* mapContour(CvSeq* contour, AffineBasis src, AffineBasis dst, CvMemStorage* storage)
00356 {
00357     CvSeq* seq = cvCreateSeq(CV_SEQ_POLYGON, sizeof(CvContour), sizeof(CvPoint), storage);
00358     for(int i = 0; i < contour->total; i++)
00359     {
00360         cv::Point2f src_point = *(CvPoint*)cvGetSeqElem(contour, i);
00361         cv::Point2f coords = src.getCoords(src_point);
00362         CvPoint dst_point = dst.getPoint(coords);
00363         cvSeqPush(seq, &dst_point);
00364     }
00365 
00366     return seq;
00367 }
00368 
00369 int PointEdgeMatcher::addModel(const PointEdgeMatcher::Model& model)
00370 {
00371     models.push_back(model);
00372     int model_id = models.size() - 1;
00373 
00374     cv::Point2f point = model.first;
00375     CvSeq* edge = model.second;
00376     for(int j = 0; j < edge->total; j++)
00377     {
00378         AffineBasis basis = getPointEdgeBasis(point, edge, j, model_id);
00379         addModelBasis(edge, j, basis);
00380     }
00381 
00382     return model_id;
00383 }
00384 
00385 AffineBasis PointEdgeMatcher::match(cv::Point2f point, CvSeq* edge, std::map<int,std::pair<int, int> >& votes) const
00386 {
00387     std::vector<int> basis_votes;
00388     basis_votes.assign(hash.getBases().size(), 0);
00389 
00390     // choose random basis
00391     int i = rand()%edge->total;
00392     AffineBasis basis = getPointEdgeBasis(point, edge, i, -1);
00393     matchBasis(edge, basis, i, basis_votes);
00394 
00395     aggregateVotes(basis_votes, votes);
00396 
00397     return basis;
00398 }
00399 
00400 void mapPoints(const std::vector<cv::Point2f>& src, const AffineBasis& src_basis, const AffineBasis& dst_basis, std::vector<cv::Point2f>& dst)
00401 {
00402     dst.clear();
00403     for(size_t i = 0; i < src.size(); i++)
00404     {
00405         dst.push_back(dst_basis.getPoint(src_basis.getCoords(src[i])));
00406     }
00407 }
00408 
00409 void mapPoints(const std::vector<KeyPointEx>& src, const AffineBasis& src_basis, const AffineBasis& dst_basis, std::vector<KeyPointEx>& dst)
00410 {
00411     dst.clear();
00412     for(size_t i = 0; i < src.size(); i++)
00413     {
00414         dst.push_back(KeyPointEx(dst_basis.getPoint(src_basis.getCoords(src[i].pt))));
00415     }
00416 }
00417 
00418 float fitPoints(const std::vector<cv::Point2f>& set1, const std::vector<cv::Point2f>& set2)
00419 {
00420     float dist = 0;
00421     for(size_t i = 0; i < set1.size(); i++)
00422     {
00423         float min_dist = 1e10;
00424         for(size_t j = 0; j < set2.size(); j++)
00425         {
00426             float _dist = length(set1[i] - set2[j]);
00427 //            min_dist = MIN(min_dist, _dist*_dist);
00428             if(_dist*_dist < min_dist)
00429             {
00430                 min_dist = _dist*_dist;
00431             }
00432         }
00433 
00434         dist += min_dist;
00435     }
00436 
00437     dist = sqrt(dist/set1.size());
00438 
00439     return dist;
00440 }
00441 
00442 float fitPointsSym(const std::vector<cv::Point2f>& set1, const std::vector<cv::Point2f>& set2)
00443 {
00444     float error1 = fitPoints(set1, set2);
00445     float error2 = fitPoints(set2, set1);
00446 
00447     return error1 + error2;
00448 }
00449 
00450 /* ******************************************************************************************* */
00451 
00452 
00453 int PointMatcher::addModel(const std::vector<KeyPointEx>& points)
00454 {
00455     template_points = points;
00456 
00457     for(size_t idx1 = 0; idx1 < points.size(); idx1++)
00458     {
00459         if(points[idx1].class_id < 0) continue;
00460 
00461         for(size_t idx2 = 0; idx2 < points.size(); idx2++)
00462         {
00463             if(points[idx2].class_id < 0) continue;
00464 
00465             if(idx1 == idx2) continue;
00466             float dist = length(points[idx1].pt - points[idx2].pt);
00467             if(dist > params.max_basis_length || dist < params.min_basis_length) continue;
00468 
00469             for(size_t idx3 = 0; idx3 < points.size(); idx3++)
00470             {
00471                 if(points[idx2].class_id < 0) continue;
00472 
00473                 if(idx3 == idx1 || idx3 == idx2) continue;
00474                 float dist1 = length(points[idx3].pt - points[idx1].pt);
00475                 float dist2 = length(points[idx3].pt - points[idx2].pt);
00476                 if(dist1 > params.max_basis_length || dist1 < params.min_basis_length ||
00477                    dist2 > params.max_basis_length || dist2 < params.min_basis_length) continue;
00478 
00479                 AffineBasis basis(points[idx1].pt, points[idx2].pt, points[idx3].pt, -1);
00480                 if(basis.getAngle() < params.min_angle || fabs(basis.getAngle() - PI) < params.min_angle) continue;
00481 
00482                 addModelBasis(points, basis);
00483             }
00484         }
00485     }
00486 
00487     printf("Added %d bases\n", hash.getBases().size());
00488 
00489     return 0;
00490 }
00491 
00492 void PointMatcher::addModelBasis(const std::vector<KeyPointEx>& points, const AffineBasis& basis)
00493 {
00494     int basis_id = hash.addBasis(basis);
00495     for(size_t i = 0; i < points.size(); i++)
00496     {
00497         hash.addEntry(basis_id, points[i].pt);
00498     }
00499 }
00500 
00501 int PointMatcher::match(const std::vector<KeyPointEx>& points, std::vector<float>& votes,
00502                         std::vector<std::pair<AffineBasis,AffineBasis> >& matched_bases) const
00503 {
00504     std::vector<int> basis_votes;
00505 
00506 #if defined(_VERBOSE)
00507     int votes_hist[] = {0, 0, 0, 0, 0, 0};
00508 #endif //_VERBOSE
00509 
00510     for(size_t idx1 = 0; idx1 < points.size(); idx1++)
00511     {
00512         if(points[idx1].class_id < 0) continue;
00513 
00514         int w = 0;
00515         if(abs(points[idx1].pt.x - 269) < 10 && abs(points[idx1].pt.y - 145) < 10)
00516         {
00517             w = 1;
00518         }
00519 
00520         std::vector<int> proximity_indices;
00521         getProximityPoints(points, points[idx1], params.max_basis_length, proximity_indices);
00522 
00523         size_t idx2 = 0;
00524         int k = 0;
00525         for(; k < 10; k++)
00526         {
00527             idx2 = proximity_indices[rand()%proximity_indices.size()];
00528             assert(points[idx2].class_id >= 0);
00529             if(idx1 == idx2) continue;
00530             float dist = length(points[idx1].pt - points[idx2].pt);
00531             if(dist > params.max_basis_length || dist < params.min_basis_length) continue;
00532             break;
00533         }
00534         if(k == 10) continue;
00535 
00536 #if defined(_VERBOSE)
00537         if(w == 1)
00538         {
00539             printf("checkpoint 1\n");
00540         }
00541 #endif //_VERBOSE
00542 
00543         size_t idx3 = 0;
00544         k = 0;
00545         for(; k < 10; k++)
00546         {
00547             idx3 = proximity_indices[rand()%proximity_indices.size()];
00548             assert(points[idx3].class_id >= 0);
00549             if(idx3 == idx1 || idx3 == idx2) continue;
00550             float dist1 = length(points[idx3].pt - points[idx1].pt);
00551             float dist2 = length(points[idx3].pt - points[idx2].pt);
00552             if(dist1 > params.max_basis_length || dist1 < params.min_basis_length ||
00553                dist2 > params.max_basis_length || dist2 < params.min_basis_length) continue;
00554 
00555             AffineBasis _basis(points[idx1].pt, points[idx2].pt, points[idx3].pt, -1);
00556             double angle = _basis.getAngle();
00557             if(angle < params.min_angle || fabs(angle - PI) < params.min_angle || cvIsNaN(angle)) continue;
00558 
00559             break;
00560         }
00561 
00562         if(k == 10) continue;
00563 
00564 #if defined(_VERBOSE)
00565         if(w == 1)
00566         {
00567             printf("checkpoint 2\n");
00568         }
00569 #endif //_VERBOSE
00570 
00571         AffineBasis basis(points[idx1].pt, points[idx2].pt, points[idx3].pt, -1);
00572         double angle = basis.getAngle();
00573         assert(angle >= params.min_angle && fabs(angle - PI) >= params.min_angle && !cvIsNaN(angle));
00574 
00575 #if defined(_VERBOSE)
00576         if(w == 1)
00577         {
00578             printf("checkpoint 3\n");
00579         }
00580 #endif //_VERBOSE
00581 
00582         basis_votes.assign(hash.getBases().size(), 0);
00583         matchBasis(points, basis, basis_votes);
00584 
00585 
00586         for(size_t votes_idx = 0; votes_idx < basis_votes.size(); votes_idx++)
00587         {
00588 #if defined(_VERBOSE)
00589             if(basis_votes[votes_idx] >= 0 && basis_votes[votes_idx] < 6)
00590             {
00591                 votes_hist[basis_votes[votes_idx]]++;
00592             }
00593 #endif //_VERBOSE
00594 
00595             if(basis_votes[votes_idx] < params.min_hash_votes) continue;
00596             AffineBasis template_basis = getBasis(votes_idx);
00597             cv::Point2f origin = template_basis.getOrigin();
00598             cv::Point2f vec[2];
00599             template_basis.getBasis(vec);
00600             vec[0] += origin;
00601             vec[1] += origin;
00602             if(length(origin - cv::Point2f(56, 53)) < 10 && length(vec[0] - cv::Point2f(51, 72)) < 10 &&
00603                length(vec[1] - cv::Point2f(106, 80)) < 10)
00604             {
00605                 int w = 1;
00606             }
00607 
00608             float validated_votes = validatePointMatch(template_points, template_basis, points, basis);
00609             float distortion_ratio = affineDistortionRatio(template_basis, basis);
00610 //            printf("basis origin %f,%f,%f, vec1 %f,%f,%f, vec2 %f,%f,%f, votes %f\n",
00611             if(validated_votes >= params.min_validated_votes && distortion_ratio > params.min_distortion_ratio)
00612             {
00613 #if defined(_VERBOSE)
00614                 printf("ratio = %f\n", distortion_ratio);
00615 #endif
00616                 std::pair<AffineBasis,AffineBasis> matched_basis(template_basis, basis);
00617                 votes.push_back(validated_votes);
00618                 matched_bases.push_back(matched_basis);
00619             }
00620         }
00621 
00622     }
00623 
00624 #if defined(_VERBOSE)
00625     printf("Votes:");
00626     for(int i = 0; i < 6; i++)
00627     {
00628         printf(" %d", votes_hist[i]);
00629     }
00630     printf("\n");
00631 #endif
00632 
00633 //    printf("Found %d bases\n", matched_bases.size());
00634 
00635     return 0;
00636 }
00637 
00638 void PointMatcher::matchBasis(const std::vector<KeyPointEx>& points, const AffineBasis& basis, std::vector<int>& votes) const
00639 {
00640     for(size_t i = 0; i < points.size(); i++)
00641     {
00642         cv::Point2f point = points[i].pt;
00643         cv::Point2f coords = basis.getCoords(point);
00644 
00645         const std::list<ModelBasisID>& entries = hash.getEntries(coords);
00646         for(std::list<ModelBasisID>::const_iterator it = entries.begin(); it != entries.end(); it++)
00647         {
00648             votes[*it]++;
00649         }
00650     }
00651 }
00652 
00653 size_t findClosestPoint(const std::vector<KeyPointEx>& set, KeyPointEx point, bool use_class_id = true)
00654 {
00655     float min_dist = 1e10;
00656     size_t min_idx = -1;
00657     for(size_t i = 0; i < set.size(); i++)
00658     {
00659         if(use_class_id && set[i].class_id != point.class_id) continue;
00660 
00661         float dist = length(set[i].pt - point.pt);
00662         if(dist < min_dist)
00663         {
00664             min_dist = dist;
00665             min_idx = i;
00666         }
00667     }
00668 
00669     return min_idx;
00670 }
00671 
00672 void findClosestPoint(const std::vector<KeyPointEx>& guess, const std::vector<KeyPointEx>& candidates, std::vector<KeyPointEx>& output, std::vector<bool>& is_detected, float max_dist)
00673 {
00674     output.resize(guess.size());
00675     is_detected.resize(guess.size());
00676     for(size_t i = 0; i < guess.size(); i++)
00677     {
00678         size_t idx = findClosestPoint(candidates, guess[i], false);
00679         KeyPointEx candidate = candidates[idx];
00680         float dist = length(guess[i].pt - candidate.pt);
00681         if(dist < max_dist)
00682         {
00683             output[i] = candidate;
00684             is_detected[i] = true;
00685         }
00686         else
00687         {
00688             output[i] = guess[i];
00689             is_detected[i] = false;
00690         }
00691     }
00692 }
00693 
00694 float validatePointMatch(const std::vector<KeyPointEx>& set1, const AffineBasis& basis1,
00695                         const std::vector<KeyPointEx>& set2, const AffineBasis& basis2)
00696 {
00697     const float min_dist = 5.0;
00698     int votes = 0;
00699     for(size_t idx1 = 0; idx1 < set1.size(); idx1++)
00700     {
00701         // project the point
00702         KeyPointEx p1_projected = KeyPointEx(basis2.getPoint(basis1.getCoords(set1[idx1].pt)), set1[idx1].size, set1[idx1].class_id);
00703 
00704         // find the closest point
00705         bool use_class_id = true;
00706 #if defined(_WITHOUT_CLASS_ID)
00707         use_class_id = false;
00708 #endif //_WITHOUT_CLASS_ID
00709         int idx2 = findClosestPoint(set2, KeyPointEx(p1_projected), use_class_id);
00710         if(idx2 < 0) continue;
00711         float dist = length(set2[idx2].pt - p1_projected.pt);
00712         if(dist < min_dist) votes++;
00713     }
00714 
00715     return float(votes);
00716 }
00717 
00718 void getProximityPoints(const std::vector<KeyPointEx>& points, KeyPointEx point, float max_dist, std::vector<int>& indices)
00719 {
00720     for(size_t i = 0; i < points.size(); i++)
00721     {
00722 //        if(points[i].class_id != point.class_id) continue; I don't know why I did that!
00723         if(points[i].class_id < 0) continue;
00724 
00725         if(length(points[i].pt - point.pt) < max_dist)
00726         {
00727             indices.push_back(i);
00728         }
00729     }
00730 }
00731 
00732 double affineDistortionRatio(const AffineBasis& basis1, const AffineBasis& basis2)
00733 {
00734     cv::Point2f _basis1[2], _basis2[2];
00735     basis1.getBasis(_basis1);
00736     basis2.getBasis(_basis2);
00737     double basis1_l1 = cv::norm(_basis1[0]);
00738     double basis1_l2 = cv::norm(_basis1[1]);
00739     double basis2_l1 = cv::norm(_basis2[0]);
00740     double basis2_l2 = cv::norm(_basis2[1]);
00741 
00742     double ratio1 = basis1_l1/basis1_l2;
00743     double ratio2 = basis2_l1/basis2_l2;
00744     double ratio = ratio1 < ratio2 ? ratio1/ratio2 : ratio2/ratio1;
00745 
00746     return ratio;
00747 }


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Nov 28 2013 11:46:23