diff --git a/README.md b/README.md index 9e9a8d1b3822f169077e3e0bba80b79ad0bcbce0..a14eae8ab3591d72f6dfe43160678e65a7d5b2cd 100644 --- a/README.md +++ b/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 diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index 191653f92527669418d058a7c2a7f6fa399d0b73..2cb70d53b86994f5359fa8671f0c8bba36bfdeb0 100644 --- a/demos/CMakeLists.txt +++ b/demos/CMakeLists.txt @@ -1,10 +1,26 @@ 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} +) diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp index 2a4de756d05d8672edeb08ba534a7fdece72b00c..1323550844357c30df9feb4ae3f72afe6f93903e 100644 --- a/demos/cosyslam.cpp +++ b/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()){ diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp index d783e93fe6c5be14199800e865f87706b848111d..9026550c6f900a238166d34460235dd588a406b6 100644 --- a/demos/cosyslam_imu.cpp +++ b/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 +} diff --git a/demos/yaml/cosyslam_imu.yaml b/demos/yaml/cosyslam_imu.yaml index 7f76fea2aa4fb3faad011ec725e179dd29b0aa00..dd0ddd2518644fe1adb54e71fc4b8d44f170546c 100644 --- a/demos/yaml/cosyslam_imu.yaml +++ b/demos/yaml/cosyslam_imu.yaml @@ -1,9 +1,9 @@ # 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] diff --git a/demos/yaml/processor_tracker_landmark_object.yaml b/demos/yaml/processor_tracker_landmark_object.yaml index 97e4abf1b58042ca2455b9c440aac8939695f92e..5ab552427e9e72099b2c95c5ef7c2c16ff99af00 100644 --- a/demos/yaml/processor_tracker_landmark_object.yaml +++ b/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 diff --git a/src/capture/capture_object.cpp b/src/capture/capture_object.cpp index 6c370c1328a603c132ebb4789ec0c870294e3eb5..d6a4700612fc92cf18ed02cd8fb050cf1e049979 100644 --- a/src/capture/capture_object.cpp +++ b/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; }