Resolve "New class isMotion"

Closes #281 (closed)

include/core/factor/factor_block_difference.h

+/**
+ * \file factor_block_difference.h
+ *
+ *  Created on: Feb 28, 2020
+ *      \author: mfourmy
+ */
+
+#ifndef FACTOR_BLOCK_DIFFERENCE_H_
+#define FACTOR_BLOCK_DIFFERENCE_H_
+
+//Wolf includes
+#include "core/factor/factor_analytic.h"
+#include "core/frame/frame_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorBlockDifference);
+
+//class
+class FactorBlockDifference : public FactorAnalytic
+{
+    private:
+        SizeEigen sb1_size_;              // the size of the state blocks
+        SizeEigen sb2_size_;              // the size of the state blocks
+        SizeEigen sb1_constrained_start_; // the index of the first state element of sb1 that is constrained
+        SizeEigen sb1_constrained_size_;  // the size of sb1 the state segment that is constrained
+        SizeEigen sb2_constrained_start_; // the index of the first state element of sb2 that is constrained
+        SizeEigen sb2_constrained_size_;  // the size of sb2 the state segment that is constrained
+        Eigen::MatrixXd J_res_sb1_;              // Jacobian
+        Eigen::MatrixXd J_res_sb2_;              // Jacobian
+
+    public:
+
+        /** \brief Constructor
+         *
+         * \param _sb_ptr the constrained state block
+         *
+         */
+        FactorBlockDifference(
+                            StateBlockPtr _sb1_ptr,
+                            StateBlockPtr _sb2_ptr,
+                            unsigned int _start_idx1 = 0,
+                            int _size1 = -1,
+                            unsigned int _start_idx2 = 0,
+                            int _size2 = -1,
+                            ProcessorBasePtr _processor_ptr = nullptr,
+                            bool _apply_loss_function = false,
+                            FactorStatus _status = FAC_ACTIVE) :
+            FactorAnalytic("BLOCK ABS",
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           nullptr,
+                           _processor_ptr,
+                           _apply_loss_function,
+                           _status,
+                           _sb1_ptr,
+                           _sb2_ptr),
+            sb1_size_(_sb1_ptr->getSize()),
+            sb2_size_(_sb2_ptr->getSize()),
+            sb1_constrained_start_(_start_idx1),
+            sb1_constrained_size_(_size1 == -1 ? sb1_size_ : _size1),
+            sb2_constrained_start_(_start_idx2),
+            sb2_constrained_size_(_size2 == -1 ? sb2_size_ : _size2)
+        {
+            // Check if constrained portion is in the stateblock vector range 
+            assert(sb1_constrained_size_+sb1_constrained_start_ <= sb1_size_);
+            assert(sb2_constrained_size_+sb2_constrained_start_ <= sb2_size_);
+            assert(sb1_constrained_size_ == sb2_constrained_size_);
+
+            // precompute Jacobian (Identity with zero columns if the stateblock part is not constrained)
+            J_res_sb1_ = Eigen::MatrixXd::Zero(sb1_constrained_size_,sb1_size_);
+            J_res_sb1_.middleCols(sb1_constrained_start_,sb1_constrained_size_) =  Eigen::MatrixXd::Identity(sb1_constrained_size_,sb1_constrained_size_);
+
+            J_res_sb2_ = Eigen::MatrixXd::Zero(sb2_constrained_size_,sb2_size_);
+            J_res_sb2_.middleCols(sb2_constrained_start_,sb1_constrained_size_) = - Eigen::MatrixXd::Identity(sb2_constrained_size_,sb2_constrained_size_);
+        }
+
+        virtual ~FactorBlockDifference() = default;
+
+        virtual std::string getTopology() const override
+        {
+            return std::string("DIFF");
+        }
+
+        /** \brief Returns the residual evaluated in the states provided
+         *
+         * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
+         *
+         **/
+        virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+        /** \brief Returns the normalized jacobians evaluated in the states
+         *
+         * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
+         * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+         *
+         * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+         * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+         * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+         *
+         **/
+        virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+                                       std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+                                       const std::vector<bool>& _compute_jacobian) const override;
+        virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+                                       std::vector<Eigen::MatrixXd>& jacobians,
+                                       const std::vector<bool>& _compute_jacobian) const override;
+
+        /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+         *
+         * Returns the pure jacobians (without measurement noise) evaluated in the current state blocks values
+         *
+         * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+         *
+         **/
+        virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+        /** \brief Returns the factor residual size
+         **/
+        virtual unsigned int getSize() const override;
+};
+
+inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+    // Check measurement and cov sizes 
+    assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+    assert(getMeasurement().size() == getMeasurementCovariance().cols());
+    assert(getMeasurement().size() == getMeasurementCovariance().rows());
+    assert(getMeasurement().size() == sb1_constrained_size_);
+    assert(getMeasurement().size() == sb2_constrained_size_);
+
+    // residual: meas -> measurement of the difference between partial stateblocks
+    return getMeasurementSquareRootInformationUpper() * (getMeasurement() - (_st_vector[1].segment(sb2_constrained_start_, sb2_constrained_size_) 
+                                                                           - _st_vector[0].segment(sb1_constrained_start_, sb1_constrained_size_)));
+}
+
+inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+                                                   std::vector<Eigen::Map<Eigen::MatrixRowXd> >& _jacobians,
+                                                   const std::vector<bool>& _compute_jacobian) const
+{
+    assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+    assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
+    assert(_compute_jacobian.size() == 2 && "Wrong number of _compute_jacobian booleans!");
+    assert(_st_vector[0].size() == sb1_size_ && "Wrong StateBlock size");
+    assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
+    assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+    assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+
+    // normalized jacobian
+    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+}
+
+inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+                                                   std::vector<Eigen::MatrixXd>& _jacobians,
+                                                   const std::vector<bool>& _compute_jacobian) const
+{
+    assert(_st_vector.size() == 2 && "Wrong number of state blocks!");
+    assert(_jacobians.size() == 2 && "Wrong number of jacobians!");
+    assert(_compute_jacobian.size() == 2 && "Wrong number of _compute_jacobian booleans!");
+    assert(_st_vector[0].size() == sb1_size_ && "Wrong StateBlock size");
+    assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
+    assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+    assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+
+    // normalized jacobian
+    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+}
+
+inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
+{
+    assert(_pure_jacobians.size() == 1 && "Wrong number of jacobians!");
+
+    _pure_jacobians[0] = J_res_sb1_;
+    _pure_jacobians[1] = J_res_sb2_;
+}
+
+inline unsigned int FactorBlockDifference::getSize() const
+{
+    return sb1_constrained_size_;
+}
+
+} // namespace wolf
+
+#endif