Skip to content

Commit

Permalink
evo stuff
Browse files Browse the repository at this point in the history
  • Loading branch information
raphaelchang committed Oct 10, 2019
1 parent 444a1c6 commit 2bf8e1b
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 0 deletions.
44 changes: 44 additions & 0 deletions scripts/misc/pose_hdf5_to_bag.py
Original file line number Diff line number Diff line change
@@ -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()
19 changes: 19 additions & 0 deletions src/ros/odometry_eval.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
2 changes: 2 additions & 0 deletions src/ros/slam_eval.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 2bf8e1b

Please sign in to comment.