ExampleAbsoluteOrientation.cpp

Example for similarity transformation estimation between two coordinate frames from corresponding 3D points., AbsoluteOrientationRANSAC 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 Also

AbsoluteOrientation

Author

esquivel

Date

12/2008

/* 
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;
}