Method getLhs

okvis_ros/okvis/okvis_ceres/src/Map.cpp:105–150

Obtain the Hessian block for a specific parameter block.

// Obtain the Hessian block for a specific parameter block.
void Map::getLhs(uint64_t parameterBlockId, Eigen::MatrixXd& H) {
  OKVIS_ASSERT_TRUE_DBG(Exception, parameterBlockExists(parameterBlockId), "parameter block not in map.");
  ResidualBlockCollection res = residuals(parameterBlockId);
  H.setZero();
  for (size_t i = 0; i < res.size(); ++i) {
    // parameters:
    ParameterBlockCollection pars = parameters(res[i].residualBlockId);

    double** parametersRaw = new double*[pars.size()];
    Eigen::VectorXd residualsEigen(res[i].errorInterfacePtr->residualDim());
    double* residualsRaw = residualsEigen.data();

    double** jacobiansRaw = new double*[pars.size()];
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
                Eigen::aligned_allocator<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > >
        jacobiansEigen(pars.size());

    double** jacobiansMinimalRaw = new double*[pars.size()];
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>,
                Eigen::aligned_allocator<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > >
        jacobiansMinimalEigen(pars.size());

    int J = -1;
    for (size_t j = 0; j < pars.size(); ++j) {
      // determine which is the relevant block
      if (pars[j].second->id() == parameterBlockId) J = j;
      parametersRaw[j] = pars[j].second->parameters();
      jacobiansEigen[j].resize(res[i].errorInterfacePtr->residualDim(), pars[j].second->dimension());
      jacobiansRaw[j] = jacobiansEigen[j].data();
      jacobiansMinimalEigen[j].resize(res[i].errorInterfacePtr->residualDim(), pars[j].second->minimalDimension());
      jacobiansMinimalRaw[j] = jacobiansMinimalEigen[j].data();
    }

    // evaluate residual block
    res[i].errorInterfacePtr->EvaluateWithMinimalJacobians(
        parametersRaw, residualsRaw, jacobiansRaw, jacobiansMinimalRaw);

    // get block
    H += jacobiansMinimalEigen[J].transpose() * jacobiansMinimalEigen[J];

    // cleanup
    delete[] parametersRaw;
    delete[] jacobiansRaw;
    delete[] jacobiansMinimalRaw;
  }
}

// Check a Jacobian with numeric differences.
bool Map::isJacobianCorrect(::ceres::ResidualBlockId residualBlockId, double relTol) const {