table_extraction.cpp

Go to the documentation of this file.

#include "cob_3d_mapping_semantics/table_extraction.h"

using namespace cob_3d_mapping;

bool
TableExtraction::isTable ()
{
  if (!poly_ptr_)
  {
    std::cerr << "Input polygon not set, aborting..." << std::endl;
    return false;
  }
  //Check if the plane spanned by the polygon is horizontal or not
  if (isHorizontal () && isHeightOk () && isSizeOk ())
    return true;
  else
    return false;
}

bool
TableExtraction::isHorizontal ()
{
  //check components of normal_ vector with threshold values
  if ((poly_ptr_->normal_ (2) >= norm_z_max_ || poly_ptr_->normal_ (2) <= norm_z_min_)
      && (poly_ptr_->normal_ (0) < norm_x_max_ && poly_ptr_->normal_ (0) > norm_x_min_)
      && (poly_ptr_->normal_ (1) < norm_y_max_ && poly_ptr_->normal_ (1) > norm_y_min_))
  {
    return true;
  }
  else
  {
    return false;
  }
}

bool
TableExtraction::isHeightOk ()
{
  if (poly_ptr_->pose_.translation () (2) > height_min_ && poly_ptr_->pose_.translation () (2) < height_max_)
    return true;
  else
    return false;
}

bool
TableExtraction::isSizeOk ()
{
  double area = poly_ptr_->computeArea3d ();
  if (area >= area_min_ && area <= area_max_)
    return true;
  return false;
}