gps fusion implemented
This commit is contained in:
admin1
@@ -1,19 +1,36 @@
|
||||
//
|
||||
// Created by Podmogilnyi Ivan on 17.08.2022.
|
||||
//
|
||||
|
||||
// C++ std library
|
||||
#include <fstream>
|
||||
#include <filesystem>
|
||||
|
||||
// ROS library
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
|
||||
float yCubesVisPos = -2.0;
|
||||
|
||||
// open_vis library
|
||||
#include <utils/quat_ops.h>
|
||||
|
||||
// thirdparty libraries
|
||||
#include <Eigen/Core>
|
||||
|
||||
|
||||
float yCubesVisPos = -0.5;
|
||||
float skew = 5.0;
|
||||
std::vector<std::vector<double>> cubeVisPoints = {
|
||||
{71.215, 32.694, yCubesVisPos+2.5},
|
||||
{71.215, 32.694, yCubesVisPos+2.0},
|
||||
// {81.668, 26.227, yCubesVisPos},
|
||||
{104.061, 36.691, yCubesVisPos+2.0},
|
||||
{104.061, 36.691, yCubesVisPos+1.5},
|
||||
// {90.726, 32.825, yCubesVisPos},
|
||||
{109.541, 60.415, yCubesVisPos+1.5},
|
||||
{109.541, 60.415, yCubesVisPos+1.0},
|
||||
// {149.264, 69.490, yCubesVisPos},
|
||||
{183.605, 91.474, yCubesVisPos+0.5},
|
||||
// {198.294, 97.113, yCubesVisPos},
|
||||
@@ -32,12 +49,248 @@ std::vector<std::vector<double>> cubeVisPoints = {
|
||||
// {1205.432, 681.457, yCubesVisPos},
|
||||
{1184.375, 735.604, yCubesVisPos},
|
||||
// {1232.697, 715.647, yCubesVisPos},
|
||||
{1244.767, 784.257, yCubesVisPos}
|
||||
};
|
||||
{1244.767, 784.257, yCubesVisPos},
|
||||
|
||||
ros::Publisher pub_cubes;
|
||||
ros::Subscriber sub_imu;
|
||||
// CUBES FOR THE GPS ALIGNED TRAJECTORY
|
||||
{-5.473, 15.819, 0.000},
|
||||
|
||||
{-15.586, 25.062, 0.000},
|
||||
|
||||
{-22.395, 43.562, 0.000},
|
||||
|
||||
{-36.035, 50.009, 0.000},
|
||||
|
||||
{-41.113, 62.946, 0.000},
|
||||
|
||||
{-47.675, 62.477, 0.000},
|
||||
|
||||
{-56.081, 76.612, 0.000},
|
||||
|
||||
{-68.155, 82.382, 0.000},
|
||||
|
||||
{-76.903, 97.032, 0.000},
|
||||
|
||||
{-93.218, 103.532, 0.000},
|
||||
|
||||
{-121.903, 137.353, 0.000},
|
||||
|
||||
{-134.567, 140.840, 0.000},
|
||||
|
||||
{-158.030, 169.709, 0.000},
|
||||
|
||||
{-170.521, 177.370, 0.000},
|
||||
|
||||
{-225.440, 236.153, 0.000},
|
||||
|
||||
{-259.810, 259.846, 0.000},
|
||||
|
||||
{-287.220, 297.381, 0.000},
|
||||
|
||||
{-313.876, 312.375, 0.000},
|
||||
|
||||
{-446.905, 450.175, 0.000},
|
||||
|
||||
{-489.056, 477.187, 0.000},
|
||||
|
||||
{-544.828, 555.099, 0.000},
|
||||
|
||||
{-568.349, 556.457, 0.000},
|
||||
|
||||
{-638.075, 643.542, 0.000},
|
||||
|
||||
{-702.377, 676.867, 0.000},
|
||||
|
||||
{-753.507, 750.003, 0.000},
|
||||
|
||||
{-807.197, 768.580, 0.000},
|
||||
|
||||
{-863.780, 829.878, 0.000},
|
||||
|
||||
{-877.002, 860.185, 0.000},
|
||||
|
||||
{-905.568, 864.266, 0.000},
|
||||
|
||||
{-915.407, 887.186, 0.000},
|
||||
|
||||
{-931.954, 899.596, 0.000},
|
||||
|
||||
{-936.091, 907.870, 0.000},
|
||||
|
||||
{-971.656, 952.685, 0.000},
|
||||
|
||||
{-1012.476, 947.723, 0.000},
|
||||
|
||||
|
||||
{-1016.629, 985.880, 0.000},
|
||||
|
||||
{-1065.223, 987.986, 0.000},
|
||||
|
||||
{-1078.723, 1012.979, 0.000},
|
||||
|
||||
{-1145.907, 1035.300, 0.000},
|
||||
|
||||
{-1234.198, 1116.285, 0.000},
|
||||
|
||||
{-1338.651, 1152.282, 0.000},
|
||||
|
||||
{-1422.556, 1202.064, 0.000},
|
||||
|
||||
{-1488.881, 1208.139, 0.000},
|
||||
|
||||
{-1613.822, 1281.229, 0.000},
|
||||
|
||||
{-1682.561, 1274.593, 0.000},
|
||||
|
||||
{-1927.827, 1384.315, 0.000},
|
||||
|
||||
{-2035.692, 1400.027, 0.000},
|
||||
|
||||
{-2224.536, 1462.204, 0.000},
|
||||
|
||||
{-2313.168, 1458.187, 0.000},
|
||||
|
||||
{-2436.409, 1496.333, 0.000},
|
||||
|
||||
{-2503.565, 1473.606, 0.000},
|
||||
|
||||
{-2605.994, 1481.901, 0.000},
|
||||
|
||||
{-2563.683, 1431.523, 0.000},
|
||||
|
||||
{-2599.753, 1395.453, 0.000},
|
||||
|
||||
{-2679.961, 1407.933, 0.000},
|
||||
|
||||
{-2630.649, 1438.753, 0.000},
|
||||
|
||||
{-2581.338, 1506.557, 0.000},
|
||||
|
||||
{-2528.483, 1197.294, 0.000},
|
||||
|
||||
{-2553.878, 1178.248, 0.000},
|
||||
|
||||
{-2553.878, 1108.412, 0.000},
|
||||
|
||||
{-2450.511, 1023.108, 0.000},
|
||||
|
||||
{-2450.511, 967.579, 0.000},
|
||||
|
||||
{-2437.486, 890.971, 0.000},
|
||||
|
||||
{-2450.511, 819.504, 0.000},
|
||||
|
||||
{-2365.134, 690.688, 0.000},
|
||||
|
||||
{-2296.590, 541.293, 0.000},
|
||||
|
||||
{-2283.679, 620.835, 0.000},
|
||||
|
||||
{-2151.879, 318.478, 0.000},
|
||||
|
||||
{-2176.409, 287.816, 0.000},
|
||||
|
||||
{-2102.820, 251.022, 0.000},
|
||||
|
||||
{-2127.350, 189.698, 0.000},
|
||||
|
||||
{-2028.905, 122.896, 0.000},
|
||||
|
||||
{-2046.821, 66.008, 0.000},
|
||||
|
||||
{-1958.025, 1.247, 0.000},
|
||||
|
||||
{-1886.644, -52.849, 0.000},
|
||||
|
||||
{-1863.596, -64.373, 0.000},
|
||||
|
||||
{-1783.239, -134.282, 0.000},
|
||||
|
||||
{-1705.906, -151.254, 0.000},
|
||||
|
||||
{-1617.356, -206.598, 0.000},
|
||||
|
||||
{-1565.032, -222.410, 0.000},
|
||||
|
||||
{-1465.740, -282.723, 0.000},
|
||||
|
||||
{-1361.237, -309.086, 0.000},
|
||||
|
||||
{-1276.829, -355.967, 0.000},
|
||||
|
||||
{-1236.832, -350.967, 0.000},
|
||||
|
||||
{-1170.082, -368.522, 0.000},
|
||||
|
||||
{-1145.877, -386.675, 0.000},
|
||||
|
||||
{-1133.776, -374.573, 0.000},
|
||||
|
||||
{-1064.985, -381.088, 0.000},
|
||||
|
||||
{-1060.602, -409.499, 0.000},
|
||||
|
||||
{-943.535, -368.153, 0.000},
|
||||
|
||||
{-852.780, -365.980, 0.000},
|
||||
|
||||
{-747.917, -375.279, 0.000},
|
||||
|
||||
{-694.244, -346.169, 0.000},
|
||||
|
||||
{-724.264, -360.724, 0.000},
|
||||
|
||||
{-601.465, -334.072, 0.000},
|
||||
|
||||
{-557.205, -336.465, 0.000},
|
||||
|
||||
{-564.189, -332.895, 0.000},
|
||||
|
||||
{-520.468, -340.201, 0.000},
|
||||
|
||||
{-476.908, -317.140, 0.000},
|
||||
|
||||
{-417.040, -318.781, 0.000},
|
||||
|
||||
{-377.842, -296.022, 0.000},
|
||||
|
||||
{-309.273, -302.223, 0.000},
|
||||
|
||||
{-267.572, -281.072, 0.000},
|
||||
|
||||
{-233.495, -287.929, 0.000},
|
||||
|
||||
{-211.795, -257.754, 0.000},
|
||||
|
||||
{-209.412, -234.544, 0.000},
|
||||
|
||||
{-144.978, -252.842, 0.000},
|
||||
|
||||
{-86.550, -255.100, 0.000},
|
||||
|
||||
{-57.889, -200.404, 0.000},
|
||||
|
||||
{-12.395, -168.373, 0.000},
|
||||
|
||||
{-8.988, -117.709, 0.000},
|
||||
|
||||
{-5.494, -73.048, 0.000},
|
||||
|
||||
{12.541, -24.783, 0.000},
|
||||
|
||||
{-8.001, 22.391, 0.000},
|
||||
|
||||
{-7.999, -85.193, 0.000},
|
||||
|
||||
{-61.402, 98.350, 0.000}
|
||||
|
||||
};
|
||||
std::vector<std::vector<double>> cashedCubeVisPoints;
|
||||
|
||||
ros::Publisher pub_cubes, pub_aligned_traj, pub_sensor_left_gps;
|
||||
ros::Subscriber sub_imu, subs_T_align, sub_stereo_left;
|
||||
visualization_msgs::Marker Marker;
|
||||
std::ofstream fT_aligned;
|
||||
nav_msgs::Path curr_path;
|
||||
|
||||
void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
|
||||
ROS_INFO_STREAM("received imu");
|
||||
@@ -45,17 +298,109 @@ void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
|
||||
|
||||
}
|
||||
|
||||
void T_align_callback(const geometry_msgs::TransformStamped &msg){
|
||||
// the main thing
|
||||
const uint pointsLen = cubeVisPoints.size();
|
||||
std::cout << "1" << std::endl;
|
||||
Eigen::MatrixXd points(pointsLen, 4);
|
||||
Eigen::Matrix4d T_align;
|
||||
|
||||
Eigen::Vector4d qRot;
|
||||
qRot << msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z, msg.transform.rotation.w;
|
||||
Eigen::Matrix3d mRot = ov_core::quat_2_Rot(qRot);
|
||||
|
||||
for (int i=0;i<3;i++){
|
||||
for (int j=0;j<3;j++){
|
||||
T_align(i,j) = mRot(i,j);
|
||||
}
|
||||
}
|
||||
std::cout << "2" << std::endl;
|
||||
T_align(0,3) = msg.transform.translation.x;
|
||||
T_align(1,3) = msg.transform.translation.y;
|
||||
T_align(2,3) = msg.transform.translation.z;
|
||||
T_align(3,3) = 1;
|
||||
std::cout << "3" << std::endl;
|
||||
|
||||
for (int i=0;i<cubeVisPoints.size();i++){
|
||||
points(i, 0) = cashedCubeVisPoints[0][0];
|
||||
points(i, 1) = cashedCubeVisPoints[0][1];
|
||||
points(i, 2) = cashedCubeVisPoints[0][2];
|
||||
points(i, 3) = 1;
|
||||
}
|
||||
std::cout << "4" << std::endl;
|
||||
std::cout << "Extracted T_align: " << T_align << std::endl;
|
||||
std::stringstream T_align_output;
|
||||
T_align_output << T_align << std::endl;
|
||||
ROS_INFO(T_align_output.str().c_str());
|
||||
fT_aligned << msg.header.stamp.toSec() << std::endl << T_align_output.str().c_str() << std::endl;
|
||||
|
||||
points = (T_align * points.transpose()).transpose();
|
||||
for (int i=0;i<cubeVisPoints.size();i++){
|
||||
cubeVisPoints[0][0] = points(i, 0);
|
||||
cubeVisPoints[0][1] = points(i, 1);
|
||||
cubeVisPoints[0][2] = points(i, 2);
|
||||
}
|
||||
std::cout << "5" << std::endl;
|
||||
|
||||
// Align the trajectory and publish to see the visualization of obtained R and t.
|
||||
Eigen::MatrixXd poses(curr_path.poses.size(), 4);
|
||||
// extract
|
||||
for (int i=0;i<curr_path.poses.size();i++){
|
||||
poses(i, 0) = curr_path.poses[i].pose.position.x;
|
||||
poses(i, 1) = curr_path.poses[i].pose.position.y;
|
||||
poses(i, 2) = curr_path.poses[i].pose.position.z;
|
||||
}
|
||||
// align
|
||||
auto aligned_poses = (T_align * poses.transpose()).transpose();
|
||||
|
||||
// fill again
|
||||
for (int i=0;i<curr_path.poses.size();i++){
|
||||
curr_path.poses[i].pose.position.x = poses(i, 0);
|
||||
curr_path.poses[i].pose.position.y = poses(i, 1);
|
||||
curr_path.poses[i].pose.position.z = poses(i, 2);
|
||||
}
|
||||
|
||||
// publish
|
||||
pub_aligned_traj.publish(curr_path);
|
||||
|
||||
}
|
||||
|
||||
void callback_stereo_left_gps(sensor_msgs::ImagePtr msg){
|
||||
msg->header.frame_id = "cam0_gps";
|
||||
pub_sensor_left_gps.publish(msg);
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[]){
|
||||
ros::init(argc, argv, "render_cubes");
|
||||
|
||||
ros::NodeHandle nh;
|
||||
|
||||
fT_aligned.open("T_aligned_history.txt");
|
||||
if (!fT_aligned.is_open()){
|
||||
ROS_INFO("error opening file!");
|
||||
}
|
||||
else{
|
||||
ROS_INFO("file open success!");
|
||||
std::string curr_path = std::filesystem::current_path();
|
||||
std::string output_str = "current path is: " + curr_path;
|
||||
ROS_INFO(output_str.c_str());
|
||||
|
||||
}
|
||||
|
||||
// the idea: set the subscriber for IMU, and every time the IMU measurement comes, publish
|
||||
// the cubes because the images are also publish when the IMU measurement arrives.
|
||||
// the cubes because the images are also published when the IMU measurement arrives.
|
||||
sub_imu = nh.subscribe("/imu/data_raw", 1000, &callback_imu);
|
||||
pub_cubes = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
|
||||
pub_aligned_traj = nh.advertise<nav_msgs::Path>("/ov_msckf/pathaligned", 2);
|
||||
|
||||
Marker.header.frame_id = "global";
|
||||
// a lame fix: create the subscirber here which sends the message from /stereo/left/image_raw to /global_fusion_node/sensor_left
|
||||
sub_stereo_left = nh.subscribe("/stereo/left/image_raw", 1000, &callback_stereo_left_gps);
|
||||
pub_sensor_left_gps = nh.advertise<sensor_msgs::Image>("/global_fusion_node/sensor_left", 2);
|
||||
|
||||
//// UNCOMMENT THIS TO RESTORE THE T_align_callback
|
||||
// subs_T_align = nh.subscribe("/ov_msckf/T_align", 1000, T_align_callback);
|
||||
|
||||
Marker.header.frame_id = "gps_global";
|
||||
Marker.header.stamp = ros::Time();
|
||||
Marker.ns = "my_namespace";
|
||||
Marker.id = 0;
|
||||
|
||||
Reference in New Issue
Block a user