TestPreemptiveRANSAC.cpp

#include <MathAlgo/PreemptiveRANSAC.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Debug/TimeMeasure.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[])
{
  const unsigned num_tries = 200;
  MeanDoubleRANSACEvaluator mdch;
  PreemptiveRANSAC<double> mdpr(&mdch);
  const unsigned min_sample_size = 
    mdch.GetMinNumSamplesForSolutionComputation();
  const int debug_try = -1;
//   mdpr.AddDebugLevel("D_PRANSAC_SolveMaster");
//   mdpr.AddDebugLevel("D_PRANSAC_EvalOrder");
//   mdpr.AddDebugLevel("D_PRANSAC_Evaluate");
//   mdpr.AddDebugLevel("D_PRANSAC_InitialReject");
//   mdpr.AddDebugLevel("D_PRANSAC_Inlier");
//   mdpr.AddDebugLevel("D_PRANSAC_GetSolution");
//   mdpr.AddDebugLevel("D_PRANSAC_GenerateSolutions");
  TimeMeasure timer;

  for (unsigned t=0; t<num_tries; t++){
    unsigned num_data = 
      (unsigned)rint(MyRandom.GetUniformDistributed(5., 200.));
    if (t==0) {
      // test what happens when there are barely enough samples
      num_data = min_sample_size;
    } else if (t==1 && min_sample_size>1){
      // test what happens when not enough data is present
      num_data = min_sample_size-1;
    }
    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 (t==(unsigned)debug_try) verbose = true;
    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;
    }
    if (t==(unsigned)debug_try){
      mdpr.AddDebugLevel("D_PRANSAC_SolveMaster");
      mdpr.AddDebugLevel("D_PRANSAC_Evaluate");
      mdpr.AddDebugLevel("D_PRANSAC_InitialReject");
      mdpr.AddDebugLevel("D_PRANSAC_GetSolution");
      mdpr.AddDebugLevel("D_PRANSAC_GenerateSolutions");
    }
    try{
      mdch.Init(data, sigma3);
    } catch (BaseException &ex) {
        cout<<"Exception failed:"<<ex.What()<<endl;
      if (data.size()>=mdch.GetMinNumSamplesForSolutionComputation() ){         
        BIASABORT;
      }
    }
    

    double inlying_data_fraction = 1.0-max_outlier_fraction-0.01;
    const double max_score = 100.0;
    double score;
    unsigned num_inliers;
    const bool auto_compute_max_samples = true;
    timer.Reset();
    timer.Start();
    mdpr.Init();
    int res = mdpr.SolveMaster(inlying_data_fraction, max_score, est_mean, 
                               est_inliers, num_inliers, score, 
                               auto_compute_max_samples);
    timer.Stop();
    // cout << setw(3) << t << ": "<<num_data<<" / "<<num_outliers<<"  time: "
//          <<timer.GetUserTime()<<endl;
    if (res!=0){
      if (num_data < min_sample_size){
        if (res != PRANSAC_NOTE_ENOUGH_SAMPLES){
          cerr << "invalid return value when not enough samples 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;
}