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