Beispiel #1
0
char			*ft_ftoa(double n, long prec)
{
	char		*str;
	long		t;
	long int	pprec;

	if (n != n)
		return (ft_strdup("nan"));
	str = NULL;
	pprec = s_get_pprec(prec);
	s_strcal(&str, n);
	if ((n - (long)(n)) * pprec > 0)
		t = (n - (long)n) * pprec + (double)1 / 10000;
	else
		t = (n - (long)n) * pprec + (double)1 / 10000;
	if (pprec == 0)
		return (str);
	t = (t < 0) ? -t : t;
	s_point(t, pprec, prec, &str);
	return (str);
}
bool HeightmapSampling::calculateNormalsFromHullSurface()
{
  normals_.points.clear();
  shared_ptr < PointCloud<PointXYZ> > sp;
  sp.reset(new PointCloud<PointXYZ> ());
  Vector3d search_point_center(0, 0, 0);
  vector < Vector3d > normals_prestep;

  for (unsigned int i = 0; i < convex_hull_vertices_.size(); i++)
  {
    PointXYZ p(0, 0, 0);

    for (unsigned int j = 0; j < convex_hull_vertices_[i].vertices.size(); j++)
    {
      unsigned int index = convex_hull_vertices_[i].vertices[j];

      p.x += convex_hull_points_->points[index].x;
      p.y += convex_hull_points_->points[index].y;
      p.z += convex_hull_points_->points[index].z;

    }

    p.x /= convex_hull_vertices_[i].vertices.size();
    p.y /= convex_hull_vertices_[i].vertices.size();
    p.z /= convex_hull_vertices_[i].vertices.size();

    search_point_center += Vector3d(p.x, p.y, p.z);

    sp->push_back(p);

    /* normal */
    Vector3d p_centr(p.x, p.y, p.z);

    const PointXYZ& p_a = convex_hull_points_-> points[convex_hull_vertices_[i].vertices[0]];
    const PointXYZ& p_b = convex_hull_points_-> points[convex_hull_vertices_[i].vertices[1]];
    const PointXYZ& p_c = convex_hull_points_-> points[convex_hull_vertices_[i].vertices[2]];

    Vector3d va(p_b.x - p_a.x, p_b.y - p_a.y, p_b.z - p_a.z);
    Vector3d vb(p_c.x - p_a.x, p_c.y - p_a.y, p_c.z - p_a.z);
    Vector3d vn = va.cross(vb);

    vn.normalize();

    normals_prestep.push_back(vn);

  }

  search_point_center /= sp->points.size();

  /* flip normals away from center */
  vector<bool> backside;
  backside.resize(normals_prestep.size(), false);
  for (unsigned int i = 0; i < normals_prestep.size(); i++)
  {
    Vector3d s_point(sp->points[i].x - search_point_center.x(), sp->points[i].y - search_point_center.y(),
                     sp->points[i].z - search_point_center.z());

    if (s_point.dot(normals_prestep[i]) < 0)
    {
      normals_prestep[i] = -normals_prestep[i];
    }

    Vector3d vec_vs(sp->points[i].x - viewp_trans_.x(), sp->points[i].y - viewp_trans_.y(),
        sp->points[i].z - viewp_trans_.z());
    vec_vs.normalize();

    if (vec_vs.dot(normals_prestep[i]) > 0)
    {
      backside[i] = true;
    }
  }

  /* set normals */
  for (unsigned int i = 0; i < normals_prestep.size(); i++)
  {
    if (backside[i])
    {
      continue;
    }

    Normal pcl_n;
    pcl_n.normal_x = normals_prestep[i].x();
    pcl_n.normal_y = normals_prestep[i].y();
    pcl_n.normal_z = normals_prestep[i].z();
    normals_.push_back(pcl_n);
  }

  /* filter and set search-points */
  for (int i = backside.size() - 1; i >= 0; i--)
  {
    if (backside[i])
    {
      sp->points.erase(sp->points.begin() + i);
    }
  }
  search_points_ = sp;

  ROS_DEBUG_STREAM("grasp_template::HeightmapSampling: #normals: " << normals_.size()
      << ", #search_points: " << search_points_->points.size() << endl);
  return true;
}