diff --git a/CMakeLists.txt b/CMakeLists.txt
index ac07260c67f935f162754f71d9341a04cb3d7130..a1d16d9bdc66fae4b37a592d417d6d3c00322265 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,12 +132,13 @@ SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_diff_drive.h
include/${PROJECT_NAME}/factor/factor_distance_3d.h
include/${PROJECT_NAME}/factor/factor_pose_2d.h
+ include/${PROJECT_NAME}/factor/factor_pose_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_pose_3d.h
+ include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h
include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h
- include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h
include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h
)
@@ -290,7 +291,6 @@ SET(SRCS_PROCESSOR
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
src/processor/processor_odom_3d.cpp
- src/processor/processor_pose.cpp
src/processor/processor_tracker.cpp
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_landmark.cpp
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index e7313e1f5e73851978574ba757e826400ef44e4c..983da16c49ad9bcc23bff502b80b0e8267723eb1 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.h
@@ -43,14 +43,18 @@ class CapturePose : public CaptureBase
public:
- CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
+ CapturePose(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_covariance);
~CapturePose() override;
virtual void emplaceFeatureAndFactor();
- Eigen::VectorXd getData(){ return data_;}
- Eigen::MatrixXd getDataCovariance(){ return data_covariance_;}
-
+ Eigen::VectorXd getData() const { return data_;}
+ Eigen::MatrixXd getDataCovariance() const { return data_covariance_;}
+ void setData(const Eigen::VectorXd& _data);
+ void setDataCovariance(const Eigen::MatrixXd& _data_covariance);
};
} //namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index 30d4c716834738159fd08ecbe3147438d34bdadf..8b65c4f71f94daa2e751cec491b8e05a1920f148 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.h
@@ -64,7 +64,7 @@ inline bool FactorPose2d::operator ()(const T* const _p, const T* const _o, T* _
{
// measurement
// Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
- auto& meas = getMeasurement();
+ auto& meas = getMeasurement();
// error
Eigen::Matrix<T,3,1> er;
diff --git a/include/core/factor/factor_pose_2d_with_extrinsics.h b/include/core/factor/factor_pose_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9b103a3d50bca689f84dfea672bfdcb0fc20c18
--- /dev/null
+++ b/include/core/factor/factor_pose_2d_with_extrinsics.h
@@ -0,0 +1,92 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of 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/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorPose2dWithExtrinsics);
+
+//class
+class FactorPose2dWithExtrinsics : public FactorAutodiff<FactorPose2dWithExtrinsics, 3, 2, 1, 2, 1>
+{
+ public:
+ FactorPose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose2dWithExtrinsics, 3, 2, 1, 2, 1>("FactorPose2dWithExtrinsics",
+ _top,
+ _ftr_ptr,
+ nullptr, nullptr, nullptr, nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getSensorP(),
+ _ftr_ptr->getCapture()->getSensorO())
+ {
+ assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!");
+ assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!");
+ }
+
+ ~FactorPose2dWithExtrinsics() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _p,
+ const T* const _o,
+ const T* const _sp,
+ const T* const _so,
+ T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorPose2dWithExtrinsics::operator ()(const T* const _p,
+ const T* const _o,
+ const T* const _sp,
+ const T* const _so,
+ T* _residuals) const
+{
+ // MAPS
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p(_p);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > sp(_sp);
+
+ Eigen::Matrix<T, 3, 1> err; // error
+ err.head(2) = getMeasurement().head(2) - (p.head(2) + Eigen::Rotation2D<T>(_o[0]) * sp.head(2));
+ err(2) = pi2pi(getMeasurement()(2) - (_o[0] + _so[0]));
+
+ // Residuals
+ Eigen::Map<Eigen::Matrix<T,3,1> > res(_residuals);
+ res = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
index 3ee3b2315356bff7025ec476d6bf332cfbdfc712..a6be027ee15d0b6be2abc4437e2b3d54d76259c3 100644
--- a/include/core/processor/processor_pose.h
+++ b/include/core/processor/processor_pose.h
@@ -40,13 +40,13 @@ struct ParamsProcessorPose : public ParamsProcessorBase
return ParamsProcessorBase::print();
}
};
-
-WOLF_PTR_TYPEDEFS(ProcessorPose);
//class
-class ProcessorPose : public ProcessorBase{
+template <unsigned int DIM>
+class ProcessorPose : public ProcessorBase
+{
public:
- ProcessorPose(ParamsProcessorPosePtr _params_pfnomove);
+ ProcessorPose(ParamsProcessorPosePtr _params_pose);
~ProcessorPose() override = default;
WOLF_PROCESSOR_CREATE(ProcessorPose, ParamsProcessorPose);
@@ -62,8 +62,138 @@ class ProcessorPose : public ProcessorBase{
void createFactorIfNecessary();
protected:
- ParamsProcessorPosePtr params_pfnomove_;
- bool is_2d_;
+ ParamsProcessorPosePtr params_pose_;
};
-} /* namespace wolf */
\ No newline at end of file
+typedef ProcessorPose<2> ProcessorPose2d;
+typedef ProcessorPose<3> ProcessorPose3d;
+
+WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose2d);
+WOLF_DECLARED_PTR_TYPEDEFS(ProcessorPose3d);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+// IMPLEMENTATION //
+////////////////////////////////////////////////////////////////////////////////////////////////
+
+#include "core/sensor/sensor_pose.h"
+#include "core/capture/capture_pose.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+
+namespace wolf {
+
+template<unsigned int DIM>
+inline ProcessorPose<DIM>::ProcessorPose(ParamsProcessorPosePtr _params_pose) :
+ ProcessorBase("ProcessorPose", DIM, _params_pose),
+ params_pose_(_params_pose)
+{
+ static_assert(DIM == 2 or DIM == 3);
+}
+
+template<unsigned int DIM>
+void ProcessorPose<DIM>::configure(SensorBasePtr _sensor)
+{
+ if (not std::dynamic_pointer_cast<SensorPose<DIM>>(_sensor))
+ {
+ throw std::runtime_error("Configuring a ProcessorPose with a sensor " + _sensor->getType() + ", should be SensorPose" + toString(DIM) + "d");
+ }
+}
+
+template<unsigned int DIM>
+void ProcessorPose<DIM>::createFactorIfNecessary()
+{
+ auto kf_it_last = buffer_frame_.getContainer().end();
+ auto kf_it = buffer_frame_.getContainer().begin();
+ while (kf_it != buffer_frame_.getContainer().end())
+ {
+ TimeStamp t = kf_it->first;
+ auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
+
+ // if capture with corresponding timestamp is not found, assume you will get it later
+ if (cap_it != buffer_capture_.getContainer().end())
+ {
+ // if a corresponding capture exists, link it to the KF and create a factor
+ auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
+ cap->link(kf_it->second);
+ FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
+
+ if (DIM == 2)
+ {
+ // TODO
+ throw std::runtime_error("FactorPose2dWithExtrinsics not implemented");
+ }
+ else
+ {
+ FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_ABS);
+ }
+ // erase removes range [first, last): it does not removes last
+ // so increment the iterator so that it points to the next element in the container
+ buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
+
+ // we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
+ kf_it_last = kf_it;
+ }
+
+ kf_it++;
+ }
+
+ // whatever happened, remove very old captures
+ buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
+
+ // now we erase the kf buffer if there was a match
+ if (kf_it_last != buffer_frame_.getContainer().end()){
+ buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
+ }
+
+}
+
+template<unsigned int DIM>
+inline void ProcessorPose<DIM>::processCapture(CaptureBasePtr _capture)
+{
+ if (!_capture)
+ {
+ WOLF_ERROR("Received capture is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("processCapture: Frame buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("processCapture: Capture buffer empty, time ", _capture->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+
+}
+
+template<unsigned int DIM>
+inline void ProcessorPose<DIM>::processKeyFrame(FrameBasePtr _keyframe_ptr)
+{
+ if (!_keyframe_ptr)
+ {
+ WOLF_ERROR("Received KF is nullptr.");
+ return;
+ }
+ // nothing to do if any of the two buffer is empty
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPose: Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ return;
+ }
+
+ createFactorIfNecessary();
+}
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorPose2d);
+WOLF_REGISTER_PROCESSOR(ProcessorPose3d);
+} // namespace wolf
\ No newline at end of file
diff --git a/schema/processor/ProcessorPose2d.schema b/schema/processor/ProcessorPose2d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..3c97f7469fc38b34aa0998e8c96fc47d177e4aa9
--- /dev/null
+++ b/schema/processor/ProcessorPose2d.schema
@@ -0,0 +1 @@
+follow: ProcessorBase.schema
\ No newline at end of file
diff --git a/schema/processor/ProcessorPose3d.schema b/schema/processor/ProcessorPose3d.schema
new file mode 100644
index 0000000000000000000000000000000000000000..3c97f7469fc38b34aa0998e8c96fc47d177e4aa9
--- /dev/null
+++ b/schema/processor/ProcessorPose3d.schema
@@ -0,0 +1 @@
+follow: ProcessorBase.schema
\ No newline at end of file
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 113ac5710753ffd6c81de747977b914046fefc5c..c493d8fc720411a957c4b16640d8a7d97571dff4 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -28,7 +28,9 @@ CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const
data_(_data),
data_covariance_(_data_covariance)
{
- //
+ assert(_data.size() == 3 or _data.size() == 7);
+ assert(_data_covariance.rows() == _data_covariance.cols());
+ assert(_data_covariance.rows() == 3 or _data_covariance.rows() == 6);
}
CapturePose::~CapturePose()
@@ -36,6 +38,21 @@ CapturePose::~CapturePose()
//
}
+void CapturePose::setData(const Eigen::VectorXd& _data)
+{
+ assert(_data.size() == 3 or _data.size() == 7);
+
+ data_ = _data;
+}
+
+void CapturePose::setDataCovariance(const Eigen::MatrixXd& _data_covariance)
+{
+ assert(_data_covariance.rows() == _data_covariance.cols());
+ assert(_data_covariance.rows() == 3 or _data_covariance.rows() == 6);
+
+ data_covariance_ = _data_covariance;
+}
+
void CapturePose::emplaceFeatureAndFactor()
{
// Emplace feature
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
deleted file mode 100644
index 10a20636ef40cfb0bfca5e8019fb90d31b40cb1c..0000000000000000000000000000000000000000
--- a/src/processor/processor_pose.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
-//
-// This file is part of 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/>.
-//
-//--------LICENSE_END--------
-/**
- * \file processor_pose.cpp
- *
- * Created on: Feb 19, 2020
- * \author: mfourmy
- */
-
-#include "core/processor/processor_pose.h"
-
-#include "core/sensor/sensor_pose.h"
-#include "core/capture/capture_pose.h"
-#include "core/factor/factor_pose_3d_with_extrinsics.h"
-
-
-namespace wolf
-{
-
-ProcessorPose::ProcessorPose(ParamsProcessorPosePtr _params_pfnomove) :
- ProcessorBase("ProcessorPose", 0, _params_pfnomove),
- params_pfnomove_(std::make_shared<ParamsProcessorPose>(*_params_pfnomove))
-{
-
-}
-
-void ProcessorPose::configure(SensorBasePtr _sensor)
-{
- auto sen_pose_2d = std::dynamic_pointer_cast<SensorPose2d>(_sensor);
- auto sen_pose_3d = std::dynamic_pointer_cast<SensorPose3d>(_sensor);
-
- if (not sen_pose_2d and not sen_pose_3d)
- {
- throw std::runtime_error("Configuring a ProcessorPose with a sensor " + _sensor->getType() + ", should be SensorPose2d or SensorPose3d");
- }
- is_2d_ = sen_pose_2d != nullptr;
-}
-
-void ProcessorPose::createFactorIfNecessary()
-{
- auto kf_it_last = buffer_frame_.getContainer().end();
- auto kf_it = buffer_frame_.getContainer().begin();
- while (kf_it != buffer_frame_.getContainer().end())
- {
- TimeStamp t = kf_it->first;
- auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
-
- // if capture with corresponding timestamp is not found, assume you will get it later
- if (cap_it != buffer_capture_.getContainer().end())
- {
- // if a corresponding capture exists, link it to the KF and create a factor
- auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
- cap->link(kf_it->second);
- FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
-
- if (is_2d_)
- {
- // TODO
- throw std::runtime_error("FactorPose2dWithExtrinsics not implemented");
- }
- else
- {
- FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_ABS);
- }
- // erase removes range [first, last): it does not removes last
- // so increment the iterator so that it points to the next element in the container
- buffer_capture_.getContainer().erase(buffer_capture_.getContainer().begin(), std::next(cap_it));
-
- // we cannot erase on the kf buffer since we are looping over it so we store the iterator for later
- kf_it_last = kf_it;
- }
-
- kf_it++;
- }
-
- // whatever happened, remove very old captures
- buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
-
- // now we erase the kf buffer if there was a match
- if (kf_it_last != buffer_frame_.getContainer().end()){
- buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
- }
-
-}
-
-inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
-{
- if (!_capture)
- {
- WOLF_ERROR("Received capture is nullptr.");
- return;
- }
- // nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("processCapture: Frame buffer empty, time ", _capture->getTimeStamp());
- return;
- }
- if(buffer_frame_.empty()){
- WOLF_DEBUG("processCapture: Capture buffer empty, time ", _capture->getTimeStamp());
- return;
- }
-
- createFactorIfNecessary();
-
-}
-
-inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr)
-{
- if (!_keyframe_ptr)
- {
- WOLF_ERROR("Received KF is nullptr.");
- return;
- }
- // nothing to do if any of the two buffer is empty
- if(buffer_frame_.empty()){
- WOLF_DEBUG("ProcessorPose: Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
- return;
- }
- if(buffer_frame_.empty()){
- WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
- return;
- }
-
- createFactorIfNecessary();
-}
-
-
-} /* namespace wolf */
-
-// Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorPose);
-} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 7299924ed68ba9bcf93f1a191bfd3401a1062a0a..ddbdd9a5d31f42c4b3b33dc5ef59cbd709da88c5 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -147,9 +147,15 @@ wolf_add_gtest(gtest_factor_odom_2d_autodiff gtest_factor_odom_2d_autodiff.cpp)
# FactorPose2d class test
wolf_add_gtest(gtest_factor_pose_2d gtest_factor_pose_2d.cpp)
+# FactorPose2dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_pose_2d_with_extrinsics gtest_factor_pose_2d_with_extrinsics.cpp)
+
# FactorPose3d class test
wolf_add_gtest(gtest_factor_pose_3d gtest_factor_pose_3d.cpp)
+# FactorPose3dWithExtrinsics class test
+wolf_add_gtest(gtest_factor_pose_3d_with_extrinsics gtest_factor_pose_3d_with_extrinsics.cpp)
+
# FactorRelativePose2d class test
wolf_add_gtest(gtest_factor_relative_pose_2d gtest_factor_relative_pose_2d.cpp)
diff --git a/test/gtest_factor_pose_2d_with_extrinsics.cpp b/test/gtest_factor_pose_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..459d557d7630170a681544fb5483a2f4cc4363ef
--- /dev/null
+++ b/test/gtest_factor_pose_2d_with_extrinsics.cpp
@@ -0,0 +1,144 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of 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/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_pose_2d_with_extrinsics.h"
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/processor/processor_pose.h"
+#include "core/math/rotations.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+// Input odometry data and covariance
+Matrix3d data_cov = 0.1*Matrix3d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor_pose2d = problem_ptr->installSensor("SensorPose2d", wolf_root + "/test/yaml/sensor_pose_2d.yaml", {wolf_root});
+auto processor_pose2d = ProcessorBase::emplace<ProcessorPose2d>(sensor_pose2d, std::make_shared<ParamsProcessorPose>());
+
+// Two frames
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() );
+
+// Capture for frm
+auto cap = CaptureBase::emplace<CapturePose>(frm, 0, sensor_pose2d, Vector3d::Zero(), data_cov);
+
+TEST(FactorPose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // measurement
+ Vector3d meas;
+ meas(2) = pi2pi(x0(2) + xs(2));
+ meas.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * xs.head<2>();
+
+ // Set states
+ frm->setState(x0,"PO");
+ sensor_pose2d->setState(xs, "PO");
+
+ // feature & factor with meas measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose2d", meas, data_cov);
+ FactorBase::emplace<FactorPose2dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
+
+ // Perturb frm, fix the extrinsics
+ frm->unfix();
+ sensor_pose2d->fix();
+ frm->perturb(10);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random pose
+ Vector3d x0 = Vector3d::Random() * 10;
+ pi2pi(x0(2));
+
+ // Random extrinsics
+ Vector3d xs = Vector3d::Random();
+ pi2pi(xs(2));
+
+ // measurement
+ Vector3d meas;
+ meas(2) = pi2pi(x0(2) + xs(2));
+ meas.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * xs.head<2>();
+
+ // Set states
+ frm->setState(x0,"PO");
+ sensor_pose2d->setState(xs, "PO");
+
+ // feature & factor with meas measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose2d", meas, data_cov);
+ FactorBase::emplace<FactorPose2dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
+
+ // Perturb extrinsics, fix the frame
+ frm->fix();
+ sensor_pose2d->unfix();
+ sensor_pose2d->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_POSE2d_APPROX(sensor_pose2d->getStateVector("PO"), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_pose_3d_with_extrinsics.cpp b/test/gtest_factor_pose_3d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4631da1cf4e5279f3d64628036115e13d31ba5be
--- /dev/null
+++ b/test/gtest_factor_pose_3d_with_extrinsics.cpp
@@ -0,0 +1,144 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of 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/>.
+//
+//--------LICENSE_END--------
+#include "core/utils/utils_gtest.h"
+
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_pose_3d_with_extrinsics.h"
+#include "core/capture/capture_pose.h"
+#include "core/sensor/sensor_pose.h"
+#include "core/processor/processor_pose.h"
+#include "core/math/rotations.h"
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+// Input odometry data and covariance
+Matrix6d data_cov = 0.1*Matrix6d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor_pose3d = problem_ptr->installSensor("SensorPose3d", wolf_root + "/test/yaml/sensor_pose_3d.yaml", {wolf_root});
+auto processor_pose2d = ProcessorBase::emplace<ProcessorPose3d>(sensor_pose3d, std::make_shared<ParamsProcessorPose>());
+
+// Two frames
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished() );
+
+// Capture for frm
+auto cap = CaptureBase::emplace<CapturePose>(frm, 0, sensor_pose3d, Vector7d::Zero(), data_cov);
+
+TEST(FactorPose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorPose2dWithExtrinsics, solve_f_fix_s)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random pose
+ Vector7d x0 = Vector7d::Random() * 10;
+ x0.tail(4).normalize();
+
+ // Random extrinsics
+ Vector7d xs = Vector7d::Random();
+ xs.tail(4).normalize();
+
+ // measurement
+ Vector7d meas;
+ meas.head<3>() = x0.head<3>() + Quaterniond(x0.tail<4>()) * xs.head<3>();
+ meas.tail<4>() = (Quaterniond(x0.tail<4>()) * Quaterniond(xs.tail<4>())).coeffs();
+
+ // Set states
+ frm->setState(x0,"PO");
+ sensor_pose3d->setState(xs, "PO");
+
+ // feature & factor with meas measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose3d", meas, data_cov);
+ FactorBase::emplace<FactorPose3dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
+
+ // Perturb frm, fix the extrinsics
+ frm->unfix();
+ sensor_pose3d->fix();
+ frm->perturb(10);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorPose2dWithExtrinsics, fix_f_solve_s)
+{
+ for (int i = 0; i < 1e2; i++)
+ {
+ // Random pose
+ Vector7d x0 = Vector7d::Random() * 10;
+ x0.tail(4).normalize();
+
+ // Random extrinsics
+ Vector7d xs = Vector7d::Random();
+ xs.tail(4).normalize();
+
+ // measurement
+ Vector7d meas;
+ meas.head<3>() = x0.head<3>() + Quaterniond(x0.tail<4>()) * xs.head<3>();
+ meas.tail<4>() = (Quaterniond(x0.tail<4>()) * Quaterniond(xs.tail<4>())).coeffs();
+
+ // Set states
+ frm->setState(x0,"PO");
+ sensor_pose3d->setState(xs, "PO");
+
+ // feature & factor with meas measurement
+ auto fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeaturePose3d", meas, data_cov);
+ FactorBase::emplace<FactorPose3dWithExtrinsics>(fea1, fea1, processor_pose2d, false, TOP_ABS);
+
+ // Perturb extrinsics, fix the frame
+ frm->fix();
+ sensor_pose3d->unfix();
+ sensor_pose3d->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_POSE3d_APPROX(sensor_pose3d->getStateVector("PO"), xs, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index c7897da7108bd0001116bf1c497901ecaad5f7dd..b5e8188673f1ba77d38e984a14472da22d7b628f 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -129,7 +129,7 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
auto params_proc = std::make_shared<ParamsProcessorPose>();
params_proc->name = "pose processor";
params_proc->time_tolerance = 0.5;
- auto proc_pose = ProcessorBase::emplace<ProcessorPose>(sensor_pose_, params_proc);
+ auto proc_pose = ProcessorBase::emplace<ProcessorPose3d>(sensor_pose_, params_proc);
}
void TearDown() override{};