示例#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
  }
}
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
    }
}