add initial orientation

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()});

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();
         }

demos/yaml/cosyslam_imu.yaml

 # 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]
+unfix_extrinsic: false
+i_p_c: [-0.0111433, 0.00260534, 0.00542655]
+i_q_c: [ 0.01424561, -0.00273651,  0.00528824,  0.9998808 ]
\ No newline at end of file
+i_q_w: [ 0.00112227, -0.01527152, -0.03007937,  0.99943021]
\ No newline at end of file

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

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

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);