diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp
index 4532e5436415fd1030e7a7dedfd14a2ea90f854c..bf7c10e0981a8d150cffcb2c525e44f81b6f5cea 100644
--- a/demos/cosyslam_imu.cpp
+++ b/demos/cosyslam_imu.cpp
@@ -100,7 +100,7 @@ int main()
// 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});
+ VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.05});
ObjectDetections dets;
@@ -114,7 +114,7 @@ int main()
rosbag::View view(bag, rosbag::TopicQuery(topics));
ros::Time bag_begin_time = view.getBeginTime();
TimeStamp init_ts(bag_begin_time.toSec());
- FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, init_ts, 0.1);
+ FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
// Variables init
Matrix6d cov;
@@ -127,6 +127,7 @@ int main()
foreach(rosbag::MessageInstance const m, view)
{
+
std::cout << "--------FRAME " << frame_id << "-------------" << std::endl;
wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>();
@@ -147,7 +148,7 @@ int main()
dets.push_back(det);
}
TimeStamp ts(m.getTime().toSec());
- CaptureObjectPtr cap_obj = std::make_shared<CaptureObject>(ts, sen, dets);
+ CaptureObjectPtr cap_obj = std::make_shared<CaptureObject>(ts - init_ts, sen, dets);
cap_obj->process();
dets.clear();
}
@@ -163,7 +164,7 @@ int main()
cosyIMU->angular_velocity.z;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
TimeStamp ts(m.getTime().toSec());
- CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+ CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(ts - init_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 12d73da8dff4b126fdbf45c9292f3be9081fb63c..a636e9174df416b599417864f1dafb80b2d62933 100644
--- a/demos/yaml/cosyslam_imu.yaml
+++ b/demos/yaml/cosyslam_imu.yaml
@@ -1,8 +1,9 @@
# rosbag name
-bag: "/demos/bag/shortIMU_positiveShift.bag"
+bag: "/demos/bag/longIMU.bag"
# Camera to IMU transformation
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
+i_q_c: [ 0.01424561, -0.00273651, 0.00528824, 0.9998808]
+# i_q_w: [ 0.81615748, 0.38330015, -0.14646648, -0.40683601] for short IMU
+i_q_w: [ 0.86842613, -0.01545895, -0.0271302, -0.49483434]
diff --git a/demos/yaml/processor_tracker_landmark_object.yaml b/demos/yaml/processor_tracker_landmark_object.yaml
index d7e3ec645423f8e14db7b03ceed70fedb077b558..2a5a5b320bc5c89e5d4c70f1636aa365c4b9471a 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: 0.0
-e_pos_thr: 10
-e_rot_thr: 20
+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
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 6d0a7ed2f09e5a6b69d8301b2d27558a55851e85..268b5677b68b0da54fdb9064289c08a5966926fb 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -164,6 +164,8 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out)
{
+ std::cout << "in new feats" << std::endl;
+
// list of landmarks in the map
auto lmk_lst = getProblem()->getMap()->getLandmarkList();
for (auto feat : detections_last_)
@@ -176,6 +178,11 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
bool feature_already_found = false;
auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
+ if (feat->getCapture() != nullptr){
+ feature_already_found = true;
+ break;
+ }
+
// Loop over the landmark to find if one is associated to feat
for(auto lmk: lmk_lst){
LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
@@ -215,7 +222,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.1 && e_rot < e_rot_thr_+0.1){
+ if (e_pos < e_pos_thr_ && e_rot < e_rot_thr_){
feature_already_found = true;
break;
}
@@ -271,10 +278,17 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
-{
+{
+ std::cout << "in find landmarks" << std::endl;
+
for (auto feat : detections_incoming_)
{
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
+
+ if (feat->getCapture() != nullptr){
+ break;
+ }
+
for (auto lmk : _landmarks_in)
{
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);