// @from http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00173.html
static bool pinv(const Eigen::Matrix2d& a, Eigen::Matrix2d& a_pinv)
{
// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
if (a.rows() < a.cols())
return false;
// SVD
Eigen::JacobiSVD<Eigen::Matrix2d> svdA(a, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector2d vSingular = svdA.singularValues();
// Build a diagonal matrix with the Inverted Singular values
// The pseudo inverted singular matrix is easy to compute :
// is formed by replacing every nonzero entry by its reciprocal (inversing).
Eigen::Vector2d vPseudoInvertedSingular(svdA.matrixV().cols(),1);
for (int iRow = 0; iRow < vSingular.rows(); iRow++)
{
if (fabs(vSingular(iRow)) <= 1e-10) // Todo : Put epsilon in parameter
{
vPseudoInvertedSingular(iRow, 0) = 0.;
}
else
{
vPseudoInvertedSingular(iRow, 0) = 1. / vSingular(iRow);
}
}
// A little optimization here
Eigen::Matrix2d mAdjointU = svdA.matrixU().adjoint().block(0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols());
// Pseudo-Inversion : V * S * U'
a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
return true;
}
/** Compute fundamental matrix out of eight feature correspondences between the previous and current frame.
* @param std::vector<Match> vector with feature matches
* @param std::vector<int> vector with eight indices of the vector with matches
* @return Eigen::Matrix3f fundamental matrix */
Eigen::Matrix3f MonoOdometer5::getF(std::vector<Match> matches, std::vector<int> indices)
{
int N = indices.size();
// see Trucco & Verri, Introductory Techniques for 3D Computer Vision, chapter 6
Eigen::Matrix<float, Eigen::Dynamic, 9> A;
A.resize(N, 9);
for(int i=0; i<N; i++)
{
cv::Point2f pPrev = matches[indices[i]].pPrev_;
cv::Point2f pCurr = matches[indices[i]].pCurr_;
A(i, 0) = pCurr.x * pPrev.x;
A(i, 1) = pCurr.x * pPrev.y;
A(i, 2) = pCurr.x;
A(i, 3) = pCurr.y * pPrev.x;
A(i, 4) = pCurr.y * pPrev.y;
A(i, 5) = pCurr.y;
A(i, 6) = pPrev.x;
A(i, 7) = pPrev.y;
A(i, 8) = 1;
}
Eigen::JacobiSVD<Eigen::Matrix<float, Eigen::Dynamic, 9>> svdA(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f F;
F << svdA.matrixV()(0, 8), svdA.matrixV()(1, 8), svdA.matrixV()(2, 8), svdA.matrixV()(3, 8), svdA.matrixV()(4, 8), svdA.matrixV()(5, 8), svdA.matrixV()(6, 8), svdA.matrixV()(7, 8), svdA.matrixV()(8, 8);
// re-enforce rank 2 constraint on fundamental matrix
Eigen::JacobiSVD<Eigen::Matrix3f> svdF(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f D(Eigen::Matrix3f::Zero());
D(0, 0) = svdF.singularValues()(0);
D(1, 1) = svdF.singularValues()(1);
F = (svdF.matrixU()) * D * (svdF.matrixV()).transpose();
return F;
}
bool mitk::PivotCalibration::ComputePivotPoint()
{
double defaultThreshold = 1e-1;
std::vector<mitk::NavigationData::Pointer> _CheckedTransforms;
for (size_t i = 0; i < m_NavigationDatas.size(); ++i)
{
if (!m_NavigationDatas.at(i)->IsDataValid())
{
MITK_WARN << "Skipping invalid transform " << i << ".";
continue;
}
_CheckedTransforms.push_back(m_NavigationDatas.at(i));
}
if (_CheckedTransforms.empty())
{
MITK_WARN << "Checked Transforms are empty";
return false;
}
unsigned int rows = 3 * _CheckedTransforms.size();
unsigned int columns = 6;
vnl_matrix< double > A(rows, columns), minusI(3, 3, 0), R(3, 3);
vnl_vector< double > b(rows), x(columns), t(3);
minusI(0, 0) = -1;
minusI(1, 1) = -1;
minusI(2, 2) = -1;
//do the computation and set the internal variables
unsigned int currentRow = 0;
for (size_t i = 0; i < _CheckedTransforms.size(); ++i)
{
t = _CheckedTransforms.at(i)->GetPosition().GetVnlVector();// t = the current position of the tracked sensor
t *= -1;
b.update(t, currentRow); //b = combines the position for each collected transform in one column vector
R = _CheckedTransforms.at(i)->GetOrientation().rotation_matrix_transpose().transpose(); // R = the current rotation of the tracked sensor, *rotation_matrix_transpose().transpose() is used to obtain original matrix
A.update(R, currentRow, 0); //A = the matrix which stores the rotations for each collected transform and -I
A.update(minusI, currentRow, 3);
currentRow += 3;
}
vnl_svd<double> svdA(A); //The singular value decomposition of matrix A
svdA.zero_out_absolute(defaultThreshold);
//there is a solution only if rank(A)=6 (columns are linearly
//independent)
if (svdA.rank() < 6)
{
MITK_WARN << "svdA.rank() < 6";
return false;
}
else
{
x = svdA.solve(b); //x = the resulting pivot point
m_ResultRMSError = (A * x - b).rms(); //the root mean sqaure error of the computation
//sets the Pivot Point
m_ResultPivotPoint[0] = x[0];
m_ResultPivotPoint[1] = x[1];
m_ResultPivotPoint[2] = x[2];
}
return true;
}
Foam::chemPointISAT<CompType, ThermoType>::chemPointISAT
(
TDACChemistryModel<CompType, ThermoType>& chemistry,
const scalarField& phi,
const scalarField& Rphi,
const scalarSquareMatrix& A,
const scalarField& scaleFactor,
const scalar& tolerance,
const label& completeSpaceSize,
const dictionary& coeffsDict,
binaryNode<CompType, ThermoType>* node
)
:
chemistry_(chemistry),
phi_(phi),
Rphi_(Rphi),
A_(A),
scaleFactor_(scaleFactor),
node_(node),
completeSpaceSize_(completeSpaceSize),
nGrowth_(0),
nActiveSpecies_(chemistry.mechRed()->NsSimp()),
simplifiedToCompleteIndex_(nActiveSpecies_),
timeTag_(chemistry_.timeSteps()),
lastTimeUsed_(chemistry_.timeSteps()),
toRemove_(false),
maxNumNewDim_(coeffsDict.lookupOrDefault("maxNumNewDim",0)),
printProportion_(coeffsDict.lookupOrDefault("printProportion",false)),
numRetrieve_(0),
nLifeTime_(0),
completeToSimplifiedIndex_
(
completeSpaceSize - (2 + (variableTimeStep() == 1 ? 1 : 0))
)
{
tolerance_ = tolerance;
if (variableTimeStep())
{
nAdditionalEqns_ = 3;
iddeltaT_ = completeSpaceSize - 1;
scaleFactor_[iddeltaT_] *= phi_[iddeltaT_] / tolerance_;
}
else
{
nAdditionalEqns_ = 2;
iddeltaT_ = completeSpaceSize; // will not be used
}
idT_ = completeSpaceSize - nAdditionalEqns_;
idp_ = completeSpaceSize - nAdditionalEqns_ + 1;
bool isMechRedActive = chemistry_.mechRed()->active();
if (isMechRedActive)
{
for (label i=0; i<completeSpaceSize-nAdditionalEqns_; i++)
{
completeToSimplifiedIndex_[i] =
chemistry.completeToSimplifiedIndex()[i];
}
for (label i=0; i<nActiveSpecies_; i++)
{
simplifiedToCompleteIndex_[i] =
chemistry.simplifiedToCompleteIndex()[i];
}
}
label reduOrCompDim = completeSpaceSize;
if (isMechRedActive)
{
reduOrCompDim = nActiveSpecies_+nAdditionalEqns_;
}
// SVD decomposition A = U*D*V^T
SVD svdA(A);
scalarDiagonalMatrix D(reduOrCompDim);
const scalarDiagonalMatrix& S = svdA.S();
// Replace the value of vector D by max(D, 1/2), first ISAT paper
for (label i=0; i<reduOrCompDim; i++)
{
D[i] = max(S[i], 0.5);
}
// Rebuild A with max length, tol and scale factor before QR decomposition
scalarRectangularMatrix Atilde(reduOrCompDim);
// Result stored in Atilde
multiply(Atilde, svdA.U(), D, svdA.V().T());
for (label i=0; i<reduOrCompDim; i++)
{
for (label j=0; j<reduOrCompDim; j++)
{
label compi = i;
if (isMechRedActive)
{
compi = simplifiedToCompleteIndex(i);
}
// SF*A/tolerance
// (where SF is diagonal with inverse of scale factors)
// SF*A is the same as dividing each line by the scale factor
// corresponding to the species of this line
Atilde(i, j) /= (tolerance*scaleFactor[compi]);
}
}
// The object LT_ (the transpose of the Q) describe the EOA, since we have
// A^T B^T B A that should be factorized into L Q^T Q L^T and is set in the
// qrDecompose function
LT_ = scalarSquareMatrix(Atilde);
qrDecompose(reduOrCompDim, LT_);
}
/*
Description:
*/
bool PivotCalibration2::ComputePivotCalibration()
{
if (this->ToolToReferenceMatrices.size() < 10)
{
this->ErrorText = "Not enough input transforms are available";
return false;
}
if (this->GetMaximumToolOrientationDifferenceDeg() < this->MinimumOrientationDifferenceDeg)
{
this->ErrorText = "Not enough variation in the input transforms";
return false;
}
unsigned int rows = 3 * this->ToolToReferenceMatrices.size();
unsigned int columns = 6;
vnl_matrix<double> A(rows, columns);
vnl_matrix<double> minusI(3, 3, 0);
minusI(0, 0) = -1;
minusI(1, 1) = -1;
minusI(2, 2) = -1;
vnl_matrix<double> R(3, 3);
vnl_vector<double> b(rows);
vnl_vector<double> x(columns);
vnl_vector<double> t(3);
std::vector<vtkSmartPointer<vtkMatrix4x4> >::const_iterator it;
std::vector<vtkSmartPointer<vtkMatrix4x4> >::const_iterator matricesEnd = this->ToolToReferenceMatrices.end();
unsigned int currentRow;
vtkMatrix4x4* referenceOrientationMatrix = this->ToolToReferenceMatrices.front();
for (currentRow = 0, it = this->ToolToReferenceMatrices.begin(); it != matricesEnd; it++, currentRow += 3)
{
for (int i = 0; i < 3; i++)
{
t(i) = (*it)->GetElement(i, 3);
}
t *= -1;
b.update(t, currentRow);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R(i, j) = (*it)->GetElement(i, j);
}
}
A.update(R, currentRow, 0);
A.update(minusI, currentRow, 3);
}
vnl_svd<double> svdA(A);
svdA.zero_out_absolute(1e-1);
x = svdA.solve(b);
//set the RMSE
this->PivotRMSE = (A * x - b).rms();
//set the transformation
this->ToolTipToToolMatrix->SetElement(0, 3, x[0]);
this->ToolTipToToolMatrix->SetElement(1, 3, x[1]);
this->ToolTipToToolMatrix->SetElement(2, 3, x[2]);
this->ErrorText.empty();
return true;
}
void Lu::abskernel(MatrixXf P, MatrixXf Q, std::vector<Matrix3f > F, MatrixXf G){
int n = P.cols();
int i;
//std::cout<<"P\n"<<P<<std::endl;
//std::cout<<"Q\n"<<Q<<std::endl;
//std::cout<<"-------------\n"<<std::endl;
for( i=0; i<n;i++){
Q.col(i)=F[i]*Q.col(i);
// std::cout<<"F\n"<<F[i]*Q.col(i)<<std::endl;
}
//std::cout<<"Q\n"<<Q<<std::endl;
// compute P' and Q'
Vector3f pbar;
Vector3f qbar;
Vector3f P_sum;
Vector3f Q_sum;
P_sum= Vector3f::Zero();
Q_sum= Vector3f::Zero();
for( i=0; i<n;i++){
P_sum = P_sum+P.col(i);
Q_sum = Q_sum+Q.col(i);
}
pbar=P_sum/n;
qbar=Q_sum/n;
// std::cout<<"pbar\n"<<pbar<<std::endl;
// std::cout<<"qbar\n"<<qbar<<std::endl;
for( i=0; i<n;i++){
P.col(i)=P.col(i)-pbar;
Q.col(i)=Q.col(i)-qbar;
}
//std::cout<<"P\n"<<P<<std::endl;
//std::cout<<"Q\n"<<Q<<std::endl;
/////////////use SVM method
Matrix3f M=Matrix3f::Zero();
Vector3f vq;
Vector3f vp;
Matrix3f Vs;
for( i=0; i<n;i++){
vq=Q.col(i);
vp=P.col(i);
Vs << vp[0]*vq[0],vp[0]*vq[1],vp[0]*vq[2],
vp[1]*vq[0],vp[1]*vq[1],vp[1]*vq[2],
vp[2]*vq[0],vp[2]*vq[1],vp[2]*vq[2];
M = M+Vs;
}
//std::cout<<"M\n"<<M<<std::endl;
//Matrix
//M.svd().compute();
JacobiSVD<Matrix3f> svdA(M,ComputeFullU | ComputeFullV);
//svdA.compute();
Matrix3f U=svdA.matrixU();
Matrix3f V=svdA.matrixV();
Ri=V*U.transpose();
//std::cout<<"U\n"<<U<<std::endl;
//std::cout<<"V\n"<<V<<std::endl;
//std::cout<<"Ri\n"<<Ri<<std::endl;
if (sgn(Ri.determinant())==1.0){
ti= estimate_t( Ri,G,F,P,n );
//std::cout<<"ti\n"<<ti<<std::endl;
if (ti(2)<0){
//R=-[V(:,1:2) -V(:,3)]*U.';
Matrix3f Va;
Va<<V(0,0),V(0,1),-V(0,2),
V(1,0),V(1,1),-V(1,2),
V(2,0),V(2,1),-V(2,2);
Ri=-Va*U.transpose();
ti = estimate_t( Ri,G,F,P,n );
}
}else{
Matrix3f Va;
Va<<V(0,0),V(0,1),-V(0,2),
V(1,0),V(1,1),-V(1,2),
V(2,0),V(2,1),-V(2,2);
// std::cout<<"Va\n"<<ti<<std::endl;
//R=[V(:,1:2) -V(:,3)]*U.';
Ri=Va*U.transpose();
ti = estimate_t( Ri,G,F,P,n );
if (ti(2) < 0){
//disp(['t_3 = ' num2str(t(3)) ]);
// we need to invert the t
Ri =- V*U.transpose();
ti = estimate_t( Ri,G,F,P,n );
}
}
// std::cout<<"ti\n"<<ti<<std::endl;
Qout = xform(P, Ri, ti);
// std::cout<<"Qout\n"<<Qout<<std::endl;
// calculate error
err = 0;
Vector3f vec;
for (i = 0 ;i< n; i++){
vec = (Matrix3f::Identity()-F[i])*Qout.col(i);
//std::cout<<"vec\n"<<vec<<std::endl;
err = err + vec.dot(vec);
}
//std::cout<<"err\n"<<err<<std::endl;
}