diff --git a/include/objectslam/landmark/landmark_object.h b/include/objectslam/landmark/landmark_object.h
index 9e46b03e01cda3023df39cdfa78e8b7b35fcde79..87c88771d335acc186e7ddae91636c2e93abad58 100644
--- a/include/objectslam/landmark/landmark_object.h
+++ b/include/objectslam/landmark/landmark_object.h
@@ -1,10 +1,30 @@
+//--------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 LANDMARK_OBJECT_H_
#define LANDMARK_OBJECT_H_
//Wolf includes
#include <core/landmark/landmark_base.h>
-#include <core/state_block/state_quaternion.h>
namespace wolf {
@@ -22,7 +42,7 @@ class LandmarkObject : public LandmarkBase
* \param _t_init : timestamp of the landmark's initialization
*
**/
- LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp& _t_init, const Matrix3d& _intrinsic);
+ LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp& _t_init);
~LandmarkObject() override;
@@ -31,21 +51,11 @@ class LandmarkObject : public LandmarkBase
**/
const std::string& getObjectType() const;
- /** \brief Returns the intrinsic matrix
- *
- **/
- const Matrix3d& getIntrinsic() const;
-
/** \brief Returns the initialisation timestamp
*
**/
const TimeStamp& getInitTimestamp() const;
- /** \brief Returns the position in the image coordinate
- *
- **/
- const Vector2d getImageCoord() const;
-
/** Factory method to create new landmarks from YAML nodes
*/
static LandmarkBasePtr create(const YAML::Node& _lmk_node);
@@ -56,7 +66,6 @@ class LandmarkObject : public LandmarkBase
private:
const std::string object_type_;
const TimeStamp t_init_;
- const Matrix3d intrinsic_;
};
diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index 7dadcb54d808cd2e9d286ecc9e64bf5c9a76702c..62016dd644475c51be74681ea3b4fc64b9d8b572 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -23,8 +23,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
double lmk_score_thr_;
double e_pos_thr_;
double e_rot_thr_;
- int fps_;
- Matrix3d intrinsic_;
+ // Matrix3d intrinsic_;
bool first_lmk_matching_;
double ratio_inliers_outliers_;
@@ -38,16 +37,8 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
lmk_score_thr_ = _server.getParam<double>(prefix + _unique_name + "/lmk_score_thr");
e_pos_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_pos_thr");
e_rot_thr_ = _server.getParam<double>(prefix + _unique_name + "/e_rot_thr");
- fps_ = _server.getParam<double>(prefix + _unique_name + "/fps");
- double fx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/fx");
- double fy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/fy");
- double cx = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cx");
- double cy = _server.getParam<double>(prefix + _unique_name + "/intrinsic/cy");
first_lmk_matching_ = _server.getParam<bool> (prefix + _unique_name + "/first_lmk_matching");
ratio_inliers_outliers_ = _server.getParam<double>(prefix + _unique_name + "/ratio_inliers_outliers");
- intrinsic_ << fx, 0, cx,
- 0, fy, cy,
- 0, 0, 1;
}
std::string print() const override
{
@@ -57,12 +48,7 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
+ "nb_vote_for_every_first_: " + std::to_string(nb_vote_for_every_first_) + "\n"
+ "lmk_score_thr_: " + std::to_string(lmk_score_thr_) + "\n"
+ "e_pos_thr_: " + std::to_string(e_pos_thr_) + "\n"
- + "e_rot_thr_: " + std::to_string(e_rot_thr_) + "\n"
- + "fps_: " + std::to_string(fps_) + "\n"
- + "fx: " + std::to_string(intrinsic_(0,0)) + "\n"
- + "fy: " + std::to_string(intrinsic_(1,1)) + "\n"
- + "cx: " + std::to_string(intrinsic_(0,2)) + "\n"
- + "cy: " + std::to_string(intrinsic_(1,2)) + "\n";
+ + "e_rot_thr_: " + std::to_string(e_rot_thr_) + "\n";
}
};
@@ -303,8 +289,6 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
int nb_vote_for_every_first_;
bool keyframe_voted_;
int nb_vote_;
- int fps_;
- Matrix3d intrinsic_;
bool first_lmk_matching_;
double ratio_inliers_outliers_;
diff --git a/src/landmark/landmark_object.cpp b/src/landmark/landmark_object.cpp
index cf913e422682508497d0aee0d27f49fbf6ba0b8b..a80e3e7350ed25bc01347debcf986b4a893422d5 100644
--- a/src/landmark/landmark_object.cpp
+++ b/src/landmark/landmark_object.cpp
@@ -1,15 +1,39 @@
+//--------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--------
+
+#include "objectslam/landmark/landmark_object.h"
#include "core/common/factory.h"
#include "core/math/rotations.h"
#include "core/yaml/yaml_conversion.h"
-#include "objectslam/landmark/landmark_object.h"
+#include <core/state_block/state_block_derived.h>
+#include "core/state_block/state_quaternion.h"
+
namespace wolf {
-LandmarkObject::LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp& _t_init, const Matrix3d& _intrinsic) :
- LandmarkBase("LandmarkObject", std::make_shared<StateBlock>(_pose.head(3)), std::make_shared<StateQuaternion>(_pose.tail(4))),
+LandmarkObject::LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp& _t_init) :
+ LandmarkBase("LandmarkObject", std::make_shared<StatePoint3d>(_pose.head(3)), std::make_shared<StateQuaternion>(_pose.tail(4))),
object_type_(_object_type),
- t_init_(_t_init),
- intrinsic_(_intrinsic)
+ t_init_(_t_init)
{
//
}
@@ -29,18 +53,6 @@ const std::string& LandmarkObject::getObjectType() const
return object_type_;
}
-const Matrix3d& LandmarkObject::getIntrinsic() const
-{
- return intrinsic_;
-}
-
-const Vector2d LandmarkObject::getImageCoord() const
-{
- Vector3d current_pose = getP()->getState();
- Vector3d image_coord = intrinsic_ * current_pose;
- return image_coord.head<2>();
-}
-
// LANDMARK SAVE AND LOAD FROM YAML
@@ -53,15 +65,7 @@ LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node)
Eigen::Vector3d pos = _lmk_node["position"] .as<Eigen::Vector3d>();
double ts_double = _lmk_node["timestamp"] .as<double>();
bool pos_fixed = _lmk_node["position fixed"] .as<bool>();
- double fx = _lmk_node["fx"] .as<double>();
- double fy = _lmk_node["fy"] .as<double>();
- double cx = _lmk_node["cx"] .as<double>();
- double cy = _lmk_node["cy"] .as<double>();
Eigen::Vector4d vquat;
- Matrix3d intrinsic;
- intrinsic << fx, 0, cx,
- 0, fy, cy,
- 0, 0, 1;
if (_lmk_node["orientation"].size() == 3)
{
@@ -74,15 +78,15 @@ LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node)
else if (_lmk_node["orientation"].size() == 4)
{
// we have been given a quaternion
- vquat = _lmk_node["orientation"] .as<Eigen::Vector4d>();
+ vquat = _lmk_node["orientation"] .as<Eigen::Vector4d>();
}
- bool ori_fixed = _lmk_node["orientation fixed"] .as<bool>();
+ bool ori_fixed = _lmk_node["orientation fixed"] .as<bool>();
Eigen::Vector7d pose; pose << pos, vquat;
// Create a new landmark
TimeStamp t_init(ts_double);
- LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type, t_init, intrinsic);
+ LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type, t_init);
lmk_ptr->setId(id);
lmk_ptr->getP()->setFixed(pos_fixed);
lmk_ptr->getO()->setFixed(ori_fixed);
@@ -99,10 +103,6 @@ YAML::Node LandmarkObject::saveToYaml() const
// Then Object specific things
node["object type"] = getObjectType();
node["timestamp"] = getInitTimestamp().get();
- node["fx"] = intrinsic_(0,0);
- node["fy"] = intrinsic_(1,1);
- node["cx"] = intrinsic_(0,2);
- node["cy"] = intrinsic_(1,2);
return node;
}
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 92f1b743ca6d9c98b7aade60055e86287cb4fd42..b506d1f016cfcfb2de8934d9752c32905561da46 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -1,9 +1,8 @@
#include <iostream>
#include <ostream>
-#include "core/math/rotations.h"
-#include <core/factor/factor_distance_3d.h>
-#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include <core/math/rotations.h>
+#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
// Wolf object plugin
#include "objectslam/processor/processor_tracker_landmark_object.h"
@@ -24,8 +23,6 @@ ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorT
min_features_for_keyframe_(_params_tracker_landmark_object->min_features_for_keyframe),
nb_vote_for_every_first_(_params_tracker_landmark_object->nb_vote_for_every_first_),
nb_vote_(0),
- fps_(_params_tracker_landmark_object->fps_),
- intrinsic_(_params_tracker_landmark_object->intrinsic_),
first_lmk_matching_(_params_tracker_landmark_object->first_lmk_matching_),
ratio_inliers_outliers_(_params_tracker_landmark_object->ratio_inliers_outliers_)
@@ -51,11 +48,9 @@ void ProcessorTrackerLandmarkObject::preProcess()
for (auto det: incoming_ptr->getObjectDetections()){
detections_incoming_.push_back(std::make_shared<FeatureObject>(det.measurement, det.meas_cov, det.object_type));
}
-
}
-
void ProcessorTrackerLandmarkObject::postProcess()
{
nb_vote_++;
@@ -125,7 +120,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _
std::string object_type = feat_obj->getObjectType();
TimeStamp t_init = getProblem()->getTimeStamp();
- return LandmarkBase::emplace<LandmarkObject>(getProblem()->getMap(), w_pose_t, object_type, t_init, intrinsic_);
+ return LandmarkBase::emplace<LandmarkObject>(getProblem()->getMap(), w_pose_t, object_type, t_init);
}
unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_new_features,
diff --git a/src/yaml/processor_tracker_landmark_object_yaml.cpp b/src/yaml/processor_tracker_landmark_object_yaml.cpp
index ee967bbb65b708b15e7e2b907afd7d49dd280996..2e73bbc9d402eeee32b92e59e72f10c84de274b6 100644
--- a/src/yaml/processor_tracker_landmark_object_yaml.cpp
+++ b/src/yaml/processor_tracker_landmark_object_yaml.cpp
@@ -45,18 +45,6 @@ static ParamsProcessorBasePtr createParamsProcessorLandmarkObject(const std::str
params->lmk_score_thr_ = config["lmk_score_thr"] .as<double>();
params->e_pos_thr_ = config["e_pos_thr"] .as<double>();
params->e_rot_thr_ = config["e_rot_thr"] .as<double>();
- params->fps_ = config["fps"] .as<int>();
-
- YAML::Node intrinsic = config["intrinsic"];
- double cx = intrinsic["cx"] .as<double>();
- double cy = intrinsic["cy"] .as<double>();
- double fx = intrinsic["fx"] .as<double>();
- double fy = intrinsic["fy"] .as<double>();
- Matrix3d intrinsic_matrix;
- intrinsic_matrix << fx, 0, cx,
- 0, fy, cy,
- 0, 0, 1;
- params->intrinsic_ = intrinsic_matrix;
return params;
}
diff --git a/test/gtest_landmark_object.cpp b/test/gtest_landmark_object.cpp
index 6f571e8231dff8d1384b07a8983ccc289e300895..5f4167aed5a8791bfe51fe832deec9ec0ba39e8a 100644
--- a/test/gtest_landmark_object.cpp
+++ b/test/gtest_landmark_object.cpp
@@ -1,7 +1,6 @@
#include <core/utils/utils_gtest.h>
-
#include "core/common/wolf.h"
#include "core/utils/logging.h"
@@ -28,8 +27,7 @@ TEST(LandmarkObject, getObjectType)
{
Vector7d p; p << 1,2,3, 0,0,0,1;
TimeStamp t;
- Matrix3d intrinsic;
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
+ LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t
ASSERT_EQ(l->getObjectType(), "thetype");
}
@@ -38,43 +36,17 @@ TEST(LandmarkObject, getPose)
Vector7d p;
TimeStamp t;
p << 0,0,0, 0,0,0,1;
- Matrix3d intrinsic;
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
+ LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t
ASSERT_MATRIX_APPROX(l->getState().vector("PO"), p, 1e-6);
}
-TEST(LandmarkObject, getImageCoord)
-{
- Vector7d p;
- TimeStamp t;
- p << 0,0,0, 0,0,0,1;
- Matrix3d intrinsic;
- intrinsic << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1;
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
- ASSERT_MATRIX_APPROX(l->getImageCoord(), p.head<2>(), 1e-6);
-}
-
-TEST(LandmarkObject, getIntrinsic)
-{
- Vector7d p; p << 1,2,3, 0,0,0,1;
- TimeStamp t;
- Matrix3d intrinsic;
- intrinsic << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1;
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
- ASSERT_MATRIX_APPROX(l->getIntrinsic(), intrinsic, 1e-6);
-}
TEST(LandmarkObject, getInitTimestamp)
{
Vector7d p; p << 1,2,3, 0,0,0,1;
TimeStamp t;
- Matrix3d intrinsic;
t.set(0.0);
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
+ LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t
ASSERT_EQ(l->getInitTimestamp(), 0.0);
}
diff --git a/test/processor_tracker_landmark_object.yaml b/test/processor_tracker_landmark_object.yaml
index a30b25705d989d8e6b13cd71285df028f973b2b5..edd2da0ab353141e78a1315fbf632acc26e02671 100644
--- a/test/processor_tracker_landmark_object.yaml
+++ b/test/processor_tracker_landmark_object.yaml
@@ -20,7 +20,5 @@ lmk_score_thr: 0.0
e_pos_thr: 0.1
e_rot_thr: 0.3
fps: 30
-reestimate_last_frame: false # for a better prior on the new keyframe: use only if no motion processor
-add_3d_cstr: false # add 3D constraints between the KF so that they do not jump when using apriltag only
max_new_features: -1
apply_loss_function: true