final commit

demos/cosyslam.cpp

@@ -22,6 +22,7 @@
 #include <wolf_ros_objectslam/CosyObjectArray.h>
 #include <wolf_ros_objectslam/CosyObject.h>
 #include <std_msgs/Header.h>
+#include <sensor_msgs/Imu.h>
 #include <boost/foreach.hpp>
 #define foreach BOOST_FOREACH
 
@@ -68,20 +69,21 @@ int main()
     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()});
-    VectorComposite sig0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
-
-    ObjectDetections dets;
-
-    ///////////////
-    // SLAM loop //
-    ///////////////
-
+    // Rosbag topics target
     std::vector<std::string> topics;
     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());
+    int frame_id = 0;
+
+    ///////////////
+    // SLAM loop //
+    ///////////////
+
+    ObjectDetections dets;
+    VectorComposite x0("PO", {Vector3d::Zero(),  Quaterniond::Identity().coeffs()});
+    VectorComposite sig0("PO", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1});
     FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
 
     // Variables init
@@ -90,7 +92,14 @@ int main()
     Vector7d poseVec;
     int number_of_kf = 0;
     int counter = 0;
-    int frame_id = 0;
+
+    // test map live
+    std::fstream file_init_world;
+    std::string init_world_filename = wolf_root + "/demos/init_world.csv";
+    file_init_world.open(init_world_filename, std::fstream::out);
+    std::string header_init_world = "id,t,object_type,px,py,pz,qx,qy,qz,qw,\n";
+    file_init_world << header_init_world;
+    std::vector<int> kf_id;
 
 
     foreach(rosbag::MessageInstance const m, view)
@@ -124,7 +133,27 @@ int main()
         number_of_kf = problem->getTrajectory()->getFrameMap().size();
         if (number_of_kf > counter){
             std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
+            kf_id.push_back(frame_id);
             counter++;
+            // save the map
+            VectorComposite state_lmk;
+            for (auto& lmk: problem->getMap()->getLandmarkList()){
+                auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+                // World to lmk
+                state_lmk = lmk_obj->getState();
+        
+                file_init_world << counter << ","
+                        << problem->getTimeStamp() << ","
+                        << lmk_obj->getObjectType() << ","
+                        << state_lmk['P'](0) << ","
+                        << state_lmk['P'](1) << ","
+                        << state_lmk['P'](2) << "," 
+                        << state_lmk['O'](0) << "," 
+                        << state_lmk['O'](1) << "," 
+                        << state_lmk['O'](2) << "," 
+                        << state_lmk['O'](3) << "\n";   
+
+            }
         }
         frame_id ++;
     }
@@ -140,7 +169,7 @@ int main()
 
     problem->print(4,true,true,true);
 
-    // Save the results
+    /// Save the results
     std::fstream file_res;
     std::fstream file_map;
     std::fstream file_init;
@@ -202,7 +231,7 @@ int main()
             Vector3d image_coord = intrinsic * c_t_lmk;
             image_coord = image_coord/image_coord(2);
             
-            file_init << 3*counter << ","
+            file_init << kf_id[counter] << ","
                     << t << ","
                     << lmk_obj->getObjectType() << ","
                     << c_M_lmk.matrix().row(0) << " "
@@ -237,6 +266,8 @@ int main()
     file_res.close();
     file_map.close();
     file_init.close();
+    file_init_world.close();
+
 
     return 0;
 }
\ No newline at end of file

demos/cosyslam_imu.cpp

@@ -156,13 +156,14 @@ int main()
     frame_id = 0;
 
     VectorComposite x0("POV", { Vector3d::Zero(), q_init, Vector3d::Zero()});
-    VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.05});
+    VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.01});
     FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
 
     foreach(rosbag::MessageInstance const m, view)
     {
         std::cout << "--------FRAME " << frame_id << "-------------" << std::endl;
-
+        TimeStamp ts(m.getTime().toSec());
+        
         wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>();
         if (cosyArray != nullptr){
             for (auto object : cosyArray->objects){
@@ -180,7 +181,6 @@ int main()
                 ObjectDetection det = {.measurement = poseVec, .detection_score = object.score, .meas_cov = cov, .object_type = object.name};
                 dets.push_back(det);
             }
-            TimeStamp ts(m.getTime().toSec());
             CaptureObjectPtr cap_obj = std::make_shared<CaptureObject>(ts - init_ts, sen, dets);
             cap_obj->process();
             dets.clear();
@@ -196,7 +196,6 @@ int main()
                 cosyIMU->angular_velocity.y,
                 cosyIMU->angular_velocity.z;
             Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
-            TimeStamp ts(m.getTime().toSec());
             CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(ts - init_ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
             cap_imu->process();
         }
@@ -258,14 +257,12 @@ 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);
 
@@ -284,7 +281,7 @@ int main()
             Vector3d image_coord = intrinsic * c_t_lmk;
             image_coord = image_coord/image_coord(2);
             
-            file_init << 3*counter << ","
+            file_init << counter << ","
                     << t << ","
                     << lmk_obj->getObjectType() << ","
                     << c_M_lmk.matrix().row(0) << " "
@@ -295,8 +292,8 @@ int main()
                     << image_coord(1) << " "
                     << "\n";
 
-            good_lmk++;
         }
+
         counter++; 
     }
 

demos/yaml/cosyslam_imu.yaml

@@ -3,7 +3,7 @@ bag: "/demos/bag/demo_imu_wolf.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.81615748,  0.38330015, -0.14646648, -0.40683601] # for short IMU
-# i_q_w: [ 0.86842613, -0.01545895, -0.0271302,  -0.49483434] # for long IMU
+i_p_c: [-0.0111433, 0.00260534, 0.00542655] # L515
+#i_p_c:  [-0.02037261, 0.00497111, 0.011167] # D435
+i_q_c: [0.0, 0.0, 0.0, 1.0]
+

demos/yaml/processor_tracker_landmark_object.yaml

@@ -9,11 +9,17 @@ 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
+
 intrinsic:
-    cx:                         638.7251222
-    cy:                         332.2076531
-    fx:                         951.15435791
-    fy:                         951.40795898
+    cx:                         608.80518282
+    cy:                         353.95008807
+    fx:                         934.69763184
+    fy:                         944.0513916
 
 lmk_score_thr: 4.0
 e_pos_thr: 0.1

src/capture/capture_object.cpp

@@ -61,6 +61,10 @@ Matrix6d polynomial_covariance(ParamsCapturePtr params, ObjectDetection det)
     // a Lambda to compute error from coeficients
     auto error = [] (std::vector<double> coefs, double r, double theta, double phi, double s)
         {
+            // case of degree 0 polynomial
+            if (coefs.size() == 1){
+                return coefs.at(0);
+            }
             // case of degree one polynomial
             if (coefs.size() == 6){
                 return coefs.at(0) + coefs.at(1)*r + coefs.at(2)*theta + coefs.at(3)*phi + coefs.at(4)*s + coefs.at(5);

yaml_error/obj_000001.yaml

+type: "Capture"
+
+object_name: "obj_000001"
+coefs_err0: [0.05]
+coefs_err1: [0.05]
+coefs_err2: [0.05]
+coefs_err3: [0.1]
+coefs_err4: [0.1]
+coefs_err5: [0.1]
\ No newline at end of file