예제 #1
0
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::constructQuadraticForm()
{
  VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
  VertexXjType* to   = static_cast<VertexXjType*>(_vertices[1]);

  // get the Jacobian of the nodes in the manifold domain
  const JacobianXiOplusType& A = jacobianOplusXi();
  const JacobianXjOplusType& B = jacobianOplusXj();

  const InformationType& omega = _information;

  bool fromNotFixed = !(from->fixed());
  bool toNotFixed = !(to->fixed());

  if (fromNotFixed || toNotFixed) {
#ifdef G2O_OPENMP
    from->lockQuadraticForm();
    to->lockQuadraticForm();
#endif
    Matrix<double, D, 1> omega_r = - omega * _error;
    if (fromNotFixed) {
      Matrix<double, VertexXiType::Dimension, D> AtO = A.transpose() * omega;
      from->b().noalias() += A.transpose() * omega_r;
      from->A().noalias() += AtO*A;
      if (toNotFixed ) {
        if (_hessianRowMajor) // we have to write to the block as transposed
          _hessianTransposed.noalias() += B.transpose() * AtO.transpose();
        else
          _hessian.noalias() += AtO * B;
      }
    } 
    if (toNotFixed ) {
      to->b().noalias() += B.transpose() * omega_r;
      to->A().noalias() += B.transpose() * omega * B;
    }
#ifdef G2O_OPENMP
    to->unlockQuadraticForm();
    from->unlockQuadraticForm();
#endif
  }
}
예제 #2
0
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
}
void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::constructQuadraticForm()
{
    VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
    VertexXjType* to   = static_cast<VertexXjType*>(_vertices[1]);

    // get the Jacobian of the nodes in the manifold domain
    const JacobianXiOplusType& A = jacobianOplusXi();
    const JacobianXjOplusType& B = jacobianOplusXj();


    bool fromNotFixed = !(from->fixed());
    bool toNotFixed = !(to->fixed());

    if (fromNotFixed || toNotFixed) {
#ifdef G2O_OPENMP
        from->lockQuadraticForm();
        to->lockQuadraticForm();
#endif
        const InformationType& omega = _information;
        Eigen::Matrix<double, D, 1, Eigen::ColMajor> omega_r = - omega * _error;
        if (this->robustKernel() == 0) {
            if (fromNotFixed) {
                Eigen::Matrix<double, VertexXiType::Dimension, D, Eigen::ColMajor> AtO = A.transpose() * omega;
                from->b().noalias() += A.transpose() * omega_r;
                from->A().noalias() += AtO*A;
                if (toNotFixed ) {
                    if (_hessianRowMajor) // we have to write to the block as transposed
                        _hessianTransposed.noalias() += B.transpose() * AtO.transpose();
                    else
                        _hessian.noalias() += AtO * B;
                }
            }
            if (toNotFixed) {
                to->b().noalias() += B.transpose() * omega_r;
                to->A().noalias() += B.transpose() * omega * B;
            }
        } else { // robust (weighted) error according to some kernel
            double error = this->chi2();
            Vector3D rho;
            this->robustKernel()->robustify(error, rho);
            InformationType weightedOmega = this->robustInformation(rho);
            //std::cout << PVAR(rho.transpose()) << std::endl;
            //std::cout << PVAR(weightedOmega) << std::endl;

            omega_r *= rho[1];
            if (fromNotFixed) {
                from->b().noalias() += A.transpose() * omega_r;
                from->A().noalias() += A.transpose() * weightedOmega * A;
                if (toNotFixed ) {
                    if (_hessianRowMajor) // we have to write to the block as transposed
                        _hessianTransposed.noalias() += B.transpose() * weightedOmega * A;
                    else
                        _hessian.noalias() += A.transpose() * weightedOmega * B;
                }
            }
            if (toNotFixed) {
                to->b().noalias() += B.transpose() * omega_r;
                to->A().noalias() += B.transpose() * weightedOmega * B;
            }
        }
#ifdef G2O_OPENMP
        to->unlockQuadraticForm();
        from->unlockQuadraticForm();
#endif
    }
}