diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index 23b6e315782d514457ab1abc8a63c8a20167d502..a8379b79927e0b211982be23f1de794f6b4b62db 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -119,7 +119,7 @@ int main()
/////////////////////////
std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
- std::string filename = wolf_root + "/demos/input_demo2.csv";
+ std::string filename = wolf_root + "/demos/input_demoMulti.csv";
std::vector<std::pair<std::string, std::vector<std::string>>> csv_values;
csv_values = read_csv(filename);
@@ -166,7 +166,7 @@ int main()
Matrix6d cov;
Vector6d sig;
// default covariance value : the real value is computed in the processor
- sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05;
+ sig << 0.02, 0.02, 0.02, 0.1, 0.1, 0.1;
cov = sig.array().matrix().asDiagonal();
std::string object_name;
object_name = csv_values.at(idx_obj).second.at(i);
diff --git a/demos/processor_tracker_landmark_object.yaml b/demos/processor_tracker_landmark_object.yaml
index 2a5a5b320bc5c89e5d4c70f1636aa365c4b9471a..85e125df3de29d4d4b77eb9489f5d9966277b90d 100644
--- a/demos/processor_tracker_landmark_object.yaml
+++ b/demos/processor_tracker_landmark_object.yaml
@@ -15,7 +15,7 @@ intrinsic:
fx: 951.15435791
fy: 951.40795898
-lmk_score_thr: 3.0
+lmk_score_thr: 4.0
e_pos_thr: 0.1
e_rot_thr: 0.3
fps: 30
diff --git a/src/capture/capture_object.cpp b/src/capture/capture_object.cpp
index d22f0e4a57f55c6c7725ec77a25da05c87ca9a4e..706cd0117c38fb6579dcef43b3977d2ea65f5bc8 100644
--- a/src/capture/capture_object.cpp
+++ b/src/capture/capture_object.cpp
@@ -6,7 +6,14 @@ namespace wolf {
static ParamsCapturePtr createParamsCapture(const std::string & _filename_dot_yaml)
{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+ YAML::Node config;
+ try{
+ config = YAML::LoadFile(_filename_dot_yaml);
+ }
+ catch(YAML::BadFile) {
+ std::cout << "Unknown Object" << std::endl;
+ return nullptr;
+ }
if (config.IsNull())
{
@@ -40,7 +47,7 @@ Isometry3d posevec_to_isometry(Vector7d pose)
return Translation<double, 3>(pose.head<3>()) * Quaterniond(pose.tail<4>());
}
-Matrix6d polynomial_covariance(ParamsCapturePtr params, ObjectDetection det, int degree)
+Matrix6d polynomial_covariance(ParamsCapturePtr params, ObjectDetection det)
{
// We need to get the inverse of the measurement for the error model
Isometry3d o_M_c = posevec_to_isometry(det.measurement);
@@ -88,8 +95,10 @@ CaptureObject::CaptureObject(const TimeStamp& ts, SensorBasePtr _sensor_ptr, con
string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
for (auto& elt: object_detections_){
ParamsCapturePtr params_capture = createParamsCapture(wolf_root + "/yaml_error/" + elt.object_type + ".yaml");
- Matrix6d cov = polynomial_covariance(params_capture, elt, 1);
- elt.meas_cov = cov;
+ if (params_capture != nullptr){
+ Matrix6d cov = polynomial_covariance(params_capture, elt);
+ elt.meas_cov = cov;
+ }
}
}
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index c47fbc349e9c8e7db957f4e0f75d6d2e15cacedf..9cd9347665c550e621169c6438afd9a9059bb05b 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -56,46 +56,46 @@ void ProcessorTrackerLandmarkObject::postProcess()
{
nb_vote_++;
- // 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){
- // Compute linear velocity
- iter--;
- FrameBasePtr current_pose = iter->second;
- TimeStamp current_ts = iter->first;
- iter--;
- FrameBasePtr last_pose = iter->second;
- TimeStamp last_ts = iter->first;
- 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;
- }
+ // // 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){
+ // // Compute linear velocity
+ // iter--;
+ // FrameBasePtr current_pose = iter->second;
+ // TimeStamp current_ts = iter->first;
+ // iter--;
+ // FrameBasePtr last_pose = iter->second;
+ // TimeStamp last_ts = iter->first;
+ // 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
- current_pose = getLast()->getFrame();
- current_quat.coeffs() = current_pose->getO()->getState();
- Vector3d new_pose = current_pose->getP()->getState() + linear_velocity*dt_frame;
- Quaterniond new_quat = exp_q(log_q(current_quat) + angular_velocity*dt_frame);
- getLast()->getFrame()->getP()->setState(new_pose);
- getLast()->getFrame()->getO()->setState(new_quat.coeffs());
- }
+ // // 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
+ // current_pose = getLast()->getFrame();
+ // current_quat.coeffs() = current_pose->getO()->getState();
+ // Vector3d new_pose = current_pose->getP()->getState() + linear_velocity*dt_frame;
+ // Quaterniond new_quat = exp_q(log_q(current_quat) + angular_velocity*dt_frame);
+ // getLast()->getFrame()->getP()->setState(new_pose);
+ // getLast()->getFrame()->getO()->setState(new_quat.coeffs());
+ // }
// Clear the landmark map
LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -214,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 < e_pos_thr_+0.05 && e_rot < e_rot_thr_+0.05){
+ if (e_pos < e_pos_thr_+0.08 && e_rot < e_rot_thr_+0.08){
feature_already_found = true;
break;
}
@@ -240,7 +240,6 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
// if no detections in last capture, no case where it is usefull to create a KF from last
if (detections_last_.empty())
return false;
-
auto origin = getOrigin();
auto incoming = getIncoming();
std::cout << "voteForKeyFrame" << std::endl;
@@ -271,7 +270,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
-{
+{
for (auto feat : detections_incoming_)
{
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
diff --git a/src/yaml/capture_yaml.cpp b/src/yaml/capture_yaml.cpp
deleted file mode 100644
index 582c1b88455cd64c7d1e9674e391e0e0004acc56..0000000000000000000000000000000000000000
--- a/src/yaml/capture_yaml.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-/**
- * \file capture_yaml.cpp
- *
- * Created on: July 20, 2021
- * \author: cézou
- */
-
-
-// wolf
-#include "core/yaml/yaml_conversion.h"
-#include "core/common/factory.h"
-#include "objectslam/capture/capture_object.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ParamsCapturePtr createParamsCapture(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config.IsNull())
- {
- WOLF_ERROR("Invalid YAML file!");
- return nullptr;
- }
- else if (config["type"].as<std::string>() == "Capture")
- {
- ParamsCapturePtr params = std::make_shared<ParamsCapture>();
-
- params->object_name = config["object_name"] .as<string>();
- params->coefs = config["coefs"] .as<string>();
-
- return params;
- }
- else
- {
- WOLF_ERROR("Wrong capture type! Should be \"Capture\"");
- return nullptr;
- }
- return nullptr;
-}
-
-} // namespace [unnamed]
-
-} // namespace wolf
\ No newline at end of file
diff --git a/yaml_error/obj_000025.yaml b/yaml_error/obj_000025.yaml
index 8a649028aeb5afadde98f4d409600bf430f4bdac..a198e2c4e53aa5a03639b2dedde555edf9bccc34 100644
--- a/yaml_error/obj_000025.yaml
+++ b/yaml_error/obj_000025.yaml
@@ -1,6 +1,6 @@
type: "Capture"
-object_name: "obj_000026"
+object_name: "obj_000025"
coefs_err0: [ 0. , 84.91150233, -0.63217472, -33.62072986,
854.66187072, 1.06670745, -0.02315998, 0.1178933 ,
-85.71334945, -0.00109375, 0.00011923, 0.63880521,