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

terminal commit

Browse Source
This commit is contained in:
admin1
2022-08-30 18:44:20 +03:00
parent 979c7a2250
commit 89f25ad747
8 changed files with 444036 additions and 154 deletions
Download Patch File Download Diff File Expand all files Collapse all files

254
ov_msckf/src/render_cubes.cpp
View File

@@ -23,7 +23,7 @@
#include <Eigen/Core>
float yCubesVisPos = -0.5;
float yCubesVisPos = -2.0;
float skew = 5.0;
std::vector<std::vector<double>> cubeVisPoints = {
{71.215, 32.694, yCubesVisPos+2.0},
@@ -52,241 +52,241 @@ std::vector<std::vector<double>> cubeVisPoints = {
{1244.767, 784.257, yCubesVisPos},
// CUBES FOR THE GPS ALIGNED TRAJECTORY
{-5.473, 15.819, 0.000},
{-5.473, 15.819, yCubesVisPos},
{-15.586, 25.062, 0.000},
{-15.586, 25.062, yCubesVisPos},
{-22.395, 43.562, 0.000},
{-22.395, 43.562, yCubesVisPos},
{-36.035, 50.009, 0.000},
{-36.035, 50.009, yCubesVisPos},
{-41.113, 62.946, 0.000},
{-41.113, 62.946, yCubesVisPos},
{-47.675, 62.477, 0.000},
{-47.675, 62.477, yCubesVisPos},
{-56.081, 76.612, 0.000},
{-56.081, 76.612, yCubesVisPos},
{-68.155, 82.382, 0.000},
{-68.155, 82.382, yCubesVisPos},
{-76.903, 97.032, 0.000},
{-76.903, 97.032, yCubesVisPos},
{-93.218, 103.532, 0.000},
{-93.218, 103.532, yCubesVisPos},
{-121.903, 137.353, 0.000},
{-121.903, 137.353, yCubesVisPos},
{-134.567, 140.840, 0.000},
{-134.567, 140.840, yCubesVisPos},
{-158.030, 169.709, 0.000},
{-158.030, 169.709, yCubesVisPos},
{-170.521, 177.370, 0.000},
{-170.521, 177.370, yCubesVisPos},
{-225.440, 236.153, 0.000},
{-225.440, 236.153, yCubesVisPos},
{-259.810, 259.846, 0.000},
{-259.810, 259.846, yCubesVisPos},
{-287.220, 297.381, 0.000},
{-287.220, 297.381, yCubesVisPos},
{-313.876, 312.375, 0.000},
{-313.876, 312.375, yCubesVisPos},
{-446.905, 450.175, 0.000},
{-446.905, 450.175, yCubesVisPos},
{-489.056, 477.187, 0.000},
{-489.056, 477.187, yCubesVisPos},
{-544.828, 555.099, 0.000},
{-544.828, 555.099, yCubesVisPos},
{-568.349, 556.457, 0.000},
{-568.349, 556.457, yCubesVisPos},
{-638.075, 643.542, 0.000},
{-638.075, 643.542, yCubesVisPos},
{-702.377, 676.867, 0.000},
{-702.377, 676.867, yCubesVisPos},
{-753.507, 750.003, 0.000},
{-753.507, 750.003, yCubesVisPos},
{-807.197, 768.580, 0.000},
{-807.197, 768.580, yCubesVisPos},
{-863.780, 829.878, 0.000},
{-863.780, 829.878, yCubesVisPos},
{-877.002, 860.185, 0.000},
{-877.002, 860.185, yCubesVisPos},
{-905.568, 864.266, 0.000},
{-905.568, 864.266, yCubesVisPos},
{-915.407, 887.186, 0.000},
{-915.407, 887.186, yCubesVisPos},
{-931.954, 899.596, 0.000},
{-931.954, 899.596, yCubesVisPos},
{-936.091, 907.870, 0.000},
{-936.091, 907.870, yCubesVisPos},
{-971.656, 952.685, 0.000},
{-971.656, 952.685, yCubesVisPos},
{-1012.476, 947.723, 0.000},
{-1012.476, 947.723, yCubesVisPos},
{-1016.629, 985.880, 0.000},
{-1016.629, 985.880, yCubesVisPos},
{-1065.223, 987.986, 0.000},
{-1065.223, 987.986, yCubesVisPos},
{-1078.723, 1012.979, 0.000},
{-1078.723, 1012.979, yCubesVisPos},
{-1145.907, 1035.300, 0.000},
{-1145.907, 1035.300, yCubesVisPos},
{-1234.198, 1116.285, 0.000},
{-1234.198, 1116.285, yCubesVisPos},
{-1338.651, 1152.282, 0.000},
{-1338.651, 1152.282, yCubesVisPos},
{-1422.556, 1202.064, 0.000},
{-1422.556, 1202.064, yCubesVisPos},
{-1488.881, 1208.139, 0.000},
{-1488.881, 1208.139, yCubesVisPos},
{-1613.822, 1281.229, 0.000},
{-1613.822, 1281.229, yCubesVisPos},
{-1682.561, 1274.593, 0.000},
{-1682.561, 1274.593, yCubesVisPos},
{-1927.827, 1384.315, 0.000},
{-1927.827, 1384.315, yCubesVisPos},
{-2035.692, 1400.027, 0.000},
{-2035.692, 1400.027, yCubesVisPos},
{-2224.536, 1462.204, 0.000},
{-2224.536, 1462.204, yCubesVisPos},
{-2313.168, 1458.187, 0.000},
{-2313.168, 1458.187, yCubesVisPos},
{-2436.409, 1496.333, 0.000},
{-2436.409, 1496.333, yCubesVisPos},
{-2503.565, 1473.606, 0.000},
{-2503.565, 1473.606, yCubesVisPos},
{-2605.994, 1481.901, 0.000},
{-2605.994, 1481.901, yCubesVisPos},
{-2563.683, 1431.523, 0.000},
{-2563.683, 1431.523, yCubesVisPos},
{-2599.753, 1395.453, 0.000},
{-2599.753, 1395.453, yCubesVisPos},
{-2679.961, 1407.933, 0.000},
{-2679.961, 1407.933, yCubesVisPos},
{-2630.649, 1438.753, 0.000},
{-2630.649, 1438.753, yCubesVisPos},
{-2581.338, 1506.557, 0.000},
{-2581.338, 1506.557, yCubesVisPos},
{-2528.483, 1197.294, 0.000},
{-2528.483, 1197.294, yCubesVisPos},
{-2553.878, 1178.248, 0.000},
{-2553.878, 1178.248, yCubesVisPos},
{-2553.878, 1108.412, 0.000},
{-2553.878, 1108.412, yCubesVisPos},
{-2450.511, 1023.108, 0.000},
{-2450.511, 1023.108, yCubesVisPos},
{-2450.511, 967.579, 0.000},
{-2450.511, 967.579, yCubesVisPos},
{-2437.486, 890.971, 0.000},
{-2437.486, 890.971, yCubesVisPos},
{-2450.511, 819.504, 0.000},
{-2450.511, 819.504, yCubesVisPos},
{-2365.134, 690.688, 0.000},
{-2365.134, 690.688, yCubesVisPos},
{-2296.590, 541.293, 0.000},
{-2296.590, 541.293, yCubesVisPos},
{-2283.679, 620.835, 0.000},
{-2283.679, 620.835, yCubesVisPos},
{-2151.879, 318.478, 0.000},
{-2151.879, 318.478, yCubesVisPos},
{-2176.409, 287.816, 0.000},
{-2176.409, 287.816, yCubesVisPos},
{-2102.820, 251.022, 0.000},
{-2102.820, 251.022, yCubesVisPos},
{-2127.350, 189.698, 0.000},
{-2127.350, 189.698, yCubesVisPos},
{-2028.905, 122.896, 0.000},
{-2028.905, 122.896, yCubesVisPos},
{-2046.821, 66.008, 0.000},
{-2046.821, 66.008, yCubesVisPos},
{-1958.025, 1.247, 0.000},
{-1958.025, 1.247, yCubesVisPos},
{-1886.644, -52.849, 0.000},
{-1886.644, -52.849, yCubesVisPos},
{-1863.596, -64.373, 0.000},
{-1863.596, -64.373, yCubesVisPos},
{-1783.239, -134.282, 0.000},
{-1783.239, -134.282, yCubesVisPos},
{-1705.906, -151.254, 0.000},
{-1705.906, -151.254, yCubesVisPos},
{-1617.356, -206.598, 0.000},
{-1617.356, -206.598, yCubesVisPos},
{-1565.032, -222.410, 0.000},
{-1565.032, -222.410, yCubesVisPos},
{-1465.740, -282.723, 0.000},
{-1465.740, -282.723, yCubesVisPos},
{-1361.237, -309.086, 0.000},
{-1361.237, -309.086, yCubesVisPos},
{-1276.829, -355.967, 0.000},
{-1276.829, -355.967, yCubesVisPos},
{-1236.832, -350.967, 0.000},
{-1236.832, -350.967, yCubesVisPos},
{-1170.082, -368.522, 0.000},
{-1170.082, -368.522, yCubesVisPos},
{-1145.877, -386.675, 0.000},
{-1145.877, -386.675, yCubesVisPos},
{-1133.776, -374.573, 0.000},
{-1133.776, -374.573, yCubesVisPos},
{-1064.985, -381.088, 0.000},
{-1064.985, -381.088, yCubesVisPos},
{-1060.602, -409.499, 0.000},
{-1060.602, -409.499, yCubesVisPos},
{-943.535, -368.153, 0.000},
{-943.535, -368.153, yCubesVisPos},
{-852.780, -365.980, 0.000},
{-852.780, -365.980, yCubesVisPos},
{-747.917, -375.279, 0.000},
{-747.917, -375.279, yCubesVisPos},
{-694.244, -346.169, 0.000},
{-694.244, -346.169, yCubesVisPos},
{-724.264, -360.724, 0.000},
{-724.264, -360.724, yCubesVisPos},
{-601.465, -334.072, 0.000},
{-601.465, -334.072, yCubesVisPos},
{-557.205, -336.465, 0.000},
{-557.205, -336.465, yCubesVisPos},
{-564.189, -332.895, 0.000},
{-564.189, -332.895, yCubesVisPos},
{-520.468, -340.201, 0.000},
{-520.468, -340.201, yCubesVisPos},
{-476.908, -317.140, 0.000},
{-476.908, -317.140, yCubesVisPos},
{-417.040, -318.781, 0.000},
{-417.040, -318.781, yCubesVisPos},
{-377.842, -296.022, 0.000},
{-377.842, -296.022, yCubesVisPos},
{-309.273, -302.223, 0.000},
{-309.273, -302.223, yCubesVisPos},
{-267.572, -281.072, 0.000},
{-267.572, -281.072, yCubesVisPos},
{-233.495, -287.929, 0.000},
{-233.495, -287.929, yCubesVisPos},
{-211.795, -257.754, 0.000},
{-211.795, -257.754, yCubesVisPos},
{-209.412, -234.544, 0.000},
{-209.412, -234.544, yCubesVisPos},
{-144.978, -252.842, 0.000},
{-144.978, -252.842, yCubesVisPos},
{-86.550, -255.100, 0.000},
{-86.550, -255.100, yCubesVisPos},
{-57.889, -200.404, 0.000},
{-57.889, -200.404, yCubesVisPos},
{-12.395, -168.373, 0.000},
{-12.395, -168.373, yCubesVisPos},
{-8.988, -117.709, 0.000},
{-8.988, -117.709, yCubesVisPos},
{-5.494, -73.048, 0.000},
{-5.494, -73.048, yCubesVisPos},
{12.541, -24.783, 0.000},
{12.541, -24.783, yCubesVisPos},
{-8.001, 22.391, 0.000},
{-8.001, 22.391, yCubesVisPos},
{-7.999, -85.193, 0.000},
{-7.999, -85.193, yCubesVisPos},
{-61.402, 98.350, 0.000}
{-61.402, 98.350, yCubesVisPos}
};
std::vector<std::vector<double>> cashedCubeVisPoints;
ros::Publisher pub_cubes, pub_aligned_traj, pub_sensor_left_gps;
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;
std::ofstream fT_aligned;
@@ -294,6 +294,8 @@ nav_msgs::Path curr_path;
void callback_imu(const sensor_msgs::Imu::ConstPtr &msg){
ROS_INFO_STREAM("received imu");
pub_cubes_gps.publish(Marker);
Marker.header.frame_id = "global";
pub_cubes.publish(Marker);
}
@@ -365,10 +367,13 @@ void T_align_callback(const geometry_msgs::TransformStamped &msg){
}
void callback_stereo_left_gps(sensor_msgs::ImagePtr msg){
msg->header.frame_id = "cam0_gps";
pub_sensor_left_gps.publish(msg);
}
//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");
@@ -390,12 +395,13 @@ int main(int argc, char* argv[]){
// 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/vis_markers", 0);
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);
// 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);

61
ov_msckf/src/ros/ROS1Visualizer.cpp
View File

@@ -39,7 +39,9 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
pub_poseimu = nh->advertise<geometry_msgs::PoseWithCovarianceStamped>("/ov_msckf/poseimu", 2);
PRINT_DEBUG("Publishing: %s\n", pub_poseimu.getTopic().c_str());
pub_odomimu = nh->advertise<nav_msgs::Odometry>("/ov_msckf/odomimu", 2);
pub_odomimu_gps = nh->advertise<nav_msgs::Odometry>("/ov_msckf/odomimugps", 2);
PRINT_DEBUG("Publishing: %s\n", pub_odomimu.getTopic().c_str());
ROS_INFO_STREAM("Publishing: " << pub_odomimu_gps.getTopic().c_str() << std::endl);
pub_pathimu = nh->advertise<nav_msgs::Path>("/ov_msckf/pathimu", 2);
PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str());
pub_poserec = nh->advertise<geometry_msgs::PoseStamped>("/ov_msckf/poserec", 2);
@@ -135,7 +137,7 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr<ros::NodeHandle> nh, std::shared_
pub_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_msckf/cam0_info", 2);
pub_gps_cam0_info = nh->advertise<sensor_msgs::CameraInfo>("/global_fusion_node/camera_info", 2);
//sensor_left_gps_pub = it.advertise("/global_fusion_node/sensor_left", 2);
sensor_left_gps_pub = it.advertise("/global_fusion_node/sensor_left", 2);
//// UNCOMMENT THIS IN ORDER TO RESTORE THE CUBES RENDERING
// vis_pub = nh->advertise<visualization_msgs::Marker>("/ov_msckf/vis_markers", 0);
// pub_camera_info = nh->advertise<sensor_msgs::CameraInfo>("/ov_mskcf/camera_info", 2);
@@ -369,7 +371,7 @@ void ROS1Visualizer::visualize_odometry(double timestamp) {
return;
// Publish our odometry message if requested
if (pub_odomimu.getNumSubscribers() != 0) {
if (pub_odomimu.getNumSubscribers() != 0 || pub_odomimu_gps.getNumSubscribers() != 0) {
nav_msgs::Odometry odomIinM;
odomIinM.header.stamp = ros::Time(timestamp);
@@ -410,6 +412,14 @@ void ROS1Visualizer::visualize_odometry(double timestamp) {
}
}
pub_odomimu.publish(odomIinM);
auto gps_odom = nav_msgs::Odometry();
gps_odom.pose.pose.orientation = odomIinM.pose.pose.orientation;
gps_odom.pose.pose.position = odomIinM.pose.pose.position;
gps_odom.pose.covariance = odomIinM.pose.covariance;
gps_odom.header.frame_id = "gps_global";
gps_odom.header.stamp = ros::Time(original_img_ts);
pub_odomimu_gps.publish(gps_odom);
}
// Publish our transform on TF
@@ -431,6 +441,14 @@ void ROS1Visualizer::visualize_odometry(double timestamp) {
trans_calib.child_frame_id_ = "cam" + std::to_string(calib.first);
if (publish_calibration_tf) {
mTfBr->sendTransform(trans_calib);
// added by PI
auto trans_calib_gps = tf::StampedTransform();
trans_calib_gps = trans_calib;
trans_calib_gps.stamp_ = ros::Time(original_img_ts);
trans_calib_gps.frame_id_ = "imu_gps";
trans_calib_gps.child_frame_id_ = "cam0_gps";
mTfBr->sendTransform(trans_calib_gps);
}
}
}
@@ -593,6 +611,10 @@ void ROS1Visualizer::callback_stereo(const sensor_msgs::ImageConstPtr &msg0, con
if (camera_last_timestamp.find(cam_id0) != camera_last_timestamp.end() && timestamp < camera_last_timestamp.at(cam_id0) + time_delta) {
return;
}
// added by PI
auto gps_cv_ptr0 = cv_bridge::toCvCopy(msg0, sensor_msgs::image_encodings::RGB8);
gps_img_msg = gps_cv_ptr0->toImageMsg();
camera_last_timestamp[cam_id0] = timestamp;
// Get the image
@@ -727,13 +749,13 @@ void ROS1Visualizer::publish_images() {
exl_msg_global = exl_msg;
// Get the original image in order to publish it to the topic for rendering with CameraPub.
cv::Mat original_img;
double original_img_ts;
_app->get_active_image(original_img_ts, original_img);
// usually the function returns -1. so, let's fill it with the ts from camera_last_timestamp.
if (original_img_ts == -1){
original_img_ts = camera_last_timestamp[0];
}
// cv::Mat original_img;
// double original_img_ts;
// _app->get_active_image(original_img_ts, original_img);
// // usually the function returns -1. so, let's fill it with the ts from camera_last_timestamp.
// if (original_img_ts == -1){
// original_img_ts = camera_last_timestamp[0];
// }
std_msgs::Header orig_img_msg_header;
// as I remember it's the timestamp in kaist is already in seconds.
ROS_INFO_STREAM("org msg: 1");
@@ -741,9 +763,8 @@ void ROS1Visualizer::publish_images() {
ROS_INFO_STREAM("org msg: 2");
orig_img_msg_header.stamp = orig_stamp;
orig_img_msg_header.frame_id = "cam0";
sensor_msgs::ImagePtr original_img_message = cv_bridge::CvImage(orig_img_msg_header, "bgr8", original_img).toImageMsg();
ROS_INFO_STREAM("The camera last timestamp for cam0 is: " << std::fixed << std::setprecision(21) << original_img_ts);
ROS_INFO_STREAM("Original image width and height are: " << original_img.cols << " " << original_img.rows);
// ROS_INFO_STREAM("The camera last timestamp for cam0 is: " << std::fixed << std::setprecision(21) << original_img_ts);
// ROS_INFO_STREAM("Original image width and height are: " << original_img.cols << " " << original_img.rows);
// Publish
it_pub_tracks.publish(exl_msg);
@@ -754,6 +775,17 @@ void ROS1Visualizer::publish_images() {
// cv::Mat cropped_image = img_history(rows, cols);
// sensor_msgs::ImagePtr sensor_left_msg = cv_bridge::CvImage(header, "bgr8", cropped_image).toImageMsg();
// sensor_left_msg->header.seq = sensor_left_id++;
// since this is a pointer, we need to initialize (heap-allocate) it in boost.
sensor_msgs::ImagePtr original_img_message = boost::make_shared<sensor_msgs::Image>();
original_img_message->header = orig_img_msg_header;
original_img_message->encoding = gps_img_msg->encoding;
original_img_message->height = gps_img_msg->height;
original_img_message->width = gps_img_msg->width;
original_img_message->is_bigendian = gps_img_msg->is_bigendian;
original_img_message->step = gps_img_msg->step;
original_img_message->data = gps_img_msg->data;
sensor_left_pub.publish(original_img_message);
original_img_message->header.frame_id = "cam0_gps";
@@ -790,7 +822,7 @@ void ROS1Visualizer::publish_images() {
auto camInfoGPS = sensor_msgs::CameraInfo();
camInfoGPS = cameraparams;
camInfoGPS.header.frame_id = "cam0_gps";
camInfoGPS.header.stamp = orig_img_msg_header.stamp;
// camInfoGPS.header.stamp = orig_img_msg_header.stamp;
camInfoGPS.height = exl_msg->height;
camInfoGPS.width = exl_msg->width / 2;
pub_gps_cam0_info.publish(camInfoGPS);
@@ -1003,6 +1035,9 @@ void ROS1Visualizer::publish_loopclosure_information() {
if (active_tracks_time1 != active_tracks_time2)
return;
// PI: Fill the timestamp.
original_img_ts = active_tracks_time1;
// Default header
std_msgs::Header header;
header.stamp = ros::Time(active_tracks_time1);

5
ov_msckf/src/ros/ROS1Visualizer.h
View File

@@ -155,7 +155,7 @@ protected:
// Our publishers
image_transport::Publisher it_pub_tracks, it_pub_loop_img_depth, it_pub_loop_img_depth_color;
ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu;
ros::Publisher pub_poseimu, pub_odomimu, pub_pathimu, pub_odomimu_gps;
ros::Publisher pub_points_msckf, pub_points_slam, pub_points_aruco, pub_points_sim;
ros::Publisher pub_loop_pose, pub_loop_point, pub_loop_extrinsic, pub_loop_intrinsics;
std::shared_ptr<tf::TransformBroadcaster> mTfBr;
@@ -172,6 +172,7 @@ protected:
ros::Publisher vis_pub;
sensor_msgs::CameraInfo camInfoGlob;
sensor_msgs::ImagePtr exl_msg_global;
sensor_msgs::ImageConstPtr gps_img_msg;
ros::Subscriber sub_camera_info, subs_T_align;
ros::Publisher pub_poserec, pub_aligned_traj;
nav_msgs::Path curr_path;
@@ -202,6 +203,8 @@ protected:
double summed_nees_pos = 0.0;
size_t summed_number = 0;
double original_img_ts;
// Start and end timestamps
bool start_time_set = false;
boost::posix_time::ptime rT1, rT2;