diff --git a/include/laser/factor/factor_container.h b/include/laser/factor/factor_container.h index 13cd114f7199ee963377af130b0432160d1abebb..03c9ea6732fc337e44039acaedda88dd9b863560 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 263b561aeb334ca74558f54c1a3616bb89ee3223..d195a1367dcdc29b171e1e95094eb49dbe4a5da8 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 f4e48afae89709890ce8cab367c3b3e041d473e6..fdc03c5938da90a8f8d1b23e22206ab6490d63ae 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 fe491488daed46ed31b51febb65e0420bfcbd9e6..c5591abea4150c1e458b95c528c9f6356eac2d7d 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 653f3d75073289116ed486879966193c81d943e6..f4ea40664bf864955c60d73816771b80eea36a4c 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 4c1630bb91122056ddada95d9dfcac3ef7db297e..aa2b5eeb16b077b23a4bd6dbb6e9272f0b009f9f 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)