Esempio n. 1
0
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
  const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
  std::vector<int> n_indices;
  std::vector<float> n_sqr_distances;

  searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);

  Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);

  Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();

  double distance = 0.0;
  double sum = 0.0;

  int valid_nn_points = 0;

  for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
  {
    Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
    if (pt.head<3> () == central_point.head<3> ())
		  continue;

    // Difference between current point and origin
    vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
    vij (valid_nn_points, 3) = 0;

    distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);

    // Multiply vij * vij'
    cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());

    sum += distance;
    valid_nn_points++;
  }

  if (valid_nn_points < 5)
  {
    //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
    rf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  cov_m /= sum;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);

  const double& e1c = solver.eigenvalues ()[0];
  const double& e2c = solver.eigenvalues ()[1];
  const double& e3c = solver.eigenvalues ()[2];

  if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
  {
    //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
    rf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  // Disambiguation
  Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
  Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
  v1.head<3> () = solver.eigenvectors ().col (2);
  v3.head<3> () = solver.eigenvectors ().col (0);

  int plusNormal = 0, plusTangentDirection1=0;
  for (int ne = 0; ne < valid_nn_points; ne++)
  {
    double dp = vij.row (ne).dot (v1);
    if (dp >= 0)
      plusTangentDirection1++;

    dp = vij.row (ne).dot (v3);
    if (dp >= 0)
      plusNormal++;
  }

  //TANGENT
  plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
  if (plusTangentDirection1 == 0)
  {
		int points = 5; //std::min(valid_nn_points*2/2+1, 11);
		int medianIndex = valid_nn_points/2;

		for (int i = -points/2; i <= points/2; i++)
			if ( vij.row (medianIndex - i).dot (v1) > 0)
				plusTangentDirection1 ++;

		if (plusTangentDirection1 < points/2+1)
			v1 *= - 1;
	} else if (plusTangentDirection1 < 0)
    v1 *= - 1;

  //Normal
  plusNormal = 2*plusNormal - valid_nn_points;
  if (plusNormal == 0)
  {
		int points = 5; //std::min(valid_nn_points*2/2+1, 11);
		int medianIndex = valid_nn_points/2;

		for (int i = -points/2; i <= points/2; i++)
			if ( vij.row (medianIndex - i).dot (v3) > 0)
				plusNormal ++;

		if (plusNormal < points/2+1)
			v3 *= - 1;
	} else if (plusNormal < 0)
    v3 *= - 1;

  rf.row (0) = v1.head<3> ().cast<float> ();
  rf.row (2) = v3.head<3> ().cast<float> ();
  rf.row (1) = rf.row (2).cross (rf.row (0));

  return (0.0f);
}
Esempio n. 2
0
File: board.hpp Progetto: 2php/pcl
template<typename PointInT, typename PointNT, typename PointOutT> float
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computePointLRF (const int &index,
                                                                                        Eigen::Matrix3f &lrf)
{
  //find Z axis

  //extract support points for Rz radius
  std::vector<int> neighbours_indices;
  std::vector<float> neighbours_distances;
  int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);

  //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
  if (n_neighbours < 6)
  {
    //PCL_WARN(
    //    "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n",
    //    getClassName().c_str(), index);

    //setting lrf to NaN
    lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  //copy neighbours coordinates into eigen matrix
  Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
  for (int i = 0; i < n_neighbours; ++i)
  {
    neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
  }

  Eigen::Vector3f x_axis, y_axis;
  //plane fitting to find direction of Z axis
  Eigen::Vector3f fitted_normal; //z_axis
  Eigen::Vector3f centroid;
  planeFitting (neigh_points_mat, centroid, fitted_normal);

  //disambiguate Z axis with normal mean
  normalDisambiguation (*normals_, neighbours_indices, fitted_normal);

  //setting LRF Z axis
  lrf.row (2).matrix () = fitted_normal;

  /////////////////////////////////////////////////////////////////////////////////////////
  //find X axis

  //extract support points for Rx radius
  if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
  {
    n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
  }

  //find point with the "most different" normal (with respect to fittedNormal)

  float min_normal_cos = std::numeric_limits<float>::max ();
  int min_normal_index = -1;

  bool margin_point_found = false;
  Eigen::Vector3f best_margin_point;
  bool best_point_found_on_margins = false;

  float radius2 = tangent_radius_ * tangent_radius_;

  float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;

  float max_boundary_angle = 0;

  if (find_holes_)
  {
    randomOrthogonalAxis (fitted_normal, x_axis);

    lrf.row (0).matrix () = x_axis;

    for (int i = 0; i < check_margin_array_size_; i++)
    {
      check_margin_array_[i] = false;
      margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
      margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
      margin_array_min_angle_normal_[i] = -1.0;
      margin_array_max_angle_normal_[i] = -1.0;
    }
    max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
  }

  for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
  {
    const int& curr_neigh_idx = neighbours_indices[curr_neigh];
    const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
    if (neigh_distance_sqr <= margin_distance2)
    {
      continue;
    }

    //point normalIndex is inside the ring between marginThresh and Radius
    margin_point_found = true;

    Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();

    float normal_cos = fitted_normal.dot (normal_mean);
    if (normal_cos < min_normal_cos)
    {
      min_normal_index = curr_neigh_idx;
      min_normal_cos = normal_cos;
      best_point_found_on_margins = false;
    }

    if (find_holes_)
    {
      //find angle with respect to random axis previously calculated
      Eigen::Vector3f indicating_normal_vect;
      directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
                              surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
      float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);

      int check_margin_array_idx = std::min (static_cast<int> (floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
      check_margin_array_[check_margin_array_idx] = true;

      if (angle < margin_array_min_angle_[check_margin_array_idx])
      {
        margin_array_min_angle_[check_margin_array_idx] = angle;
        margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
      }
      if (angle > margin_array_max_angle_[check_margin_array_idx])
      {
        margin_array_max_angle_[check_margin_array_idx] = angle;
        margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
      }
    }

  } //for each neighbor

  if (!margin_point_found)
  {
    //find among points with neighDistance <= marginThresh*radius
    for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
    {
      const int& curr_neigh_idx = neighbours_indices[curr_neigh];
      const float& neigh_distance_sqr = neighbours_distances[curr_neigh];

      if (neigh_distance_sqr > margin_distance2)
        continue;

      Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();

      float normal_cos = fitted_normal.dot (normal_mean);

      if (normal_cos < min_normal_cos)
      {
        min_normal_index = curr_neigh_idx;
        min_normal_cos = normal_cos;
      }
    }//for each neighbor

    // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
    if (min_normal_index == -1)
    {
      lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
      return (std::numeric_limits<float>::max ());
    }
    //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
    directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
                            surface_->at (min_normal_index).getVector3fMap (), x_axis);
    y_axis = fitted_normal.cross (x_axis);

    lrf.row (0).matrix () = x_axis;
    lrf.row (1).matrix () = y_axis;
    //z axis already set


    return (min_normal_cos);
  }

  if (!find_holes_)
  {
    if (best_point_found_on_margins)
    {
      //if most inclined normal is on support margin
      directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
      y_axis = fitted_normal.cross (x_axis);

      lrf.row (0).matrix () = x_axis;
      lrf.row (1).matrix () = y_axis;
      //z axis already set

      return (min_normal_cos);
    }

    // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
    if (min_normal_index == -1)
    {
      lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
      return (std::numeric_limits<float>::max ());
    }

    directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
                            surface_->at (min_normal_index).getVector3fMap (), x_axis);
    y_axis = fitted_normal.cross (x_axis);

    lrf.row (0).matrix () = x_axis;
    lrf.row (1).matrix () = y_axis;
    //z axis already set

    return (min_normal_cos);
  }// if(!find_holes_)

  //check if there is at least a hole
  bool is_hole_present = false;
  for (int i = 0; i < check_margin_array_size_; i++)
  {
    if (!check_margin_array_[i])
    {
      is_hole_present = true;
      break;
    }
  }

  if (!is_hole_present)
  {
    if (best_point_found_on_margins)
    {
      //if most inclined normal is on support margin
      directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
      y_axis = fitted_normal.cross (x_axis);

      lrf.row (0).matrix () = x_axis;
      lrf.row (1).matrix () = y_axis;
      //z axis already set

      return (min_normal_cos);
    }

    // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
    if (min_normal_index == -1)
    {
      lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
      return (std::numeric_limits<float>::max ());
    }

    //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
    directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
                            surface_->at (min_normal_index).getVector3fMap (), x_axis);
    y_axis = fitted_normal.cross (x_axis);

    lrf.row (0).matrix () = x_axis;
    lrf.row (1).matrix () = y_axis;
    //z axis already set

    return (min_normal_cos);
  }//if (!is_hole_present)

  //case hole found
  //find missing region
  float angle = 0.0;
  int hole_end;
  int hole_first;

  //find first no border pie
  int first_no_border = -1;
  if (check_margin_array_[check_margin_array_size_ - 1])
  {
    first_no_border = 0;
  }
  else
  {
    for (int i = 0; i < check_margin_array_size_; i++)
    {
      if (check_margin_array_[i])
      {
        first_no_border = i;
        break;
      }
    }
  }

  //float steep_prob = 0.0;
  float max_hole_prob = -std::numeric_limits<float>::max ();

  //find holes
  for (int ch = first_no_border; ch < check_margin_array_size_; ch++)
  {
    if (!check_margin_array_[ch])
    {
      //border beginning found
      hole_first = ch;
      hole_end = hole_first + 1;
      while (!check_margin_array_[hole_end % check_margin_array_size_])
      {
        ++hole_end;
      }
      //border end found, find angle

      if ((hole_end - hole_first) > 0)
      {
        //check if hole can be a shapeness hole
        int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
            % check_margin_array_size_;
        int following_hole = (hole_end) % check_margin_array_size_;
        float normal_begin = margin_array_max_angle_normal_[previous_hole];
        float normal_end = margin_array_min_angle_normal_[following_hole];
        normal_begin -= min_normal_cos;
        normal_end -= min_normal_cos;
        normal_begin = normal_begin / (1.0f - min_normal_cos);
        normal_end = normal_end / (1.0f - min_normal_cos);
        normal_begin = 1.0f - normal_begin;
        normal_end = 1.0f - normal_end;

        //evaluate P(Hole);
        float hole_width = 0.0f;
        if (following_hole < previous_hole)
        {
          hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI)
              - margin_array_max_angle_[previous_hole];
        }
        else
        {
          hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
        }
        float hole_prob = hole_width / (2 * static_cast<float> (M_PI));

        //evaluate P(zmin|Hole)
        float steep_prob = (normal_end + normal_begin) / 2.0f;

        //check hole prob and after that, check steepThresh

        if (hole_prob > hole_size_prob_thresh_)
        {
          if (steep_prob > steep_thresh_)
          {
            if (hole_prob > max_hole_prob)
            {
              max_hole_prob = hole_prob;

              float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
              if (following_hole < previous_hole)
              {
                angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
                    * static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
              }
              else
              {
                angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
                    - margin_array_max_angle_[previous_hole]) * angle_weight;
              }
            }
          }
        }
      } //(hole_end-hole_first) > 0

      if (hole_end >= check_margin_array_size_)
      {
        break;
      }
      else
      {
        ch = hole_end - 1;
      }
    }
  }

  if (max_hole_prob > -std::numeric_limits<float>::max ())
  {
    //hole found
    Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
    x_axis = rotation * x_axis;

    min_normal_cos -= 10.0f;
  }
  else
  {
    if (best_point_found_on_margins)
    {
      //if most inclined normal is on support margin
      directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
    }
    else
    {
      // Check if we are not in a degenerate case (all the neighboring normals are NaNs)
      if (min_normal_index == -1)
      {
        lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
        return (std::numeric_limits<float>::max ());
      }

      //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
      directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
                              surface_->at (min_normal_index).getVector3fMap (), x_axis);
    }
  }

  y_axis = fitted_normal.cross (x_axis);

  lrf.row (0).matrix () = x_axis;
  lrf.row (1).matrix () = y_axis;
  //z axis already set

  return (min_normal_cos);
}