/**
 * @function fillEq_PLP
 * @brief  Fill PLP equations ( 3 Eq - 2 DOF )
 */
void trifocalTensor::fillEq_PLP( cv::Point3f _A, 
				 cv::Point3f _B,
				 cv::Point3f _C ) {

  Eigen::Vector3f A; A << _A.x, _A.y, _A.z;
  Eigen::Vector3f B; B << _B.x, _B.y, _B.z;
  Eigen::Vector3f C; C << _C.x, _C.y, _C.z;
  std::cout << "Correspondence PLP: " << A.transpose() <<"," <<B.transpose() <<"," <<C.transpose() << std::endl;
  // 3 equations
  for( int s = 0; s < 3; ++s ) {
    // T0, T1, T2
    for( int i = 0; i < 3; ++i ) {
      for( int j = 0; j < 3; ++j ) {
	for( int q = 0; q < 3; ++q ) {
	  for( int k = 0; k < 3; ++k ) {
	    if( epsilon(k,q,s) != 0 ) {
	      mEq( mPointer, 9*i + 3*j + q) = A(i)*B(j)*C(k)*epsilon(k,q,s);
	    }
	  } // k
	} // q
      } // j
    } // i
    
    mPointer++;
  } // s
  
  
}
/**
 * @function fillEq_LLL
 * @brief  Fill LLL equations ( 3 Equations, 2 DOF )
 */
void trifocalTensor::fillEq_LLL( cv::Point3f _A, 
				 cv::Point3f _B,
				 cv::Point3f _C ) {

  Eigen::Vector3f A; A << _A.x, _A.y, _A.z;
  Eigen::Vector3f B; B << _B.x, _B.y, _B.z;
  Eigen::Vector3f C; C << _C.x, _C.y, _C.z;
  
  std::cout << "[LLL] Correspondence: " << A.transpose() <<"," <<B.transpose() <<"," <<C.transpose() << std::endl;
  // 3 equations
  for( int s = 0; s < 3; ++s ) {
    // T0, T1, T2
    for( int i = 0; i < 3; ++i ) {
      for( int j = 0; j < 3; ++j ) {
	for( int k = 0; k < 3; ++k ) {
	  for( int r = 0; r < 3; ++r ) {	      
	    mEq( mPointer, 9*i + 3*j + k) += A(r)*B(j)*C(k)*epsilon(r,i,s);
	  } // r
	} // k
      } // j
    } // i
    
    mPointer++;
  } // s

}
Beispiel #3
0
  /** \brief Computes the gradient parameters of the patch (patch gradient components dx_ dy_, Hessian H_, Shi-Tomasi Score s_, Eigenvalues of the Hessian e0_ and e1_).
   *         The expanded patch patchWithBorder_ must be set.
   *         Sets validGradientParameters_ afterwards to true.
   */
  void computeGradientParameters() const{
    if(!validGradientParameters_){
      H_.setZero();
      const int refStep = patchSize+2;
      float* it_dx = dx_;
      float* it_dy = dy_;
      const float* it;
      Eigen::Vector3f J;
      J.setZero();
      for(int y=0; y<patchSize; ++y){
        it = patchWithBorder_ + (y+1)*refStep + 1;
        for(int x=0; x<patchSize; ++x, ++it, ++it_dx, ++it_dy){
          J[0] = 0.5 * (it[1] - it[-1]);
          J[1] = 0.5 * (it[refStep] - it[-refStep]);
          J[2] = 1;
          *it_dx = J[0];
          *it_dy = J[1];
          H_ += J*J.transpose();
        }
      }
      const float dXX = H_(0,0)/(patchSize*patchSize);
      const float dYY = H_(1,1)/(patchSize*patchSize);
      const float dXY = H_(0,1)/(patchSize*patchSize);

      e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
      e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
      s_ = e0_;
      validGradientParameters_ = true;
    }
  }
float robotDepth()
{
	// Two interesting points
	float alpha1 = (x + (sx-height)/2)/(float)height*2*tan(VFOV/2.);
	float alpha2 = (x - (sx+height)/2)/(float)height*2*tan(VFOV/2.);
	float beta = (y - width/2)/(float)width*2*tan(HFOV/2.);

	// Vectors
	Eigen::Vector3f v1(1,-beta,-alpha1);
	Eigen::Vector3f v2(1,-beta,-alpha2);

	// Normalization
	v1 = v1/v1.norm();
	v2 = v2/v2.norm();

	// Center
	Eigen::Vector3f c = (v1+v2)/2.;
	float c_norm = c.norm();

	// Projection
	Eigen::MatrixXf proj_mat = c.transpose()*v1;
	float proj = proj_mat(0,0);

	// Orthogonal part in v1
	Eigen::Vector3f orth = v1 - proj/c_norm*c;

	// Norm
	float orth_norm = orth.norm();

	// Approximate depth
	float d = H/2.*proj/orth_norm;
	return d;
}
/**
 * @function fillEq_PLL 
 * @brief  Fill PLL equations ( 1 Equation - 1 DOF )
 */
void trifocalTensor::fillEq_PLL( cv::Point3f _A, 
				 cv::Point3f _B,
				 cv::Point3f _C ) {

  Eigen::Vector3f A; A << _A.x, _A.y, _A.z;
  Eigen::Vector3f B; B << _B.x, _B.y, _B.z;
  Eigen::Vector3f C; C << _C.x, _C.y, _C.z;
  std::cout << "PLL Correspondence: " << A.transpose() <<"," <<B.transpose() <<"," <<C.transpose() << std::endl;

    // T0, T1, T2
    for( int i = 0; i < 3; ++i ) {
      for( int j = 0; j < 3; ++j ) {
	for( int k = 0; k < 3; ++k ) {
	    mEq( mPointer, 9*i + 3*j + k) += A(i)*B(j)*C(k);
	} // k
      } // j
    } // i
    mPointer++;

}
void GenericDistanceConstraint::gradientFct(
    const unsigned int i,
    const unsigned int numberOfParticles,
    const float mass[],
    const Eigen::Vector3f x[],
    void *userData,
    Eigen::Matrix<float, 1, 3> &jacobian)
{
    Eigen::Vector3f n = x[i] - x[1 - i];
    n.normalize();
    jacobian = n.transpose();
}
Beispiel #7
0
Eigen::Matrix4f lookAt(const Eigen::Vector3f &eye,
                       const Eigen::Vector3f &center,
                       const Eigen::Vector3f &up) {
    Eigen::Vector3f f = (center - eye).normalized();
    Eigen::Vector3f s = f.cross(up).normalized();
    Eigen::Vector3f u = s.cross(f);

    Eigen::Matrix4f Result = Eigen::Matrix4f::Identity();
    Result(0, 0) = s(0);
    Result(0, 1) = s(1);
    Result(0, 2) = s(2);
    Result(1, 0) = u(0);
    Result(1, 1) = u(1);
    Result(1, 2) = u(2);
    Result(2, 0) = -f(0);
    Result(2, 1) = -f(1);
    Result(2, 2) = -f(2);
    Result(0, 3) = -s.transpose() * eye;
    Result(1, 3) = -u.transpose() * eye;
    Result(2, 3) = f.transpose() * eye;
    return Result;
}
Beispiel #8
0
Eigen::Matrix3f
ElementHex::GetDeformationGrad(const Eigen::Vector3f & p,
    const std::vector<Eigen::Vector3f> & X,
    const std::vector<Eigen::Vector3f> & u)
{
  Eigen::Matrix3f F = Eigen::Matrix3f::Identity();
  for(int ii = 0;ii<8;ii++){
    int idx = GetNodeIndex(ii);
    Eigen::Vector3f gradN = ShapeFunGrad(ii,p,X);
    //outer product
    F += u[idx] * gradN.transpose();
  }
  return F;
}
Beispiel #9
0
void OCLSLAM<CR, CW>::display ()
{
    double angle = 180.0 / M_PI * 2.0 * std::atan2 (q_g.vec ().norm (), q_g.w ());  // in degrees
    Eigen::Vector3f axis = ((angle == 0.0) ? Eigen::Vector3f::Zero () : q_g.vec ().normalized ());
    std::cout << "    Time step             :    " << timeStep << std::endl;
    std::cout << "    Latency               :    " << timer.stop () << " [ms]" << std::endl;
    std::cout << "    ICP iterations        :    " << icp.k << std::endl;
    std::cout << "    ICP latency           :    " << lICP << " [ms]" << std::endl;
    std::cout << "    Localization               " << std::endl;
    std::cout << "    - Translation vector  :    " << t_g.transpose () << " [mm]" << std::endl;
    std::cout << "    - Rotation axis       :    " << axis.transpose () << std::endl;
    std::cout << "    - Rotation angle      :    " << angle << " [degrees]" << std::endl;
    std::cout << "===========================    " << std::endl;
    timer.start ();
}
Beispiel #10
0
wcFloat wcCostEstimator::cost( const wcMetaVector& meta )
{
	const Eigen::Vector3f V1(Eigen::Vector3f().Ones());
/*
	for( wcPos idx = 0; idx < 3; idx++ )
		if(meta(idx)<WC_PRECISION) return 1.0f;

	wcMetaVector delta0 = (meta - optima);
	wcMetaVector delta1 = (meta - V1);
	wcFloat		 maxQR	= ((V1.transpose()*Qc*V1 + V1.transpose()*Rc*V1)(0));

	return (delta0.transpose()*Qc*delta0 + delta1.transpose()*Rc*delta1)(0)/maxQR;
*/
	for( wcPos idx = 0; idx < 3; idx++ )
		if(meta(idx)<WC_PRECISION) return 1.0f;

	wcMetaVector delta0 = (meta - optima);
	wcFloat		 maxQR	= ((V1.transpose()*Qc*V1))(0);

	return (delta0.transpose()*Qc*delta0)(0)/maxQR;
	
}
Beispiel #11
0
template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelectorT>::computePointIntensityGradient (
  const pcl::PointCloud <PointInT> &cloud, const std::vector <int> &indices,
  const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
{
  if (indices.size () < 3)
  {
    gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
    return;
  }

  Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
  Eigen::Vector3f b = Eigen::Vector3f::Zero ();

  for (size_t i_point = 0; i_point < indices.size (); ++i_point)
  {
    PointInT p = cloud.points[indices[i_point]];
    if (!pcl_isfinite (p.x) ||
        !pcl_isfinite (p.y) ||
        !pcl_isfinite (p.z) ||
        !pcl_isfinite (intensity_ (p)))
      continue;

    p.x -= point[0];
    p.y -= point[1];
    p.z -= point[2];
    intensity_.demean (p, mean_intensity);

    A (0, 0) += p.x * p.x;
    A (0, 1) += p.x * p.y;
    A (0, 2) += p.x * p.z;

    A (1, 1) += p.y * p.y;
    A (1, 2) += p.y * p.z;

    A (2, 2) += p.z * p.z;

    b[0] += p.x * intensity_ (p);
    b[1] += p.y * intensity_ (p);
    b[2] += p.z * intensity_ (p);
  }
  // Fill in the lower triangle of A
  A (1, 0) = A (0, 1);
  A (2, 0) = A (0, 2);
  A (2, 1) = A (1, 2);

//*
  Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
/*/

  Eigen::Vector3f eigen_values;
  Eigen::Matrix3f eigen_vectors;
  eigen33 (A, eigen_vectors, eigen_values);

  b = eigen_vectors.transpose () * b;

  if ( eigen_values (0) != 0)
    b (0) /= eigen_values (0);
  else
    b (0) = 0;

  if ( eigen_values (1) != 0)
    b (1) /= eigen_values (1);
  else
    b (1) = 0;

  if ( eigen_values (2) != 0)
    b (2) /= eigen_values (2);
  else
    b (2) = 0;


  Eigen::Vector3f x = eigen_vectors * b;

//  if (A.col (0).squaredNorm () != 0)
//    x [0] /= A.col (0).squaredNorm ();
//  b -= x [0] * A.col (0);
//
//
//  if (A.col (1).squaredNorm ()  != 0)
//    x [1] /= A.col (1).squaredNorm ();
//  b -= x[1] * A.col (1);
//
//  x [2] = b.dot (A.col (2));
//  if (A.col (2).squaredNorm () != 0)
//    x[2] /= A.col (2).squaredNorm ();
  // Fit a hyperplane to the data

//*/
//  std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n";
//  std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n";
  // Project the gradient vector, x, onto the tangent plane
  gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
}
Beispiel #12
0
    });


    auto isAtTarget = [this](){
        Eigen::Vector3f lookat = glwidget_->camera_.getRotation() * -Eigen::Vector3f::UnitZ();
        Eigen::Vector3f look_proj = lookat;
        look_proj[1] = 0;
        look_proj.normalize();
        float rads = acos(lookat.dot(look_proj));
        //qDebug() << "rads" << rads;


        Eigen::Vector3f pos = glwidget_->camera_.getPosition();

        std::cout << "target: " << target_.transpose() << std::endl;
        std::cout << "pos: " << pos.transpose() << std::endl;

        float dist = (pos - target_).norm();
        distance_text_->setText(QString::number(dist));
//        qDebug() << "dist" << dist;

        return dist < 2 && fabs(rads) < 0.2;
    };

    // Start when moving the camera
    connect(&glwidget_->camera_, &Camera::modified, [this, isAtTarget, elapsed_time_text](){
        if(!running_){
            if(ready_to_start_){
                time_.restart();
                running_ = true;
            } else {
Beispiel #13
0
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
{
  const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());

  std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles);
  std::vector <float> triangle_area (number_of_triangles);
  std::vector <float> distance_weight (number_of_triangles);

  float total_area = 0.0f;
  const float coeff = 1.0f / 12.0f;
  const float coeff_1_div_3 = 1.0f / 3.0f;

  Eigen::Vector3f feature_point (point.x, point.y, point.z);

  std::set <unsigned int>::const_iterator it;
  unsigned int i_triangle = 0;
  for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
  {
    Eigen::Vector3f pt[3];
    for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
    {
      const unsigned int index = triangles_[*it].vertices[i_vertex];
      pt[i_vertex] (0) = surface_->points[index].x;
      pt[i_vertex] (1) = surface_->points[index].y;
      pt[i_vertex] (2) = surface_->points[index].z;
    }

    const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
    triangle_area[i_triangle] = curr_area;
    total_area += curr_area;

    distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);

    Eigen::Matrix3f curr_scatter_matrix;
    curr_scatter_matrix.setZero ();
    for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
    {
      Eigen::Vector3f vec = pt[i_pt] - feature_point;
      curr_scatter_matrix += vec * (vec.transpose ());
      for (unsigned int j_pt = 0; j_pt < 3; j_pt++)
        curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ());
    }
    scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
  }

  if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
    total_area = 1.0f / total_area;
  else
    total_area = 1.0f;

  Eigen::Matrix3f overall_scatter_matrix;
  overall_scatter_matrix.setZero ();
  std::vector<float> total_weight (number_of_triangles);
  const float denominator = 1.0f / 6.0f;
  for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
  {
    float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
    overall_scatter_matrix += factor * scatter_matrices[i_triangle];
    total_weight[i_triangle] = factor * denominator;
  }

  Eigen::Vector3f v1, v2, v3;
  computeEigenVectors (overall_scatter_matrix, v1, v2, v3);

  float h1 = 0.0f;
  float h3 = 0.0f;
  for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
  {
    Eigen::Vector3f pt[3];
    for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
    {
      const unsigned int index = triangles_[*it].vertices[i_vertex];
      pt[i_vertex] (0) = surface_->points[index].x;
      pt[i_vertex] (1) = surface_->points[index].y;
      pt[i_vertex] (2) = surface_->points[index].z;
    }

    float factor1 = 0.0f;
    float factor3 = 0.0f;
    for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
    {
      Eigen::Vector3f vec = pt[i_pt] - feature_point;
      factor1 += vec.dot (v1);
      factor3 += vec.dot (v3);
    }
    h1 += total_weight[i_triangle] * factor1;
    h3 += total_weight[i_triangle] * factor3;
  }

  if (h1 < 0.0f) v1 = -v1;
  if (h3 < 0.0f) v3 = -v3;

  v2 = v3.cross (v1);

  lrf_matrix.row (0) = v1;
  lrf_matrix.row (1) = v2;
  lrf_matrix.row (2) = v3;
}
Beispiel #14
0
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
                                                               PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
                                                               PointInTPtr & grid, pcl::PointIndices & indices)
{

  Eigen::Vector3f plane_normal;
  plane_normal[0] = -centroid[0];
  plane_normal[1] = -centroid[1];
  plane_normal[2] = -centroid[2];
  Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
  plane_normal.normalize ();
  Eigen::Vector3f axis = plane_normal.cross (z_vector);
  double rotation = -asin (axis.norm ());
  axis.normalize ();

  Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));

  grid->points.resize (processed->points.size ());
  for (size_t k = 0; k < processed->points.size (); k++)
    grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();

  pcl::transformPointCloud (*grid, *grid, transformPC);

  Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
  Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);

  centroid4f = transformPC * centroid4f;
  normal_centroid4f = transformPC * normal_centroid4f;

  Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);

  Eigen::Vector4f farthest_away;
  pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
  farthest_away[3] = 0;

  float max_dist = (farthest_away - centroid4f).norm ();

  pcl::demeanPointCloud (*grid, centroid4f, *grid);

  Eigen::Matrix4f center_mat;
  center_mat.setIdentity (4, 4);
  center_mat (0, 3) = -centroid4f[0];
  center_mat (1, 3) = -centroid4f[1];
  center_mat (2, 3) = -centroid4f[2];

  Eigen::Matrix3f scatter;
  scatter.setZero ();
  float sum_w = 0.f;

  //for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
  for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
  {
    Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
    float d_k = (pvector).norm ();
    float w = (max_dist - d_k);
    Eigen::Vector3f diff = (pvector);
    Eigen::Matrix3f mat = diff * diff.transpose ();
    scatter = scatter + mat * w;
    sum_w += w;
  }

  scatter /= sum_w;

  Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
  Eigen::Vector3f evx = svd.matrixV ().col (0);
  Eigen::Vector3f evy = svd.matrixV ().col (1);
  Eigen::Vector3f evz = svd.matrixV ().col (2);
  Eigen::Vector3f evxminus = evx * -1;
  Eigen::Vector3f evyminus = evy * -1;
  Eigen::Vector3f evzminus = evz * -1;

  float s_xplus, s_xminus, s_yplus, s_yminus;
  s_xplus = s_xminus = s_yplus = s_yminus = 0.f;

  //disambiguate rf using all points
  for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
  {
    Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
    float dist_x, dist_y;
    dist_x = std::abs (evx.dot (pvector));
    dist_y = std::abs (evy.dot (pvector));

    if ((pvector).dot (evx) >= 0)
      s_xplus += dist_x;
    else
      s_xminus += dist_x;

    if ((pvector).dot (evy) >= 0)
      s_yplus += dist_y;
    else
      s_yminus += dist_y;

  }

  if (s_xplus < s_xminus)
    evx = evxminus;

  if (s_yplus < s_yminus)
    evy = evyminus;

  //select the axis that could be disambiguated more easily
  float fx, fy;
  float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
  float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
  float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
  float min_y = static_cast<float> (std::min (s_yplus, s_yminus));

  fx = (min_x / max_x);
  fy = (min_y / max_y);

  Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
  if (normal3f.dot (evz) < 0)
    evz = evzminus;

  //if fx/y close to 1, it was hard to disambiguate
  //what if both are equally easy or difficult to disambiguate, namely fy == fx or very close

  float max_axis = std::max (fx, fy);
  float min_axis = std::min (fx, fy);

  if ((min_axis / max_axis) > axis_ratio_)
  {
    PCL_WARN("Both axis are equally easy/difficult to disambiguate\n");

    Eigen::Vector3f evy_copy = evy;
    Eigen::Vector3f evxminus = evx * -1;
    Eigen::Vector3f evyminus = evy * -1;

    if (min_axis > min_axis_value_)
    {
      //combination of all possibilities
      evy = evx.cross (evz);
      Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      evx = evxminus;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      evx = evy_copy;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      evx = evyminus;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

    }
    else
    {
      //1-st case (evx selected)
      evy = evx.cross (evz);
      Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      //2-nd case (evy selected)
      evx = evy_copy;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);
    }
  }
  else
  {
    if (fy < fx)
    {
      evx = evy;
      fx = fy;
    }

    evy = evx.cross (evz);
    Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
    transformations.push_back (trans);

  }

  return true;
}
template <typename PointInT, typename PointNT, typename PointLabelT, typename PointOutT> void
cob_3d_features::OrganizedCurvatureEstimation<PointInT,PointNT,PointLabelT,PointOutT>::computePointCurvatures (
  const NormalCloudIn &normals,
  const int index,
  const std::vector<int> &indices,
  const std::vector<float> &sqr_distances,
  float &pcx, float &pcy, float &pcz, float &pc1, float &pc2,
  int &label_out)
{
  Eigen::Vector3f n_idx(normals.points[index].normal);
  Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - n_idx * n_idx.transpose(); // projection matrix

  std::vector<Eigen::Vector3f> normals_projected;
  normals_projected.reserve(n_points_);
  Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
  Eigen::Vector3f p_idx = surface_->points[index].getVector3fMap();
  float angle = 0.0; // to discriminate convex and concave surface
  int prob_concave = 0, prob_convex = 0;
  for (std::vector<int>::const_iterator it = indices.begin(); it != indices.end(); ++it)
  {
    PointNT const* n_i = &(normals.points[*it]);
    if ( pcl_isnan(n_i->normal[2]) ) continue;
    normals_projected.push_back( M * Eigen::Vector3f(n_i->normal) );
    centroid += normals_projected.back();
    if ( (surface_->points[*it].getVector3fMap() - p_idx).normalized().dot(n_idx) > 0.0) ++prob_concave;
    else ++prob_convex;
  }

  if (normals_projected.size() <=1) return;
  float num_p_inv = 1.0f / normals_projected.size();
  centroid *= num_p_inv;

  Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
  {
    std::vector<Eigen::Vector3f>::iterator n_it = normals_projected.begin();
    std::vector<float>::const_iterator d_it = sqr_distances.begin();
    for (; n_it != normals_projected.end(); ++n_it, ++d_it)
    {
      Eigen::Vector3f demean = *n_it - centroid;
      //cov += 1.0f / sqrt(*d_it) * demean * demean.transpose();
      cov += demean * demean.transpose();
    }
  }

  Eigen::Vector3f eigenvalues;
  Eigen::Matrix3f eigenvectors;
  pcl::eigen33(cov, eigenvectors, eigenvalues);
  pcx = eigenvectors (0,2);
  pcy = eigenvectors (1,2);
  pcz = eigenvectors (2,2);
  if (prob_concave < prob_convex) // if convex, make eigenvalues negative
    num_p_inv *= surface_->points[index].z * (-1);
  //num_p_inv *= 1.0 / (10.0*centroid.norm()) * surface_->points[index].z * (-1);
  else
    num_p_inv *= surface_->points[index].z;

  pc1 = eigenvalues (2) * num_p_inv;
  pc2 = eigenvalues (1) * num_p_inv;
  //normals_->points[index].curvature = curvatures_->points[index].pc1;
  if (std::abs(pc1) >= edge_curvature_threshold_ && label_out == 0)
    label_out = I_EDGE;
}
Beispiel #16
0
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormalMirror (
    const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
{
  float bad_point = std::numeric_limits<float>::quiet_NaN ();

  const int width = input_->width;
  const int height = input_->height;

  // ==============================================================
  if (normal_estimation_method_ == COVARIANCE_MATRIX) 
  {
    if (!init_covariance_matrix_)
      initCovarianceMatrixMethod ();

    const int start_x = pos_x - rect_width_2_;
    const int start_y = pos_y - rect_height_2_;
    const int end_x = start_x + rect_width_;
    const int end_y = start_y + rect_height_;

    unsigned count = 0;
    sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_XYZ_, _1, _2, _3, _4), count);
    
    // no valid points within the rectangular reagion?
    if (count == 0)
    {
      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
      return;
    }

    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
    Eigen::Vector3f center;
    typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
    typename IntegralImage2D<float, 3>::ElementType tmp_center;
    typename IntegralImage2D<float, 3>::SecondOrderType tmp_so_elements;

    center[0] = 0;
    center[1] = 0;
    center[2] = 0;
    tmp_center[0] = 0;
    tmp_center[1] = 0;
    tmp_center[2] = 0;
    so_elements[0] = 0;
    so_elements[1] = 0;
    so_elements[2] = 0;
    so_elements[3] = 0;
    so_elements[4] = 0;
    so_elements[5] = 0;

    sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), tmp_center);
    sumArea<typename IntegralImage2D<float, 3>::SecondOrderType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getSecondOrderSumSE, &integral_image_XYZ_, _1, _2, _3, _4), so_elements);

    center[0] = float (tmp_center[0]);
    center[1] = float (tmp_center[1]);
    center[2] = float (tmp_center[2]);

    covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
    covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
    covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
    covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
    covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
    covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
    covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
    float eigen_value;
    Eigen::Vector3f eigen_vector;
    pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
    flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
    normal.getNormalVector3fMap () = eigen_vector;

    // Compute the curvature surface change
    if (eigen_value > 0.0)
      normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
    else
      normal.curvature = 0;

    return;
  }
  // =======================================================
  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT) 
  {
    if (!init_average_3d_gradient_)
      initAverage3DGradientMethod ();

    const int start_x = pos_x - rect_width_2_;
    const int start_y = pos_y - rect_height_2_;
    const int end_x = start_x + rect_width_;
    const int end_y = start_y + rect_height_;

    unsigned count_x = 0;
    unsigned count_y = 0;

    sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DX_, _1, _2, _3, _4), count_x);
    sumArea<unsigned>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFiniteElementsCountSE, &integral_image_DY_, _1, _2, _3, _4), count_y);


    if (count_x == 0 || count_y == 0)
    {
      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
      return;
    }
    Eigen::Vector3d gradient_x (0, 0, 0);
    Eigen::Vector3d gradient_y (0, 0, 0);

    sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DX_, _1, _2, _3, _4), gradient_x);
    sumArea<typename IntegralImage2D<float, 3>::ElementType>(start_x, start_y, end_x, end_y, width, height, boost::bind(&IntegralImage2D<float, 3>::getFirstOrderSumSE, &integral_image_DY_, _1, _2, _3, _4), gradient_y);


    Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
    double normal_length = normal_vector.squaredNorm ();
    if (normal_length == 0.0f)
    {
      normal.getNormalVector3fMap ().setConstant (bad_point);
      normal.curvature = bad_point;
      return;
    }

    normal_vector /= sqrt (normal_length);

    float nx = static_cast<float> (normal_vector [0]);
    float ny = static_cast<float> (normal_vector [1]);
    float nz = static_cast<float> (normal_vector [2]);

    flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);

    normal.normal_x = nx;
    normal.normal_y = ny;
    normal.normal_z = nz;
    normal.curvature = bad_point;
    return;
  }
  // ======================================================
  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE) 
  {
    if (!init_depth_change_)
      initAverageDepthChangeMethod ();

    int point_index_L_x = pos_x - rect_width_4_ - 1;
    int point_index_L_y = pos_y;
    int point_index_R_x = pos_x + rect_width_4_ + 1;
    int point_index_R_y = pos_y;
    int point_index_U_x = pos_x - 1;
    int point_index_U_y = pos_y - rect_height_4_;
    int point_index_D_x = pos_x + 1;
    int point_index_D_y = pos_y + rect_height_4_;

    if (point_index_L_x < 0)
      point_index_L_x = -point_index_L_x;
    if (point_index_U_x < 0)
      point_index_U_x = -point_index_U_x;
    if (point_index_U_y < 0)
      point_index_U_y = -point_index_U_y;

    if (point_index_R_x >= width)
      point_index_R_x = width-(point_index_R_x-(width-1));
    if (point_index_D_x >= width)
      point_index_D_x = width-(point_index_D_x-(width-1));
    if (point_index_D_y >= height)
      point_index_D_y = height-(point_index_D_y-(height-1));

    const int start_x_L = pos_x - rect_width_2_;
    const int start_y_L = pos_y - rect_height_4_;
    const int end_x_L = start_x_L + rect_width_2_;
    const int end_y_L = start_y_L + rect_height_2_;

    const int start_x_R = pos_x + 1;
    const int start_y_R = pos_y - rect_height_4_;
    const int end_x_R = start_x_R + rect_width_2_;
    const int end_y_R = start_y_R + rect_height_2_;

    const int start_x_U = pos_x - rect_width_4_;
    const int start_y_U = pos_y - rect_height_2_;
    const int end_x_U = start_x_U + rect_width_2_;
    const int end_y_U = start_y_U + rect_height_2_;

    const int start_x_D = pos_x - rect_width_4_;
    const int start_y_D = pos_y + 1;
    const int end_x_D = start_x_D + rect_width_2_;
    const int end_y_D = start_y_D + rect_height_2_;

    unsigned count_L_z = 0;
    unsigned count_R_z = 0;
    unsigned count_U_z = 0;
    unsigned count_D_z = 0;

    sumArea<unsigned>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_L_z);
    sumArea<unsigned>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_R_z);
    sumArea<unsigned>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_U_z);
    sumArea<unsigned>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFiniteElementsCountSE, &integral_image_depth_, _1, _2, _3, _4), count_D_z);

    if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
    {
      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = bad_point;
      return;
    }

    float mean_L_z = 0;
    float mean_R_z = 0;
    float mean_U_z = 0;
    float mean_D_z = 0;

    sumArea<float>(start_x_L, start_y_L, end_x_L, end_y_L, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_L_z);
    sumArea<float>(start_x_R, start_y_R, end_x_R, end_y_R, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_R_z);
    sumArea<float>(start_x_U, start_y_U, end_x_U, end_y_U, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_U_z);
    sumArea<float>(start_x_D, start_y_D, end_x_D, end_y_D, width, height, boost::bind(&IntegralImage2D<float, 1>::getFirstOrderSumSE, &integral_image_depth_, _1, _2, _3, _4), mean_D_z);

    mean_L_z /= float (count_L_z);
    mean_R_z /= float (count_R_z);
    mean_U_z /= float (count_U_z);
    mean_D_z /= float (count_D_z);


    PointInT pointL = input_->points[point_index_L_y*width + point_index_L_x];
    PointInT pointR = input_->points[point_index_R_y*width + point_index_R_x];
    PointInT pointU = input_->points[point_index_U_y*width + point_index_U_x];
    PointInT pointD = input_->points[point_index_D_y*width + point_index_D_x];

    const float mean_x_z = mean_R_z - mean_L_z;
    const float mean_y_z = mean_D_z - mean_U_z;

    const float mean_x_x = pointR.x - pointL.x;
    const float mean_x_y = pointR.y - pointL.y;
    const float mean_y_x = pointD.x - pointU.x;
    const float mean_y_y = pointD.y - pointU.y;

    float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
    float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
    float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;

    const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);

    if (normal_length == 0.0f)
    {
      normal.getNormalVector3fMap ().setConstant (bad_point);
      normal.curvature = bad_point;
      return;
    }

    flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
    
    const float scale = 1.0f / sqrtf (normal_length);

    normal.normal_x = normal_x * scale;
    normal.normal_y = normal_y * scale;
    normal.normal_z = normal_z * scale;
    normal.curvature = bad_point;

    return;
  }
  // ========================================================
  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT) 
  {
    PCL_THROW_EXCEPTION (PCLException, "BORDER_POLICY_MIRROR not supported for normal estimation method SIMPLE_3D_GRADIENT");
  }

  normal.getNormalVector3fMap ().setConstant (bad_point);
  normal.curvature = bad_point;
  return;
}
template <typename PointInT, typename PointOutT> void
pcl::IntegralImageNormalEstimation<PointInT, PointOutT>::computePointNormal (
    const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
{
  float bad_point = std::numeric_limits<float>::quiet_NaN ();

  if (normal_estimation_method_ == COVARIANCE_MATRIX)
  {
    if (!init_covariance_matrix_)
      initCovarianceMatrixMethod ();

    unsigned count = integral_image_XYZ_.getFiniteElementsCount (pos_x - (rect_width_2_), pos_y - (rect_height_2_), rect_width_, rect_height_);

    // no valid points within the rectangular reagion?
    if (count == 0)
    {
      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
      return;
    }

    EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
    Eigen::Vector3f center;
    typename IntegralImage2D<float, 3>::SecondOrderType so_elements;
    center = integral_image_XYZ_.getFirstOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_).cast<float> ();
    so_elements = integral_image_XYZ_.getSecondOrderSum(pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);

    covariance_matrix.coeffRef (0) = static_cast<float> (so_elements [0]);
    covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = static_cast<float> (so_elements [1]);
    covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = static_cast<float> (so_elements [2]);
    covariance_matrix.coeffRef (4) = static_cast<float> (so_elements [3]);
    covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = static_cast<float> (so_elements [4]);
    covariance_matrix.coeffRef (8) = static_cast<float> (so_elements [5]);
    covariance_matrix -= (center * center.transpose ()) / static_cast<float> (count);
    float eigen_value;
    Eigen::Vector3f eigen_vector;
    pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
    //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector);
    pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, eigen_vector[0], eigen_vector[1], eigen_vector[2]);
    normal.getNormalVector3fMap () = eigen_vector;

    // Compute the curvature surface change
    if (eigen_value > 0.0)
      normal.curvature = fabsf (eigen_value / (covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8)));
    else
      normal.curvature = 0;

    return;
  }
  else if (normal_estimation_method_ == AVERAGE_3D_GRADIENT)
  {
    if (!init_average_3d_gradient_)
      initAverage3DGradientMethod ();

    unsigned count_x = integral_image_DX_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
    unsigned count_y = integral_image_DY_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
    if (count_x == 0 || count_y == 0)
    {
      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
      return;
    }
    Eigen::Vector3d gradient_x = integral_image_DX_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
    Eigen::Vector3d gradient_y = integral_image_DY_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);

    Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
    double normal_length = normal_vector.squaredNorm ();
    if (normal_length == 0.0f)
    {
      normal.getNormalVector4fMap ().setConstant (bad_point);
      normal.curvature = bad_point;
      return;
    }

    normal_vector /= sqrt (normal_length);

    float nx = static_cast<float> (normal_vector [0]);
    float ny = static_cast<float> (normal_vector [1]);
    float nz = static_cast<float> (normal_vector [2]);

    //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
    pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);

    normal.normal_x = nx;
    normal.normal_y = ny;
    normal.normal_z = nz;
    normal.curvature = bad_point;
    return;
  }
  else if (normal_estimation_method_ == AVERAGE_DEPTH_CHANGE)
  {
    if (!init_depth_change_)
      initAverageDepthChangeMethod ();

//    unsigned count = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, rect_height_);
//    if (count == 0)
//    {
//      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
//      return;
//    }
//    const float mean_L_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ - 1, pos_y - rect_height_2_    , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
//    const float mean_R_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_ + 1, pos_y - rect_height_2_    , rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
//    const float mean_U_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_    , pos_y - rect_height_2_ - 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));
//    const float mean_D_z = integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_    , pos_y - rect_height_2_ + 1, rect_width_ - 1, rect_height_ - 1) / ((rect_width_-1)*(rect_height_-1));

    // width and height are at least 3 x 3
    unsigned count_L_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
    unsigned count_R_z = integral_image_depth_.getFiniteElementsCount (pos_x + 1            , pos_y - rect_height_4_, rect_width_2_, rect_height_2_);
    unsigned count_U_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_);
    unsigned count_D_z = integral_image_depth_.getFiniteElementsCount (pos_x - rect_width_4_, pos_y + 1             , rect_width_2_, rect_height_2_);

    if (count_L_z == 0 || count_R_z == 0 || count_U_z == 0 || count_D_z == 0)
    {
      normal.normal_x = normal.normal_y = normal.normal_z = normal.curvature = std::numeric_limits<float>::quiet_NaN ();
      return;
    }

    float mean_L_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_L_z);
    float mean_R_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x + 1            , pos_y - rect_height_4_, rect_width_2_, rect_height_2_) / count_R_z);
    float mean_U_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y - rect_height_2_, rect_width_2_, rect_height_2_) / count_U_z);
    float mean_D_z = static_cast<float> (integral_image_depth_.getFirstOrderSum (pos_x - rect_width_4_, pos_y + 1             , rect_width_2_, rect_height_2_) / count_D_z);

    PointInT pointL = input_->points[point_index - rect_width_4_ - 1];
    PointInT pointR = input_->points[point_index + rect_width_4_ + 1];
    PointInT pointU = input_->points[point_index - rect_height_4_ * input_->width - 1];
    PointInT pointD = input_->points[point_index + rect_height_4_ * input_->width + 1];

    const float mean_x_z = mean_R_z - mean_L_z;
    const float mean_y_z = mean_D_z - mean_U_z;

    const float mean_x_x = pointR.x - pointL.x;
    const float mean_x_y = pointR.y - pointL.y;
    const float mean_y_x = pointD.x - pointU.x;
    const float mean_y_y = pointD.y - pointU.y;

    float normal_x = mean_x_y * mean_y_z - mean_x_z * mean_y_y;
    float normal_y = mean_x_z * mean_y_x - mean_x_x * mean_y_z;
    float normal_z = mean_x_x * mean_y_y - mean_x_y * mean_y_x;

    const float normal_length = (normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);

    if (normal_length == 0.0f)
    {
      normal.getNormalVector4fMap ().setConstant (bad_point);
      normal.curvature = bad_point;
      return;
    }

    pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_x, normal_y, normal_z);
    
    const float scale = 1.0f / sqrt (normal_length);

    normal.normal_x = normal_x * scale;
    normal.normal_y = normal_y * scale;
    normal.normal_z = normal_z * scale;
    normal.curvature = bad_point;

    return;
  }
  else if (normal_estimation_method_ == SIMPLE_3D_GRADIENT)
  {
    if (!init_simple_3d_gradient_)
      initSimple3DGradientMethod ();

    // this method does not work if lots of NaNs are in the neighborhood of the point
    Eigen::Vector3d gradient_x = integral_image_XYZ_.getFirstOrderSum (pos_x + rect_width_2_, pos_y - rect_height_2_, 1, rect_height_) -
                                 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, 1, rect_height_);

    Eigen::Vector3d gradient_y = integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y + rect_height_2_, rect_width_, 1) -
                                 integral_image_XYZ_.getFirstOrderSum (pos_x - rect_width_2_, pos_y - rect_height_2_, rect_width_, 1);
    Eigen::Vector3d normal_vector = gradient_y.cross (gradient_x);
    double normal_length = normal_vector.squaredNorm ();
    if (normal_length == 0.0f)
    {
      normal.getNormalVector4fMap ().setConstant (bad_point);
      normal.curvature = bad_point;
      return;
    }

    normal_vector /= sqrt (normal_length);

    float nx = static_cast<float> (normal_vector [0]);
    float ny = static_cast<float> (normal_vector [1]);
    float nz = static_cast<float> (normal_vector [2]);

    //pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, normal_vector);
    pcl::flipNormalTowardsViewpoint (input_->points[point_index], vpx_, vpy_, vpz_, nx, ny, nz);
    
    normal.normal_x = nx;
    normal.normal_y = ny;
    normal.normal_z = nz;
    normal.curvature = bad_point;
    return;
  }

  normal.getNormalVector4fMap ().setConstant (bad_point);
  normal.curvature = bad_point;
  return;
}
Beispiel #18
0
int
 main (int argc, char** argv)
{
    // Get input object and scene
    if (argc < 2)
    {
        pcl::console::print_error ("Syntax is: %s cloud1.pcd (cloud2.pcd)\n", argv[0]);
        return (1);
    }
    
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGBA>);

    // Load object and scene
    pcl::console::print_highlight ("Loading point clouds...\n");
    if(argc<3)
    {
        if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0)
            pcl::console::print_error ("Error loading first file!\n");
        *cloud_out = *cloud_in;
        
        //transform cloud
        Eigen::Affine3f transformation;
        transformation.setIdentity();
        transformation.translate(Eigen::Vector3f(0.3,0.02,-0.1));
        float roll, pitch, yaw;
        roll = 0.02; pitch = 1.2; yaw = 0;
        Eigen::AngleAxisf rollAngle(roll, Eigen::Vector3f::UnitX());
        Eigen::AngleAxisf pitchAngle(pitch, Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf yawAngle(yaw, Eigen::Vector3f::UnitZ());
        Eigen::Quaternion<float> q = rollAngle*pitchAngle*yawAngle;
        transformation.rotate(q);
        
        pcl::transformPointCloud<pcl::PointXYZRGBA>(*cloud_in, *cloud_out, transformation);
        std::cout << "Transformed " << cloud_in->points.size () << " data points:"
            << std::endl;
    }else{
       if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[1], *cloud_in) < 0 ||
        pcl::io::loadPCDFile<pcl::PointXYZRGBA> (argv[2], *cloud_out) < 0)
        {
            pcl::console::print_error ("Error loading files!\n");
            return (1);
        } 
    }
    
    // Fill in the CloudIn data
//     cloud_in->width    = 100;
//     cloud_in->height   = 1;
//     cloud_in->is_dense = false;
//     cloud_in->points.resize (cloud_in->width * cloud_in->height);
//     for (size_t i = 0; i < cloud_in->points.size (); ++i)
//     {
//         cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
//         cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
//         cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
//     }

    std::cout << "size:" << cloud_out->points.size() << std::endl;
      
    {
        pcl::ScopeTime("icp proces");
        
        pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
        icp.setInputSource(cloud_in);
        icp.setInputTarget(cloud_out);
        pcl::PointCloud<pcl::PointXYZRGBA> Final;
        icp.setMaximumIterations(1000000);
        icp.setRANSACOutlierRejectionThreshold(0.01);
        icp.align(Final);
        std::cout << "has converged:" << icp.hasConverged() << " score: " <<
        icp.getFitnessScore() << std::endl;
        std::cout << icp.getFinalTransformation() << std::endl;
        
        //translation, rotation
        Eigen::Matrix4f icp_transformation=icp.getFinalTransformation();
        Eigen::Matrix3f icp_rotation = icp_transformation.block<3,3>(0,0);
        Eigen::Vector3f euler = icp_rotation.eulerAngles(0,1,2);
        std::cout << "rotation: " << euler.transpose() << std::endl;
        std::cout << "translation:" << icp_transformation.block<3,1>(0,3).transpose() << std::endl;
    }
  

 return (0);
}
template<typename Point> void
TableObjectCluster<Point>::calculateBoundingBox(
  const PointCloudPtr& cloud,
  const pcl::PointIndices& indices,
  const Eigen::Vector3f& plane_normal,
  const Eigen::Vector3f& plane_point,
  Eigen::Vector3f& position,
  Eigen::Quaternion<float>& orientation,
  Eigen::Vector3f& size)
{
  // transform to table coordinate frame and project points on X-Y, save max height
  Eigen::Affine3f tf;
  pcl::getTransformationFromTwoUnitVectorsAndOrigin(plane_normal.unitOrthogonal(), plane_normal, plane_point, tf);
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc2d(new pcl::PointCloud<pcl::PointXYZ>);
  float height = 0.0;
  for(std::vector<int>::const_iterator it=indices.indices.begin(); it != indices.indices.end(); ++it)
  {
    Eigen::Vector3f tmp = tf * (*cloud)[*it].getVector3fMap();
    height = std::max<float>(height, fabs(tmp(2)));
    pc2d->push_back(pcl::PointXYZ(tmp(0),tmp(1),0.0));
  }

  // create convex hull of projected points
  #ifdef PCL_MINOR_VERSION >= 6
  pcl::PointCloud<pcl::PointXYZ>::Ptr conv_hull(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ConvexHull<pcl::PointXYZ> chull;
  chull.setDimension(2);
  chull.setInputCloud(pc2d);
  chull.reconstruct(*conv_hull);
  #endif

  /*for(int i=0; i<conv_hull->size(); ++i)
    std::cout << (*conv_hull)[i].x << "," << (*conv_hull)[i].y << std::endl;*/

  // find the minimal bounding rectangle in 2D and table coordinates
  Eigen::Vector2f p1, p2, p3;
  cob_3d_mapping::MinimalRectangle2D mr2d;
  mr2d.setConvexHull(conv_hull->points);
  mr2d.rotatingCalipers(p2, p1, p3);
  /*std::cout << "BB: \n" << p1[0] << "," << p1[1] <<"\n"
            << p2[0] << "," << p2[1] <<"\n"
            << p3[0] << "," << p3[1] <<"\n ---" << std::endl;*/

  // compute center of rectangle
  position[0] = 0.5f*(p1[0] + p3[0]);
  position[1] = 0.5f*(p1[1] + p3[1]);
  position[2] = 0.0f;
  // transform back
  Eigen::Affine3f inv_tf = tf.inverse();
  position = inv_tf * position;
  // set size of bounding box
  float norm_1 = (p3-p2).norm();
  float norm_2 = (p1-p2).norm();
  // BoundingBox coordinates: X:= main direction, Z:= table normal
  Eigen::Vector3f direction; // y direction
  if (norm_1 < norm_2)
  {
    direction = Eigen::Vector3f(p3[0]-p2[0], p3[1]-p2[1], 0) / (norm_1);
    size[0] = norm_2 * 0.5f;
    size[1] = norm_1 * 0.5f;
  }
  else
  {
    direction = Eigen::Vector3f(p1[0]-p2[0], p1[1]-p2[1], 0) / (norm_2);
    size[0] = norm_1 * 0.5f;
    size[1] = norm_2 * 0.5f;
  }
  size[2] = -height;


  direction = inv_tf.rotation() * direction;
  orientation = pcl::getTransformationFromTwoUnitVectors(direction, plane_normal).rotation(); // (y, z-direction)

  return;
  Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - plane_normal * plane_normal.transpose();
  Eigen::Vector3f xn = M * Eigen::Vector3f::UnitX(); // X-axis project on normal
  Eigen::Vector3f xxn = M * direction;
  float cos_phi = acos(xn.normalized().dot(xxn.normalized())); // angle between xn and main direction
  cos_phi = cos(0.5f * cos_phi);
  float sin_phi = sqrt(1.0f-cos_phi*cos_phi);
  //orientation.w() = cos_phi;
  //orientation.x() = sin_phi * plane_normal(0);
  //orientation.y() = sin_phi * plane_normal(1);
  //orientation.z() = sin_phi * plane_normal(2);
}
bool Pcl_grabing::isBlock()
{
    update_kinect_points();
    int npts = transformed_pclKinect_clr_ptr_->points.size();
    Eigen::Vector3f pt;
    Eigen::Vector3f dist;
    vector<int> index;
    index.clear();
    double distance = 1;
    BlockColor<<0,0,0;
    for (int i = 0; i < npts; i++) 
    {
        pt = transformed_pclKinect_clr_ptr_->points[i].getVector3fMap();
        dist = pt - TableCentroid;
        dist[2]=0;
        distance = dist.norm();
        if(distance < TableRadius)
            if(pt[2]>(TableHeight+0.003) && pt[2]<BlockMaxHeight)
            {
                index.push_back(i);
                BlockColor(0)+=transformed_pclKinect_clr_ptr_->points[i].r;
                BlockColor(1)+=transformed_pclKinect_clr_ptr_->points[i].g;
                BlockColor(2)+=transformed_pclKinect_clr_ptr_->points[i].b;
            }
    }
    int n_block_points = index.size();
    if(n_block_points<10)
    {
        ROS_INFO("There is no block on the stool.");
        return 0;
    }
    ROS_INFO("There is a block with %d points", n_block_points);
    BlockColor/=n_block_points;
    ROS_INFO_STREAM("The block color:"<<BlockColor.transpose());
    
    display_ptr_->header = transformed_pclKinect_clr_ptr_->header;
    display_ptr_->is_dense = transformed_pclKinect_clr_ptr_->is_dense;
    display_ptr_->width = n_block_points; //transformed_pclKinect_clr_ptr_->width;
    display_ptr_->height = transformed_pclKinect_clr_ptr_->height;
    display_ptr_->points.resize(n_block_points);
    for (int i = 0; i < n_block_points; i++) 
    {
        display_ptr_->points[i].getVector3fMap() = transformed_pclKinect_clr_ptr_->points[index[i]].getVector3fMap();
    }
    display_points(*display_ptr_);
    
    Eigen::Vector3f BlockCentroid;
    BlockCentroid =pcl_wsn.compute_centroid(display_ptr_);
    ROS_INFO_STREAM("The centroid of the block:"<<BlockCentroid.transpose());


    vector<int> block_index;
    block_index.clear();
    for (int i = 0; i < n_block_points; i++) 
    {
        pt=display_ptr_->points[i].getVector3fMap();
        dist = pt - BlockCentroid;
        dist[2]=0;
        distance = dist.norm();
        if(distance < BlockTopRadius)
        {
            block_index.push_back(i);
        }
    }
    int n_block_top = block_index.size();
    ROS_INFO("There are %d points around the block's top center",n_block_top);
    pcl::PointCloud<pcl::PointXYZ>::Ptr block_ptr_(new PointCloud<pcl::PointXYZ>);
    block_ptr_->header=display_ptr_->header;
    block_ptr_->is_dense=display_ptr_->is_dense;
    block_ptr_->width=n_block_top;
    block_ptr_->height=display_ptr_->height;
    block_ptr_->points.resize(n_block_top);   
    for (int i = 0; i < n_block_top; i++) 
    {
        block_ptr_->points[i].getVector3fMap()=display_ptr_->points[block_index[i]].getVector3fMap();
    }

    BlockTopCentroid = pcl_wsn.compute_centroid(block_ptr_);
    ROS_INFO_STREAM("The centroid of the block's top:"<<BlockTopCentroid.transpose());
    //display_points(*block_ptr_);


    block_index.clear();
    for(int i = 0; i < n_block_points; i++)
    {
        pt=display_ptr_->points[i].getVector3fMap();
        if(abs(pt[2]-BlockTopCentroid[2])<0.002)
        {
            block_index.push_back(i);
        }
    }
    n_block_top = block_index.size();
    block_ptr_->header=display_ptr_->header;
    block_ptr_->is_dense=display_ptr_->is_dense;
    block_ptr_->width=n_block_top;
    block_ptr_->height=display_ptr_->height;
    block_ptr_->points.resize(n_block_top);   
    for (int i = 0; i < n_block_top; i++) 
    {
        block_ptr_->points[i].getVector3fMap()=display_ptr_->points[block_index[i]].getVector3fMap();
    }
    
    double block_dist;
    pcl_wsn.fit_points_to_plane(block_ptr_,Block_Normal,block_dist);
    Block_Major = pcl_wsn.get_major_axis();
    ROS_INFO_STREAM("The major vector of the block's top:"<<Block_Major.transpose());
    //display_points(*block_ptr_);

    return true;
}
Eigen::Matrix3f Homography::calcHomography( const Eigen::Matrix3f &R, const Eigen::Vector3f& t, const Eigen::Vector3f& n, float d ) const
{
    Eigen::Matrix3f A = R + (t*n.transpose()) / d;
    return A;
}