void Noob_Position_Filter::filter_impossible_positions(std::vector<Position >& suggestions)
{
    for(int i = 0; i < suggestions.size(); ++i)
    {
        Position& position = suggestions[i];
        //x position in row 0
        position(0,0) = std::max(std::min(position(0,0), position(0,1)), (float)-3.7);
        position(0,1) = std::min(std::max(position(0,0), position(0,1)), (float)3.7);
        //y position in row 1
        position(1,0) = std::max(std::min(position(1,0), position(1,1)), (float)-2.7);
        position(1,1) = std::min(std::max(position(1,0), position(1,1)), (float)2.7);

        bool invalid = position(0,0) == 3.7 || position(0,1) == -3.7
                || position(1,0) == 2.7 || position(1,1) == -2.7;

        //When there is no information, no overlay should be false, otherwise true
        bool no_overlay = position_information.size() > 0;
        if(!invalid)
        {
            foreach(const Eigen::Matrix2f& info, position_information)
            {
                Eigen::Matrix2f overlay;
                overlay.row(0) = find_overlay(info.row(0), position.row(0) + Eigen::Matrix<float, 1, 2>(-way_moved(0), -way_moved(0)));
                overlay.row(1) = find_overlay(info.row(1), position.row(1) + Eigen::Matrix<float, 1, 2>(-way_moved(1), -way_moved(1)));
                if( overlay(0,0) >= info(0,0) - overlay_tolerance &&
                    overlay(1,0) >= info(1,0) - overlay_tolerance )
                {
                    no_overlay = false;
                    break;
                }
            }
        }
/**
    Kreis an die Punkte (2xN-Array) anfitten.
*/
Eigen::Vector3f fit_circle(const Eigen::Array2Xf& points) {
    Eigen::Array2f mean = points.rowwise().mean();
    Eigen::Array2Xf uv(points.colwise() - mean);
    
    Eigen::Array2f Sp2 = uv.square().rowwise().sum();
    Eigen::Array2f Sp3 = uv.cube().rowwise().sum();
    
    float Suv = (uv.row(0) * uv.row(1)).sum();
    float Suvv = (uv.row(0) * uv.row(1).square()).sum();
    float Svuu = (uv.row(0).square() * uv.row(1)).sum();
    
    // Rechte Seite berechnen
    Eigen::Vector2f b((Sp3 + Eigen::Array2f(Suvv, Svuu)) / 2);
    
    // Linke Seite berechnen
    Eigen::Matrix2f A;
    A << Sp2(0), Suv, Suv, Sp2(1);
    
    // Gleichungssystem Lösen
    Eigen::Array2f mid = A.inverse() * b;
    
    // Quadrat des Radius berechnen
    float radius = sqrt(mid.square().sum() + Sp2.sum() / points.cols());
    return (Eigen::Vector3f() << (mid + mean), radius).finished();
}
  void FindObjectOnPlane::generateAngles(
    const cv::Mat& blob_image, const cv::Point2f& test_point,
    std::vector<double>& angles,
    std::vector<double>& max_x,
    std::vector<double>& max_y,
    const image_geometry::PinholeCameraModel& model,
    const jsk_recognition_utils::Plane::Ptr& plane)
  {
    angles.clear();
    const double angle_step = 3;
    std::vector<cv::Point> indices;
    for (int j = 0; j < blob_image.rows; j++) {
        for (int i = 0; i < blob_image.cols; i++) {
          if (blob_image.at<uchar>(j, i) != 0) { // need to check
            indices.push_back(cv::Point(i, j));
          }
        }
    }
    for (double angle = -180; angle < 180; angle += angle_step) {
      Eigen::Vector2f ux(cos(angle/180*M_PI), sin(angle/180*M_PI));
      //Eigen::Vector2f uy(sin(angle/180*M_PI), -cos(angle/180*M_PI));
      cv::Point2d uy_end = getUyEnd(test_point, cv::Point2d(test_point.x + ux[0], test_point.y + ux[1]),
                                    model,
                                    plane);
      Eigen::Vector2f uy(uy_end.x - test_point.x, uy_end.y - test_point.y);

      Eigen::Matrix2f A;
      A << ux[0], uy[0],
        ux[1], uy[1];
      bool excluded = false;
      double max_alpha = -DBL_MAX;
      double max_beta = -DBL_MAX;
      for (size_t i = 0; i < indices.size(); i++) {
        Eigen::Vector2f p_q = Eigen::Vector2f(indices[i].x, indices[i].y) - Eigen::Vector2f(test_point.x, test_point.y);
        Eigen::Vector2f a_b = A.inverse() * p_q;
        double alpha = a_b[0];
        double beta = a_b[1];
        if (alpha < 0 || beta < 0) {
          excluded = true;
          break;
        }
        if (alpha > max_alpha) {
          max_alpha = alpha;
        }
        if (beta > max_beta) {
          max_beta = beta;
        }
        
      }
      if (!excluded) {
        angles.push_back(angle);
        max_x.push_back(max_alpha);
        max_y.push_back(max_beta);
      }
    }
  }
Beispiel #4
0
 /** \brief Resets the FeatureWarping.
  *
  */
 void reset(){
   affineTransform_.setIdentity();
   valid_pixelCorners_ = false;
   valid_bearingCorners_ = false;
   valid_affineTransform_ = true;
   isIdentity_ = true;
 }
 float PoseWithCovarianceStampedToGaussianPointCloud::gaussian(const Eigen::Vector2f& input,
                                                               const Eigen::Vector2f& mean,
                                                               const Eigen::Matrix2f& S,
                                                               const Eigen::Matrix2f& S_inv)
 {
   Eigen::Vector2f diff = input - mean;
   if (normalize_method_ == "normalize_area") {
     return normalize_value_ * 1 / (2 * M_PI * sqrt(S.determinant())) * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
   }
   else if (normalize_method_ == "normalize_height") {
     return normalize_value_ * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
   }
 }
Beispiel #6
0
    auto match(vfloat2 p, vfloat2 tr_prediction,
	       F A, F B, GD Ag,
	       const int winsize,
	       const float min_ev_th,
	       const int max_interations,
	       const float convergence_delta)
    {
      typedef typename F::value_type V;
      int WS = winsize;
      int ws = winsize;
      int hws = ws/2;

      // Gradient matrix
      Eigen::Matrix2f G = Eigen::Matrix2f::Zero();
      int cpt = 0;
      for(int r = -hws; r <= hws; r++)
	for(int c = -hws; c <= hws; c++)
	  {
	    vfloat2 n = p + vfloat2(r, c);
	    if (A.has(n.cast<int>()))
	      {
		Eigen::Matrix2f m;
		auto g = Ag.linear_interpolate(n);
		float gx = g[0];
		float gy = g[1];
		m <<
		  gx * gx, gx * gy,
		  gx * gy, gy * gy;
		G += m;
		cpt++;
	      }
	  }

      // Check minimum eigenvalue.
      float min_ev = 99999.f;
      auto ev = (G / cpt).eigenvalues();
      for (int i = 0; i < ev.size(); i++)
	if (fabs(ev[i].real()) < min_ev) min_ev = fabs(ev[i].real());

      if (min_ev < min_ev_th)
	return std::pair<vfloat2, float>(vfloat2(-1,-1), FLT_MAX);

      Eigen::Matrix2f G1 = G.inverse();

      // Precompute gs and as.
      vfloat2 prediction_ = p + tr_prediction;
      vfloat2 v = prediction_;
      Eigen::Vector2f nk = Eigen::Vector2f::Ones();

      char gs_buffer[WS * WS * sizeof(vfloat2)];
      vfloat2* gs = (vfloat2*) gs_buffer;
      // was: vfloat2 gs[WS * WS];

      typedef plus_promotion<V> S;
      char as_buffer[WS * WS * sizeof(S)];
      S* as = (S*) as_buffer;
      // was: S as[WS * WS];
      {
	for(int i = 0, r = -hws; r <= hws; r++)
	  {
	    for(int c = -hws; c <= hws; c++)
	      {
		vfloat2 n = p + vfloat2(r, c);
		if (Ag.has(n.cast<int>()))
		  {
		    gs[i] = Ag.linear_interpolate(n).template cast<float>();
		    as[i] = cast<S>(A.linear_interpolate(n));
		  }
		i++;
	      }
	  }
      }
      auto domain = B.domain();// - border(hws + 1);

      // Gradient descent
      for (int k = 0; k <= max_interations && nk.norm() >= convergence_delta; k++)
	{
	  Eigen::Vector2f bk = Eigen::Vector2f::Zero();
	  // Temporal difference.
	  int i = 0;
	  for(int r = -hws; r <= hws; r++)
	    {
	      for(int c = -hws; c <= hws; c++)          
		{
		  vfloat2 n = p + vfloat2(r, c);
		  if (Ag.has(n.cast<int>()))
		    {
		      vfloat2 n2 = v + vfloat2(r, c);
		      auto g = gs[i];
		      float dt = (cast<float>(as[i]) - cast<float>(B.linear_interpolate(n2)));
		      bk += Eigen::Vector2f{g[0] * dt, g[1] * dt};
		    }
		  i++;
		}
	    }

	  nk = G1 * bk;
	  v += vfloat2{nk[0], nk[1]};

	  if (!domain.has(v.cast<int>()))
	    return std::pair<vfloat2, float>(vfloat2(0, 0), FLT_MAX);
	}

      // Compute the SSD.
      float err = 0;
      for(int r = -hws; r <= hws; r++)
	for(int c = -hws; c <= hws; c++)
	  {
	    vfloat2 n2 = v + vfloat2(r, c);
	    int i = (r+hws) * ws + (c+hws);
	    {
	      err += fabs(cast<float>(as[i] - cast<S>(B.linear_interpolate(n2))));
	      cpt++;
	    }
	  }

      return std::pair<vfloat2, float>(v - p, err / (cpt));


    }
Beispiel #7
0
// Test isPatchInFrame
TEST_F(PatchTesting, isPatchInFrameWithWarping) {
  Eigen::Matrix2f aff;
  aff.setIdentity();
  warp_.set_affineTransfrom(aff);
  c_.set_c(cv::Point2f(0,0));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(patchSize_/2-0.1,patchSize_/2-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(patchSize_/2,patchSize_/2));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(patchSize_/2+0.1,patchSize_/2+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-0.1,imgSize_-patchSize_/2-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2,imgSize_-patchSize_/2));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2+0.1,imgSize_-patchSize_/2+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(imgSize_,imgSize_));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(0,0));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  c_.set_c(cv::Point2f(patchSize_/2+1-0.1,patchSize_/2+1-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(patchSize_/2+1+0.1,patchSize_/2+1+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1-0.1,imgSize_-patchSize_/2-1-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1,imgSize_-patchSize_/2-1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1+0.1,imgSize_-patchSize_/2-1+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  c_.set_c(cv::Point2f(imgSize_,imgSize_));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  aff << 0, -1, 1, 0;
  warp_.set_affineTransfrom(aff);
  c_.set_c(cv::Point2f(0,0));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(patchSize_/2-0.1,patchSize_/2-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(patchSize_/2,patchSize_/2));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(patchSize_/2+0.1,patchSize_/2+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-0.1,imgSize_-patchSize_/2-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2,imgSize_-patchSize_/2));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2+0.1,imgSize_-patchSize_/2+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(imgSize_,imgSize_));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f(0,0));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  c_.set_c(cv::Point2f(patchSize_/2+1-0.1,patchSize_/2+1-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  c_.set_c(cv::Point2f(patchSize_/2+1,patchSize_/2+1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(patchSize_/2+1+0.1,patchSize_/2+1+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1-0.1,imgSize_-patchSize_/2-1-0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1,imgSize_-patchSize_/2-1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),true);
  c_.set_c(cv::Point2f(imgSize_-patchSize_/2-1+0.1,imgSize_-patchSize_/2-1+0.1));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  c_.set_c(cv::Point2f(imgSize_,imgSize_));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_),true),false);
  aff << cos(M_PI/6.0), -sin(M_PI/6.0), sin(M_PI/6.0), cos(M_PI/6.0);
  warp_.set_affineTransfrom(aff);
  c_.set_c(cv::Point2f((sin(M_PI/6.0)+cos(M_PI/6.0))*patchSize_/2+1e-6,(sin(M_PI/6.0)+cos(M_PI/6.0))*patchSize_/2+1e-6));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),true);
  c_.set_c(cv::Point2f((sin(M_PI/6.0)+cos(M_PI/6.0))*patchSize_/2+1e-6,(sin(M_PI/6.0)+cos(M_PI/6.0))*patchSize_/2-1e-6));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
  c_.set_c(cv::Point2f((sin(M_PI/6.0)+cos(M_PI/6.0))*patchSize_/2-1e-6,(sin(M_PI/6.0)+cos(M_PI/6.0))*patchSize_/2+1e-6));
  ASSERT_EQ(p_.isPatchInFrame(img1_,c_.get_c(),warp_.get_affineTransform(&c_)),false);
}
 void PoseWithCovarianceStampedToGaussianPointCloud::convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
 {
   boost::mutex::scoped_lock lock(mutex_);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
   Eigen::Vector2f mean;
   Eigen::Matrix2f S;
   if (cut_plane_ == "xy" || cut_plane_ == "flipped_xy") {
     mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
     S << msg->pose.covariance[0], msg->pose.covariance[1], 
       msg->pose.covariance[6], msg->pose.covariance[7];
   }
   else if (cut_plane_ == "yz" || cut_plane_ == "flipped_yz") {
     mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
     S << msg->pose.covariance[7], msg->pose.covariance[8], 
       msg->pose.covariance[13], msg->pose.covariance[14];
   }
   else if (cut_plane_ == "zx" || cut_plane_ == "flipped_zx") {
     mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
     S << msg->pose.covariance[14], msg->pose.covariance[12], 
       msg->pose.covariance[0], msg->pose.covariance[2];
   }
   double max_sigma = 0;
   for (size_t i = 0; i < 2; i++) {
     for (size_t j = 0; j < 2; j++) {
       double sigma = sqrt(S(i, j));
       if (max_sigma < sigma) {
         max_sigma = sigma;
       }
     }
   }
   Eigen::Matrix2f S_inv = S.inverse();
   double dx = 6.0 * max_sigma / sampling_num_;
   double dy = 6.0 * max_sigma / sampling_num_;
   for (double x = - 3.0 * max_sigma; x <= 3.0 * max_sigma; x += dx) {
     for (double y = - 3.0 * max_sigma; y <= 3.0 * max_sigma; y += dy) {
       Eigen::Vector2f diff(x, y);
       Eigen::Vector2f input = diff + mean;
       float z = gaussian(input, mean, S, S_inv);
       pcl::PointXYZ p;
       if (cut_plane_ == "xy") {
         p.x = input[0];
         p.y = input[1];
         p.z = z + msg->pose.pose.position.z;
       }
       else if (cut_plane_ == "yz") {
         p.y = input[0];
         p.z = input[1];
         p.x = z + msg->pose.pose.position.x;
       }
       else if (cut_plane_ == "zx") {
         p.z = input[0];
         p.x = input[1];
         p.y = z + msg->pose.pose.position.y;
       }
       else if (cut_plane_ == "flipped_xy") {
         p.x = input[0];
         p.y = input[1];
         p.z = -z + msg->pose.pose.position.z;
       }
       else if (cut_plane_ == "flipped_yz") {
         p.y = input[0];
         p.z = input[1];
         p.x = -z + msg->pose.pose.position.x;
       }
       else if (cut_plane_ == "flipped_zx") {
         p.z = input[0];
         p.x = input[1];
         p.y = -z + msg->pose.pose.position.y;
       }
       cloud->points.push_back(p);
     }
   }
   sensor_msgs::PointCloud2 ros_cloud;
   pcl::toROSMsg(*cloud, ros_cloud);
   ros_cloud.header = msg->header;
   pub_.publish(ros_cloud);
 }
template <typename PointT> void
pcl16::approximatePolygon2D (const typename pcl16::PointCloud<PointT>::VectorType &polygon, 
                           typename pcl16::PointCloud<PointT>::VectorType &approx_polygon, 
                           float threshold, bool refine, bool closed)
{
  approx_polygon.clear ();
  if (polygon.size () < 3)
    return;
  
  std::vector<std::pair<unsigned, unsigned> > intervals;
  std::pair<unsigned,unsigned> interval (0, 0);
  
  if (closed)
  {
    float max_distance = .0f;
    for (unsigned idx = 1; idx < polygon.size (); ++idx)
    {
      float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) + 
                       (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);

      if (distance > max_distance)
      {
        max_distance = distance;
        interval.second = idx;
      }
    }

    for (unsigned idx = 1; idx < polygon.size (); ++idx)
    {
      float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) + 
                       (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);

      if (distance > max_distance)
      {
        max_distance = distance;
        interval.first = idx;
      }
    }

    if (max_distance < threshold * threshold)
      return;

    intervals.push_back (interval);
    std::swap (interval.first, interval.second);
    intervals.push_back (interval);
  }
  else
  {
    interval.first = 0;
    interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
    intervals.push_back (interval);
  }
  
  std::vector<unsigned> result;
  // recursively refine
  while (!intervals.empty ())
  {
    std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
    float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
    float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
    float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
    
    float linelen = 1.0f / sqrt (line_x * line_x + line_y * line_y);
    
    line_x *= linelen;
    line_y *= linelen;
    line_d *= linelen;
    
    float max_distance = 0.0;
    unsigned first_index = currentInterval.first + 1;
    unsigned max_index = 0;

    // => 0-crossing
    if (currentInterval.first > currentInterval.second)
    {
      for (unsigned idx = first_index; idx < polygon.size(); idx++)
      {
        float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
        if (distance > max_distance)
        {
          max_distance = distance;
          max_index  = idx;
        }
      }
      first_index = 0;
    }

    for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
    {
      float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
      if (distance > max_distance)
      {
        max_distance = distance;
        max_index  = idx;
      }
    }

    if (max_distance > threshold)
    {
      std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
      currentInterval.second = max_index;
      intervals.push_back (interval);
    }
    else
    {
      result.push_back (currentInterval.second);
      intervals.pop_back ();
    }
  }
  
  approx_polygon.reserve (result.size ());
  if (refine)
  {
    std::vector<Eigen::Vector3f> lines (result.size ());
    std::reverse (result.begin (), result.end ());
    for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
    {
      unsigned nIdx = rIdx + 1;
      if (nIdx == result.size ())
        nIdx = 0;
      
      Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
      Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
      unsigned pIdx = result[rIdx];
      unsigned num_points = 0;
      if (pIdx > result[nIdx])
      {
        num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
        for (; pIdx < polygon.size (); ++pIdx)
        {
          covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
          covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
          covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
          centroid [0] += polygon [pIdx].x;
          centroid [1] += polygon [pIdx].y;
        }
        pIdx = 0;
      }
      
      num_points += result[nIdx] - pIdx;
      for (; pIdx < result[nIdx]; ++pIdx)
      {
        covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
        covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
        covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
        centroid [0] += polygon [pIdx].x;
        centroid [1] += polygon [pIdx].y;
      }
      
      covariance.coeffRef (2) = covariance.coeff (1);
      
      float norm = 1.0f / float (num_points);
      centroid *= norm;
      covariance *= norm;
      covariance.coeffRef (0) -= centroid [0] * centroid [0];
      covariance.coeffRef (1) -= centroid [0] * centroid [1];
      covariance.coeffRef (3) -= centroid [1] * centroid [1];
      
      float eval;
      Eigen::Vector2f normal;
      eigen22 (covariance, eval, normal);

      // select the one which is more "parallel" to the original line
      Eigen::Vector2f direction;
      direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
      direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
      direction.normalize ();
      
      if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
      {
        std::swap (normal [0], normal [1]);
        normal [0] = -normal [0];
      }
      
      // needs to be on the left side of the edge
      if (direction [0] * normal [1] < direction [1] * normal [0])
        normal *= -1.0;
      
      lines [rIdx].head<2> () = normal;
      lines [rIdx] [2] = -normal.dot (centroid);
    }
    
    float threshold2 = threshold * threshold;
    for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
    {
      unsigned nIdx = rIdx + 1;
      if (nIdx == result.size ())
        nIdx = 0;      
      
      Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
      vertex /= vertex [2];
      vertex [2] = 0.0;

      PointT point;      
      // test whether we need another edge since the intersection point is too far away from the original vertex
      Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
      pq [2] = 0.0;
      
      float distance = pq.squaredNorm ();
      if (distance > threshold2)
      {
        // test whether the old point is inside the new polygon or outside
        if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
            (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
        {
          float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
          float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];

          point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
          point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];

          approx_polygon.push_back (point);

          vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
          vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
        }
      }
      point.getVector3fMap () = vertex;
      approx_polygon.push_back (point);
    }
  }
  else
  {
    // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)    
    for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
      approx_polygon.push_back (polygon [*it]);
  }
}