sac_model_line.h

Go to the documentation of this file.

 /*
 * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: sac_model_line.h 20633 2009-08-04 07:19:09Z tfoote $
 *
 */

#ifndef _SAMPLE_CONSENSUS_SACMODELLINE_H_
#define _SAMPLE_CONSENSUS_SACMODELLINE_H_

#include <sac_model.h>
#include <model_types.h>

#define MAX_ITERATIONS_UNIQUE 1000

namespace sample_consensus
{
  class SACModelLine : public SACModel
  {
    public:

      SACModelLine () { }


      virtual ~SACModelLine () { }

      virtual void getSamples (int &iterations, std::vector<int> &samples);


      bool testModelCoefficients (const std::vector<double> &model_coefficients) { return true; }

      virtual bool computeModelCoefficients (const std::vector<int> &samples);

      virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
      virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
      virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);

      virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, PointCloud &projected_points);

      virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
      virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);


      virtual int getModelType () { return (SACMODEL_LINE); }


      inline pcl::PointXYZ
        cross (const pcl::PointXYZ &p1, const pcl::PointXYZ &p2)
      {
        pcl::PointXYZ r;
        r.x = p1.y * p2.z - p1.z * p2.y;
        r.y = p1.z * p2.x - p1.x * p2.z;
        r.z = p1.x * p2.y - p1.y * p2.x;
        return (r);
      }


      inline void
        computeCentroid (const PointCloud &points, const std::vector<int> &indices, pcl::PointXYZ &centroid)
      {
        centroid.x = centroid.y = centroid.z = 0;
        // For each point in the cloud
        for (unsigned int i = 0; i < indices.size (); i++)
        {
          centroid.x += points.points.at (indices.at (i)).x;
          centroid.y += points.points.at (indices.at (i)).y;
          centroid.z += points.points.at (indices.at (i)).z;
        }

        centroid.x /= indices.size ();
        centroid.y /= indices.size ();
        centroid.z /= indices.size ();
      }
      

      inline void
        computeCovarianceMatrix (const PointCloud &points, const std::vector<int> &indices, Eigen::Matrix3d &covariance_matrix, pcl::PointXYZ &centroid)
      {
        computeCentroid (points, indices, centroid);

        // Initialize to 0
        covariance_matrix = Eigen::Matrix3d::Zero ();

        for (unsigned int j = 0; j < indices.size (); j++)
        {
          covariance_matrix (0, 0) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].x - centroid.x);
          covariance_matrix (0, 1) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].y - centroid.y);
          covariance_matrix (0, 2) += (points.points[indices.at (j)].x - centroid.x) * (points.points[indices.at (j)].z - centroid.z);

          covariance_matrix (1, 0) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].x - centroid.x);
          covariance_matrix (1, 1) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].y - centroid.y);
          covariance_matrix (1, 2) += (points.points[indices.at (j)].y - centroid.y) * (points.points[indices.at (j)].z - centroid.z);

          covariance_matrix (2, 0) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].x - centroid.x);
          covariance_matrix (2, 1) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].y - centroid.y);
          covariance_matrix (2, 2) += (points.points[indices.at (j)].z - centroid.z) * (points.points[indices.at (j)].z - centroid.z);
        }
      }
    };
}

#endif