splitted evaluateJacobians in 2 functions row and col major

include/core/factor/factor_analytic.h

@@ -67,7 +67,7 @@ class FactorAnalytic: public FactorBase
             // load parameters evaluation value
             std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
             for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
-                state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXd>((double*)parameters[i], state_block_sizes_vector_[i]));
+                state_blocks_map_.emplace_back((double*)parameters[i], state_block_sizes_vector_[i]);
 
             // residuals
             Eigen::Map<Eigen::VectorXd> residuals_map((double*)residuals, getSize());
@@ -83,24 +83,34 @@ class FactorAnalytic: public FactorBase
                 {
                     compute_jacobians_[i] = (jacobians[i] != nullptr);
                     if (jacobians[i] != nullptr)
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixRowXd>((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]));
+                        jacobians_map_.emplace_back((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]);
                     else
-                        jacobians_map_.push_back(Eigen::Map<Eigen::MatrixRowXd>(nullptr, 0, 0)); //TODO: check if it can be done
+                        jacobians_map_.emplace_back(nullptr, 0, 0); //TODO: check if it can be done
                 }
 
                 // evaluate jacobians
                 evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
             }
             return true;
-
-            return true;
         };
 
         /** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
          **/
-        // TODO
         virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
         {
+            assert(_states_ptr.size() == state_block_sizes_vector_.size());
+
+            // load parameters evaluation value
+            std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
+            for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
+                state_blocks_map_.emplace_back(_states_ptr[i], state_block_sizes_vector_[i]);
+
+            // residuals
+            residual_ = evaluateResiduals(state_blocks_map_);
+
+            // compute jacobians
+            jacobians_.resize(state_block_sizes_vector_.size());
+            evaluateJacobians(state_blocks_map_, jacobians_, std::vector<bool>(state_block_sizes_vector_.size(),true));
         };
 
         /** \brief Returns the residual evaluated in the states provided
@@ -120,7 +130,12 @@ class FactorAnalytic: public FactorBase
          * \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 = 0;
+        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 = 0;
+        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 = 0;
 
         /** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
          *

include/core/factor/factor_block_absolute.h

@@ -90,9 +90,12 @@ class FactorBlockAbsolute : public FactorAnalytic
          * \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,
+        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
          *
@@ -120,9 +123,24 @@ inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector<
         return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
 }
 
-inline void FactorBlockAbsolute::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
+inline void FactorBlockAbsolute::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() == 1 && "Wrong number of state blocks!");
+    assert(jacobians.size() == 1 && "Wrong number of jacobians!");
+    assert(_compute_jacobian.size() == 1 && "Wrong number of _compute_jacobian booleans!");
+    assert(_st_vector.front().size() == sb_size_ && "Wrong StateBlock size");
+    assert(_st_vector.front().size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
+
+    // normalized jacobian
+    if (_compute_jacobian.front())
+        jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
+}
+
+inline void FactorBlockAbsolute::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() == 1 && "Wrong number of state blocks!");
     assert(jacobians.size() == 1 && "Wrong number of jacobians!");

include/core/factor/factor_odom_2D.h

@@ -44,11 +44,11 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
         bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
                          T* _residuals) const;
 
-    public:
-        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
-        {
-            return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-        }
+//    public:
+//        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+//        {
+//            return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+//        }
 
 };
 

include/core/factor/factor_odom_2D_analytic.h

@@ -35,82 +35,15 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
             return std::string("MOTION");
         }
 
-
-//        /** \brief Returns the factor residual size
-//         *
-//         * Returns the factor residual size
-//         *
-//         **/
-//        virtual unsigned int getSize() const
-//        {
-//            return 3;
-//        }
-//
-//        /** \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
-//        {
-//            Eigen::VectorXd residual(3);
-//            Eigen::VectorXd expected_measurement(3);
-//
-//            // Expected measurement
-//            Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
-//            expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1)
-//            expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
-//
-//            // Residual
-//            residual = expected_measurement - getMeasurement();
-//            while (residual(2) > M_PI)
-//                residual(2) = residual(2) - 2*M_PI;
-//            while (residual(2) <= -M_PI)
-//                residual(2) = residual(2) + 2*M_PI;
-//            residual = getMeasurementSquareRootInformationUpper() * residual;
-//
-//            return residual;
-//        }
-//
-//        /** \brief Returns the jacobians evaluated in the states provided
-//         *
-//         * Returns the 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
+    public:
+//        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
+//                                    const NodeBasePtr& _correspondant_ptr,
+//                                    const ProcessorBasePtr& _processor_ptr = nullptr)
 //        {
-//            jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
-//                             sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
-//                             0,                     0;
-//            jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-//            jacobians[1] << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-//                            -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-//                            -1;
-//            jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-//            jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-//                            -sin(_st_vector[1](0)),cos(_st_vector[1](0)),
-//                            0,                     0;
-//            jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-//
-//            jacobians[3] << 0, 0, 1;
-//            jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[0];
+//            return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr,
+//                                                          std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
 //        }
 
-    public:
-        static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
-                                              const NodeBasePtr& _correspondant_ptr,
-                                              const ProcessorBasePtr& _processor_ptr = nullptr)
-        {
-            return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
-        }
-
 };
 
 } // namespace wolf

include/core/factor/factor_relative_2D_analytic.h

@@ -85,9 +85,12 @@ class FactorRelative2DAnalytic : public FactorAnalytic
          * \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,
+        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
          * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
@@ -118,44 +121,104 @@ inline Eigen::VectorXd FactorRelative2DAnalytic::evaluateResiduals(
     return residual;
 }
 
-inline void FactorRelative2DAnalytic::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
+inline void FactorRelative2DAnalytic::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(jacobians.size() == 4);
+    assert(_st_vector.size() == 4);
+    assert(_compute_jacobian.size() == 4);
+
+    if (_compute_jacobian[0])
+    {
+        jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
+                        -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+                        sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
+                        0, 0).finished();
+    }
+    if (_compute_jacobian[1])
+    {
+        jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
+                        -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+                        -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
+                        -1).finished();
+    }
+    if (_compute_jacobian[2])
+    {
+        jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,2) <<
+                        cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+                        -sin(_st_vector[1](0)), cos(_st_vector[1](0)),
+                        0, 0).finished();
+    }
+    if (_compute_jacobian[3])
+    {
+        jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(3,1) <<
+                        0, 0, 1).finished();
+    }
+}
+
+inline void FactorRelative2DAnalytic::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+                                                        std::vector<Eigen::MatrixXd>& jacobians,
+                                                        const std::vector<bool>& _compute_jacobian) const
 {
-    jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)), sin(_st_vector[1](0)), -cos(_st_vector[1](0)), 0, 0;
-    jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
-    jacobians[1]
-            << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0))
-                    + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)), -(_st_vector[2](0)
-            - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)), -1;
-    jacobians[1] = getMeasurementSquareRootInformationUpper() * jacobians[1];
-    jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)), -sin(_st_vector[1](0)), cos(_st_vector[1](0)), 0, 0;
-    jacobians[2] = getMeasurementSquareRootInformationUpper() * jacobians[2];
-    jacobians[3] << 0, 0, 1;
-    jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
+    assert(jacobians.size() == 4);
+    assert(_st_vector.size() == 4);
+    assert(_compute_jacobian.size() == 4);
+
+    if (_compute_jacobian[0])
+    {
+        jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
+                        -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+                        sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
+                        0, 0).finished();
+    }
+    if (_compute_jacobian[1])
+    {
+        jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
+                        -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+                        -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
+                        -1).finished();
+    }
+    if (_compute_jacobian[2])
+    {
+        jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,2) <<
+                        cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+                        -sin(_st_vector[1](0)), cos(_st_vector[1](0)),
+                        0, 0).finished();
+    }
+    if (_compute_jacobian[3])
+    {
+        jacobians[3] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixXd(3,1) <<
+                        0, 0, 1).finished();
+    }
 }
 
 inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
 {
-    jacobians[0]
-              << -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)), sin(
-            getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)), 0, 0;
-
-    jacobians[1]
-              << -(getStateBlockPtrVector()[2]->getState()(0) - getStateBlockPtrVector()[0]->getState()(0))
-                    * sin(getStateBlockPtrVector()[1]->getState()(0))
-                    + (getStateBlockPtrVector()[2]->getState()(1) - getStateBlockPtrVector()[0]->getState()(1))
-                            * cos(getStateBlockPtrVector()[1]->getState()(0)), -(getStateBlockPtrVector()[2]->getState()(0)
-            - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
-            - (getStateBlockPtrVector()[2]->getState()(1) - getStateBlockPtrVector()[0]->getState()(1))
-                    * sin(getStateBlockPtrVector()[1]->getState()(0)), -1;
-
-    jacobians[2]
-              << cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)), -sin(
-            getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)), 0, 0;
-
-    jacobians[3]
-              << 0, 0, 1;
+    assert(jacobians.size() == 4);
+
+    jacobians[0] = (Eigen::MatrixXd(3,2) <<
+                    -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)),
+                    sin(getStateBlockPtrVector()[1]->getState()(0)), -cos(getStateBlockPtrVector()[1]->getState()(0)),
+                    0, 0).finished();
+
+    jacobians[1] = (Eigen::MatrixXd(3,1) <<
+                    -(getStateBlockPtrVector()[2]->getState()(0)
+                            - getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+                            + (getStateBlockPtrVector()[2]->getState()(1)
+                            - getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
+                    -(getStateBlockPtrVector()[2]->getState()(0)
+                            - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
+                            - (getStateBlockPtrVector()[2]->getState()(1)
+                            - getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0)),
+                    -1).finished();
+
+    jacobians[2] = (Eigen::MatrixXd(3,2) <<
+                    cos(getStateBlockPtrVector()[1]->getState()(0)), sin(getStateBlockPtrVector()[1]->getState()(0)),
+                    -sin(getStateBlockPtrVector()[1]->getState()(0)), cos(getStateBlockPtrVector()[1]->getState()(0)),
+                    0, 0).finished();
+
+    jacobians[3] = (Eigen::MatrixXd(3,1) << 0, 0, 1).finished();
 }
 
 } // namespace wolf

test/gtest_factor_autodiff.cpp

@@ -208,12 +208,12 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
     fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
     std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
     t = clock();
-    //TODO FactorAnalytic::evaluate
-//    fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
-//    std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
-//
-//    for (auto i = 0; i < Jautodiff.size(); i++)
-//        ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
+    fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+    std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+
+    ASSERT_EQ(Janalytic.size(), Jautodiff.size());
+    for (auto i = 0; i < Jautodiff.size(); i++)
+        ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
 }
 
 int main(int argc, char **argv)