Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Unifies sac_model_normal_parallel_plane with sac_model_normal_plane, removes duplicated code #696

Merged
merged 1 commit into from
May 24, 2014
Merged

Unifies sac_model_normal_parallel_plane with sac_model_normal_plane, removes duplicated code #696

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
131 changes: 0 additions & 131 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,137 +43,6 @@

#include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::selectWithinDistance] No input dataset containing normals was given!\n");
return;
}

// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}

// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;

int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());

// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (coeff.dot (p));

// Calculate the angular distance between the point normal and the plane normal
double d_normal = getAngle3D (n, coeff);
d_normal = (std::min) (d_normal, fabs(M_PI - d_normal));

double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::countWithinDistance] No input dataset containing normals was given!\n");
return (0);
}

// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);

// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;

int nr_p = 0;

// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (coeff.dot (p));

// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, coeff));
d_normal = (std::min) (d_normal, fabs(M_PI - d_normal));

if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
}


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::getDistancesToModel] No input dataset containing normals was given!\n");
return;
}

// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}

// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;

distances.resize (indices_->size ());

// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (coeff.dot (p));

// Calculate the angular distance between the point normal and the plane normal
double d_normal = getAngle3D (n, coeff);
d_normal = (std::min) (d_normal, fabs (M_PI - d_normal));

distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
Expand Down
40 changes: 4 additions & 36 deletions sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,7 @@
#ifndef PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_
#define PCL_SAMPLE_CONSENSUS_MODEL_NORMALPARALLELPLANE_H_

#include <pcl/sample_consensus/sac_model.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/sample_consensus/sac_model_normal_plane.h>
#include <pcl/sample_consensus/model_types.h>

namespace pcl
Expand Down Expand Up @@ -83,7 +81,7 @@ namespace pcl
* \ingroup sample_consensus
*/
template <typename PointT, typename PointNT>
class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelPlane<PointT>, public SampleConsensusModelFromNormals<PointT, PointNT>
class SampleConsensusModelNormalParallelPlane : public SampleConsensusModelNormalPlane<PointT, PointNT>
{
public:
using SampleConsensusModel<PointT>::input_;
Expand All @@ -107,8 +105,7 @@ namespace pcl
*/
SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud,
bool random = false)
: SampleConsensusModelPlane<PointT> (cloud, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
: SampleConsensusModelNormalPlane<PointT, PointNT> (cloud, random)
, axis_ (Eigen::Vector4f::Zero ())
, distance_from_origin_ (0)
, eps_angle_ (-1.0)
Expand All @@ -125,8 +122,7 @@ namespace pcl
SampleConsensusModelNormalParallelPlane (const PointCloudConstPtr &cloud,
const std::vector<int> &indices,
bool random = false)
: SampleConsensusModelPlane<PointT> (cloud, indices, random)
, SampleConsensusModelFromNormals<PointT, PointNT> ()
: SampleConsensusModelNormalPlane<PointT, PointNT> (cloud, indices, random)
, axis_ (Eigen::Vector4f::Zero ())
, distance_from_origin_ (0)
, eps_angle_ (-1.0)
Expand Down Expand Up @@ -179,34 +175,6 @@ namespace pcl
inline double
getEpsDist () { return (eps_dist_); }

/** \brief Select all the points which respect the given model coefficients as inliers.
* \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
* \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
* \param[out] inliers the resultant model inliers
*/
void
selectWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold,
std::vector<int> &inliers);

/** \brief Count all the points which respect the given model coefficients as inliers.
*
* \param[in] model_coefficients the coefficients of a model that we need to compute distances to
* \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
* \return the resultant number of inliers
*/
virtual int
countWithinDistance (const Eigen::VectorXf &model_coefficients,
const double threshold);

/** \brief Compute all distances from the cloud data to a given plane model.
* \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to
* \param[out] distances the resultant estimated distances
*/
void
getDistancesToModel (const Eigen::VectorXf &model_coefficients,
std::vector<double> &distances);

/** \brief Return an unique id for this model (SACMODEL_NORMAL_PARALLEL_PLANE). */
inline pcl::SacModel
getModelType () const { return (SACMODEL_NORMAL_PARALLEL_PLANE); }
Expand Down