diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt index a53c11a9d9605976e48e460a5a556d72b07d4b7b..93d17fbe0befb99c22622c7f48f5fd67511467df 100644 --- a/demos/CMakeLists.txt +++ b/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} +) + diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp index 1fd581695e4822c2f8e8fe188680a8e4f51706b5..0b67d3e7cfdaf9ac73eb72a033e1da745b819459 100644 --- a/demos/cosyslam.cpp +++ b/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); //////////////////////////// diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a0d76e2d299eeea83fcb9e41daa7d80466d17501 --- /dev/null +++ b/demos/cosyslam_imu.cpp @@ -0,0 +1,286 @@ +#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 diff --git a/demos/yaml/cosyslam_imu.yaml b/demos/yaml/cosyslam_imu.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7ff2f0ce3bb909749fa29383c543cef260c155e6 --- /dev/null +++ b/demos/yaml/cosyslam_imu.yaml @@ -0,0 +1,7 @@ +# 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 diff --git a/demos/yaml/processor_imu.yaml b/demos/yaml/processor_imu.yaml new file mode 100644 index 0000000000000000000000000000000000000000..f92b74c06167c96ef4294e00600c7270bd0d20e2 --- /dev/null +++ b/demos/yaml/processor_imu.yaml @@ -0,0 +1,10 @@ +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 diff --git a/demos/processor_tracker_landmark_object.yaml b/demos/yaml/processor_tracker_landmark_object.yaml similarity index 100% rename from demos/processor_tracker_landmark_object.yaml rename to demos/yaml/processor_tracker_landmark_object.yaml diff --git a/demos/yaml/realsense_imu.yaml b/demos/yaml/realsense_imu.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3525e4a4108fb7a098cd557b127ee59805b20f35 --- /dev/null +++ b/demos/yaml/realsense_imu.yaml @@ -0,0 +1,10 @@ +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)