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);
}
LaplacianOperator::LaplacianOperator( ObjEntity* mesh,MeshDrawerImpl* drawer ):mState(STATE_CHOOSING_STATIC),OperatorImpl(mesh,drawer)
{
mVerticesController = new VerticesController(mesh,drawer);
assert(mesh != nullptr);
clock_t clock_start;
clock_start = clock();
mMeshVertexCount = mesh->getVertexCount();
initAdjacentMatrix();
// printMatrix(adjacentMatrix,"adjacentMatrix");
// printMatrix(degreeMatrix,"degreeMatrix");
Eigen::SparseMatrix<double> laplacianOperator;
computeLaplacianOperator(laplacianOperator);
#ifdef MY_DEBUG
ofstream out("debug.txt",ios::app);
out << "laplacianOperator:";
for (Eigen::SparseMatrix<double>::InnerIterator it(laplacianOperator,283); it; it++)
{
out << it.row() << ' ';
}
out << endl;
#endif // MY_DEBUG
//printMatrix(laplacianOperator,"laplacianOperator");
Eigen::Matrix<double,Eigen::Dynamic,3> verInput(mMeshVertexCount,3);
for (int i = 0; i < mMeshVertexCount; i++)
{
Vertex* v = mesh->m_vertexList->at(i);
verInput(i,0) = v->x; verInput(i,1) = v->y; verInput(i,2) = v->z;
}
Eigen::Matrix<double,Eigen::Dynamic,3> deltaInput = laplacianOperator * verInput;
#ifdef MY_DEBUG
out << "verInput 283:" << verInput(283,0) << ' ' << verInput(283,1) << ' ' << verInput(283,2) << endl;
out << "deltaInput 283:" << deltaInput(283,0) << ' ' << deltaInput(283,1) << ' ' << deltaInput(283,2) << endl;
#endif // MY_DEBUG
//printMatrix(deltaInput,"deltaInput");
Eigen::SparseMatrix<double> laplacianOperator3D(6*mMeshVertexCount,3*mMeshVertexCount);
laplacianOperator3D.reserve(Eigen::VectorXi::Constant(3*mMeshVertexCount,40));
//Eigen::MatrixXd laplacianOperator(laplacianOperator);
vector<Vertex*>* meshVertex = mesh->m_vertexList;
/* operate laplacianOperator3D */
for (int i = 0; i < mMeshVertexCount; i++)
{
//printf("%d ",i);
/* Generate Matrix A*/
Vertex* v = meshVertex->at(i);
vector<Vertex*> vertexInA;
vertexInA.push_back(v);
vector<int> adjIndex;
getAdjVertex(i,adjIndex);
for (int j = 0; j < adjIndex.size(); j++)
{
vertexInA.push_back(meshVertex->at(adjIndex.at(j)));
}
Eigen::Matrix<double,Eigen::Dynamic,7> matrixA;
matrixA.resize(vertexInA.size()*3,7);
for (int j = 0; j < vertexInA.size(); j++)
{
Eigen::Matrix<double,3,7> m;
constructMatrix(vertexInA.at(j),m);
int r = matrixA.rows();
matrixA.middleRows<3>(j*3) = m;
// printMatrix(m,"m");
}
Eigen::Matrix<double,7,Eigen::Dynamic> pinvA;
/* 当该顶点是孤立的的时候，transposeA*matrixA可能是奇异的，因此pinv就会出现-1.#IND*/
if(adjIndex.size() == 0)
{
pinvA.resize(7,3);
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++;
}
Eigen::VectorXd b(mMeshVertexCount_X3*2);
b.setZero();
int ret;
defoMesh = ObjUtility::createObjEntity("defo.obj",ret);
vector<Vertex*>* defoVertex = defoMesh->m_vertexList;
int defoSize = defoVertex->size();
int nodefoSize = mMeshVertexCount;
for (int j = 0; j < mMeshVertexCount; j++)
{
b(mMeshVertexCount_X3+3*j) = defoVertex->at(j)->x;
b(mMeshVertexCount_X3+3*j+1) = defoVertex->at(j)->y;
b(mMeshVertexCount_X3+3*j+2) = defoVertex->at(j)->z;
}
A_prime.makeCompressed();
Eigen::SparseMatrix<double> A_prime_T = Eigen::SparseMatrix<double>(A_prime.transpose());
Eigen::VectorXd A_prime_T_b = A_prime_T*b;
Eigen::SparseMatrix<double> sym = A_prime_T*A_prime;
long t = (clock()-clock_start);
printf("constructTime:%d\n",t);
#ifdef MY_DEBUG
ofstream outSym("sym.txt");
outSym << sym << endl;
outSym.close();
ofstream outAprimeT("AprimeT.txt");
outAprimeT << A_prime_T << endl;
outAprimeT.close();
ofstream outAprime("A_prime.txt");
outAprime << A_prime << endl;
outAprime.close();
#endif
#ifdef MY_DEBUG
Eigen::EigenSolver<Eigen::MatrixXd > es(sym);
ofstream outEigen("eigenvalues.txt",ios::app);
outEigen << es.eigenvalues() << endl;
outEigen.close();
#endif // MY_DEBUG
clock_start = clock();
//Eigen::SimplicialLLT<Eigen::SparseMatrix<double> > solver;
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;
solver.compute(sym);
t = (clock()-clock_start);
printf("factorizationTime:%d\n",t);
clock_start = clock();
// ofstream outLU("LU.txt",ios::app);
// outLU << "L:" << endl;
// outLU << solver.matrixL() << endl;
// outLU << "------------------------------------------------------------------" << endl;
// outLU << "U:" << endl;
// outLU << solver.matrixU() << endl;
// outLU.close();
if(solver.info()!= Eigen::Success) {
// decomposition failed
return;
}
Eigen::VectorXd v_p = solver.solve(A_prime_T_b);
t = (clock()-clock_start);
printf("SolveTime:%d\n",t);
//printMatrix(v_p,"v_p");
writeToDisk(v_p);
if(solver.info()!= Eigen::Success) {
// solving failed
return;
}
// Eigen::VectorXd v_p = A_prime.householderQr().solve(b);
// //Eigen::VectorXd v_p = A_prime.jacobiSvd().solve(b);
//
//
// printMatrix(laplacianOperator3D,"laplacianOperator3D");
// printMatrix(A_prime,"A_prime");
// printMatrix(adjacentMatrix,"adjacentMatrix");
// printMatrix(b,"b");
// printMatrix(v_p,"v_p");
//
//cout << laplacianOperator;
}
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));
++point_id;
}
optimizer.initializeOptimization();
optimizer.setVerbose(true);
g2o::StructureOnlySolver<3> structure_only_ba;
cout << "Performing full BA:" << endl;
optimizer.optimize(5);
// Set the computed points in the final data structure
out_point_estimates = in_point_estimates;
for (tr1::unordered_map<int, int>::iterator it = pointid_2_trueid.begin(); it != pointid_2_trueid.end(); ++it)
{
g2o::HyperGraph::VertexIDMap::iterator v_it = optimizer.vertices().find(it->first);
if (v_it == optimizer.vertices().end())
{
cerr << "Vertex " << it->first << " not in graph!" << endl;
exit(-1);
}
g2o::VertexPointXYZ * v_p = dynamic_cast<g2o::VertexPointXYZ *>(v_it->second);
if (v_p == 0)
{
cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
exit(-1);
}
out_point_estimates[it->second] = v_p->estimate();
}
// Set the unchange
}
