From cf0b0720506f809004f2d10a7ffab77af306f2f3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 6 Feb 2020 15:37:20 +0100
Subject: [PATCH] Resolve "Loss function API"
---
include/laser/factor/factor_container.h | 13 +---------
include/laser/factor/factor_point_2D.h | 25 +++++++++++++++----
.../laser/factor/factor_point_to_line_2D.h | 24 +++++++++++++++---
src/processor/processor_closeloop_icp.cpp | 11 ++++----
src/processor/processor_odom_icp.cpp | 7 ++++--
.../processor_tracker_feature_polyline_2D.cpp | 6 +++--
6 files changed, 57 insertions(+), 29 deletions(-)
diff --git a/include/laser/factor/factor_container.h b/include/laser/factor/factor_container.h
index 13cd114f7..03c9ea673 100644
--- a/include/laser/factor/factor_container.h
+++ b/include/laser/factor/factor_container.h
@@ -22,7 +22,7 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
const LandmarkContainerPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
const unsigned int _corner,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorContainer,3,2,1,2,1>("FactorContainer",
nullptr,
nullptr,
@@ -130,17 +130,6 @@ class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
return true;
}
-
- public:
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
-
- return std::make_shared<FactorContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
- }
-
};
} // namespace wolf
diff --git a/include/laser/factor/factor_point_2D.h b/include/laser/factor/factor_point_2D.h
index 263b561ae..d195a1367 100644
--- a/include/laser/factor/factor_point_2D.h
+++ b/include/laser/factor/factor_point_2D.h
@@ -26,12 +26,27 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
public:
FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ const LandmarkPolyline2DPtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ unsigned int _ftr_point_id,
+ int _lmk_point_id,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("FactorPoint2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ nullptr, nullptr, nullptr, _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO(),
+ _lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ feature_point_id_(_ftr_point_id),
+ landmark_point_id_(_lmk_point_id),
+ point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)),
+ measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
diff --git a/include/laser/factor/factor_point_to_line_2D.h b/include/laser/factor/factor_point_to_line_2D.h
index f4e48afae..fdc03c593 100644
--- a/include/laser/factor/factor_point_to_line_2D.h
+++ b/include/laser/factor/factor_point_to_line_2D.h
@@ -29,10 +29,28 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("FactorPointToLine2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ nullptr,
+ nullptr,
+ nullptr,
+ _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO(),
+ _lmk_ptr->getPointStateBlock(_lmk_point_id),
+ _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+ landmark_point_id_(_lmk_point_id),
+ landmark_point_aux_id_(_lmk_point_aux_id),
+ feature_point_id_(_ftr_point_id),
+ point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+ measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)),
+ measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "FactorPointToLine2D" << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
index fe491488d..c5591abea 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -228,11 +228,12 @@ CapturesAligned ProcessorCloseLoopICP::bestCandidate(std::map<double, CapturesAl
return _capture_candidates.begin()->second;
}
FactorBasePtr ProcessorCloseLoopICP::emplaceFeatureAndFactor(CapturesAligned &_captures_aligned)
- {
- _captures_aligned.align_result.res_covar = Eigen::Vector3s(0.01,0.01,0.01).asDiagonal();
- auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own, _captures_aligned.align_result);
- return FactorBase::emplace<FactorOdom2D>(ftr, ftr, _captures_aligned.capture_other->getFrame(), shared_from_this());
- }
+{
+ _captures_aligned.align_result.res_covar = Eigen::Vector3s(0.01,0.01,0.01).asDiagonal();
+ auto ftr = FeatureBase::emplace<FeatureICPAlign>(_captures_aligned.capture_own, _captures_aligned.align_result);
+ return FactorBase::emplace<FactorOdom2D>(ftr, ftr, _captures_aligned.capture_other->getFrame(), shared_from_this(), params_->apply_loss_function);
+}
+
}
// Register in the SensorFactory
#include "core/processor/processor_factory.h"
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 653f3d750..f4ea40664 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -311,8 +311,11 @@ FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)
{
- return FactorBase::emplace<FactorOdom2D>(_feature, _feature, origin_ptr_->getFrame(),
- shared_from_this());
+ return FactorBase::emplace<FactorOdom2D>(_feature,
+ _feature,
+ origin_ptr_->getFrame(),
+ shared_from_this(),
+ params_->apply_loss_function);
}
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 4c1630bb9..aa2b5eeb1 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -575,7 +575,8 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline
shared_from_this(),
_ftr_point_id,
_lmk_point_id,
- _lmk_prev_point_id);
+ _lmk_prev_point_id,
+ params_->apply_loss_function);
}
void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
@@ -590,7 +591,8 @@ void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr
_polyline_landmark,
shared_from_this(),
_ftr_point_id,
- _lmk_point_id);
+ _lmk_point_id,
+ params_->apply_loss_function);
}
LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePtr _feature_ptr)
--
GitLab