constellation.cpp
Go to the documentation of this file.
00001 /*
00002  *  constellation.cpp
00003  *  online_patch
00004  *
00005  *  Created by Victor  Eruhimov on 5/3/09.
00006  *  Copyright 2009 Argus Corp. All rights reserved.
00007  *
00008  */
00009 
00010 #include "outlet_pose_estimation/detail/constellation.h"
00011 #include "outlet_pose_estimation/detail/outlet_tuple.h"
00012 #include "outlet_pose_estimation/detail/affine_transform.h"
00013 
00014 using namespace std;
00015 
00016 int iterate_indices(vector<int>& indices, int max_index, int min_valid_indices, int* workspace)
00017 {
00018     int indices_count = indices.size();
00019     while(1)
00020     {
00021         // increment indices
00022         for(int i = 0; i < indices_count; i++)
00023         {
00024             indices[i]++;
00025             if(indices[i] < max_index) 
00026             {
00027                 break;
00028             }
00029             else
00030             {
00031                 indices[i] = -1;
00032             }    
00033         }
00034         
00035         // now check indices
00036         int valid_indices = 0;
00037         for(int j = 0; j < max_index; j++) workspace[j] = 0;
00038         int repeat_flag = 0;
00039         
00040         for(int i = 0; i < indices_count; i++) 
00041         {
00042             if(indices[i] >= 0)
00043             {
00044                 valid_indices++;
00045 
00046                 if(++workspace[indices[i]] > 1)
00047                 {
00048                     repeat_flag = 1;
00049                 }
00050             }
00051         }
00052         
00053         if(repeat_flag == 0 && valid_indices >= min_valid_indices)
00054         {
00055             return 0;
00056         }
00057         
00058         if(valid_indices == 0)
00059         {
00060             return -1;
00061         }
00062     }
00063 }
00064 
00065 int validate_parts(const vector<feature_t>& train, const vector<feature_t>& test, const vector<int>& indices, int* min_part_count)
00066 {
00067     int part_count[] = {0, 0};
00068     const int min_valid_parts = 5; // since we use homography matching, almost any 4 points will be matched with low error
00069     
00070     for(int i = 0; i < (int)indices.size(); i++)
00071     {
00072         if(indices[i] == -1) continue;
00073         
00074         if(test[i].class_id != train[indices[i]].class_id)
00075         {
00076             return -1;
00077         }
00078         
00079         part_count[train[indices[i]].class_id]++;
00080     }
00081     
00082     if(part_count[0] < min_part_count[0] || part_count[1] < min_part_count[1] || 
00083        part_count[0] + part_count[1] < min_valid_parts)
00084     {
00085         return -1;
00086     }
00087     
00088     return 0;
00089 }
00090 
00091 float min_idx(float v1, float v2, int i1, int i2)
00092 {
00093     if(i1 && i2)
00094     {
00095         return MIN(v1, v2);
00096     }
00097     else if(i1)
00098     {
00099         return v1;
00100     }
00101     else return v2;        
00102 }
00103 
00104 float max_idx(float v1, float v2, int i1, int i2)
00105 {
00106     if(i1 && i2)
00107     {
00108         return MAX(v1, v2);
00109     }
00110     else if(i1)
00111     {
00112         return v1;
00113     }
00114     else return v2;        
00115 }
00116 
00117 int validate_order(const vector<feature_t>& train, const vector<feature_t>& test, const vector<int>& indices)
00118 {
00119     const int min_offset = 10;
00120     vector<int> parts_index;
00121     
00122     const int parts_number = 6;
00123     CvPoint centers[parts_number];
00124     parts_index.assign(parts_number, 0);
00125     
00126     for(int i = 0; i < (int)indices.size(); i++)
00127     {
00128         if(indices[i] == -1) continue;
00129         centers[indices[i]] = test[i].pt;
00130         parts_index[indices[i]] = 1;
00131     }
00132 
00133     if((centers[0].x > centers[4].x)*parts_index[0]*parts_index[4] || 
00134        (centers[4].x > centers[1].x)*parts_index[4]*parts_index[1] ||
00135        (centers[2].x > centers[5].x)*parts_index[2]*parts_index[5] || 
00136        (centers[5].x > centers[3].x)*parts_index[5]*parts_index[3])
00137     {
00138         return -1;
00139     }
00140     
00141     if((min_idx(centers[0].y, centers[1].y, parts_index[0], parts_index[1]) < centers[4].y)*
00142             (parts_index[0] | parts_index[1])*parts_index[4] || 
00143         (min_idx(centers[2].y, centers[3].y, parts_index[2], parts_index[3]) < centers[5].y)*
00144             (parts_index[2] | parts_index[3])*parts_index[5] || 
00145         (centers[5].y < max_idx(centers[0].y, centers[1].y, parts_index[0], parts_index[1]) + min_offset)*
00146             (parts_index[0] | parts_index[1])*parts_index[5] ||
00147        (max_idx(centers[0].y, centers[1].y, parts_index[0], parts_index[1]) + min_offset > 
00148         min_idx(centers[2].y, centers[3].y, parts_index[2], parts_index[3]))*
00149        (parts_index[0] | parts_index[1])*(parts_index[2] | parts_index[3]))
00150     {
00151         return -1;
00152     }
00153     
00154     return 0;
00155 }
00156 
00157 float calc_set_std(const vector<feature_t>& features, const vector<int>& indices)
00158 {
00159     CvPoint2D32f sum = cvPoint2D32f(0, 0);
00160     CvPoint2D32f sum2 = cvPoint2D32f(0, 0);
00161     int count = 0;
00162     
00163     if(indices.size() > 0)
00164     {
00165         for(int i = 0; i < (int)indices.size(); i++)
00166         {
00167             if(indices[i] < 0) continue;
00168             
00169             sum.x += features[i].pt.x;
00170             sum.y += features[i].pt.y;
00171             sum2.x += features[i].pt.x*features[i].pt.x;
00172             sum2.y += features[i].pt.y*features[i].pt.y;
00173             
00174             count++;
00175         }
00176     }
00177     else
00178     {
00179         for(int i = 0; i < (int)features.size(); i++)
00180         {
00181             sum.x += features[i].pt.x;
00182             sum.y += features[i].pt.y;
00183             sum2.x += features[i].pt.x*features[i].pt.x;
00184             sum2.y += features[i].pt.y*features[i].pt.y;
00185         }
00186         
00187         count = features.size();
00188     }
00189     
00190     float sigmax = sqrt(sum2.x/count - sum.x*sum.x/(count*count));
00191     float sigmay = sqrt(sum2.y/count - sum.y*sum.y/(count*count));
00192     
00193     return MAX(sigmax, sigmay);
00194 }
00195 
00196 float CalcReprojectionError(CvMat* src_points, CvMat* dst_points, CvMat* src_proj_points, CvMat* homography)
00197 {
00198     cvPerspectiveTransform(src_points, src_proj_points, homography);
00199     float error = 0.0f;
00200     for(int i = 0; i < dst_points->rows; i++)
00201     {
00202         float* dst_points_ptr = (float*)(dst_points->data.ptr + i*dst_points->step);
00203         float* src_proj_points_ptr = (float*)(src_proj_points->data.ptr + i*src_proj_points->step);
00204         error += sqrt((dst_points_ptr[0] - src_proj_points_ptr[0])*(dst_points_ptr[0] - src_proj_points_ptr[0]) + 
00205                       (dst_points_ptr[1] - src_proj_points_ptr[1])*(dst_points_ptr[1] - src_proj_points_ptr[1]));
00206     }
00207     
00208     error /= dst_points->rows;
00209     
00210     return error;
00211 }
00212 
00213 void count_parts(const vector<feature_t>& features, int* min_part_count, int parts_number)
00214 {
00215     int* part_count = new int[parts_number];
00216     
00217     for(int i = 0; i < parts_number; i++) part_count[i] = 0;
00218 
00219     for(int i = 0; i < (int)features.size(); i++)
00220     {
00221         part_count[features[i].class_id]++;
00222     }
00223     
00224     for(int i = 0; i < parts_number; i++)
00225     {
00226         min_part_count[i] = MIN(min_part_count[i], part_count[i]);
00227     }
00228     
00229     delete []part_count;
00230 }
00231 
00232 void DetectObjectConstellation(const vector<feature_t>& train, const vector<feature_t>& input, CvMat* homography, vector<int>& indices)
00233 {
00234     int parts_number = train.size();
00235     int test_feature_count = input.size();
00236     indices.resize(test_feature_count);
00237     
00238     for(int i = 0; i < test_feature_count; i++) indices[i] = -1;
00239     
00240     if(test_feature_count > 9)
00241     {
00242         printf("The number of features is %d, exiting...\n", test_feature_count);
00243         return;
00244     }
00245     
00246     const int min_valid_indices = 4;
00247     int* workspace = new int[parts_number];
00248     int ret_flag = 0;
00249     
00250     CvMat* _train_points = cvCreateMat(parts_number, 2, CV_32FC1);
00251     CvMat* _test_points = cvCreateMat(test_feature_count, 2, CV_32FC1);
00252     
00253     vector<int> min_indices = indices;
00254     float min_error = 1e10;
00255     int min_part_count[] = {3, 1};
00256     count_parts(input, min_part_count, 2);
00257     CvMat* min_homography = cvCloneMat(homography);
00258     
00259     float train_set_size = calc_set_std(train);
00260 //    printf("Train set size = %f\n", train_set_size);
00261     const float set_size_factor = 2.0f;
00262     
00263 //    printf("Starting the parts search...\n");
00264     while(1)
00265     {
00266         ret_flag = iterate_indices(indices, parts_number, min_valid_indices, workspace);
00267         if(ret_flag == -1)
00268         {
00269             break;
00270         }
00271         
00272         if(validate_parts(train, input, indices, min_part_count) == -1)
00273         {
00274             continue;
00275         }
00276         
00277         if(validate_order(train, input, indices) == -1)
00278         {
00279             continue;
00280         }
00281         
00282         if(calc_set_std(input, indices) > train_set_size*set_size_factor)
00283         {
00284             continue;
00285         }
00286 
00287 //        printf("Tested indices %d%d%d%d%d%d\n", indices[0], indices[1], indices[2], indices[3], indices[4], indices[5]);
00288         
00289 #if defined(_HOMOGRAPHY)
00290         // fill train and test points
00291         int point_count = 0;
00292         for(int i = 0; i < test_feature_count; i++)
00293         {
00294             if(indices[i] == -1) continue;
00295             
00296             cvmSet(_train_points, point_count, 0, train[indices[i]].center.x);
00297             cvmSet(_train_points, point_count, 1, train[indices[i]].center.y);
00298             cvmSet(_test_points, point_count, 0, input[i].center.x);
00299             cvmSet(_test_points, point_count, 1, input[i].center.y);
00300             point_count++;
00301         }
00302         
00303         CvMat train_points, test_points;
00304         CvRect subrect = cvRect(0, 0, 2, point_count);
00305         cvGetSubRect(_train_points, &train_points, subrect);
00306         cvGetSubRect(_test_points, &test_points, subrect);
00307         CvMat src_proj_points;
00308         cvGetSubRect(_test_points2, &src_proj_points, subrect);
00309 
00310         const double ransacReprojThreshold = 5.0;
00311         cvFindHomography(&train_points, &test_points, homography, CV_RANSAC, ransacReprojThreshold);
00312                             
00313         train_points.cols = 1;
00314         train_points.type = hack->type;
00315         test_points.cols = 1;
00316         test_points.type = hack->type;
00317         src_proj_points.cols = 1;
00318         src_proj_points.type = hack->type;
00319         
00320         float error = CalcReprojectionError(&train_points, &test_points, &src_proj_points, homography);
00321 #else
00322         // fill the points
00323         vector<CvPoint> p1, p2;
00324         for(int i = 0; i < test_feature_count; i++)
00325         {
00326             if(indices[i] == -1) continue;
00327             
00328             p1.push_back(train[indices[i]].pt);
00329             p2.push_back(input[i].pt);
00330         }
00331         
00332         FindAffineTransform(p1, p2, homography);
00333         
00334         float error = CalcAffineReprojectionError(p1, p2, homography);
00335         
00336 #endif //_HOMOGRAPHY
00337         if(error < min_error)
00338         {
00339             min_error = error;
00340             min_indices = indices;
00341             cvCopy(homography, min_homography);
00342         }
00343         
00344     }
00345 //    printf("Parts search finished\n");
00346 //    printf("Final set size = %f\n", calc_set_std(input, min_indices));
00347     
00348     indices = min_indices;
00349     cvCopy(min_homography, homography);
00350     
00351     delete []workspace;
00352     cvReleaseMat(&min_homography);
00353     
00354     cvReleaseMat(&_train_points);
00355     cvReleaseMat(&_test_points);
00356 }
00357 
00358 void features2points(const vector<feature_t>& features, vector<CvPoint2D32f>& points)
00359 {
00360     for(int i = 0; i < (int)features.size(); i++)
00361     {
00362         points.push_back(cvPoint2D32f(features[i].pt.x, features[i].pt.y));
00363     }
00364 }
00365 
00366 void points2features(const vector<CvPoint2D32f>& points, vector<feature_t>& features)
00367 {
00368     features.resize(points.size());
00369     for(int i = 0; i < (int)points.size(); i++)
00370     {
00371         features[i].pt = cvPoint(points[i].x, points[i].y);
00372     }
00373 }
00374 
00375 void map_features(const vector<feature_t>& src_features, CvMat* homography, vector<feature_t>& dst_features)
00376 {
00377     vector<CvPoint2D32f> src_points, dst_points;
00378     
00379     features2points(src_features, src_points);
00380     map_vector_homography(src_points, homography, dst_points);
00381     dst_features = src_features;
00382     points2features(dst_points, dst_features);
00383 }
00384 
00385 void InferMissingObjects(const vector<feature_t>& train, const vector<feature_t>& input, CvMat* homography, const vector<int>& indices, 
00386                          vector<feature_t>& full)
00387 {
00388     int parts_number = train.size();
00389     
00390     vector<feature_t> train_mapped;
00391 #if defined(_HOMOGRAPHY)
00392     map_features(train, homography, train_mapped);
00393 #else
00394     MapFeaturesAffine(train, train_mapped, homography); //TBD need to unify the transformation or get rid of homography
00395 #endif //_HOMOGRAPHY
00396     
00397     // find missing indices
00398     vector<int> miss_indices;
00399     miss_indices.assign(parts_number, 0);
00400     
00401     full.resize(parts_number);
00402     for(int i = 0; i < (int)indices.size(); i++)
00403     {
00404         if(indices[i] >= 0)
00405         {
00406             miss_indices[indices[i]] = 1;
00407             full[indices[i]] = input[i];
00408         }
00409     }
00410     
00411     for(int i = 0; i < (int)miss_indices.size(); i++)
00412     {
00413         if(miss_indices[i] == 0)
00414         {
00415             full[i] = train_mapped[i];
00416         }
00417     }
00418 }
00419 
00420 void FilterOutletFeatures(const vector<feature_t>& src_features, vector<feature_t>& dst_features, float max_dist)
00421 {
00422     vector<int> ground_idx;
00423     // find all ground hole candidates
00424     for(int i = 0; i < (int)src_features.size(); i++)
00425     {
00426         if(src_features[i].class_id == 1)
00427         {
00428             ground_idx.push_back(i);
00429         }
00430     }
00431     
00432     // find all pairs of ground holes that are close enough to each other and filter out one of them
00433     vector<int> ground_idx_filtered;
00434     for(int i = 0; i < (int)ground_idx.size(); i++)
00435     {
00436         int flag = 0;
00437         for(int j = 0; j < (int)ground_idx_filtered.size(); j++)
00438         {
00439             if(length(src_features[ground_idx[i]].pt - src_features[ground_idx_filtered[j]].pt) < max_dist)
00440             {
00441                 flag = 1;
00442                 break;
00443             }
00444         }
00445         
00446         if(flag == 0)
00447         {
00448             ground_idx_filtered.push_back(ground_idx[i]);
00449         }
00450     }
00451     
00452     ground_idx = ground_idx_filtered;
00453 //    printf("Found %d clusters\n", (int)ground_idx.size());
00454     
00455     // now filter out all features that are far enough from ground holes
00456     vector<int> indices;
00457     indices.assign(src_features.size(), 0);
00458     for(int i = 0; i < (int)ground_idx.size(); i++)
00459     {
00460         for(int j = 0; j < (int)src_features.size(); j++)
00461         {
00462             if(length(src_features[j].pt - src_features[ground_idx[i]].pt) < max_dist)
00463             {
00464                 indices[j] = 1;
00465             }
00466         }
00467     }
00468     
00469     // now copy the features
00470     for(int i = 0; i < (int)src_features.size(); i++)
00471     {
00472         if(indices[i])
00473         {
00474             dst_features.push_back(src_features[i]);
00475         }
00476     }
00477 }
00478 
00479 //-----------------------
00480 void FilterOutletFeatures(const vector<feature_t>& src_features, vector<feature_t>& dst_features, vector<int>& dst_indexes, float max_dist)
00481 {
00482     vector<int> ground_idx;
00483         dst_indexes.clear();
00484     // find all ground hole candidates
00485     for(int i = 0; i < (int)src_features.size(); i++)
00486     {
00487         if(src_features[i].class_id == 1)
00488         {
00489             ground_idx.push_back(i);
00490         }
00491     }
00492     
00493     // find all pairs of ground holes that are close enough to each other and filter out one of them
00494     vector<int> ground_idx_filtered;
00495     for(int i = 0; i < (int)ground_idx.size(); i++)
00496     {
00497         int flag = 0;
00498         for(int j = 0; j < (int)ground_idx_filtered.size(); j++)
00499         {
00500             if(length(src_features[ground_idx[i]].pt - src_features[ground_idx_filtered[j]].pt) < max_dist)
00501             {
00502                 flag = 1;
00503                 break;
00504             }
00505         }
00506         
00507         if(flag == 0)
00508         {
00509             ground_idx_filtered.push_back(ground_idx[i]);
00510         }
00511     }
00512     
00513     ground_idx = ground_idx_filtered;
00514     vector<int> indices;
00515     indices.assign(src_features.size(), 0);
00516     for(int i = 0; i < (int)ground_idx.size(); i++)
00517     {
00518         for(int j = 0; j < (int)src_features.size(); j++)
00519         {
00520             if(length(src_features[j].pt - src_features[ground_idx[i]].pt) < max_dist)
00521             {
00522                 indices[j] = 1;
00523             }
00524         }
00525     }
00526     
00527     // now copy the features
00528     for(int i = 0; i < (int)src_features.size(); i++)
00529     {
00530         if(indices[i])
00531         {
00532             dst_features.push_back(src_features[i]);
00533                         dst_indexes.push_back(i);
00534         }
00535     }
00536 }
00537 //----------------------------------
00538 void ClusterOutletFeatures(const vector<feature_t>& src_features, vector<feature_t>& clusters, float max_dist)
00539 {
00540     vector<int> ground_idx;
00541     // find all ground hole candidates
00542     for(int i = 0; i < (int)src_features.size(); i++)
00543     {
00544         if(src_features[i].class_id == 1)
00545         {
00546             ground_idx.push_back(i);
00547         }
00548     }
00549     
00550     // find all pairs of ground holes that are close enough to each other and filter out one of them
00551     vector<int> ground_idx_filtered;
00552     for(int i = 0; i < (int)ground_idx.size(); i++)
00553     {
00554         int flag = 0;
00555         for(int j = 0; j < (int)ground_idx_filtered.size(); j++)
00556         {
00557             if(length(src_features[ground_idx[i]].pt - src_features[ground_idx_filtered[j]].pt) < max_dist)
00558             {
00559                 flag = 1;
00560                 break;
00561             }
00562         }
00563         
00564         if(flag == 0)
00565         {
00566             ground_idx_filtered.push_back(ground_idx[i]);
00567         }
00568     }
00569     
00570     ground_idx = ground_idx_filtered;
00571 //    printf("Found %d clusters\n", (int)ground_idx.size());
00572     
00573     // now copy the features
00574     for(int i = 0; i < (int)ground_idx.size(); i++)
00575     {
00576         clusters.push_back(src_features[ground_idx[i]]);
00577     }
00578 }
00579 
00580 void SelectNeighborFeatures(const vector<feature_t>& src_features, CvPoint center, vector<feature_t>& dst_features, float max_dist)
00581 {
00582     for(int i = 0; i < (int)src_features.size(); i++)
00583     {
00584         if(length(src_features[i].pt - center) < max_dist)
00585         {
00586             dst_features.push_back(src_features[i]);
00587         }
00588     }
00589 }


outlet_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 14:29:52