From 129ed8c4c8bb4adc41f87c46860e05ee58893169 Mon Sep 17 00:00:00 2001
From: Cesar Debeunne <cdebeunne@nilgiri.laas.fr>
Date: Fri, 6 Aug 2021 17:31:36 +0200
Subject: [PATCH] parsing with rosbags
---
.gitignore | 1 +
demos/CMakeLists.txt | 27 +-
demos/cosyslam.cpp | 230 ++++++------------
.../processor_tracker_landmark_object.cpp | 5 +-
4 files changed, 111 insertions(+), 152 deletions(-)
diff --git a/.gitignore b/.gitignore
index 234d870..c8958c5 100644
--- a/.gitignore
+++ b/.gitignore
@@ -25,6 +25,7 @@ src/examples/map_polyline_example_write.yaml
/Default/
*.csv
+*.bag
*.csv#
results_*
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 6620c03..a53c11a 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,15 +1,40 @@
-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}
)
diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index a8379b7..1fd5816 100644
--- a/demos/cosyslam.cpp
+++ b/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++;
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 9cd9347..a5c33ed 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/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;
}
}
--
GitLab