diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index 0b67d3e7cfdaf9ac73eb72a033e1da745b819459..be0615e194aaf1105b726da831f28b7d105dcae6 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -5,6 +5,7 @@
#include <utility>
#include <string>
#include <Eigen/Dense>
+#include <yaml-cpp/yaml.h>
#include "core/problem/problem.h"
#include <core/ceres_wrapper/solver_ceres.h>
@@ -64,7 +65,7 @@ int main()
ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
auto sen = problem->installSensor("SensorPose", "sensor_pose", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), params_sp);
- auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/processor_tracker_landmark_object.yaml");
+ auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/yaml/processor_tracker_landmark_object.yaml");
auto prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject>(prc);
VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp
index a0d76e2d299eeea83fcb9e41daa7d80466d17501..4532e5436415fd1030e7a7dedfd14a2ea90f854c 100644
--- a/demos/cosyslam_imu.cpp
+++ b/demos/cosyslam_imu.cpp
@@ -59,10 +59,12 @@ int main()
// Retrieve parameters from config file
std::string bagfile = config["bag"].as<std::string>();
bool unfix_extrinsic = config["unfix_extrinsic"].as<bool>();
- std::vector<double> c_p_i_vec = config["c_p_i"].as<std::vector<double>>();
- std::vector<double> c_q_i_vec = config["c_q_i"].as<std::vector<double>>();
- Eigen::Map<Vector3d> c_p_i(c_p_i_vec.data());
- Eigen::Map<Vector4d> c_q_i(c_q_i_vec.data());
+ std::vector<double> i_p_c_vec = config["i_p_c"].as<std::vector<double>>();
+ std::vector<double> i_q_c_vec = config["i_q_c"].as<std::vector<double>>();
+ std::vector<double> i_q_w_vec = config["i_q_w"].as<std::vector<double>>();
+ Eigen::Map<Vector3d> i_p_c(i_p_c_vec.data());
+ Eigen::Map<Vector4d> i_q_c(i_q_c_vec.data());
+ Eigen::Map<Vector4d> i_q_w(i_q_w_vec.data());
// Rosbag opening
rosbag::Bag bag;
@@ -79,25 +81,26 @@ int main()
// Cosypose processor
ParamsSensorPosePtr params_sp = std::make_shared<ParamsSensorPose>();
- auto sen = problem->installSensor("SensorPose", "sensor_pose", (Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), params_sp);
+ Vector7d extr_cam; extr_cam << i_p_c, i_q_c;
+ auto sen = problem->installSensor("SensorPose", "sensor_pose", extr_cam, params_sp);
+ if (unfix_extrinsic){
+ sen->unfixExtrinsics();
+ }
+ else{
+ sen->fixExtrinsics();
+ }
auto prc = problem->installProcessor("ProcessorTrackerLandmarkObject", "objects_wrapper", "sensor_pose", wolf_root + "/demos/yaml/processor_tracker_landmark_object.yaml");
auto prc_obj = std::static_pointer_cast<ProcessorTrackerLandmarkObject>(prc);
// IMU processor
- Vector7d extr_imu; extr_imu << c_p_i, c_q_i;
- SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu", extr_imu, wolf_root + "/demos/yaml/realsense_imu.yaml");
+ SensorBasePtr sen_imu_base = problem->installSensor("SensorImu", "SenImu",(Vector7d() << 0, 0, 0, 0, 0, 0, 1).finished(), wolf_root + "/demos/yaml/realsense_imu.yaml");
SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
- if (unfix_extrinsic){
- sen_imu->unfixExtrinsics();
- }
- else{
- sen_imu->fixExtrinsics();
- }
ProcessorBasePtr proc_imu_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", wolf_root + "/demos/yaml/processor_imu.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_imu_base);
- VectorComposite x0("POV", { Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()});
- VectorComposite sig0("POV", {Vector3d::Ones(), Vector3d::Ones(), Vector3d::Ones()});
+ // The initial orientation is given in the config file
+ VectorComposite x0("POV", { Vector3d::Zero(), i_q_w, Vector3d::Zero()});
+ VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones(), Vector3d::Ones()*0.05});
ObjectDetections dets;
@@ -106,8 +109,8 @@ int main()
///////////////
std::vector<std::string> topics;
- topics.push_back(std::string("/cosypose"));
topics.push_back(std::string("/imu"));
+ topics.push_back(std::string("/cosypose"));
rosbag::View view(bag, rosbag::TopicQuery(topics));
ros::Time bag_begin_time = view.getBeginTime();
TimeStamp init_ts(bag_begin_time.toSec());
@@ -143,7 +146,7 @@ int main()
ObjectDetection det = {.measurement = poseVec, .detection_score = object.score, .meas_cov = cov, .object_type = object.name};
dets.push_back(det);
}
- TimeStamp ts(cosyArray->header.stamp.sec, cosyArray->header.stamp.nsec);
+ TimeStamp ts(m.getTime().toSec());
CaptureObjectPtr cap_obj = std::make_shared<CaptureObject>(ts, sen, dets);
cap_obj->process();
dets.clear();
@@ -159,7 +162,7 @@ int main()
cosyIMU->angular_velocity.y,
cosyIMU->angular_velocity.z;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
- TimeStamp ts(cosyIMU->header.stamp.sec, cosyIMU->header.stamp.nsec);
+ TimeStamp ts(m.getTime().toSec());
CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
cap_imu->process();
}
diff --git a/demos/yaml/cosyslam_imu.yaml b/demos/yaml/cosyslam_imu.yaml
index 7ff2f0ce3bb909749fa29383c543cef260c155e6..12d73da8dff4b126fdbf45c9292f3be9081fb63c 100644
--- a/demos/yaml/cosyslam_imu.yaml
+++ b/demos/yaml/cosyslam_imu.yaml
@@ -1,7 +1,8 @@
# rosbag name
-bag: "/demos/bag/shortIMU.bag"
+bag: "/demos/bag/shortIMU_positiveShift.bag"
# Camera to IMU transformation
-unfix_extrinsic: True
-c_p_i: [-0.00772693, -0.00208873, -0.00794716]
-c_q_i: [0.01048408, 0.01594998, 0.00134348, 0.99981692]
\ No newline at end of file
+unfix_extrinsic: false
+i_p_c: [-0.0111433, 0.00260534, 0.00542655]
+i_q_c: [ 0.01424561, -0.00273651, 0.00528824, 0.9998808 ]
+i_q_w: [ 0.00112227, -0.01527152, -0.03007937, 0.99943021]
\ No newline at end of file
diff --git a/demos/yaml/processor_imu.yaml b/demos/yaml/processor_imu.yaml
index f92b74c06167c96ef4294e00600c7270bd0d20e2..b67f8597746ce9c580fb4a01d5082a1387c6a446 100644
--- a/demos/yaml/processor_imu.yaml
+++ b/demos/yaml/processor_imu.yaml
@@ -4,7 +4,7 @@ keyframe_vote:
max_buff_length: 100000000000
max_time_span: 0.19999
voting_active: true
-name: Main imu
+name: SenImu
time_tolerance: 0.0005
type: ProcessorImu
unmeasured_perturbation_std: 1.0e-06
diff --git a/demos/yaml/processor_tracker_landmark_object.yaml b/demos/yaml/processor_tracker_landmark_object.yaml
index 85e125df3de29d4d4b77eb9489f5d9966277b90d..d7e3ec645423f8e14db7b03ceed70fedb077b558 100644
--- a/demos/yaml/processor_tracker_landmark_object.yaml
+++ b/demos/yaml/processor_tracker_landmark_object.yaml
@@ -15,9 +15,9 @@ intrinsic:
fx: 951.15435791
fy: 951.40795898
-lmk_score_thr: 4.0
-e_pos_thr: 0.1
-e_rot_thr: 0.3
+lmk_score_thr: 0.0
+e_pos_thr: 10
+e_rot_thr: 20
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
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index a5c33ede44d7cedf4fdf2809d90fbedb8f36cbc9..6d0a7ed2f09e5a6b69d8301b2d27558a55851e85 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -171,6 +171,7 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
// max_new_features reached
if (_max_new_features != -1 && _features_out.size() == _max_new_features)
break;
+
bool feature_already_found = false;
auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
@@ -223,7 +224,6 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
if (!feature_already_found)
{
- std::cout << feat_obj->getObjectType() << std::endl;
// If the feature is not in the map & not in the list of newly detected features yet, then we add it.
_features_out.push_back(feat);