diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index 5b9ccf175d4415b891f500e17663027cccd8d139..88f28a3880d513ace93166d6e38cc80a178455be 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -61,6 +61,27 @@ class FactorAnalytic: public FactorBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
+ FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
+
~FactorAnalytic() override = default;
/** \brief Returns a vector of pointers to the states
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 3807d1210081a90c43562377cf266910aad13b6d..c10c0b58b799f6171a418c9a6217b4d69a9e6f88 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -110,10 +110,10 @@ class FactorAutodiff : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -461,10 +461,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -776,10 +776,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -1079,10 +1079,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -1370,10 +1370,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -1648,10 +1648,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -1729,13 +1729,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
if (jacobians == nullptr)
{
(*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- parameters[6],
- residuals);
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ residuals);
}
// also compute jacobians
else
@@ -1747,13 +1747,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
// call functor
(*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- jets_6_.data(),
- residuals_jets_.data());
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
@@ -1914,10 +1914,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -1990,12 +1990,12 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
if (jacobians == nullptr)
{
(*static_cast<FacT const*>(this))(parameters[0],
- parameters[1],
- parameters[2],
- parameters[3],
- parameters[4],
- parameters[5],
- residuals);
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ residuals);
}
// also compute jacobians
else
@@ -2007,12 +2007,12 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
// call functor
(*static_cast<FacT const*>(this))(jets_0_.data(),
- jets_1_.data(),
- jets_2_.data(),
- jets_3_.data(),
- jets_4_.data(),
- jets_5_.data(),
- residuals_jets_.data());
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ residuals_jets_.data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
@@ -2163,10 +2163,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -2400,10 +2400,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -2629,10 +2629,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -2846,10 +2846,10 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
@@ -3050,10 +3050,10 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
FactorAutodiff(_tp,
_top,
_feature_ptr,
- FrameBasePtrList({_frame_other_ptr}),
- CaptureBasePtrList({_capture_other_ptr}),
- FeatureBasePtrList({_feature_other_ptr}),
- LandmarkBasePtrList({_landmark_other_ptr}),
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index aa942ce6c8468924775c74b6763eb796f0cfaed7..3be53aee698ecda49d198f8164c829cf368c2942 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -104,23 +104,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
void setProblem(ProblemPtr) override final;
public:
- /** \brief Default constructor.
- *
- * IMPORTANT: "other" means "of another branch of the wolf tree".
- * You should only provide a non-nullptr in frame/capture/feature_other_ptr in case of a frame/capture/feature involved in this factor
- * that does not located in the same branch.
- **/
- FactorBase(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status = FAC_ACTIVE);
-
FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index cfd768ec8ad6d4a9e3039b82c1bb0c8f58d7ac4c..b41394215b429155edd7d23afcc7d41916456c40 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -32,10 +32,45 @@ FactorAnalytic::FactorAnalytic(const std::string& _tp,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, FactorStatus _status,
+ bool _apply_loss_function,
+ FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- FactorBase(_tp, _top, _feature_ptr, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAnalytic(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
+ _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr){}
+
+FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
+ FactorBase(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ _processor_ptr,
+ _apply_loss_function,
+ _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_ptr_const_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 891a319de0bc62e43aa3493164be9f41168f8d30..37e1bcdaa4625017048c5104dd120d6f71c41674 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -27,58 +27,6 @@ namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
-FactorBase::FactorBase(const std::string& _tp,
- const FactorTopology& _top,
- const FeatureBasePtr& _feature_ptr,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- NodeBase("FACTOR", _tp),
- feature_ptr_(), // will be filled in link()
- processor_ptr_(_processor_ptr),
- frame_other_list_(),
- capture_other_list_(),
- feature_other_list_(),
- landmark_other_list_(),
- frame_other_const_list_(),
- capture_other_const_list_(),
- feature_other_const_list_(),
- landmark_other_const_list_(),
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function)
-{
- if (_frame_other_ptr)
- {
- frame_other_list_.push_back(_frame_other_ptr);
- frame_other_const_list_.push_back(_frame_other_ptr);
- }
- if (_capture_other_ptr)
- {
- capture_other_list_.push_back(_capture_other_ptr);
- capture_other_const_list_.push_back(_capture_other_ptr);
- }
- if (_feature_other_ptr)
- {
- feature_other_list_.push_back(_feature_other_ptr);
- feature_other_const_list_.push_back(_feature_other_ptr);
- }
- if (_landmark_other_ptr)
- {
- landmark_other_list_.push_back(_landmark_other_ptr);
- landmark_other_const_list_.push_back(_landmark_other_ptr);
- }
-
- assert(_feature_ptr && "null feature pointer when creating a factor");
- measurement_ = _feature_ptr->getMeasurement();
- measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
-}
-
FactorBase::FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -91,7 +39,7 @@ FactorBase::FactorBase(const std::string& _tp,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
- processor_ptr_(_processor_ptr),
+ processor_ptr_(_processor_ptr),
frame_other_list_(),
capture_other_list_(),
feature_other_list_(),
@@ -105,22 +53,22 @@ FactorBase::FactorBase(const std::string& _tp,
status_(_status),
apply_loss_function_(_apply_loss_function)
{
- for (auto& Fo : _frame_other_list)
+ for (auto Fo : _frame_other_list)
{
frame_other_list_.push_back(Fo);
frame_other_const_list_.push_back(Fo);
}
- for (auto& Co : _capture_other_list)
+ for (auto Co : _capture_other_list)
{
capture_other_list_.push_back(Co);
capture_other_const_list_.push_back(Co);
}
- for (auto& fo : _feature_other_list)
+ for (auto fo : _feature_other_list)
{
feature_other_list_.push_back(fo);
feature_other_const_list_.push_back(fo);
}
- for (auto& Lo : landmark_other_list_)
+ for (auto Lo : _landmark_other_list)
{
landmark_other_list_.push_back(Lo);
landmark_other_const_list_.push_back(Lo);
diff --git a/test/dummy/factor_dummy.h b/test/dummy/factor_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..73174d1468e07807e26ab2a6dcf4579ad5a00ef8
--- /dev/null
+++ b/test/dummy/factor_dummy.h
@@ -0,0 +1,112 @@
+//--------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--------
+#ifndef FACTOR_DUMMY_H
+#define FACTOR_DUMMY_H
+
+#include "core/factor/factor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorDummy);
+
+class FactorDummy : public FactorBase
+{
+ public:
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtr& _frame_other,
+ const CaptureBasePtr& _capture_other,
+ const FeatureBasePtr& _feature_other,
+ const LandmarkBasePtr& _landmark_other) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other ? FrameBasePtrList({_frame_other}) : FrameBasePtrList(),
+ _capture_other ? CaptureBasePtrList({_capture_other}) : CaptureBasePtrList(),
+ _feature_other ? FeatureBasePtrList({_feature_other}) : FeatureBasePtrList(),
+ _landmark_other ? LandmarkBasePtrList({_landmark_other}) : LandmarkBasePtrList(),
+ nullptr,
+ false)
+ {
+ //
+ }
+ FactorDummy(const FeatureBasePtr& _feature,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list) :
+ FactorBase("Dummy",
+ TOP_OTHER,
+ _feature,
+ _frame_other_list,
+ _capture_other_list,
+ _feature_other_list,
+ _landmark_other_list,
+ nullptr,
+ false)
+ {
+ //
+ }
+ ~FactorDummy() override = default;
+
+ bool evaluate(double const* const* _parameters,
+ double* _residuals,
+ double** _jacobians) const override
+ {
+ return true;
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr,
+ Eigen::VectorXd& _residual,
+ std::vector<Eigen::MatrixXd>& _jacobians) const override
+ {
+
+ }
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JacobianMethod::JAC_ANALYTIC;
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return std::vector<StateBlockConstPtr>();
+ }
+
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return std::vector<StateBlockPtr>();
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return std::vector<unsigned int>();
+ }
+
+ unsigned int getSize() const override
+ {
+ return 0;
+ }
+};
+
+}
+
+#endif // FACTOR_DUMMY_H
\ No newline at end of file
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 9d6a62c6715436fcb3182443da1ad9af91b4c08a..b8f8266c43dea8060c28d5677cbb1082527f11a8 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -79,10 +79,10 @@ inline FactorFeatureDummy::FactorFeatureDummy(const FeatureBasePtr& _feature_ptr
FactorBase("FactorFeatureDummy",
TOP_OTHER,
_feature_ptr,
- nullptr,
- nullptr,
- _feature_other_ptr,
- nullptr,
+ FrameBasePtrList(),
+ CaptureBasePtrList(),
+ FeatureBasePtrList({_feature_other_ptr}),
+ LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status)
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index 8ff4c63f114594d5b4c5de2a2900e7741c172e42..ceff8247e73c02bb8ac9bb2ca7151486e3f69f87 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -92,10 +92,10 @@ inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& _feature_p
FactorBase("FactorFeatureDummy",
TOP_OTHER,
_feature_ptr,
- nullptr,
- nullptr,
- nullptr,
- _landmark_other_ptr,
+ FrameBasePtrList(),
+ CaptureBasePtrList(),
+ FeatureBasePtrList(),
+ LandmarkBasePtrList({_landmark_other_ptr}),
_processor_ptr,
_apply_loss_function,
_status)
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index 4c8df1549d4c59a02b3d9bb754eca4e2b6b0533b..fca61dad1708a8ad71ff73b8049cbf99c0d3d9bf 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -29,6 +29,7 @@
#include "core/utils/utils_gtest.h"
#include "core/factor/factor_base.h"
+#include "dummy/factor_dummy.h"
using namespace wolf;
using namespace Eigen;
@@ -55,59 +56,6 @@ class FactorBaseTest : public testing::Test
// virtual void TearDown(){}
};
-class FactorDummy : public FactorBase
-{
- public:
- FactorDummy(const FeatureBasePtr& _feature,
- const FrameBasePtr& _frame_other,
- const CaptureBasePtr& _capture_other,
- const FeatureBasePtr& _feature_other,
- const LandmarkBasePtr& _landmark_other) :
- FactorBase("Dummy",
- TOP_OTHER,
- _feature,
- _frame_other,
- _capture_other,
- _feature_other,
- _landmark_other,
- nullptr,
- false)
- {
- //
- }
- FactorDummy(const FeatureBasePtr& _feature,
- const FrameBasePtrList& _frame_other_list,
- const CaptureBasePtrList& _capture_other_list,
- const FeatureBasePtrList& _feature_other_list,
- const LandmarkBasePtrList& _landmark_other_list) :
- FactorBase("Dummy",
- TOP_OTHER,
- _feature,
- _frame_other_list,
- _capture_other_list,
- _feature_other_list,
- _landmark_other_list,
- nullptr,
- false)
- {
- //
- }
- ~FactorDummy() override = default;
-
- bool evaluate(double const* const* _parameters,
- double* _residuals,
- double** _jacobians) const override {return true;}
- void evaluate(const std::vector<const double*>& _states_ptr,
- Eigen::VectorXd& _residual,
- std::vector<Eigen::MatrixXd>& _jacobians) const override {}
- JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
- std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockConstPtr>();}
- std::vector<StateBlockPtr> getStateBlockPtrVector() override {return std::vector<StateBlockPtr>();}
- std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>();}
- unsigned int getSize() const override {return 0;}
-
-};
-
TEST_F(FactorBaseTest, constructor_from_pointers)
{
FactorDummy fac(f0,nullptr,C0,f1,nullptr);