plane3D.h

Go to the documentation of this file.

#ifndef _DLR_GEOMETRY_PLANE3D_H_
#define _DLR_GEOMETRY_PLANE3D_H_

#include <iostream>
#include <dlrNumeric/vector3D.h>

// xxx
#include <dlrNumeric/utilities.h>

namespace dlr {

  namespace geometry {
    
    class Plane3D {
    public:
      
      Plane3D()
        : m_origin(0.0, 0.0, 0.0), m_directionVector0(1.0, 0.0, 0.0),
          m_directionVector1(0.0, 1.0, 0.0) {}

    
      Plane3D(const Vector3D& point0,
              const Vector3D& point1,
              const Vector3D& point2,
              bool orthonormalize = true);

    
      template <class Iterator> 
      Plane3D(Iterator beginIterator,
              Iterator endIterator,
              double inlierPercentage = 1.0);

    
      Plane3D(const Plane3D& source);


      ~Plane3D() {}


      Plane3D&
      operator=(const Plane3D& source);


      const Vector3D&
      getDirectionVector0() const {return m_directionVector0;}
      

      const Vector3D&
      getDirectionVector1() const {return m_directionVector1;}


      // xxx
      Vector3D
      getNormal() const {
        return dlr::cross(m_directionVector0, m_directionVector1);
      }
      

      const Vector3D&
      getOrigin() const {return m_origin;}

      // xxx
      double
      findDistance(const Vector3D& point) {
        Vector3D offset = point - m_origin;
        offset -= dot(offset, m_directionVector0) * m_directionVector0;
        offset -= dot(offset, m_directionVector1) * m_directionVector1;
        return magnitude(offset);
      }
        
      
    private:
      // Private member functions.
      template <class Iterator> 
      void
      estimateFromSequence(Iterator beginIterator,
                           Iterator endIterator);


      // Private data members.
      Vector3D m_origin;
      Vector3D m_directionVector0;
      Vector3D m_directionVector1;

    }; // class Plane3D



    /* ======= Non-member functions. ======= */

    std::ostream&
    operator<<(std::ostream& stream, const Plane3D& plane);
    
    
  } // namespace utilities
    
} // namespace dlr


/* ======= Inline and template function definitions. ======= */

#include <dlrLinearAlgebra/linearAlgebra.h>
#include <dlrNumeric/subArray1D.h>

namespace dlr {

  namespace geometry {


    // This constructor initializes the plane using a collection of
    // points.
    template <class Iterator>
    Plane3D::
    Plane3D(Iterator beginIterator,
            Iterator endIterator,
            double inlierPercentage)
      : m_origin(0.0, 0.0, 0.0),
        m_directionVector0(1.0, 0.0, 0.0),
        m_directionVector1(0.0, 1.0, 0.0)
    {
      // Note(xxx): Hasty implementation!  Clean up soon.
      std::vector<Vector3D> allPointsVector;
      std::copy(beginIterator, endIterator,
                std::back_inserter(allPointsVector));

      int numberToInclude =
        static_cast<int>(allPointsVector.size() * inlierPercentage + 0.5);
      int numberToIgnore = allPointsVector.size() - numberToInclude;

      Array1D<double> distances(allPointsVector.size());
      Array1D<size_t> ignoredPoints(numberToIgnore);
      Array1D<Vector3D> currentPoints(allPointsVector.size());
      std::copy(allPointsVector.begin(), allPointsVector.end(),
                currentPoints.begin());
      bool isDone = false;
      ignoredPoints = 0;
      // xxx
      size_t iterationCount = 0;
      while(!isDone) {
        this->estimateFromSequence(currentPoints.begin(), currentPoints.end());
        for(size_t index0 = 0; index0 < allPointsVector.size(); ++index0) {
          distances[index0] = this->findDistance(allPointsVector[index0]);
        }
        Array1D<size_t> pointIndices = argsort(distances);
        if(static_cast<int>(currentPoints.size()) != numberToInclude) {
          currentPoints.reinit(numberToInclude);
        }
        for(int index1 = 0; index1 < numberToInclude; ++index1) {
          currentPoints[index1] = allPointsVector[pointIndices[index1]];
        }
        int index3 = 0;
        isDone = true;
        for(int index2 = numberToInclude;
            index2 < static_cast<int>(allPointsVector.size());
            ++index2) {
          if(ignoredPoints[index3] != pointIndices[index2]) {
            isDone = false;
            break;
          }
        }
        // xxx
        if(++iterationCount >= 3) {
          break;
        }
        if(numberToInclude < static_cast<int>(allPointsVector.size())) {
          ignoredPoints = subArray(pointIndices, Slice(numberToInclude, 0));
        } else {
          ignoredPoints.clear();
        }
      }
    }



    template <class Iterator> 
    void
    Plane3D::
    estimateFromSequence(Iterator beginIterator,
                         Iterator endIterator)
    {
      // Get mean point.
      dlr::Vector3D meanPoint;
      Iterator targetIterator = beginIterator;
      size_t count = 0;
      while(targetIterator != endIterator) {
        meanPoint += *targetIterator;
        ++count;
        ++targetIterator;
      }
      meanPoint /= static_cast<double>(count);

      // Get 3x3 covariance matrix.
      dlr::Array2D<double> covarianceMatrix(3, 3);
      covarianceMatrix = 0.0;
      targetIterator = beginIterator;
      while(targetIterator != endIterator) {
        const double xValue = targetIterator->x() - meanPoint.x();
        const double yValue = targetIterator->y() - meanPoint.y();
        const double zValue = targetIterator->z() - meanPoint.z();
        covarianceMatrix(0, 0) += xValue * xValue;
        covarianceMatrix(0, 1) += xValue * yValue;
        covarianceMatrix(0, 2) += xValue * zValue;
        covarianceMatrix(1, 1) += yValue * yValue;
        covarianceMatrix(1, 2) += yValue * zValue;
        covarianceMatrix(2, 2) += zValue * zValue;
        ++targetIterator;
      }
      covarianceMatrix(1, 0) = covarianceMatrix(0, 1);
      covarianceMatrix(2, 0) = covarianceMatrix(0, 2);
      covarianceMatrix(2, 1) = covarianceMatrix(1, 2);
      covarianceMatrix /= static_cast<double>(count);
      
      // Solve for best fit plane.
      dlr::Array1D<double> eigenvalues;
      dlr::Array2D<double> eigenvectors;
      dlr::linearAlgebra::eigenvectorsSymmetric(
        covarianceMatrix, eigenvalues, eigenvectors);
      dlr::Vector3D direction0(
        eigenvectors(0, 0), eigenvectors(1, 0), eigenvectors(2, 0));
      dlr::Vector3D direction1(
        eigenvectors(0, 1), eigenvectors(1, 1), eigenvectors(2, 1));
      direction0 /= dlr::magnitude(direction0);
      direction1 /= dlr::magnitude(direction1);

      m_origin = meanPoint;
      m_directionVector0 = direction0;
      m_directionVector1 = direction1;
    }

    
  } // namespace geometry
  
} // namespace dlr


#endif /* #ifndef _DLR_GEOMETRY_PLANE3D_H_ */

---