void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus() { VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]); VertexXjType* vj = static_cast<VertexXjType*>(_vertices[1]); bool iNotFixed = !(vi->fixed()); bool jNotFixed = !(vj->fixed()); if (!iNotFixed && !jNotFixed) return; #ifdef G2O_OPENMP vi->lockQuadraticForm(); vj->lockQuadraticForm(); #endif const double delta = 1e-9; const double scalar = 1.0 / (2*delta); ErrorVector errorBak; ErrorVector errorBeforeNumeric = _error; if (iNotFixed) { //Xi - estimate the jacobian numerically double add_vi[VertexXiType::Dimension]; std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0); // add small step along the unit vector in each dimension for (int d = 0; d < VertexXiType::Dimension; ++d) { vi->push(); add_vi[d] = delta; vi->oplus(add_vi); computeError(); errorBak = _error; vi->pop(); vi->push(); add_vi[d] = -delta; vi->oplus(add_vi); computeError(); errorBak -= _error; vi->pop(); add_vi[d] = 0.0; _jacobianOplusXi.col(d) = scalar * errorBak; } // end dimension } if (jNotFixed) { //Xj - estimate the jacobian numerically double add_vj[VertexXjType::Dimension]; std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0); // add small step along the unit vector in each dimension for (int d = 0; d < VertexXjType::Dimension; ++d) { vj->push(); add_vj[d] = delta; vj->oplus(add_vj); computeError(); errorBak = _error; vj->pop(); vj->push(); add_vj[d] = -delta; vj->oplus(add_vj); computeError(); errorBak -= _error; vj->pop(); add_vj[d] = 0.0; _jacobianOplusXj.col(d) = scalar * errorBak; } } // end dimension _error = errorBeforeNumeric; #ifdef G2O_OPENMP vj->unlockQuadraticForm(); vi->unlockQuadraticForm(); #endif }