Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Development v2.4 - Camera classes, Masks, Formatting and Smaller Fixes #186

Merged
merged 23 commits into from
Jul 19, 2021
Merged
Changes from 1 commit
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
be59e09
Add basic camera classes
goldbattle Jun 22, 2021
3b17893
Added camera class details
goldbattle Jun 22, 2021
1fdfc53
Convert trackers to use camera class, add mask ROI support
goldbattle Jun 23, 2021
ec2177e
remove unuses DOG grider
goldbattle Jun 23, 2021
952208b
clang format
goldbattle Jun 23, 2021
503ffff
clang format
goldbattle Jun 23, 2021
75c6695
camera should be for a single camera.... oops..
goldbattle Jun 23, 2021
6341a6c
compiles with new trackers, need to load params still
goldbattle Jun 23, 2021
d94c2b6
load mask and histogram params correctly
goldbattle Jun 23, 2021
fb27c5e
Add docker documentation instructions!
goldbattle Jun 24, 2021
f1dae32
try to improve stereo tracking, small cleanup of passed param
goldbattle Jun 24, 2021
b324bfe
Fix mask for mono subscribe
goldbattle Jun 25, 2021
3192d79
update rviz configs
goldbattle Jul 1, 2021
9ca890b
Small bug fix on bound checks, and other cleanup
goldbattle Jul 1, 2021
474ea3c
New serial reader to handle image dropping for stereo, remove stereo …
goldbattle Jul 1, 2021
846426f
small fix of subscribe for binocular
goldbattle Jul 6, 2021
9f4d3e9
Fix radtan norm intinsic jacob (fixes #169)
goldbattle Jul 6, 2021
a5cbd85
Add live alignment of groundtruth if we have it (for better dev'ing)
goldbattle Jul 6, 2021
6ba2cbb
clang format
goldbattle Jul 6, 2021
92f0b94
Added - KAIST VIO mav dataset
goldbattle Jul 7, 2021
dedc08e
Small cleanup in featdb
goldbattle Jul 14, 2021
34eb130
Option to not build aruco or ROS through cmake variables
goldbattle Jul 14, 2021
1cbde30
update readme
goldbattle Jul 19, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add live alignment of groundtruth if we have it (for better dev'ing)
goldbattle committed Jul 6, 2021

Verified

This commit was signed with the committer’s verified signature.
fwyzard Andrea Bocci
commit a5cbd85106fcf85efc5f930d6c8766e4cabe1b26
2 changes: 2 additions & 0 deletions ov_eval/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -95,6 +95,8 @@ target_link_libraries(ov_eval_lib ${thirdparty_libraries})
if (catkin_FOUND)
add_executable(pose_to_file src/pose_to_file.cpp)
target_link_libraries(pose_to_file ov_eval_lib ${thirdparty_libraries})
add_executable(live_align_trajectory src/live_align_trajectory.cpp)
target_link_libraries(live_align_trajectory ov_eval_lib ${thirdparty_libraries})
endif()


2 changes: 1 addition & 1 deletion ov_eval/src/alignment/AlignUtils.cpp
Original file line number Diff line number Diff line change
@@ -169,7 +169,7 @@ void AlignUtils::perform_association(double offset, double max_difference, std::
printf(RED "[ALIGN]: Was unable to associate groundtruth and estimate trajectories\n" RESET);
printf(RED "[ALIGN]: %d total matches....\n" RESET, (int)est_times.size());
printf(RED "[ALIGN]: Do the time of the files match??\n" RESET);
std::exit(EXIT_FAILURE);
return;
}
assert(est_times_temp.size() == gt_times_temp.size());
// printf("[TRAJ]: %d est poses and %d gt poses => %d matches\n",(int)est_times.size(),(int)gt_times.size(),(int)est_times_temp.size());
2 changes: 1 addition & 1 deletion ov_eval/src/calc/ResultTrajectory.cpp
Original file line number Diff line number Diff line change
@@ -39,7 +39,7 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st
AlignUtils::perform_association(0, 0.02, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori, gt_covpos);

// Return failure if we didn't have any common timestamps
if (est_poses.size() < 2) {
if (est_poses.size() < 3) {
printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
std::exit(EXIT_FAILURE);
137 changes: 137 additions & 0 deletions ov_eval/src/live_align_trajectory.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,137 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
* Copyright (C) 2021 Patrick Geneva
* Copyright (C) 2021 Guoquan Huang
* Copyright (C) 2021 OpenVINS Contributors
* Copyright (C) 2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/

#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>

#include "alignment/AlignTrajectory.h"
#include "alignment/AlignUtils.h"
#include "utils/Colors.h"
#include "utils/Loader.h"
#include "utils/Math.h"

ros::Publisher pub_path;
void align_and_publish(const nav_msgs::Path::ConstPtr &msg);
std::vector<double> times_gt;
std::vector<Eigen::Matrix<double, 7, 1>> poses_gt;
std::string alignment_type;

int main(int argc, char **argv) {

// Create ros node
ros::init(argc, argv, "live_align_trajectory");
ros::NodeHandle nh("~");

// Load what type of alignment we should use
nh.param<std::string>("alignment_type", alignment_type, "posyaw");

// If we don't have it, or it is empty then error
if (!nh.hasParam("path_gt")) {
ROS_ERROR("[LOAD]: Please provide a groundtruth file path!!!");
std::exit(EXIT_FAILURE);
}

// Load our groundtruth from file
std::string path_to_gt;
nh.param<std::string>("path_gt", path_to_gt, "");
boost::filesystem::path infolder(path_to_gt);
if (infolder.extension() == ".csv") {
std::vector<Eigen::Matrix3d> cov_ori_temp, cov_pos_temp;
ov_eval::Loader::load_data_csv(path_to_gt, times_gt, poses_gt, cov_ori_temp, cov_pos_temp);
} else {
std::vector<Eigen::Matrix3d> cov_ori_temp, cov_pos_temp;
ov_eval::Loader::load_data(path_to_gt, times_gt, poses_gt, cov_ori_temp, cov_pos_temp);
}

// 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);
ROS_INFO("Subscribing: %s", sub.getTopic().c_str());
ROS_INFO("Publishing: %s", pub_path.getTopic().c_str());

// Spin
ros::spin();
}

void align_and_publish(const nav_msgs::Path::ConstPtr &msg) {

// Convert the message into correct vector format
// tx ty tz qx qy qz qw
std::vector<double> times_temp;
std::vector<Eigen::Matrix<double, 7, 1>> poses_temp;
for (auto const &pose : msg->poses) {
times_temp.push_back(pose.header.stamp.toSec());
Eigen::Matrix<double, 7, 1> pose_tmp;
pose_tmp << pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w;
poses_temp.push_back(pose_tmp);
}

// Intersect timestamps
std::vector<double> gt_times_temp = times_gt;
std::vector<Eigen::Matrix<double, 7, 1>> gt_poses_temp = poses_gt;
ov_eval::AlignUtils::perform_association(0, 0.02, times_temp, gt_times_temp, poses_temp, gt_poses_temp);

// Return failure if we didn't have any common timestamps
if (poses_temp.size() < 3) {
printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
return;
}

// Perform alignment of the trajectories
Eigen::Matrix3d R_ESTtoGT;
Eigen::Vector3d t_ESTinGT;
double s_ESTtoGT;
ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_type);
Eigen::Vector4d q_ESTtoGT = ov_eval::Math::rot_2_quat(R_ESTtoGT);
printf("[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);

// Finally lets calculate the aligned trajectories
// TODO: maybe in the future we could live publish trajectory errors...
// NOTE: We downsample the number of poses as needed to prevent rviz crashes
// NOTE: https://github.com/ros-visualization/rviz/issues/1107
nav_msgs::Path arr_groundtruth;
arr_groundtruth.header = msg->header;
for (size_t i = 0; i < gt_times_temp.size(); i += std::floor(gt_times_temp.size() / 16384.0) + 1) {

// Convert into the correct frame
double timestamp = gt_times_temp.at(i);
Eigen::Matrix<double, 7, 1> pose_inGT = gt_poses_temp.at(i);
Eigen::Vector3d pos_IinEST = R_ESTtoGT.transpose() * (pose_inGT.block(0, 0, 3, 1) - t_ESTinGT) / s_ESTtoGT;
Eigen::Vector4d quat_ESTtoI = ov_eval::Math::quat_multiply(pose_inGT.block(3, 0, 4, 1), q_ESTtoGT);
// Finally push back
geometry_msgs::PoseStamped posetemp;
posetemp.header = msg->header;
posetemp.header.stamp = ros::Time(timestamp);
posetemp.pose.orientation.x = quat_ESTtoI(0);
posetemp.pose.orientation.y = quat_ESTtoI(1);
posetemp.pose.orientation.z = quat_ESTtoI(2);
posetemp.pose.orientation.w = quat_ESTtoI(3);
posetemp.pose.position.x = pos_IinEST(0);
posetemp.pose.position.y = pos_IinEST(1);
posetemp.pose.position.z = pos_IinEST(2);
arr_groundtruth.poses.push_back(posetemp);
}
pub_path.publish(arr_groundtruth);
}
2 changes: 1 addition & 1 deletion ov_eval/src/plot_trajectories.cpp
Original file line number Diff line number Diff line change
@@ -114,7 +114,7 @@ int main(int argc, char **argv) {
ov_eval::AlignUtils::perform_association(0, 0.02, times_temp, gt_times_temp, poses_temp, gt_poses_temp);

// Return failure if we didn't have any common timestamps
if (poses_temp.size() < 2) {
if (poses_temp.size() < 3) {
printf(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
printf(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
std::exit(EXIT_FAILURE);
73 changes: 73 additions & 0 deletions ov_eval/src/utils/Loader.cpp
Original file line number Diff line number Diff line change
@@ -106,6 +106,79 @@ void Loader::load_data(std::string path_traj, std::vector<double> &times, std::v
// printf("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str());
}


void Loader::load_data_csv(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos) {


// Try to open our trajectory file
std::ifstream file(path_traj);
if (!file.is_open()) {
printf(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
std::exit(EXIT_FAILURE);
}

// Loop through each line of this file
std::string current_line;
while (std::getline(file, current_line)) {

// Skip if we start with a comment
if (!current_line.find("#"))
continue;

// Loop variables
int i = 0;
std::istringstream s(current_line);
std::string field;
Eigen::Matrix<double, 20, 1> data;

// Loop through this line (groundtruth state [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel])
while (std::getline(s, field, ',')) {
// Skip if empty
if (field.empty() || i >= data.rows())
continue;
// save the data to our vector
data(i) = std::atof(field.c_str());
i++;
}

// Only a valid line if we have all the parameters
// Times are in nanoseconds -> convert to seconds
// Our "fixed" state vector from the ETH GT format [q,p,v,bg,ba]
if (i >= 8) {
times.push_back(1e-9 * data(0));
Eigen::Matrix<double, 7, 1> imustate;
imustate(0, 0) = data(1, 0); // pos
imustate(1, 0) = data(2, 0);
imustate(2, 0) = data(3, 0);
imustate(3, 0) = data(5, 0); // quat (xyzw)
imustate(4, 0) = data(6, 0);
imustate(5, 0) = data(7, 0);
imustate(6, 0) = data(4, 0);
poses.push_back(imustate);
}
}

// Finally close the file
file.close();

// Error if we don't have any data
if (times.empty()) {
printf(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
std::exit(EXIT_FAILURE);
}

// Assert that they are all equal
if (times.size() != poses.size()) {
printf(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
printf(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
std::exit(EXIT_FAILURE);
}

}

void Loader::load_simulation(std::string path, std::vector<Eigen::VectorXd> &values) {

// Try to open our trajectory file
11 changes: 11 additions & 0 deletions ov_eval/src/utils/Loader.h
Original file line number Diff line number Diff line change
@@ -50,6 +50,17 @@ class Loader {
static void load_data(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos);

/**
* @brief This will load *comma* separated trajectory into memory (ASL/ETH format)
* @param path_traj Path to the trajectory file that we want to read in.
* @param times Timesteps in seconds for each pose
* @param poses Pose at every timestep [pos,quat]
* @param cov_ori Vector of orientation covariances at each timestep (empty if we can't load)
* @param cov_pos Vector of position covariances at each timestep (empty if we can't load)
*/
static void load_data_csv(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos);

/**
* @brief Load an arbitrary sized row of *space* separated values, used for our simulation
* @param path Path to our text file to load
10 changes: 9 additions & 1 deletion ov_msckf/launch/pgeneva_ros_eth.launch
Original file line number Diff line number Diff line change
@@ -4,7 +4,9 @@
<arg name="max_cameras" default="2" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
<arg name="bag" default="/home/patrick/datasets/eth/V1_01_easy.bag" />
<arg name="dataset" default="V1_02_medium" /> <!-- V1_01_easy, V1_02_medium, V2_02_medium -->
<arg name="bag" default="/home/patrick/datasets/eth/$(arg dataset).bag" />
<arg name="bag_gt" default="$(find ov_data)/euroc_mav/$(arg dataset).csv" />

<!-- imu starting thresholds -->
<arg name="init_window_time" default="0.75" />
@@ -125,6 +127,12 @@
<!-- play the dataset -->
<node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) $(arg bag)" required="true"/>

<!-- path viz of aligned gt -->
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
<param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="$(arg bag_gt)" />
</node>

<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
8 changes: 8 additions & 0 deletions ov_msckf/launch/pgeneva_ros_kaist.launch
Original file line number Diff line number Diff line change
@@ -10,6 +10,8 @@
<arg name="path_est" default="/tmp/traj_estimate.txt" />
<arg name="path_time" default="/tmp/traj_timing.txt" />

<!-- groundtruth information -->
<arg name="bag_gt" default="$(find ov_data)/kaist/urban39.csv" /> <!-- urban28, urban38, urban39 -->

<!-- MASTER NODE! -->
<!-- <node name="run_subscribe_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true" launch-prefix="gdb -ex run &#45;&#45;args">-->
@@ -123,6 +125,12 @@

</node>

<!-- path viz of aligned gt -->
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
<param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="$(arg bag_gt)" />
</node>

<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
10 changes: 9 additions & 1 deletion ov_msckf/launch/pgeneva_ros_tum.launch
Original file line number Diff line number Diff line change
@@ -4,7 +4,9 @@
<arg name="max_cameras" default="1" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="0" />
<arg name="bag" default="/home/patrick/datasets/tum/dataset-room1_512_16.bag" />
<arg name="dataset" default="dataset-room1_512_16" /> <!-- dataset-room1_512_16, dataset-room3_512_16 -->
<arg name="bag" default="/home/patrick/datasets/tum/$(arg dataset).bag" />
<arg name="bag_gt" default="$(find ov_data)/tum_vi/$(arg dataset).csv" />

<!-- imu starting thresholds -->
<arg name="init_window_time" default="1.0" />
@@ -119,6 +121,12 @@
<!-- play the dataset -->
<node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) $(arg bag)" required="true"/>

<!-- path viz of aligned gt -->
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
<param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="$(arg bag_gt)" />
</node>

<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen">
12 changes: 10 additions & 2 deletions ov_msckf/launch/pgeneva_ros_uzhfpv.launch
Original file line number Diff line number Diff line change
@@ -1,11 +1,13 @@
<launch>

<!-- mono or stereo and what ros bag to play -->
<arg name="max_cameras" default="2" />
<arg name="max_cameras" default="1" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="0" />
<arg name="bag_durr" default="999999" />
<arg name="bag" default="/media/patrick/RPNG\ FLASH\ 2/uzhfpv_newer/indoor_45_2_snapdragon_with_gt.bag" />
<arg name="dataset" default="indoor_45_4_snapdragon_with_gt" /> <!-- indoor_45_2_snapdragon_with_gt, indoor_45_4_snapdragon_with_gt -->
<arg name="bag" default="/media/patrick/RPNG\ FLASH\ 2/uzhfpv_newer/$(arg dataset).bag" />
<arg name="bag_gt" default="$(find ov_data)/uzh_fpv/$(arg dataset).txt" />

<!-- what configuration mode we are in -->
<!-- 0: indoor forward facing -->
@@ -203,6 +205,12 @@
<!-- play the dataset -->
<node pkg="rosbag" type="play" name="rosbag" args="-d 1 -s $(arg bag_start) -u $(arg bag_durr) $(arg bag)" required="true"/>

<!-- path viz of aligned gt -->
<node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
<param name="alignment_type" type="str" value="posyaw" />
<param name="path_gt" type="str" value="$(arg bag_gt)" />
</node>

<!-- record the trajectory if enabled -->
<group if="$(arg dosave)">
<node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen">
5 changes: 3 additions & 2 deletions ov_msckf/launch/pgeneva_serial_eth.launch
Original file line number Diff line number Diff line change
@@ -4,9 +4,10 @@
<!-- mono or stereo and what ros bag to play -->
<arg name="max_cameras" default="2" />
<arg name="use_stereo" default="true" />
<arg name="dataset" default="V1_02_medium" /> <!-- V1_01_easy, V1_02_medium, V2_02_medium -->
<arg name="bag_start" default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
<arg name="bag" default="/home/patrick/datasets/eth/V1_01_easy.bag" />
<arg name="bag_gt" default="$(find ov_data)/euroc_mav/V1_01_easy.csv" /> <!-- $(find ov_data)/euroc_mav/V1_01_easy.csv -->
<arg name="bag" default="/home/patrick/datasets/eth/$(arg dataset).bag" />
<arg name="bag_gt" default="$(find ov_data)/euroc_mav/$(arg dataset).csv" />

<!-- imu starting thresholds -->
<arg name="init_window_time" default="0.75" />
5 changes: 3 additions & 2 deletions ov_msckf/launch/pgeneva_serial_tum.launch
Original file line number Diff line number Diff line change
@@ -4,8 +4,9 @@
<arg name="max_cameras" default="1" />
<arg name="use_stereo" default="true" />
<arg name="bag_start" default="4" />
<arg name="bag" default="/home/patrick/datasets/tum/dataset-room1_512_16.bag" />
<arg name="bag_gt" default="$(find ov_data)/tum_vi/dataset-room1_512_16.csv" /> <!-- $(find ov_data)/tum_vi/dataset-room1_512_16.csv -->
<arg name="dataset" default="dataset-room1_512_16" /> <!-- dataset-room1_512_16, dataset-room3_512_16 -->
<arg name="bag" default="/home/patrick/datasets/tum/$(arg dataset).bag" />
<arg name="bag_gt" default="$(find ov_data)/tum_vi/$(arg dataset).csv" />

<!-- imu starting thresholds -->
<arg name="init_window_time" default="1.0" />