C++ Vector3f::transpose方法代码示例

/**
 * @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

}

  /** \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();
}

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;
}

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;
}

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 ();
}

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;
	
}

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;
}

    });


    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 {

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;
}

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;
//.........这里部分代码省略.........

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;
}