float OptSO3::linesearch(Matrix3f& R, Matrix3f& M_t_min, const Matrix3f& H,
float N, float t_max, float dt)
{
Matrix3f A = R.transpose() * H;
EigenSolver<MatrixXf> eig(A);
MatrixXcf U = eig.eigenvectors();
MatrixXcf invU = U.inverse();
VectorXcf d = eig.eigenvalues();
#ifndef NDEBUG
cout<<"A"<<endl<<A<<endl;
cout<<"U"<<endl<<U<<endl;
cout<<"d"<<endl<<d<<endl;
#endif
Matrix3f R_t_min=R;
float f_t_min = 999999.0f;
float t_min = 0.0f;
//for(int i_t =0; i_t<10; i_t++)
for(float t =0.0f; t<t_max; t+=dt)
{
//float t= ts[i_t];
VectorXcf expD = ((d*t).array().exp());
MatrixXf MN = (U*expD.asDiagonal()*invU).real();
Matrix3f R_t = R*MN.topLeftCorner(3,3);
float detR = R_t.determinant();
float maxDeviationFromI = ((R_t*R_t.transpose()
- Matrix3f::Identity()).cwiseAbs()).maxCoeff();
if ((R_t(0,0)==R_t(0,0))
&& (abs(detR-1.0f)< 1e-2)
&& (maxDeviationFromI <1e-1))
{
float f_t = evalCostFunction(R_t)/float(N);
#ifndef NDEBUG
cout<< " f_t = "<<f_t<<endl;
#endif
if (f_t_min > f_t && f_t != 0.0f)
{
R_t_min = R_t;
M_t_min = MN.topLeftCorner(3,3);
f_t_min = f_t;
t_min = t;
}
}else{
cout<<"R_t is corruputed detR="<<detR
<<"; max deviation from I="<<maxDeviationFromI
<<"; nans? "<<R_t(0,0)<<" f_t_min="<<f_t_min<<endl;
}
}
if(f_t_min == 999999.0f) return f_t_min;
// case where the MN is nans
R = R_t_min;
#ifndef NDEBUG
#endif
cout<<"R: det(R) = "<<R.determinant()<<endl<<R<<endl;
cout<< "t_min="<<t_min<<" f_t_min="<<f_t_min<<endl;
return f_t_min;
}
void NeighbourJoining::findMinQ(const MatrixXf& Q, const MatrixXi& rowsID, Pair& p) {
Q.topLeftCorner(numCurrentNodes, numCurrentNodes).minCoeff(&pair.i,
&pair.j);
pair.iID = rowsID(pair.i);
pair.jID = rowsID(pair.j);
//cout << " closestPair: " << pair.i << ", " << pair.j << endl;
//cout << " closestPairID: " << pair.iID << ", " << pair.jID << endl;
}
void NeighbourJoining::calcQ(const MatrixXf& currentD, MatrixXf& Q) {
//calculates sum of the rows of the distance matrix
Matrix<float, latentNodes, 1> sumRows;
//MatrixXf sumRows (numObservableNodes+1,1) ;
sumRows.head(numCurrentNodes) = currentD.topLeftCorner(numCurrentNodes,
numCurrentNodes).colwise().sum();
//cout << sumRows.head(numCurrentNodes); cout << endl;
//Q = (n-2) * currentD
Q.topLeftCorner(numCurrentNodes, numCurrentNodes) = (numCurrentNodes - 2)
* currentD.topLeftCorner(numCurrentNodes, numCurrentNodes);
//each row in Q - sumRows and each column in Q - sumRows
Q.topLeftCorner(numCurrentNodes, numCurrentNodes).colwise() -= sumRows.head(
numCurrentNodes);
Q.topLeftCorner(numCurrentNodes, numCurrentNodes).rowwise() -= sumRows.head(
numCurrentNodes).transpose();
Q.diagonal().setZero();
//cout << "Q Matrix:" << endl; printMatrix(Q);
}
void NeighbourJoining::printMatrix(const MatrixXf& M) {
cout << M.topLeftCorner(numCurrentNodes, numCurrentNodes);
cout << endl;
}