Eigen::MatrixXd MyMath::my_Rv_2_RM( Eigen::MatrixXd rvvv ) // rv(3,1); // Exponential Map - so(3)=>SO(3) // IGNORE T !!!!!!!!!!!!!!!!!! (zero pitch screw)
{
Eigen::MatrixXd RRR(3,3);
double theta = rvvv.norm();
Eigen::MatrixXd I3 = Eigen::MatrixXd::Identity(3,3);
Eigen::MatrixXd W( 3,1 );
Eigen::MatrixXd W_hat( 3,3 ); // Wmega Hat
double thresh_shouldBeZero = 0.00009;
if (theta < (double)thresh_shouldBeZero)
{
RRR = I3;
}
else
{
W = rvvv / theta;
W_hat << 0, -W(2), W(1), // Wmega Hat
W(2), 0, -W(0),
-W(1), W(0), 0;
RRR = I3 + W_hat*(double)qSin(theta) + (W_hat*W_hat)*(double)(1.0-qCos(theta)); // Rodrigues!!!
}
///////////
return RRR;
///////////
}
void createCostMatrix()
{
int reps = 1; // 10 = 1.71s / 1.8s (0.17 / 0.18 seconds for dense matrix mult)
//ros::WallTime start = ros::WallTime::now();
{
boost::progress_timer t;
for (int i=0; i<reps; ++i)
dense_R = dense_A.transpose() * dense_A;
printf("%d reps of dense matrix multiplication: ", reps);
}
reps = 1; // 10000 = 0.5s / 0.83s (50us / 83us for sparse matrix mult)
{
boost::progress_timer t;
for (int i=0; i<reps; ++i)
sparse_R = sparse_A.transpose() * sparse_A;
printf("%d reps of sparse matrix multiplication: ", reps);
}
Eigen::MatrixXd temp = sparse_R;
Eigen::MatrixXd diff = dense_R - temp;
printf("Error norm = %f\n", diff.norm());
}
void BranchList::calculateOrientations()
{
for (int i = 0; i < mBranches.size(); i++)
{
Eigen::MatrixXd positions = mBranches[i]->getPositions();
Eigen::MatrixXd diff = positions.rightCols(positions.cols() - 1) - positions.leftCols(positions.cols() - 1);
Eigen::MatrixXd orientations(positions.rows(),positions.cols());
orientations.leftCols(orientations.cols() - 1) = diff / diff.norm();
orientations.rightCols(1) = orientations.col(orientations.cols() - 1);
mBranches[i]->setOrientations(orientations);
}
}
void analyzeRank(const std::string inputMatrixFileName,const std::string inputGraphFileName,const std::string outputFileName,const int input_Min_i,const int input_Min_j, const int input_NumRows,const int input_NumCols,std::string mode){
Eigen::MatrixXd inputMatrix = readBinaryIntoMatrixXd(inputMatrixFileName);
Eigen::SparseMatrix<double> inputGraph = readMtxIntoSparseMatrix(inputGraphFileName);
int min_i,min_j,numRows,numCols;
int matrixSize = inputMatrix.rows();
if (mode == "topOffDiag"){
int split = matrixSize/2;
min_i = 0;
min_j = split + 1;
numRows = split + 1;
numCols = matrixSize - split - 1;
} else if (mode == "bottOffDiag"){
int split = matrixSize/2;
min_i = split + 1;
min_j = 0;
numRows = matrixSize - split - 1;
numCols = split + 1;
}else{
min_i = input_Min_i;
min_j = input_Min_j;
numRows = input_NumRows;
numCols = input_NumCols;
}
int currRank = 0;
int currRankComp1 = 0;
int currRankComp3 = 0;
int currRankComp5 = 0;
int nextRank = -1;
int depth = 0;
Eigen::MatrixXd currBlock = inputMatrix.block(min_i,min_j,numRows,numCols);
Eigen::MatrixXd U,V;
std::vector<double> numPoints,boundaryError,SVDError,singularValues;
std::vector<double> boundaryErrorComp1,boundaryErrorComp3,boundaryErrorComp5;
while (currRank != nextRank){
nextRank = currRank;
PS_Boundary_LowRankApprox(inputMatrix,inputGraph,U,V,min_i,min_j,numRows,numCols,1e-15,currRank,depth);
numPoints.push_back(currRank);
double relError = (U * V.transpose() - currBlock).norm()/currBlock.norm();
boundaryError.push_back(relError);
PS_Boundary_LowRankApprox(inputMatrix,inputGraph,U,V,min_i,min_j,numRows,numCols,1e-1,currRankComp1,depth);
relError = (U * V.transpose() - currBlock).norm()/currBlock.norm();
boundaryErrorComp1.push_back(relError);
PS_Boundary_LowRankApprox(inputMatrix,inputGraph,U,V,min_i,min_j,numRows,numCols,1e-3,currRankComp3,depth);
relError = (U * V.transpose() - currBlock).norm()/currBlock.norm();
boundaryErrorComp3.push_back(relError);
PS_Boundary_LowRankApprox(inputMatrix,inputGraph,U,V,min_i,min_j,numRows,numCols,1e-5,currRankComp5,depth);
relError = (U * V.transpose() - currBlock).norm()/currBlock.norm();
boundaryErrorComp5.push_back(relError);
depth ++;
}
saveVectorAsText(outputFileName + "numPointsVsBoundaryDistance",numPoints);
saveVectorAsText(outputFileName + "boundaryErrorVsBoundaryDistance",boundaryError);
saveVectorAsText(outputFileName + "boundaryErrorCompVsBoundaryDistance1",boundaryErrorComp1);
saveVectorAsText(outputFileName + "boundaryErrorCompVsBoundaryDistance3",boundaryErrorComp3);
saveVectorAsText(outputFileName + "boundaryErrorCompVsBoundaryDistance5",boundaryErrorComp5);
// Use svd to calculate the optimal low-rank approximation error
Eigen::JacobiSVD<Eigen::MatrixXd> svd(currBlock,Eigen::ComputeThinU|Eigen::ComputeThinV);
Eigen::VectorXd s = svd.singularValues();
for (unsigned int i = 0; i < s.rows(); i++)
singularValues.push_back(s(i));
for (unsigned int i = 0; i < numPoints.size(); i++){
int rank = numPoints[i];
U = svd.matrixU().leftCols(rank);
V = svd.matrixV().leftCols(rank);
Eigen::MatrixXd K = Eigen::MatrixXd::Zero(rank,rank);
for (int j = 0; j < rank; j++)
K(j,j) = singularValues[j];
double relError = (U * K * V.transpose() - currBlock).norm()/currBlock.norm();
SVDError.push_back(relError);
}
saveVectorAsText(outputFileName + "SVDErrorVsBoundaryDistance",SVDError);
saveVectorAsText(outputFileName + "SingularValueDecay",singularValues);
// Calculate distance from boundary vs pivot size
PS_Boundary_LowRankApprox(inputMatrix,inputGraph,U,V,min_i,min_j,numRows,numCols,1e-15,currRank,matrixSize,outputFileName + "distanceFromBoundaryVsPivotSize");
}