pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v2

Browse Source
This commit is contained in:
Иван Подмогильный
2022-08-07 15:19:46 +03:00
parent 0909f2ee11
commit 541e52a2c2
3 changed files with 85 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
ov_eval/src/live_align_trajectory.cpp
View File

@@ -20,6 +20,7 @@
*/
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
@@ -30,7 +31,7 @@
#include "utils/print.h"
#include "utils/quat_ops.h"
ros::Publisher pub_path;
ros::Publisher pub_path, pub_T_align;
void align_and_publish(const nav_msgs::Path::ConstPtr &msg);
std::vector<double> times_gt;
std::vector<Eigen::Matrix<double, 7, 1>> poses_gt;
@@ -71,6 +72,7 @@ int main(int argc, char **argv) {
// Our subscribe and publish nodes
ros::Subscriber sub = nh.subscribe("/ov_msckf/pathimu", 1, align_and_publish);
pub_path = nh.advertise<nav_msgs::Path>("/ov_msckf/pathgt", 2);
pub_T_align = nh.advertise<geometry_msgs::TransformStamped>("/ov_msckf/T_align", 2);
ROS_INFO("Subscribing: %s", sub.getTopic().c_str());
ROS_INFO("Publishing: %s", pub_path.getTopic().c_str());
@@ -114,9 +116,43 @@ void align_and_publish(const nav_msgs::Path::ConstPtr &msg) {
// Perform alignment of the trajectories
Eigen::Matrix3d R_ESTtoGT;
Eigen::Vector3d t_ESTinGT;
// scale.
double s_ESTtoGT;
ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_type);
// Added by PI
Eigen::Matrix4d T_ESTinGT;
T_ESTinGT <<
s_ESTtoGT * R_ESTtoGT(0, 0), R_ESTtoGT(0, 1), R_ESTtoGT(0, 2), t_ESTinGT(0),
R_ESTtoGT(1, 0), s_ESTtoGT * R_ESTtoGT(1, 1), R_ESTtoGT(1, 2), t_ESTinGT(1),
R_ESTtoGT(2, 0), R_ESTtoGT(1, 0), s_ESTtoGT * R_ESTtoGT(1, 0), t_ESTinGT(2),
0, 0, 0, 1;
// Every time align_and_publish is called, we will publish the values R and t.
// Assuming that the current timestamp is the timestamp of the last pose in the queue.
geometry_msgs::TransformStamped transformStamped;
geometry_msgs::Transform transform;
geometry_msgs::Quaternion q_to_transform;
geometry_msgs::Vector3 v_to_transform;
// End of added by PI
Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT);
q_to_transform.x = q_ESTtoGT(0);
q_to_transform.y = q_ESTtoGT(1);
q_to_transform.z = q_ESTtoGT(2);
q_to_transform.w = q_ESTtoGT(3);
v_to_transform.x = t_ESTinGT(0);
v_to_transform.x = t_ESTinGT(1);
v_to_transform.x = t_ESTinGT(2);
transform.rotation = q_to_transform;
transform.translation = v_to_transform;
transformStamped.transform = transform;
transformStamped.header = msg->poses[msg->poses.size()-1].header;
pub_T_align.publish(transformStamped);
PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);