C++ Matrix3f::row方法代码示例

visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}

template <typename PointInT, typename PointOutT> void
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
  //check whether used with search radius or search k-neighbors
  if (this->getKSearch () != 0)
  {
    PCL_ERROR(
      "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
      getClassName().c_str ());
    return;
  }
  tree_->setSortedResults (true);

  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // point result
    Eigen::Matrix3f rf;
    PointOutT& output_rf = output[i];

    //output_rf.confidence = getLocalRF ((*indices_)[i], rf);
    //if (output_rf.confidence == std::numeric_limits<float>::max ())
    if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
    {
      output.is_dense = false;
    }

    output_rf.x_axis.getNormalVector3fMap () = rf.row (0);
    output_rf.y_axis.getNormalVector3fMap () = rf.row (1);
    output_rf.z_axis.getNormalVector3fMap () = rf.row (2);
  }

}

template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  //check whether used with search radius or search k-neighbors
  if (this->getKSearch () != 0)
  {
    PCL_ERROR(
        "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
        getClassName().c_str());
    return;
  }

  this->resetData ();
  for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
  {
    Eigen::Matrix3f currentLrf;
    PointOutT &rf = output[point_idx];

    //rf.confidence = computePointLRF (*indices_[point_idx], currentLrf);
    //if (rf.confidence == std::numeric_limits<float>::max ())
    if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
    {
      output.is_dense = false;
    }

    rf.x_axis.getNormalVector3fMap () = currentLrf.row (0);
    rf.y_axis.getNormalVector3fMap () = currentLrf.row (1);
    rf.z_axis.getNormalVector3fMap () = currentLrf.row (2);
  }
}

void
pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
  if (threads_ < 0)
     threads_ = 1;

  //check whether used with search radius or search k-neighbors
  if (this->getKSearch () != 0)
  {
    PCL_ERROR(
        "[pcl::%s::computeFeatureEigen] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
        getClassName().c_str ());
    return;
  }
  tree_->setSortedResults (true);

  int data_size = static_cast<int> (indices_->size ());

  // Set up the output channels
  output.channels["shot_lrf"].name = "shot_lrf";
  output.channels["shot_lrf"].offset = 0;
  output.channels["shot_lrf"].size = 4;
  output.channels["shot_lrf"].count = 9;
  output.channels["shot_lrf"].datatype = sensor_msgs::PointField::FLOAT32;

  //output.points.resize (indices_->size (), 10);
  output.points.resize (data_size, 9);

#pragma omp parallel for num_threads(threads_)
  for (int i = 0; i < data_size; ++i)
  {
    // point result
    Eigen::Matrix3f rf;

    //output.points (i, 9) = getLocalRF ((*indices_)[i], rf);
    //if (output.points (i, 9) == std::numeric_limits<float>::max ())
    if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
    {
      output.is_dense = false;
    }

    output.points.block<1, 3> (i, 0) = rf.row (0);
    output.points.block<1, 3> (i, 3) = rf.row (1);
    output.points.block<1, 3> (i, 6) = rf.row (2);
  }

}

template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
    const pcl::PointCloud<PointT> &cloud_src, 
    const std::vector<int> &indices_src, 
    const pcl::PointCloud<PointT> &cloud_tgt,
    const std::vector<int> &indices_tgt, 
    Eigen::VectorXf &transform)
{
  transform.resize (16);
  Eigen::Vector4f centroid_src, centroid_tgt;
  // Estimate the centroids of source, target
  compute3DCentroid (cloud_src, indices_src, centroid_src);
  compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);

  // Subtract the centroids from source, target
  Eigen::MatrixXf cloud_src_demean;
  demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);

  Eigen::MatrixXf cloud_tgt_demean;
  demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix3f R = v * u.transpose ();

  // Return the correct transformation
  transform.segment<3> (0) = R.row (0); transform[12]  = 0;
  transform.segment<3> (4) = R.row (1); transform[13]  = 0;
  transform.segment<3> (8) = R.row (2); transform[14] = 0;

  Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
  transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
}

bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
                 Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
                 bool& oIsOrthographic) {
  oIsOrthographic = isOrthographic(iMatrix.matrix());

  // get appropriate rows
  std::vector<int> rows = {0,1,2};
  if (!oIsOrthographic) rows[2] = 3;

  // get A matrix (upper left 3x3) and t vector
  Eigen::Matrix3f A;
  Eigen::Vector3f t;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      A(i,j) = iMatrix(rows[i],j);
    }
    t[i] = iMatrix(rows[i],3);
  }

  // determine translation vector
  oPose.setIdentity();
  oPose.translation() = -(A.inverse()*t);

  // determine calibration matrix
  Eigen::Matrix3f AAtrans = A*A.transpose();
  AAtrans.col(0).swap(AAtrans.col(2));
  AAtrans.row(0).swap(AAtrans.row(2));
  Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
  oCalib = llt.matrixU();
  oCalib.col(0).swap(oCalib.col(2));
  oCalib.row(0).swap(oCalib.row(2));
  oCalib.transposeInPlace();

  // compute rotation matrix
  oPose.linear() = (oCalib.inverse()*A).transpose();

  return true;
}

int main( )
  {
  Eigen::VectorXf a(3);
  Eigen::VectorXf b(a);
  Eigen::VectorXf c(b);

  a = Eigen::Vector4f(1, 2, 3, 4);
  b = Eigen::Vector4f(3, 2, 1, 0);

  c = a + b;

  Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> x(1,3);
  Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> y(3,1);

  x = y;

  x.resize(4,4);
  x.resize(2,3);

  y = x;

  y = a;

  Eigen::Vector3f d = a.block<3, 1>(0, 0);

  c = a;

  Eigen::Vector3f e = a.block<3, 1>(0, 0);

  Eigen::ArrayXf ddd = c;

  ddd = a;

  Eigen::Matrix3f f;

  f.row(0) = e;

  Eigen::Affine3f m;


  m.matrix().row(0).head<3>() = e;

  return EXIT_SUCCESS;
  }

osgpcl::PointCloudGeometry* osgpcl::SurfelFactoryFF<PointT, NormalT, RadiusT>::buildGeometry(
        bool unique_state)   {

    typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>();
    typename pcl::PointCloud<PointT>::ConstPtr  xyz = this->getInputCloud<PointT>();
    typename pcl::PointCloud<RadiusT>::ConstPtr rads = this->getInputCloud<RadiusT>();

        if (xyz ==NULL) return NULL;
        if (normals ==NULL) return NULL;
        if (rads ==NULL) return NULL;

        if (xyz->points.size() != normals->points.size()) return NULL;
        if (rads->points.size() != normals->points.size()) return NULL;

          pcl::IndicesConstPtr indices = indices_;
        {
            bool rebuild_indices= false;
        if (indices_ == NULL) rebuild_indices=true;
        else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true;
        if (rebuild_indices){
            pcl::IndicesPtr idxs(new std::vector<int>);
            idxs->reserve(xyz->points.size());
            for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i);
            indices= idxs;
        }
        }

        osg::Vec3Array* pts = new osg::Vec3Array;
        osg::Vec3Array* npts = new osg::Vec3Array;
        int fan_size = 1+ circle_cache.rows();
        pts->reserve(indices->size()*fan_size );
        npts->reserve(indices->size()*fan_size);

        osg::Geometry* geom = new osg::Geometry;

        for(int i=0, pstart=0; i<indices->size(); i++){
            const int& idx = (*indices)[i];
          const PointT& pt =  xyz->points[idx];
          const NormalT& npt = normals->points[idx];
          pts->push_back(osg::Vec3(pt.x, pt.y, pt.z));
          npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));

          pcl::Normal nt;

          Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
          rot.row(2) << npt.getNormalVector3fMap().transpose();
          Eigen::Vector3f ax2(1,0,0);
          if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
              ax2 << 0,1,0;
              if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
                  ax2 << 0,0,1;
              }
          }
          rot.row(1) << ax2.cross(npt.getNormalVector3fMap()).normalized().transpose();
          rot.row(0) = ( ax2 - ax2.dot(npt.getNormalVector3fMap() )*npt.getNormalVector3fMap() ).normalized();
          rot = rot*rads->points[idx].radius;

          for(int j=0; j<circle_cache.rows(); j++){
              Eigen::Vector3f apt = rot*circle_cache.row(j).transpose() + pt.getVector3fMap();
              pts->push_back(osg::Vec3(apt[0],apt[1],apt[2]));
              npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
          }
            geom->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, pstart,  fan_size ) );
            pstart+= fan_size;
        }
        geom->setVertexArray( pts );
        geom->setNormalArray(npts);
        geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );

        geom->setStateSet(stateset_);
        return geom;
}

///
/// @par Detailed description 
/// ... 
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
Eigen::Matrix3f
sasmath::Math::
find_u(sasmol::SasMol &mol, int &frame)
{

    Eigen::Matrix3f r ;

    float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ; 
    float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ;
    float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ;
    
    float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ;
    float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ;
    float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ;

    float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ;
    float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ;
    float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ;

    r << rxx,rxy,rxz,
         ryx,ryy,ryz,
         rzx,rzy,rzz;

    Eigen::Matrix3f rtr = r.transpose() * r ;
    
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr);

    if (eigensolver.info() != Eigen::Success)
    {
          std::cout << "find_u calculation failed" << std::endl ;
    }

    Eigen::Matrix<float,3,1> uk ;  // eigenvalues
    Eigen::Matrix3f ak ;          // eigenvectors

    uk = eigensolver.eigenvalues() ; 
    ak = eigensolver.eigenvectors() ;

    Eigen::Matrix3f akt = ak.transpose() ;
    Eigen::Matrix3f new_ak  ;

    new_ak.row(0) = akt.row(2) ; //sort eigenvectors
    new_ak.row(1) = akt.row(1) ;
    new_ak.row(2) = akt.row(0) ;

    // python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0]

    //Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ;

    Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ;
    Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ;

    Eigen::Matrix<float,3,1> urak0 ; 
    if(uk[2] == 0.0)
    { 
        urak0 = 1E15 * rak0 ;
    } 
    else
    {    
        urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ;
    }
    
    Eigen::Matrix<float,3,1> urak1 ; 
    if(uk[1] == 0.0)
    { 
        urak1 = 1E15 * rak1 ;
    } 
    else
    {    
        urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ;
    }

    Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ;
    Eigen::Matrix3f bk ;

    bk << urak0,urak1,urak2;

    Eigen::Matrix3f u ;

    u =  (bk * new_ak).transpose() ;

    return u ;
/*
u = array([[-0.94513198,  0.31068658,  0.100992  ],
       [-0.3006246 , -0.70612572, -0.64110165],
       [-0.12786863, -0.63628635,  0.76078203]])
check:

u = 
-0.945133  0.310686  0.100992
-0.300624 -0.706126 -0.641102
-0.127869 -0.636287  0.760782 
*/
}

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

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

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

void SensorFusion::normalize()
{
	float error = 0;
	Eigen::Matrix3f temporary;
	float renorm = 0;

	error = -dcmMatrix.row(0).dot(dcmMatrix.row(1)) * 0.5f; //eq.19

	temporary.row(0) = dcmMatrix.row(1) * error;
	temporary.row(1) = dcmMatrix.row(0) * error;

	temporary.row(0) += dcmMatrix.row(0);
	temporary.row(1) += dcmMatrix.row(1);

	temporary.row(2) = temporary.row(0).cross(temporary.row(1)); // c= a x b //eq.20

	renorm = 0.5f *(3.0f - temporary.row(0).dot(temporary.row(0))); //eq.21
	dcmMatrix.row(0) = temporary.row(0) * renorm;

	renorm = 0.5f *(3.0f - temporary.row(1).dot(temporary.row(1))); //eq.21
	dcmMatrix.row(1) = temporary.row(1) * renorm;

	renorm = 0.5f *(3.0f - temporary.row(2).dot(temporary.row(2))); //eq.21
	dcmMatrix.row(2) = temporary.row(2) * renorm;
}

  void get3DMoments(vector<Point> feature_points, int feat_index, int index)
  {
    //    for(int i=0; i<feature_points.size(); i++)
    //   ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
    //ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
    
    //Extract the indices for the points in the point cloud data
    pcl::PointIndices point_indices;
     
    for(int i=0; i<feature_points.size(); i++)
      {
	//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
	point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
      }
    
    //ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
    Eigen::Vector4f centroid;
    Eigen::Matrix3f covariance_matrix;
    
    // Estimate the XYZ centroid
    pcl::compute3DCentroid (pcl_in, point_indices, centroid); 
#ifdef DEBUG
    ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif

    //ROS_INFO("Computing Covariance ");
    //Compute the centroid and the covariance of the points
    computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
    
    //Print the 3D Moments
    //ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
    std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif

    for(int i=0; i<3; i++)
      {
	feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
	for(int j=0; j<3; j++)
	  {
	    feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
	  }
      }

    //Get the principal components and the quaternion
    Eigen::Matrix3f evecs;
    Eigen::Vector3f evals;
    pcl::eigen33 (covariance_matrix, evecs, evals);
    
    Eigen::Matrix3f rotation;
    rotation.row (0) = evecs.col (0);
    rotation.row (1) = evecs.col (1);
    //rotation.row (2) = evecs.col (2);
    rotation.row (2) = rotation.row (0).cross (rotation.row (1));
    //rotation.transposeInPlace ();
#ifdef DEBUG
    std::cerr << "Rotation matrix: " << endl;
    std::cerr << rotation << endl;
    std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif

    rotation.transposeInPlace ();
    Eigen::Quaternion<float> qt (rotation);
    qt.normalize ();

    //Publish Marker
    visualization_msgs::Marker marker;	
    
    marker.header.frame_id = "/openni_rgb_optical_frame";
    marker.header.stamp = ros::Time().now();
    marker.ns = "Triangulation";
    marker.id = index+1;	
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(5);		
    
    //centroid position
    marker.type = visualization_msgs::Marker::SPHERE;
    
    
    marker.pose.position.x = centroid(0);
    marker.pose.position.y = centroid(1);
    marker.pose.position.z = centroid(2);	
    marker.pose.orientation.x = qt.x();
    marker.pose.orientation.y = qt.y();
    marker.pose.orientation.z = qt.z();
    marker.pose.orientation.w = qt.w();			
    
    marker.scale.x = sqrt(evals(0)) *2;
    marker.scale.y =  sqrt(evals(1)) *2;
    marker.scale.z =  sqrt(evals(2)) *2;
    
    //red
    marker.color.a = 0.5;
    marker.color.r = rand()/((double)RAND_MAX + 1);
    marker.color.g = rand()/((double)RAND_MAX + 1);
    marker.color.b = rand()/((double)RAND_MAX + 1);
    object_pose_marker_pub_.publish(marker);	
    
  }