registerPoints3D.h

Go to the documentation of this file.

#ifndef _DLRCOMPUTERVISION_REGISTERPOINTS3D_H_
#define _DLRCOMPUTERVISION_REGISTERPOINTS3D_H_

#include <dlrNumeric/transform3D.h>

namespace dlr {

  namespace computerVision {
  
    template <class InIter0, class InIter1>
    Transform3D
    registerPoints3D(InIter0 fromPointsBegin, InIter0 fromPointsEnd,
                     InIter1 toPointsBegin);


    template <class InIter0, class InIter1, class InIter2>
    Transform3D
    registerPoints3D(InIter0 fromPointsBegin, InIter0 fromPointsEnd,
                     InIter1 toPointsBegin, InIter2 selectedFlagsBegin);



    template <class InIter0, class InIter1, class OutIter0>
    Transform3D
    registerPoints3D(InIter0 fromPointsBegin, InIter0 fromPointsEnd,
                     InIter1 toPointsBegin, OutIter0 selectedFlagsBegin,
                     double inclusion, double maximumResidual = -1.0,
                     size_t maximumIterations = 5);
  
  
  } // namespace computerVision    

} // namespace dlr

/* ========== Definitions of inline and template functions follow ========== */

#include <limits>
#include <dlrCommon/functional.h>
#include <dlrCommon/types.h>
#include <dlrNumeric/array1D.h>
#include <dlrNumeric/array2D.h>
#include <dlrNumeric/quaternion.h>
#include <dlrNumeric/rotations.h>
#include <dlrNumeric/utilities.h>
#include <dlrNumeric/vector3D.h>
#include <dlrLinearAlgebra/linearAlgebra.h>

namespace dlr {

  namespace computerVision {
    
    namespace privateCode {

      const Array2D<double>&
      getHornNCoefficientMatrix()
      {
        static Array2D<double> _NCoefficients(
          "[[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], "
          " [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0], "
          " [0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], "
          " [0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0], "
          " [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0], "
          " [1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, -1.0], "
          " [0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], "
          " [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], "
          " [0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], "
          " [0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], "
          " [-1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, -1.0], "
          " [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0], "
          " [0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0], "
          " [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], "
          " [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0], "
          " [-1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0]]");
        return _NCoefficients;
      }

    } // namespace privateCode
  

    template <class InIter0, class InIter1>
    Transform3D
    registerPoints3D(InIter0 fromPointsBegin, InIter0 fromPointsEnd,
                     InIter1 toPointsBegin)
    {
      std::vector<bool> flags(fromPointsEnd - fromPointsBegin, true);
      return registerPoints3D(
        fromPointsBegin, fromPointsEnd, toPointsBegin, flags.begin());
    }

  
    template <class InIter0, class InIter1, class InIter2>
    Transform3D
    registerPoints3D(InIter0 fromPointsBegin, InIter0 fromPointsEnd,
                     InIter1 toPointsBegin, InIter2 flagsBegin)
    {
      // Compute the mean of each point cloud, so we can translate its
      // center to the origin.
      InIter0 fromIter = fromPointsBegin;
      InIter1 toIter = toPointsBegin;
      InIter2 flagsIter = flagsBegin;
      Vector3D fromMean(0.0, 0.0, 0.0);
      Vector3D toMean(0.0, 0.0, 0.0);
      size_t count = 0;
      while(fromIter != fromPointsEnd) {
        if(*flagsIter) {
          fromMean += *fromIter;
          toMean += *toIter;
          ++count;
        }
        ++fromIter;
        ++toIter;
        ++flagsIter;
      }
      if(count == 0) {
        DLR_THROW(ValueException, "registerPoints3D()",
                  "No points to register!");
      }
      fromMean /= static_cast<double>(count);
      toMean /= static_cast<double>(count);

      // Now translate each point cloud so that its center of mass is at
      // the origin.  We'll use these translated point clouds to compute
      // the best fit rotation.  We use Nx3 arrays to represent these
      // translated ponits so that we'll be able to conveniently do
      // linear algebra later.
      fromIter = fromPointsBegin;
      toIter = toPointsBegin;
      flagsIter = flagsBegin;
      Array2D<double> translatedFromPoints(count, 3);
      Array2D<double> translatedToPoints(count, 3);
      size_t arrayIndex = 0;
      while(fromIter != fromPointsEnd) {
        if(*flagsIter) {
          Vector3D& fromPoint = *fromIter;
          Vector3D& toPoint = *toIter;
        
          // Copy the first element in the current row.
          translatedFromPoints[arrayIndex] = fromPoint.x() - fromMean.x();
          translatedToPoints[arrayIndex] = toPoint.x() - toMean.x();
        
          // Advance to next element in the current row.
          ++arrayIndex;
          translatedFromPoints[arrayIndex] = fromPoint.y() - fromMean.y();
          translatedToPoints[arrayIndex] = toPoint.y() - toMean.y();
        
          // Advance to next element in the current row.
          ++arrayIndex;
          translatedFromPoints[arrayIndex] = fromPoint.z() - fromMean.z();
          translatedToPoints[arrayIndex] = toPoint.z() - toMean.z();
        
          // Wrap around to next row.
          ++arrayIndex;
        }
        // Advance to next point.
        ++fromIter;
        ++toIter;
        ++flagsIter;
      }

      // Compute the (not quite) covariance matrix between the two point
      // clouds.  This is a  3x3 matrix.
      Array2D<double> matrixM = matrixMultiply(
        translatedFromPoints.transpose(), translatedToPoints);

      // Select elements as describe by Horn.  The member function
      // ravel() simply returns a flattened (1D) array referencing the
      // elements of the Array2D instance in row-major order.
      Array1D<double> vectorN = matrixMultiply(
        privateCode::getHornNCoefficientMatrix(), matrixM.ravel());
      Array2D<double> matrixN(4, 4, vectorN.data(), vectorN.refCountPtr());

      // Find the largest eigenvector of matrixN.  This is a unit
      // quaternion describing the best fit rotation.
      Array1D<Float64> eValues;
      Array2D<Float64> eVectors;
      if(sizeof(double) == sizeof(Float64)) {
        eigenvectorsSymmetric(matrixN, eValues, eVectors);
      } else {
        Array2D<Float64> tempArray(matrixN.rows(), matrixN.columns());
        tempArray.copy(matrixN);
        eigenvectorsSymmetric(matrixN, eValues, eVectors);
      }

      // The function argmax returns the index of the largest element of
      // eValues.
      size_t index0 = argmax(eValues);
      Quaternion q0(static_cast<double>(eVectors(0, index0)),
                    static_cast<double>(eVectors(1, index0)),
                    static_cast<double>(eVectors(2, index0)),
                    static_cast<double>(eVectors(3, index0)));

      // Convert the unit quaternion to a rotation matrix.
      Transform3D xf = quaternionToTransform3D(q0);

      // And add in best fit translation.
      Vector3D bestFitTranslation = toMean - xf * fromMean;
      xf.setValue<0, 3>(bestFitTranslation.x());
      xf.setValue<1, 3>(bestFitTranslation.y());
      xf.setValue<2, 3>(bestFitTranslation.z());
      return xf;
    }
  

    template <class InIter0, class InIter1, class OutIter0>
    Transform3D
    registerPoints3D(InIter0 fromPointsBegin, InIter0 fromPointsEnd,
                     InIter1 toPointsBegin, OutIter0 selectedFlagsBegin,
                     double inclusion, double maximumResidual,
                     size_t maximumIterations)
    {
      // Sort out arguments, array sizes, etc.
      size_t numberOfPoints = fromPointsEnd - fromPointsBegin;
      size_t numberToExclude = 0;
      if(inclusion > 0.0 && inclusion < 1.0) {
        size_t numberToInclude = static_cast<size_t>(
          std::floor(inclusion * numberOfPoints));
        if(numberToInclude >= numberOfPoints) {
          DLR_THROW(ValueException, "registerPoints3D()",
                    "Inclusion percentage is too low... all points excluded.");
        }
        numberToExclude = numberOfPoints - numberToInclude;
      }

      // Register repeatedly until we converge.
      Transform3D resultTransform;
      std::vector<bool> flags(numberOfPoints, true);
      for(size_t iterationIndex = 0;
          iterationIndex < maximumIterations;
          ++iterationIndex) {
        // Do a sub-registration.
        resultTransform = registerPoints3D(
          fromPointsBegin, fromPointsEnd, toPointsBegin, flags.begin());

        // Compute residuals.
        std::vector<Vector3D> transformedPoints(numberOfPoints);
        std::transform(fromPointsBegin, fromPointsEnd,
                       transformedPoints.begin(), resultTransform.getFunctor());

        std::vector<double> squaredResiduals(numberOfPoints);
        typedef double (*magVec3D)(const Vector3D&);
        std::transform(
          transformedPoints.begin(), transformedPoints.end(),
          toPointsBegin, squaredResiduals.begin(),
          composeFunctor_1_2(
            std::ptr_fun(static_cast<magVec3D>(magnitudeSquared)),
            std::minus<Vector3D>()));

        // Figure out how big a residual has to be before we ignore the
        // associated point.
        double residualThreshold =
          (maximumResidual > 0.0)
          ? maximumResidual : std::numeric_limits<double>::max();
        if(numberToExclude != 0) {
          // Find the numberToExclude largest residuals.
          std::vector<double> sortedSquaredResiduals(numberToExclude);
          std::partial_sort_copy(
            squaredResiduals.begin(), squaredResiduals.end(),
            sortedSquaredResiduals.begin(), sortedSquaredResiduals.end(),
            std::greater<double>());
          double threshold = sortedSquaredResiduals[numberToExclude - 1];
          if(threshold < residualThreshold) {
            residualThreshold = threshold;
          }
        }

        // Check which points exceed the residual threshold and should
        // be counted as outliers.
        std::vector<bool> newFlags(numberOfPoints);
        std::transform(
          squaredResiduals.begin(), squaredResiduals.end(), newFlags.begin(),
          std::bind2nd(std::less<double>(), residualThreshold));

        // Any change from last iteration?
        if(std::equal(flags.begin(), flags.end(), newFlags.begin())) {
          // No change.  We're done.
          break;
        }
      
        // Reset flags so the next iteration will exclude points which
        // exceed the threshold.
        std::copy(newFlags.begin(), newFlags.end(), flags.begin());
      } // for

      std::copy(flags.begin(), flags.end(), selectedFlagsBegin);
      return resultTransform;
    }
  
  } // namespace computerVision    

} // namespace dlr

#endif /* #ifndef _DLRCOMPUTERVISION_REGISTERPOINTS3D_H_ */

---