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

642
ov_init/src/test_dynamic_mle.cpp Normal file
View File

@@ -0,0 +1,642 @@
/*
* 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 <ceres/ceres.h>
#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 "utils/colors.h"
#include "utils/sensor_data.h"
#include "ceres/Factor_GenericPrior.h"
#include "ceres/Factor_ImageReprojCalib.h"
#include "ceres/Factor_ImuCPIv1.h"
#include "ceres/State_JPLQuatLocal.h"
#include "init/InertialInitializerOptions.h"
#include "sim/Simulator.h"
#include "utils/helper.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); }
// 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_mle");
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);
//===================================================================================
//===================================================================================
//===================================================================================
// Ceres problem stuff
// NOTE: By default the problem takes ownership of the memory
ceres::Problem problem;
// Our system states (map from time to index)
std::map<double, int> map_states;
std::vector<double *> ceres_vars_ori;
std::vector<double *> ceres_vars_pos;
std::vector<double *> ceres_vars_vel;
std::vector<double *> ceres_vars_bias_g;
std::vector<double *> ceres_vars_bias_a;
// Feature states (3dof p_FinG)
std::map<size_t, int> map_features;
std::vector<double *> ceres_vars_feat;
typedef std::vector<std::pair<Factor_ImageReprojCalib *, std::vector<double *>>> factpair;
std::map<size_t, factpair> map_features_delayed; // only valid as long as problem is
// Setup extrinsic calibration q_ItoC, p_IinC (map from camera id to index)
std::map<size_t, int> map_calib_cam2imu;
std::vector<double *> ceres_vars_calib_cam2imu_ori;
std::vector<double *> ceres_vars_calib_cam2imu_pos;
// Setup intrinsic calibration focal, center, distortion (map from camera id to index)
std::map<size_t, int> map_calib_cam;
std::vector<double *> ceres_vars_calib_cam_intrinsics;
// Set the optimization settings
// NOTE: We use dense schur since after eliminating features we have a dense problem
// NOTE: http://ceres-solver.org/solving_faqs.html#solving
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
// options.linear_solver_type = ceres::SPARSE_SCHUR;
// options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
options.num_threads = 6;
options.max_solver_time_in_seconds = 9999;
options.max_num_iterations = 20;
options.minimizer_progress_to_stdout = true;
// options.use_nonmonotonic_steps = true;
// options.function_tolerance = 1e-5;
// options.gradient_tolerance = 1e-4 * options.function_tolerance;
// DEBUG: check analytical jacobians using ceres finite differences
// options.check_gradients = true;
// options.gradient_check_relative_precision = 1e-4;
// options.gradient_check_numeric_derivative_relative_step_size = 1e-8;
//===================================================================================
//===================================================================================
//===================================================================================
// Random number generator used to perturb the groundtruth features
// NOTE: It seems that if this too large it can really prevent good optimization
// NOTE: Values greater 5cm seems to fail, not sure if this is reasonable or not...
std::mt19937 gen_state_perturb(params.sim_seed_preturb);
double feat_noise = 0.05; // meters
// Buffer our camera image
double buffer_timecam = -1;
double buffer_timecam_last = -1;
std::vector<int> buffer_camids;
std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>> buffer_feats;
auto imu_readings = std::make_shared<std::vector<ov_core::ImuData>>();
// Simulate!
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) {
if (buffer_timecam != -1) {
// Get our predicted state at the requested camera timestep
double timestamp_k = buffer_timecam_last;
double timestamp_k1 = buffer_timecam;
Eigen::Matrix<double, 16, 1> state_k1;
std::shared_ptr<ov_core::CpiV1> cpi = nullptr;
if (map_states.empty()) {
// Add the first ever pose to the problem
// TODO: do not initialize from the groundtruth pose
double time1 = timestamp_k1 + sim.get_true_parameters().calib_camimu_dt;
Eigen::Matrix<double, 17, 1> gt_imustate;
assert(sim.get_state(time1, gt_imustate));
state_k1 = gt_imustate.block(1, 0, 16, 1);
} else {
// Get our previous state timestamp (newest time) and biases to integrate with
assert(timestamp_k != -1);
// timestamp_k = map_states.rbegin()->first;
Eigen::Vector4d quat_k;
for (int i = 0; i < 4; i++) {
quat_k(i) = ceres_vars_ori.at(map_states.at(timestamp_k))[i];
}
Eigen::Vector3d pos_k, vel_k, bias_g_k, bias_a_k;
for (int i = 0; i < 3; i++) {
pos_k(i) = ceres_vars_pos.at(map_states.at(timestamp_k))[i];
vel_k(i) = ceres_vars_vel.at(map_states.at(timestamp_k))[i];
bias_g_k(i) = ceres_vars_bias_g.at(map_states.at(timestamp_k))[i];
bias_a_k(i) = ceres_vars_bias_a.at(map_states.at(timestamp_k))[i];
}
Eigen::Vector3d gravity;
gravity << 0.0, 0.0, params.gravity_mag;
// Preintegrate from previous state
// Then append a new state with preintegration factor
// TODO: estimate the timeoffset? don't use the true?
double time0 = timestamp_k + sim.get_true_parameters().calib_camimu_dt;
double time1 = timestamp_k1 + sim.get_true_parameters().calib_camimu_dt;
auto readings = InitializerHelper::select_imu_readings(*imu_readings, time0, time1);
cpi = std::make_shared<ov_core::CpiV1>(params.sigma_w, params.sigma_wb, params.sigma_a, params.sigma_ab, false);
cpi->setLinearizationPoints(bias_g_k, bias_a_k, quat_k, gravity);
for (size_t k = 0; k < readings.size() - 1; k++) {
auto imu0 = readings.at(k);
auto imu1 = readings.at(k + 1);
cpi->feed_IMU(imu0.timestamp, imu1.timestamp, imu0.wm, imu0.am, imu1.wm, imu1.am);
}
assert(timestamp_k1 > timestamp_k);
// Compute the predicted state
state_k1.block(0, 0, 4, 1) = ov_core::quat_multiply(cpi->q_k2tau, quat_k);
state_k1.block(4, 0, 3, 1) =
pos_k + vel_k * cpi->DT - 0.5 * cpi->grav * std::pow(cpi->DT, 2) + ov_core::quat_2_Rot(quat_k).transpose() * cpi->alpha_tau;
state_k1.block(7, 0, 3, 1) = vel_k - cpi->grav * cpi->DT + ov_core::quat_2_Rot(quat_k).transpose() * cpi->beta_tau;
state_k1.block(10, 0, 3, 1) = bias_g_k;
state_k1.block(13, 0, 3, 1) = bias_a_k;
}
// ================================================================
// ADDING GRAPH STATE / ESTIMATES!
// ================================================================
// Load our state variables into our allocated state pointers
auto *var_ori = new double[4];
for (int i = 0; i < 4; i++) {
var_ori[i] = state_k1(0 + i, 0);
}
auto *var_pos = new double[3];
auto *var_vel = new double[3];
auto *var_bias_g = new double[3];
auto *var_bias_a = new double[3];
for (int i = 0; i < 3; i++) {
var_pos[i] = state_k1(4 + i, 0);
var_vel[i] = state_k1(7 + i, 0);
var_bias_g[i] = state_k1(10 + i, 0);
var_bias_a[i] = state_k1(13 + i, 0);
}
// Now actually create the parameter block in the ceres problem
auto ceres_jplquat = new State_JPLQuatLocal();
problem.AddParameterBlock(var_ori, 4, ceres_jplquat);
problem.AddParameterBlock(var_pos, 3);
problem.AddParameterBlock(var_vel, 3);
problem.AddParameterBlock(var_bias_g, 3);
problem.AddParameterBlock(var_bias_a, 3);
// Fix this first ever pose to constrain the problem
// On the Comparison of Gauge Freedom Handling in Optimization-Based Visual-Inertial State Estimation
// Zichao Zhang; Guillermo Gallego; Davide Scaramuzza
// https://ieeexplore.ieee.org/document/8354808
if (map_states.empty()) {
// Construct state and prior
Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
for (int i = 0; i < 4; i++) {
x_lin(0 + i) = var_ori[i];
}
for (int i = 0; i < 3; i++) {
x_lin(4 + i) = var_pos[i];
// x_lin(7 + i) = var_bias_g[i];
// x_lin(10 + i) = var_bias_a[i];
}
Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(4, 1);
Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(4, 4);
prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1e-8, 2); // 4dof unobservable yaw and position
// prior_Info.block(4, 4, 3, 3) *= 1.0 / std::pow(1e-1, 2); // bias_g prior
// prior_Info.block(7, 7, 3, 3) *= 1.0 / std::pow(1e-1, 2); // bias_a prior
// Construct state type and ceres parameter pointers
std::vector<std::string> x_types;
std::vector<double *> factor_params;
factor_params.push_back(var_ori);
x_types.emplace_back("quat_yaw");
factor_params.push_back(var_pos);
x_types.emplace_back("vec3");
// factor_params.push_back(var_bias_g);
// x_types.emplace_back("vec3");
// factor_params.push_back(var_bias_a);
// x_types.emplace_back("vec3");
// Append it to the problem
auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
problem.AddResidualBlock(factor_prior, nullptr, factor_params);
// problem.SetParameterBlockConstant(var_ori);
// problem.SetParameterBlockConstant(var_pos);
}
// Append to our historical vector of states
map_states.insert({timestamp_k1, (int)ceres_vars_ori.size()});
ceres_vars_ori.push_back(var_ori);
ceres_vars_pos.push_back(var_pos);
ceres_vars_vel.push_back(var_vel);
ceres_vars_bias_g.push_back(var_bias_g);
ceres_vars_bias_a.push_back(var_bias_a);
buffer_timecam_last = timestamp_k1;
// ================================================================
// ADDING GRAPH FACTORS!
// ================================================================
// Append the new IMU factor
if (cpi != nullptr) {
assert(timestamp_k != -1);
std::vector<double *> factor_params;
factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k)));
factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_bias_g.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_vel.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_bias_a.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
auto *factor_imu = new Factor_ImuCPIv1(cpi->DT, cpi->grav, cpi->alpha_tau, cpi->beta_tau, cpi->q_k2tau, cpi->b_a_lin,
cpi->b_w_lin, cpi->J_q, cpi->J_b, cpi->J_a, cpi->H_b, cpi->H_a, cpi->P_meas);
problem.AddResidualBlock(factor_imu, nullptr, factor_params);
}
// Then, append new feature observations factors seen from this frame (initialize features as needed)
// We first loop through each camera and for each we append measurements as needed
assert(buffer_camids.size() == buffer_feats.size());
for (size_t n = 0; n < buffer_camids.size(); n++) {
// First make sure we have calibration states added
int cam_id = buffer_camids.at(n);
if (map_calib_cam2imu.find(cam_id) == map_calib_cam2imu.end()) {
auto *var_calib_ori = new double[4];
for (int i = 0; i < 4; i++) {
var_calib_ori[i] = params.camera_extrinsics.at(cam_id)(0 + i, 0);
}
auto *var_calib_pos = new double[3];
for (int i = 0; i < 3; i++) {
var_calib_pos[i] = params.camera_extrinsics.at(cam_id)(4 + i, 0);
}
auto ceres_calib_jplquat = new State_JPLQuatLocal();
problem.AddParameterBlock(var_calib_ori, 4, ceres_calib_jplquat);
problem.AddParameterBlock(var_calib_pos, 3);
map_calib_cam2imu.insert({cam_id, (int)ceres_vars_calib_cam2imu_ori.size()});
ceres_vars_calib_cam2imu_ori.push_back(var_calib_ori);
ceres_vars_calib_cam2imu_pos.push_back(var_calib_pos);
// Construct state and prior
Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(7, 1);
for (int i = 0; i < 4; i++) {
x_lin(0 + i) = var_calib_ori[i];
}
for (int i = 0; i < 3; i++) {
x_lin(4 + i) = var_calib_pos[i];
}
Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(6, 1);
Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(6, 6);
prior_Info.block(0, 0, 3, 3) *= 1.0 / std::pow(0.001, 2);
prior_Info.block(3, 3, 3, 3) *= 1.0 / std::pow(0.01, 2);
// Construct state type and ceres parameter pointers
std::vector<std::string> x_types;
std::vector<double *> factor_params;
factor_params.push_back(var_calib_ori);
x_types.emplace_back("quat");
factor_params.push_back(var_calib_pos);
x_types.emplace_back("vec3");
if (!params.init_dyn_mle_opt_calib) {
problem.SetParameterBlockConstant(var_calib_ori);
problem.SetParameterBlockConstant(var_calib_pos);
} else {
auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
problem.AddResidualBlock(factor_prior, nullptr, factor_params);
}
}
bool is_fisheye = (std::dynamic_pointer_cast<ov_core::CamEqui>(params.camera_intrinsics.at(cam_id)) != nullptr);
if (map_calib_cam.find(cam_id) == map_calib_cam.end()) {
auto *var_calib_cam = new double[8];
for (int i = 0; i < 8; i++) {
var_calib_cam[i] = params.camera_intrinsics.at(cam_id)->get_value()(i, 0);
}
problem.AddParameterBlock(var_calib_cam, 8);
map_calib_cam.insert({cam_id, (int)ceres_vars_calib_cam_intrinsics.size()});
ceres_vars_calib_cam_intrinsics.push_back(var_calib_cam);
// Construct state and prior
Eigen::MatrixXd x_lin = Eigen::MatrixXd::Zero(8, 1);
for (int i = 0; i < 8; i++) {
x_lin(0 + i) = var_calib_cam[i];
}
Eigen::MatrixXd prior_grad = Eigen::MatrixXd::Zero(8, 1);
Eigen::MatrixXd prior_Info = Eigen::MatrixXd::Identity(8, 8);
prior_Info.block(0, 0, 4, 4) *= 1.0 / std::pow(1.0, 2);
prior_Info.block(4, 4, 4, 4) *= 1.0 / std::pow(0.005, 2);
// Construct state type and ceres parameter pointers
std::vector<std::string> x_types;
std::vector<double *> factor_params;
factor_params.push_back(var_calib_cam);
x_types.emplace_back("vec8");
if (!params.init_dyn_mle_opt_calib) {
problem.SetParameterBlockConstant(var_calib_cam);
} else {
auto *factor_prior = new Factor_GenericPrior(x_lin, x_types, prior_Info, prior_grad);
problem.AddResidualBlock(factor_prior, nullptr, factor_params);
}
}
// Now loop through each feature observed at this k+1 timestamp
for (auto const &feat : buffer_feats.at(n)) {
size_t featid = feat.first;
Eigen::Vector2d uv_raw;
uv_raw << feat.second(0), feat.second(1);
// Next make sure that we have the feature state
// Normally, we should need to triangulate this once we have enough measurements
// TODO: do not initialize features from the groundtruth map
if (map_features.find(featid) == map_features.end()) {
assert(params.use_stereo);
Eigen::Vector3d p_FinG = sim.get_map().at(featid);
auto *var_feat = new double[3];
for (int i = 0; i < 3; i++) {
std::normal_distribution<double> w(0, 1);
var_feat[i] = p_FinG(i) + feat_noise * w(gen_state_perturb);
}
map_features.insert({featid, (int)ceres_vars_feat.size()});
map_features_delayed.insert({featid, factpair()});
ceres_vars_feat.push_back(var_feat);
}
// Factor parameters it is a function of
std::vector<double *> factor_params;
factor_params.push_back(ceres_vars_ori.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_pos.at(map_states.at(timestamp_k1)));
factor_params.push_back(ceres_vars_feat.at(map_features.at(featid)));
factor_params.push_back(ceres_vars_calib_cam2imu_ori.at(map_calib_cam2imu.at(cam_id)));
factor_params.push_back(ceres_vars_calib_cam2imu_pos.at(map_calib_cam2imu.at(cam_id)));
factor_params.push_back(ceres_vars_calib_cam_intrinsics.at(map_calib_cam.at(cam_id)));
auto *factor_pinhole = new Factor_ImageReprojCalib(uv_raw, params.sigma_pix, is_fisheye);
if (map_features_delayed.find(featid) != map_features_delayed.end()) {
map_features_delayed.at(featid).push_back({factor_pinhole, factor_params});
} else {
ceres::LossFunction *loss_function = nullptr;
// ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
}
}
}
}
buffer_timecam = time_cam;
buffer_camids = camids;
buffer_feats = feats;
// If a delayed feature has enough measurements we can add it to the problem!
// We will wait for enough observations before adding the parameter
// This should make it much more stable since the parameter / optimization is more constrained
size_t min_num_meas_to_optimize = 10;
auto it0 = map_features_delayed.begin();
while (it0 != map_features_delayed.end()) {
size_t featid = (*it0).first;
auto factors = (*it0).second;
if (factors.size() >= min_num_meas_to_optimize) {
problem.AddParameterBlock(ceres_vars_feat.at(map_features.at(featid)), 3);
for (auto const &factorpair : factors) {
auto factor_pinhole = factorpair.first;
auto factor_params = factorpair.second;
ceres::LossFunction *loss_function = nullptr;
// ceres::LossFunction *loss_function = new ceres::CauchyLoss(1.0);
problem.AddResidualBlock(factor_pinhole, loss_function, factor_params);
}
it0 = map_features_delayed.erase(it0);
} else {
it0++;
}
}
// ================================================================
// PERFORM ACTUAL OPTIMIZATION!
// ================================================================
// We can try to optimize every few frames, but this can cause the IMU to drift
// Thus this can't be too large, nor too small to reduce the computation
if (map_states.size() % 10 == 0 && map_states.size() > min_num_meas_to_optimize) {
// COMPUTE: error before optimization
double error_before = 0.0;
if (!map_features.empty()) {
for (auto &feat : map_features) {
Eigen::Vector3d pos1, pos2;
pos1 << ceres_vars_feat.at(feat.second)[0], ceres_vars_feat.at(feat.second)[1], ceres_vars_feat.at(feat.second)[2];
pos2 = sim.get_map().at(feat.first);
error_before += (pos2 - pos1).norm();
}
error_before /= (double)map_features.size();
}
// Optimize the ceres graph
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
PRINT_INFO("[CERES]: %s\n", summary.message.c_str());
PRINT_INFO("[CERES]: %d iterations | %zu states, %zu features | %d parameters and %d residuals | cost %.4e => %.4e\n",
(int)summary.iterations.size(), map_states.size(), map_features.size(), summary.num_parameters, summary.num_residuals,
summary.initial_cost, summary.final_cost);
// COMPUTE: error after optimization
double error_after = 0.0;
if (!map_features.empty()) {
for (auto &feat : map_features) {
Eigen::Vector3d pos1, pos2;
pos1 << ceres_vars_feat.at(feat.second)[0], ceres_vars_feat.at(feat.second)[1], ceres_vars_feat.at(feat.second)[2];
pos2 = sim.get_map().at(feat.first);
error_after += (pos2 - pos1).norm();
}
error_after /= (double)map_features.size();
}
PRINT_INFO("[CERES]: map error %.4f => %.4f (meters) for %zu features\n", error_before, error_after, map_features.size());
// Print feature positions to console
// https://gist.github.com/goldbattle/177a6b2cccb4b4208e687b0abae4bc9f
// for (auto &feat : map_features) {
// size_t featid = feat.first;
// std::cout << featid << ",";
// std::cout << ceres_vars_feat[map_features[featid]][0] << "," << ceres_vars_feat[map_features[featid]][1] << ","
// << ceres_vars_feat[map_features[featid]][2] << ",";
// std::cout << sim.get_map().at(featid)[0] << "," << sim.get_map().at(featid)[1] << "," << sim.get_map().at(featid)[2] <<
// ","; std::cout << map_features_num_meas[feat.first] << ";" << std::endl;
// }
}
#if ROS_AVAILABLE == 1
// Visualize in RVIZ our current estimates and
if (map_states.size() % 1 == 0) {
// 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 &statepair : map_states) {
geometry_msgs::PoseStamped poseEST, poseGT;
poseEST.header.stamp = ros::Time(statepair.first);
poseEST.header.frame_id = "global";
poseEST.pose.orientation.x = ceres_vars_ori[statepair.second][0];
poseEST.pose.orientation.y = ceres_vars_ori[statepair.second][1];
poseEST.pose.orientation.z = ceres_vars_ori[statepair.second][2];
poseEST.pose.orientation.w = ceres_vars_ori[statepair.second][3];
poseEST.pose.position.x = ceres_vars_pos[statepair.second][0];
poseEST.pose.position.y = ceres_vars_pos[statepair.second][1];
poseEST.pose.position.z = ceres_vars_pos[statepair.second][2];
Eigen::Matrix<double, 17, 1> gt_imustate;
assert(sim.get_state(statepair.first + sim.get_true_parameters().calib_camimu_dt, gt_imustate));
poseGT.header.stamp = ros::Time(statepair.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 &featpair : map_features) {
if (map_features_delayed.find(featpair.first) != map_features_delayed.end())
continue;
geometry_msgs::Point32 p;
p.x = (float)ceres_vars_feat[map_features[featpair.first]][0];
p.y = (float)ceres_vars_feat[map_features[featpair.first]][1];
p.z = (float)ceres_vars_feat[map_features[featpair.first]][2];
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 * map_features.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 * map_features.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 : map_features) {
if (map_features_delayed.find(featpair.first) != map_features_delayed.end())
continue;
*out_x = (float)sim.get_map().at(featpair.first)(0); // no change in id since no tracker is used
++out_x;
*out_y = (float)sim.get_map().at(featpair.first)(1); // no change in id since no tracker is used
++out_y;
*out_z = (float)sim.get_map().at(featpair.first)(2); // no change in id since no tracker is used
++out_z;
}
pub_points_sim.publish(cloud);
}
#endif
}
}
// Done!
return EXIT_SUCCESS;
};