bug fix

README.md

@@ -2,3 +2,7 @@ WOLF - Windowed Localization Frames | objectslam Plugin
 ===================================
 
 For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
+
+To compile this plugin without failure, you need to delete /demos
+
+If you want to compile the demos, you need to install wolf_ros_objectslam and to source ros and its workspace. 
\ No newline at end of file

demos/CMakeLists.txt

 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
+)
 FIND_PACKAGE(wolfimu REQUIRED)
 
 INCLUDE_DIRECTORIES(
+    ${rosbag_INCLUDE_DIRS}
+    ${wolf_ros_objectslam_INCLUDE_DIRS}
+    ${std_msgs_INCLUDE_DIRS}
     ${wolfimu_INCLUDE_DIRS}
 )
 
@@ -13,22 +29,20 @@ ADD_EXECUTABLE(demo_toy_pbe demo_toy_pbe.cpp)
 TARGET_LINK_LIBRARIES(demo_toy_pbe 
     ${wolfcore_LIBRARIES}
     ${PLUGIN_NAME}
-    # ${catkin_LIBRARIES}
+    ${catkin_LIBRARIES}
 )
 
-# ADD_EXECUTABLE(cosyslam cosyslam.cpp)
-# TARGET_LINK_LIBRARIES(cosyslam 
-#     ${wolfcore_LIBRARIES}
-#     ${PLUGIN_NAME}
-#     ${catkin_LIBRARIES}
-# )
-
-# ADD_EXECUTABLE(cosyslam_imu cosyslam_imu.cpp)
-# TARGET_LINK_LIBRARIES(cosyslam_imu 
-#     ${wolfcore_LIBRARIES}
-#     ${PLUGIN_NAME}
-#     ${catkin_LIBRARIES}
-#     ${wolfimu_LIBRARIES}
-# )
-
+ADD_EXECUTABLE(cosyslam cosyslam.cpp)
+TARGET_LINK_LIBRARIES(cosyslam 
+    ${wolfcore_LIBRARIES}
+    ${PLUGIN_NAME}
+    ${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

@@ -19,6 +19,7 @@
 
 #include <rosbag/bag.h>
 #include <rosbag/view.h>
+#include "geometry_msgs/PoseStamped.h"
 #include <wolf_ros_objectslam/CosyObjectArray.h>
 #include <wolf_ros_objectslam/CosyObject.h>
 #include <std_msgs/Header.h>
@@ -104,6 +105,8 @@ int main()
 
     foreach(rosbag::MessageInstance const m, view)
     {
+        TimeStamp ts(m.getTime().toSec());
+        if ((ts-init_ts) > 43.5) break;
         std::cout << "--------FRAME " << frame_id << "-------------" << std::endl;
 
         wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>();
@@ -123,7 +126,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 = std::make_shared<CaptureObject>(ts-init_ts, sen, dets);
             cap->process();
             dets.clear();
@@ -188,6 +190,10 @@ int main()
     std::string header_init = "id,ts,object_id,pose,image_coord,\n";
     file_init << header_init;
 
+    // Rosbag opening
+    rosbag::Bag bag_results;
+    bag_results.open("results.bag", rosbag::bagmode::Write);
+
     VectorComposite state_est;
     VectorComposite state_lmk;
     CaptureBasePtr cap_cosy;
@@ -206,6 +212,21 @@ int main()
                  << state_est['O'](2) << "," 
                  << state_est['O'](3) << "\n";
 
+
+        // write in bag
+        
+        geometry_msgs::PoseStamped pose;
+        pose.header.stamp.fromSec(t.get()+init_ts.get());
+        pose.header.frame_id = "cosy";
+        pose.pose.position.x = state_est['P'](0);
+        pose.pose.position.y = state_est['P'](1);
+        pose.pose.position.z = state_est['P'](2);
+        pose.pose.orientation.x = state_est['O'](0);
+        pose.pose.orientation.y = state_est['O'](1);
+        pose.pose.orientation.z = state_est['O'](2);
+        pose.pose.orientation.w = state_est['O'](3);
+        bag_results.write("/robot_pose", pose.header.stamp, pose);
+
         // World to robot
         Vector7d pose_rob;
         pose_rob << state_est['P'], state_est['O']; 
@@ -246,6 +267,7 @@ int main()
         }
         counter++; 
     }
+    bag_results.close();
 
     counter = 0;
     for (auto& elt: problem->getMap()->getLandmarkList()){

demos/cosyslam_imu.cpp

@@ -5,6 +5,7 @@
 #include <utility>
 #include <string>
 #include <Eigen/Dense>
+#include <random>
 #include <yaml-cpp/yaml.h>
 
 #include "core/problem/problem.h"
@@ -31,12 +32,20 @@
 #include <wolf_ros_objectslam/CosyObject.h>
 #include <sensor_msgs/Imu.h>
 #include <std_msgs/Header.h>
+#include "geometry_msgs/PoseStamped.h"
 #include <boost/foreach.hpp>
 #define foreach BOOST_FOREACH
 
 using namespace wolf;
 using namespace Eigen;
 
+float get_random()
+{
+    static std::default_random_engine e;
+    static std::uniform_real_distribution<> dis(0, 1); // range 0 - 1
+    return dis(e);
+}
+
 Isometry3d posevec_to_isometry(Vector7d pose)
 {
     return Translation<double, 3>(pose.head<3>()) * Quaterniond(pose.tail<4>());
@@ -109,7 +118,7 @@ int main()
     // Initial orientation with IMU//
     /////////////////////////////////
 
-    Vector3d acc_mean;
+    Vector3d acc_mean(0.0,0.0,0.0);
     int N_iter = 10;
     foreach(rosbag::MessageInstance const m, view)
     {
@@ -154,6 +163,7 @@ int main()
     int number_of_kf = 0;
     int counter = 0;
     frame_id = 0;
+    float prob = 1.0;
 
     VectorComposite x0("POV", { Vector3d::Zero(), q_init, Vector3d::Zero()});
     VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.01});
@@ -167,9 +177,14 @@ int main()
         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,
+
+                // robustness test : remove feature with prob p
+                float random_float = get_random();
+                if (random_float > prob) continue;
+                
+                poseVec << object.pose.position.x*1.13,
+                    object.pose.position.y*1.13,
+                    object.pose.position.z*1.13,
                     object.pose.orientation.x,
                     object.pose.orientation.y,
                     object.pose.orientation.z,
@@ -231,6 +246,10 @@ int main()
     file_res.open(res_filename, std::fstream::out);
     file_map.open(map_filename, std::fstream::out);
 
+    // Rosbag opening
+    rosbag::Bag bag_results;
+    bag_results.open("results.bag", rosbag::bagmode::Write);
+
     // Write headers for csv files
     std::string header_res = "id,t,px,py,pz,qx,qy,qz,qw,\n";
     file_res << header_res;
@@ -257,6 +276,20 @@ int main()
                  << state_est['O'](1) << "," 
                  << state_est['O'](2) << "," 
                  << state_est['O'](3) << "\n";
+
+        // write in bag
+        
+        geometry_msgs::PoseStamped pose;
+        pose.header.stamp.fromSec(t.get()+init_ts.get());
+        pose.header.frame_id = "cosy";
+        pose.pose.position.x = state_est['P'](0);
+        pose.pose.position.y = state_est['P'](1);
+        pose.pose.position.z = state_est['P'](2);
+        pose.pose.orientation.x = state_est['O'](0);
+        pose.pose.orientation.y = state_est['O'](1);
+        pose.pose.orientation.z = state_est['O'](2);
+        pose.pose.orientation.w = state_est['O'](3);
+        bag_results.write("/robot_pose", pose.header.stamp, pose);
         
         // World to robot
         Vector7d pose_rob;
@@ -296,6 +329,7 @@ int main()
 
         counter++; 
     }
+    bag_results.close();
 
     counter = 0;
     for (auto& elt: problem->getMap()->getLandmarkList()){
@@ -318,4 +352,4 @@ int main()
     file_init.close();
 
     return 0;
-}
\ No newline at end of file
+}

demos/yaml/cosyslam_imu.yaml

 # rosbag name
-bag: "/demos/bag/demo_imu_wolf.bag"
+bag: "/demos/bag/tless_circular_cp.bag"
 
 # Camera to IMU transformation
 unfix_extrinsic: false
 i_p_c: [-0.0111433, 0.00260534, 0.00542655] # L515
-#i_p_c:  [-0.02037261, 0.00497111, 0.011167] # D435
+# 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

@@ -5,7 +5,7 @@ time_tolerance: 0.1222
 vote:
     voting_active:              true
     min_time_vote:              0 # s
-    max_time_vote:              0.1 # s
+    max_time_vote:              0.05 # s
     min_features_for_keyframe:  1
     nb_vote_for_every_first:    0
 
@@ -21,11 +21,11 @@ intrinsic:
     fx:                         934.69763184
     fy:                         944.0513916
 
-lmk_score_thr: 4.0
+lmk_score_thr: 0.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
 max_new_features: -1
-apply_loss_function: true
\ No newline at end of file
+apply_loss_function: true

src/capture/capture_object.cpp

@@ -10,7 +10,7 @@ static ParamsCapturePtr createParamsCapture(const std::string & _filename_dot_ya
     try{
         config = YAML::LoadFile(_filename_dot_yaml);
     }
-    catch(YAML::BadFile) {
+    catch(YAML::BadFile const&) {
         std::cout << "Unknown Object" << std::endl;
         return nullptr;
     }