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