CellDoubleArray FaceToCellOperator::apply(FaceDoubleArray & u){ CellDoubleArray out = g->makeCellDoubleArray(); /*CellToFaceCoefficients::iterator cIt; for(cIt = coefficients.begin(); cIt != coefficients.end(); cIt++){ CellToFaceIndex c2f = (*cIt).first; out(c2f.i,c2f.j) += ((*cIt).second)*u(c2f.faceIndex); }*/ FaceToCellCoefficients::iterator cIt; for(cIt = coefficients.begin(); cIt != coefficients.end(); cIt++){ int i = (*cIt).first.i, j = (*cIt).first.j; FaceCoefficients::iterator fIt; for(fIt = (*cIt).second.begin(); fIt != (*cIt).second.end(); fIt++){ int faceIndex = (*fIt).first; double c = (*fIt).second; out(i,j) += c*u(faceIndex); } } for(int i = g->iMin; i <= g->iMax; i++){ for(int j = g->jMin; j <= g->jMax; j++){ out(i,j) += constantTerm(i,j); } } return out; }
/* ************************************************************************* */ boost::shared_ptr<GaussianFactor> LinearizedHessianFactor::linearize(const Values& c) const { // Construct an error vector in key-order from the Values Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; const Value& newPt = c.at(key); const Value& linPt = lin_points_.at(key); dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt); index += linPt.dim(); } // f2 = f1 - 2*dx'*g1 + dx'*G1*dx //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx; double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx; // g2 = g1 - G1*dx //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx; Vector g = linearTerm() - squaredTerm() * dx; std::vector<Vector> gs; std::size_t offset = 0; for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { const std::size_t dim = info_.getDim(i); gs.push_back(g.segment(offset, dim)); offset += dim; } // G2 = G1 // Do Nothing std::vector<Matrix> Gs; for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) { Gs.push_back(info_.diagonalBlock(i)); for(DenseIndex j = i + 1; j < info_.nBlocks()-1; ++j) { Gs.push_back(info_.aboveDiagonalBlock(i, j)); } } // Create a Hessian Factor from the modified info matrix //return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo)); return boost::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f)); }
/* ************************************************************************* */ double LinearizedHessianFactor::error(const Values& c) const { // Construct an error vector in key-order from the Values Vector dx = Vector::Zero(dim()); size_t index = 0; for(unsigned int i = 0; i < this->size(); ++i){ Key key = this->keys()[i]; const Value& newPt = c.at(key); const Value& linPt = lin_points_.at(key); dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt); index += linPt.dim(); } // error 0.5*(f - 2*x'*g + x'*G*x) double f = constantTerm(); double xtg = dx.dot(linearTerm()); double xGx = dx.transpose() * squaredTerm() * dx; return 0.5 * (f - 2.0 * xtg + xGx); }
Gradient::Gradient(Grid * g){ //cout << "Trying to create gradient with h = " << g->getH() << endl; this->g = g; double h = g->getH(); constantTerm.resize(g->faces.size()); constantTerm = 0; for(int i = g->iMin; i <= g->iMax; i++){ for(int j = g->jMin; j <= g->jMax; j++){ if(g->cells(i,j)->isUncovered()){ //cout << " is uncovered." << endl; CellIndex cellIndex(i,j); TinyVector<Face*,5> faces = g->cells(i,j)->faces; if(g->isFaceUncovered(i,j,B)){ FaceIndex faceIndex = faces(B)->getIndex(); Coord centroid = faces(B)->getCentroid(); TinyVector<double,2> n = faces(B)->getNormal(); double x, y, r; x = centroid(0); y = centroid(1); r = sqrt(pow2(x) + pow2(y)); TinyVector<double,2> gradU; double pi = acos(-1); //gradU(0) = 3*pow(x,2)*pow(y-2,2)+sin(2*(x-pow(y,2))); //gradU(1) = 2*pow(x,3)*(y-2) - 2*y*sin(2*(x-pow(y,2))); gradU(0) = 0;//-2*pi*sin(2*pi*r)*x/r; gradU(1) = 0;//-2*pi*sin(2*pi*r)*y/r; constantTerm(faceIndex) += gradU(0)*n(0) + gradU(1)*n(1); //cout << "\tAdding boundary condition to constant term." << endl; } if(faces(N) != 0){ FaceIndex faceIndex = faces(N)->getIndex(); if(faces(N)->isRegular()){ coefficients[faceIndex][cellIndex] = -1/h; } else if(faces(N)->isIrregular()){ if(coefficients[faceIndex].count(cellIndex) == 0){ double alpha = g->cells(i,j)->getFace(N)->getArea(); if(g->isFaceUncovered(i+1,j,N)){ CellIndex ind1(i,j); CellIndex ind2(i,j+1); CellIndex ind3(i+1,j); CellIndex ind4(i+1,j+1); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } else if(g->isFaceUncovered(i-1,j,N)){ CellIndex ind1(i,j); CellIndex ind2(i,j+1); CellIndex ind3(i-1,j); CellIndex ind4(i-1,j+1); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } } } //cout << "\tDid north face" << endl; } if(faces(S) != 0){ FaceIndex faceIndex = faces(S)->getIndex(); if(faces(S)->isRegular()){ coefficients[faceIndex][cellIndex] = 1/h; } else if(faces(S)->isIrregular()){ if(coefficients[faceIndex].count(cellIndex) == 0){ double alpha = g->cells(i,j)->getFace(S)->getArea(); if(g->isFaceUncovered(i+1,j,S)){ CellIndex ind1(i,j-1); CellIndex ind2(i,j); CellIndex ind3(i+1,j-1); CellIndex ind4(i+1,j); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } else if(g->isFaceUncovered(i-1,j,S)){ CellIndex ind1(i,j-1); CellIndex ind2(i,j); CellIndex ind3(i-1,j-1); CellIndex ind4(i-1,j); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } } } //cout << "\tDid south face" << endl; } if(faces(E) != 0){ FaceIndex faceIndex = faces(E)->getIndex(); CellIndex cellIndex(i,j); if(faces(E)->isRegular()){ coefficients[faceIndex][cellIndex] = -1/h; } else if(faces(E)->isIrregular()){ if(coefficients[faceIndex].count(cellIndex) == 0){ double alpha = g->cells(i,j)->getFace(E)->getArea(); if(g->isFaceUncovered(i,j+1,E)){ CellIndex ind1(i,j); CellIndex ind2(i+1,j); CellIndex ind3(i,j+1); CellIndex ind4(i+1,j+1); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } else if(g->isFaceUncovered(i,j-1,E)){ CellIndex ind1(i,j); CellIndex ind2(i+1,j); CellIndex ind3(i,j-1); CellIndex ind4(i+1,j-1); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } } } //cout << "\tDid east face" << endl; } if(faces(W) != 0){ FaceIndex faceIndex = faces(W)->getIndex(); CellIndex cellIndex(i,j); if(faces(W)->isRegular()){ coefficients[faceIndex][cellIndex] = 1/h; } else if(faces(W)->isIrregular()){ if(coefficients[faceIndex].count(cellIndex) == 0){ double alpha = g->cells(i,j)->getFace(W)->getArea(); if(g->isFaceUncovered(i,j+1,W)){ CellIndex ind1(i+1,j); CellIndex ind2(i,j); CellIndex ind3(i+1,j+1); CellIndex ind4(i,j+1); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } else if(g->isFaceUncovered(i,j-1,W)){ CellIndex ind1(i+1,j); CellIndex ind2(i,j); CellIndex ind3(i+1,j-1); CellIndex ind4(i,j-1); interpolateIrregularFace(alpha,ind1,ind2,ind3,ind4,faceIndex); } } } //cout << "\tDid west face" << endl; } } } } //cout << "Resized" << endl; /* for(int i = g->iMin; i <= g->iMax; i++){ for(int j = g->jMin; j <= g->jMax; j++){ cout << "i = " << i << " j = " << j << endl; if(g->cells(i,j)->isUncovered()){ cout << "Uncovered" << endl; double c = 1; CellIndex index(i,j); TinyVector<Face*,5> faces = g->cells(i,j)->faces; cout << "Got faces" << endl; for(int k = 0; k < 5; k++){ cout << "Testing face " << k << endl; if(faces(k) != 0 && faces(k)->isUncovered()){ cout << "Face " << k << " is uncovered "; cout << "and has index " << faces(k)->getIndex() << endl; cout << coefficients(faces(k)->getIndex()).empty() << endl; coefficients(faces(k)->getIndex())[index] = c; } } } } }*/ }