From 626a8a5be874522c4bcd47377ee953aa744f51b5 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 6 Feb 2025 11:08:58 +0100
Subject: [PATCH] implemented FactorHomogeneous3dAbsolute and tests
---
.../factor/factor_homogeneous_3d_absolute.h | 91 +++++++++++++++++++
src/common/node_state_blocks.cpp | 9 +-
test/gtest_factor_absolute.cpp | 74 ++++++++++++++-
3 files changed, 168 insertions(+), 6 deletions(-)
create mode 100644 include/core/factor/factor_homogeneous_3d_absolute.h
diff --git a/include/core/factor/factor_homogeneous_3d_absolute.h b/include/core/factor/factor_homogeneous_3d_absolute.h
new file mode 100644
index 000000000..f19bf8aef
--- /dev/null
+++ b/include/core/factor/factor_homogeneous_3d_absolute.h
@@ -0,0 +1,91 @@
+// WOLF - Copyright (C) 2020,2021,2022,2023,2024,2025
+// Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+// Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/state_block/state_homogeneous_3d.h"
+#include "core/common/node_state_blocks.h"
+#include "core/math/rotations.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(FactorHomogeneous3dAbsolute);
+
+// class
+class FactorHomogeneous3dAbsolute : public FactorAutodiff<FactorHomogeneous3dAbsolute, 3, 4>
+{
+ public:
+ /** \brief Constructor
+ *
+ * \param _measurement the measurement (3D euclidean point or 4D homogeneous point)
+ * \param _measurement_sqrt_information_upper the square root factorization of the measurement noise information matrix
+ * \param _node_state_blocks the NodeStateBlocks (Frame,Capture,Landmark,Sensor) containing the factored state block
+ * \param _key the key of the factored 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
+ *
+ */
+ FactorHomogeneous3dAbsolute(const Eigen::VectorXd& _measurement,
+ const Eigen::MatrixXd& _measurement_sqrt_information_upper,
+ const NodeStateBlocksPtr& _node_state_blocks,
+ const char& _key,
+ ProcessorBasePtr _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE)
+ : FactorAutodiff("FactorHomogeneous3dAbsolute",
+ TOP_ABS,
+ _measurement.size() == 4 ? _measurement.head<3>() / _measurement(3) : _measurement,
+ _measurement_sqrt_information_upper,
+ {_node_state_blocks},
+ _processor_ptr,
+ {_node_state_blocks->getStateBlock(_key)},
+ _apply_loss_function,
+ _status)
+ {
+ assert(_node_state_blocks->getStateBlock(_key) &&
+ "FactorHomogeneous3dAbsolute: _node_state_blocks does not have _key");
+ assert(std::dynamic_pointer_cast<StateHomogeneous3d>(_node_state_blocks->getStateBlock(_key)) &&
+ "FactorHomogeneous3dAbsolute: state block should be of type StateHomogeneous3d");
+ assert(_measurement.size() == 3 or _measurement.size() == 4);
+ MatrixSizeCheck<3, 1>::check(measurement_);
+ MatrixSizeCheck<3, 3>::check(_measurement_sqrt_information_upper);
+ }
+
+ ~FactorHomogeneous3dAbsolute() override = default;
+
+ template <typename T>
+ bool operator()(const T* const _h, T* _residuals) const;
+};
+
+template <typename T>
+inline bool FactorHomogeneous3dAbsolute::operator()(const T* const _h, T* _residuals) const
+{
+ // residual (measurement is 3D point)
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
+ res = getMeasurementSquareRootInformationUpper() *
+ (getMeasurement() - Eigen::Matrix<T, 3, 1>(_h[0] / _h[3], _h[1] / _h[3], _h[2] / _h[3]));
+
+ return true;
+}
+
+} // namespace wolf
diff --git a/src/common/node_state_blocks.cpp b/src/common/node_state_blocks.cpp
index a1159a572..5e9c39779 100644
--- a/src/common/node_state_blocks.cpp
+++ b/src/common/node_state_blocks.cpp
@@ -24,6 +24,7 @@
#include "core/math/covariance.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_homogeneous_3d_absolute.h"
using namespace Eigen;
@@ -199,12 +200,16 @@ void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
_key + ", factor " + std::to_string(factor_prior_map_.at(_key)->id()));
// EMPLACE FACTOR
- bool is_quaternion = (std::dynamic_pointer_cast<StateQuaternion>(_sb) != nullptr);
- if (is_quaternion)
+ if (std::dynamic_pointer_cast<StateQuaternion>(_sb))
{
factor_prior_map_[_key] = FactorBase::emplace<FactorQuaternionAbsolute>(
nullptr, _x, computeSqrtUpper(_cov.inverse()), shared_from_this(), _key, nullptr, false);
}
+ else if (std::dynamic_pointer_cast<StateHomogeneous3d>(_sb))
+ {
+ factor_prior_map_[_key] = FactorBase::emplace<FactorHomogeneous3dAbsolute>(
+ nullptr, _x, computeSqrtUpper(_cov.inverse()), shared_from_this(), _key, nullptr, false);
+ }
else
{
factor_prior_map_[_key] = FactorBase::emplace<FactorBlockAbsolute>(nullptr,
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 431d0a2ce..a0a89c2f2 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -22,6 +22,7 @@
#include "core/utils/utils_gtest.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_homogeneous_3d_absolute.h"
#include "core/capture/capture_motion.h"
using namespace Eigen;
@@ -46,7 +47,7 @@ void randomSetup(int dim)
if (frm) frm->remove();
// Problem and solver
- problem = Problem::create(dim);
+ problem = Problem::create(dim, {{'H', "StateHomogeneous3d"}});
solver = SolverCeres::create(problem, wolf_dir + "/test/yaml/solver_ceres.yaml", {wolf_dir});
// random pose
@@ -54,8 +55,10 @@ void randomSetup(int dim)
pose = VectorComposite{
{'P', Vector2d::Random() * 10}, {'O', Vector1d::Random() * M_PI}, {'V', Vector2d::Random() * 2}};
else
- pose = VectorComposite{
- {'P', Vector3d::Random() * 10}, {'O', Vector4d::Random().normalized()}, {'V', Vector3d::Random() * 2}};
+ pose = VectorComposite{{'P', Vector3d::Random() * 10},
+ {'O', Vector4d::Random().normalized()},
+ {'V', Vector3d::Random() * 2},
+ {'H', (Vector4d() << Vector3d::Random() * 10, 1).finished()}};
;
// frame and capture
@@ -192,7 +195,7 @@ TEST(FactorQuatAbs, fac_block_abs_q)
}
}
-TEST(FactorQuatAbs, fac_block_abs_angle)
+TEST(FactorAngleAbs, fac_block_abs_angle)
{
for (auto i = 0; i < N; i++)
{
@@ -222,6 +225,69 @@ TEST(FactorQuatAbs, fac_block_abs_angle)
}
}
+TEST(FactorHomogAbs, fac_homog_abs_4dmeas)
+{
+ for (auto i = 0; i < N; i++)
+ {
+ randomSetup(3);
+
+ // measurement 4D (homogeneous) multiplied by an arbitrary number
+ auto fea = FeatureBase::emplace<FeatureBase>(cap, "H_POS", 10.56 * pose.at('H'), 1e-3 * Matrix3d::Identity());
+ FactorBase::emplace<FactorHomogeneous3dAbsolute>(fea,
+ fea->getMeasurement(),
+ fea->getMeasurementSquareRootInformationUpper(),
+ fea->getFrame(),
+ 'H',
+ 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 is constrained
+ ASSERT_MATRIX_APPROX(frm->getStateBlock('H')->getState().head<3>() / frm->getStateBlock('H')->getState()(3),
+ pose.at('H').head<3>() / pose.at('H')(3),
+ 1e-6);
+ }
+}
+
+TEST(FactorHomogAbs, fac_homog_abs_3dmeas)
+{
+ for (auto i = 0; i < N; i++)
+ {
+ randomSetup(3);
+
+ // measurement 3D (euclidean point)
+ auto fea =
+ FeatureBase::emplace<FeatureBase>(cap, "POSITION", pose.at('H').head<3>(), 1e-3 * Matrix3d::Identity());
+ FactorBase::emplace<FactorHomogeneous3dAbsolute>(fea,
+ fea->getMeasurement(),
+ fea->getMeasurementSquareRootInformationUpper(),
+ fea->getFrame(),
+ 'H',
+ 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 is constrained
+ ASSERT_MATRIX_APPROX(frm->getStateBlock('H')->getState().head<3>() / frm->getStateBlock('H')->getState()(3),
+ pose.at('H').head<3>() / pose.at('H')(3),
+ 1e-6);
+ }
+}
+
// --------------------------- DEATH TESTS --------------------------------------
// FactorBlockAbsolute in a StateQuaternion
TEST(FactorBlockAbs, fac_block_wrong1)
--
GitLab