ExampleAbsoluteOrientation.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

/**
  @example ExampleAbsoluteOrientation.cpp
  @relates AbsoluteOrientation, AbsoluteOrientationRANSAC
  @brief   Example for similarity transformation estimation between
           two coordinate frames from corresponding 3D points.

  AbsoluteOrientation estimates the relationship between two coordinate
  systems using pairs of measurements of the coordinates of a number of
  3D points in both systems. A SVD-based solution to the least-squares
  problem for at least four points based on Horn's approach is used.

  @note The transformation between the coordinate systems is given
  as rotation dR, translation dt, and isometric scale ds, such that
  points X in the first coordinate system are related to points Y
  in the second coordinate system by Y = ds*dR*X + dt.

  @see AbsoluteOrientation
  @ingroup g_examples
  @author esquivel
  @date 12/2008
 */

#include <Base/Math/Random.hh>
#include <Geometry/AbsoluteOrientation.hh>
#include <Geometry/AbsoluteOrientationRANSAC.hh>
#include <Utils/Param.hh>
#include <iostream>
#include <string>
#include <vector>

using namespace BIAS;
using namespace std;

int main(int argc, char *argv[])
{
  // register parameters
  BIAS::Param params;
  int* numParam = params.AddParamInt("num", "Number of 3D points", 10, 3,
                                     std::numeric_limits<int>::max(), 'n', 0);
  double* rangeParam = params.AddParamDouble("range", "Max. distance of 3D points "
                                             "(in m)", 10.0, 1.e-8, 1e16, 'r', 0);
  double* errorParam = params.AddParamDouble("error", "Max. error on 3D points "
                                             "(in percent)", 0.01, 0.0, 1.0, 'e', 0);
  bool* helpParam = params.AddParamBool("help", "Show help about parameters",
                                        false, 'h', 0);
  std::vector<std::string> methods;
  std::vector<int> methodIDs;
  methods.push_back("HORN");
  methods.push_back("KABSCH");
  methodIDs.push_back(AbsoluteOrientation::HORN_ALGORITHM);
  methodIDs.push_back(AbsoluteOrientation::KABSCH_ALGORITHM);
  int* methodParam = params.AddParamEnum("method", "Rotation estimation algorithm "
                                         "(HORN, KABSCH)", methods, methodIDs[0],
                                         &methodIDs, 'm', 0);
  params.SetGroupName(0, "Test parameters");
  params.ParseCommandLine(argc, argv);
  if (*helpParam) {
    params.Usage();
    return 0;
  }

  unsigned int num = *numParam;           // number of 3D points
  double range = *rangeParam;             // range for 3D points (in m)
  double noise = *errorParam;             // noise for 3D points (in %)

  std::vector< BIAS::Vector3<double> > X; // 3D points in first coord. system
  std::vector< BIAS::Vector3<double> > Y; // 3D points in second coord. system

  AbsoluteOrientation::RotationEstimationMethod method =
    (AbsoluteOrientation::RotationEstimationMethod)*methodParam;
  cout << "- Compute absolute orientation from " << num << " 3D points";
  if (noise > 0) cout << " with noise of " << 100.0*noise << "% using ";
  if (method == AbsoluteOrientation::HORN_ALGORITHM)
    cout << "Horn's method";
  else if (method == AbsoluteOrientation::KABSCH_ALGORITHM)
    cout << "Kabsch algorithm";
  else
    cout << "undefined method";
  cout << " for rotation estimation" << endl;

  BIAS::Random random;
  random.Reset();

  // Compute random similarity transformation Y = ds*dR*X + dt
  BIAS::RMatrix dR;
  BIAS::Vector3<double> dr, dt;
  double dphi = random.GetUniformDistributed(-0.5*M_PI, 0.5*M_PI);
  double ds = random.GetUniformDistributed(1.0/range, range);
  for (unsigned int k = 0; k < 3; k++) {
    dr[k] = random.GetUniformDistributed(-range, range);
    dt[k] = random.GetUniformDistributed(-range, range);
    //dr[k] = random.GetNormalDistributed(0.0, range);
    //dt[k] = random.GetNormalDistributed(0.0, range);
  }
  dr.Normalize();
  dR.SetFromAxisAngle(dphi*dr);

  cout << "- Rotation dR = " << dphi*180.0/M_PI << " degree around axis ("
       << dr[0] << ", " << dr[1] << ", " << dr[2] << ")" << endl;
  cout << "- Translation dt = (" << dt[0] << ", " << dt[1] << ", "
       << dt[2] << ")" << endl;
  cout << "- Scale ds = " << ds << endl;

  // Compute random corresponding 3D points in both coordinate frames
  X.reserve(num);
  Y.reserve(num);
  BIAS::Vector3<double> x, y;
  BIAS::Vector3<double> errx(0.0), erry(0.0);
  for (unsigned int i = 0; i < num; i++) {
    for (unsigned int k = 0; k < 3; k++) {
      x[k] = random.GetUniformDistributed(-range, range);
      //x[k] = random.GetNormalDistributed(0.0, range);
    }
    y = ds*dR*x + dt;
    // Add normal distributed noise to 3D point positions
    if (noise > 0) {
      for (unsigned int k = 0; k < 3; k++) {
        errx[k] = random.GetNormalDistributed(0.0, noise*range);
        erry[k] = random.GetNormalDistributed(0.0, noise*range);
      }
      x.AddIP(errx);
      y.AddIP(erry);
    }
    X.push_back(x);
    Y.push_back(y);
    cout << "  [" << i+1 << "] X = (" << x[0] << ", " << x[1] << ", "
         << x[2] << "), Y = (" << y[0] << ", " << y[1] << ", " << y[2] << ")";
    if (noise > 0) {
      cout << ", errors = " << errx.NormL2() << " and " << erry.NormL2();
    }
    cout << endl;
  }

  // Compute absolute orientation
  BIAS::RMatrix estR;
  BIAS::Vector3<double> estr, estt;
  double estphi, ests;
  BIAS::AbsoluteOrientation estimator;
  estimator.SetRotationAlgorithm(method);
  int res = estimator.Compute(X, Y, estR, estt, ests);
  if (res != 0) {
    cout << "[ERROR] Estimation failed!" << endl << endl;
    return -1;
  }
  estR.GetRotationAxisAngle(estr, estphi);

  // Compare result with ground truth and print estimation error
  BIAS::Vector3<double> errr;
  double errphi, error;
  std::vector<double> errors;
  BIAS::RMatrix(estR*dR.Transpose()).GetRotationAxisAngle(errr, errphi);
  error = estimator.GetResidualErrors(errors, X, Y, estR, estt, ests);
  cout << "- Estimated rotation dR = " << estphi*180.0/M_PI
       << " degree around axis (" << estr[0] << ", " << estr[1] << ", "
       << estr[2] << ")" << ", error = " << errphi*180.0/M_PI
       << " degree (" << 100.0*fabs(errphi-dphi)/dphi << "%)" << endl;
  cout << "- Estimated translation dt = (" << estt[0] << ", " << estt[1]
       << ", " << estt[2] << "), error = " << (estt-dt).NormL2() << " ("
       << 100.0*(estt-dt).NormL2()/dt.NormL2() << "%)" << endl;
  cout << "- Estimated scale ds = " << ests << ", error = "
       << fabs(ests-ds) << " (" << 100.0*fabs(ests-ds)/ds << "%)" << endl;
  cout << "- Average residual error of solution = "
       << sqrt(error/(double)num) << endl << endl;

  return 0;
}