80 lines
2.5 KiB
C++
80 lines
2.5 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file EssentialMatrixConstraint.cpp
|
|
* @author Frank Dellaert
|
|
* @author Pablo Alcantarilla
|
|
* @date Jan 5, 2014
|
|
**/
|
|
|
|
#include <gtsam/slam/EssentialMatrixConstraint.h>
|
|
//#include <gtsam/linear/GaussianFactor.h>
|
|
//#include <gtsam/base/Testable.h>
|
|
|
|
#include <ostream>
|
|
|
|
namespace gtsam {
|
|
|
|
/* ************************************************************************* */
|
|
void EssentialMatrixConstraint::print(const std::string& s,
|
|
const KeyFormatter& keyFormatter) const {
|
|
std::cout << s << "EssentialMatrixConstraint(" << keyFormatter(this->key<1>())
|
|
<< "," << keyFormatter(this->key<2>()) << ")\n";
|
|
measuredE_.print(" measured: ");
|
|
this->noiseModel_->print(" noise model: ");
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
|
|
double tol) const {
|
|
const This *e = dynamic_cast<const This*>(&expected);
|
|
return e != nullptr && Base::equals(*e, tol)
|
|
&& this->measuredE_.equals(e->measuredE_, tol);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
Vector EssentialMatrixConstraint::evaluateError(const Pose3& p1,
|
|
const Pose3& p2, OptionalMatrixType Hp1,
|
|
OptionalMatrixType Hp2) const {
|
|
|
|
// compute relative Pose3 between p1 and p2
|
|
Pose3 _1P2_ = p1.between(p2, Hp1, Hp2);
|
|
|
|
// convert to EssentialMatrix
|
|
Matrix D_hx_1P2;
|
|
EssentialMatrix hx;
|
|
if (Hp1 || Hp2) {
|
|
hx = EssentialMatrix::FromPose3(_1P2_, D_hx_1P2);
|
|
} else {
|
|
hx = EssentialMatrix::FromPose3(_1P2_, OptionalNone);
|
|
}
|
|
|
|
// Calculate derivatives if needed
|
|
if (Hp1) {
|
|
// Hp1 will already contain the 6*6 derivative D_1P2_p1
|
|
const Matrix& D_1P2_p1 = *Hp1;
|
|
// The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
|
|
*Hp1 = D_hx_1P2 * D_1P2_p1;
|
|
}
|
|
if (Hp2) {
|
|
// Hp2 will already contain the 6*6 derivative D_1P2_p1
|
|
const Matrix& D_1P2_p2 = *Hp2;
|
|
// The 5*6 derivative is obtained by chaining with 5*6 D_hx_1P2:
|
|
*Hp2 = D_hx_1P2 * D_1P2_p2;
|
|
}
|
|
|
|
// manifold equivalent of h(x)-z -> log(z,h(x))
|
|
return measuredE_.localCoordinates(hx); // 5D error
|
|
}
|
|
|
|
} /// namespace gtsam
|