float pseudo_inv(const Eigen::MatrixXf *mat_in,
Eigen::MatrixXf *mat_out) {
int dim = 0;
// Get matrices dimension :
if (mat_in->cols () != mat_in->rows ()) {
THROW_ERR("Cannot compute matrix pseudo_inverse");
} else {
dim = mat_in->cols ();
}
mat_out->resize (dim, dim);
Eigen::MatrixXf U (dim,dim);
Eigen::MatrixXf eig_val (dim, 1);
Eigen::MatrixXf eig_val_inv (dim, dim);
Eigen::MatrixXf V (dim, dim);
float det;
eig_val_inv = Eigen::MatrixXf::Identity(dim,dim);
// Compute the SVD decomposition
Eigen::JacobiSVD<Eigen::MatrixXf> svd(*mat_in, Eigen::ComputeFullU | Eigen::ComputeFullV);
eig_val = svd.singularValues();
U = svd.matrixU();
V = svd.matrixV();
// Compute pseudo-inverse
// - quick'n'dirty inversion of eigen matrix
for (int i = 0; i<dim; ++i) {
if (eig_val(i,0) != 0.f)
eig_val_inv(i,i) = 1.f / eig_val(i,0);
else
eig_val_inv(i,i) = 0.f;
}
*mat_out = V.transpose() * eig_val_inv * U.transpose();
// Compute determinant from eigenvalues..
det = 1.f;
for (int i=0; i<dim; ++i) {
det *= eig_val(i,0);
}
return det;
}
// Small hack to compute pseudo-inverse for a 6x6 matrix using SVD method
float pseudo_inv_6(const Eigen::Matrix <float, 6,6> *mat_in,
Eigen::Matrix <float, 6,6> *mat_out)
{
Eigen::Matrix <float, 6,6> U;
Eigen::Matrix <float, 6,1> eig_val;
Eigen::Matrix <float, 6,6> eig_val_inv;
Eigen::Matrix <float, 6,6> V;
float det;
eig_val_inv = Eigen::MatrixXf::Identity(6,6);
// Compute the SVD decomposition
Eigen::JacobiSVD<Eigen::MatrixXf> svd(*mat_in,
Eigen::ComputeThinU | Eigen::ComputeThinV);
eig_val = svd.singularValues();
U = svd.matrixU();
V = svd.matrixV();
// Compute pseudo-inverse
// - quick'n'dirty inversion of eigen matrix
for (int i = 0; i<6; ++i) {
if (eig_val(i,0) != 0.f)
eig_val_inv(i,i) = 1.f / eig_val(i,0);
else
eig_val_inv(i,i) = 0.f;
}
*mat_out = V.transpose() * eig_val_inv * U.transpose();
// Compute determinant from eigenvalues..
det = 1.f;
for (int i=0; i<6; ++i) {
det *= eig_val(i,0);
}
return det;
}
void LocalGeometryRef::SVD(int num_eigen)
{
vnl_symmetric_eigensystem<double> eigen(this->probability_matrix);
vnl_matrix<double> eig_vec = eigen.V;
vnl_diag_matrix<double> eig_val = eigen.D;
vnl_vector<double> temp_vec(this->num_row);
for(int row = 0; row<eig_val.rows(); row++)
{
temp_vec(row) = eig_val(row,row);
}
this->eigVals.set_size(num_eigen);
this->eigVecs.set_size(this->num_row, num_eigen);
for(int i=0; i<num_eigen; i++)
{
this->eigVals(i) = temp_vec.max_value();
int pos = temp_vec.arg_max();
temp_vec(pos) = temp_vec.min_value() - 1;
this->eigVecs.set_column(i, eig_vec.get_column(pos) * this->eigVals(i) );
}
//output files for debugging
FILE *fp1 = fopen("eigenvecters.txt","w");
for(int i=0; i<this->num_row; i++)
{
for(int j=0; j<num_eigen; j++)
{
fprintf(fp1,"%f\t",this->eigVecs(i, j));
}
fprintf(fp1,"\n");
}
fclose(fp1);
FILE *fp2 = fopen("eigenvalues.txt","w");
for(int i=0; i<num_eigen; i++)
{
fprintf(fp2,"%f\t",this->eigVals(i));
std::cout<<this->eigVals(i)<<std::endl;
fprintf(fp2,"\n");
}
fclose(fp2);
}
