diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8f7b6c61371c51c3f23e4690459f427918564fe1..7200c40b3e80d1e832a91fc90a8dffcbd80ac247 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -166,6 +166,7 @@ SET(HDRS_HARDWARE
)
SET(HDRS_LANDMARK
include/${PROJECT_NAME}/landmark/landmark_base.h
+ include/${PROJECT_NAME}/landmark/landmark_external.h
include/${PROJECT_NAME}/landmark/landmark_match.h
)
SET(HDRS_MATH
@@ -287,6 +288,7 @@ SET(SRCS_HARDWARE
)
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
+ src/landmark/landmark_external.cpp
)
SET(SRCS_MAP
src/map/map_base.cpp
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 374f5f74bce180ddc27bb8a012ad4f26a5f669b9..ec561544efd9844aab37d30d889241cc5f929eb3 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -49,8 +49,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
- //std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
- //std::vector<StateBlockConstPtr> state_block_const_vec_; ///< vector of state blocks, additional to those in the state_block_map from HasStateBlocks.
static unsigned int landmark_id_count_;
diff --git a/include/core/landmark/landmark_external.h b/include/core/landmark/landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ff6df805cbb5fdaef8d73ee250709b48d210508
--- /dev/null
+++ b/include/core/landmark/landmark_external.h
@@ -0,0 +1,67 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023,2024 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/landmark/landmark_base.h"
+
+namespace wolf
+{
+WOLF_PTR_TYPEDEFS(LandmarkExternal);
+
+// class LandmarkExternal
+class LandmarkExternal : public LandmarkBase
+{
+ protected:
+ int external_id_; ///< Id provided by an external tracker (-1 untracked)
+ int external_type_; ///< Type provided by an external classifier (-1 unclassified)
+
+ public:
+ /** \brief Constructor with the position state pointer (optional orientation state pointer)
+ *
+ * Constructor with type, and state pointer
+ * \param _external_id ID provided by an external tracker (-1 for untracked)
+ * \param _external_type TYPE provided by an external classifier (-1 for unknown)
+ * \param _p_ptr StateBlock pointer to the position
+ * \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
+ *
+ **/
+ LandmarkExternal(const int& _external_id,
+ const int& _external_type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr = nullptr);
+ ~LandmarkExternal() = default;
+ YAML::Node saveToYaml() const override;
+
+ /** \brief Creator for Factory<LandmarkExternal, YAML::Node>
+ * Caution: This creator does not set the landmark's anchor frame and sensor.
+ * These need to be set afterwards.
+ */
+ static LandmarkExternalPtr create(const YAML::Node& _node);
+
+ void setExternalId(const int& _external_id);
+ int getExternalId() const;
+ void setExternalType(const int& _external_id);
+ int getExternalType() const;
+};
+
+} // namespace wolf
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index 8fd3f54172bd993960d4da2a2db28110394a6283..917b53fabded917629bea44be902e6aa00e37bb2 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -25,6 +25,8 @@
#include "core/common/wolf.h"
#include "core/processor/processor_tracker.h"
#include "core/processor/track_matrix.h"
+#include "core/landmark/landmark_external.h"
+#include "core/feature/feature_landmark_external.h"
namespace wolf
{
@@ -41,8 +43,9 @@ struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than
///< min_features_for_keyframe)
- bool check_dist_when_ids_match; ///< check the match_dist_th also in detections with matching external ID
- bool close_loops_when_ids_match; ///< emplace loop closure factors if external ID matches
+ bool check_dist_when_ids_match; ///< check the match_dist_th also in detections with matching external ID
+ bool close_loop_when_id_match; ///< emplace loop closure factors if external ID matches
+ bool close_loop_when_type_match; ///< emplace loop closure factors if external TYPE matches (and match_dist_th holds)
ParamsProcessorLandmarkExternal() = default;
ParamsProcessorLandmarkExternal(std::string _unique_name, const wolf::ParamsServer& _server)
@@ -55,8 +58,9 @@ struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/time_span");
new_features_for_keyframe =
_server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/new_features_for_keyframe");
- check_dist_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/check_dist_when_ids_match");
- close_loops_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/close_loops_when_ids_match");
+ check_dist_when_ids_match = _server.getParam<bool>(prefix + _unique_name + "/check_dist_when_ids_match");
+ close_loop_when_id_match = _server.getParam<bool>(prefix + _unique_name + "/close_loop_when_id_match");
+ close_loop_when_type_match = _server.getParam<bool>(prefix + _unique_name + "/close_loop_when_type_match");
}
};
@@ -78,7 +82,6 @@ class ProcessorLandmarkExternal : public ProcessorTracker
ParamsProcessorLandmarkExternalPtr params_tfle_;
TrackMatrix track_matrix_;
std::set<SizeStd> lmks_ids_origin_;
- std::map<int, SizeStd> external_id_to_landmark_id_;
/** Pre-process incoming Capture
*
@@ -126,13 +129,13 @@ class ProcessorLandmarkExternal : public ProcessorTracker
/** \brief Emplaces a landmark or modifies (if needed) a landmark
* \param _feature_ptr pointer to the Feature
*/
- LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+ LandmarkExternalPtr emplaceLandmark(FeatureExternalPtr _feature_ptr);
/** \brief Modifies (if needed) a landmark
* \param _landmark pointer to the landmark
* \param _feature pointer to the Feature.
*/
- void modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature);
+ void modifyLandmark(LandmarkExternalPtr _landmark, FeatureExternalPtr _feature);
/** Post-process
*
@@ -150,8 +153,6 @@ class ProcessorLandmarkExternal : public ProcessorTracker
const VectorComposite& _pose1,
const VectorComposite& _pose2,
const VectorComposite& _pose_sen) const;
-
- SizeStd getAvailableLandmarkId() const;
};
inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5569280d4fa7d6dde04b26606fda51d67fece67a..4ce1c2c2601b55f7ea8e35d2be8f2cad06ef8082 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -38,7 +38,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
NodeBase("LANDMARK", _type),
HasStateBlocks(""),
map_ptr_(),
- //state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
if (_p_ptr)
@@ -52,84 +51,6 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
}
-LandmarkBase::~LandmarkBase()
-{
- removeStateBlocks(getProblem());
-}
-
-void LandmarkBase::remove(bool viral_remove_empty_parent)
-{
- /* Order of removing:
- * Factors are removed first, and afterwards the rest of nodes containing state blocks.
- * In case multi-threading, solver can be called while removing.
- * This order avoids SolverManager to ignore notifications (efficiency)
- */
- if (!is_removing_)
- {
- is_removing_ = true;
- LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
-
- // remove constrained by
- while (!constrained_by_list_.empty())
- {
- constrained_by_list_.front()->remove(viral_remove_empty_parent);
- }
-
- // Remove State Blocks
- removeStateBlocks(getProblem());
-
- // remove from upstream
- auto M = map_ptr_.lock();
- if (M)
- M->removeLandmark(this_L);
- }
-}
-
-// std::vector<StateBlockConstPtr> LandmarkBase::getUsedStateBlockVec() const
-// {
-// std::vector<StateBlockConstPtr> used_state_block_vec(0);
-
-// // normal state blocks in {P,O,V,W}
-// for (auto key : getStructure())
-// {
-// const auto sbp = getStateBlock(key);
-// if (sbp)
-// used_state_block_vec.push_back(sbp);
-// }
-
-// // // other state blocks in a vector
-// // for (auto sbp : state_block_vec_)
-// // if (sbp)
-// // used_state_block_vec.push_back(sbp);
-
-// return used_state_block_vec;
-// }
-
-// std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec()
-// {
-// std::vector<StateBlockPtr> used_state_block_vec(0);
-
-// // normal state blocks in {P,O,V,W}
-// for (auto key : getStructure())
-// {
-// auto sbp = getStateBlock(key);
-// if (sbp)
-// used_state_block_vec.push_back(sbp);
-// }
-
-// // // other state blocks in a vector
-// // for (auto sbp : state_block_vec_)
-// // if (sbp)
-// // used_state_block_vec.push_back(sbp);
-
-// return used_state_block_vec;
-// }
-
-bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
-{
- return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
-}
-
YAML::Node LandmarkBase::saveToYaml() const
{
YAML::Node node;
@@ -149,69 +70,6 @@ YAML::Node LandmarkBase::saveToYaml() const
return node;
}
-void LandmarkBase::link(MapBasePtr _map_ptr)
-{
- assert(!is_removing_ && "linking a removed landmark");
- assert(this->getMap() == nullptr && "linking an already linked landmark");
-
- if(_map_ptr)
- {
- this->setMap(_map_ptr);
- _map_ptr->addLandmark(shared_from_this());
- this->setProblem(_map_ptr->getProblem());
- }
- else
- {
- WOLF_WARN("Linking with nullptr");
- }
-}
-
-void LandmarkBase::setProblem(ProblemPtr _problem)
-{
- if (_problem == nullptr || _problem == this->getProblem())
- return;
-
- NodeBase::setProblem(_problem);
- registerNewStateBlocks(_problem);
-}
-
-FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.push_back(_fac_ptr);
- return _fac_ptr;
-}
-
-void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
- constrained_by_list_.remove(_fac_ptr);
-}
-
-bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
- return std::find(constrained_by_list_.begin(),
- constrained_by_list_.end(),
- _factor) != constrained_by_list_.end();
-}
-
-void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
-{
- _stream << _tabs << "Lmk" << id() << " " << getType();
- if (_constr_by)
- {
- _stream << "\t<-- ";
- for (auto cby : getConstrainedByList())
- if (cby)
- _stream << "Fac" << cby->id() << " \t";
- }
- _stream << std::endl;
-
- printState(_metric, _state_blocks, _stream, _tabs);
-}
-
-void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
-{
- printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
-}
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
@@ -238,97 +96,10 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk;
}
-CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
-{
- CheckLog log;
- std::stringstream inconsistency_explanation;
-
- if (_verbose)
- {
- _stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
- _stream << _tabs << " -> Prb @ " << getProblem().get() << std::endl;
- _stream << _tabs << " -> Map @ " << getMap().get() << std::endl;
- for (auto pair : getStateBlockMap())
- {
- auto sb = pair.second;
- _stream << _tabs << " " << pair.first << " sb @ " << sb.get();
- if (sb)
- {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
-
- auto map_ptr = getMap();
- // check problem and map pointers
- inconsistency_explanation << "Landmarks' problem ptr "
- << getProblem().get() << " different from Map's problem ptr "
- << map_ptr->getProblem().get() << "\n";
-
- log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
-
- // check constrained-by factors
- for (auto cby : getConstrainedByList())
- {
- if (_verbose)
- {
- _stream << _tabs << " " << "<- Fac" << cby->id() << " ->";
-
- for (auto Low : cby->getLandmarkOtherList())
- {
- if (!Low.expired())
- {
- auto Lo = Low.lock();
- _stream << " Lmk" << Lo->id();
- }
- }
- _stream << std::endl;
- }
-
- inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
- << " not found among constrained-by factors\n";
- log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
-
- for (auto sb : cby->getStateBlockPtrVector()) {
- if (_verbose) {
- _stream << _tabs << " " << "sb @ " << sb.get();
- if (sb) {
- auto lp = sb->getLocalParametrization();
- if (lp)
- _stream << " (lp @ " << lp.get() << ")";
- }
- _stream << std::endl;
- }
- }
- }
-
- // check map
- inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
- << " ---> Map" << map_ptr
- << " -X-> Lmk" << id();
- auto map_lmk_list = map_ptr->getLandmarkList();
- auto map_has_lmk = std::find(map_lmk_list.begin(),
- map_lmk_list.end(),
- _lmk_ptr);
- log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
-
- return log;
-}
-bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
-{
- auto lmk_ptr = std::static_pointer_cast<const LandmarkBase>(_node_ptr);
- auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
- _log.compose(local_log);
-
- return _log.is_consistent_;
-}
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
+const bool WOLF_UNUSED registered_lmk_external = FactoryLandmark::registerCreator("LandmarkExternal", LandmarkExternal::create);
}
} // namespace wolf
diff --git a/src/landmark/landmark_external.cpp b/src/landmark/landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3dd8334a5d1a1be3f6d34d8b0e86c4c31c7089b2
--- /dev/null
+++ b/src/landmark/landmark_external.cpp
@@ -0,0 +1,77 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023,2024 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/landmark/landmark_external.h"
+
+namespace wolf
+{
+
+LandmarkExternal::LandmarkExternal(const int& _external_id,
+ const int& _external_type,
+ StateBlockPtr _p_ptr,
+ StateBlockPtr _o_ptr = nullptr)
+ : LandmarkBase("LandmarkExternal", _p_ptr, _o_ptr), external_id_(_external_id), external_type_(_external_type)
+{
+ assert(external_id_ >= 0 or external_id_ == -1);
+ assert(external_type_ >= 0 or external_type_ == -1);
+}
+
+YAML::Node LandmarkExternal::saveToYaml() const
+{
+ YAML::Node node = LandmarkBase::saveToYaml();
+ node["external_id"] = external_id_;
+ node["external_type"] = external_type_;
+ return node;
+}
+
+LandmarkBasePtr LandmarkExternal::create(const YAML::Node& _node)
+{
+ unsigned int external_id = _node["external_id"].as<int>();
+ unsigned int external_type = _node["external_type"].as<int>();
+ Eigen::VectorXd pos = _node["position"].as<Eigen::VectorXd>();
+ bool pos_fixed = _node["position fixed"].as<bool>();
+
+ StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
+ StateBlockPtr ori_sb = nullptr;
+
+ if (_node["orientation"])
+ {
+ Eigen::VectorXd ori = _node["orientation"].as<Eigen::VectorXd>();
+ bool ori_fixed = _node["orientation fixed"].as<bool>();
+
+ if (ori.size() == 4)
+ ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
+ else
+ ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
+ }
+
+ return std::make_shared<LandmarkExternal>(external_id, external_type, pos_sb, ori_sb);
+}
+
+// Register landmark creator
+namespace
+{
+const bool WOLF_UNUSED registered_lmk_base =
+ FactoryLandmark::registerCreator("LandmarkExternal", LandmarkExternal::create);
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index d7bfd41ecef277a1b64d04b1e7ec2bea65c5434a..fe9f4f6bf9163c719c9e0f50a08b39517b5e4dfe 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -22,12 +22,10 @@
#include "core/processor/processor_landmark_external.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/feature/feature_landmark_external.h"
#include "core/factor/factor_relative_pose_2d_with_extrinsics.h"
#include "core/factor/factor_relative_position_2d_with_extrinsics.h"
#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
#include "core/factor/factor_relative_position_3d_with_extrinsics.h"
-#include "core/landmark/landmark_base.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
@@ -128,16 +126,16 @@ unsigned int ProcessorLandmarkExternal::processKnown()
params_tfle_->match_dist_th)
{
WOLF_DEBUG("Feature last: ",
- feat_lmk_last,
+ feat_lmk_last->id(),
" matched with feature ",
- feat_lmk_candidate,
+ feat_lmk_candidate->id(),
" with landmark ID: ",
- feat_lmk_candidate->landmarkId());
+ feat_lmk_last->landmarkId());
matched = true;
- // set LANDMARK_ID if candidate has LANDMARK_ID
- if (feat_lmk_candidate->landmarkId() != 0)
- feat_lmk_last->setLandmarkId(feat_lmk_candidate->landmarkId());
+ // set LANDMARK_ID if defined
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_candidate->setLandmarkId(feat_lmk_last->landmarkId());
// grow track
track_matrix_.add(feat_lmk_last, feat_lmk_candidate);
@@ -169,20 +167,20 @@ unsigned int ProcessorLandmarkExternal::processKnown()
params_tfle_->match_dist_th)
{
WOLF_DEBUG("Feature last: ",
- feat_lmk_last,
+ feat_lmk_last->id(),
" matched with feature ",
- feat_lmk_candidate,
+ feat_lmk_candidate->id(),
" with landmark ID: ",
- feat_lmk_candidate->landmarkId());
+ feat_lmk_last->landmarkId());
matched = true;
- // set LANDMARK_ID if candidate has LANDMARK_ID
- if (feat_lmk_candidate->landmarkId() != 0)
- feat_lmk_last->setLandmarkId(feat_lmk_candidate->landmarkId());
+ // set LANDMARK_ID if defined
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_candidate->setLandmarkId(feat_lmk_last->landmarkId());
- // set EXTERNAL_ID if candidate has EXTERNAL_ID
- if (feat_lmk_candidate->getExternalId() != -1)
- feat_lmk_last->setExternalId(feat_lmk_candidate->getExternalId());
+ // set EXTERNAL_ID if defined
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_candidate->setExternalId(feat_lmk_last->getExternalId());
// grow track
track_matrix_.add(feat_lmk_last, feat_lmk_candidate);
@@ -212,24 +210,24 @@ unsigned int ProcessorLandmarkExternal::processKnown()
params_tfle_->match_dist_th)
{
WOLF_DEBUG("Feature last: ",
- feat_lmk_last,
+ feat_lmk_last->id(),
" matched with feature ",
- feat_lmk_candidate,
+ feat_lmk_candidate->id(),
" with landmark ID: ",
- feat_lmk_candidate->landmarkId());
+ feat_lmk_last->landmarkId());
matched = true;
- // set LANDMARK_ID if candidate has LANDMARK_ID
- if (feat_lmk_candidate->landmarkId() != 0)
- feat_lmk_last->setLandmarkId(feat_lmk_candidate->landmarkId());
+ // set LANDMARK_ID if defined
+ if (feat_lmk_last->landmarkId() != 0)
+ feat_lmk_candidate->setLandmarkId(feat_lmk_last->landmarkId());
- // set TYPE if candidate has TYPE
- if (feat_lmk_candidate->getExternalType() != -1)
- feat_lmk_last->setExternalType(feat_lmk_candidate->getExternalType());
+ // set TYPE if defined
+ if (feat_lmk_last->getExternalType() != -1)
+ feat_lmk_candidate->setExternalType(feat_lmk_last->getExternalType());
- // set EXTERNAL_ID if candidate has EXTERNAL_ID
- if (feat_lmk_candidate->getExternalId() != -1)
- feat_lmk_last->setExternalId(feat_lmk_candidate->getExternalId());
+ // set EXTERNAL_ID if defined
+ if (feat_lmk_last->getExternalId() != -1)
+ feat_lmk_candidate->setExternalId(feat_lmk_last->getExternalId());
// grow track
track_matrix_.add(feat_lmk_last, feat_lmk_candidate);
@@ -253,19 +251,7 @@ unsigned int ProcessorLandmarkExternal::processKnown()
WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
while (not new_features_incoming_.empty())
{
- auto ftr = std::static_pointer_cast<FeatureLandmarkExternal>(new_features_incoming_.front());
-
- // loop closure
- if (params_tfle_->close_loops_when_ids_match and ftr->getExternalId() != -1 and
- external_id_to_landmark_id_.count(ftr->getExternalId()) != 0)
- {
- WOLF_DEBUG("Feature ",
- ftr->id(),
- " set to close loop with landmark ",
- external_id_to_landmark_id_.at(ftr->getExternalId()));
-
- ftr->setLandmarkId(external_id_to_landmark_id_.at(ftr->getExternalId()));
- }
+ auto ftr = new_features_incoming_.front();
// new track
track_matrix_.newTrack(ftr);
@@ -408,37 +394,68 @@ void ProcessorLandmarkExternal::establishFactors()
auto feature = std::static_pointer_cast<FeatureLandmarkExternal>(track_matrix_.feature(track_id, last_ptr_));
- // Generate a new LANDMARK_ID if not set
- WOLF_DEBUG_COND(feature->landmarkId() == 0, "Landmark id not set, getting available id...");
- if (feature->landmarkId() == 0) // id 0 will not be used
+ // Landmark match
+ LandmarkExternalPtr lmk;
+ if (feature->landmarkId() == 0) // landmark id 0 never used
{
- feature->setLandmarkId(getAvailableLandmarkId());
- }
- WOLF_DEBUG("Landmark id: ", feature->landmarkId());
-
- // store correspondence EXTERNAL_ID - LANDMARK_ID
- if (feature->getExternalId() != -1)
- external_id_to_landmark_id_[feature->getExternalId()] = feature->landmarkId();
+ WOLF_DEBUG("Landmark id not set");
- // get landmark
- LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
+ // LOOP CLOSURE
+ // loop closure with TYPE
+ if (params_tfle_->close_loop_when_type_match and feature->getExternalType() != -1)
+ {
+ // TODO
+ WOLF_DEBUG_COND(lmk, "Found landmark ", lmk->id());
+ }
+ // loop closure with ID
+ if (params_tfle_->close_loop_when_id_match and feature->getExternalId() != -1)
+ {
+ // TODO
+ WOLF_DEBUG_COND(lmk, "Found landmark ", lmk->id());
+ }
- // emplace landmark if doesn't exist
- WOLF_DEBUG_COND(not lmk, "Landmark does not exist, emplacing...");
- if (not lmk) lmk = emplaceLandmark(feature);
+ // Emplace new landmark (loop closure not found or not enabled)
+ if (not lmk)
+ {
+ lmk = emplaceLandmark(feature);
+ WOLF_DEBUG("New landmark ", lmk->id());
+ }
+
+ // set LANDMARK_ID in all features of the track
+ for (auto feat_pair : track_matrix_.track(track_id))
+ {
+ feat_pair.second->setLandmarkId(lmk->id());
+ WOLF_DEBUG("Setting landmark id in feature ",
+ feat_pair.second->id(),
+ " landmark_id: ",
+ feat_pair.second->landmarkId());
+ // TODO: check all features do not disagree
+ }
+ }
+ // get landmark
+ else
+ {
+ lmk = std::dynamic_pointer_cast<LandmarkExternal>(
+ getProblem()->getMap()->getLandmark(feature->landmarkId()));
+ if (not lmk)
+ throw std::runtime_error(
+ "ProcessorLandmarkExternal::establishFactors: Feature has LANDMARK_ID but there is no landmark of "
+ "type LandmarkExternal with this id.");
+ }
// modify landmark (add missing state blocks)
modifyLandmark(lmk, feature);
- // emplace factor
- auto fac = emplaceFactor(feature, lmk);
-
- // TODO: emplace factors for previous tracked features (in keyframes)
+ // Emplace factors for all tracked features in keyframes
+ for (auto feat_pair : track_matrix_.trackAtKeyframes(track_id))
+ if (feat_pair.second->getFactorList().empty())
+ {
+ auto fac = emplaceFactor(feature, lmk);
+ if (fac) fac_list.push_back(fac);
+ }
// store tracked landmarks from origin (used by voteForKeyframe())
lmks_ids_origin_.insert(lmk->id());
-
- if (fac) fac_list.push_back(fac);
}
WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
@@ -480,7 +497,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
return nullptr; // not going to happen
}
-LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
+LandmarkExternalPtr ProcessorLandmarkExternal::emplaceLandmark(FeatureExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -490,11 +507,12 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
assert(getProblem());
assert(getProblem()->getMap());
- if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
+ if (_feature->landmarkId() != 0)
throw std::runtime_error(
- "ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+ "ProcessorLandmarkExternal::emplaceLandmark: attempting to emplace a landmark for a feature that has been "
+ "already matched with an existing one");
- LandmarkBasePtr lmk;
+ LandmarkExternalPtr lmk;
// 2D - Relative Position
if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
@@ -506,9 +524,11 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkBase>(
- getProblem()->getMap(), "LandmarkRelativePosition2d", std::make_shared<StatePoint2d>(lmk_p), nullptr);
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
}
// 2D - Relative Pose
else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
@@ -522,11 +542,11 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePose2d",
- std::make_shared<StatePoint2d>(lmk_p),
- std::make_shared<StateAngle>(lmk_o));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
}
// 3D - Relative Position
else if (getProblem()->getDim() == 3 and
@@ -539,9 +559,11 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
- lmk = LandmarkBase::emplace<LandmarkBase>(
- getProblem()->getMap(), "LandmarkRelativePosition3d", std::make_shared<StatePoint3d>(lmk_p), nullptr);
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
}
// 3D - Relative Pose
else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
@@ -554,11 +576,11 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
- lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
- "LandmarkRelativePose3d",
- std::make_shared<StatePoint3d>(lmk_p),
- std::make_shared<StateQuaternion>(lmk_o));
- lmk->setId(_feature->landmarkId());
+ lmk = LandmarkBase::emplace<LandmarkExternal>(getProblem()->getMap(),
+ _feature->getExternalId(),
+ _feature->getExternalType(),
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
}
else
throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
@@ -566,7 +588,7 @@ LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _featu
return lmk;
}
-void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, FeatureBasePtr _feature)
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkExternalPtr _landmark, FeatureLandmarkExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
@@ -622,17 +644,6 @@ void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark, Featur
throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark unknown case");
}
-SizeStd ProcessorLandmarkExternal::getAvailableLandmarkId() const
-{
- auto lmk_list = getProblem()->getMap()->getLandmarkList();
-
- SizeStd max_id = 0;
- for (auto lmk : lmk_list)
- if (lmk->id() > max_id) max_id = lmk->id();
-
- return max_id + 1;
-}
-
void ProcessorLandmarkExternal::advanceDerived()
{
// Reset here the list of correspondences.
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index d6e50e7609f164392dc346b5a7bb357f6cf85107..1e43f1618b6740f97113bd125969ddd23b0c6148 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -140,6 +140,8 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
params->new_features_for_keyframe = 1000;
params->filter_track_length_th = _track_length_th;
params->time_span = _time_span;
+ params->close_loop_when_id_match = _init_landmarks;
+ params->close_loop_when_type_match = false;
processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
if (dim == 2)
@@ -173,17 +175,17 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
if (dim == 2)
lmk = LandmarkBase::emplace<LandmarkBase>(
_init_landmarks ? problem->getMap() : nullptr,
- "LandmarkExternal",
+ (orientation ? "LandmarkPose2d" : "LandmarkPosition2d"),
std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
(orientation ? std::make_shared<StateAngle>(Vector1d::Random() * M_PI) : nullptr));
else
lmk = LandmarkBase::emplace<LandmarkBase>(
_init_landmarks ? problem->getMap() : nullptr,
- "LandmarkExternal",
+ (orientation ? "LandmarkPose3d" : "LandmarkPosition3d"),
std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
(orientation ? std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) : nullptr));
- lmk->setId(i + 1);
+ lmk->setExternalId(i + 1); // TODO LandmarkExternal
landmarks.push_back(lmk);
state_landmarks.push_back(lmk->getState());
}
@@ -256,13 +258,15 @@ CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmar
double quality = (double)i / (double)(landmarks.size() - 1); // increasing from 0 to 1
// add detection
+ auto step = (int)round(t.get() / dt);
// with ID
- if (mode == 0 or (mode == 3 and i % 3 == 0)) cap->addDetection(landmarks.at(i)->id(), -1, meas, cov, quality);
+ if (mode == 0 or (mode == 3 and i % 3 == 0) or (mode == 4 and step % 3 == 0))
+ cap->addDetection(landmarks.at(i)->id(), -1, meas, cov, quality);
// with TYPE
- else if (mode == 1 or (mode == 3 and i % 3 == 1))
+ else if (mode == 1 or (mode == 3 and i % 3 == 1) or (mode == 4 and step % 3 == 1))
cap->addDetection(-1, landmarks.at(i)->id(), meas, cov, quality);
// with nothing
- else if (mode == 2 or (mode == 3 and i % 3 == 2))
+ else if (mode == 2 or (mode == 3 and i % 3 == 2) or (mode == 4 and step % 3 == 2))
cap->addDetection(-1, -1, meas, cov, quality);
}
@@ -280,7 +284,7 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
{
initProblem(_dim, _orientation, _mode, _quality_th, _dist_th, _track_length, _time_span, _init_landmarks);
- auto n_keyframes = problem->getTrajectory()->size();
+ FactorBasePtrList fac_list;
ASSERT_TRUE(problem->check());
@@ -288,50 +292,64 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
{
WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
- // detection of landmarks
- auto cap = computeCaptureLandmarks();
- ASSERT_TRUE(problem->check());
+ // store previous number of kf, lmks, and factors
+ auto n_prev_kf = problem->getTrajectory()->size();
+ auto n_prev_lmk = problem->getMap()->getLandmarkList().size();
+ fac_list.clear();
+ problem->getTrajectory()->getFactorList(fac_list);
+ auto n_prev_fac = fac_list.size();
- // process detections
- cap->process();
- ASSERT_TRUE(problem->check());
- problem->print(4, 1, 1, 1);
+ // Things to check
+ bool any_active_track = _quality_th <= 1 and i >= _track_length - 1;
+ bool should_emplace_KF =
+ (t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt > _time_span and
+ any_active_track);
+ bool should_emplace_factors = (i == 1 or should_emplace_KF) and any_active_track;
+
+ WOLF_INFO("should emplace factors: ",
+ should_emplace_factors,
+ " _quality_th = ",
+ _quality_th,
+ " track_length-1 = ",
+ _track_length - 1);
- // CHECKS
- FactorBasePtrList fac_list;
- problem->getTrajectory()->getFactorList(fac_list);
- // should emplace KF in last?
- bool any_active_track = _quality_th <= 1 and i >= _track_length - 1;
- bool should_emplace_KF = (t - dt > _time_span and any_active_track);
- bool should_emplace_factors = i == 1 or should_emplace_KF;
WOLF_INFO("should emplace KF: ",
should_emplace_KF,
- "should emplace factors: ",
- should_emplace_factors,
" i = ",
i,
- " t-dt = ",
- t - dt,
+ " t-last_frame_stamp-dt = ",
+ t - problem->getTrajectory()->getLastFrame()->getTimeStamp().get() - dt,
" time_span = ",
_time_span,
" track_length-1 = ",
_track_length - 1);
+ // detection of landmarks
+ auto cap = computeCaptureLandmarks();
+ ASSERT_TRUE(problem->check());
+
+ // process detections
+ cap->process();
+ ASSERT_TRUE(problem->check());
+ problem->print(4, 1, 1, 1);
+
+ // CHECKS
+ fac_list.clear();
+ problem->getTrajectory()->getFactorList(fac_list);
// Check voted&emplaced a keyframe
if (should_emplace_KF)
{
- ASSERT_EQ(problem->getTrajectory()->size(), n_keyframes + 1);
- n_keyframes++;
+ ASSERT_EQ(problem->getTrajectory()->size(), n_prev_kf + 1);
}
else
{
- ASSERT_EQ(problem->getTrajectory()->size(), n_keyframes);
+ ASSERT_EQ(problem->getTrajectory()->size(), n_prev_kf);
}
// Check emplaced factors and landmarks
if (should_emplace_factors)
{
- ASSERT_FALSE(fac_list.empty());
+ ASSERT_GT(fac_list.size(), n_prev_fac);
// factors' type
for (auto fac : fac_list)
@@ -355,19 +373,19 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
// check state correctly initialized
for (auto lmk_map : landmarks_map)
{
- ASSERT_TRUE(lmk_map->id() < landmarks.size());
+ ASSERT_TRUE(lmk_map->id() <= landmarks.size());
ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id() - 1)->id());
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id() - 1));
}
}
}
else
{
// no factors emplaced
- ASSERT_TRUE(fac_list.empty());
+ ASSERT_EQ(fac_list.size(), n_prev_fac);
// no landmarks created yet (if not added by the test)
- ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not _init_landmarks);
+ ASSERT_EQ(problem->getMap()->getLandmarkList().size(), n_prev_lmk);
}
// step with random movement
@@ -391,7 +409,7 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
// check values
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id() - 1));
}
}
}