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

initial commit

Browse Source
This commit is contained in:
Иван Подмогильный
2022-08-05 08:23:25 +03:00
commit 5ecdc6abcf
387 changed files with 3010095 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

355
ov_init/src/test_dynamic_init.cpp Normal file
View File

@@ -0,0 +1,355 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
* Copyright (C) 2018-2022 Patrick Geneva
* Copyright (C) 2018-2022 Guoquan Huang
* Copyright (C) 2018-2022 OpenVINS Contributors
* Copyright (C) 2018-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 <cmath>
#include <csignal>
#include <deque>
#include <iomanip>
#include <sstream>
#include <unistd.h>
#include <vector>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem.hpp>
#if ROS_AVAILABLE == 1
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#endif
#include "dynamic/DynamicInitializer.h"
#include "init/InertialInitializerOptions.h"
#include "sim/Simulator.h"
#include "track/TrackSIM.h"
#include "utils/colors.h"
#include "utils/sensor_data.h"
using namespace ov_init;
// Define the function to be called when ctrl-c (SIGINT) is sent to process
void signal_callback_handler(int signum) { std::exit(signum); }
// taken from ov_eval/src/alignment/AlignUtils.h
static inline double get_best_yaw(const Eigen::Matrix<double, 3, 3> &C) {
double A = C(0, 1) - C(1, 0);
double B = C(0, 0) + C(1, 1);
// return M_PI_2 - atan2(B, A);
return atan2(A, B);
}
// taken from ov_eval/src/alignment/AlignUtils.h
void align_posyaw_single(const Eigen::Vector4d &q_es_0, const Eigen::Vector3d &p_es_0, const Eigen::Vector4d &q_gt_0,
const Eigen::Vector3d &p_gt_0, Eigen::Matrix3d &R, Eigen::Vector3d &t) {
Eigen::Matrix3d g_rot = ov_core::quat_2_Rot(q_gt_0).transpose();
Eigen::Matrix3d est_rot = ov_core::quat_2_Rot(q_es_0).transpose();
Eigen::Matrix3d C_R = est_rot * g_rot.transpose();
double theta = get_best_yaw(C_R);
R = ov_core::rot_z(theta);
t.noalias() = p_gt_0 - R * p_es_0;
}
// Main function
int main(int argc, char **argv) {
// Ensure we have a path, if the user passes it then we should use it
std::string config_path = "unset_path_to_config.yaml";
if (argc > 1) {
config_path = argv[1];
}
#if ROS_AVAILABLE == 1
// Launch our ros node
ros::init(argc, argv, "test_dynamic_init");
auto nh = std::make_shared<ros::NodeHandle>("~");
nh->param<std::string>("config_path", config_path, config_path);
// Topics to publish
auto pub_pathimu = nh->advertise<nav_msgs::Path>("/ov_msckf/pathimu", 2);
PRINT_DEBUG("Publishing: %s\n", pub_pathimu.getTopic().c_str());
auto pub_pathgt = nh->advertise<nav_msgs::Path>("/ov_msckf/pathgt", 2);
PRINT_DEBUG("Publishing: %s\n", pub_pathgt.getTopic().c_str());
auto pub_loop_point = nh->advertise<sensor_msgs::PointCloud>("/ov_msckf/loop_feats", 2);
PRINT_DEBUG("Publishing: %s\n", pub_loop_point.getTopic().c_str());
auto pub_points_sim = nh->advertise<sensor_msgs::PointCloud2>("/ov_msckf/points_sim", 2);
PRINT_DEBUG("Publishing: %s\n", pub_points_sim.getTopic().c_str());
#endif
// Load the config
auto parser = std::make_shared<ov_core::YamlParser>(config_path);
#if ROS_AVAILABLE == 1
parser->set_node_handler(nh);
#endif
// Verbosity
std::string verbosity = "INFO";
parser->parse_config("verbosity", verbosity);
ov_core::Printer::setPrintLevel(verbosity);
// Create the simulator
InertialInitializerOptions params;
params.print_and_load(parser);
params.print_and_load_simulation(parser);
if (!parser->successful()) {
PRINT_ERROR(RED "unable to parse all parameters, please fix\n" RESET);
std::exit(EXIT_FAILURE);
}
Simulator sim(params);
// Our initialization class objects
auto imu_readings = std::make_shared<std::vector<ov_core::ImuData>>();
auto tracker = std::make_shared<ov_core::TrackSIM>(params.camera_intrinsics, 0);
auto initializer = std::make_shared<DynamicInitializer>(params, tracker->get_feature_database(), imu_readings);
//===================================================================================
//===================================================================================
//===================================================================================
// Buffer our camera image
double buffer_timecam = -1;
std::vector<int> buffer_camids;
std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>> buffer_feats;
// Continue to simulate until we have processed all the measurements
signal(SIGINT, signal_callback_handler);
while (sim.ok()) {
// IMU: get the next simulated IMU measurement if we have it
ov_core::ImuData message_imu;
bool hasimu = sim.get_next_imu(message_imu.timestamp, message_imu.wm, message_imu.am);
if (hasimu) {
imu_readings->push_back(message_imu);
}
// CAM: get the next simulated camera uv measurements if we have them
double time_cam;
std::vector<int> camids;
std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>> feats;
bool hascam = sim.get_next_cam(time_cam, camids, feats);
if (hascam) {
// Pass to our feature database / tracker
if (buffer_timecam != -1) {
// Feed it
tracker->feed_measurement_simulation(buffer_timecam, buffer_camids, buffer_feats);
// Display the resulting tracks
// cv::Mat img_history;
// tracker->display_history(img_history, 255, 255, 0, 255, 255, 255);
// cv::imshow("Track History", img_history);
// cv::waitKey(1);
}
buffer_timecam = time_cam;
buffer_camids = camids;
buffer_feats = feats;
// Return states of our initializer
double timestamp = -1;
Eigen::MatrixXd covariance;
std::vector<std::shared_ptr<ov_type::Type>> order;
std::shared_ptr<ov_type::IMU> _imu;
std::map<double, std::shared_ptr<ov_type::PoseJPL>> _clones_IMU;
std::unordered_map<size_t, std::shared_ptr<ov_type::Landmark>> _features_SLAM;
std::unordered_map<size_t, std::shared_ptr<ov_type::PoseJPL>> _calib_IMUtoCAM;
std::unordered_map<size_t, std::shared_ptr<ov_type::Vec>> _cam_intrinsics;
// First we will try to make sure we have all the data required for our initialization
boost::posix_time::ptime rT1 = boost::posix_time::microsec_clock::local_time();
bool success =
initializer->initialize(timestamp, covariance, order, _imu, _clones_IMU, _features_SLAM, _calib_IMUtoCAM, _cam_intrinsics);
boost::posix_time::ptime rT2 = boost::posix_time::microsec_clock::local_time();
double time = (rT2 - rT1).total_microseconds() * 1e-6;
if (success) {
// Debug that we finished!
PRINT_INFO(GREEN "success! got initialized state information (%.4f seconds)\n" RESET, time);
// First lets align the groundtruth state with the IMU state
// NOTE: imu biases do not have to be corrected with the pos yaw alignment here...
Eigen::Matrix<double, 17, 1> gt_imu;
assert(sim.get_state(timestamp + sim.get_true_parameters().calib_camimu_dt, gt_imu));
Eigen::Matrix3d R_ESTtoGT_imu;
Eigen::Vector3d t_ESTinGT_imu;
align_posyaw_single(_imu->quat(), _imu->pos(), gt_imu.block(1, 0, 4, 1), gt_imu.block(5, 0, 3, 1), R_ESTtoGT_imu, t_ESTinGT_imu);
gt_imu.block(1, 0, 4, 1) = ov_core::quat_multiply(gt_imu.block(1, 0, 4, 1), ov_core::rot_2_quat(R_ESTtoGT_imu));
gt_imu.block(5, 0, 3, 1) = R_ESTtoGT_imu.transpose() * (gt_imu.block(5, 0, 3, 1) - t_ESTinGT_imu);
gt_imu.block(8, 0, 3, 1) = R_ESTtoGT_imu.transpose() * gt_imu.block(8, 0, 3, 1);
// Finally compute the error
Eigen::Matrix<double, 15, 1> err = Eigen::Matrix<double, 15, 1>::Zero();
Eigen::Matrix3d R_GtoI_gt = ov_core::quat_2_Rot(gt_imu.block(1, 0, 4, 1));
Eigen::Matrix3d R_GtoI_hat = _imu->Rot();
err.block(0, 0, 3, 1) = -ov_core::log_so3(R_GtoI_gt * R_GtoI_hat.transpose());
err.block(3, 0, 3, 1) = gt_imu.block(5, 0, 3, 1) - _imu->pos();
err.block(6, 0, 3, 1) = gt_imu.block(8, 0, 3, 1) - _imu->vel();
err.block(9, 0, 3, 1) = gt_imu.block(11, 0, 3, 1) - _imu->bias_g();
err.block(12, 0, 3, 1) = gt_imu.block(14, 0, 3, 1) - _imu->bias_a();
// debug print the error of the recovered IMU state
PRINT_INFO(REDPURPLE "e_ori = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f,%.3f (true) | %.3f,%.3f,%.3f,%.3f (est)\n" RESET,
180.0 / M_PI * err(0 + 0), 180.0 / M_PI * err(0 + 1), 180.0 / M_PI * err(0 + 2), gt_imu(1), gt_imu(2), gt_imu(3),
gt_imu(4), _imu->quat()(0), _imu->quat()(1), _imu->quat()(2), _imu->quat()(3));
PRINT_INFO(REDPURPLE "e_pos = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f (true) | %.3f,%.3f,%.3f (est)\n" RESET, err(3 + 0), err(3 + 1),
err(3 + 2), gt_imu(5), gt_imu(6), gt_imu(7), _imu->pos()(0), _imu->pos()(1), _imu->pos()(2));
PRINT_INFO(REDPURPLE "e_vel = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f (true) | %.3f,%.3f,%.3f (est)\n" RESET, err(6 + 0), err(6 + 1),
err(6 + 2), gt_imu(8), gt_imu(9), gt_imu(10), _imu->vel()(0), _imu->vel()(1), _imu->vel()(2));
PRINT_INFO(REDPURPLE "e_bias_g = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f (true) | %.3f,%.3f,%.3f (est)\n" RESET, err(9 + 0), err(9 + 1),
err(9 + 2), gt_imu(11), gt_imu(12), gt_imu(13), _imu->bias_g()(0), _imu->bias_g()(1), _imu->bias_g()(2));
PRINT_INFO(REDPURPLE "e_bias_a = %.3f,%.3f,%.3f | %.3f,%.3f,%.3f (true) | %.3f,%.3f,%.3f (est)\n" RESET, err(12 + 0), err(12 + 1),
err(12 + 2), gt_imu(14), gt_imu(15), gt_imu(16), _imu->bias_a()(0), _imu->bias_a()(1), _imu->bias_a()(2));
// calculate normalized estimation error squared
// the recovered error should be on the order of the state size (15 or 3 for marginals)
Eigen::MatrixXd information = covariance.inverse();
double nees_total = (err.transpose() * information * err)(0, 0);
double nees_ori = (err.block(0, 0, 3, 1).transpose() * information.block(0, 0, 3, 3) * err.block(0, 0, 3, 1))(0, 0);
double nees_pos = (err.block(3, 0, 3, 1).transpose() * information.block(3, 3, 3, 3) * err.block(3, 0, 3, 1))(0, 0);
double nees_vel = (err.block(6, 0, 3, 1).transpose() * information.block(6, 6, 3, 3) * err.block(6, 0, 3, 1))(0, 0);
double nees_bg = (err.block(9, 0, 3, 1).transpose() * information.block(9, 9, 3, 3) * err.block(9, 0, 3, 1))(0, 0);
double nees_ba = (err.block(12, 0, 3, 1).transpose() * information.block(12, 12, 3, 3) * err.block(12, 0, 3, 1))(0, 0);
PRINT_INFO(REDPURPLE "nees total = %.3f | ori = %.3f | pos = %.3f (ideal is 15 and 3)\n" RESET, nees_total, nees_ori, nees_pos);
PRINT_INFO(REDPURPLE "nees vel = %.3f | bg = %.3f | ba = %.3f (ideal 3)\n" RESET, nees_vel, nees_bg, nees_ba);
#if ROS_AVAILABLE == 1
// Align the groundtruth to the current estimate yaw
// Only use the first frame so we can see the drift
double oldestpose_time = -1;
std::shared_ptr<ov_type::PoseJPL> oldestpose = nullptr;
for (auto const &_pose : _clones_IMU) {
if (oldestpose == nullptr || _pose.first < oldestpose_time) {
oldestpose_time = _pose.first;
oldestpose = _pose.second;
}
}
assert(oldestpose != nullptr);
Eigen::Vector4d q_es_0, q_gt_0;
Eigen::Vector3d p_es_0, p_gt_0;
q_es_0 = oldestpose->quat();
p_es_0 = oldestpose->pos();
Eigen::Matrix<double, 17, 1> gt_imustate_0;
assert(sim.get_state(oldestpose_time + sim.get_true_parameters().calib_camimu_dt, gt_imustate_0));
q_gt_0 = gt_imustate_0.block(1, 0, 4, 1);
p_gt_0 = gt_imustate_0.block(5, 0, 3, 1);
Eigen::Matrix3d R_ESTtoGT;
Eigen::Vector3d t_ESTinGT;
align_posyaw_single(q_es_0, p_es_0, q_gt_0, p_gt_0, R_ESTtoGT, t_ESTinGT);
// Pose states
nav_msgs::Path arrEST, arrGT;
arrEST.header.stamp = ros::Time::now();
arrEST.header.frame_id = "global";
arrGT.header.stamp = ros::Time::now();
arrGT.header.frame_id = "global";
for (auto const &_pose : _clones_IMU) {
geometry_msgs::PoseStamped poseEST, poseGT;
poseEST.header.stamp = ros::Time(_pose.first);
poseEST.header.frame_id = "global";
poseEST.pose.orientation.x = _pose.second->quat()(0, 0);
poseEST.pose.orientation.y = _pose.second->quat()(1, 0);
poseEST.pose.orientation.z = _pose.second->quat()(2, 0);
poseEST.pose.orientation.w = _pose.second->quat()(3, 0);
poseEST.pose.position.x = _pose.second->pos()(0, 0);
poseEST.pose.position.y = _pose.second->pos()(1, 0);
poseEST.pose.position.z = _pose.second->pos()(2, 0);
Eigen::Matrix<double, 17, 1> gt_imustate;
assert(sim.get_state(_pose.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate));
gt_imustate.block(1, 0, 4, 1) = ov_core::quat_multiply(gt_imustate.block(1, 0, 4, 1), ov_core::rot_2_quat(R_ESTtoGT));
gt_imustate.block(5, 0, 3, 1) = R_ESTtoGT.transpose() * (gt_imustate.block(5, 0, 3, 1) - t_ESTinGT);
poseGT.header.stamp = ros::Time(_pose.first);
poseGT.header.frame_id = "global";
poseGT.pose.orientation.x = gt_imustate(1);
poseGT.pose.orientation.y = gt_imustate(2);
poseGT.pose.orientation.z = gt_imustate(3);
poseGT.pose.orientation.w = gt_imustate(4);
poseGT.pose.position.x = gt_imustate(5);
poseGT.pose.position.y = gt_imustate(6);
poseGT.pose.position.z = gt_imustate(7);
arrEST.poses.push_back(poseEST);
arrGT.poses.push_back(poseGT);
}
pub_pathimu.publish(arrEST);
pub_pathgt.publish(arrGT);
// Features ESTIMATES pointcloud
sensor_msgs::PointCloud point_cloud;
point_cloud.header.frame_id = "global";
point_cloud.header.stamp = ros::Time::now();
for (auto const &featpair : _features_SLAM) {
geometry_msgs::Point32 p;
p.x = (float)featpair.second->get_xyz(false)(0, 0);
p.y = (float)featpair.second->get_xyz(false)(1, 0);
p.z = (float)featpair.second->get_xyz(false)(2, 0);
point_cloud.points.push_back(p);
}
pub_loop_point.publish(point_cloud);
// Features GROUNDTRUTH pointcloud
sensor_msgs::PointCloud2 cloud;
cloud.header.frame_id = "global";
cloud.header.stamp = ros::Time::now();
cloud.width = 3 * _features_SLAM.size();
cloud.height = 1;
cloud.is_bigendian = false;
cloud.is_dense = false; // there may be invalid points
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(1, "xyz");
modifier.resize(3 * _features_SLAM.size());
sensor_msgs::PointCloud2Iterator<float> out_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> out_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> out_z(cloud, "z");
for (auto &featpair : _features_SLAM) {
// TrackSIM adds 1 to sim id if num_aruco is zero
Eigen::Vector3d feat = sim.get_map().at(featpair.first - 1);
feat = R_ESTtoGT.transpose() * (feat - t_ESTinGT);
*out_x = (float)feat(0);
++out_x;
*out_y = (float)feat(1);
++out_y;
*out_z = (float)feat(2);
++out_z;
}
pub_points_sim.publish(cloud);
#endif
// Wait for user approval
do {
cout << '\n' << "Press a key to continue...";
} while (cin.get() != '\n');
// Reset our tracker and simulator so we can try to init again
if (params.sim_do_perturbation) {
sim.perturb_parameters(params);
}
imu_readings = std::make_shared<std::vector<ov_core::ImuData>>();
tracker = std::make_shared<ov_core::TrackSIM>(params.camera_intrinsics, 0);
initializer = std::make_shared<DynamicInitializer>(params, tracker->get_feature_database(), imu_readings);
} else if (timestamp != -1) {
PRINT_INFO(RED "failed (%.4f seconds)\n\n" RESET, time);
}
}
}
// Done!
return EXIT_SUCCESS;
}