Skip to content
Snippets Groups Projects
Commit c1264529 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

working

parent 8c0b6580
No related branches found
No related tags found
3 merge requests!28release after RAL,!27After 2nd RAL submission,!20Resolve "Adapt to new generic factors API"
...@@ -21,6 +21,7 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, ...@@ -21,6 +21,7 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1,
FactorGnssFix2d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorGnssFix2d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2d", FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2d",
TOP_ABS,
_ftr_ptr, _ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
...@@ -43,11 +44,6 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, ...@@ -43,11 +44,6 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1,
~FactorGnssFix2d() override = default; ~FactorGnssFix2d() override = default;
std::string getTopology() const override
{
return std::string("ABS");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x, bool operator ()(const T* const _x,
const T* const _o, const T* const _o,
......
...@@ -20,6 +20,7 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, ...@@ -20,6 +20,7 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1,
FactorGnssFix3d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorGnssFix3d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3d", FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3d",
TOP_ABS,
_ftr_ptr, _ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
...@@ -42,11 +43,6 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, ...@@ -42,11 +43,6 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1,
~FactorGnssFix3d() override = default; ~FactorGnssFix3d() override = default;
std::string getTopology() const override
{
return std::string("ABS");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x, bool operator ()(const T* const _x,
const T* const _o, const T* const _o,
......
...@@ -30,6 +30,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, ...@@ -30,6 +30,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange", FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
TOP_ABS,
_ftr_ptr, _ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
...@@ -64,11 +65,6 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, ...@@ -64,11 +65,6 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
~FactorGnssPseudoRange() override = default; ~FactorGnssPseudoRange() override = default;
std::string getTopology() const override
{
return std::string("ABS");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x, bool operator ()(const T* const _x,
const T* const _o, const T* const _o,
......
...@@ -21,6 +21,7 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, ...@@ -21,6 +21,7 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3,
FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d", FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d",
TOP_GEOM,
_ftr_ptr, _ftr_ptr,
_frame_other_ptr, _frame_other_ptr,
nullptr, nullptr,
...@@ -44,11 +45,6 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, ...@@ -44,11 +45,6 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3,
virtual ~FactorGnssSingleDiff2d() = default; virtual ~FactorGnssSingleDiff2d() = default;
virtual std::string getTopology() const override
{
return std::string("MOTION");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x1, bool operator ()(const T* const _x1,
const T* const _o1, const T* const _o1,
......
...@@ -31,6 +31,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1 ...@@ -31,6 +31,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp", FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp",
TOP_GEOM,
_ftr_k, _ftr_k,
nullptr, nullptr,
_ftr_r->getCapture(), _ftr_r->getCapture(),
...@@ -66,11 +67,6 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1 ...@@ -66,11 +67,6 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
~FactorGnssTdcp() override = default; ~FactorGnssTdcp() override = default;
std::string getTopology() const override
{
return std::string("GEOM");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x_r, bool operator ()(const T* const _x_r,
const T* const _o_r, const T* const _o_r,
......
...@@ -21,6 +21,7 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, ...@@ -21,6 +21,7 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
FactorGnssTdcp2d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorGnssTdcp2d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d", FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d",
TOP_GEOM,
_ftr_ptr, _ftr_ptr,
_frame_other_ptr, _frame_other_ptr,
nullptr, nullptr,
...@@ -44,11 +45,6 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, ...@@ -44,11 +45,6 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
~FactorGnssTdcp2d() override = default; ~FactorGnssTdcp2d() override = default;
std::string getTopology() const override
{
return std::string("GEOM");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x1, bool operator ()(const T* const _x1,
const T* const _o1, const T* const _o1,
......
...@@ -21,22 +21,23 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, ...@@ -21,22 +21,23 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
FactorGnssTdcp3d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorGnssTdcp3d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d", FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d",
_ftr_ptr, TOP_GEOM,
_frame_other_ptr, _ftr_ptr,
nullptr, _frame_other_ptr,
nullptr, nullptr,
nullptr, nullptr,
_processor_ptr, nullptr,
_apply_loss_function, _processor_ptr,
_status, _apply_loss_function,
_frame_other_ptr->getP(), _status,
_frame_other_ptr->getO(), _frame_other_ptr->getP(),
_ftr_ptr->getFrame()->getP(), _frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getP(),
_sensor_gnss_ptr->getP(), _ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapPitch(), _sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapYaw()), _sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr) sensor_gnss_ptr_(_sensor_gnss_ptr)
{ {
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU"); WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
...@@ -44,11 +45,6 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, ...@@ -44,11 +45,6 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
~FactorGnssTdcp3d() override = default; ~FactorGnssTdcp3d() override = default;
std::string getTopology() const override
{
return std::string("GEOM");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x1, bool operator ()(const T* const _x1,
const T* const _o1, const T* const _o1,
......
...@@ -25,6 +25,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, ...@@ -25,6 +25,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
bool _apply_loss_function = false, bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpBatch", FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpBatch",
TOP_GEOM,
_ftr_ptr, _ftr_ptr,
_capture_other_ptr->getFrame(), _capture_other_ptr->getFrame(),
_capture_other_ptr, _capture_other_ptr,
...@@ -50,11 +51,6 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, ...@@ -50,11 +51,6 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
~FactorGnssTdcpBatch() override = default; ~FactorGnssTdcpBatch() override = default;
std::string getTopology() const override
{
return std::string("GEOM");
}
template<typename T> template<typename T>
bool operator ()(const T* const _x1, bool operator ()(const T* const _x1,
const T* const _o1, const T* const _o1,
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment