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>::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
}
}
