C++ SparseMatrix::coeff方法代码示例

void compare(const Physika::SparseMatrix<mytype> &a, const Eigen::SparseMatrix<mytype> &b)
{
    /*if (a.nonZeros() != b.nonZeros())
    {
        cout<<"a.nonzeros:"<<a.nonZeros()<<endl;
        cout << "b.nonZeros:" << b.nonZeros() << endl;
        cout << "correctness bad!" << endl;
        return;
    }*/
        std::vector<Physika::Trituple<mytype>> v;
        bool correctness = true;
        for(unsigned int i = 0;i<a.rows();++i)
        {
            v = a.getRowElements(i);
            for(unsigned int j=0;j<v.size();++j)
            {
                int row = v[j].row(), col = v[j].col();
                mytype value = v[j].value();
                if(b.coeff(row,col) != value)
                {
                    cout<<"eror: "<<row<<' '<<col<<" value: psm "<<value<<" "<<b.coeff(row,col)<<endl;
                    correctness = false;
                    break;
                }
            }
        }
		for (unsigned int i = 0; i < b.outerSize();++i)
           for (Eigen::SparseMatrix<mytype>::InnerIterator it(b, i); it; ++it)
            {
                if (it.value() != a(it.row(), it.col()))
                {
                    cout << "eror: " << it.row() << ' ' << it.col() << " value: psm " << a(it.row(),it.col()) << " " << it.value() << endl;
                    correctness = false;
                    break;
                }
            }
        if(correctness) cout<<"correctness OK!"<<endl;
        else cout<<"correctness bad!"<<endl;
}

void checkPseudoDerivatives(const Eigen::SparseMatrix<double> &dampingPseudoDerivatives, const EnergyCondition<double> &c, const Eigen::VectorXd &uv, Eigen::VectorXd &x, Eigen::VectorXd &v, double k, double d, double dx, double tol)
{
	Eigen::VectorXd dCdt = numericalCTimeDerivative(c, x, v, uv, dx);

	for (int i = 0; i < x.size(); ++i)
	{
		for (int j = 0; j < x.size(); ++j)
		{
			Eigen::VectorXd d2Cdidj = numericalSecondCDerivative(c, x, uv, i, j, dx);
			double expectedCoeff = -d * (d2Cdidj.array() * dCdt.array()).sum();
			double actualCoeff = dampingPseudoDerivatives.coeff(i, j);
			Microsoft::VisualStudio::CppUnitTestFramework::Assert::AreEqual(actualCoeff, expectedCoeff, tol);
		}
	}
}

    void NewtonIterationBlackoilInterleaved::formInterleavedSystem(const std::vector<ADB>& eqs,
                                                                   const Eigen::SparseMatrix<double, Eigen::RowMajor>& A,
                                                                   Mat& istlA) const
    {
        const int np = eqs.size();

        // Find sparsity structure as union of basic block sparsity structures,
        // corresponding to the jacobians with respect to pressure.
        // Use addition to get to the union structure.
        Eigen::SparseMatrix<double> structure = eqs[0].derivative()[0];
        for (int phase = 0; phase < np; ++phase) {
            structure += eqs[phase].derivative()[0];
        }
        Eigen::SparseMatrix<double, Eigen::RowMajor> s = structure;

        // Create ISTL matrix with interleaved rows and columns (block structured).
        assert(np == 3);
        istlA.setSize(s.rows(), s.cols(), s.nonZeros());
        istlA.setBuildMode(Mat::row_wise);
        const int* ia = s.outerIndexPtr();
        const int* ja = s.innerIndexPtr();
        for (Mat::CreateIterator row = istlA.createbegin(); row != istlA.createend(); ++row) {
            int ri = row.index();
            for (int i = ia[ri]; i < ia[ri + 1]; ++i) {
                row.insert(ja[i]);
            }
        }
        const int size = s.rows();
        Span span[3] = { Span(size, 1, 0),
                         Span(size, 1, size),
                         Span(size, 1, 2*size) };
        for (int row = 0; row < size; ++row) {
            for (int col_ix = ia[row]; col_ix < ia[row + 1]; ++col_ix) {
                const int col = ja[col_ix];
                MatrixBlockType block;
                for (int p1 = 0; p1 < np; ++p1) {
                    for (int p2 = 0; p2 < np; ++p2) {
                        block[p1][p2] = A.coeff(span[p1][row], span[p2][col]);
                    }
                }
                istlA[row][col] = block;
            }
        }
    }

Eigen::SparseMatrix<double> joint2conditional(Eigen::SparseMatrix<double> edgePot)// pa is the second dimension
{	// second dimension of edgePot is the parent
	Eigen::SparseMatrix<double> Conditional;
	Conditional.resize(edgePot.rows(), edgePot.cols());

	Eigen::SparseVector<double> Parent_Marginal;
	Parent_Marginal.resize(edgePot.cols());
	for (int id_col = 0; id_col < edgePot.cols(); id_col++)
	{
		Eigen::SparseVector<double> tmp_vec = edgePot.block(0, id_col, edgePot.rows(), 1);
		Parent_Marginal.coeffRef(id_col) = tmp_vec.sum();
		if (Parent_Marginal.coeff(id_col)>TOLERANCE)
			for (int id_row = 0; id_row < edgePot.rows(); id_row++)
			{
				Conditional.coeffRef(id_row, id_col) = edgePot.coeff(id_row, id_col) / Parent_Marginal.coeff(id_col);
			}
	}
	Conditional.makeCompressed();
	Conditional.prune(TOLERANCE);
	return Conditional;
}

IGL_INLINE void igl::PolyVectorFieldFinder<DerivedV, DerivedF>::
minQuadWithKnownMini(const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &Q,
                          const Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > &f,
                     const Eigen::VectorXi isConstrained,
                          const Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &xknown,
                          Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic, 1> &x)
{
  int N = Q.rows();

  int nc = xknown.rows();
  Eigen::VectorXi known; known.setZero(nc,1);
  Eigen::VectorXi unknown; unknown.setZero(N-nc,1);

  int indk = 0, indu = 0;
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
    {
      known[indk] = i;
      indk++;
    }
    else
    {
      unknown[indu] = i;
      indu++;
    }

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>> Quu, Quk;

  igl::slice(Q,unknown, unknown, Quu);
  igl::slice(Q,unknown, known, Quk);


  std::vector<typename Eigen::Triplet<std::complex<typename DerivedV::Scalar> > > tripletList;

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > fu(N-nc,1);

  igl::slice(f,unknown, Eigen::VectorXi::Zero(1,1), fu);

  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > rhs = (Quk*xknown).sparseView()+.5*fu;

  Eigen::SparseLU< Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>> solver;
  solver.compute(-Quu);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Decomposition failed!"<<std::endl;
    return;
  }
  Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar>>  b  = solver.solve(rhs);
  if(solver.info()!=Eigen::Success)
  {
    std::cerr<<"Solving failed!"<<std::endl;
    return;
  }

  indk = 0, indu = 0;
  x.setZero(N,1);
  for (int i = 0; i<N; ++i)
    if (isConstrained[i])
      x[i] = xknown[indk++];
    else
      x[i] = b.coeff(indu++,0);

}

//.........这里部分代码省略.........
			pinvA.setZero();
		}
		else
		{
			Eigen::Matrix<double,7,Eigen::Dynamic> transposeA = matrixA.transpose();
			pinvA = (transposeA * matrixA).inverse()*transposeA;
		}

#ifdef MY_DEBUG
		cout << "MatrixA" << endl << matrixA << endl << endl << "PinvA:" << endl;
		cout << pinvA << endl;
		ofstream sqare("transposeAmatrixA.txt");
		sqare << (transposeA * matrixA) << endl << endl;
		sqare << (transposeA * matrixA).inverse() << endl;
		sqare.close();
#endif // MY_DEBUG

		Eigen::Matrix<double,1,Eigen::Dynamic> s = pinvA.row(0);
		Eigen::Matrix<double,1,Eigen::Dynamic> h1 = pinvA.row(1);
		Eigen::Matrix<double,1,Eigen::Dynamic> h2 = pinvA.row(2);
		Eigen::Matrix<double,1,Eigen::Dynamic> h3 = pinvA.row(3);

		Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaX = s*deltaInput(i,0)-h3*deltaInput(i,1)+h2*deltaInput(i,2);
		Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaY = h3*deltaInput(i,0)+s*deltaInput(i,1)-h1*deltaInput(i,2);
		Eigen::Matrix<double,1,Eigen::Dynamic> TDeltaZ = -h2*deltaInput(i,0)-h1*deltaInput(i,1)+s*deltaInput(i,2);

		Eigen::Matrix<double,3,Eigen::Dynamic> TDelta;
		TDelta.resize(3,TDeltaX.cols());
		TDelta.row(0) = TDeltaX; TDelta.row(1) = TDeltaY; TDelta.row(2) = TDeltaZ;

		for (int j = 0; j < 3; j++)
		{
			int opRow = j*mMeshVertexCount+i;
			laplacianOperator3D.insert(opRow,i) = j == 0 ? -TDelta(j,0) + laplacianOperator.coeff(i,i) : -TDelta(j,0);
			laplacianOperator3D.insert(opRow,i+mMeshVertexCount) = j == 1 ? -TDelta(j,1) + laplacianOperator.coeff(i,i) : -TDelta(j,1);
			laplacianOperator3D.insert(opRow,i+2*mMeshVertexCount) = j == 2 ? -TDelta(j,2) + laplacianOperator.coeff(i,i) : -TDelta(j,2);
			int curTDeltaCol = 3;
			for (int a = 0; a < adjIndex.size(); a++)
			{
				int adj = adjIndex.at(a);
				laplacianOperator3D.insert(opRow,adj) = j == 0 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++);
				laplacianOperator3D.insert(opRow,adj+mMeshVertexCount) = j == 1 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) :  -TDelta(j,curTDeltaCol++);
				laplacianOperator3D.insert(opRow,adj+2*mMeshVertexCount) = j == 2 ? -TDelta(j,curTDeltaCol++) + laplacianOperator.coeff(i,adj) : -TDelta(j,curTDeltaCol++);
			}
		}		
	}
#ifdef MY_DEBUG
	ofstream outL3d("laplacianOperator3D.txt");
	outL3d << laplacianOperator3D << endl;
	outL3d.close();
#endif


	int mMeshVertexCount_X3 = 3*mMeshVertexCount;
	Eigen::SparseMatrix<double>& A_prime = laplacianOperator3D;	
//	printMatrix(laplacianOperator3D,"laplacianOperator3D");
//	printMatrix(A_prime,"A_prime");

	int offset = 0;
	for(int j = 0; j < mMeshVertexCount_X3; j+=3)
	{
		A_prime.insert(mMeshVertexCount_X3+j,offset) = 1;
		A_prime.insert(mMeshVertexCount_X3+j+1,offset+mMeshVertexCount) = 1;
		A_prime.insert(mMeshVertexCount_X3+j+2,offset+2*mMeshVertexCount) = 1;
		offset++;
	}

  void
  sba_process_impl(const Eigen::SparseMatrix<int> &x, const Eigen::SparseMatrix<int> & y,
                   const Eigen::SparseMatrix<int> &disparity, const Eigen::Matrix3d & K,
                   const VectorQuaterniond & quaternions, const VectorVector3d & Ts,
                   const VectorVector3d & in_point_estimates, VectorVector3d & out_point_estimates)
  {
    g2o::SparseOptimizer optimizer;
    optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
    optimizer.setVerbose(false);
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    if (0)
    {
      linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
    }
    else
    {
      linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    }

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(&optimizer, linearSolver);

    optimizer.setSolver(solver_ptr);

    double baseline = 0.075; // 7.5 cm baseline

    // set up camera params
    g2o::VertexSCam::setKcam(K(0, 0), K(1, 1), K(0, 2), K(1, 2), baseline);

    // set up the camera vertices
    unsigned int vertex_id = 0;
    for (size_t i = 0; i < quaternions.size(); ++i)
    {
      g2o::SE3Quat pose(quaternions[i], Ts[i]);

      g2o::VertexSCam * v_se3 = new g2o::VertexSCam();

      v_se3->setId(vertex_id);
      v_se3->estimate() = pose;
      v_se3->setAll(); // set aux transforms

      optimizer.addVertex(v_se3);
      vertex_id++;
    }

    int point_id = vertex_id;

    tr1::unordered_map<int, int> pointid_2_trueid;

    // add point projections to this vertex
    for (size_t i = 0; i < in_point_estimates.size(); ++i)
    {
      // First, make sure, the point is visible in several views
      {
        unsigned int count = 0;
        for (size_t j = 0; j < quaternions.size(); ++j)
        {
          if ((x.coeff(j, i) == 0) && (y.coeff(j, i) == 0) && (disparity.coeff(j, i) == 0))
            continue;
          ++count;
          if (count >= 2)
            break;
        }
        if (count < 2)
          continue;
      }

      // Add the point to the optimizer
      g2o::VertexPointXYZ * v_p = new g2o::VertexPointXYZ();

      v_p->setId(point_id);
      v_p->setMarginalized(true);
      v_p->estimate() = in_point_estimates.at(i);

      // Add the different views to the optimizer
      for (size_t j = 0; j < quaternions.size(); ++j)
      {
        // check whether the point is visible in that view
        if ((x.coeff(j, i) == 0) && (y.coeff(j, i) == 0) && (disparity.coeff(j, i) == 0))
          continue;

        g2o::Edge_XYZ_VSC * e = new g2o::Edge_XYZ_VSC();

        e->vertices()[0] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);

        e->vertices()[1] = dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertices().find(j)->second);

        Vector3d z(x.coeff(j, i), y.coeff(j, i), disparity.coeff(j, i));
        e->measurement() = z;
        e->inverseMeasurement() = -z;
        e->information() = Matrix3d::Identity();

        e->setRobustKernel(true);
        e->setHuberWidth(1);

        optimizer.addEdge(e);
      }

      optimizer.addVertex(v_p);

      pointid_2_trueid.insert(make_pair(point_id, i));
//.........这里部分代码省略.........