void CovarianceMatrix::DoExecute() {
/*
* This reports the Covariance, Correlation and Hessian matrix
*/
LOG_TRACE();
auto minimiser_ = model_->managers().minimiser()->active_minimiser();
covariance_matrix_ = minimiser_->covariance_matrix();
cache_ << "*" << label_ << " " << "(" << type_ << ")" << "\n";
cache_ << "Covariance_Matrix " << REPORT_R_MATRIX << "\n";
for (unsigned i = 0; i < covariance_matrix_.size1(); ++i) {
for (unsigned j = 0; j < covariance_matrix_.size2(); ++j)
cache_ << covariance_matrix_(i, j) << " ";
cache_ << "\n";
}
if (model_->run_mode() == RunMode::kMCMC) {
auto mcmc_ = model_->managers().mcmc()->active_mcmc();
if (mcmc_->recalculate_covariance()) {
cache_ << REPORT_END << "\n\n";
LOG_FINE() << "During the mcmc run you recalculated the the covariance matrix, so we will print the modified matrix at the end of the chain";
cache_ << "Modified_covariance_matrix " << REPORT_R_MATRIX << "\n";
auto covariance = mcmc_->covariance_matrix();
for (unsigned i = 0; i < covariance.size1(); ++i) {
for (unsigned j = 0; j < covariance.size2() - 1; ++j)
cache_ << covariance(i, j) << " ";
cache_ << covariance(i, covariance.size2() - 1) << "\n";
}
}
}
ready_for_writing_ = true;
}
void StrictGaussianClassifier::set_covariance( const vector<DataItem>& data,
const Eigen::Vector2d& mean )
{
// variables
int i = 0;
int num_data = 0;
// reset the covariance matrix
covariance_matrix_ << 0, 0,
0, 0;
// sum the values of the features over all of the data
for( num_data = 0, i = 0; i < data.size(); i++ )
{
// case: the data is of the desired class
if( data[i].actual_class == class_name_ )
{
// add the data to the sums
covariance_matrix_( 0, 0 ) +=
( data[i].feature_vector( 0 ) - mean( 0 ) ) *
( data[i].feature_vector( 0 ) - mean( 0 ) );
covariance_matrix_( 1, 0 ) +=
( data[i].feature_vector( 1 ) - mean( 1 ) ) *
( data[i].feature_vector( 0 ) - mean( 0 ) );
covariance_matrix_( 1, 1 ) +=
( data[i].feature_vector( 1 ) - mean( 1 ) ) *
( data[i].feature_vector( 1 ) - mean( 1 ) );
num_data++;
}
}
// set the covariance above the diagonal
covariance_matrix_( 0, 1 ) = covariance_matrix_( 1, 0 );
// scale the result
covariance_matrix_ = ( 1.0 / ( (double) num_data - 1.0) ) *
covariance_matrix_;
// update the other covariance related members
inverse_covariance_matrix_ = covariance_matrix_.inverse();
covariance_determinant_ = covariance_matrix_.determinant();
// no return - void
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::computePointPrincipalCurvatures (
const pcl::PointCloud<PointNT> &normals, int p_idx, const std::vector<int> &indices,
float &pcx, float &pcy, float &pcz, float &pc1, float &pc2)
{
EIGEN_ALIGN16 Eigen::Matrix3f I = Eigen::Matrix3f::Identity ();
Eigen::Vector3f n_idx (normals.points[p_idx].normal[0], normals.points[p_idx].normal[1], normals.points[p_idx].normal[2]);
EIGEN_ALIGN16 Eigen::Matrix3f M = I - n_idx * n_idx.transpose (); // projection matrix (into tangent plane)
// Project normals into the tangent plane
Eigen::Vector3f normal;
projected_normals_.resize (indices.size ());
xyz_centroid_.setZero ();
for (size_t idx = 0; idx < indices.size(); ++idx)
{
normal[0] = normals.points[indices[idx]].normal[0];
normal[1] = normals.points[indices[idx]].normal[1];
normal[2] = normals.points[indices[idx]].normal[2];
projected_normals_[idx] = M * normal;
xyz_centroid_ += projected_normals_[idx];
}
// Estimate the XYZ centroid
xyz_centroid_ /= static_cast<float> (indices.size ());
// Initialize to 0
covariance_matrix_.setZero ();
double demean_xy, demean_xz, demean_yz;
// For each point in the cloud
for (size_t idx = 0; idx < indices.size (); ++idx)
{
demean_ = projected_normals_[idx] - xyz_centroid_;
demean_xy = demean_[0] * demean_[1];
demean_xz = demean_[0] * demean_[2];
demean_yz = demean_[1] * demean_[2];
covariance_matrix_(0, 0) += demean_[0] * demean_[0];
covariance_matrix_(0, 1) += static_cast<float> (demean_xy);
covariance_matrix_(0, 2) += static_cast<float> (demean_xz);
covariance_matrix_(1, 0) += static_cast<float> (demean_xy);
covariance_matrix_(1, 1) += demean_[1] * demean_[1];
covariance_matrix_(1, 2) += static_cast<float> (demean_yz);
covariance_matrix_(2, 0) += static_cast<float> (demean_xz);
covariance_matrix_(2, 1) += static_cast<float> (demean_yz);
covariance_matrix_(2, 2) += demean_[2] * demean_[2];
}
// Extract the eigenvalues and eigenvectors
pcl::eigen33 (covariance_matrix_, eigenvalues_);
pcl::computeCorrespondingEigenVector (covariance_matrix_, eigenvalues_ [2], eigenvector_);
pcx = eigenvector_ [0];
pcy = eigenvector_ [1];
pcz = eigenvector_ [2];
float indices_size = 1.0f / static_cast<float> (indices.size ());
pc1 = eigenvalues_ [2] * indices_size;
pc2 = eigenvalues_ [1] * indices_size;
}
