From 6e74c91dd7082685dff3111aee2ffdc2245deeed Mon Sep 17 00:00:00 2001
From: Cesar Debeunne <cdebeunne@nilgiri.laas.fr>
Date: Fri, 1 Oct 2021 15:15:19 +0200
Subject: [PATCH] final commit
---
demos/cosyslam.cpp | 55 +++++++++++++++----
demos/cosyslam_imu.cpp | 17 +++---
demos/yaml/cosyslam_imu.yaml | 8 +--
.../processor_tracker_landmark_object.yaml | 14 +++--
src/capture/capture_object.cpp | 4 ++
yaml_error/obj_000001.yaml | 9 +++
6 files changed, 77 insertions(+), 30 deletions(-)
create mode 100644 yaml_error/obj_000001.yaml
diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index 9c8702b..fcbd509 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -22,6 +22,7 @@
#include <wolf_ros_objectslam/CosyObjectArray.h>
#include <wolf_ros_objectslam/CosyObject.h>
#include <std_msgs/Header.h>
+#include <sensor_msgs/Imu.h>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
@@ -68,20 +69,21 @@ int main()
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);
- VectorComposite x0("PO", {Vector3d(0, 0, 0), Quaterniond::Identity().coeffs()});
- VectorComposite sig0("PO", {Vector3d(1, 1, 1), Vector3d(1, 1, 1)});
-
- ObjectDetections dets;
-
- ///////////////
- // SLAM loop //
- ///////////////
-
+ // Rosbag topics target
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());
+ int frame_id = 0;
+
+ ///////////////
+ // SLAM loop //
+ ///////////////
+
+ ObjectDetections dets;
+ VectorComposite x0("PO", {Vector3d::Zero(), Quaterniond::Identity().coeffs()});
+ VectorComposite sig0("PO", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1});
FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
// Variables init
@@ -90,7 +92,14 @@ int main()
Vector7d poseVec;
int number_of_kf = 0;
int counter = 0;
- int frame_id = 0;
+
+ // test map live
+ std::fstream file_init_world;
+ std::string init_world_filename = wolf_root + "/demos/init_world.csv";
+ file_init_world.open(init_world_filename, std::fstream::out);
+ std::string header_init_world = "id,t,object_type,px,py,pz,qx,qy,qz,qw,\n";
+ file_init_world << header_init_world;
+ std::vector<int> kf_id;
foreach(rosbag::MessageInstance const m, view)
@@ -124,7 +133,27 @@ int main()
number_of_kf = problem->getTrajectory()->getFrameMap().size();
if (number_of_kf > counter){
std::string report = solver->solve(SolverCeres::ReportVerbosity::FULL);
+ kf_id.push_back(frame_id);
counter++;
+ // save the map
+ VectorComposite state_lmk;
+ for (auto& lmk: problem->getMap()->getLandmarkList()){
+ auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
+ // World to lmk
+ state_lmk = lmk_obj->getState();
+
+ file_init_world << counter << ","
+ << problem->getTimeStamp() << ","
+ << lmk_obj->getObjectType() << ","
+ << state_lmk['P'](0) << ","
+ << state_lmk['P'](1) << ","
+ << state_lmk['P'](2) << ","
+ << state_lmk['O'](0) << ","
+ << state_lmk['O'](1) << ","
+ << state_lmk['O'](2) << ","
+ << state_lmk['O'](3) << "\n";
+
+ }
}
frame_id ++;
}
@@ -140,7 +169,7 @@ int main()
problem->print(4,true,true,true);
- // Save the results
+ /// Save the results
std::fstream file_res;
std::fstream file_map;
std::fstream file_init;
@@ -202,7 +231,7 @@ int main()
Vector3d image_coord = intrinsic * c_t_lmk;
image_coord = image_coord/image_coord(2);
- file_init << 3*counter << ","
+ file_init << kf_id[counter] << ","
<< t << ","
<< lmk_obj->getObjectType() << ","
<< c_M_lmk.matrix().row(0) << " "
@@ -237,6 +266,8 @@ int main()
file_res.close();
file_map.close();
file_init.close();
+ file_init_world.close();
+
return 0;
}
\ No newline at end of file
diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp
index ee95329..3bacab8 100644
--- a/demos/cosyslam_imu.cpp
+++ b/demos/cosyslam_imu.cpp
@@ -156,13 +156,14 @@ int main()
frame_id = 0;
VectorComposite x0("POV", { Vector3d::Zero(), q_init, Vector3d::Zero()});
- VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.05});
+ VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.01});
FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
foreach(rosbag::MessageInstance const m, view)
{
std::cout << "--------FRAME " << frame_id << "-------------" << std::endl;
-
+ TimeStamp ts(m.getTime().toSec());
+
wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>();
if (cosyArray != nullptr){
for (auto object : cosyArray->objects){
@@ -180,7 +181,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_obj = std::make_shared<CaptureObject>(ts - init_ts, sen, dets);
cap_obj->process();
dets.clear();
@@ -196,7 +196,6 @@ int main()
cosyIMU->angular_velocity.y,
cosyIMU->angular_velocity.z;
Matrix6d acc_gyr_cov = sen_imu->getNoiseCov();
- TimeStamp ts(m.getTime().toSec());
CaptureImuPtr cap_imu = std::make_shared<CaptureImu>(ts - init_ts, sen_imu, acc_gyr_meas, acc_gyr_cov);
cap_imu->process();
}
@@ -258,14 +257,12 @@ int main()
<< 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);
@@ -284,7 +281,7 @@ int main()
Vector3d image_coord = intrinsic * c_t_lmk;
image_coord = image_coord/image_coord(2);
- file_init << 3*counter << ","
+ file_init << counter << ","
<< t << ","
<< lmk_obj->getObjectType() << ","
<< c_M_lmk.matrix().row(0) << " "
@@ -295,8 +292,8 @@ int main()
<< image_coord(1) << " "
<< "\n";
- good_lmk++;
}
+
counter++;
}
diff --git a/demos/yaml/cosyslam_imu.yaml b/demos/yaml/cosyslam_imu.yaml
index aa4da67..7f76fea 100644
--- a/demos/yaml/cosyslam_imu.yaml
+++ b/demos/yaml/cosyslam_imu.yaml
@@ -3,7 +3,7 @@ bag: "/demos/bag/demo_imu_wolf.bag"
# Camera to IMU transformation
unfix_extrinsic: false
-i_p_c: [-0.0111433, 0.00260534, 0.00542655]
-i_q_c: [ 0.01424561, -0.00273651, 0.00528824, 0.9998808]
-i_q_w: [ 0.81615748, 0.38330015, -0.14646648, -0.40683601] # for short IMU
-# i_q_w: [ 0.86842613, -0.01545895, -0.0271302, -0.49483434] # for long IMU
+i_p_c: [-0.0111433, 0.00260534, 0.00542655] # L515
+#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 85e125d..97e4abf 100644
--- a/demos/yaml/processor_tracker_landmark_object.yaml
+++ b/demos/yaml/processor_tracker_landmark_object.yaml
@@ -9,11 +9,17 @@ vote:
min_features_for_keyframe: 1
nb_vote_for_every_first: 0
+# intrinsic:
+# cx: 638.7251222
+# cy: 332.2076531
+# fx: 951.15435791
+# fy: 951.40795898
+
intrinsic:
- cx: 638.7251222
- cy: 332.2076531
- fx: 951.15435791
- fy: 951.40795898
+ cx: 608.80518282
+ cy: 353.95008807
+ fx: 934.69763184
+ fy: 944.0513916
lmk_score_thr: 4.0
e_pos_thr: 0.1
diff --git a/src/capture/capture_object.cpp b/src/capture/capture_object.cpp
index 706cd01..6c370c1 100644
--- a/src/capture/capture_object.cpp
+++ b/src/capture/capture_object.cpp
@@ -61,6 +61,10 @@ Matrix6d polynomial_covariance(ParamsCapturePtr params, ObjectDetection det)
// a Lambda to compute error from coeficients
auto error = [] (std::vector<double> coefs, double r, double theta, double phi, double s)
{
+ // case of degree 0 polynomial
+ if (coefs.size() == 1){
+ return coefs.at(0);
+ }
// case of degree one polynomial
if (coefs.size() == 6){
return coefs.at(0) + coefs.at(1)*r + coefs.at(2)*theta + coefs.at(3)*phi + coefs.at(4)*s + coefs.at(5);
diff --git a/yaml_error/obj_000001.yaml b/yaml_error/obj_000001.yaml
new file mode 100644
index 0000000..d039a07
--- /dev/null
+++ b/yaml_error/obj_000001.yaml
@@ -0,0 +1,9 @@
+type: "Capture"
+
+object_name: "obj_000001"
+coefs_err0: [0.05]
+coefs_err1: [0.05]
+coefs_err2: [0.05]
+coefs_err3: [0.1]
+coefs_err4: [0.1]
+coefs_err5: [0.1]
\ No newline at end of file
--
GitLab