working

include/core/factor/factor_block_absolute.h

@@ -43,43 +43,15 @@ class FactorBlockAbsolute : public FactorAnalytic
 
     public:
 
-        /** \brief Constructor
-         *
-         * \param _sb_ptr the constrained state block
-         *
-         */
-        FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
-                            StateBlockPtr _sb_ptr,
-                            ProcessorBasePtr _processor_ptr,
-                            bool _apply_loss_function,
-                            FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("FactorBlockAbsolute",
-                           TOP_ABS,
-                           _feature_ptr,
-                           nullptr,
-                           nullptr,
-                           nullptr,
-                           nullptr,
-                           _processor_ptr,
-                           _apply_loss_function,
-                           _status,
-                           _sb_ptr),
-            sb_size_(_sb_ptr->getSize()),
-            sb_constrained_start_(0),
-            sb_constrained_size_(sb_size_),
-            is_angle_(std::dynamic_pointer_cast<StateAngle>(_sb_ptr))
-        {
-            assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
-
-            // precompute Jacobian (Identity)
-            J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
-        }
-
         /** \brief Constructor for segment of state block
          *
+         * \param _feature_ptr the feature where the factor is emplaced
          * \param _sb_ptr the constrained state block
          * \param _start_idx the index of the first state element that is constrained
-         * \param _start_idx the size of the state segment that is constrained, -1 = all the
+         * \param _size the size of the state segment that is constrained, -1 = all the
+         * \param _processor_ptr processor that created the factor
+         * \param _apply_loss_function if the factor should have loss function
+         * \param _status factor status
          *
          */
         FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
@@ -105,7 +77,9 @@ class FactorBlockAbsolute : public FactorAnalytic
             sb_constrained_size_(_size == -1 ? sb_size_ : _size),
             is_angle_(std::dynamic_pointer_cast<StateAngle>(_sb_ptr))
         {
+            assert(_sb_ptr->getLocalSize() == _sb_ptr->getSize());
             assert(sb_constrained_size_+sb_constrained_start_ <= sb_size_);
+            assert(sb_constrained_size_ > 0);
 
             // precompute Jacobian (Identity)
             if (sb_constrained_start_ == 0)
@@ -117,6 +91,30 @@ class FactorBlockAbsolute : public FactorAnalytic
             }
         }
 
+        /** \brief Constructor for the whole state block
+         *
+         * \param _feature_ptr the feature where the factor is emplaced
+         * \param _sb_ptr the constrained state block
+         * \param _processor_ptr processor that created the factor
+         * \param _apply_loss_function if the factor should have loss function
+         * \param _status factor status
+         *
+         */
+        FactorBlockAbsolute(FeatureBasePtr _feature_ptr,
+                            StateBlockPtr _sb_ptr,
+                            ProcessorBasePtr _processor_ptr,
+                            bool _apply_loss_function,
+                            FactorStatus _status = FAC_ACTIVE) :
+            FactorBlockAbsolute(_feature_ptr,
+                                _sb_ptr,
+                                0, 
+                                -1,
+                                _processor_ptr,
+                                _apply_loss_function,
+                                _status)
+        {
+        }
+
         ~FactorBlockAbsolute() override = default;
 
         /** \brief Returns the residual evaluated in the states provided
@@ -137,11 +135,11 @@ class FactorBlockAbsolute : public FactorAnalytic
          *
          **/
         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;
+                               std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+                               const std::vector<bool>& _compute_jacobian) const override;
         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;
+                               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
          *
@@ -164,11 +162,17 @@ inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector<
 
     // residual
     if (is_angle_)
+    {
         return getMeasurementSquareRootInformationUpper() * Eigen::Vector1d(pi2pi(_st_vector.front()(0) - getMeasurement()(0)));
+    }
     else if (sb_constrained_size_ == _st_vector.front().size())
-        return getMeasurementSquareRootInformationUpper() * _st_vector.front() - getMeasurement();
+    {
+        return getMeasurementSquareRootInformationUpper() * (_st_vector.front() - getMeasurement());
+    }
     else
+    {
         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,

include/core/factor/factor_quaternion_absolute.h

@@ -23,7 +23,7 @@
 
 //Wolf includes
 #include "core/factor/factor_autodiff.h"
-#include "core/state_block/local_parametrization_quaternion.h"
+#include "core/state_block/state_quaternion.h"
 #include "core/frame/frame_base.h"
 #include "core/math/rotations.h"
 
@@ -53,7 +53,7 @@ class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3
                                                          _status,
                                                          _sb_ptr)
         {
-            //
+            assert(std::dynamic_pointer_cast<StateQuaternion>(_sb_ptr));
         }
 
         ~FactorQuaternionAbsolute() override = default;

schema/problem/Problem.schema

@@ -8,7 +8,7 @@ problem:
     _type: int
     _mandatory: true
     _options: [2, 3]
-    _doc: Dimension of the problem. '2' for 2D or '3' for 3D
+    _doc: "Dimension of the problem. '2' for 2D or '3' for 3D"
 map:
   _type: derived
   _base: MapBase.schema

schema/problem/Problem2d.schema

@@ -10,4 +10,4 @@ problem:
     _mandatory: false
     _default: 2
     _options: [2]
-    _doc: Dimension of the problem: '2' for 2D
\ No newline at end of file
+    _doc: "Dimension of the problem: '2' for 2D"
\ No newline at end of file

schema/problem/Problem3d.schema

@@ -10,4 +10,4 @@ problem:
     _mandatory: false
     _default: 3
     _options: [3]
-    _doc: Dimension of the problem: '3' for 3D
\ No newline at end of file
+    _doc: "Dimension of the problem: '3' for 3D"
\ No newline at end of file

schema/solver/SolverCeres.schema

@@ -45,7 +45,7 @@ use_nonmonotonic_steps:
   _doc: If the solver is allowed to update the solution with non-monotonic steps. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers.
 
 max_consecutive_nonmonotonic_steps:
-  _mandatory: $use_nonmonotonic_steps
+  _mandatory: false
   _type: unsigned int
-  _default: 0
+  _default: 2
   _doc: Amount of consecutive non-monotonic steps allowed. Only used in LEVENBERG_MARQUARDT and DOGLEG minimizers.
\ No newline at end of file

test/gtest_factor_absolute.cpp

@@ -32,29 +32,40 @@ using namespace wolf;
 using std::cout;
 using std::endl;
 
-Vector10d pose9toPose10(Vector9d _pose9)
-{
-    return (Vector10d() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished();
-}
-
-// Input pose9 and covariance
-Vector9d pose(Vector9d::Random());
-Vector10d pose10 = pose9toPose10(pose);
-Eigen::Matrix<double, 9, 9> data_cov = 0.2 * Eigen::Matrix<double,9,9>::Identity();
-
-// perturbated priors
-Vector10d x0 = pose9toPose10(pose + Vector9d::Random()*0.25);
+int N = 10;
 
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 3);
-SolverCeres solver(problem_ptr);
+ProblemPtr problem;
+SolverCeresPtr solver;
+FrameBasePtr frm;
+CaptureBasePtr cap;
+VectorComposite pose;
+Matrix9d pose_cov = 1e-3 * Matrix9d::Identity();
 
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, problem_ptr->stateZero() );
-
-// Capture
-// auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, 10, 9, nullptr);
-auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pose10, nullptr);
+void randomSetup(int dim)
+{
+    if (problem)
+        problem.reset();
+    if (solver)
+        solver.reset();
+    if (frm)
+        frm->remove();
+
+    // Problem and solver
+    problem = Problem::create("POV", dim);
+    solver = std::make_shared<SolverCeres>(problem);
+
+    // random pose
+    if (dim == 2)
+        pose = VectorComposite("POV", {Vector2d::Random() * 10, Vector1d::Random() * M_PI, Vector2d::Random()*2});
+    else
+        pose = VectorComposite("POV", {Vector3d::Random() * 10, Vector4d::Random().normalized(), Vector3d::Random()*2});
+
+    // frame and capture
+    frm = problem->emplaceFrame(0, pose);
+    cap = CaptureBase::emplace<CaptureBase>(frm, "ABS CAPTURE", 0);
+    
+    ASSERT_TRUE(problem->check(0));
+}
 
 ////////////////////////////////////////////////////////
 /* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
@@ -64,72 +75,152 @@ auto cap0 = CaptureBase::emplace<CaptureMotion>(frm0, "IMU ABS", 0, nullptr, pos
 
 TEST(FactorBlockAbs, fac_block_abs_p)
 {
-    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>());
-    FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(), nullptr, false);
+    for (auto i = 0; i < N; i++)
+    {
+        randomSetup(3);
 
-    ASSERT_TRUE(problem_ptr->check(0));
+        auto fea = FeatureBase::emplace<FeatureBase>(cap, "POSITION", pose.at('P'), 1e-3 * Matrix3d::Identity());
+        FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getP(), nullptr, false);
 
-    // Unfix frame 0, perturb frm0
-    frm0->unfix();
-    frm0->setState(x0,"POV");
+        // Unfix and perturb frame
+        frm->unfix();
+        frm->perturb();
 
-    // solve for frm0
-    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+        // solve for frm
+        std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
 
-    //only orientation is constrained
-    ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
+        //only position is constrained
+        ASSERT_MATRIX_APPROX(frm->getP()->getState(), pose.at('P'), 1e-6);
+    }
 }
 
 TEST(FactorBlockAbs, fac_block_abs_p_tail2)
 {
-    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>());
-    FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getP(),1,2, nullptr, false);
-
-    // Unfix frame 0, perturb frm0
-    frm0->unfix();
-    frm0->setState(x0,"POV");
-
-    // solve for frm0
-    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
-    //only orientation is constrained
-    ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
+    for (auto i = 0; i < N; i++)
+    {
+        randomSetup(3);
+        
+        auto fea = FeatureBase::emplace<FeatureBase>(cap, "POSITION TAIL 2", pose.at('P').tail<2>(), 1e-3 * Matrix2d::Identity());
+        FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getP(),1,2, nullptr, false);
+
+        // Unfix and perturb frame
+        frm->unfix();
+        frm->perturb();
+
+        // solve for frm
+        std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+        
+        //only position tail is constrained
+        ASSERT_MATRIX_APPROX(frm->getP()->getState().tail<2>(), pose.at('P').tail<2>(), 1e-6);
+    }
 }
 
 TEST(FactorBlockAbs, fac_block_abs_v)
 {
-    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureVelocity", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>());
-    FactorBase::emplace<FactorBlockAbsolute>(fea0, fea0, fea0->getFrame()->getV(), nullptr, false);
-
-    ASSERT_TRUE(problem_ptr->check(0));
-    
-    // Unfix frame 0, perturb frm0
-    frm0->unfix();
-    frm0->setState(x0,"POV");
-
-    // solve for frm0
-    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
+    for (auto i = 0; i < N; i++)
+    {
+        randomSetup(3);
+        
+        auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureVelocity", pose.at('V'), 1e-3 * Matrix3d::Identity());
+        FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getV(), nullptr, false);
+
+        ASSERT_TRUE(problem->check(0));
+        
+        // Unfix and perturb frame
+        frm->unfix();
+        frm->perturb();
+
+        // solve for frm
+        std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+        
+        //only velocity is constrained
+        ASSERT_MATRIX_APPROX(frm->getV()->getState(), pose.at('V'), 1e-6);
+    }
+}
 
-    //only velocity is constrained
-    ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
+TEST(FactorQuatAbs, fac_block_abs_q)
+{
+    for (auto i = 0; i < N; i++)
+    {
+        randomSetup(3);
+        
+        auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureQuaternion", pose.at('O'), 1e-3 * Matrix3d::Identity());
+        FactorBase::emplace<FactorQuaternionAbsolute>(fea, fea, fea->getFrame()->getO(), nullptr, false);
+
+        ASSERT_TRUE(problem->check(0));
+        
+        // Unfix and perturb frame
+        frm->unfix();
+        frm->perturb();
+
+        // solve for frm
+        std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+        
+        //only quaternion is constrained
+        ASSERT_QUATERNION_VECTOR_APPROX(frm->getO()->getState(), pose.at('O'), 1e-6);
+    }
 }
 
-TEST(FactorQuatAbs, fac_block_abs_o)
+TEST(FactorQuatAbs, fac_block_abs_angle)
 {
-    auto fea0 = FeatureBase::emplace<FeatureBase>(cap0, "FeatureQuaternion", pose10.segment<4>(3), data_cov.block<3,3>(3,3));
-    FactorBase::emplace<FactorQuaternionAbsolute>(fea0, fea0, fea0->getFrame()->getO(), nullptr, false);
+    for (auto i = 0; i < N; i++)
+    {
+        randomSetup(2);
+        
+        auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureAngle", pose.at('O'), 1e-3 * Matrix1d::Identity());
+        FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getO(), nullptr, false);
+
+        ASSERT_TRUE(problem->check(0));
+        
+        // Unfix and perturb frame
+        frm->unfix();
+        frm->perturb();
+
+        // solve for frm
+        std::string report = solver->solve(wolf::SolverManager::ReportVerbosity::FULL);
+        //std::cout << report << std::endl;
+        
+        //only quaternion is constrained
+        ASSERT_MATRIX_APPROX(frm->getO()->getState(), pose.at('O'), 1e-6);
+    }
+}
 
-    ASSERT_TRUE(problem_ptr->check(0));
+// --------------------------- DEATH TESTS --------------------------------------
+// FactorBlockAbsolute in a StateQuaternion
+TEST(FactorBlockAbs, fac_block_wrong1)
+{
+    randomSetup(3);
     
-    // Unfix frame 0, perturb frm0
-    frm0->unfix();
-    frm0->setState(x0,"POV");
-
-    // solve for frm0
-    std::string brief_report = solver.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
-
-    //only velocity is constrained
-    ASSERT_QUATERNION_VECTOR_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
+    auto fea = FeatureBase::emplace<FeatureBase>(cap, "QUATERNION", pose.at('O'), 1e-3 * Matrix3d::Identity());
+    ASSERT_DEATH(FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getO(), nullptr, false),"");
+}
+// FactorQuaternionAbsolute in a StateBlock
+TEST(FactorBlockAbs, fac_block_wrong2)
+{
+    randomSetup(3);
+    
+    auto fea = FeatureBase::emplace<FeatureBase>(cap, "POSITION", pose.at('P'), 1e-3 * Matrix3d::Identity());
+    ASSERT_DEATH(FactorBase::emplace<FactorQuaternionAbsolute>(fea, fea, fea->getFrame()->getP(), nullptr, false),"");
+}
+// sizes mismatch
+TEST(FactorBlockAbs, fac_block_wrong3)
+{
+    randomSetup(3);
+    
+    auto fea = FeatureBase::emplace<FeatureBase>(cap, "POSITION tail 2", pose.at('P').tail<2>(), 1e-3 * Matrix2d::Identity());
+    ASSERT_DEATH(FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getP(),2,2, nullptr, false),"");
+}
+// sizes mismatch
+TEST(FactorBlockAbs, fac_block_wrong4)
+{
+    randomSetup(2);
+    
+    auto fea = FeatureBase::emplace<FeatureBase>(cap, "ANGLE", pose.at('O'), 1e-3 * Matrix1d::Identity());
+    ASSERT_DEATH(FactorBase::emplace<FactorBlockAbsolute>(fea, fea, fea->getFrame()->getO(),0,2, nullptr, false),"");
 }
 
 int main(int argc, char **argv)