C++ eigen::VectorXf类代码示例

 inline vcl_size_t size(Eigen::VectorXf const & v) { return v.rows(); }

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
      const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
      const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
{
  int h_index, h_p;

  // Clear the resultant point histogram
  pfh_histogram.setZero ();

  // Factorization constant
  float hist_incr = 100.0f / static_cast<float> (indices.size () * (indices.size () - 1) / 2);

  std::pair<int, int> key;
  // Iterate over all the points in the neighborhood
  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    for (size_t j_idx = 0; j_idx < i_idx; ++j_idx)
    {
      // If the 3D points are invalid, don't bother estimating, just continue
      if (!isFinite (cloud.points[indices[i_idx]]) || !isFinite (cloud.points[indices[j_idx]]))
        continue;

      if (use_cache_)
      {
        // In order to create the key, always use the smaller index as the first key pair member
        int p1, p2;
  //      if (indices[i_idx] >= indices[j_idx])
  //      {
          p1 = indices[i_idx];
          p2 = indices[j_idx];
  //      }
  //      else
  //      {
  //        p1 = indices[j_idx];
  //        p2 = indices[i_idx];
  //      }
        key = std::pair<int, int> (p1, p2);

        // Check to see if we already estimated this pair in the global hashmap
        std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
        if (fm_it != feature_map_.end ())
          pfh_tuple_ = fm_it->second;
        else
        {
          // Compute the pair NNi to NNj
          if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
                                    pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
            continue;
        }
      }
      else
        if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
                                  pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
          continue;

      // Normalize the f1, f2, f3 features and push them in the histogram
      f_index_[0] = static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
      if (f_index_[0] < 0)         f_index_[0] = 0;
      if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;

      f_index_[1] = static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
      if (f_index_[1] < 0)         f_index_[1] = 0;
      if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;

      f_index_[2] = static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
      if (f_index_[2] < 0)         f_index_[2] = 0;
      if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;

      // Copy into the histogram
      h_index = 0;
      h_p     = 1;
      for (int d = 0; d < 3; ++d)
      {
        h_index += h_p * f_index_[d];
        h_p     *= nr_split;
      }
      pfh_histogram[h_index] += hist_incr;

      if (use_cache_)
      {
        // Save the value in the hashmap
        feature_map_[key] = pfh_tuple_;

        // Use a maximum cache so that we don't go overboard on RAM usage
        key_list_.push (key);
        // Check to see if we need to remove an element due to exceeding max_size
        if (key_list_.size () > max_cache_size_)
        {
          // Remove the last element.
          feature_map_.erase (key_list_.back ());
          key_list_.pop ();
        }
      }
    }
  }
}

template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
    const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 3 samples
  if (samples.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
    return (false);
  }

  if (!normals_)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n");
    return (false);
  }

  Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
  Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
  Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0);

  Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
  Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
  Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0);

  //calculate apex (intersection of the three planes defined by points and belonging normals
  Eigen::Vector4f ortho12 = n1.cross3(n2);
  Eigen::Vector4f ortho23 = n2.cross3(n3);
  Eigen::Vector4f ortho31 = n3.cross3(n1);

  float denominator = n1.dot(ortho23);

  float d1 = p1.dot (n1);
  float d2 = p2.dot (n2);
  float d3 = p3.dot (n3);

  Eigen::Vector4f apex = (d1 * ortho23 + d2 * ortho31 + d3 * ortho12) / denominator;

  //compute axis (normal of plane defined by: { apex+(p1-apex)/(||p1-apex||), apex+(p2-apex)/(||p2-apex||), apex+(p3-apex)/(||p3-apex||)}
  Eigen::Vector4f ap1 = p1 - apex;
  Eigen::Vector4f ap2 = p2 - apex;
  Eigen::Vector4f ap3 = p3 - apex;

  Eigen::Vector4f np1 = apex + (ap1/ap1.norm ());
  Eigen::Vector4f np2 = apex + (ap2/ap2.norm ());
  Eigen::Vector4f np3 = apex + (ap3/ap3.norm ());

  Eigen::Vector4f np1np2 = np2 - np1;
  Eigen::Vector4f np1np3 = np3 - np1;

  Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3);
  axis_dir.normalize ();

  // normalize the vector (apex->p) for opening angle calculation
  ap1.normalize ();
  ap2.normalize ();
  ap3.normalize ();

  //compute opening angle
  float opening_angle = ( acosf (ap1.dot (axis_dir)) + acosf (ap2.dot (axis_dir)) + acosf (ap3.dot (axis_dir)) ) / 3.0f;

  model_coefficients.resize (7);
  // model_coefficients.template head<3> ()    = line_pt.template head<3> ();
  model_coefficients[0] = apex[0];
  model_coefficients[1] = apex[1];
  model_coefficients[2] = apex[2];
  // model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
  model_coefficients[3] = axis_dir[0];
  model_coefficients[4] = axis_dir[1];
  model_coefficients[5] = axis_dir[2];
  // cone radius
  model_coefficients[6] = opening_angle;

  if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
    return (false);
  if (model_coefficients[6] !=  std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
    return (false);

  return (true);
}

TEST (RANSAC, SampleConsensusModelCircle3D)
{
  srand (0);

  // Use a custom point cloud for these tests until we need something better
  PointCloud<PointXYZ> cloud;
  cloud.points.resize (20);

  cloud.points[0].x = 1.0f;  	    cloud.points[0].y = 5.0f;        cloud.points[0].z = -2.9000001f;
  cloud.points[1].x = 1.034202f;    cloud.points[1].y = 5.0f;        cloud.points[1].z = -2.9060307f;
  cloud.points[2].x = 1.0642787f;   cloud.points[2].y = 5.0f;        cloud.points[2].z = -2.9233956f;
  cloud.points[3].x = 1.0866026f;   cloud.points[3].y = 5.0f;  	     cloud.points[3].z = -2.95f;
  cloud.points[4].x = 1.0984808f;   cloud.points[4].y = 5.0f;  	     cloud.points[4].z = -2.9826353f;
  cloud.points[5].x = 1.0984808f;   cloud.points[5].y = 5.0f;        cloud.points[5].z = -3.0173647f;
  cloud.points[6].x = 1.0866026f;   cloud.points[6].y = 5.0f;  	     cloud.points[6].z = -3.05f;
  cloud.points[7].x = 1.0642787f;   cloud.points[7].y = 5.0f;  	     cloud.points[7].z = -3.0766044f;
  cloud.points[8].x = 1.034202f;    cloud.points[8].y = 5.0f;  	     cloud.points[8].z = -3.0939693f;
  cloud.points[9].x = 1.0f;         cloud.points[9].y = 5.0f;  	     cloud.points[9].z = -3.0999999f;
  cloud.points[10].x = 0.96579796f; cloud.points[10].y = 5.0f; 	     cloud.points[10].z = -3.0939693f;
  cloud.points[11].x = 0.93572122f; cloud.points[11].y = 5.0f; 	     cloud.points[11].z = -3.0766044f;
  cloud.points[12].x = 0.91339743f; cloud.points[12].y = 5.0f; 	     cloud.points[12].z = -3.05f;
  cloud.points[13].x = 0.90151924f; cloud.points[13].y = 5.0f; 	     cloud.points[13].z = -3.0173647f;
  cloud.points[14].x = 0.90151924f; cloud.points[14].y = 5.0f; 	     cloud.points[14].z = -2.9826353f;
  cloud.points[15].x = 0.91339743f; cloud.points[15].y = 5.0f; 	     cloud.points[15].z = -2.95f;
  cloud.points[16].x = 0.93572122f; cloud.points[16].y = 5.0f; 	     cloud.points[16].z = -2.9233956f;
  cloud.points[17].x = 0.96579796f; cloud.points[17].y = 5.0;        cloud.points[17].z = -2.9060307f;
  cloud.points[18].x = 0.85000002f; cloud.points[18].y = 4.8499999f; cloud.points[18].z = -3.1500001f;
  cloud.points[19].x = 1.15f; 	    cloud.points[19].y = 5.1500001f; cloud.points[19].z = -2.8499999f;

  // Create a shared 3d circle model pointer directly
  SampleConsensusModelCircle3DPtr model (new SampleConsensusModelCircle3D<PointXYZ> (cloud.makeShared ()));

  // Create the RANSAC object
  RandomSampleConsensus<PointXYZ> sac (model, 0.03);

  // Algorithm tests
  bool result = sac.computeModel ();
  ASSERT_EQ (result, true);

  std::vector<int> sample;
  sac.getModel (sample);
  EXPECT_EQ (int (sample.size ()), 3);

  std::vector<int> inliers;
  sac.getInliers (inliers);
  EXPECT_EQ (int (inliers.size ()), 18);

  Eigen::VectorXf coeff;
  sac.getModelCoefficients (coeff);
  EXPECT_EQ (int (coeff.size ()), 7);
  EXPECT_NEAR (coeff[0],  1, 1e-3);
  EXPECT_NEAR (coeff[1],  5, 1e-3);
  EXPECT_NEAR (coeff[2], -3, 1e-3);
  EXPECT_NEAR (coeff[3],0.1, 1e-3);
  EXPECT_NEAR (coeff[4],  0, 1e-3);
  EXPECT_NEAR (coeff[5], -1, 1e-3);
  EXPECT_NEAR (coeff[6],  0, 1e-3);

  Eigen::VectorXf coeff_refined;
  model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
  EXPECT_EQ (int (coeff_refined.size ()), 7);
  EXPECT_NEAR (coeff_refined[0],  1, 1e-3);
  EXPECT_NEAR (coeff_refined[1],  5, 1e-3);
  EXPECT_NEAR (coeff_refined[2], -3, 1e-3);
  EXPECT_NEAR (coeff_refined[3],0.1, 1e-3);
  EXPECT_NEAR (coeff_refined[4],  0, 1e-3);
  EXPECT_NEAR (coeff_refined[5], -1, 1e-3);
  EXPECT_NEAR (coeff_refined[6],  0, 1e-3);
}

	Eigen::MatrixXf featureGradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
		if (ntype_ == NO_NORMALIZATION )
			return kernelGradient( a, b );
		else if (ntype_ == NORMALIZE_SYMMETRIC ) {
			Eigen::MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
			Eigen::MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
		}
		else if (ntype_ == NORMALIZE_AFTER ) {
			Eigen::MatrixXf fb = lattice_.compute( b );
			
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm2 = norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
			return - r + kernelGradient( a*norm_.asDiagonal(), b );
		}
		else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
			Eigen::MatrixXf fa = lattice_.compute( a, true );
			
			Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
			Eigen::VectorXf norm2 = norm_.array()*norm_.array();
			Eigen::MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
			return -r+kernelGradient( a, b*norm_.asDiagonal() );
		}
	}

bool
ObjectModelPlaneFromLines::computeModelCoefficients (const std::vector<int> &sample,
		Eigen::VectorXf &modelCoefficients){

	line line1,line2;

	//Create line1 from sample[0] and sample[1]
	line1.pbase = (*inputPointCloud->getPointCloud())[sample[0]];
	line1.director.resize(3);
	line1.director[0]=(*inputPointCloud->getPointCloud())[sample[1]].getX() - (*inputPointCloud->getPointCloud())[sample[0]].getX();
	line1.director[1]=(*inputPointCloud->getPointCloud())[sample[1]].getY() - (*inputPointCloud->getPointCloud())[sample[0]].getY();
	line1.director[2]=(*inputPointCloud->getPointCloud())[sample[1]].getZ() - (*inputPointCloud->getPointCloud())[sample[0]].getZ();

	//Create line1 from sample[2] and sample[3]
	line2.pbase = (*inputPointCloud->getPointCloud())[sample[2]];
	line2.director.resize(3);
	line2.director[0]=(*inputPointCloud->getPointCloud())[sample[3]].getX() - (*inputPointCloud->getPointCloud())[sample[2]].getX();
	line2.director[1]=(*inputPointCloud->getPointCloud())[sample[3]].getY() - (*inputPointCloud->getPointCloud())[sample[2]].getY();
	line2.director[2]=(*inputPointCloud->getPointCloud())[sample[3]].getZ() - (*inputPointCloud->getPointCloud())[sample[2]].getZ();



	//Compute the model co-efficient of the lines from these lines
	std::vector<double> normal;
	normal.resize(3);

	crossProduct3D(line1.director,line2.director,normal);

	modelCoefficients.resize(4);

	modelCoefficients[0] = normal[0];
	modelCoefficients[1] = normal[1];
	modelCoefficients[2] = normal[2];

	modelCoefficients[3]= -modelCoefficients[0] * line1.pbase.getX() -
			modelCoefficients[1] * line1.pbase.getY() -
			modelCoefficients[2] * line1.pbase.getZ();


	if (abs(modelCoefficients[0])<geometryEpsilon&&abs(modelCoefficients[1])<geometryEpsilon&&abs(modelCoefficients[2])<geometryEpsilon)	{
		//Lines are parallel
		if (contains(line1, line2.pbase))
			return false;

		//Use a line's director vector and both pBase's difference to create the plane.
		std::vector<double> baseDifference;
		baseDifference.resize(3);
		baseDifference[0]=line1.pbase.getX()-line2.pbase.getX();
		baseDifference[1]=line1.pbase.getY()-line2.pbase.getY();
		baseDifference[2]=line1.pbase.getZ()-line2.pbase.getZ();

		crossProduct3D(line1.director,baseDifference,normal);
		modelCoefficients[0] = normal[0];
		modelCoefficients[1] = normal[1];
		modelCoefficients[2] = normal[2];

		modelCoefficients[3]=-modelCoefficients[0] * line1.pbase.getX() -modelCoefficients[1] * line1.pbase.getY() -modelCoefficients[2] * line1.pbase.getZ();

	}	else {

		double  x = modelCoefficients[0]*line2.pbase.getX()+
				modelCoefficients[1]*line2.pbase.getY()+
				modelCoefficients[2]*line2.pbase.getZ()+modelCoefficients[3];
		if (abs(x)>=geometryEpsilon) {
			cout<<"Lines do not intersect"<<endl;
			return false;
		}
	}

	return true;
}

int cPCA2::computePC(std::vector<float>& x,
                     size_t nrow,
                     size_t ncol,
                     bool is_center,
                     bool is_scale,
                     bool is_corr)
{
    _ncol     = ncol;
    _nrow     = nrow;
    _is_center = is_center;
    _is_scale  = is_scale;
    _is_corr   = is_corr;

    if (x.size() != _nrow*_ncol)     { return -1; }
    if ((1 == _ncol) || (1 == nrow)) { return -1; }

    // convert vector to Eigen 2-dimensional matrix
    _xXf.resize(_nrow, _ncol);

    for (size_t i = 0; i < _nrow; ++i) {
        for (size_t j = 0; j < _ncol; ++j) {
            _xXf(i, j) = x[j + i*_ncol];
        }
    }

    // mean and standard deviation for each column
    Eigen::VectorXf     mean_vector(_ncol),
                        sd_vector(_ncol);
    size_t              zero_sd_num = 0;
    float               denom = static_cast<float>((_nrow > 1) ? _nrow - 1 : 1);

    mean_vector = _xXf.colwise().mean();

    Eigen::VectorXf     curr_col;

    for (size_t i = 0; i < _ncol; ++i) {
        curr_col = Eigen::VectorXf::Constant(_nrow, mean_vector(i));    // mean(x) for column x
        curr_col = _xXf.col(i) - curr_col;                              // x - mean(x)
        curr_col = curr_col.array().square();                           // (x-mean(x))^2

        sd_vector(i) = std::sqrt((curr_col.sum())/denom);

        if (0 == sd_vector(i)) {
            zero_sd_num++;
        }
    }

    // if colums with sd == 0 are too many, don't continue calculation
    if (1 > _ncol-zero_sd_num) {
        return -1;
    }

    // delete columns with sd == 0
    Eigen::MatrixXf     tmp(_nrow, _ncol-zero_sd_num);
    Eigen::VectorXf     tmp_mean_vector(_ncol-zero_sd_num);

    size_t              curr_col_num = 0;

    for (size_t i = 0; i < _ncol; ++i) {
        if (0 != sd_vector(i)) {
            tmp.col(curr_col_num) = _xXf.col(i);
            tmp_mean_vector(curr_col_num) = mean_vector(i);
            curr_col_num++;
        }
        else {
            _eliminated_columns.push_back(i);
        }
    }

    _ncol      -= zero_sd_num;
    _xXf        = tmp;
    mean_vector = tmp_mean_vector;
    tmp.resize(0, 0);
    tmp_mean_vector.resize(0);

    // shift to zero
    if (true == _is_center) {
        for (size_t i = 0; i < _ncol; ++i) {
            _xXf.col(i) -= Eigen::VectorXf::Constant(_nrow, mean_vector(i));
        }
    }

    // scale to unit variance
    if ( (false == _is_corr) || (true == _is_scale)) {
        for (size_t i = 0; i < _ncol; ++i) {
            _xXf.col(i) /= std::sqrt(_xXf.col(i).array().square().sum()/denom);
        }
    }

#ifndef NDEBUG
    std::cout << "\nScaled matrix:\n";
    std::cout << _xXf << std::endl;
    std::cout << "\nMean before scaling:\n" << mean_vector.transpose();
    std::cout << "\nStandard deviation before scaling:\n" << sd_vector.transpose();
#endif

    // when _nrow < _ncol then svd will be used
    // if corr is true and _nrow > _ncol then correlation matrix will be used
    // (TODO): What about covariance?
    if ((_nrow < _ncol) || (false == _is_corr)) {
//.........这里部分代码省略.........

int main(int argc, char *argv[])
{
	if ( argc != 3 )
    {
        std::cout<<"usage: "<< argv[0] <<" <input file> <output file>\n";
        return 1;
    }
	
	std::ifstream infile(argv[1]);
	std::ofstream outfile(argv[2]);
	
	float poissonRatio, youngModulus;
	infile >> poissonRatio >> youngModulus;

	Eigen::Matrix3f D;
	D <<
		1.0f,        	poissonRatio,	0.0f,
		poissonRatio,	1.0,         	0.0f,
		0.0f,        	0.0f,        	(1.0f - poissonRatio) / 2.0f;

	D *= youngModulus / (1.0f - pow(poissonRatio, 2.0f));

	infile >> nodesCount;
	nodesX.resize(nodesCount);
	nodesY.resize(nodesCount);

	for (int i = 0; i < nodesCount; ++i)
	{
		infile >> nodesX[i] >> nodesY[i];
	}

	int elementCount;
	infile >> elementCount;

	for (int i = 0; i < elementCount; ++i)
	{
		Element element;
		infile >> element.nodesIds[0] >> element.nodesIds[1] >> element.nodesIds[2];
		elements.push_back(element);
	}

	int constraintCount;
	infile >> constraintCount;

	for (int i = 0; i < constraintCount; ++i)
	{
		Constraint constraint;
		int type;
		infile >> constraint.node >> type;
		constraint.type = static_cast<Constraint::Type>(type);
		constraints.push_back(constraint);
	}

	loads.resize(2 * nodesCount);
	loads.setZero();

	int loadsCount;
	infile >> loadsCount;

	for (int i = 0; i < loadsCount; ++i)
	{
		int node;
		float x, y;
		infile >> node >> x >> y;
		loads[2 * node + 0] = x;
		loads[2 * node + 1] = y;
	}
	
	std::vector<Eigen::Triplet<float> > triplets;
	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		it->CalculateStiffnessMatrix(D, triplets);
	}

	Eigen::SparseMatrix<float> globalK(2 * nodesCount, 2 * nodesCount);
	globalK.setFromTriplets(triplets.begin(), triplets.end());

	ApplyConstraints(globalK, constraints);

	Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > solver(globalK);

	Eigen::VectorXf displacements = solver.solve(loads);

	outfile << displacements << std::endl;

	for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
	{
		Eigen::Matrix<float, 6, 1> delta;
		delta << displacements.segment<2>(2 * it->nodesIds[0]),
		         displacements.segment<2>(2 * it->nodesIds[1]),
		         displacements.segment<2>(2 * it->nodesIds[2]);

		Eigen::Vector3f sigma = D * it->B * delta;
		float sigma_mises = sqrt(sigma[0] * sigma[0] - sigma[0] * sigma[1] + sigma[1] * sigma[1] + 3.0f * sigma[2] * sigma[2]);

		outfile << sigma_mises << std::endl;
	}
	return 0;
}

template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);

  // normalize the vector perpendicular to the plane...
  mc.normalize ();
  // ... and store the resulting normal as a local copy of the model coefficients
  Eigen::Vector4f tmp_mc = model_coefficients;
  tmp_mc[0] = mc[0];
  tmp_mc[1] = mc[1];
  tmp_mc[2] = mc[2];

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < input_->points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      // use normalized coefficients to calculate the scalar projection
      float distance_to_plane = tmp_mc.dot (p);

      pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // Calculate the distance from the point to the plane
      Eigen::Vector4f p (input_->points[inliers[i]].x,
                         input_->points[inliers[i]].y,
                         input_->points[inliers[i]].z,
                         1);
      // use normalized coefficients to calculate the scalar projection
      float distance_to_plane = tmp_mc.dot (p);

      pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
      pp = p - mc * distance_to_plane;        // mc[3] = 0, therefore the 3rd coordinate is safe
    }
  }
}

template <typename PointT> bool
pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 4 samples
  if (samples.size () != 4)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
    return (false);
  }

  Eigen::Matrix4f temp;
  for (int i = 0; i < 4; i++)
  {
    temp (i, 0) = input_->points[samples[i]].x;
    temp (i, 1) = input_->points[samples[i]].y;
    temp (i, 2) = input_->points[samples[i]].z;
    temp (i, 3) = 1;
  }
  float m11 = temp.determinant ();
  if (m11 == 0)
    return (false);             // the points don't define a sphere!

  for (int i = 0; i < 4; ++i)
    temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
                  (input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
                  (input_->points[samples[i]].z) * (input_->points[samples[i]].z);
  float m12 = temp.determinant ();

  for (int i = 0; i < 4; ++i)
  {
    temp (i, 1) = temp (i, 0);
    temp (i, 0) = input_->points[samples[i]].x;
  }
  float m13 = temp.determinant ();

  for (int i = 0; i < 4; ++i)
  {
    temp (i, 2) = temp (i, 1);
    temp (i, 1) = input_->points[samples[i]].y;
  }
  float m14 = temp.determinant ();

  for (int i = 0; i < 4; ++i)
  {
    temp (i, 0) = temp (i, 2);
    temp (i, 1) = input_->points[samples[i]].x;
    temp (i, 2) = input_->points[samples[i]].y;
    temp (i, 3) = input_->points[samples[i]].z;
  }
  float m15 = temp.determinant ();

  // Center (x , y, z)
  model_coefficients.resize (4);
  model_coefficients[0] = 0.5f * m12 / m11;
  model_coefficients[1] = 0.5f * m13 / m11;
  model_coefficients[2] = 0.5f * m14 / m11;
  // Radius
  model_coefficients[3] = sqrtf (
                                 model_coefficients[0] * model_coefficients[0] +
                                 model_coefficients[1] * model_coefficients[1] +
                                 model_coefficients[2] * model_coefficients[2] - m15 / m11);

  return (true);
}

template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
      PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header   = input_->header;
  projected_points.is_dense = input_->is_dense;

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      float dx = input_->points[inliers[i]].x - model_coefficients[0];
      float dy = input_->points[inliers[i]].y - model_coefficients[1];
      float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );

      projected_points.points[inliers[i]].x = a * dx + model_coefficients[0];
      projected_points.points[inliers[i]].y = a * dy + model_coefficients[1];
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      float dx = input_->points[inliers[i]].x - model_coefficients[0];
      float dy = input_->points[inliers[i]].y - model_coefficients[1];
      float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );

      projected_points.points[i].x = a * dx + model_coefficients[0];
      projected_points.points[i].y = a * dy + model_coefficients[1];
    }
  }
}

template<typename PointT> inline void 
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag) 
{
  if (!compute_done_)
    initCompute ();
  if (!compute_done_)
    PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");

  Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
  const size_t n = eigenvectors_.cols ();// number of eigen vectors
  Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
  Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
  Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
  Eigen::VectorXf h = y - input;
  if (h.norm() > 0) 
    h.normalize ();
  else
    h.setZero ();
  float gamma = h.dot(input - mean_.head<3>());
  Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
  D.block(0,0,n,n) = a * a.transpose();
  D /=  float(n)/float((n+1) * (n+1));
  for(std::size_t i=0; i < a.size(); i++) {
    D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
    D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
    D(i,D.cols()-1) = D(D.rows()-1,i);
    D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
  }

  Eigen::MatrixXf R(D.rows(), D.cols());
  Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
  Eigen::VectorXf alphap = D_evd.eigenvalues().real();
  eigenvalues_.resize(eigenvalues_.size() +1);
  for(std::size_t i=0;i<eigenvalues_.size();i++) {
    eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
    R.col(i) = D.col(D.cols()-i-1);
  }
  Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
  Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
  Up.rightCols<1>() = h;
  eigenvectors_ = Up*R;
  if (!basis_only_) {
    Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
    coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
    for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
      coefficients_(coefficients_.rows()-1,i) = 0;
      coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
    }
    a.resize(a.size()+1);
    a(a.size()-1) = 0;
    coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
  }
  mean_.head<3>() = meanp;
  switch (flag) 
  {
    case increase:
      if (eigenvectors_.rows() >= eigenvectors_.cols())
        break;
    case preserve:
      if (!basis_only_)
        coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
      eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
      eigenvalues_.resize(eigenvalues_.size()-1);
      break;
    default:
      PCL_ERROR("[pcl::PCA] unknown flag\n");
  }
}

  int Fit(Vector& res_G,  // residual under NULL -- may change when permuting
          Vector& v_G,    // variance under NULL -- may change when permuting
          Matrix& X_G,    // covariance
          Matrix& G_G,    // genotype
          Vector& w_G)    // weight
  {
    this->nPeople = X_G.rows;
    this->nMarker = G_G.cols;
    this->nCovariate = X_G.cols;

    // calculation w_sqrt
    G_to_Eigen(w_G, &this->w_sqrt);
    w_sqrt = w_sqrt.cwiseSqrt();

    // calculate K = G * W * G'
    Eigen::MatrixXf G;
    G_to_Eigen(G_G, &G);
    this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose();

    // calculate Q = ||res * K||
    Eigen::VectorXf res;
    G_to_Eigen(res_G, &res);
    this->Q = (this->K_sqrt * res).squaredNorm();

    // calculate P0 = V - V X (X' V X)^(-1) X' V
    Eigen::VectorXf v;
    G_to_Eigen(v_G, &v);
    if (this->nCovariate == 1) {
      P0 = -v * v.transpose() / v.sum();
      // printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols());
      // printf("dim(v) = %d\n", v.size());
      P0.diagonal() += v;
      // printf("dim(v) = %d\n", v.size());
    } else {
      Eigen::MatrixXf X;
      G_to_Eigen(X_G, &X);
      Eigen::MatrixXf XtV;  // X^t V
      XtV.noalias() = X.transpose() * v.asDiagonal();
      P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV;
      P0.diagonal() += v;
    }
    // dump();
    // Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose();
    // dumpToFile(tmp, "out.tmp");
    // eigen decomposition
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
    es.compute(K_sqrt * P0 * K_sqrt.transpose());

#ifdef DEBUG
    std::ofstream k("K");
    k << K_sqrt;
    k.close();
#endif
    // std::ofstream p("P0");
    // p << P0;
    // p.close();

    this->mixChiSq.reset();
    int r_ub = std::min(nPeople, nMarker);
    int r = 0;  // es.eigenvalues().size();
    int eigen_len = es.eigenvalues().size();
    for (int i = eigen_len - 1; i >= 0; i--) {
      if (es.eigenvalues()[i] > ZBOUND && r < r_ub) {
        this->mixChiSq.addLambda(es.eigenvalues()[i]);
        r++;
      } else {
        break;
      }
    }
    // calculate p-value
    this->pValue = this->mixChiSq.getPvalue(this->Q);
    if (this->pValue <= 0.0 || this->pValue == 1.0) {
      this->pValue = this->mixChiSq.getLiuPvalue(this->Q);
    }
    return 0;
  };

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computePointPFHRGBSignature (
    const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
    const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram)
{
  int h_index, h_p;

  // Clear the resultant point histogram
  pfhrgb_histogram.setZero ();

  // Factorization constant
  float hist_incr = 100.0f / static_cast<float> (indices.size () * indices.size () - 1);

  // Iterate over all the points in the neighborhood
  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    for (size_t j_idx = 0; j_idx < indices.size (); ++j_idx)
    {
      // Avoid unnecessary returns
      if (i_idx == j_idx)
        continue;

      // Compute the pair NNi to NNj
      if (!computeRGBPairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
                                   pfhrgb_tuple_[0], pfhrgb_tuple_[1], pfhrgb_tuple_[2], pfhrgb_tuple_[3],
                                   pfhrgb_tuple_[4], pfhrgb_tuple_[5], pfhrgb_tuple_[6]))
        continue;

      // Normalize the f1, f2, f3, f5, f6, f7 features and push them in the histogram
      f_index_[0] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[0] + M_PI) * d_pi_)));
      if (f_index_[0] < 0)         f_index_[0] = 0;
      if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;

      f_index_[1] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[1] + 1.0) * 0.5)));
      if (f_index_[1] < 0)         f_index_[1] = 0;
      if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;

      f_index_[2] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[2] + 1.0) * 0.5)));
      if (f_index_[2] < 0)         f_index_[2] = 0;
      if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;

      // color ratios are in [-1, 1]
      f_index_[4] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[4] + 1.0) * 0.5)));
      if (f_index_[4] < 0)         f_index_[4] = 0;
      if (f_index_[4] >= nr_split) f_index_[4] = nr_split - 1;

      f_index_[5] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[5] + 1.0) * 0.5)));
      if (f_index_[5] < 0)         f_index_[5] = 0;
      if (f_index_[5] >= nr_split) f_index_[5] = nr_split - 1;

      f_index_[6] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[6] + 1.0) * 0.5)));
      if (f_index_[6] < 0)         f_index_[6] = 0;
      if (f_index_[6] >= nr_split) f_index_[6] = nr_split - 1;


      // Copy into the histogram
      h_index = 0;
      h_p     = 1;
      for (int d = 0; d < 3; ++d)
      {
        h_index += h_p * f_index_[d];
        h_p     *= nr_split;
      }
      pfhrgb_histogram[h_index] += hist_incr;

      // and the colors
      h_index = 125;
      h_p     = 1;
      for (int d = 4; d < 7; ++d)
      {
        h_index += h_p * f_index_[d];
        h_p     *= nr_split;
      }
      pfhrgb_histogram[h_index] += hist_incr;
    }
  }
}

template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
      PointCloud &projected_points, bool copy_data_fields)
{
  // Needs a valid set of model coefficients
  if (model_coefficients.size () != 7)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
    return;
  }

  projected_points.header   = input_->header;
  projected_points.is_dense = input_->is_dense;

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // what i have:
      // P : Sample Point
      Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
      // C : Circle Center
      Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
      // N : Circle (Plane) Normal
      Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
      // r : Radius
      double r = model_coefficients[3];

      Eigen::Vector3d helper_vectorPC = P - C;
      // 1.1. get line parameter
      //float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ;
      double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
      // Projected Point on plane
      Eigen::Vector3d P_proj = P + lambda * N;
      Eigen::Vector3d helper_vectorP_projC = P_proj - C;

      // K : Point on Circle
      Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();

      projected_points.points[i].x = static_cast<float> (K[0]);
      projected_points.points[i].y = static_cast<float> (K[1]);
      projected_points.points[i].z = static_cast<float> (K[2]);
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = uint32_t (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the plane
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      // what i have:
      // P : Sample Point
      Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
      // C : Circle Center
      Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
      // N : Circle (Plane) Normal
      Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
      // r : Radius
      double r = model_coefficients[3];

      Eigen::Vector3d helper_vectorPC = P - C;
      // 1.1. get line parameter
      double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
      // Projected Point on plane
      Eigen::Vector3d P_proj = P + lambda * N;
      Eigen::Vector3d helper_vectorP_projC = P_proj - C;

      // K : Point on Circle
      Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();

      projected_points.points[i].x = static_cast<float> (K[0]);
      projected_points.points[i].y = static_cast<float> (K[1]);
      projected_points.points[i].z = static_cast<float> (K[2]);
    }
  }
}