//--------------------------------------------------------------//
void
CheckPositiveDefiniteCorrMatrix(double corr01, double corr02, double corr12, std::string & errTxt)
{
NRLib::Matrix corr_matrix(3, 3, 0);
for(int i=0; i<3; i++)
corr_matrix(i,i) = 1;
corr_matrix(0,1) = corr01;
corr_matrix(1,0) = corr01;
corr_matrix(0,2) = corr02;
corr_matrix(2,0) = corr02;
corr_matrix(1,2) = corr12;
corr_matrix(2,1) = corr12;
NRLib::Vector eigen_values(3, 0);
NRLib::Matrix eigen_vectors(3, 3, 0);
NRLib::ComputeEigenVectors(corr_matrix, eigen_values, eigen_vectors);
bool pos_def = true;
for( int i=0; i<3; i++) {
if(eigen_values(i) < 0)
pos_def = false;
}
if(pos_def == false)
errTxt += "The correlations given in the tabulated rock physics model need to generate a positive definite matrix\n";
}
inline void solvePlaneParametersEigen(const Eigen::Matrix3f &covariance_matrix,
float &nx, float &ny, float &nz,
float &lam0, float &lam1, float &lam2)
{
Eigen::Matrix3f eigen_vectors;
Eigen::Vector3f eigen_values;
Eigen::SelfAdjointEigenSolver
<Eigen::Matrix3f> eigensolver(covariance_matrix);
if (eigensolver.info() != Eigen::Success) {
nx = ny = nz = lam0 = lam1 = lam2 = std::numeric_limits
<float>::quiet_NaN();
return;
}
eigen_vectors = eigensolver.eigenvectors();
eigen_values = eigensolver.eigenvalues();
// first column, no?
nx = eigen_vectors(0, 0);
ny = eigen_vectors(1, 0);
nz = eigen_vectors(2, 0);
lam0 = eigen_values(0);
lam1 = eigen_values(1);
lam2 = eigen_values(2);
}
void
RockPhysicsInversion4D::SolveGEVProblem(NRLib::Matrix sigma_prior,
NRLib::Matrix sigma_posterior,
NRLib::Matrix & vOut){
//Compute transforms v1, v2, v3 and v4 ----------------------------------------
// computing filter
NRLib::SymmetricMatrix sigma_priorInv(6);
for(int i=0;i<6;i++)
for(int j=0;j<=i;j++)
sigma_priorInv(j,i)=sigma_prior(i,j);
NRLib::CholeskyInvert(sigma_priorInv);
NRLib::Matrix eye = NRLib::IdentityMatrix(6);
NRLib::Matrix filter = eye- sigma_priorInv*sigma_posterior;
// computing components
NRLib::Vector eigen_values(6);
NRLib::Matrix eigen_vectors(6,6);
NRLib::ComputeEigenVectors( filter ,eigen_values,eigen_vectors);
// extracting four best components
std::vector<int> current_index(6);
current_index[0] = 0;current_index[1] = 1;
current_index[2] = 2;current_index[3] = 3;
current_index[4] = 4;current_index[5] = 5;
std::vector<int> best_index(4);
std::vector<int> keep_index(6);
keep_index = current_index;
NRLib::Vector current_value(6);
NRLib::Vector keep_value(6);
keep_value = eigen_values;
for(int i=0;i<4;i++){
current_index=keep_index;
current_value =keep_value;
double maxVal=-999; // (the theoretical max value is less than 1 and larger than zero)
for(int j=0;j<6-i;j++){
if(current_value(j) > maxVal){
maxVal=current_value(j);
best_index[i]=current_index[j];
}
}
int counter=0;
for(int j=0;j<6-i;j++){
if(current_value(j) != maxVal){
keep_value(counter)=current_value(j);
keep_index[counter]=current_index[j];
counter++;
}
}
}
vOut.resize(6,4);
for(int i=0;i<4;i++)
for(int j=0;j<6;j++)
vOut(j,i)=eigen_vectors(j,best_index[i]);
}
void
Tabulated::CalculateSigmaSqrt(const NRLib::Grid2D<double> & Sigma)
{
// Calculate square root of positive definite correlation matrix
NRLib::Matrix corr_matrix(n_variables_,n_variables_);
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++)
corr_matrix(i,j) = Sigma(i,j);
}
NRLib::Vector eigen_values(n_variables_);
NRLib::Matrix eigen_vectors(n_variables_,n_variables_);
NRLib::ComputeEigenVectors(corr_matrix, eigen_values, eigen_vectors);
NRLib::Matrix eigen_vectors_transpose(n_variables_,n_variables_);
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++)
eigen_vectors_transpose(j,i) = eigen_vectors(i,j);
}
NRLib::Matrix lambda_square(n_variables_,n_variables_);
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++) {
if(i == j)
lambda_square(i,j) = std::sqrt(eigen_values(i));
else
lambda_square(i,j) = 0;
}
}
NRLib::Matrix Sigma_sqrt1(n_variables_,n_variables_);
NRLib::Matrix Sigma_sqrt(n_variables_,n_variables_);
Sigma_sqrt1 = eigen_vectors * lambda_square;
Sigma_sqrt = Sigma_sqrt1 * eigen_vectors_transpose;
for(int i=0; i<n_variables_; i++) {
for(int j=0; j<n_variables_; j++)
Sigma_sqrt_(i,j) = Sigma_sqrt(i,j);
}
}
void getSpectralProfile(const pcl::PointCloud<PointT> &cloud, SpectralProfile &spectralProfile)
{
Eigen::Matrix3d eigen_vectors;
Eigen::Vector3d eigen_values;
sensor_msgs::PointCloud2 cloudMsg2;
pcl::toROSMsg (cloud,cloudMsg2);
sensor_msgs::PointCloud cloudMsg;
sensor_msgs::convertPointCloud2ToPointCloud(cloudMsg2,cloudMsg);
cloud_geometry::nearest::computePatchEigenNormalized (cloudMsg,eigen_vectors,eigen_values,spectralProfile.centroid);
spectralProfile.setEigValues (eigen_values);
float minEigV=FLT_MAX;
for(int i=0;i<3;i++)
{
cout<<"eig value:"<<eigen_values(i)<<endl;
if(minEigV>eigen_values(i))
{
minEigV=eigen_values(i);
cout<<"min eig value:"<<minEigV<<endl;
spectralProfile.normal=eigen_vectors.col(i);
// check the angle with line joining the centroid to origin
VectorG centroid(spectralProfile.centroid.x,spectralProfile.centroid.y,spectralProfile.centroid.z);
VectorG camera=originalFrames[cloud.points[1].cameraIndex]->getCameraTrans().getOrigin();
VectorG cent2cam=camera.subtract(centroid);
VectorG normal(spectralProfile.normal[0],spectralProfile.normal[1],spectralProfile.normal[2]);
if (normal.dotProduct(cent2cam) < 0)
{
// flip the sign of the normal
spectralProfile.normal[0] = -spectralProfile.normal[0];
spectralProfile.normal[1] = -spectralProfile.normal[1];
spectralProfile.normal[2] = -spectralProfile.normal[2];
}
}
}
assert(minEigV==spectralProfile.getDescendingLambda (2));
}
geometry_msgs::Quaternion calc_quaternion_average( std::vector<geometry_msgs::Pose> pose_vector ){
Eigen::Matrix4f averaging_matrix;
Eigen::Vector4f quaternion;
averaging_matrix.setZero();
for( unsigned int sample_ii=0; sample_ii<pose_vector.size(); sample_ii++){
quaternion(0) = pose_vector[sample_ii].orientation.x;
quaternion(1) = pose_vector[sample_ii].orientation.y;
quaternion(2) = pose_vector[sample_ii].orientation.z;
quaternion(3) = pose_vector[sample_ii].orientation.w;
averaging_matrix = averaging_matrix + quaternion*quaternion.transpose();
}// for all samples
Eigen::EigenSolver<Eigen::Matrix4f> ev_solver;
ev_solver.compute( averaging_matrix);
Eigen::Vector4cf eigen_values = ev_solver.eigenvalues();
float max_ev=eigen_values(0).real();
unsigned int max_ii = 0;
for( unsigned int ev_ii=1; ev_ii<4; ev_ii++){
if( eigen_values(ev_ii).real()>max_ev ){
max_ev = eigen_values(ev_ii).real();
max_ii=ev_ii;
}
}
Eigen::Vector4f eigen_vector = ev_solver.eigenvectors().col(max_ii).real();
eigen_vector.normalize();
geometry_msgs::Quaternion quaternion_msg;
quaternion_msg.x = eigen_vector(0);
quaternion_msg.y = eigen_vector(1);
quaternion_msg.z = eigen_vector(2);
quaternion_msg.w = eigen_vector(3);
return quaternion_msg;
}
int POD<TruthModelType>::pod( mode_set_type& ModeSet, bool is_primal, const wn_type& elements_set, bool use_solutions )
{
M_backend = backend_type::build( BACKEND_PETSC );
Eigen::SelfAdjointEigenSolver< matrixN_type > eigen_solver;
M_use_solutions = use_solutions;
fillPodMatrix( elements_set );
int size = M_pod_matrix.cols();
//store the matrix
if ( M_store_pod_matrix )
{
std::ofstream matrix_file;
LOG(INFO)<<"saving Pod matrix in a file \n";
matrix_file.open( "PodMatrix",std::ios::out );
matrix_file<<M_pod_matrix.rows();
matrix_file<<"\n";
matrix_file<<M_pod_matrix.cols();
matrix_file<<"\n";
for ( int i=0; i<M_pod_matrix.rows(); i++ )
{
for ( int j=0; j<M_pod_matrix.cols(); j++ )
{
matrix_file<< std::setprecision( 16 ) <<M_pod_matrix( i,j )<<" ";
}
matrix_file<<"\n";
}
LOG(INFO)<<" matrix wrote in file named PodMatrix \n";
}
else if ( M_store_pod_matrix_format_octave )
{
std::ofstream matrix_file;
LOG(INFO)<<"saving Pod matrix in a file \n";
matrix_file.open( "PodMatrixOctave.mat",std::ios::out );
matrix_file<<"# name: A\n";
matrix_file<<"# type: matrix\n";
matrix_file<<"# rows: "<<M_pod_matrix.rows()<<"\n";
matrix_file<<"# columns: "<<M_pod_matrix.cols()<<"\n";
for ( int i=0; i<M_pod_matrix.rows(); i++ )
{
for ( int j=0; j<M_pod_matrix.cols(); j++ )
{
matrix_file<< std::setprecision( 16 ) <<M_pod_matrix( i,j )<<" ";
}
matrix_file<<"\n";
}
LOG(INFO)<<" matrix wrote in file named PodMatrix \n";
matrix_file.close();
}
eigen_solver.compute( M_pod_matrix ); // solve M_pod_matrix psi = lambda psi
int number_of_eigenvalues = eigen_solver.eigenvalues().size();
LOG(INFO)<<"Number of eigenvalues : "<<number_of_eigenvalues<<"\n";
//we copy eigenvalues in a std::vector beacause it's easier to manipulate it
std::vector<double> eigen_values( number_of_eigenvalues );
int too_small_index = 0;
for ( int i=0; i<number_of_eigenvalues; i++ )
{
if ( imag( eigen_solver.eigenvalues()[i] )>1e-12 )
{
throw std::logic_error( "[POD::pod] ERROR : complex eigenvalues were found" );
}
eigen_values[i]=real( eigen_solver.eigenvalues()[i] );
}
//tab will contains index of maximum eigenvalues
std::vector<int> max_idx;
std::vector<double> copy_eigen_values ( eigen_values );
int number_of_good_eigenvectors=0;
bool go = true;
double min_eigenvalue = option(_name="pod.minimum-eigenvalue").template as<double>();
//for(int maxmode=0; maxmode<M_Nm; maxmode++)
auto max = std::max_element( copy_eigen_values.begin(), copy_eigen_values.end() );
double idx = std::distance( copy_eigen_values.begin(), max );
if( *max < min_eigenvalue )
go=false;
while( go )
{
number_of_good_eigenvectors++;
max_idx.push_back( idx );
copy_eigen_values[idx]=0;
max = std::max_element( copy_eigen_values.begin(), copy_eigen_values.end() );
idx = std::distance( copy_eigen_values.begin(), max );
if( *max < min_eigenvalue )
go = false;
if( number_of_good_eigenvectors == M_Nm )
go = false;
if( number_of_good_eigenvectors == number_of_eigenvalues )
go = false;
}
CHECK( number_of_good_eigenvectors > 0 )<<"The max eigenvalue "<<*max<<" is under the minimum eigenvalue set by the user "<<min_eigenvalue<<" and we have zero eigenvectors !\n";
int position_of_largest_eigenvalue=max_idx[0];
if( M_Nm == -1 )
{
//in this case the user doesn't know a priori
//what value give to M_Nm
//this case appears for when apply-POD-to-WN=true
M_Nm = number_of_good_eigenvectors;
}
if ( M_Nm > number_of_good_eigenvectors && number_of_good_eigenvectors>0 && is_primal )
{
//in this case, the user set M_Nm but with respect to
//the minimum eigenvalue set, we don't find enough eigenvectors
M_Nm = number_of_good_eigenvectors;
}
for ( int i=0; i<M_Nm; i++ )
{
element_ptrtype mode ( new element_type( M_model->functionSpace() ) );
mode->zero();
int index=0;
if( M_use_solutions )
{
M_bdf->setRestart( true );
for ( M_bdf->restart(); !M_bdf->isFinished(); M_bdf->next() )
{
M_bdf->loadCurrent();
double psi_k = real( eigen_solver.eigenvectors().col( position_of_largest_eigenvalue )[index] );
M_bdf->unknown( 0 ).scale( psi_k );
mode->add( 1 , M_bdf->unknown( 0 ) );
index++;
}
}//if use solutions
else
{
double eigenvalue = eigen_values[ position_of_largest_eigenvalue ];
for(int j=0; j<size; j++)
{
double psi_k = real( eigen_solver.eigenvectors().col( position_of_largest_eigenvalue )[index] );
mode->add( psi_k , elements_set[j] );
index++;
}
}//if use elements set
//check
auto eigenvector = eigen_solver.eigenvectors().col( position_of_largest_eigenvalue );
double eigenvalue = eigen_values[position_of_largest_eigenvalue];
auto Aw = M_pod_matrix*eigenvector;
auto lambdaw = eigenvalue*eigenvector;
double Awnorm = Aw.norm();
double lambdawnorm = lambdaw.norm();
double eigenvectornorm = eigenvector.norm();
double check_tol = option(_name="pod.check-tol").template as<double>();
CHECK( math::abs(Awnorm - lambdawnorm) < check_tol )<<" A w : "<<Awnorm<<" and lambda w : "<<lambdawnorm<<" so math::abs(A w - lambda w) : "<<math::abs(Awnorm - lambdawnorm)<<" and pod.check-tol : "<<check_tol<<" -- eigenvalue : "<<eigenvalue<<"\n";
if( (i+1) < max_idx.size() )
position_of_largest_eigenvalue=max_idx[i+1];
ModeSet.push_back( *mode );
}// loop on number of modes
//check orthogonality
if( option(_name="pod.check-orthogonality").template as<bool>() )
{
position_of_largest_eigenvalue=max_idx[0];
for(int i=1; i<M_Nm; i++)
{
auto modei = ModeSet[i-1];
for(int j=i+1; j<M_Nm; j++)
{
auto modej = ModeSet[j-1];
double prod = M_model->scalarProductForPod( modei , modej );
CHECK( prod < 1e-10 )<<"scalar product between mode "<<i<<" and mode "<<j<<" is not null and is "<<prod<<"\n";
}
double eigenvalue = eigen_values[ position_of_largest_eigenvalue ];
double prod = M_model->scalarProductForPod( modei , modei );
{
CHECK( (prod - eigenvalue) < 1e-12 )<<"scalar product between mode "<<i<<" and mode "<<i<<" is "<<prod<<" and associated eigenvalue : "<<eigenvalue<<" ( i = "<<i<<") \n";
}
if( i < max_idx.size() )
position_of_largest_eigenvalue=max_idx[i];
}
}
if ( M_store_pod_matrix_format_octave )
{
std::cout<<"M_store_pod_matrix_format_octave = "<<M_store_pod_matrix_format_octave<<std::endl;
std::ofstream eigenvalue_file;
eigenvalue_file.open( "eigen_values.mat",std::ios::out );
eigenvalue_file<<"# name: E\n";
eigenvalue_file<<"# type: matrix\n";
eigenvalue_file<<"# rows: "<<number_of_eigenvalues<<"\n";
eigenvalue_file<<"# columns: 1\n";
for ( int i=0; i<number_of_eigenvalues; i++ )
{
eigenvalue_file<<std::setprecision( 16 )<<eigen_values[i]<<"\n";
}
eigenvalue_file.close();
std::ofstream eigenvector_file( ( boost::format( "eigen_vectors" ) ).str().c_str() );
for ( int j=0; j<M_pod_matrix.cols(); j++ )
{
for ( int i=0; i<number_of_eigenvalues; i++ )
{
eigenvector_file<<std::setprecision( 16 )<<eigen_solver.eigenvectors().col( i )[j] <<" ";
}
eigenvector_file<<"\n";
}
eigenvector_file.close();
for ( int i=0; i<M_Nm; i++ )
{
std::ofstream mode_file( ( boost::format( "mode_%1%" ) %i ).str().c_str() );
element_type e = ModeSet[i];
for ( size_type j=0; j<e.size(); j++ ) mode_file<<std::setprecision( 16 )<<e( j )<<"\n";
mode_file.close();
}
}
return M_Nm;
}
