IMU integration & yaml parsing

demos/CMakeLists.txt

@@ -15,11 +15,13 @@ FIND_PACKAGE(catkin REQUIRED COMPONENTS
   wolf_ros_objectslam
   wolf_ros_node
 )
+FIND_PACKAGE(wolfimu REQUIRED)
 
 INCLUDE_DIRECTORIES(
     ${rosbag_INCLUDE_DIRS}
     ${wolf_ros_objectslam_INCLUDE_DIRS}
     ${std_msgs_INCLUDE_DIRS}
+    ${wolfimu_INCLUDE_DIRS}
 )
 
 
@@ -37,4 +39,12 @@ TARGET_LINK_LIBRARIES(cosyslam
     ${catkin_LIBRARIES}
 )
 
+ADD_EXECUTABLE(cosyslam_imu cosyslam_imu.cpp)
+TARGET_LINK_LIBRARIES(cosyslam_imu 
+    ${wolfcore_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${catkin_LIBRARIES}
+    ${wolfimu_LIBRARIES}
+)
+
 

demos/cosyslam.cpp

@@ -41,10 +41,17 @@ Vector7d isometry_to_posevec(Isometry3d M)
 
 int main()
 {
+    // Config init
+    std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+    std::string yaml_path = wolf_root + "/demos/yaml/cosyslam_imu.yaml";
+    YAML::Node config = YAML::LoadFile(yaml_path);
+
+    // Retrieve parameters from config file
+    std::string bagfile = config["bag"].as<std::string>();
+
     // Rosbag opening
     rosbag::Bag bag;
-    std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
-    std::string bagpath = wolf_root + "/demos/demoMulti.bag";
+    std::string bagpath = wolf_root + bagfile;
     bag.open(bagpath, rosbag::bagmode::Read);
 
     ////////////////////////////

demos/cosyslam_imu.cpp

+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <stdexcept>
+#include <utility>
+#include <string>
+#include <Eigen/Dense>
+#include <yaml-cpp/yaml.h>
+
+#include "core/problem/problem.h"
+#include <core/ceres_wrapper/solver_ceres.h>
+#include "core/sensor/sensor_pose.h" // not necessary
+#include "core/frame/frame_base.h"
+
+// Object SLAM
+#include "objectslam/processor/processor_tracker_landmark_object.h"
+#include "objectslam/capture/capture_object.h"
+#include "objectslam/internal/config.h"
+#include "objectslam/landmark/landmark_object.h"
+
+// IMU
+#include "imu/sensor/sensor_imu.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu.h"
+#include "imu/factor/factor_imu.h"
+
+// ROS
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <wolf_ros_objectslam/CosyObjectArray.h>
+#include <wolf_ros_objectslam/CosyObject.h>
+#include <sensor_msgs/Imu.h>
+#include <std_msgs/Header.h>
+#include <boost/foreach.hpp>
+#define foreach BOOST_FOREACH
+
+using namespace wolf;
+using namespace Eigen;
+
+Isometry3d posevec_to_isometry(Vector7d pose)
+{
+    return Translation<double, 3>(pose.head<3>()) * Quaterniond(pose.tail<4>());
+}
+
+Vector7d isometry_to_posevec(Isometry3d M)
+{
+    Vector7d pose_vec;
+    pose_vec << M.translation(), Quaterniond(M.rotation()).coeffs();
+    return pose_vec;
+}
+
+int main()
+{
+    // Config init
+    std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
+    std::string yaml_path = wolf_root + "/demos/yaml/cosyslam_imu.yaml";
+    YAML::Node config = YAML::LoadFile(yaml_path);
+
+    // 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());
+
+    // Rosbag opening
+    rosbag::Bag bag;
+    std::string bagpath = wolf_root + bagfile;
+    bag.open(bagpath, rosbag::bagmode::Read);
+
+    ////////////////////////////
+    // setup the wolf problem //
+    ////////////////////////////
+
+    ProblemPtr problem = Problem::create("POV", 3);
+    SolverCeresPtr solver = std::make_shared<SolverCeres>(problem);
+    solver->getSolverOptions().max_num_iterations = 200;
+
+    // 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);
+    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");
+    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()});
+
+    ObjectDetections dets;
+
+    ///////////////
+    // SLAM loop //
+    ///////////////
+
+    std::vector<std::string> topics;
+    topics.push_back(std::string("/cosypose"));
+    topics.push_back(std::string("/imu"));
+    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);
+
+    // Variables init
+    Matrix6d cov;
+    Vector6d sig;
+    Vector7d poseVec;
+    int number_of_kf = 0;
+    int counter = 0;
+    int frame_id = 0;
+
+
+    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>();
+        if (cosyArray != nullptr){
+            for (auto object : cosyArray->objects){
+                poseVec << object.pose.position.x,
+                    object.pose.position.y,
+                    object.pose.position.z,
+                    object.pose.orientation.x,
+                    object.pose.orientation.y,
+                    object.pose.orientation.z,
+                    object.pose.orientation.w;
+
+                // default covariance value : the real value is computed in the processor
+                sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05;
+                cov = sig.array().matrix().asDiagonal();
+                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);
+            CaptureObjectPtr cap_obj = std::make_shared<CaptureObject>(ts, sen, dets);
+            cap_obj->process();
+            dets.clear();
+        }
+
+        sensor_msgs::Imu::ConstPtr cosyIMU = m.instantiate<sensor_msgs::Imu>();
+        if (cosyIMU != nullptr){
+            Vector6d acc_gyr_meas;
+            acc_gyr_meas << cosyIMU->linear_acceleration.x,
+                cosyIMU->linear_acceleration.y,
+                cosyIMU->linear_acceleration.z,
+                cosyIMU->angular_velocity.x,
+                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);
+            CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
+            cap_imu->process();
+        }
+
+        // Solve if a KF was voted
+        number_of_kf = problem->getTrajectory()->getFrameMap().size();
+        if (number_of_kf > counter){
+            std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
+            counter++;
+        }
+        frame_id ++;
+    }
+
+    bag.close();
+
+    //////////////////////
+    // results analysis //
+    //////////////////////
+
+    std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
+    std::cout << report << std::endl;
+
+    problem->print(4,true,true,true);
+
+    // Save the results
+    std::fstream file_res;
+    std::fstream file_map;
+    std::fstream file_init;
+    std::string res_filename = wolf_root + "/demos/result.csv";
+    std::string map_filename = wolf_root + "/demos/map.csv";
+    std::string init_filename = wolf_root + "/demos/init.csv";
+    file_init.open(init_filename, std::fstream::out);
+    file_res.open(res_filename, std::fstream::out);
+    file_map.open(map_filename, std::fstream::out);
+
+    // Write headers for csv files
+    std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw,\n";
+    file_res << header_res;
+    std::string header_map = "id,px,py,pz,qx,qy,qz,qw,\n";
+    file_map << header_map;
+    std::string header_init = "id,ts,object_id,pose,image_coord,\n";
+    file_init << header_init;
+
+    VectorComposite state_est;
+    VectorComposite state_lmk;
+    CaptureBasePtr cap_cosy;
+    counter = 0;
+    for (auto& elt: problem->getTrajectory()->getFrameMap()){ 
+        TimeStamp t = elt.first; 
+        FrameBasePtr kf = elt.second; 
+        state_est = kf->getState();
+        file_res << counter << "," 
+                 << t << ","
+                 << state_est['P'](0) << ","
+                 << state_est['P'](1) << ","
+                 << state_est['P'](2) << "," 
+                 << state_est['O'](0) << "," 
+                 << 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);
+
+            // World to lmk
+            state_lmk = lmk_obj->getState();
+            Vector7d pose_lmk;
+            pose_lmk << state_lmk['P'], state_lmk['O'];
+            Isometry3d w_M_lmk = posevec_to_isometry(pose_lmk);
+
+            // Cam to lmk
+            Isometry3d c_M_lmk =  w_M_c.inverse() * w_M_lmk;
+
+            // coordinates of the object in the image
+            Matrix3d intrinsic = lmk_obj->getIntrinsic();
+            Vector3d c_t_lmk = c_M_lmk.translation();
+            Vector3d image_coord = intrinsic * c_t_lmk;
+            image_coord = image_coord/image_coord(2);
+            
+            file_init << 3*counter << ","
+                    << t << ","
+                    << lmk_obj->getObjectType() << ","
+                    << c_M_lmk.matrix().row(0) << " "
+                    << c_M_lmk.matrix().row(1) << " "
+                    << c_M_lmk.matrix().row(2) << " "
+                    << c_M_lmk.matrix().row(3) << "  ,"
+                    << image_coord(0) << " "
+                    << image_coord(1) << " "
+                    << "\n";
+
+            good_lmk++;
+        }
+        counter++; 
+    }
+
+    counter = 0;
+    for (auto& elt: problem->getMap()->getLandmarkList()){
+        state_est = elt->getState();
+        if (elt->getConstrainedByList().size() < 15){
+            continue;
+        }
+        file_map << counter << ","
+                 << state_est['P'](0) << ","
+                 << state_est['P'](1) << ","
+                 << state_est['P'](2) << "," 
+                 << state_est['O'](0) << "," 
+                 << state_est['O'](1) << "," 
+                 << state_est['O'](2) << "," 
+                 << state_est['O'](3) << "\n";
+        counter++; 
+    } 
+    file_res.close();
+    file_map.close();
+    file_init.close();
+
+    return 0;
+}
\ No newline at end of file

demos/yaml/cosyslam_imu.yaml

+# rosbag name
+bag: "/demos/bag/shortIMU.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

demos/yaml/processor_imu.yaml

+keyframe_vote:
+  angle_turned: 1000
+  dist_traveled: 20000.0
+  max_buff_length: 100000000000
+  max_time_span: 0.19999
+  voting_active: true
+name: Main imu
+time_tolerance: 0.0005
+type: ProcessorImu
+unmeasured_perturbation_std: 1.0e-06

demos/yaml/realsense_imu.yaml

+type: "SensorImu"             # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+name: "SenImu"        # This is ignored. The name provided to the SensorFactory prevails
+motion_variances: 
+   a_noise:                0.00017     # standard deviation of Acceleration noise (same for all the axis) in m/s2
+   w_noise:                0.003    # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+   ab_initial_stdev:       0.002     # m/s2    - initial bias 
+   wb_initial_stdev:       0.017     # rad/sec - initial bias 
+   ab_rate_stdev:          0.0001       # m/s2/sqrt(s)           
+   wb_rate_stdev:          0.0001    # rad/s/sqrt(s)