commit
ec34f9fdf5
|
@ -10,7 +10,11 @@
|
||||||
#include <gtsam/geometry/CameraSet.h>
|
#include <gtsam/geometry/CameraSet.h>
|
||||||
#include <gtsam/linear/JacobianFactor.h>
|
#include <gtsam/linear/JacobianFactor.h>
|
||||||
#include <gtsam/linear/VectorValues.h>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
|
|
||||||
#include <iosfwd>
|
#include <iosfwd>
|
||||||
|
#include <map>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
@ -76,7 +80,7 @@ public:
|
||||||
|
|
||||||
/// print
|
/// print
|
||||||
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||||
DefaultKeyFormatter) const {
|
DefaultKeyFormatter) const override {
|
||||||
std::cout << " RegularImplicitSchurFactor " << std::endl;
|
std::cout << " RegularImplicitSchurFactor " << std::endl;
|
||||||
Factor::print(s);
|
Factor::print(s);
|
||||||
for (size_t pos = 0; pos < size(); ++pos) {
|
for (size_t pos = 0; pos < size(); ++pos) {
|
||||||
|
@ -88,7 +92,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// equals
|
/// equals
|
||||||
bool equals(const GaussianFactor& lf, double tol) const {
|
bool equals(const GaussianFactor& lf, double tol) const override {
|
||||||
const This* f = dynamic_cast<const This*>(&lf);
|
const This* f = dynamic_cast<const This*>(&lf);
|
||||||
if (!f)
|
if (!f)
|
||||||
return false;
|
return false;
|
||||||
|
@ -104,37 +108,36 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Degrees of freedom of camera
|
/// Degrees of freedom of camera
|
||||||
virtual DenseIndex getDim(const_iterator variable) const {
|
DenseIndex getDim(const_iterator variable) const override {
|
||||||
return D;
|
return D;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual void updateHessian(const KeyVector& keys,
|
void updateHessian(const KeyVector& keys,
|
||||||
SymmetricBlockMatrix* info) const {
|
SymmetricBlockMatrix* info) const override {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"RegularImplicitSchurFactor::updateHessian non implemented");
|
"RegularImplicitSchurFactor::updateHessian non implemented");
|
||||||
}
|
}
|
||||||
virtual Matrix augmentedJacobian() const {
|
Matrix augmentedJacobian() const override {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"RegularImplicitSchurFactor::augmentedJacobian non implemented");
|
"RegularImplicitSchurFactor::augmentedJacobian non implemented");
|
||||||
return Matrix();
|
return Matrix();
|
||||||
}
|
}
|
||||||
virtual std::pair<Matrix, Vector> jacobian() const {
|
std::pair<Matrix, Vector> jacobian() const override {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"RegularImplicitSchurFactor::jacobian non implemented");
|
"RegularImplicitSchurFactor::jacobian non implemented");
|
||||||
return std::make_pair(Matrix(), Vector());
|
return std::make_pair(Matrix(), Vector());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// *Compute* full augmented information matrix
|
/// *Compute* full augmented information matrix
|
||||||
virtual Matrix augmentedInformation() const {
|
Matrix augmentedInformation() const override {
|
||||||
|
|
||||||
// Do the Schur complement
|
// Do the Schur complement
|
||||||
SymmetricBlockMatrix augmentedHessian = //
|
SymmetricBlockMatrix augmentedHessian =
|
||||||
Set::SchurComplement(FBlocks_, E_, b_);
|
Set::SchurComplement(FBlocks_, E_, b_);
|
||||||
return augmentedHessian.selfadjointView();
|
return augmentedHessian.selfadjointView();
|
||||||
}
|
}
|
||||||
|
|
||||||
/// *Compute* full information matrix
|
/// *Compute* full information matrix
|
||||||
virtual Matrix information() const {
|
Matrix information() const override {
|
||||||
Matrix augmented = augmentedInformation();
|
Matrix augmented = augmentedInformation();
|
||||||
int m = this->keys_.size();
|
int m = this->keys_.size();
|
||||||
size_t M = D * m;
|
size_t M = D * m;
|
||||||
|
@ -145,7 +148,7 @@ public:
|
||||||
using GaussianFactor::hessianDiagonal;
|
using GaussianFactor::hessianDiagonal;
|
||||||
|
|
||||||
/// Add the diagonal of the Hessian for this factor to existing VectorValues
|
/// Add the diagonal of the Hessian for this factor to existing VectorValues
|
||||||
virtual void hessianDiagonalAdd(VectorValues &d) const override {
|
void hessianDiagonalAdd(VectorValues &d) const override {
|
||||||
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
|
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
|
||||||
for (size_t k = 0; k < size(); ++k) { // for each camera
|
for (size_t k = 0; k < size(); ++k) { // for each camera
|
||||||
Key j = keys_[k];
|
Key j = keys_[k];
|
||||||
|
@ -176,7 +179,7 @@ public:
|
||||||
* @brief add the contribution of this factor to the diagonal of the hessian
|
* @brief add the contribution of this factor to the diagonal of the hessian
|
||||||
* d(output) = d(input) + deltaHessianFactor
|
* d(output) = d(input) + deltaHessianFactor
|
||||||
*/
|
*/
|
||||||
virtual void hessianDiagonal(double* d) const {
|
void hessianDiagonal(double* d) const override {
|
||||||
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
|
// diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
|
||||||
// Use eigen magic to access raw memory
|
// Use eigen magic to access raw memory
|
||||||
typedef Eigen::Matrix<double, D, 1> DVector;
|
typedef Eigen::Matrix<double, D, 1> DVector;
|
||||||
|
@ -202,7 +205,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Return the block diagonal of the Hessian for this factor
|
/// Return the block diagonal of the Hessian for this factor
|
||||||
virtual std::map<Key, Matrix> hessianBlockDiagonal() const {
|
std::map<Key, Matrix> hessianBlockDiagonal() const override {
|
||||||
std::map<Key, Matrix> blocks;
|
std::map<Key, Matrix> blocks;
|
||||||
// F'*(I - E*P*E')*F
|
// F'*(I - E*P*E')*F
|
||||||
for (size_t pos = 0; pos < size(); ++pos) {
|
for (size_t pos = 0; pos < size(); ++pos) {
|
||||||
|
@ -227,17 +230,18 @@ public:
|
||||||
return blocks;
|
return blocks;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual GaussianFactor::shared_ptr clone() const {
|
GaussianFactor::shared_ptr clone() const override {
|
||||||
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
|
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
|
||||||
FBlocks_, PointCovariance_, E_, b_);
|
FBlocks_, PointCovariance_, E_, b_);
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"RegularImplicitSchurFactor::clone non implemented");
|
"RegularImplicitSchurFactor::clone non implemented");
|
||||||
}
|
}
|
||||||
virtual bool empty() const {
|
|
||||||
|
bool empty() const override {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
virtual GaussianFactor::shared_ptr negate() const {
|
GaussianFactor::shared_ptr negate() const override {
|
||||||
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
|
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
|
||||||
FBlocks_, PointCovariance_, E_, b_);
|
FBlocks_, PointCovariance_, E_, b_);
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
|
@ -288,7 +292,7 @@ public:
|
||||||
* f = nonlinear error
|
* f = nonlinear error
|
||||||
* (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
|
* (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
|
||||||
*/
|
*/
|
||||||
virtual double error(const VectorValues& x) const {
|
double error(const VectorValues& x) const override {
|
||||||
|
|
||||||
// resize does not do malloc if correct size
|
// resize does not do malloc if correct size
|
||||||
e1.resize(size());
|
e1.resize(size());
|
||||||
|
@ -383,13 +387,12 @@ public:
|
||||||
void multiplyHessianAdd(double alpha, const double* x, double* y,
|
void multiplyHessianAdd(double alpha, const double* x, double* y,
|
||||||
std::vector<size_t> keys) const {
|
std::vector<size_t> keys) const {
|
||||||
}
|
}
|
||||||
;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x
|
* @brief Hessian-vector multiply, i.e. y += F'*alpha*(I - E*P*E')*F*x
|
||||||
*/
|
*/
|
||||||
void multiplyHessianAdd(double alpha, const VectorValues& x,
|
void multiplyHessianAdd(double alpha, const VectorValues& x,
|
||||||
VectorValues& y) const {
|
VectorValues& y) const override {
|
||||||
|
|
||||||
// resize does not do malloc if correct size
|
// resize does not do malloc if correct size
|
||||||
e1.resize(size());
|
e1.resize(size());
|
||||||
|
@ -432,7 +435,7 @@ public:
|
||||||
/**
|
/**
|
||||||
* Calculate gradient, which is -F'Q*b, see paper
|
* Calculate gradient, which is -F'Q*b, see paper
|
||||||
*/
|
*/
|
||||||
VectorValues gradientAtZero() const {
|
VectorValues gradientAtZero() const override {
|
||||||
// calculate Q*b
|
// calculate Q*b
|
||||||
e1.resize(size());
|
e1.resize(size());
|
||||||
e2.resize(size());
|
e2.resize(size());
|
||||||
|
@ -454,7 +457,7 @@ public:
|
||||||
/**
|
/**
|
||||||
* Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
|
* Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
|
||||||
*/
|
*/
|
||||||
virtual void gradientAtZero(double* d) const {
|
void gradientAtZero(double* d) const override {
|
||||||
|
|
||||||
// Use eigen magic to access raw memory
|
// Use eigen magic to access raw memory
|
||||||
typedef Eigen::Matrix<double, D, 1> DVector;
|
typedef Eigen::Matrix<double, D, 1> DVector;
|
||||||
|
@ -474,7 +477,7 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Gradient wrt a key at any values
|
/// Gradient wrt a key at any values
|
||||||
Vector gradient(Key key, const VectorValues& x) const {
|
Vector gradient(Key key, const VectorValues& x) const override {
|
||||||
throw std::runtime_error(
|
throw std::runtime_error(
|
||||||
"gradient for RegularImplicitSchurFactor is not implemented yet");
|
"gradient for RegularImplicitSchurFactor is not implemented yet");
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue