diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 70712879e117b2c54285499756eee5f1c9f322b8..b6fbaced76b9a47f34ee4fe27c29cc18a93b74e3 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/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,
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index f760bd2de64e279c605b524e2c755585ead4edfd..6bae82064b5144fcf21e7c4c36d9f4757667aace 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/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;
diff --git a/schema/problem/Problem.schema b/schema/problem/Problem.schema
index effd887627b2f151d5a7bdd3ba77544584ac3ada..406421cb40f1d44ffa01462ec764b36014637fce 100644
--- a/schema/problem/Problem.schema
+++ b/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
diff --git a/schema/problem/Problem2d.schema b/schema/problem/Problem2d.schema
index ee574b2556c28567e118e26c987a95cf1874c5f7..5aa2e1d229a32a714e14ff4660ef5183117e9976 100644
--- a/schema/problem/Problem2d.schema
+++ b/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
diff --git a/schema/problem/Problem3d.schema b/schema/problem/Problem3d.schema
index f89471756640123a7fe325a3776ce096a5e8735d..7eb62a0597e4badc1e00b7b4bb278b6f07f23e6c 100644
--- a/schema/problem/Problem3d.schema
+++ b/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
diff --git a/schema/solver/SolverCeres.schema b/schema/solver/SolverCeres.schema
index 6310da70265e5dc2fe8beb4e43b0299ff06ddb08..ab4feb415ea6c0bb85649791aef382caa0609fee 100644
--- a/schema/solver/SolverCeres.schema
+++ b/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
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 82e17f02fd966f27b09897ceac054ad449c40025..c41dbd7bd327a60f233d44bb6b08914434e85b58 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/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)