From 2bf8e1bd0f43db98e0ca2dc9583e639106309d36 Mon Sep 17 00:00:00 2001
From: raphaelchang <raphaelchang1227@gmail.com>
Date: Thu, 10 Oct 2019 01:54:04 -0400
Subject: [PATCH] evo stuff

---
 scripts/misc/pose_hdf5_to_bag.py | 44 ++++++++++++++++++++++++++++++++
 src/ros/odometry_eval.cc         | 19 ++++++++++++++
 src/ros/slam_eval.cc             |  2 ++
 3 files changed, 65 insertions(+)
 create mode 100644 scripts/misc/pose_hdf5_to_bag.py

diff --git a/scripts/misc/pose_hdf5_to_bag.py b/scripts/misc/pose_hdf5_to_bag.py
new file mode 100644
index 0000000..bd45ef1
--- /dev/null
+++ b/scripts/misc/pose_hdf5_to_bag.py
@@ -0,0 +1,44 @@
+import h5py
+import argparse
+import rosbag
+from geometry_msgs.msg import PoseStamped
+
+parser = argparse.ArgumentParser(description='hdf5 odometry results file to bag')
+parser.add_argument('results_path', help='odometry results file')
+parser.add_argument('output_file', help='output bag file')
+args = parser.parse_args()
+
+with h5py.File(args.results_path, 'r') as f:
+    estimated = f['estimated_poses'][:]
+    ground_truth = f['ground_truth_poses'][:]
+    attrs = dict(f['attributes'].attrs.items())
+
+bag = rosbag.Bag(args.output_file, 'w')
+
+seq = 0
+for estrow, gndrow in zip(estimated, ground_truth):
+    estp = PoseStamped()
+    estp.header.seq = seq
+    estp.header.stamp.secs = seq
+    estp.pose.position.x = estrow[0]
+    estp.pose.position.y = estrow[1]
+    estp.pose.position.z = estrow[2]
+    estp.pose.orientation.x = estrow[3]
+    estp.pose.orientation.y = estrow[4]
+    estp.pose.orientation.z = estrow[5]
+    estp.pose.orientation.w = estrow[6]
+    gndp = PoseStamped()
+    gndp.header.seq = seq
+    gndp.header.stamp.secs = seq
+    gndp.pose.position.x = gndrow[0]
+    gndp.pose.position.y = gndrow[1]
+    gndp.pose.position.z = gndrow[2]
+    gndp.pose.orientation.x = gndrow[3]
+    gndp.pose.orientation.y = gndrow[4]
+    gndp.pose.orientation.z = gndrow[5]
+    gndp.pose.orientation.w = gndrow[6]
+    bag.write('/omni_slam/odometry', estp)
+    bag.write('/omni_slam/odometry_truth', gndp)
+    seq += 1
+
+bag.close()
diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc
index dd6f7c0..34ba0cd 100644
--- a/src/ros/odometry_eval.cc
+++ b/src/ros/odometry_eval.cc
@@ -113,6 +113,25 @@ template <bool Stereo>
 void OdometryEval<Stereo>::GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data)
 {
     module::OdometryModule::Stats &stats = odometryModule_->GetStats();
+    bool first = true;
+    for (const std::unique_ptr<data::Frame> &frame : this->trackingModule_->GetFrames())
+    {
+        if (frame->HasEstimatedPose() || first)
+        {
+            const Matrix<double, 3, 4> &pose = (first && !frame->HasEstimatedPose()) ? frame->GetPose() : frame->GetEstimatedPose();
+            Quaterniond quat(pose.block<3, 3>(0, 0));
+            quat.normalize();
+            data["estimated_poses"].emplace_back(std::vector<double>{pose(0, 3), pose(1, 3), pose(2, 3), quat.x(), quat.y(), quat.z(), quat.w()});
+        }
+        if (frame->HasPose())
+        {
+            const Matrix<double, 3, 4> &pose = frame->GetPose();
+            Quaterniond quat(pose.block<3, 3>(0, 0));
+            quat.normalize();
+            data["ground_truth_poses"].emplace_back(std::vector<double>{pose(0, 3), pose(1, 3), pose(2, 3), quat.x(), quat.y(), quat.z(), quat.w()});
+        }
+        first = false;
+    }
 }
 
 template <bool Stereo>
diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc
index 01177e7..c1a6e76 100644
--- a/src/ros/slam_eval.cc
+++ b/src/ros/slam_eval.cc
@@ -60,6 +60,8 @@ void SLAMEval::Finish()
 
 void SLAMEval::GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data)
 {
+    OdometryEval<true>::GetResultsData(data);
+    ReconstructionEval<true>::GetResultsData(data);
 }
 
 void SLAMEval::Visualize(cv_bridge::CvImagePtr &base_img)