TestCOSAC.cpp

#include <MathAlgo/COSAC.hh>
#include "MeanDoubleRANSACEvaluator.hh"
#include "TestMeanDoubleHelper.hh"

using namespace BIAS;
using namespace std;

Random MyRandom;
bool verbose = false;
const double max_outlier_fraction = 0.5;

void DebugOut(const unsigned t, const unsigned num_data, 
              const unsigned num_outliers, const double sigma3, 
              const int ret_val_solve_master, const vector<double> &data, 
              const vector<bool> &gt_inliers, const double gt_mean,
              const double &est_mean, const unsigned est_num_inliers,
              const double &score, const double &max_score);

int main(int argc, char *argv[])
{
  MeanDoubleRANSACEvaluator mdch;
  COSAC<double> cosac(&mdch);
  const unsigned min_sample_size = 
    mdch.GetMinNumSamplesForSolutionComputation();

  const unsigned num_tries = 1000;
  for (unsigned t=0; t<num_tries; t++){
    unsigned num_data = 
      (unsigned)rint(MyRandom.GetUniformDistributed(1., 30.));
    const unsigned min_num_outliers = (unsigned)floor(0.1*(double)num_data);
    unsigned max_num_outliers = 
      (unsigned)floor(max_outlier_fraction*(double)num_data);
    if (num_data>=min_sample_size && max_num_outliers+min_sample_size>num_data){
      max_num_outliers = num_data-min_sample_size;
    }
    const unsigned num_outliers = 
      MyRandom.GetUniformDistributedInt(min_num_outliers, max_num_outliers);
    const double sigma3 = 
      MyRandom.GetUniformDistributed(0., 1.0);
    vector<double> data;
    vector<bool> gt_inliers, est_inliers;
    double gt_mean, est_mean;
    if (verbose) {
      cout << "------------ "<<t<<" -------------\n";
    }
    GenerateRandomData(MyRandom,num_data, num_outliers, sigma3, min_sample_size, data,
                       gt_inliers, gt_mean);
    if (verbose) {
      cout << "  "<<num_data<<" samples ("<<num_outliers<<" outliers)\n"
           << "  mean: "<<gt_mean<<" \tsigma3: "<<sigma3<<endl;
    }
    double inlying_data_fraction =  
      1.0 - ((double)num_outliers / (double)num_data) - 0.01;
    const double max_score = 100.0;
    double score;
    unsigned num_inliers;
    try{
      mdch.Init(data, sigma3);
    } catch (BaseException &ex) {
        cout<<"Error:"<<ex.What()<<endl;
      if (data.size()>=mdch.GetMinNumSamplesForSolutionComputation() ){
        BIASABORT;
      }
    }
    //cosac.AddDebugLevel("D_COSAC_Evaluate");

    if (t==577){
      cosac.AddDebugLevel("D_COSAC_GetSolution");
    }
    int res = cosac.SolveMaster(inlying_data_fraction, max_score, est_mean, 
                                est_inliers, num_inliers, score);
    if (res!=0){
      if (num_data < min_sample_size){
        if (res != COSAC_NOT_ENOUGH_MEASUREMENTS){
          cerr << "\n----\ntry "<<t<<": ";
          cerr << "invalid return value ("<< res
               <<") when not enough measurements are present\n";
          BIASABORT;
        }
        continue;
      }
      DebugOut(t, num_data, num_outliers, sigma3, res, data, gt_inliers, 
               gt_mean, est_mean, num_inliers, score, max_score);
               
      if (res<0){
        BIASABORT;
      }
    } 
    if (verbose){
      cout << "  found solution: "<<est_mean<<" (gt is "<<gt_mean<<", diff "
           <<fabs(est_mean-gt_mean)/sigma3<<"*sigma3)\n";
    }
    if (!CheckResult(est_inliers, est_mean, gt_inliers, gt_mean, sigma3)){
      DebugOut(t, num_data, num_outliers, sigma3, res, data, gt_inliers, 
               gt_mean, est_mean, num_inliers, score, max_score);
      cerr << "wrong result\n";
      BIASABORT;
    }
    if (!verbose) {
      cout << "#" << flush;
    }
  }

  if (!verbose) {
    cout << "\n";
  }

  return 0;
}


void DebugOut(const unsigned t, const unsigned num_data, 
              const unsigned num_outliers, const double sigma3, 
              const int ret_val_solve_master, const vector<double> &data, 
              const vector<bool> &gt_inliers, const double gt_mean,
              const double &est_mean, const unsigned est_num_inliers,
              const double &score, const double &max_score)
{
  cerr << "\n---- try "<<t<<" -------\n" 
       << "(num samples: "<<num_data
       <<", num_outliers: "<<num_outliers<<", sigma3: "<<sigma3<<"): res="
       <<ret_val_solve_master<<endl;
  if (ret_val_solve_master<0){
    cerr << "Error computing solution\n";
  } else if (ret_val_solve_master>0){
    cerr << "Solution violates min quality criteria\n";
    cout << "estimated num inliers: "<<est_num_inliers
         << " (expected min "<<num_data-num_outliers<<")\n";
    cout << "estimated score: "<<score
         << " (expected max "<<max_score<<")\n";
  }
  for (unsigned k=0; k<data.size(); k++){
    cout << data[k]<<"("<<((gt_inliers[k])?("+"):("-"))<< ")  ";
  }
  cout << endl;
  cout << "gt_mean : "<<gt_mean<<endl;
  cout << "est_mean: "<<est_mean<<endl;
  cout << "est num inliers: "<<est_num_inliers<<endl;
  cout << "est score: "<<score<<endl;
}