diff --git a/CMakeLists.txt b/CMakeLists.txt
index 03941f81161807effe4b4bd14d0a411a5f5d5d14..0242a96b77867854aead97822c282920234509fd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -127,6 +127,7 @@ include_directories(BEFORE "include")
#HEADERS
SET(HDRS_CAPTURE
+include/objectslam/capture/capture_object.h
)
SET(HDRS_COMMON
)
@@ -227,6 +228,9 @@ IF(BUILD_DEMOS)
ADD_SUBDIRECTORY(demos)
ENDIF(BUILD_DEMOS)
+MESSAGE("Building demos.")
+ADD_SUBDIRECTORY(demos)
+
#install library
#=============================================================
diff --git a/cmake_modules/wolfobjectslamConfig.cmake b/cmake_modules/wolfobjectslamConfig.cmake
index 1fe81b49748991112acdc1f4a2ceff3a5ddeb464..228ef01c39a77072ef3729f458fd795de5b122bc 100644
--- a/cmake_modules/wolfobjectslamConfig.cmake
+++ b/cmake_modules/wolfobjectslamConfig.cmake
@@ -65,9 +65,9 @@ set(wolfobjectslam_FOUND TRUE)
# Now we gather all the required dependencies for Wolf Laser
-FIND_PACKAGE(wolfvision REQUIRED)
-list(APPEND wolfobjectslam_INCLUDE_DIRS ${wolfvision_INCLUDE_DIR})
-list(APPEND wolfobjectslam_LIBRARIES ${wolfvision_LIBRARY})
+#FIND_PACKAGE(wolfvision REQUIRED)
+#list(APPEND wolfobjectslam_INCLUDE_DIRS ${wolfvision_INCLUDE_DIR})
+#list(APPEND wolfobjectslam_LIBRARIES ${wolfvision_LIBRARY})
if(NOT OpenCV_FOUND)
FIND_PACKAGE(OpenCV REQUIRED)
diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index 0f67e0962df6dc8542928b8b88cec776d3d04b0a..e6c256c192b55ef6174859b1549df3c26f957a9c 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -14,6 +14,7 @@
#include "objectslam/processor/processor_tracker_landmark_object.h"
#include "objectslam/capture/capture_object.h"
#include "objectslam/internal/config.h"
+#include "objectslam/landmark/landmark_object.h"
using namespace wolf;
using namespace Eigen;
@@ -145,7 +146,7 @@ int main()
// setup the wolf problem //
////////////////////////////
- ProblemPtr problem = Problem::create("POV", 3);
+ ProblemPtr problem = Problem::create("PO", 3);
SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
solver->getSolverOptions().max_num_iterations = 200;
@@ -154,8 +155,8 @@ int main()
auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/processor_tracker_landmark_object.yaml");
auto prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject>(prc);
- VectorComposite x0("POV", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)});
- VectorComposite sig0("POV", {Vector3d(1, 1, 1), Vector3d(1, 1, 1), Vector3d(1,1,1)});
+ VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
+ VectorComposite sig0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
ObjectDetections dets;
@@ -223,8 +224,11 @@ int main()
// Save the results
std::fstream file_res;
std::fstream file_map;
+ std::fstream file_init;
std::string res_filename = wolf_root + "/demos/result.csv";
std::string map_filename = wolf_root + "/demos/map.csv";
+ std::string init_filename = wolf_root + "/demos/init.csv";
+ file_init.open(init_filename, std::fstream::out);
file_res.open(res_filename, std::fstream::out);
file_map.open(map_filename, std::fstream::out);
@@ -233,8 +237,11 @@ int main()
file_res << header_res;
std::string header_map = "id,px,py,pz,qx,qy,qz,qw,\n";
file_map << header_map;
+ std::string header_init = "id,ts,object_id,pose,\n";
+ file_init << header_init;
VectorComposite state_est;
+ VectorComposite state_lmk;
CaptureBasePtr cap_cosy;
counter = 0;
for (auto& elt: problem->getTrajectory()->getFrameMap()){
@@ -250,6 +257,38 @@ int main()
<< state_est['O'](1) << ","
<< state_est['O'](2) << ","
<< state_est['O'](3) << "\n";
+
+ // World to robot
+ Vector7d pose_rob;
+ pose_rob << state_est['P'], state_est['O'];
+ Isometry3d w_M_c = posevec_to_isometry(pose_rob);
+
+
+ int good_lmk = 0;
+ for (auto& lmk: problem->getMap()->getLandmarkList()){
+ auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+
+ if (good_lmk == 0 || good_lmk == 1 || good_lmk == 2){
+ // World to lmk
+ state_lmk = lmk->getState();
+ Vector7d pose_lmk;
+ pose_lmk << state_lmk['P'], state_lmk['O'];
+ Isometry3d w_M_lmk = posevec_to_isometry(pose_lmk);
+
+ // Cam to lmk
+ Isometry3d c_M_lmk = w_M_c.inverse() * w_M_lmk;
+
+ file_init << counter << ","
+ << t << ","
+ << lmk_obj->getObjectType() << ","
+ << c_M_lmk.matrix().row(0) << " "
+ << c_M_lmk.matrix().row(1) << " "
+ << c_M_lmk.matrix().row(2) << " "
+ << c_M_lmk.matrix().row(3) << " "
+ << "\n";
+ }
+ good_lmk++;
+ }
counter++;
}
@@ -271,6 +310,7 @@ int main()
}
file_res.close();
file_map.close();
+ file_init.close();
return 0;
}
\ No newline at end of file
diff --git a/demos/processor_tracker_landmark_object.yaml b/demos/processor_tracker_landmark_object.yaml
index f95f2422744379c4e658feb8d41e734a99da12a4..2a5a5b320bc5c89e5d4c70f1636aa365c4b9471a 100644
--- a/demos/processor_tracker_landmark_object.yaml
+++ b/demos/processor_tracker_landmark_object.yaml
@@ -9,6 +9,16 @@ vote:
min_features_for_keyframe: 1
nb_vote_for_every_first: 0
+intrinsic:
+ cx: 638.7251222
+ cy: 332.2076531
+ fx: 951.15435791
+ fy: 951.40795898
+
+lmk_score_thr: 3.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
diff --git a/include/objectslam/landmark/landmark_object.h b/include/objectslam/landmark/landmark_object.h
index b36d05c8d58dfebe7aea1b72da8939e21959633d..8fc7ca6195e5fa69f884af1ed518cf57a61816c0 100644
--- a/include/objectslam/landmark/landmark_object.h
+++ b/include/objectslam/landmark/landmark_object.h
@@ -26,7 +26,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);
+ LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp& _t_init, const Matrix3d& _intrinsic);
~LandmarkObject() override;
@@ -35,10 +35,20 @@ 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;
+ 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
*/
@@ -50,6 +60,7 @@ 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 fb34c7ee7d9e6c9cc277a6d7f593047edf1f224a..dfb3ab9ad0d98c6306289370d7e2f4b5dce05587 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -14,22 +14,38 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerLandmarkObject);
struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandmark
{
-
+
double min_time_vote_;
double max_time_vote_;
int nb_vote_for_every_first_;
bool add_3d_cstr_;
bool reestimate_last_frame_;
+ double lmk_score_thr_;
+ double e_pos_thr_;
+ double e_rot_thr_;
+ int fps_;
+ Matrix3d intrinsic_;
ParamsProcessorTrackerLandmarkObject() = default;
ParamsProcessorTrackerLandmarkObject(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorTrackerLandmark(_unique_name, _server)
{
- min_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/min_time_vote");
- max_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_vote");
- nb_vote_for_every_first_ = _server.getParam<int>(prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first");
- add_3d_cstr_ = _server.getParam<bool>(prefix + _unique_name + "/add_3d_cstr");
- reestimate_last_frame_ = _server.getParam<bool>(prefix + _unique_name + "/reestimate_last_frame");
+ min_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/min_time_vote");
+ max_time_vote_ = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_vote");
+ nb_vote_for_every_first_ = _server.getParam<int>(prefix + _unique_name + "/keyframe_vote/nb_vote_for_every_first");
+ add_3d_cstr_ = _server.getParam<bool>(prefix + _unique_name + "/add_3d_cstr");
+ reestimate_last_frame_ = _server.getParam<bool>(prefix + _unique_name + "/reestimate_last_frame");
+ 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");
+ intrinsic_ << fx, 0, cx,
+ 0, fy, cy,
+ 0, 0, 1;
}
std::string print() const override
{
@@ -38,7 +54,15 @@ struct ParamsProcessorTrackerLandmarkObject : public ParamsProcessorTrackerLandm
+ "max_time_vote_: " + std::to_string(max_time_vote_) + "\n"
+ "nb_vote_for_every_first_: " + std::to_string(nb_vote_for_every_first_) + "\n"
+ "add_3d_cstr_: " + std::to_string(add_3d_cstr_) + "\n"
- + "reestimate_last_frame_: " + std::to_string(reestimate_last_frame_) + "\n";
+ + "reestimate_last_frame_: " + std::to_string(reestimate_last_frame_) + "\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";
}
};
@@ -136,10 +160,15 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
protected:
double min_time_vote_;
double max_time_vote_;
+ double lmk_score_thr_;
+ double e_pos_thr_;
+ double e_rot_thr_;
unsigned int min_features_for_keyframe_;
int nb_vote_for_every_first_;
bool add_3d_cstr_;
int nb_vote_;
+ int fps_;
+ Matrix3d intrinsic_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
diff --git a/src/landmark/landmark_object.cpp b/src/landmark/landmark_object.cpp
index 8fccfb9364652d6a51b87de80ae02badd2e93113..cf913e422682508497d0aee0d27f49fbf6ba0b8b 100644
--- a/src/landmark/landmark_object.cpp
+++ b/src/landmark/landmark_object.cpp
@@ -5,10 +5,11 @@
namespace wolf {
-LandmarkObject::LandmarkObject(Eigen::Vector7d& _pose, const std::string& _object_type, const TimeStamp _t_init) :
+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))),
object_type_(_object_type),
- t_init_(_t_init)
+ t_init_(_t_init),
+ intrinsic_(_intrinsic)
{
//
}
@@ -18,7 +19,7 @@ LandmarkObject::~LandmarkObject()
//
}
-const TimeStamp LandmarkObject::getInitTimestamp() const
+const TimeStamp& LandmarkObject::getInitTimestamp() const
{
return t_init_;
}
@@ -28,6 +29,18 @@ 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
@@ -38,9 +51,17 @@ LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node)
unsigned int id = _lmk_node["id"] .as<unsigned int>();
std::string object_type = _lmk_node["object type"] .as<std::string>();
Eigen::Vector3d pos = _lmk_node["position"] .as<Eigen::Vector3d>();
- double ts_double = _lmk_node["timestamp"] .as<double>();
+ 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)
{
@@ -61,7 +82,7 @@ LandmarkBasePtr LandmarkObject::create(const YAML::Node& _lmk_node)
// Create a new landmark
TimeStamp t_init(ts_double);
- LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type, t_init);
+ LandmarkObjectPtr lmk_ptr = std::make_shared<LandmarkObject>(pose, object_type, t_init, intrinsic);
lmk_ptr->setId(id);
lmk_ptr->getP()->setFixed(pos_fixed);
lmk_ptr->getO()->setFixed(ori_fixed);
@@ -77,6 +98,11 @@ 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 9de7f27191c46d881b1840990aa52aad40b98b9e..c47fbc349e9c8e7db957f4e0f75d6d2e15cacedf 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -12,17 +12,22 @@ namespace wolf {
// Constructor
ProcessorTrackerLandmarkObject::ProcessorTrackerLandmarkObject( ParamsProcessorTrackerLandmarkObjectPtr _params_tracker_landmark_object) :
- ProcessorTrackerLandmark("ProcessorTrackerLandmarkObject", "POV", _params_tracker_landmark_object ),
+ ProcessorTrackerLandmark("ProcessorTrackerLandmarkObject", "PO", _params_tracker_landmark_object ),
reestimate_last_frame_(_params_tracker_landmark_object->reestimate_last_frame_),
n_reset_(0),
min_time_vote_(_params_tracker_landmark_object->min_time_vote_),
max_time_vote_(_params_tracker_landmark_object->max_time_vote_),
+ lmk_score_thr_(_params_tracker_landmark_object->lmk_score_thr_),
+ e_pos_thr_(_params_tracker_landmark_object->e_pos_thr_),
+ e_rot_thr_(_params_tracker_landmark_object->e_rot_thr_),
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_),
add_3d_cstr_(_params_tracker_landmark_object->add_3d_cstr_),
- nb_vote_(0)
+ nb_vote_(0),
+ fps_(_params_tracker_landmark_object->fps_),
+ intrinsic_(_params_tracker_landmark_object->intrinsic_)
{
-
+ std::cout << _params_tracker_landmark_object->print() << std::endl;
}
ProcessorTrackerLandmarkObject::~ProcessorTrackerLandmarkObject(){}
@@ -54,6 +59,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
// Prediction with constant speed hypothesis
double map_size = getProblem()->getTrajectory()->getFrameMap().size();
auto iter = getProblem()->getTrajectory()->getFrameMap().end();
+ double dt_frame = 1.0/double(fps_);
// Check if we have enough kf
if (map_size > 2){
@@ -67,13 +73,22 @@ void ProcessorTrackerLandmarkObject::postProcess()
double dt_kf = current_ts.get()-last_ts.get();
Vector3d linear_velocity = (current_pose->getP()->getState()-last_pose->getP()->getState())/dt_kf;
+ if (linear_velocity(0) == 0){
+ iter--;
+ current_pose = last_pose;
+ current_ts = last_ts;
+ last_pose = iter->second;
+ last_ts = iter->first;
+ dt_kf = current_ts.get()-last_ts.get();
+ linear_velocity = (current_pose->getP()->getState()-last_pose->getP()->getState())/dt_kf;
+ }
+
// Compute angular velocity with log map
Quaterniond current_quat(current_pose->getO()->getState().data());
Quaterniond last_quat(last_pose->getO()->getState().data());
Vector3d angular_velocity = log_q(current_quat.conjugate() * last_quat)/dt_kf;
// Integrate speed on current pose
- double dt_frame = 0.033333;
current_pose = getLast()->getFrame();
current_quat.coeffs() = current_pose->getO()->getState();
Vector3d new_pose = current_pose->getP()->getState() + linear_velocity*dt_frame;
@@ -90,7 +105,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
int number_of_factors = lmk_obj->getConstrainedByList().size();
double lmk_score = number_of_factors / dt_lmk;
- if (lmk_score < 3){
+ if (lmk_score < lmk_score_thr_){
iter_map->get()->remove();
}
@@ -142,7 +157,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);
+ return LandmarkBase::emplace<LandmarkObject>(getProblem()->getMap(), w_pose_t, object_type, t_init, intrinsic_);
}
unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_new_features,
@@ -199,7 +214,7 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
double e_pos = (pos_lmk - pos_feat).norm();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
- if (e_pos < 0.15 && e_rot < 0.4){
+ if (e_pos < e_pos_thr_+0.05 && e_rot < e_rot_thr_+0.05){
feature_already_found = true;
break;
}
@@ -265,6 +280,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
{
+
// world to rob
Vector3d pos = getLast()->getFrame()->getP()->getState();
Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
@@ -295,7 +311,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
double e_pos = (pos_lmk - pos_feat).norm();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
- if (e_rot < 0.3 && e_pos < 0.1){
+ if (e_rot < e_rot_thr_ && e_pos < e_pos_thr_){
_features_out.push_back(feat);
double score(1.0);
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
diff --git a/src/yaml/processor_tracker_landmark_object_yaml.cpp b/src/yaml/processor_tracker_landmark_object_yaml.cpp
index 88275f6fa7f3f4cbfaa24aeb47bf028386d247a8..08191efbf0488ed8272eb85420d8d76ad40743bb 100644
--- a/src/yaml/processor_tracker_landmark_object_yaml.cpp
+++ b/src/yaml/processor_tracker_landmark_object_yaml.cpp
@@ -45,6 +45,22 @@ static ParamsProcessorBasePtr createParamsProcessorLandmarkObject(const std::str
params->max_new_features = config["max_new_features"] .as<int>();
params->apply_loss_function = config["apply_loss_function"] .as<bool>();
+ 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;
}
else
diff --git a/test/gtest_landmark_object.cpp b/test/gtest_landmark_object.cpp
index 19eda41f1b5e1166585dcd892467c622fceb4f28..c23c6b6308c056d21cb5cf52517decf175dd943b 100644
--- a/test/gtest_landmark_object.cpp
+++ b/test/gtest_landmark_object.cpp
@@ -28,7 +28,8 @@ TEST(LandmarkObject, getObjectType)
{
Vector7d p;
TimeStamp t;
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t
+ Matrix3d intrinsic;
+ LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
ASSERT_EQ(l->getObjectType(), "thetype");
}
@@ -37,16 +38,43 @@ TEST(LandmarkObject, getPose)
Vector7d p;
TimeStamp t;
p << 0,0,0, 0,0,0,1;
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t
+ Matrix3d intrinsic;
+ LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // 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;
+ 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;
TimeStamp t;
+ Matrix3d intrinsic;
t.set(0.0);
- LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t); // pose, object_type, t
+ LandmarkObjectPtr l = std::make_shared<LandmarkObject>(p, "thetype", t, intrinsic); // pose, object_type, t
ASSERT_EQ(l->getInitTimestamp(), 0.0);
}
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index c66b12bc10686c3c7fbac772ea0a28e8cdc2ff98..6ee563efecfb1b0ffd520eece02b483f589cd2d8 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -230,6 +230,8 @@ TEST_F(ProcessorTrackerLandmarkObject_class, detectNewFeatures)
Eigen::Quaterniond quat;
Eigen::Vector7d pose;
Eigen::Matrix6d meas_cov = Matrix6d::Identity();
+ // Eigen::Matrix3d intrinsic = Matrix3d::Identity();
+ // TimeStamp t;
std::string object_type;
// feature 0
diff --git a/test/map_objects.yaml b/test/map_objects.yaml
index 6d728b1093aa1159a1fc0597b34364238208669a..00036e88f4195d4b9d83ca24ca5323d6ef4e5781 100644
--- a/test/map_objects.yaml
+++ b/test/map_objects.yaml
@@ -9,6 +9,10 @@ landmarks:
object type: "obj1"
position: [0, 0, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
+ fx: 400
+ fy: 400
+ cx: 600
+ cy: 600
timestamp: 0.0
position fixed: true
orientation fixed: true
@@ -18,6 +22,10 @@ landmarks:
object type: "obj2"
position: [0, 0, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
+ fx: 400
+ fy: 400
+ cx: 600
+ cy: 600
timestamp: 0.0
position fixed: true
orientation fixed: true
@@ -27,6 +35,10 @@ landmarks:
object type: "obj3"
position: [0, 0, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
+ fx: 400
+ fy: 400
+ cx: 600
+ cy: 600
timestamp: 0.0
position fixed: true
orientation fixed: true
@@ -36,6 +48,10 @@ landmarks:
object type: "obj4"
position: [0, 0, 0]
orientation: [0, 0, 0] # roll pitch yaw in degrees
+ fx: 400
+ fy: 400
+ cx: 600
+ cy: 600
timestamp: 0.0
position fixed: true
orientation fixed: true
diff --git a/test/processor_tracker_landmark_object.yaml b/test/processor_tracker_landmark_object.yaml
index 223aee9cc383a7fdcf8048167ad139ffa9e0797e..373f1895324fdd520fb72f57213ce868332b2f1d 100644
--- a/test/processor_tracker_landmark_object.yaml
+++ b/test/processor_tracker_landmark_object.yaml
@@ -7,7 +7,15 @@ vote:
min_features_for_keyframe: 12
nb_vote_for_every_first: 50
-reestimate_last_frame: true # for a better prior on the new keyframe: use only if no motion processor
-add_3d_cstr: true # add 3D constraints between the KF so that they do not jump when using apriltag only
+intrinsic:
+ cx: 638.7251222
+ cy: 332.2076531
+ fx: 951.15435791
+ fy: 951.40795898
+
+lmk_score_thr: 3.0
+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
\ No newline at end of file