text: /********************************************************************************
   * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
   * Faculty of Engineering, University of Southern Denmark
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *     http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   ********************************************************************************/
  
  #include "PlaneModel.hpp"
  
  #include <rw/core/macros.hpp>
  #include <rw/math/LinearAlgebra.hpp>
  
  using namespace rw::math;
  using namespace rwsim::util;
  
  double PlaneModel::refit (std::vector< rw::math::Vector3D<> >& data)
  {
      if (data.size () < 3)
          RW_THROW ("Data size must be 3 or more!");
  
      if (data.size () == 3) {
          if (MetricUtil::dist2 (data[1], data[0]) < 0.1 ||
              MetricUtil::dist2 (data[2], data[1]) < 0.1 ||
              MetricUtil::dist2 (data[0], data[2]) < 0.1) {
              _n = Vector3D<> (0, 0, 0);
              return 1000;
          }
  
          _n = normalize (cross (data[1] - data[0], data[2] - data[0]));
          if (_n (2) < 0)
              _n = -_n;
          const rw::math::Vector3D<>& p = data[0];
          _d                            = -_n (0) * p (0) - _n (1) * p (1) - _n (2) * p (2);
      }
      else {
          using namespace rw::math;
  
          Eigen::MatrixXd covar (Eigen::MatrixXd::Zero (3, 3));
          Vector3D<> centroid (0, 0, 0);
          for (Vector3D<>& v : data) {
              centroid += v;
              for (size_t j = 0; j < 3; j++)
                  for (size_t k = 0; k < 3; k++)
                      covar (j, k) += v (j) * v (k);
          }
          // std::cout << "COVAR: " << covar << std::endl;
  
          // 3. Compute Covariance matrix
          // 3.1 using the variables from 2.1 we create the covariance matrix
          for (size_t j = 0; j < 3; j++)
              for (size_t k = 0; k < 3; k++)
                  covar (j, k) = covar (j, k) - centroid[j] * centroid[k] / data.size ();
          Vector3D<> c = centroid / ((double) data.size ());
          // 4. get eigenvectors from the covariance matrix
          typedef std::pair< Eigen::MatrixXd, Eigen::VectorXd > ResultType;
          // std::cout << "COVAR: " << covar << std::endl;
          ResultType res = LinearAlgebra::eigenDecompositionSymmetric (covar);
  
          // 4.1 create the rotationmatrix from the normalized eigenvectors
          // find max and the second maximal eigenvalue
          size_t maxEigIdx = 2, midEigIdx = 1, minEigIdx = 0;
          double maxEigVal = res.second (maxEigIdx);
          double midEigVal = res.second (midEigIdx);
          double minEigVal = res.second (minEigIdx);
          if (maxEigVal < midEigVal) {
              std::swap (midEigVal, maxEigVal);
              std::swap (midEigIdx, maxEigIdx);
          }
          if (minEigVal > midEigVal) {
              std::swap (midEigVal, minEigVal);
              std::swap (midEigIdx, minEigIdx);
              if (midEigVal > maxEigVal) {
                  std::swap (midEigVal, maxEigVal);
                  std::swap (midEigIdx, maxEigIdx);
              }
          }
          // specify x and y axis, x will be the axis with largest spred
          Vector3D<> maxAxis (
              res.first (0, maxEigIdx), res.first (1, maxEigIdx), res.first (2, maxEigIdx));
          Vector3D<> midAxis (
              res.first (0, midEigIdx), res.first (1, midEigIdx), res.first (2, midEigIdx));
  
          // compute the z axis as the cross product
          _n = normalize (cross (maxAxis, midAxis));
          // normal should allways point in the z direction
          if (_n (2) < 0)
              _n = -_n;
          _d = -_n (0) * c (0) - _n (1) * c (1) - _n (2) * c (2);
      }
  
      // calculate the fit error as the squared mean over the distance of a point from the plane
      double sum = 0;
      for (Vector3D<>& p : data) {
          const double fitE = fitError (p);
          sum               = fitE * fitE;
      }
      sum /= data.size ();
  
      return sum;
  }
  