ExampleRANSACPlane.cpp

#include <Base/Common/BIASpragma.hh>

#include "Base/Math/Random.hh"
#include <vector>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <float.h>
#include "ExampleRANSACPlane.hh"

using namespace BIAS;
using namespace std;

void Read3DPointsFromFile(char *filename, std::vector<HomgPoint3D> &points)
{
  ifstream infile(filename);
  if (!infile) {
    BIASERR("Failed to open file " << filename);
    return;
  }
  char c;
  HomgPoint3D point(0,0,0,1);
  int i = 0;
  while (!infile.eof()) {
    infile >> point[0] >> point[1] >> point[2] >> c;
    if (!infile.eof()) {
      points.push_back(point);
      cout << "- point " << i << " : "<< point << endl;
      i++;
    }
  }
}


void GeneratePlanar3DPoints(std::vector<HomgPoint3D>& points,
                            unsigned int numInliers, unsigned int numOutliers)
{
  // generate points on x/y-plane at z-coordinate = 2
  Random randomizer;
  double range = 10.0;
  double noise = 0.1;
  const double z = 2.0;
  HomgPoint3D point3D;
  for (unsigned int i = 0; i < (numInliers + numOutliers); i++){
    point3D[0] = randomizer.GetUniformDistributed(-range, range);
    point3D[1] = randomizer.GetUniformDistributed(-range, range);
    point3D[2] = z + randomizer.GetUniformDistributed(-noise, noise);;
    point3D[3] = 1.0;
    if (i >= numInliers) {
      // make this point an outlier
      point3D[2] = randomizer.GetUniformDistributed(-range, range);
    }
    points.push_back(point3D);
    cout << "- point " << i << " : " << point3D << " ("
         << (i >= numInliers ? "outlier" : "inlier") << ")" << endl;
  }
}

// -----------------------------------------------------------------

/** @example ExampleRANSACPlane.cpp
    @relates PlaneRANSAC, RANSAC
    @ingroup g_examples    
    @brief   Simple example for 3D plane fitting with RANSAC class
             BIAS::PlaneRANSAC.

    This is an example file on how to use the RANSAC algorithm.
    It tries to find a dominant plane in a number of 3D points.

    @author MIP
*/

int main(int argc, char *argv[])
{
  int res = 0;
  const unsigned int numPoints = 20;

  // inlier_fraction is passed onto SolveMaster and contains the assumed
  // fraction of inliers in the data. This parameter is used to determine
  // the number of samples drawn and is therefore important, unless the
  // number of samples is set explicitly
  double inlier_fraction = 0.8;

  // determines if promising solutions are to be refined based on all inliers or not
  bool refine_solution = true;

  // max distance between point and plane for point to be classified as inlier
  double  maxsquareddistance = 0.01;

  HomgPlane3D solution;
  vector<bool> inliers;
  bool debug = false; //true;
  PlaneRANSAC ransac;
  vector<HomgPoint3D> points;

  // set ground truth plane
  HomgPlane3D gt_plane(0, 0, -1, 2); // x/y-plane with z = 2

  // load or create input data
  if (argc < 2) {
    cout << endl << "Generating " << numPoints << " random noisy points "
         << "for testing..." << endl;
    const unsigned int outliers = (int)rint((1.0 - inlier_fraction) * numPoints);
    GeneratePlanar3DPoints(points, numPoints - outliers, outliers);
  } else {
    cout << endl << "Reading 3d points for testing from file " << argv[1] << "..." << endl;
    Read3DPointsFromFile(argv[1], points);
  }

  if (debug) {
    ransac.AddDebugLevel("D_RANSAC_SAMPLES");
    ransac.AddDebugLevel("D_RANSAC_SOLVE_MASTER");
    ransac.AddDebugLevel("D_DOUBLE_RANSAC_SOLUTION");
  }

  // initialize RANSAC parameters
  ransac.Init(points, maxsquareddistance, refine_solution, inlier_fraction, -1.0);

  // start computation
  ransac.SolveMaster(inlier_fraction, solution, inliers);

  // show results
  cout << endl << "Inlier/outlier points after computation : " << endl;
  for (unsigned int i = 0; i < inliers.size(); i++){
    cout << "- point " << i << " is " << (inliers[i] ? "inlier" : "outlier") << endl;
  }
  cout << endl << "Final solution : " << solution << endl
       << "Ground truth : " << gt_plane << endl
       << "Difference : " << (solution - gt_plane).NormL2() << endl << endl;

  return res;
}