C++ MatrixXd::norm方法代码示例

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");

}