sac.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$
 *
 */

#ifndef _SAMPLE_CONSENSUS_SAC_H_
#define _SAMPLE_CONSENSUS_SAC_H_

#include <geometry_msgs/Point32.h>     // ROS float point type
#include <sensor_msgs/PointCloud.h>  // ROS point cloud type

#include <stdlib.h>
#include <door_handle_detector/sample_consensus/sac_model.h>

namespace sample_consensus
{
  class SAC
  {
    public:


      SAC ()
      {
        //srand ((unsigned)time (0)); // set a random seed
      };


      SAC (SACModel *model) : sac_model_(model)
      {
        //srand ((unsigned)time (0)); // set a random seed
      };


      virtual ~SAC () { }


      virtual inline void
        setThreshold (double threshold)
      {
        this->threshold_ = threshold;
      }


      virtual inline void
        setMaxIterations (int max_iterations)
      {
        this->max_iterations_ = max_iterations;
      }


      virtual inline void
        setProbability (double probability)
      {
        this->probability_ = probability;
      }


      virtual bool computeModel (int debug = 0) = 0;


      virtual void
        computeCoefficients (std::vector<double> &coefficients)
      {
        sac_model_->computeModelCoefficients (sac_model_->getBestModel ());
        coefficients = sac_model_->getModelCoefficients ();
      }


      virtual void
        refineCoefficients (std::vector<double> &refined_coefficients)
      {
        sac_model_->refitModel (sac_model_->getBestInliers (), refined_coefficients);
      }


      virtual int removeInliers () { return (sac_model_->removeInliers ()); }


      virtual std::vector<int>
        getInliers ()
      {
        return (sac_model_->getBestInliers ());
      }


      sensor_msgs::PointCloud getPointCloud (std::vector<int> indices);


      virtual void
        projectPointsToModel (const std::vector<int> &indices, const std::vector<double> &model_coefficients,
                              sensor_msgs::PointCloud &projected_points)
      {
        sac_model_->projectPoints (indices, model_coefficients, projected_points);
      }



      std::set<int>
        getRandomSamples (sensor_msgs::PointCloud points, int nr_samples)
      {
        std::set<int> random_idx;
        for (int i = 0; i < nr_samples; i++)
          random_idx.insert ((int) (points.points.size () * (rand () / (RAND_MAX + 1.0))));
        return (random_idx);
      }


      std::set<int>
        getRandomSamples (sensor_msgs::PointCloud points, std::vector<int> indices, int nr_samples)
      {
        std::set<int> random_idx;
        for (int i = 0; i < nr_samples; i++)
          random_idx.insert ((int) (indices.size () * (rand () / (RAND_MAX + 1.0))));
        return (random_idx);
      }

    protected:
      SACModel *sac_model_;

      double probability_;

      int iterations_;

      int max_iterations_;

      double threshold_;
  };
}

#endif