parsing with rosbags

.gitignore

@@ -25,6 +25,7 @@ src/examples/map_polyline_example_write.yaml
 /Default/
 
 *.csv
+*.bag
 *.csv#
 
 results_*

demos/CMakeLists.txt

-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
+INCLUDE_DIRECTORIES(
+    ${CMAKE_CURRENT_SOURCE_DIR}
+    ${catkin_INCLUDE_DIRS}
+)
+
+FIND_PACKAGE(catkin REQUIRED COMPONENTS
+  rosbag
+  rosconsole
+  roscpp
+  roslib
+  sensor_msgs
+  std_msgs
+  geometry_msgs
+  tf
+  wolf_ros_objectslam
+  wolf_ros_node
+)
+
+INCLUDE_DIRECTORIES(
+    ${rosbag_INCLUDE_DIRS}
+    ${wolf_ros_objectslam_INCLUDE_DIRS}
+    ${std_msgs_INCLUDE_DIRS}
+)
+
 
 ADD_EXECUTABLE(demo_toy_pbe demo_toy_pbe.cpp)
 TARGET_LINK_LIBRARIES(demo_toy_pbe 
     ${wolfcore_LIBRARIES}
     ${PLUGIN_NAME}
+    ${catkin_LIBRARIES}
 )
 
 ADD_EXECUTABLE(cosyslam cosyslam.cpp)
 TARGET_LINK_LIBRARIES(cosyslam 
     ${wolfcore_LIBRARIES}
     ${PLUGIN_NAME}
+    ${catkin_LIBRARIES}
 )
 
 

demos/cosyslam.cpp

@@ -16,6 +16,14 @@
 #include "objectslam/internal/config.h"
 #include "objectslam/landmark/landmark_object.h"
 
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <wolf_ros_objectslam/CosyObjectArray.h>
+#include <wolf_ros_objectslam/CosyObject.h>
+#include <std_msgs/Header.h>
+#include <boost/foreach.hpp>
+#define foreach BOOST_FOREACH
+
 using namespace wolf;
 using namespace Eigen;
 
@@ -31,97 +39,13 @@ Vector7d isometry_to_posevec(Isometry3d M)
     return pose_vec;
 }
 
-std::vector<std::pair<std::string, std::vector<std::string>>> read_csv(std::string filename)
-{
-    // create an input filestream
-    std::ifstream myFile(filename);
-
-    // Create a vector of <string, int vector> pairs to store the result
-    std::vector<std::pair<std::string, std::vector<std::string>>> result;
-
-    // Make sure the file is open
-    if (!myFile.is_open())
-        throw std::runtime_error("Could not open file");
-
-    // Variables
-    std::string line, colname, val;
-
-    // read the columns name
-    if (myFile.good())
-    {
-
-        // extract the first line in the file
-        std::getline(myFile, line);
-
-        // Create a stringstream from line
-        std::stringstream ss(line);
-
-        // Extract each column name
-        while (std::getline(ss, colname, ','))
-        {
-
-            // Initialize and add <colname, string vector> pairs to result
-            result.push_back({colname, std::vector<std::string>{}});
-        }
-    }
-
-    // Read data, line by line
-    while (std::getline(myFile, line))
-    {
-        // Create a stringstream of the current line
-        std::stringstream ss(line);
-
-        // Keep track of the current column index
-        int colIdx = 0;
-
-        // Extract each string and store it in result
-        while (std::getline(ss, val, ','))
-        {
-            result.at(colIdx).second.push_back(val);
-            colIdx++;
-        }
-    }
-
-    // Close file
-    myFile.close();
-
-    return result;
-}
-
-Isometry3d str_to_isometry(std::string pose)
-{
-    // Let's initialize the eigen objects
-    Matrix4d pose_matrix;
-
-    // Parse the string
-    std::string::size_type sz;
-    for (int i = 0; i < 4; i++)
-    {
-        for (int j = 0; j < 4; j++)
-        {
-            pose_matrix(i, j) = std::stod(pose, &sz);
-            pose = pose.substr(sz);
-        }
-    }
-
-    // Create the transformation
-    Isometry3d trans;
-    trans.linear() = pose_matrix.topLeftCorner(3, 3);
-    trans.translation() = pose_matrix.topRightCorner(3, 1);
-
-    return trans;
-}
-
 int main()
 {
-    /////////////////////////
-    // read the input data //
-    /////////////////////////
-
+    // Rosbag opening
+    rosbag::Bag bag;
     std::string wolf_root = _WOLF_OBJECTSLAM_ROOT_DIR;
-    std::string filename = wolf_root + "/demos/input_demoMulti.csv";
-    std::vector<std::pair<std::string, std::vector<std::string>>> csv_values;
-    csv_values = read_csv(filename);
+    std::string bagpath = wolf_root + "/demos/demoMulti.bag";
+    bag.open(bagpath, rosbag::bagmode::Read);
 
     ////////////////////////////
     // setup the wolf problem //
@@ -138,61 +62,55 @@ int main()
 
     VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
     VectorComposite sig0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
-    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
 
     ObjectDetections dets;
 
-    // Parsing variables
-    int len = csv_values.at(0).second.size();
-    int idx_pose = 4;
-    int idx_obj = 3;
-    int idx_t = 9;
-    int idx_s = 6;
-
     ///////////////
     // SLAM loop //
     ///////////////
 
-    double t_cur = 0;
+    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());
+    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;
+
 
-    for (int i = 0; i < len; i++)
+    foreach(rosbag::MessageInstance const m, view)
     {
-        std::cout << i << std::endl;
-
-        // Measurement data
-        Isometry3d c_M_o = str_to_isometry(csv_values.at(idx_pose).second.at(i));
-        Matrix6d cov;
-        Vector6d sig;
-        // default covariance value : the real value is computed in the processor
-        sig << 0.02, 0.02, 0.02, 0.1, 0.1, 0.1;
-        cov = sig.array().matrix().asDiagonal();
-        std::string object_name;
-        object_name = csv_values.at(idx_obj).second.at(i);
-        double t;
-        t = std::stod(csv_values.at(idx_t).second.at(i));
-        double s;
-        s = std::stod(csv_values.at(idx_s).second.at(i));
-        ObjectDetection det = {.measurement = isometry_to_posevec(c_M_o), .detection_score = s, .meas_cov = cov, .object_type = object_name};
-
-        if (t == t_cur){
-            // Capture on the current frame
-            dets.push_back(det);
-        } else if (i == len-1){
-            // Capture on the last frame
-            dets.push_back(det);
-            CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets);
+        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 = std::make_shared<CaptureObject>(ts, sen, dets);
             cap->process();
-        } else {
-            // Capture on a new frame
-            CaptureObjectPtr cap = std::make_shared<CaptureObject>(t_cur, sen, dets);
-            cap->process();
-
             dets.clear();
-            dets.push_back(det);
         }
-        t_cur = t;
 
         // Solve if a KF was voted
         number_of_kf = problem->getTrajectory()->getFrameMap().size();
@@ -200,8 +118,15 @@ int main()
             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;
 
@@ -223,7 +148,7 @@ int main()
     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,\n";
+    std::string header_init = "id,ts,object_id,pose,image_coord,\n";
     file_init << header_init;
 
     VectorComposite state_est;
@@ -254,25 +179,32 @@ int main()
         for (auto& lmk: problem->getMap()->getLandmarkList()){
             auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
 
-            if (good_lmk == 0 || good_lmk == 1 || good_lmk == 2){
-                // World to lmk
-                state_lmk = lmk->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;
-                
-                file_init << 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) << " "
-                        << "\n";
-                }
+            // 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++; 

src/processor/processor_tracker_landmark_object.cpp

@@ -214,7 +214,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.08 && e_rot < e_rot_thr_+0.08){
+                if (e_pos < e_pos_thr_+0.1 && e_rot < e_rot_thr_+0.1){
                     feature_already_found = true;
                     break;   
                 }      
@@ -223,12 +223,13 @@ 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);
 
             // link feature (they are created unlinked in preprocess())
             feat->link(_capture);
-
+            break;
         }
     }