Esempio n. 1
0
  inline void VectorAverage<real, dimension>::doPCA(Eigen::Matrix<real, dimension, 1>& eigen_values) const
  {
    // The following step is necessary for cases where the values in the covariance matrix are small
    // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors.
    //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>();
    //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance, false);
    //eigen_values = ei_symm.eigenvalues().template cast<real>();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_, false);
    eigen_values = ei_symm.eigenvalues();
  }
Esempio n. 2
0
  inline void VectorAverage<real, dimension>::getEigenVector1(Eigen::Matrix<real, dimension, 1>& eigen_vector1) const
  {
    // The following step is necessary for cases where the values in the covariance matrix are small
    // In this case float accuracy is nor enough to calculate the eigenvalues and eigenvectors.
    //Eigen::Matrix<double, dimension, dimension> tmp_covariance = covariance_.template cast<double>();
    //Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, dimension, dimension> > ei_symm(tmp_covariance);
    //eigen_values = ei_symm.eigenvalues().template cast<real>();
    //Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors().template cast<real>();

    //cout << "My covariance is \n"<<covariance_<<"\n";
    //cout << "My mean is \n"<<mean_<<"\n";
    //cout << "My Eigenvectors \n"<<eigen_vectors<<"\n";

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<real, dimension, dimension> > ei_symm(covariance_);
    Eigen::Matrix<real, dimension, dimension> eigen_vectors = ei_symm.eigenvectors();
    eigen_vector1 = eigen_vectors.col(0);
  }
Esempio n. 3
0
void ObjectModelLine::optimizeModelCoefficients (const std::vector<int> &inliers,
		const Eigen::VectorXd &model_coefficients,
		Eigen::VectorXd &optimized_coefficients){
	assert (model_coefficients.size () == 6);

	if (inliers.size () == 0)
	{
		cout<<"[ObjectModelLine::optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients";
		optimized_coefficients = model_coefficients;
		return;
	}

	assert (inliers.size () > 2);

	optimized_coefficients.resize (6);

	// Compute the 3x3 covariance matrix
	Eigen::Vector4d centroid = Centroid3D::computeCentroid(this->inputPointCloud, inliers);
//	compute3DCentroid (this->inputPointCloud, inliers, centroid);
	Eigen::Matrix3d covariance_matrix;
//	computeCovarianceMatrix (inputPointCloud, inliers, centroid, covariance_matrix);
	 covariance_matrix = Covariance3D::computeCovarianceMatrix (inputPointCloud, inliers, centroid);
	optimized_coefficients[0] = centroid[0];
	optimized_coefficients[1] = centroid[1];
	optimized_coefficients[2] = centroid[2];

	// Extract the eigenvalues and eigenvectors
	//cloud_geometry::eigen_cov (covariance_matrix, eigen_values, eigen_vectors);
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> ei_symm (covariance_matrix);
//	EIGEN_ALIGN_MEMORY Eigen::Vector3d eigen_values  = ei_symm.eigenvalues ();
	EIGEN_ALIGN_MEMORY Eigen::Matrix3d eigen_vectors = ei_symm.eigenvectors ();

#ifdef EIGEN3
	optimized_coefficients.tail<3> () = eigen_vectors.col (2).normalized ();
#else
	optimized_coefficients.end<3> () = eigen_vectors.col (2).normalized ();
#endif

}
Esempio n. 4
0
template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix_.setZero ();
  if (input_->height == 1 || input_->width == 1)
  {
    PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", getName ().c_str ());
    return;
  }

  // we just want to use every 16th column and row -> skip = 2^4
  const unsigned int skip = input_->width >> 4;
  Eigen::Matrix<Scalar, 4, 4> A = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> B = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> C = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> D = Eigen::Matrix<Scalar, 4, 4>::Zero ();

  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += skip, idx += input_->width * (skip-1))
  {
    for (unsigned xIdx = 0; xIdx < input_->width; xIdx += skip, idx += skip)
    {
      const PointT& point = input_->points[idx];
      if (isFinite (point))
      {
        Scalar xx = point.x * point.x;
        Scalar xy = point.x * point.y;
        Scalar xz = point.x * point.z;
        Scalar yy = point.y * point.y;
        Scalar yz = point.y * point.z;
        Scalar zz = point.z * point.z;
        Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

        A.coeffRef (0) += xx;
        A.coeffRef (1) += xy;
        A.coeffRef (2) += xz;
        A.coeffRef (3) += point.x;

        A.coeffRef (5) += yy;
        A.coeffRef (6) += yz;
        A.coeffRef (7) += point.y;

        A.coeffRef (10) += zz;
        A.coeffRef (11) += point.z;
        A.coeffRef (15) += 1.0;

        B.coeffRef (0) -= xx * xIdx;
        B.coeffRef (1) -= xy * xIdx;
        B.coeffRef (2) -= xz * xIdx;
        B.coeffRef (3) -= point.x * xIdx;

        B.coeffRef (5) -= yy * xIdx;
        B.coeffRef (6) -= yz * xIdx;
        B.coeffRef (7) -= point.y * xIdx;

        B.coeffRef (10) -= zz * xIdx;
        B.coeffRef (11) -= point.z * xIdx;

        B.coeffRef (15) -= xIdx;

        C.coeffRef (0) -= xx * yIdx;
        C.coeffRef (1) -= xy * yIdx;
        C.coeffRef (2) -= xz * yIdx;
        C.coeffRef (3) -= point.x * yIdx;

        C.coeffRef (5) -= yy * yIdx;
        C.coeffRef (6) -= yz * yIdx;
        C.coeffRef (7) -= point.y * yIdx;

        C.coeffRef (10) -= zz * yIdx;
        C.coeffRef (11) -= point.z * yIdx;

        C.coeffRef (15) -= yIdx;

        D.coeffRef (0) += xx * xx_yy;
        D.coeffRef (1) += xy * xx_yy;
        D.coeffRef (2) += xz * xx_yy;
        D.coeffRef (3) += point.x * xx_yy;

        D.coeffRef (5) += yy * xx_yy;
        D.coeffRef (6) += yz * xx_yy;
        D.coeffRef (7) += point.y * xx_yy;

        D.coeffRef (10) += zz * xx_yy;
        D.coeffRef (11) += point.z * xx_yy;

        D.coeffRef (15) += xx_yy;
      }
    }
  }

  makeSymmetric(A);
  makeSymmetric(B);
  makeSymmetric(C);
  makeSymmetric(D);

  Eigen::Matrix<Scalar, 12, 12> X = Eigen::Matrix<Scalar, 12, 12>::Zero ();
  X.topLeftCorner<4,4> () = A;
  X.block<4,4> (0, 8) = B;
  X.block<4,4> (8, 0) = B;
  X.block<4,4> (4, 4) = A;
  X.block<4,4> (4, 8) = C;
  X.block<4,4> (8, 4) = C;
  X.block<4,4> (8, 8) = D;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12> > ei_symm(X);
  Eigen::Matrix<Scalar, 12, 12> eigen_vectors = ei_symm.eigenvectors();

  // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device.
  Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X *  eigen_vectors.col (0);
  if ( residual_sqr.coeff (0) > eps_ * A.coeff (15))
  {
    PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\n", getName ().c_str ());
    return;
  }

  projection_matrix_.coeffRef (0) = eigen_vectors.coeff (0);
  projection_matrix_.coeffRef (1) = eigen_vectors.coeff (12);
  projection_matrix_.coeffRef (2) = eigen_vectors.coeff (24);
  projection_matrix_.coeffRef (3) = eigen_vectors.coeff (36);
  projection_matrix_.coeffRef (4) = eigen_vectors.coeff (48);
  projection_matrix_.coeffRef (5) = eigen_vectors.coeff (60);
  projection_matrix_.coeffRef (6) = eigen_vectors.coeff (72);
  projection_matrix_.coeffRef (7) = eigen_vectors.coeff (84);
  projection_matrix_.coeffRef (8) = eigen_vectors.coeff (96);
  projection_matrix_.coeffRef (9) = eigen_vectors.coeff (108);
  projection_matrix_.coeffRef (10) = eigen_vectors.coeff (120);
  projection_matrix_.coeffRef (11) = eigen_vectors.coeff (132);

  if (projection_matrix_.coeff (0) < 0)
    projection_matrix_ *= -1.0;

  // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
  // and R being the rotation matrix
  KR_ = projection_matrix_.topLeftCorner <3, 3> ();

  // precalculate KR * KR^T needed by calculations during nn-search
  KR_KRT_ = KR_ * KR_.transpose ();
}
Esempio n. 5
0
template <typename PointT> double 
pcl::estimateProjectionMatrix (
    typename pcl::PointCloud<PointT>::ConstPtr cloud, 
    Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, 
    const std::vector<int>& indices)
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix.setZero ();
  if (cloud->height == 1 || cloud->width == 1)
  {
    PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
    return (-1.0);
  }
  
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();

  pcl::ConstCloudIterator <PointT> pointIt (*cloud, indices);
  
  while (pointIt)
  {
    unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width;
    unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width;
    
    const PointT& point = *pointIt;
    if (pcl_isfinite (point.x))
    {
      Scalar xx = point.x * point.x;
      Scalar xy = point.x * point.y;
      Scalar xz = point.x * point.z;
      Scalar yy = point.y * point.y;
      Scalar yz = point.y * point.z;
      Scalar zz = point.z * point.z;
      Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

      A.coeffRef (0) += xx;
      A.coeffRef (1) += xy;
      A.coeffRef (2) += xz;
      A.coeffRef (3) += point.x;

      A.coeffRef (5) += yy;
      A.coeffRef (6) += yz;
      A.coeffRef (7) += point.y;

      A.coeffRef (10) += zz;
      A.coeffRef (11) += point.z;
      A.coeffRef (15) += 1.0;

      B.coeffRef (0) -= xx * xIdx;
      B.coeffRef (1) -= xy * xIdx;
      B.coeffRef (2) -= xz * xIdx;
      B.coeffRef (3) -= point.x * static_cast<double>(xIdx);

      B.coeffRef (5) -= yy * xIdx;
      B.coeffRef (6) -= yz * xIdx;
      B.coeffRef (7) -= point.y * static_cast<double>(xIdx);

      B.coeffRef (10) -= zz * xIdx;
      B.coeffRef (11) -= point.z * static_cast<double>(xIdx);

      B.coeffRef (15) -= xIdx;

      C.coeffRef (0) -= xx * yIdx;
      C.coeffRef (1) -= xy * yIdx;
      C.coeffRef (2) -= xz * yIdx;
      C.coeffRef (3) -= point.x * static_cast<double>(yIdx);

      C.coeffRef (5) -= yy * yIdx;
      C.coeffRef (6) -= yz * yIdx;
      C.coeffRef (7) -= point.y * static_cast<double>(yIdx);

      C.coeffRef (10) -= zz * yIdx;
      C.coeffRef (11) -= point.z * static_cast<double>(yIdx);

      C.coeffRef (15) -= yIdx;

      D.coeffRef (0) += xx * xx_yy;
      D.coeffRef (1) += xy * xx_yy;
      D.coeffRef (2) += xz * xx_yy;
      D.coeffRef (3) += point.x * xx_yy;

      D.coeffRef (5) += yy * xx_yy;
      D.coeffRef (6) += yz * xx_yy;
      D.coeffRef (7) += point.y * xx_yy;

      D.coeffRef (10) += zz * xx_yy;
      D.coeffRef (11) += point.z * xx_yy;

      D.coeffRef (15) += xx_yy;
    }
    
    ++pointIt;
  } // while  
  
  pcl::common::internal::makeSymmetric (A);
  pcl::common::internal::makeSymmetric (B);
  pcl::common::internal::makeSymmetric (C);
  pcl::common::internal::makeSymmetric (D);

  Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero ();
  X.topLeftCorner<4,4> ().matrix () = A;
  X.block<4,4> (0, 8).matrix () = B;
  X.block<4,4> (8, 0).matrix () = B;
  X.block<4,4> (4, 4).matrix () = A;
  X.block<4,4> (4, 8).matrix () = C;
  X.block<4,4> (8, 4).matrix () = C;
  X.block<4,4> (8, 8).matrix () = D;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm (X);
  Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors ();

  // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device.
  Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X *  eigen_vectors.col (0);
  
  double residual = residual_sqr.coeff (0);

  projection_matrix.coeffRef (0) = static_cast <float> (eigen_vectors.coeff (0));
  projection_matrix.coeffRef (1) = static_cast <float> (eigen_vectors.coeff (12));
  projection_matrix.coeffRef (2) = static_cast <float> (eigen_vectors.coeff (24));
  projection_matrix.coeffRef (3) = static_cast <float> (eigen_vectors.coeff (36));
  projection_matrix.coeffRef (4) = static_cast <float> (eigen_vectors.coeff (48));
  projection_matrix.coeffRef (5) = static_cast <float> (eigen_vectors.coeff (60));
  projection_matrix.coeffRef (6) = static_cast <float> (eigen_vectors.coeff (72));
  projection_matrix.coeffRef (7) = static_cast <float> (eigen_vectors.coeff (84));
  projection_matrix.coeffRef (8) = static_cast <float> (eigen_vectors.coeff (96));
  projection_matrix.coeffRef (9) = static_cast <float> (eigen_vectors.coeff (108));
  projection_matrix.coeffRef (10) = static_cast <float> (eigen_vectors.coeff (120));
  projection_matrix.coeffRef (11) = static_cast <float> (eigen_vectors.coeff (132));

  if (projection_matrix.coeff (0) < 0)
    projection_matrix *= -1.0;

  return (residual);
}