467 lines
13 KiB
C++
467 lines
13 KiB
C++
//
|
|
// 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>
|
|
|
|
|
|
// open_vis library
|
|
#include <utils/quat_ops.h>
|
|
|
|
// thirdparty libraries
|
|
#include <Eigen/Core>
|
|
|
|
|
|
float yCubesVisPos = -2.0;
|
|
float skew = 5.0;
|
|
std::vector<std::vector<double>> cubeVisPoints = {
|
|
{71.215, 32.694, yCubesVisPos+2.0},
|
|
// {81.668, 26.227, yCubesVisPos},
|
|
{104.061, 36.691, yCubesVisPos+1.5},
|
|
// {90.726, 32.825, yCubesVisPos},
|
|
{109.541, 60.415, yCubesVisPos+1.0},
|
|
// {149.264, 69.490, yCubesVisPos},
|
|
{183.605, 91.474, yCubesVisPos+0.5},
|
|
// {198.294, 97.113, yCubesVisPos},
|
|
{251.391, 154.108, yCubesVisPos-1.0},
|
|
// {310.030, 190.208, yCubesVisPos},
|
|
{330.554, 183.244, yCubesVisPos-2.0},
|
|
{746.086, 450.563, yCubesVisPos-2.0},
|
|
// {792.608, 527.140, yCubesVisPos},
|
|
{883.669, 517.444, yCubesVisPos-2.0},
|
|
{957.811, 652.638, yCubesVisPos-2.0},
|
|
{1086.045, 669.317, yCubesVisPos-2.0},
|
|
{1125.310, 728.214, yCubesVisPos-2.0},
|
|
{731.996, 425.021, yCubesVisPos-2.0},
|
|
|
|
{1186.527, 700.189, yCubesVisPos},
|
|
// {1205.432, 681.457, yCubesVisPos},
|
|
{1184.375, 735.604, yCubesVisPos},
|
|
// {1232.697, 715.647, yCubesVisPos},
|
|
{1244.767, 784.257, yCubesVisPos},
|
|
|
|
// CUBES FOR THE GPS ALIGNED TRAJECTORY
|
|
{-5.473, 15.819, yCubesVisPos},
|
|
|
|
{-15.586, 25.062, yCubesVisPos},
|
|
|
|
{-22.395, 43.562, yCubesVisPos},
|
|
|
|
{-36.035, 50.009, yCubesVisPos},
|
|
|
|
{-41.113, 62.946, yCubesVisPos},
|
|
|
|
{-47.675, 62.477, yCubesVisPos},
|
|
|
|
{-56.081, 76.612, yCubesVisPos},
|
|
|
|
{-68.155, 82.382, yCubesVisPos},
|
|
|
|
{-76.903, 97.032, yCubesVisPos},
|
|
|
|
{-93.218, 103.532, yCubesVisPos},
|
|
|
|
{-121.903, 137.353, yCubesVisPos},
|
|
|
|
{-134.567, 140.840, yCubesVisPos},
|
|
|
|
{-158.030, 169.709, yCubesVisPos},
|
|
|
|
{-170.521, 177.370, yCubesVisPos},
|
|
|
|
{-225.440, 236.153, yCubesVisPos},
|
|
|
|
{-259.810, 259.846, yCubesVisPos},
|
|
|
|
{-287.220, 297.381, yCubesVisPos},
|
|
|
|
{-313.876, 312.375, yCubesVisPos},
|
|
|
|
{-446.905, 450.175, yCubesVisPos},
|
|
|
|
{-489.056, 477.187, yCubesVisPos},
|
|
|
|
{-544.828, 555.099, yCubesVisPos},
|
|
|
|
{-568.349, 556.457, yCubesVisPos},
|
|
|
|
{-638.075, 643.542, yCubesVisPos},
|
|
|
|
{-702.377, 676.867, yCubesVisPos},
|
|
|
|
{-753.507, 750.003, yCubesVisPos},
|
|
|
|
{-807.197, 768.580, yCubesVisPos},
|
|
|
|
{-863.780, 829.878, yCubesVisPos},
|
|
|
|
{-877.002, 860.185, yCubesVisPos},
|
|
|
|
{-905.568, 864.266, yCubesVisPos},
|
|
|
|
{-915.407, 887.186, yCubesVisPos},
|
|
|
|
{-931.954, 899.596, yCubesVisPos},
|
|
|
|
{-936.091, 907.870, yCubesVisPos},
|
|
|
|
{-971.656, 952.685, yCubesVisPos},
|
|
|
|
{-1012.476, 947.723, yCubesVisPos},
|
|
|
|
|
|
{-1016.629, 985.880, yCubesVisPos},
|
|
|
|
{-1065.223, 987.986, yCubesVisPos},
|
|
|
|
{-1078.723, 1012.979, yCubesVisPos},
|
|
|
|
{-1145.907, 1035.300, yCubesVisPos},
|
|
|
|
{-1234.198, 1116.285, yCubesVisPos},
|
|
|
|
{-1338.651, 1152.282, yCubesVisPos},
|
|
|
|
{-1422.556, 1202.064, yCubesVisPos},
|
|
|
|
{-1488.881, 1208.139, yCubesVisPos},
|
|
|
|
{-1613.822, 1281.229, yCubesVisPos},
|
|
|
|
{-1682.561, 1274.593, yCubesVisPos},
|
|
|
|
{-1927.827, 1384.315, yCubesVisPos},
|
|
|
|
{-2035.692, 1400.027, yCubesVisPos},
|
|
|
|
{-2224.536, 1462.204, yCubesVisPos},
|
|
|
|
{-2313.168, 1458.187, yCubesVisPos},
|
|
|
|
{-2436.409, 1496.333, yCubesVisPos},
|
|
|
|
{-2503.565, 1473.606, yCubesVisPos},
|
|
|
|
{-2605.994, 1481.901, yCubesVisPos},
|
|
|
|
{-2563.683, 1431.523, yCubesVisPos},
|
|
|
|
{-2599.753, 1395.453, yCubesVisPos},
|
|
|
|
{-2679.961, 1407.933, yCubesVisPos},
|
|
|
|
{-2630.649, 1438.753, yCubesVisPos},
|
|
|
|
{-2581.338, 1506.557, yCubesVisPos},
|
|
|
|
{-2528.483, 1197.294, yCubesVisPos},
|
|
|
|
{-2553.878, 1178.248, yCubesVisPos},
|
|
|
|
{-2553.878, 1108.412, yCubesVisPos},
|
|
|
|
{-2450.511, 1023.108, yCubesVisPos},
|
|
|
|
{-2450.511, 967.579, yCubesVisPos},
|
|
|
|
{-2437.486, 890.971, yCubesVisPos},
|
|
|
|
{-2450.511, 819.504, yCubesVisPos},
|
|
|
|
{-2365.134, 690.688, yCubesVisPos},
|
|
|
|
{-2296.590, 541.293, yCubesVisPos},
|
|
|
|
{-2283.679, 620.835, yCubesVisPos},
|
|
|
|
{-2151.879, 318.478, yCubesVisPos},
|
|
|
|
{-2176.409, 287.816, yCubesVisPos},
|
|
|
|
{-2102.820, 251.022, yCubesVisPos},
|
|
|
|
{-2127.350, 189.698, yCubesVisPos},
|
|
|
|
{-2028.905, 122.896, yCubesVisPos},
|
|
|
|
{-2046.821, 66.008, yCubesVisPos},
|
|
|
|
{-1958.025, 1.247, yCubesVisPos},
|
|
|
|
{-1886.644, -52.849, yCubesVisPos},
|
|
|
|
{-1863.596, -64.373, yCubesVisPos},
|
|
|
|
{-1783.239, -134.282, yCubesVisPos},
|
|
|
|
{-1705.906, -151.254, yCubesVisPos},
|
|
|
|
{-1617.356, -206.598, yCubesVisPos},
|
|
|
|
{-1565.032, -222.410, yCubesVisPos},
|
|
|
|
{-1465.740, -282.723, yCubesVisPos},
|
|
|
|
{-1361.237, -309.086, yCubesVisPos},
|
|
|
|
{-1276.829, -355.967, yCubesVisPos},
|
|
|
|
{-1236.832, -350.967, yCubesVisPos},
|
|
|
|
{-1170.082, -368.522, yCubesVisPos},
|
|
|
|
{-1145.877, -386.675, yCubesVisPos},
|
|
|
|
{-1133.776, -374.573, yCubesVisPos},
|
|
|
|
{-1064.985, -381.088, yCubesVisPos},
|
|
|
|
{-1060.602, -409.499, yCubesVisPos},
|
|
|
|
{-943.535, -368.153, yCubesVisPos},
|
|
|
|
{-852.780, -365.980, yCubesVisPos},
|
|
|
|
{-747.917, -375.279, yCubesVisPos},
|
|
|
|
{-694.244, -346.169, yCubesVisPos},
|
|
|
|
{-724.264, -360.724, yCubesVisPos},
|
|
|
|
{-601.465, -334.072, yCubesVisPos},
|
|
|
|
{-557.205, -336.465, yCubesVisPos},
|
|
|
|
{-564.189, -332.895, yCubesVisPos},
|
|
|
|
{-520.468, -340.201, yCubesVisPos},
|
|
|
|
{-476.908, -317.140, yCubesVisPos},
|
|
|
|
{-417.040, -318.781, yCubesVisPos},
|
|
|
|
{-377.842, -296.022, yCubesVisPos},
|
|
|
|
{-309.273, -302.223, yCubesVisPos},
|
|
|
|
{-267.572, -281.072, yCubesVisPos},
|
|
|
|
{-233.495, -287.929, yCubesVisPos},
|
|
|
|
{-211.795, -257.754, yCubesVisPos},
|
|
|
|
{-209.412, -234.544, yCubesVisPos},
|
|
|
|
{-144.978, -252.842, yCubesVisPos},
|
|
|
|
{-86.550, -255.100, yCubesVisPos},
|
|
|
|
{-57.889, -200.404, yCubesVisPos},
|
|
|
|
{-12.395, -168.373, yCubesVisPos},
|
|
|
|
{-8.988, -117.709, yCubesVisPos},
|
|
|
|
{-5.494, -73.048, yCubesVisPos},
|
|
|
|
{12.541, -24.783, yCubesVisPos},
|
|
|
|
{-8.001, 22.391, yCubesVisPos},
|
|
|
|
{-7.999, -85.193, yCubesVisPos},
|
|
|
|
{-61.402, 98.350, yCubesVisPos}
|
|
|
|
};
|
|
std::vector<std::vector<double>> cashedCubeVisPoints;
|
|
|
|
ros::Publisher pub_cubes, pub_cubes_gps, pub_aligned_traj, pub_sensor_left_gps;
|
|
ros::Subscriber sub_imu, subs_T_align, sub_stereo_left;
|
|
visualization_msgs::Marker Marker, Marker_gps;
|
|
std::ofstream fT_aligned;
|
|
nav_msgs::Path curr_path;
|
|
|
|
void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
|
|
ROS_INFO_STREAM("received imu");
|
|
if (pub_cubes.getNumSubscribers() != 0 || pub_cubes_gps.getNumSubscribers() != 0){
|
|
pub_cubes_gps.publish(Marker_gps);
|
|
pub_cubes.publish(Marker);
|
|
}
|
|
}
|
|
|
|
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){
|
|
// auto new_msg = sensor_msgs::ImagePtr();
|
|
// new_msg = msg;
|
|
//// new_msg->header.stamp = ros::Time(0.0);
|
|
// new_msg->header.frame_id = "cam0_gps";
|
|
// pub_sensor_left_gps.publish(new_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 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/vio_vis_markers", 0);
|
|
pub_cubes_gps = nh.advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
|
|
pub_aligned_traj = nh.advertise<nav_msgs::Path>("/ov_msckf/pathaligned", 2);
|
|
|
|
// 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;
|
|
Marker.type = visualization_msgs::Marker::CUBE_LIST;
|
|
Marker.action = visualization_msgs::Marker::ADD;
|
|
Marker.pose.orientation.x = 0.0;
|
|
Marker.pose.orientation.y = 0.0;
|
|
Marker.pose.orientation.z = 0.0;
|
|
Marker.pose.orientation.w = 1.0;
|
|
Marker.scale.x = 6.8f;
|
|
Marker.scale.y = 6.8f;
|
|
Marker.scale.z = 6.8f;
|
|
Marker.color.a = 1.0; // Don't forget to set the alpha!
|
|
Marker.color.r = 0.0;
|
|
Marker.color.g = 1.0;
|
|
Marker.color.b = 0.0;
|
|
//only if using a MESH_RESOURCE marker type:
|
|
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
|
|
|
|
for (auto & cubeVisPoint : cubeVisPoints){
|
|
geometry_msgs::Point tempPoint;
|
|
tempPoint.x = cubeVisPoint[0];
|
|
tempPoint.y = cubeVisPoint[1];
|
|
tempPoint.z = cubeVisPoint[2];
|
|
Marker.points.push_back(tempPoint);
|
|
}
|
|
|
|
Marker_gps.header.frame_id = "global";
|
|
Marker_gps.header.stamp = ros::Time();
|
|
Marker_gps.ns = "gps_namespace";
|
|
Marker_gps.id = 1;
|
|
Marker_gps.type = visualization_msgs::Marker::CUBE_LIST;
|
|
Marker_gps.action = visualization_msgs::Marker::ADD;
|
|
Marker_gps.pose.orientation.x = 0.0;
|
|
Marker_gps.pose.orientation.y = 0.0;
|
|
Marker_gps.pose.orientation.z = 0.0;
|
|
Marker_gps.pose.orientation.w = 1.0;
|
|
Marker_gps.scale.x = 6.8f;
|
|
Marker_gps.scale.y = 6.8f;
|
|
Marker_gps.scale.z = 6.8f;
|
|
Marker_gps.color.a = 1.0; // Don't forget to set the alpha!
|
|
Marker_gps.color.r = 0.0;
|
|
Marker_gps.color.g = 1.0;
|
|
Marker_gps.color.b = 0.0;
|
|
//only if using a MESH_RESOURCE marker type:
|
|
Marker.mesh_resource = "package://pr2_description/meshes/base_v0/base.dae";
|
|
|
|
for (auto & cubeVisPoint : cubeVisPoints){
|
|
geometry_msgs::Point tempPoint;
|
|
tempPoint.x = cubeVisPoint[0];
|
|
tempPoint.y = cubeVisPoint[1];
|
|
tempPoint.z = cubeVisPoint[2];
|
|
Marker_gps.points.push_back(tempPoint);
|
|
}
|
|
|
|
ros::spin();
|
|
}
|