void BaseMultiEdge<D, E>::constructQuadraticForm() { const InformationType& omega = _information; Matrix<double, D, 1> omega_r = - omega * _error; for (size_t i = 0; i < _vertices.size(); ++i) { OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]); bool istatus = !(from->fixed()); if (istatus) { const MatrixXd& A = _jacobianOplus[i]; MatrixXd AtO = A.transpose() * omega; int fromDim = from->dimension(); Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim); Map<VectorXd> fromB(from->bData(), fromDim); // ii block in the hessian #ifdef G2O_OPENMP from->lockQuadraticForm(); #endif fromMap.noalias() += AtO * A; fromB.noalias() += A.transpose() * omega_r; // compute the off-diagonal blocks ij for all j for (size_t j = i+1; j < _vertices.size(); ++j) { OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]); #ifdef G2O_OPENMP to->lockQuadraticForm(); #endif bool jstatus = !(to->fixed()); if (jstatus) { const MatrixXd& B = _jacobianOplus[j]; int idx = computeUpperTriangleIndex(i, j); assert(idx < (int)_hessian.size()); HessianHelper& hhelper = _hessian[idx]; if (hhelper.transposed) { // we have to write to the block as transposed hhelper.matrix.noalias() += B.transpose() * AtO.transpose(); } else { hhelper.matrix.noalias() += AtO * B; } } #ifdef G2O_OPENMP to->unlockQuadraticForm(); #endif } #ifdef G2O_OPENMP from->unlockQuadraticForm(); #endif } } }