hjkim
82d3725779
FAST-LIVO2 dual/triple-camera VIO-LIO mapping workspace (ROS2 Humble),
including the N-camera port from Omni-LIVO, rpg_vikit multi-camera
parameter loader, and the um982_driver/gnss_comm packages staged here
ahead of RTK fusion work.
FAST-LIVO2 and rpg_vikit were previously separate git clones tracking
their own GitHub history (Robotic-Developer-Road org, humble/main
branches) — that history is preserved locally in
src/{FAST-LIVO2,rpg_vikit}/.git-github-backup (not pushed here) and
these two are now tracked flat as part of this repo going forward.
2858 lines
108 KiB
C++
Executable File
2858 lines
108 KiB
C++
Executable File
/*
|
|
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.
|
|
|
|
Developer: Chunran Zheng <zhengcr@connect.hku.hk>
|
|
|
|
For commercial use, please contact me at <zhengcr@connect.hku.hk> or
|
|
Prof. Fu Zhang at <fuzhang@hku.hk>.
|
|
|
|
This file is subject to the terms and conditions outlined in the 'LICENSE' file,
|
|
which is included as part of this source code package.
|
|
|
|
Multi-camera vectorization (N-camera joint ESIKF + Omni-LIVO's Cross-View
|
|
Temporal Migration / Adaptive Multi-View ESIKF) ported from Omni-LIVO — see
|
|
docs/OMNI_LIVO_DUAL_CAMERA_PORTING_PLAN.md Phase 4. Loop-closure hooks that
|
|
were declared but never implemented/called in Omni-LIVO itself
|
|
(resetAfterLoopClosure, applyTrajectoryTransformToVisualMap,
|
|
computeCrossCameraWarpMatrix, evaluateCrossCameraConsistency,
|
|
extractPatchSafely) were left out as dead code.
|
|
*/
|
|
|
|
#include "vio.h"
|
|
#include <algorithm>
|
|
#include <numeric>
|
|
|
|
using namespace Eigen;
|
|
|
|
VIOManager::VIOManager()
|
|
{
|
|
}
|
|
|
|
VIOManager::~VIOManager()
|
|
{
|
|
delete visual_submap;
|
|
for (auto &pair : warp_map) delete pair.second;
|
|
warp_map.clear();
|
|
for (auto &pair : feat_map) delete pair.second;
|
|
feat_map.clear();
|
|
}
|
|
|
|
size_t VIOManager::capVisualMap()
|
|
{
|
|
if (!map_sliding_en_) return 0;
|
|
if ((int)feat_map.size() <= max_visual_voxels_) return 0;
|
|
|
|
// Drop oldest voxels first (smallest creation_timestamp_).
|
|
std::vector<std::pair<double, VOXEL_LOCATION>> aged;
|
|
aged.reserve(feat_map.size());
|
|
for (const auto &kv : feat_map) { aged.emplace_back(kv.second->creation_timestamp_, kv.first); }
|
|
const size_t to_drop = feat_map.size() - (size_t)max_visual_voxels_;
|
|
std::nth_element(aged.begin(), aged.begin() + to_drop, aged.end(), [](const auto &a, const auto &b) { return a.first < b.first; });
|
|
|
|
size_t erased = 0;
|
|
for (size_t i = 0; i < to_drop; ++i)
|
|
{
|
|
auto it = feat_map.find(aged[i].second);
|
|
if (it != feat_map.end())
|
|
{
|
|
delete it->second; // ~VOXEL_POINTS frees VisualPoints + Features
|
|
feat_map.erase(it);
|
|
sub_feat_map.erase(aged[i].second);
|
|
++erased;
|
|
}
|
|
}
|
|
std::cout << "[ VIO ] Cap visual map: erased " << erased << " oldest voxels, remaining " << feat_map.size() << std::endl;
|
|
return erased;
|
|
}
|
|
|
|
void VIOManager::setImuToLidarExtrinsic(const V3D &transl, const M3D &rot)
|
|
{
|
|
Pli = -rot.transpose() * transl;
|
|
Rli = rot.transpose();
|
|
}
|
|
|
|
void VIOManager::setLidarToCameraExtrinsic(std::vector<std::vector<double>> &R, std::vector<std::vector<double>> &P)
|
|
{
|
|
Rcl_vec.resize(cams.size());
|
|
Pcl_vec.resize(cams.size());
|
|
for (size_t i = 0; i < cams.size(); i++)
|
|
{
|
|
Rcl_vec[i] << R[i][0], R[i][1], R[i][2],
|
|
R[i][3], R[i][4], R[i][5],
|
|
R[i][6], R[i][7], R[i][8];
|
|
Pcl_vec[i] << P[i][0], P[i][1], P[i][2];
|
|
}
|
|
}
|
|
|
|
void VIOManager::initializeVIO()
|
|
{
|
|
if (cams.empty())
|
|
{
|
|
std::cerr << "[VIOManager::initializeVIO] Error: no cameras!" << std::endl;
|
|
return;
|
|
}
|
|
// NOTE (inherited from Omni-LIVO): width/height are derived only from
|
|
// cams[0] and reused for every camera's image-buffer indexing throughout
|
|
// this file (getImagePatch, precomputeReferencePatches, updateState, ...).
|
|
// All cameras in a given deployment must therefore share the same
|
|
// processed resolution; a config with mismatched per-camera resolutions
|
|
// will silently misbehave. See porting plan risk list.
|
|
width = cams[0]->width();
|
|
height = cams[0]->height();
|
|
|
|
visual_submap = new SubSparseMap;
|
|
image_resize_factor = cams[0]->scale();
|
|
|
|
Rci_vec.resize(cams.size());
|
|
Rcl_vec.resize(cams.size());
|
|
Rcw_vec.resize(cams.size());
|
|
Pci_vec.resize(cams.size());
|
|
Pcl_vec.resize(cams.size());
|
|
Pcw_vec.resize(cams.size());
|
|
|
|
Jdphi_dR_vec.resize(cams.size());
|
|
Jdp_dt_vec.resize(cams.size());
|
|
Jdp_dR_vec.resize(cams.size());
|
|
|
|
prev_cov_scale_per_cam_.resize(cams.size(), 10.0);
|
|
prev_avg_error_per_cam_.resize(cams.size(), 5.0);
|
|
prev_n_meas_per_cam_.resize(cams.size(), 0);
|
|
|
|
if (state && state->inv_expo_time_per_cam.empty())
|
|
{
|
|
state->inv_expo_time_per_cam.resize(cams.size(), 1.0);
|
|
std::cout << "[VIO] Initialized " << cams.size() << " camera exposure parameters (inv_expo=1.0)" << std::endl;
|
|
}
|
|
if (state_propagat && state_propagat->inv_expo_time_per_cam.empty())
|
|
{
|
|
state_propagat->inv_expo_time_per_cam.resize(cams.size(), 1.0);
|
|
}
|
|
|
|
for (size_t i = 0; i < cams.size(); i++)
|
|
{
|
|
Rci_vec[i] = Rcl_vec[i] * Rli;
|
|
Pci_vec[i] = Rcl_vec[i] * Pli + Pcl_vec[i];
|
|
Jdphi_dR_vec[i] = Rci_vec[i];
|
|
Eigen::Vector3d Pic_i = -Rci_vec[i].transpose() * Pci_vec[i];
|
|
Eigen::Matrix3d tmp;
|
|
tmp << 0, -Pic_i.z(), Pic_i.y(),
|
|
Pic_i.z(), 0, -Pic_i.x(),
|
|
-Pic_i.y(), Pic_i.x(), 0;
|
|
Jdp_dR_vec[i] = -Rci_vec[i] * tmp;
|
|
}
|
|
|
|
if (grid_size > 10)
|
|
{
|
|
grid_n_width = ceil(static_cast<double>(width / grid_size));
|
|
grid_n_height = ceil(static_cast<double>(height / grid_size));
|
|
}
|
|
else
|
|
{
|
|
grid_size = static_cast<int>(height / grid_n_height);
|
|
grid_n_height = ceil(static_cast<double>(height / grid_size));
|
|
grid_n_width = ceil(static_cast<double>(width / grid_size));
|
|
}
|
|
length = grid_n_width * grid_n_height;
|
|
|
|
if (colmap_output_en)
|
|
{
|
|
fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), ios::out);
|
|
fout_colmap << "# Image list with two lines of data per image:\n";
|
|
fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n";
|
|
fout_colmap << "# POINTS2D[] as (X, Y, POINT3D_ID)\n";
|
|
fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), ios::out);
|
|
fout_camera << "# Camera list with one line of data per camera:\n";
|
|
fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n";
|
|
for (size_t i = 0; i < cams.size(); i++)
|
|
{
|
|
fout_camera << i + 1 << " PINHOLE " << cams[i]->width() << " " << cams[i]->height() << " " << std::fixed << std::setprecision(9) << cams[i]->fx() << " "
|
|
<< cams[i]->fy() << " " << cams[i]->cx() << " " << cams[i]->cy() << std::endl;
|
|
}
|
|
fout_camera.close();
|
|
}
|
|
|
|
// Placeholder sizing only — resetGrid() (called at the top of every
|
|
// processFrame()) resizes these to the actual multi-camera cell count
|
|
// (length * cams.size()) before they're used.
|
|
size_t total_cells = (size_t)length * std::max<size_t>(1, cams.size());
|
|
grid_num.resize(total_cells);
|
|
map_index.resize(total_cells);
|
|
map_dist.resize(total_cells);
|
|
update_flag.resize(total_cells);
|
|
scan_value.resize(total_cells);
|
|
|
|
patch_size_total = patch_size * patch_size;
|
|
patch_size_half = static_cast<int>(patch_size / 2);
|
|
patch_buffer.resize(patch_size_total);
|
|
warp_len = patch_size_total * patch_pyrimid_level;
|
|
border = (patch_size_half + 2) * 8;
|
|
|
|
retrieve_voxel_points.reserve(total_cells);
|
|
append_voxel_points.reserve(total_cells);
|
|
|
|
sub_feat_map.clear();
|
|
|
|
initializeCameraPhotoParams();
|
|
total_cross_camera_observations = 0;
|
|
successful_cross_camera_tracks = 0;
|
|
|
|
std::cout << "Cross-camera direct tracking " << (enable_cross_camera_tracking ? "enabled!" : "disabled!") << std::endl;
|
|
if (enable_dynamic_covariance_)
|
|
{
|
|
std::cout << "Dynamic covariance enabled! (warmup=" << dynamic_cov_warmup_frames << " frames, min=" << min_cov_scale << ", max=" << max_cov_scale
|
|
<< ")" << std::endl;
|
|
}
|
|
else
|
|
{
|
|
std::cout << "Dynamic covariance disabled!" << std::endl;
|
|
}
|
|
}
|
|
|
|
void VIOManager::initializeRaycast()
|
|
{
|
|
std::cout << "[ VIO ] Starting raycast initialization..." << std::endl;
|
|
raycast_en = true;
|
|
int num_cameras = cams.size();
|
|
if (num_cameras == 0)
|
|
{
|
|
std::cerr << "[ VIO ] No cameras available for raycast initialization!" << std::endl;
|
|
raycast_en = false;
|
|
return;
|
|
}
|
|
if (grid_size <= 0 || grid_n_height <= 0 || grid_n_width <= 0 || length <= 0)
|
|
{
|
|
std::cerr << "[ VIO ] Grid parameters not properly initialized: grid_size=" << grid_size << ", grid_n_height=" << grid_n_height
|
|
<< ", grid_n_width=" << grid_n_width << ", length=" << length << std::endl;
|
|
raycast_en = false;
|
|
return;
|
|
}
|
|
|
|
border_flag.clear();
|
|
rays_with_sample_points.clear();
|
|
border_flag.resize(num_cameras);
|
|
rays_with_sample_points.resize(num_cameras);
|
|
|
|
std::cout << "[ VIO ] Initializing raycast for " << num_cameras << " cameras: grid_size=" << grid_size << ", grid_n_height=" << grid_n_height
|
|
<< ", grid_n_width=" << grid_n_width << ", length=" << length << std::endl;
|
|
|
|
float d_min = 0.1;
|
|
float d_max = 3.0;
|
|
float step = 0.2;
|
|
int total_sample_points = 0;
|
|
for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++)
|
|
{
|
|
border_flag[cam_idx].resize(length, 0);
|
|
rays_with_sample_points[cam_idx].clear();
|
|
rays_with_sample_points[cam_idx].reserve(length);
|
|
|
|
int camera_sample_points = 0;
|
|
for (int grid_row = 1; grid_row <= grid_n_height; grid_row++)
|
|
{
|
|
for (int grid_col = 1; grid_col <= grid_n_width; grid_col++)
|
|
{
|
|
std::vector<V3D> SamplePointsEachGrid;
|
|
int index = (grid_row - 1) * grid_n_width + grid_col - 1;
|
|
|
|
if (grid_row == 1 || grid_col == 1 || grid_row == grid_n_height || grid_col == grid_n_width) border_flag[cam_idx][index] = 1;
|
|
|
|
int u = grid_size / 2 + (grid_col - 1) * grid_size;
|
|
int v = grid_size / 2 + (grid_row - 1) * grid_size;
|
|
|
|
for (float d_temp = d_min; d_temp <= d_max; d_temp += step)
|
|
{
|
|
V3D xyz;
|
|
try
|
|
{
|
|
xyz = cams[cam_idx]->cam2world(u, v);
|
|
xyz *= d_temp / xyz[2];
|
|
SamplePointsEachGrid.push_back(xyz);
|
|
camera_sample_points++;
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
std::cerr << "[ VIO ] Exception in camera " << cam_idx << " raycast computation: " << e.what() << std::endl;
|
|
}
|
|
}
|
|
rays_with_sample_points[cam_idx].push_back(SamplePointsEachGrid);
|
|
}
|
|
}
|
|
total_sample_points += camera_sample_points;
|
|
}
|
|
|
|
std::cout << "[ VIO ] Raycast initialization completed! Total sample points: " << total_sample_points << std::endl;
|
|
}
|
|
|
|
void VIOManager::resetGrid()
|
|
{
|
|
// Grid buffers are indexed per-camera-cell (camera_grid_offset[cam] +
|
|
// local_index in retrieveFromVisualSparseMap/generateVisualMapPoints), so
|
|
// they must be sized to all cameras' cells combined, not just one
|
|
// camera's `length`, or a second+ camera's writes go out of bounds.
|
|
size_t total_cells = (size_t)length * std::max<size_t>(1, cams.size());
|
|
|
|
grid_num.assign(total_cells, TYPE_UNKNOWN);
|
|
map_index.assign(total_cells, 0);
|
|
map_dist.assign(total_cells, 10000.0f);
|
|
update_flag.assign(total_cells, 0);
|
|
scan_value.assign(total_cells, 0.0f);
|
|
|
|
retrieve_voxel_points.clear();
|
|
retrieve_voxel_points.resize(total_cells);
|
|
|
|
append_voxel_points.clear();
|
|
append_voxel_points.resize(total_cells);
|
|
|
|
total_points = 0;
|
|
}
|
|
|
|
void VIOManager::computeProjectionJacobian(int cam_idx, V3D p, MD(2, 3) & J)
|
|
{
|
|
double fx_i = cams[cam_idx]->fx();
|
|
double fy_i = cams[cam_idx]->fy();
|
|
const double x = p[0];
|
|
const double y = p[1];
|
|
const double z_inv = 1.0 / p[2];
|
|
const double z_inv_2 = z_inv * z_inv;
|
|
|
|
J(0, 0) = fx_i * z_inv;
|
|
J(0, 1) = 0.0;
|
|
J(0, 2) = -fx_i * x * z_inv_2;
|
|
|
|
J(1, 0) = 0.0;
|
|
J(1, 1) = fy_i * z_inv;
|
|
J(1, 2) = -fy_i * y * z_inv_2;
|
|
}
|
|
|
|
void VIOManager::getImagePatch(cv::Mat img, V2D pc, float *patch_tmp, int level)
|
|
{
|
|
const float u_ref = pc[0];
|
|
const float v_ref = pc[1];
|
|
const int scale = (1 << level);
|
|
const int u_ref_i = floorf(pc[0] / scale) * scale;
|
|
const int v_ref_i = floorf(pc[1] / scale) * scale;
|
|
const float subpix_u_ref = (u_ref - u_ref_i) / scale;
|
|
const float subpix_v_ref = (v_ref - v_ref_i) / scale;
|
|
const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref);
|
|
const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref);
|
|
const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref;
|
|
const float w_ref_br = subpix_u_ref * subpix_v_ref;
|
|
for (int x = 0; x < patch_size; x++)
|
|
{
|
|
uint8_t *img_ptr = (uint8_t *)img.data + (v_ref_i - patch_size_half * scale + x * scale) * width + (u_ref_i - patch_size_half * scale);
|
|
for (int y = 0; y < patch_size; y++, img_ptr += scale)
|
|
{
|
|
patch_tmp[patch_size_total * level + x * patch_size + y] =
|
|
w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale];
|
|
}
|
|
}
|
|
}
|
|
|
|
void VIOManager::insertPointIntoVoxelMap(VisualPoint *pt_new)
|
|
{
|
|
V3D pt_w(pt_new->pos_[0], pt_new->pos_[1], pt_new->pos_[2]);
|
|
double voxel_size = 0.5;
|
|
float loc_xyz[3];
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
loc_xyz[j] = pt_w[j] / voxel_size;
|
|
if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; }
|
|
}
|
|
VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);
|
|
auto iter = feat_map.find(position);
|
|
if (iter != feat_map.end())
|
|
{
|
|
iter->second->voxel_points.push_back(pt_new);
|
|
iter->second->count++;
|
|
}
|
|
else
|
|
{
|
|
VOXEL_POINTS *ot = new VOXEL_POINTS(0);
|
|
ot->creation_timestamp_ = current_timestamp_;
|
|
ot->voxel_points.push_back(pt_new);
|
|
feat_map[position] = ot;
|
|
}
|
|
}
|
|
|
|
void VIOManager::getWarpMatrixAffineHomography(const vk::AbstractCamera &cam, const V2D &px_ref, const V3D &xyz_ref, const V3D &normal_ref,
|
|
const SE3<double> &T_cur_ref, const int level_ref, Matrix2d &A_cur_ref)
|
|
{
|
|
const V3D t = T_cur_ref.inverse().translation();
|
|
const Eigen::Matrix3d H_cur_ref = T_cur_ref.rotationMatrix() * (normal_ref.dot(xyz_ref) * Eigen::Matrix3d::Identity() - t * normal_ref.transpose());
|
|
const int kHalfPatchSize = 4;
|
|
V3D f_du_ref(cam.cam2world(px_ref + Eigen::Vector2d(kHalfPatchSize, 0) * (1 << level_ref)));
|
|
V3D f_dv_ref(cam.cam2world(px_ref + Eigen::Vector2d(0, kHalfPatchSize) * (1 << level_ref)));
|
|
const V3D f_cur(H_cur_ref * xyz_ref);
|
|
const V3D f_du_cur = H_cur_ref * f_du_ref;
|
|
const V3D f_dv_cur = H_cur_ref * f_dv_ref;
|
|
V2D px_cur(cam.world2cam(f_cur));
|
|
V2D px_du_cur(cam.world2cam(f_du_cur));
|
|
V2D px_dv_cur(cam.world2cam(f_dv_cur));
|
|
A_cur_ref.col(0) = (px_du_cur - px_cur) / kHalfPatchSize;
|
|
A_cur_ref.col(1) = (px_dv_cur - px_cur) / kHalfPatchSize;
|
|
}
|
|
|
|
void VIOManager::getWarpMatrixAffine(const vk::AbstractCamera &cam, const Vector2d &px_ref, const Vector3d &f_ref, const double depth_ref,
|
|
const SE3<double> &T_cur_ref, const int level_ref, const int pyramid_level, const int halfpatch_size,
|
|
Matrix2d &A_cur_ref)
|
|
{
|
|
const Vector3d xyz_ref(f_ref * depth_ref);
|
|
Vector3d xyz_du_ref(cam.cam2world(px_ref + Vector2d(halfpatch_size, 0) * (1 << level_ref) * (1 << pyramid_level)));
|
|
Vector3d xyz_dv_ref(cam.cam2world(px_ref + Vector2d(0, halfpatch_size) * (1 << level_ref) * (1 << pyramid_level)));
|
|
xyz_du_ref *= xyz_ref[2] / xyz_du_ref[2];
|
|
xyz_dv_ref *= xyz_ref[2] / xyz_dv_ref[2];
|
|
const Vector2d px_cur(cam.world2cam(T_cur_ref * (xyz_ref)));
|
|
const Vector2d px_du(cam.world2cam(T_cur_ref * (xyz_du_ref)));
|
|
const Vector2d px_dv(cam.world2cam(T_cur_ref * (xyz_dv_ref)));
|
|
A_cur_ref.col(0) = (px_du - px_cur) / halfpatch_size;
|
|
A_cur_ref.col(1) = (px_dv - px_cur) / halfpatch_size;
|
|
}
|
|
|
|
void VIOManager::warpAffine(const Matrix2d &A_cur_ref, const cv::Mat &img_ref, const Vector2d &px_ref, const int level_ref, const int search_level,
|
|
const int pyramid_level, const int halfpatch_size, float *patch)
|
|
{
|
|
const int patch_size = halfpatch_size * 2;
|
|
const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
|
|
if (isnan(A_ref_cur(0, 0)))
|
|
{
|
|
printf("Affine warp is NaN, probably camera has no translation\n");
|
|
return;
|
|
}
|
|
|
|
float *patch_ptr = patch;
|
|
for (int y = 0; y < patch_size; ++y)
|
|
{
|
|
for (int x = 0; x < patch_size; ++x)
|
|
{
|
|
Vector2f px_patch(x - halfpatch_size, y - halfpatch_size);
|
|
px_patch *= (1 << search_level);
|
|
px_patch *= (1 << pyramid_level);
|
|
const Vector2f px(A_ref_cur * px_patch + px_ref.cast<float>());
|
|
if (px[0] < 0 || px[1] < 0 || px[0] >= img_ref.cols - 1 || px[1] >= img_ref.rows - 1)
|
|
patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = 0;
|
|
else
|
|
patch_ptr[patch_size_total * pyramid_level + y * patch_size + x] = (float)vk::interpolateMat_8u(img_ref, px[0], px[1]);
|
|
}
|
|
}
|
|
}
|
|
|
|
int VIOManager::getBestSearchLevel(const Matrix2d &A_cur_ref, const int max_level)
|
|
{
|
|
int search_level = 0;
|
|
double D = A_cur_ref.determinant();
|
|
while (D > 3.0 && search_level < max_level)
|
|
{
|
|
search_level += 1;
|
|
D *= 0.25;
|
|
}
|
|
return search_level;
|
|
}
|
|
|
|
double VIOManager::calculateNCC(float *ref_patch, float *cur_patch, int patch_size)
|
|
{
|
|
double sum_ref = std::accumulate(ref_patch, ref_patch + patch_size, 0.0);
|
|
double mean_ref = sum_ref / patch_size;
|
|
|
|
double sum_cur = std::accumulate(cur_patch, cur_patch + patch_size, 0.0);
|
|
double mean_curr = sum_cur / patch_size;
|
|
|
|
double numerator = 0, demoniator1 = 0, demoniator2 = 0;
|
|
for (int i = 0; i < patch_size; i++)
|
|
{
|
|
double n = (ref_patch[i] - mean_ref) * (cur_patch[i] - mean_curr);
|
|
numerator += n;
|
|
demoniator1 += (ref_patch[i] - mean_ref) * (ref_patch[i] - mean_ref);
|
|
demoniator2 += (cur_patch[i] - mean_curr) * (cur_patch[i] - mean_curr);
|
|
}
|
|
return numerator / sqrt(demoniator1 * demoniator2 + 1e-10);
|
|
}
|
|
|
|
// True if `pt`'s reference patch was extracted from a *different* camera than
|
|
// `current_cam_id` and that other camera has previously handed off
|
|
// observations to the current one (i.e. this point is being tracked across
|
|
// the boundary between two non-overlapping camera FoVs).
|
|
bool VIOManager::isRealCrossCameraPoint(VisualPoint *pt, int current_cam_id)
|
|
{
|
|
if (!pt || !pt->ref_patch) return false;
|
|
|
|
CrossCameraData &data = pt->cross_cam_data_;
|
|
int ref_cam_id = pt->ref_patch->cam_id_;
|
|
int cur_cam_id = current_cam_id;
|
|
|
|
if (ref_cam_id == cur_cam_id) return false;
|
|
|
|
if (data.has_migration_history && data.migration_source_cam == ref_cam_id) { return true; }
|
|
|
|
for (auto obs : pt->obs_)
|
|
{
|
|
if (obs->cam_id_ == ref_cam_id && obs->cam_id_ != cur_cam_id) { return true; }
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
void VIOManager::updateCrossCameraHistory(VisualPoint *pt, int cam_id)
|
|
{
|
|
if (!pt) return;
|
|
|
|
CrossCameraData &data = pt->cross_cam_data_;
|
|
if (data.primary_cam_idx != -1 && data.primary_cam_idx != cam_id)
|
|
{
|
|
if (!data.currently_visible.test(data.primary_cam_idx) && data.currently_visible.test(cam_id)) { data.cross_camera_migrations++; }
|
|
}
|
|
data.primary_cam_idx = cam_id;
|
|
}
|
|
|
|
void VIOManager::retrieveFromVisualSparseMap(const std::vector<cv::Mat> imgs, vector<pointWithVar> &pg,
|
|
const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map)
|
|
{
|
|
if (feat_map.empty())
|
|
{
|
|
std::cout << "[VIO] feat_map is empty, skipping visual point retrieval" << std::endl;
|
|
return;
|
|
}
|
|
if (camera_photo_params.empty()) { initializeCameraPhotoParams(); }
|
|
|
|
double ts0 = omp_get_wtime();
|
|
visual_submap->reset();
|
|
sub_feat_map.clear();
|
|
|
|
float voxel_size = 0.5f;
|
|
if (!normal_en) warp_map.clear();
|
|
|
|
if (depth_imgs_buffer_.size() != cams.size())
|
|
{
|
|
depth_imgs_buffer_.resize(cams.size());
|
|
for (size_t i = 0; i < cams.size(); i++) { depth_imgs_buffer_[i] = cv::Mat::zeros(height, width, CV_32FC1); }
|
|
}
|
|
else
|
|
{
|
|
for (auto &depth_img : depth_imgs_buffer_) { depth_img.setTo(0); }
|
|
}
|
|
std::vector<cv::Mat> &depth_imgs = depth_imgs_buffer_;
|
|
|
|
grid_n_width = ceil(static_cast<double>(width / grid_size));
|
|
grid_n_height = ceil(static_cast<double>(height / grid_size));
|
|
int cells_per_camera = grid_n_width * grid_n_height;
|
|
int num_cameras = (int)cams.size();
|
|
int total_cells = cells_per_camera * num_cameras;
|
|
|
|
std::vector<int> camera_grid_offset(num_cameras);
|
|
for (int i = 0; i < num_cameras; i++) { camera_grid_offset[i] = i * cells_per_camera; }
|
|
|
|
grid_num.assign(total_cells, TYPE_UNKNOWN);
|
|
map_dist.assign(total_cells, std::numeric_limits<float>::max());
|
|
retrieve_voxel_points.assign(total_cells, nullptr);
|
|
scan_value.assign(total_cells, 0.0f);
|
|
|
|
if ((int)retrieve_voxel_points_list_buffer_.size() != total_cells) { retrieve_voxel_points_list_buffer_.resize(total_cells); }
|
|
for (auto &set : retrieve_voxel_points_list_buffer_) { set.clear(); }
|
|
std::vector<std::unordered_set<VisualPoint *>> &retrieve_voxel_points_list = retrieve_voxel_points_list_buffer_;
|
|
|
|
// Splat each LiDAR point's depth into every camera's depth buffer (used
|
|
// below for depth-continuity filtering of candidate patches).
|
|
for (int i_point = 0; i_point < (int)pg.size(); i_point++)
|
|
{
|
|
V3D pt_w = pg[i_point].point_w;
|
|
int loc_xyz[3];
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
loc_xyz[j] = (int)std::floor(pt_w[j] / voxel_size);
|
|
if (loc_xyz[j] < 0) loc_xyz[j] -= 1;
|
|
}
|
|
VOXEL_LOCATION position(loc_xyz[0], loc_xyz[1], loc_xyz[2]);
|
|
sub_feat_map[position] = 0;
|
|
|
|
#ifdef MP_EN
|
|
omp_set_num_threads(MP_PROC_NUM);
|
|
#pragma omp parallel for
|
|
#endif
|
|
for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++)
|
|
{
|
|
V3D pt_c = new_frame_->w2f(pt_w, cam_idx);
|
|
if (pt_c[2] > 0)
|
|
{
|
|
V2D px = new_frame_->w2c(pt_w, cam_idx);
|
|
if (cams[cam_idx]->isInFrame(px.cast<int>(), border))
|
|
{
|
|
float depth = (float)pt_c[2];
|
|
int col = (int)px[0];
|
|
int row = (int)px[1];
|
|
if (row >= 0 && row < height && col >= 0 && col < width)
|
|
{
|
|
float *it = (float *)depth_imgs[cam_idx].data;
|
|
it[row * width + col] = depth;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Project every voxel's existing points into every camera to find which
|
|
// grid cells have candidate map points visible this frame.
|
|
std::vector<VOXEL_LOCATION> DeleteKeyList;
|
|
for (auto &iter_sf : sub_feat_map)
|
|
{
|
|
VOXEL_LOCATION position = iter_sf.first;
|
|
auto corre_voxel = feat_map.find(position);
|
|
if (corre_voxel != feat_map.end())
|
|
{
|
|
bool voxel_in_fov = false;
|
|
std::vector<VisualPoint *> &voxel_points = corre_voxel->second->voxel_points;
|
|
|
|
for (VisualPoint *pt : voxel_points)
|
|
{
|
|
if (!pt || pt->obs_.empty()) continue;
|
|
if (!pt->is_normal_initialized_) continue;
|
|
|
|
if (enable_cross_camera_tracking)
|
|
{
|
|
pt->cross_cam_data_.previously_visible = pt->cross_cam_data_.currently_visible;
|
|
pt->cross_cam_data_.currently_visible.reset();
|
|
}
|
|
|
|
bool this_pt_in_fov = false;
|
|
for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++)
|
|
{
|
|
V3D dir_c = new_frame_->w2f(pt->pos_, cam_idx);
|
|
if (dir_c[2] < 0) continue;
|
|
|
|
V2D pc = new_frame_->w2c(pt->pos_, cam_idx);
|
|
if (!cams[cam_idx]->isInFrame(pc.cast<int>(), border)) continue;
|
|
|
|
this_pt_in_fov = true;
|
|
|
|
if (enable_cross_camera_tracking) { pt->cross_cam_data_.currently_visible.set(cam_idx); }
|
|
|
|
int grid_col = (int)(pc[0] / grid_size);
|
|
int grid_row = (int)(pc[1] / grid_size);
|
|
int local_index = grid_row * grid_n_width + grid_col;
|
|
int index = camera_grid_offset[cam_idx] + local_index;
|
|
|
|
if (local_index < 0 || local_index >= cells_per_camera) continue;
|
|
|
|
grid_num[index] = TYPE_MAP;
|
|
retrieve_voxel_points_list[index].insert(pt);
|
|
|
|
V3D obs_vec = new_frame_->pos(cam_idx) - pt->pos_;
|
|
float cur_dist = (float)obs_vec.norm();
|
|
if (cur_dist <= map_dist[index])
|
|
{
|
|
map_dist[index] = cur_dist;
|
|
retrieve_voxel_points[index] = pt;
|
|
}
|
|
}
|
|
|
|
if (this_pt_in_fov) voxel_in_fov = true;
|
|
}
|
|
|
|
if (!voxel_in_fov) DeleteKeyList.push_back(position);
|
|
}
|
|
}
|
|
for (auto &key : DeleteKeyList) { sub_feat_map.erase(key); }
|
|
|
|
// For every occupied grid cell, pick the best reference patch + warp for
|
|
// each candidate point and score it by photometric error, in parallel.
|
|
struct CandidatePoint
|
|
{
|
|
VisualPoint *pt;
|
|
float error;
|
|
int search_level;
|
|
Matrix2d A_cur_ref_zero;
|
|
std::vector<float> patch_wrap;
|
|
Feature *ref_ftr;
|
|
V2D pc;
|
|
};
|
|
|
|
int num_threads = 1;
|
|
#ifdef MP_EN
|
|
num_threads = omp_get_max_threads();
|
|
#endif
|
|
std::vector<std::vector<std::unordered_set<VisualPoint *>>> thread_camera_processed_points(num_threads);
|
|
std::vector<std::vector<std::vector<CandidatePoint>>> thread_camera_candidates(num_threads);
|
|
std::vector<std::vector<float>> thread_patch_wrap_buffer(num_threads);
|
|
std::vector<std::vector<float>> thread_patch_buffer_buffer(num_threads);
|
|
|
|
for (int t = 0; t < num_threads; t++)
|
|
{
|
|
thread_camera_processed_points[t].resize(num_cameras);
|
|
thread_camera_candidates[t].resize(num_cameras);
|
|
thread_patch_wrap_buffer[t].resize(warp_len);
|
|
thread_patch_buffer_buffer[t].resize(patch_size_total);
|
|
}
|
|
|
|
#ifdef MP_EN
|
|
omp_set_num_threads(MP_PROC_NUM);
|
|
#pragma omp parallel for schedule(dynamic, 16)
|
|
#endif
|
|
for (int i = 0; i < total_cells; i++)
|
|
{
|
|
int thread_id = 0;
|
|
#ifdef MP_EN
|
|
thread_id = omp_get_thread_num();
|
|
#endif
|
|
std::vector<std::vector<CandidatePoint>> &camera_candidates = thread_camera_candidates[thread_id];
|
|
std::vector<std::unordered_set<VisualPoint *>> &camera_processed_points = thread_camera_processed_points[thread_id];
|
|
std::vector<float> &patch_wrap = thread_patch_wrap_buffer[thread_id];
|
|
std::vector<float> &patch_buffer_local = thread_patch_buffer_buffer[thread_id];
|
|
|
|
if (grid_num[i] != TYPE_MAP) continue;
|
|
|
|
int cam_idx = i / cells_per_camera;
|
|
|
|
for (auto pt : retrieve_voxel_points_list[i])
|
|
{
|
|
if (!pt) continue;
|
|
if (camera_processed_points[cam_idx].find(pt) != camera_processed_points[cam_idx].end()) continue;
|
|
camera_processed_points[cam_idx].insert(pt);
|
|
|
|
if (!pt->is_normal_initialized_) continue;
|
|
|
|
V2D pc = new_frame_->w2c(pt->pos_, cam_idx);
|
|
if (!cams[cam_idx]->isInFrame(pc.cast<int>(), border)) continue;
|
|
|
|
V3D pt_cam = new_frame_->w2f(pt->pos_, cam_idx);
|
|
bool depth_continous = false;
|
|
float *depth_ptr = reinterpret_cast<float *>(depth_imgs[cam_idx].data);
|
|
|
|
int base_col = int(pc[0]) - patch_size_half;
|
|
int base_row = int(pc[1]) - patch_size_half;
|
|
int center_offset = patch_size_half;
|
|
float pt_depth = static_cast<float>(pt_cam[2]);
|
|
|
|
const int sample_offsets[][2] = {{0, 0}, {patch_size, 0}, {0, patch_size}, {patch_size, patch_size},
|
|
{center_offset, 0}, {patch_size, center_offset}, {0, center_offset}, {center_offset, patch_size}};
|
|
|
|
for (int s = 0; s < 8; s++)
|
|
{
|
|
int col = base_col + sample_offsets[s][0];
|
|
int row = base_row + sample_offsets[s][1];
|
|
if (row < 0 || row >= height || col < 0 || col >= width) continue;
|
|
if (s == 0) continue;
|
|
|
|
float depth = depth_ptr[row * width + col];
|
|
if (depth == 0.) continue;
|
|
if (abs(pt_depth - depth) > 0.5)
|
|
{
|
|
depth_continous = true;
|
|
break;
|
|
}
|
|
}
|
|
if (depth_continous) continue;
|
|
|
|
Feature *ref_ftr = nullptr;
|
|
if (normal_en)
|
|
{
|
|
if (pt->obs_.size() == 1)
|
|
{
|
|
ref_ftr = *pt->obs_.begin();
|
|
pt->ref_patch = ref_ftr;
|
|
pt->has_ref_patch_ = true;
|
|
}
|
|
else if (pt->has_ref_patch_ && pt->ref_patch)
|
|
{
|
|
if ((int)cams.size() == 1) { ref_ftr = pt->ref_patch; }
|
|
else
|
|
{
|
|
bool found_current_cam = false;
|
|
for (auto it = pt->obs_.begin(); it != pt->obs_.end(); ++it)
|
|
{
|
|
if ((*it)->cam_id_ == cam_idx)
|
|
{
|
|
ref_ftr = *it;
|
|
found_current_cam = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!found_current_cam) { ref_ftr = pt->ref_patch; }
|
|
else { pt->ref_patch = ref_ftr; }
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// No reference patch chosen yet: prefer an observation from the
|
|
// current camera; otherwise pick whichever observation looks most
|
|
// photometrically consistent with a couple of the others.
|
|
for (auto it = pt->obs_.begin(); it != pt->obs_.end(); ++it)
|
|
{
|
|
if ((*it)->cam_id_ == cam_idx)
|
|
{
|
|
ref_ftr = *it;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!ref_ftr && pt->obs_.size() > 0)
|
|
{
|
|
if (pt->obs_.size() <= 3) { ref_ftr = *pt->obs_.begin(); }
|
|
else
|
|
{
|
|
float photometric_errors_min = std::numeric_limits<float>::max();
|
|
int compare_count = 0;
|
|
const int max_compare = 2;
|
|
|
|
for (auto it = pt->obs_.begin(); it != pt->obs_.end() && compare_count < max_compare + 1; ++it, ++compare_count)
|
|
{
|
|
Feature *ref_patch_temp = *it;
|
|
float *patch_temp = ref_patch_temp->patch_;
|
|
float photometric_errors = 0.0f;
|
|
int sample_count = 0;
|
|
|
|
auto itm = pt->obs_.begin();
|
|
for (int k = 0; k < max_compare && itm != pt->obs_.end(); ++itm, ++k)
|
|
{
|
|
if ((*itm)->id_ == ref_patch_temp->id_) continue;
|
|
float *patch_cache = (*itm)->patch_;
|
|
Eigen::Map<Eigen::VectorXf> patch_a(patch_temp, patch_size_total);
|
|
Eigen::Map<Eigen::VectorXf> patch_b(patch_cache, patch_size_total);
|
|
photometric_errors += (patch_a - patch_b).squaredNorm();
|
|
sample_count++;
|
|
}
|
|
if (sample_count > 0) photometric_errors /= (float)sample_count;
|
|
|
|
if (photometric_errors < photometric_errors_min)
|
|
{
|
|
photometric_errors_min = photometric_errors;
|
|
ref_ftr = ref_patch_temp;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pt->ref_patch = ref_ftr;
|
|
pt->has_ref_patch_ = (ref_ftr != nullptr);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (!pt->getCloseViewObs(new_frame_->pos(cam_idx), ref_ftr, pc)) continue;
|
|
}
|
|
if (!ref_ftr) continue;
|
|
|
|
int search_level;
|
|
Matrix2d A_cur_ref_zero;
|
|
if (normal_en)
|
|
{
|
|
V3D norm_vec = (ref_ftr->T_f_w_.rotationMatrix() * pt->normal_).normalized();
|
|
V3D pf = ref_ftr->T_f_w_ * pt->pos_;
|
|
SE3<double> T_cur_ref = new_frame_->T_f_w_[cam_idx] * ref_ftr->T_f_w_.inverse();
|
|
getWarpMatrixAffineHomography(*cams[cam_idx], ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref_zero);
|
|
search_level = getBestSearchLevel(A_cur_ref_zero, 2);
|
|
}
|
|
else
|
|
{
|
|
auto iter_warp = warp_map.find(ref_ftr->id_);
|
|
if (iter_warp != warp_map.end())
|
|
{
|
|
search_level = iter_warp->second->search_level;
|
|
A_cur_ref_zero = iter_warp->second->A_cur_ref;
|
|
}
|
|
else
|
|
{
|
|
getWarpMatrixAffine(*cams[cam_idx], ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(),
|
|
new_frame_->T_f_w_[cam_idx] * ref_ftr->T_f_w_.inverse(), ref_ftr->level_, 0, patch_size_half, A_cur_ref_zero);
|
|
search_level = getBestSearchLevel(A_cur_ref_zero, 2);
|
|
Warp *ot = new Warp(search_level, A_cur_ref_zero);
|
|
warp_map[ref_ftr->id_] = ot;
|
|
}
|
|
}
|
|
|
|
for (int pyramid_level = 0; pyramid_level <= patch_pyrimid_level - 1; pyramid_level++)
|
|
{
|
|
warpAffine(A_cur_ref_zero, ref_ftr->img_, ref_ftr->px_, ref_ftr->level_, search_level, pyramid_level, patch_size_half, patch_wrap.data());
|
|
}
|
|
getImagePatch(imgs[cam_idx], pc, patch_buffer_local.data(), 0);
|
|
|
|
float error = 0.0;
|
|
if (exposure_estimate_en)
|
|
{
|
|
double cur_inv_expo = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0;
|
|
for (int ind = 0; ind < patch_size_total; ind++)
|
|
{
|
|
float diff = ref_ftr->inv_expo_time_ * patch_wrap[ind] - cur_inv_expo * patch_buffer_local[ind];
|
|
error += diff * diff;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for (int ind = 0; ind < patch_size_total; ind++)
|
|
{
|
|
float diff = patch_wrap[ind] - patch_buffer_local[ind];
|
|
error += diff * diff;
|
|
}
|
|
}
|
|
|
|
if (ncc_en)
|
|
{
|
|
double ncc = calculateNCC(patch_wrap.data(), patch_buffer_local.data(), patch_size_total);
|
|
if (ncc < ncc_thre) continue;
|
|
}
|
|
|
|
float error_threshold = outlier_threshold * patch_size_total;
|
|
if (error > error_threshold) continue;
|
|
|
|
CandidatePoint candidate;
|
|
candidate.pt = pt;
|
|
candidate.error = error;
|
|
candidate.search_level = search_level;
|
|
candidate.A_cur_ref_zero = A_cur_ref_zero;
|
|
candidate.patch_wrap = patch_wrap;
|
|
candidate.ref_ftr = ref_ftr;
|
|
candidate.pc = pc;
|
|
camera_candidates[cam_idx].push_back(candidate);
|
|
|
|
CrossCameraData &cc_data = pt->cross_cam_data_;
|
|
cc_data.currently_visible.set(cam_idx);
|
|
if (cc_data.primary_cam_idx != -1 && cc_data.primary_cam_idx != cam_idx)
|
|
{
|
|
if (!cc_data.has_migration_history || cc_data.migration_source_cam != cc_data.primary_cam_idx)
|
|
{
|
|
cc_data.migration_source_cam = cc_data.primary_cam_idx;
|
|
cc_data.has_migration_history = true;
|
|
cc_data.cross_camera_migrations++;
|
|
}
|
|
}
|
|
cc_data.previous_cam_idx = cam_idx;
|
|
cc_data.primary_cam_idx = cam_idx;
|
|
}
|
|
}
|
|
|
|
// Merge per-thread candidates, then keep the best `points_per_camera_min`
|
|
// per camera unconditionally, and fill remaining slots (up to
|
|
// `max_total_points` total) with the globally-best leftover candidates.
|
|
std::vector<std::vector<CandidatePoint>> per_camera_candidates(num_cameras);
|
|
for (int t = 0; t < num_threads; t++)
|
|
{
|
|
for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++)
|
|
{
|
|
per_camera_candidates[cam_idx].insert(per_camera_candidates[cam_idx].end(), thread_camera_candidates[t][cam_idx].begin(),
|
|
thread_camera_candidates[t][cam_idx].end());
|
|
}
|
|
}
|
|
|
|
for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++)
|
|
{
|
|
size_t n_candidates = per_camera_candidates[cam_idx].size();
|
|
if (n_candidates == 0) continue;
|
|
size_t n_to_sort = std::min(n_candidates, (size_t)points_per_camera_max);
|
|
std::partial_sort(per_camera_candidates[cam_idx].begin(), per_camera_candidates[cam_idx].begin() + n_to_sort, per_camera_candidates[cam_idx].end(),
|
|
[](const CandidatePoint &a, const CandidatePoint &b) { return a.error < b.error; });
|
|
}
|
|
|
|
std::vector<int> points_per_camera(num_cameras, 0);
|
|
for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++)
|
|
{
|
|
int n_to_keep = std::min((int)per_camera_candidates[cam_idx].size(), points_per_camera_min);
|
|
for (int i = 0; i < n_to_keep; i++)
|
|
{
|
|
const auto &cand = per_camera_candidates[cam_idx][i];
|
|
visual_submap->voxel_points.push_back(cand.pt);
|
|
visual_submap->propa_errors.push_back(cand.error);
|
|
visual_submap->search_levels.push_back(cand.search_level);
|
|
visual_submap->errors.push_back(cand.error);
|
|
visual_submap->warp_patch.push_back(cand.patch_wrap);
|
|
visual_submap->inv_expo_list.push_back(cand.ref_ftr->inv_expo_time_);
|
|
visual_submap->camera_ids.push_back(cam_idx);
|
|
points_per_camera[cam_idx]++;
|
|
}
|
|
per_camera_candidates[cam_idx].erase(per_camera_candidates[cam_idx].begin(), per_camera_candidates[cam_idx].begin() + n_to_keep);
|
|
}
|
|
|
|
std::vector<std::pair<CandidatePoint, int>> remaining_candidates;
|
|
remaining_candidates.reserve(max_total_points);
|
|
for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++)
|
|
{
|
|
int max_remaining = points_per_camera_max - points_per_camera[cam_idx];
|
|
int n_remaining = std::min((int)per_camera_candidates[cam_idx].size(), max_remaining);
|
|
for (int i = 0; i < n_remaining; i++) { remaining_candidates.push_back({per_camera_candidates[cam_idx][i], cam_idx}); }
|
|
}
|
|
|
|
std::sort(remaining_candidates.begin(), remaining_candidates.end(), [](const auto &a, const auto &b) { return a.first.error < b.first.error; });
|
|
|
|
int current_total = visual_submap->voxel_points.size();
|
|
int remaining_quota = max_total_points - current_total;
|
|
int n_add = std::min((int)remaining_candidates.size(), remaining_quota);
|
|
|
|
for (int i = 0; i < n_add; i++)
|
|
{
|
|
const auto &cand = remaining_candidates[i].first;
|
|
int cam_idx = remaining_candidates[i].second;
|
|
visual_submap->voxel_points.push_back(cand.pt);
|
|
visual_submap->propa_errors.push_back(cand.error);
|
|
visual_submap->search_levels.push_back(cand.search_level);
|
|
visual_submap->errors.push_back(cand.error);
|
|
visual_submap->warp_patch.push_back(cand.patch_wrap);
|
|
visual_submap->inv_expo_list.push_back(cand.ref_ftr->inv_expo_time_);
|
|
visual_submap->camera_ids.push_back(cam_idx);
|
|
points_per_camera[cam_idx]++;
|
|
}
|
|
|
|
total_points = (int)visual_submap->voxel_points.size();
|
|
|
|
if (raycast_en)
|
|
{
|
|
float voxel_size_rc = 0.5f;
|
|
int loc_xyz[3];
|
|
for (int cam_idx = 0; cam_idx < num_cameras; cam_idx++)
|
|
{
|
|
int cam_offset = camera_grid_offset[cam_idx];
|
|
for (int local_i = 0; local_i < cells_per_camera; local_i++)
|
|
{
|
|
int i = cam_offset + local_i;
|
|
if (grid_num[i] == TYPE_MAP) continue;
|
|
|
|
bool is_border = false;
|
|
if (cam_idx < (int)border_flag.size() && local_i < (int)border_flag[cam_idx].size() && border_flag[cam_idx][local_i] == 1) { is_border = true; }
|
|
if (is_border) continue;
|
|
|
|
bool found_feature = false;
|
|
if (cam_idx >= (int)rays_with_sample_points.size()) continue;
|
|
if (local_i >= (int)rays_with_sample_points[cam_idx].size()) continue;
|
|
|
|
for (const auto &sample_point_cam : rays_with_sample_points[cam_idx][local_i])
|
|
{
|
|
V3D sample_point_w = new_frame_->f2w(sample_point_cam, cam_idx);
|
|
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
loc_xyz[j] = floor(sample_point_w[j] / voxel_size_rc);
|
|
if (loc_xyz[j] < 0) { loc_xyz[j] -= 1.0; }
|
|
}
|
|
|
|
VOXEL_LOCATION sample_pos(loc_xyz[0], loc_xyz[1], loc_xyz[2]);
|
|
|
|
auto corre_sub_feat_map = sub_feat_map.find(sample_pos);
|
|
if (corre_sub_feat_map != sub_feat_map.end()) break;
|
|
|
|
auto corre_feat_map = feat_map.find(sample_pos);
|
|
if (corre_feat_map != feat_map.end())
|
|
{
|
|
std::vector<VisualPoint *> &voxel_points = corre_feat_map->second->voxel_points;
|
|
int voxel_num = voxel_points.size();
|
|
if (voxel_num == 0) continue;
|
|
|
|
for (int j = 0; j < voxel_num; j++)
|
|
{
|
|
VisualPoint *pt = voxel_points[j];
|
|
if (pt == nullptr) continue;
|
|
if (pt->obs_.size() == 0) continue;
|
|
bool point_visible = false;
|
|
for (int check_cam = 0; check_cam < (int)cams.size(); check_cam++)
|
|
{
|
|
V3D dir = new_frame_->w2f(pt->pos_, check_cam);
|
|
if (dir[2] < 0) continue;
|
|
|
|
V2D pc = new_frame_->w2c(pt->pos_, check_cam);
|
|
if (new_frame_->cams_[check_cam]->isInFrame(pc.cast<int>(), border))
|
|
{
|
|
point_visible = true;
|
|
|
|
grid_num[i] = TYPE_MAP;
|
|
|
|
Vector3d obs_vec = new_frame_->pos(check_cam) - pt->pos_;
|
|
float cur_dist = obs_vec.norm();
|
|
if (cur_dist <= map_dist[i])
|
|
{
|
|
map_dist[i] = cur_dist;
|
|
retrieve_voxel_points[i] = pt;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
if (point_visible)
|
|
{
|
|
found_feature = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (found_feature)
|
|
{
|
|
sub_feat_map[sample_pos] = 0;
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
auto iter = plane_map.find(sample_pos);
|
|
if (iter != plane_map.end())
|
|
{
|
|
VoxelOctoTree *current_octo;
|
|
current_octo = iter->second->find_correspond(sample_point_w);
|
|
if (current_octo->plane_ptr_->is_plane_)
|
|
{
|
|
pointWithVar plane_center;
|
|
VoxelPlane &plane = *current_octo->plane_ptr_;
|
|
plane_center.point_w = plane.center_;
|
|
plane_center.normal = plane.normal_;
|
|
visual_submap->add_from_voxel_map.push_back(plane_center);
|
|
found_feature = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (found_feature) break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
double VIOManager::calculateCoVarianceScale(double realtime_avg_error, int realtime_n_meas)
|
|
{
|
|
if (frame_count < dynamic_cov_warmup_frames) { return warmup_cov_scale; }
|
|
|
|
double t = (realtime_avg_error - 1.0) / (dynamic_cov_error_max - 1.0);
|
|
t = std::min(1.0, std::max(0.0, t));
|
|
double cov_scale = min_cov_scale + t * (max_cov_scale - min_cov_scale);
|
|
cov_scale = std::max(min_cov_scale, std::min(max_cov_scale, cov_scale));
|
|
|
|
double alpha = 0.6;
|
|
double smoothed_scale = alpha * cov_scale + (1.0 - alpha) * prev_cov_scale_;
|
|
prev_cov_scale_ = smoothed_scale;
|
|
|
|
if (frame_count % 30 == 0 || smoothed_scale > min_cov_scale * 1.5)
|
|
{
|
|
printf("[DynCov] frame=%d, MSE=%.1f, cov=%.1f (range: %.1f~%.1f)\n", frame_count, realtime_avg_error, smoothed_scale, min_cov_scale, max_cov_scale);
|
|
}
|
|
|
|
return smoothed_scale;
|
|
}
|
|
|
|
double VIOManager::calculateCoVarianceScalePerCam(int cam_idx, double realtime_avg_error, int realtime_n_meas)
|
|
{
|
|
if (frame_count < dynamic_cov_warmup_frames) { return warmup_cov_scale; }
|
|
|
|
const int min_required_meas = 5;
|
|
if (realtime_n_meas < min_required_meas) { return max_cov_scale * 10.0; }
|
|
|
|
double t = (realtime_avg_error - 1.0) / (dynamic_cov_error_max - 1.0);
|
|
t = std::min(1.0, std::max(0.0, t));
|
|
double cov_scale = min_cov_scale + t * (max_cov_scale - min_cov_scale);
|
|
cov_scale = std::max(min_cov_scale, std::min(max_cov_scale, cov_scale));
|
|
|
|
double alpha = 0.6;
|
|
double smoothed_scale = alpha * cov_scale + (1.0 - alpha) * prev_cov_scale_per_cam_[cam_idx];
|
|
prev_cov_scale_per_cam_[cam_idx] = smoothed_scale;
|
|
|
|
return smoothed_scale;
|
|
}
|
|
|
|
void VIOManager::computeJacobianAndUpdateEKF(const std::vector<cv::Mat> imgs)
|
|
{
|
|
if (total_points < 2)
|
|
{
|
|
compute_jacobian_time = update_ekf_time = 0.0;
|
|
return;
|
|
}
|
|
double original_img_point_cov = img_point_cov;
|
|
if (enable_dynamic_covariance_)
|
|
{
|
|
double cov_scale = calculateCoVarianceScale(prev_avg_error_, prev_n_meas_);
|
|
img_point_cov = cov_scale;
|
|
}
|
|
|
|
compute_jacobian_time = update_ekf_time = 0.0;
|
|
for (int level = patch_pyrimid_level - 1; level >= 0; level--)
|
|
{
|
|
if (inverse_composition_en) { updateStateInverse(imgs, level); }
|
|
else { updateState(imgs, level); }
|
|
}
|
|
state->cov -= G * state->cov;
|
|
updateFrameState(*state);
|
|
img_point_cov = original_img_point_cov;
|
|
}
|
|
|
|
void VIOManager::generateVisualMapPoints(const std::vector<cv::Mat> &imgs, std::vector<pointWithVar> &pg)
|
|
{
|
|
if (pg.size() <= 10) return;
|
|
|
|
int num_cameras = static_cast<int>(imgs.size());
|
|
int cells_per_camera = grid_n_width * grid_n_height;
|
|
int total_cells = cells_per_camera * num_cameras;
|
|
|
|
std::vector<int> best_cam_idx(total_cells, -1);
|
|
if ((int)append_voxel_points.size() != total_cells) { append_voxel_points.resize(total_cells); }
|
|
|
|
std::vector<int> camera_grid_offset(num_cameras);
|
|
for (int i = 0; i < num_cameras; i++) { camera_grid_offset[i] = i * cells_per_camera; }
|
|
|
|
for (size_t i = 0; i < pg.size(); i++)
|
|
{
|
|
if (pg[i].normal == V3D(0, 0, 0)) continue;
|
|
|
|
V3D pt = pg[i].point_w;
|
|
for (size_t cam_idx = 0; cam_idx < imgs.size(); cam_idx++)
|
|
{
|
|
V2D pc = new_frame_->w2c(pt, cam_idx);
|
|
if (!cams[cam_idx]->isInFrame(pc.cast<int>(), border)) continue;
|
|
int grid_col = static_cast<int>(pc[0] / grid_size);
|
|
int grid_row = static_cast<int>(pc[1] / grid_size);
|
|
if (grid_col < 0 || grid_col >= grid_n_width || grid_row < 0 || grid_row >= grid_n_height) continue;
|
|
|
|
int local_index = grid_row * grid_n_width + grid_col;
|
|
int index = camera_grid_offset[cam_idx] + local_index;
|
|
if (index < 0 || index >= total_cells) continue;
|
|
if (grid_num[index] == TYPE_MAP) continue;
|
|
|
|
float cur_value = vk::shiTomasiScore(imgs[cam_idx], pc[0], pc[1]);
|
|
if (cur_value > scan_value[index])
|
|
{
|
|
scan_value[index] = cur_value;
|
|
append_voxel_points[index] = pg[i];
|
|
best_cam_idx[index] = static_cast<int>(cam_idx);
|
|
grid_num[index] = TYPE_POINTCLOUD;
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < total_cells; i++)
|
|
{
|
|
if (grid_num[i] != TYPE_POINTCLOUD) continue;
|
|
if (best_cam_idx[i] < 0 || best_cam_idx[i] >= (int)imgs.size()) continue;
|
|
|
|
pointWithVar pt_var = append_voxel_points[i];
|
|
V3D pt = pt_var.point_w;
|
|
if (pt.norm() < 0.0001) continue;
|
|
|
|
int cam_idx = best_cam_idx[i];
|
|
V2D pc = new_frame_->w2c(pt, cam_idx);
|
|
if (!cams[cam_idx]->isInFrame(pc.cast<int>(), border)) continue;
|
|
V3D pt_cam = new_frame_->w2f(pt, cam_idx);
|
|
if (pt_cam[2] <= 0) continue;
|
|
|
|
try
|
|
{
|
|
float *patch = new float[patch_size_total];
|
|
getImagePatch(imgs[cam_idx], pc, patch, 0);
|
|
VisualPoint *pt_new = new VisualPoint(pt);
|
|
Vector3d f = cams[cam_idx]->cam2world(pc);
|
|
Feature *ftr_new = new Feature(pt_new, patch, pc, f, new_frame_->T_f_w_[cam_idx], 0, cam_idx);
|
|
ftr_new->cam_id_ = cam_idx;
|
|
ftr_new->img_ = imgs[cam_idx];
|
|
ftr_new->id_ = new_frame_->id_;
|
|
ftr_new->inv_expo_time_ = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0;
|
|
pt_new->addFrameRef(ftr_new);
|
|
|
|
if (enable_cross_camera_tracking)
|
|
{
|
|
pt_new->cross_cam_data_.primary_cam_idx = cam_idx;
|
|
pt_new->cross_cam_data_.cross_camera_migrations = 0;
|
|
pt_new->cross_cam_data_.currently_visible.reset();
|
|
pt_new->cross_cam_data_.currently_visible.set(cam_idx);
|
|
}
|
|
pt_new->covariance_ = pt_var.var;
|
|
pt_new->is_normal_initialized_ = true;
|
|
V3D norm_vec = new_frame_->T_f_w_[cam_idx].rotationMatrix() * pt_var.normal;
|
|
V3D dir = new_frame_->T_f_w_[cam_idx] * pt;
|
|
if (dir.dot(norm_vec) < 0) pt_new->normal_ = -pt_var.normal;
|
|
else pt_new->normal_ = pt_var.normal;
|
|
pt_new->previous_normal_ = pt_new->normal_;
|
|
insertPointIntoVoxelMap(pt_new);
|
|
}
|
|
catch (const std::exception &e)
|
|
{
|
|
std::cerr << "Exception in generateVisualMapPoints: " << e.what() << std::endl;
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
void VIOManager::updateVisualMapPoints(std::vector<cv::Mat> &imgs)
|
|
{
|
|
if (total_points == 0) return;
|
|
|
|
for (int i = 0; i < total_points; i++)
|
|
{
|
|
VisualPoint *pt = visual_submap->voxel_points[i];
|
|
if (pt == nullptr) continue;
|
|
if (pt->is_converged_)
|
|
{
|
|
pt->deleteNonRefPatchFeatures();
|
|
continue;
|
|
}
|
|
if (pt->obs_.empty()) continue;
|
|
Feature *last_feature = pt->obs_.back();
|
|
for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++)
|
|
{
|
|
SE3<double> pose_cur = new_frame_->T_f_w_[cam_idx];
|
|
V2D pc = new_frame_->w2c(pt->pos_, cam_idx);
|
|
if (!cams[cam_idx]->isInFrame(pc.cast<int>(), border)) continue;
|
|
SE3<double> pose_ref = last_feature->T_f_w_;
|
|
SE3<double> delta_pose = pose_ref * pose_cur.inverse();
|
|
double delta_p = delta_pose.translation().norm();
|
|
double cos_val = 0.5 * (delta_pose.rotationMatrix().trace() - 1);
|
|
if (cos_val > 1.0) cos_val = 1.0;
|
|
if (cos_val < -1.0) cos_val = -1.0;
|
|
double delta_theta = std::acos(cos_val);
|
|
bool add_flag = false;
|
|
if (delta_p > 0.5 || delta_theta > 0.3) add_flag = true;
|
|
V2D last_px = last_feature->px_;
|
|
double pixel_dist = (pc - last_px).norm();
|
|
if (pixel_dist > 40.0) add_flag = true;
|
|
if (pt->obs_.size() >= 30)
|
|
{
|
|
Feature *ref_ftr = nullptr;
|
|
pt->findMinScoreFeature(new_frame_->pos(cam_idx), ref_ftr);
|
|
if (ref_ftr) pt->deleteFeatureRef(ref_ftr);
|
|
}
|
|
if (add_flag)
|
|
{
|
|
float *patch_temp = new float[patch_size_total];
|
|
getImagePatch(imgs[cam_idx], pc, patch_temp, 0);
|
|
Vector3d f = cams[cam_idx]->cam2world(pc);
|
|
int search_level = ((int)visual_submap->search_levels.size() > i) ? visual_submap->search_levels[i] : 0;
|
|
|
|
Feature *ftr_new = new Feature(pt, patch_temp, pc, f, new_frame_->T_f_w_[cam_idx], search_level, cam_idx);
|
|
ftr_new->cam_id_ = cam_idx;
|
|
ftr_new->img_ = imgs[cam_idx];
|
|
ftr_new->id_ = new_frame_->id_;
|
|
ftr_new->inv_expo_time_ = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0;
|
|
pt->addFrameRef(ftr_new);
|
|
update_flag[i] = 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void VIOManager::updateReferencePatch(const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map)
|
|
{
|
|
if (total_points == 0) return;
|
|
for (int i = 0; i < (int)visual_submap->voxel_points.size(); i++)
|
|
{
|
|
VisualPoint *pt = visual_submap->voxel_points[i];
|
|
if (!pt->is_normal_initialized_ || pt->is_converged_ || pt->obs_.size() <= 5) continue;
|
|
if (update_flag[i] == 0) continue;
|
|
{
|
|
const V3D &p_w = pt->pos_;
|
|
float loc_xyz[3];
|
|
for (int j = 0; j < 3; j++)
|
|
{
|
|
loc_xyz[j] = p_w[j] / 0.5;
|
|
if (loc_xyz[j] < 0) loc_xyz[j] -= 1.0f;
|
|
}
|
|
VOXEL_LOCATION position((int64_t)loc_xyz[0], (int64_t)loc_xyz[1], (int64_t)loc_xyz[2]);
|
|
auto it_plane = plane_map.find(position);
|
|
if (it_plane != plane_map.end())
|
|
{
|
|
VoxelOctoTree *current_octo = it_plane->second->find_correspond(p_w);
|
|
if (current_octo->plane_ptr_->is_plane_)
|
|
{
|
|
VoxelPlane &plane = *current_octo->plane_ptr_;
|
|
float dis_to_plane = plane.normal_.dot(p_w) + plane.d_;
|
|
float dis_to_plane_abs = std::fabs(dis_to_plane);
|
|
float dis_to_center = (plane.center_ - p_w).squaredNorm();
|
|
float range_dis = std::sqrt(dis_to_center - dis_to_plane * dis_to_plane);
|
|
|
|
if (range_dis <= 3.0f * plane.radius_)
|
|
{
|
|
Eigen::Matrix<double, 1, 6> J_nq;
|
|
J_nq.block<1, 3>(0, 0) = p_w - plane.center_;
|
|
J_nq.block<1, 3>(0, 3) = -plane.normal_.transpose();
|
|
double sigma_l = J_nq * plane.plane_var_ * J_nq.transpose();
|
|
sigma_l += plane.normal_.transpose() * pt->covariance_ * plane.normal_;
|
|
|
|
if (dis_to_plane_abs < 3.0 * std::sqrt(sigma_l))
|
|
{
|
|
if (pt->previous_normal_.dot(plane.normal_) < 0) pt->normal_ = -plane.normal_;
|
|
else pt->normal_ = plane.normal_;
|
|
|
|
double normal_update = (pt->normal_ - pt->previous_normal_).norm();
|
|
pt->previous_normal_ = pt->normal_;
|
|
if (normal_update < 0.0001 && pt->obs_.size() > 10) { pt->is_converged_ = true; }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
float score_max = -1e6;
|
|
Feature *best_ref_ftr = nullptr;
|
|
for (auto it = pt->obs_.begin(); it != pt->obs_.end(); ++it)
|
|
{
|
|
Feature *ref_patch_temp = *it;
|
|
if (!ref_patch_temp) continue;
|
|
float *patch_temp = ref_patch_temp->patch_;
|
|
V3D pf = ref_patch_temp->T_f_w_ * pt->pos_;
|
|
V3D norm_vec = ref_patch_temp->T_f_w_.rotationMatrix() * pt->normal_;
|
|
pf.normalize();
|
|
double cos_angle = pf.dot(norm_vec);
|
|
if (std::fabs(ref_patch_temp->mean_) < 1e-6)
|
|
{
|
|
float sum_val = std::accumulate(patch_temp, patch_temp + patch_size_total, 0.0f);
|
|
ref_patch_temp->mean_ = sum_val / (float)patch_size_total;
|
|
}
|
|
float ref_mean = ref_patch_temp->mean_;
|
|
float sumNCC = 0.0f;
|
|
int countNCC = 0;
|
|
|
|
for (auto itm = pt->obs_.begin(); itm != pt->obs_.end(); ++itm)
|
|
{
|
|
if ((*itm)->id_ == ref_patch_temp->id_) continue;
|
|
float *patch_cache = (*itm)->patch_;
|
|
if (std::fabs((*itm)->mean_) < 1e-6)
|
|
{
|
|
float sum_val2 = std::accumulate(patch_cache, patch_cache + patch_size_total, 0.0f);
|
|
(*itm)->mean_ = sum_val2 / (float)patch_size_total;
|
|
}
|
|
float other_mean = (*itm)->mean_;
|
|
|
|
Eigen::Map<Eigen::VectorXf> patch_a(patch_temp, patch_size_total);
|
|
Eigen::Map<Eigen::VectorXf> patch_b(patch_cache, patch_size_total);
|
|
Eigen::VectorXf diff1 = (patch_a.cast<double>().array() - ref_mean).matrix().cast<float>();
|
|
Eigen::VectorXf diff2 = (patch_b.cast<double>().array() - other_mean).matrix().cast<float>();
|
|
double numerator = diff1.dot(diff2);
|
|
double denominator1 = diff1.squaredNorm();
|
|
double denominator2 = diff2.squaredNorm();
|
|
double ncc_val = numerator / std::sqrt(denominator1 * denominator2 + 1e-10);
|
|
sumNCC += std::fabs(ncc_val);
|
|
countNCC++;
|
|
}
|
|
float NCC_avg = (countNCC > 0) ? (sumNCC / (float)countNCC) : 0.0f;
|
|
float score = NCC_avg + (float)cos_angle;
|
|
ref_patch_temp->score_ = score;
|
|
if (score > score_max)
|
|
{
|
|
score_max = score;
|
|
best_ref_ftr = ref_patch_temp;
|
|
}
|
|
}
|
|
if (best_ref_ftr)
|
|
{
|
|
pt->ref_patch = best_ref_ftr;
|
|
pt->has_ref_patch_ = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
void VIOManager::projectPatchFromRefToCur(const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &plane_map)
|
|
{
|
|
// Debug-only visualization (invoked from processFrame() only when
|
|
// plot_flag is set); always uses camera 0.
|
|
if (total_points == 0) return;
|
|
|
|
int patch_size_dbg = 25;
|
|
string dir = string(ROOT_DIR) + "Log/ref_cur_combine/";
|
|
|
|
cv::Mat result = cv::Mat::zeros(height, width, CV_8UC1);
|
|
cv::Mat result_normal = cv::Mat::zeros(height, width, CV_8UC1);
|
|
|
|
cv::Mat img_photometric_error = new_frame_->imgs_[0].clone();
|
|
|
|
uchar *it = (uchar *)result.data;
|
|
uchar *it_normal = (uchar *)result_normal.data;
|
|
|
|
struct pixel_member
|
|
{
|
|
Vector2f pixel_pos;
|
|
uint8_t pixel_value;
|
|
};
|
|
|
|
int num = 0;
|
|
for (int i = 0; i < (int)visual_submap->voxel_points.size(); i++)
|
|
{
|
|
VisualPoint *pt = visual_submap->voxel_points[i];
|
|
|
|
if (pt->is_normal_initialized_)
|
|
{
|
|
Feature *ref_ftr = pt->ref_patch;
|
|
V2D pc(new_frame_->w2c(pt->pos_, 0));
|
|
V2D pc_prior(new_frame_->w2c_prior(pt->pos_, 0));
|
|
|
|
V3D norm_vec(ref_ftr->T_f_w_.rotationMatrix() * pt->normal_);
|
|
V3D pf(ref_ftr->T_f_w_ * pt->pos_);
|
|
if (pf.dot(norm_vec) < 0) norm_vec = -norm_vec;
|
|
cv::Mat img_cur = new_frame_->imgs_[0];
|
|
cv::Mat img_ref = ref_ftr->img_;
|
|
|
|
SE3<double> T_cur_ref = new_frame_->T_f_w_[0] * ref_ftr->T_f_w_.inverse();
|
|
Matrix2d A_cur_ref;
|
|
getWarpMatrixAffineHomography(*cams[0], ref_ftr->px_, pf, norm_vec, T_cur_ref, 0, A_cur_ref);
|
|
int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2);
|
|
|
|
double D = A_cur_ref.determinant();
|
|
if (D > 3) continue;
|
|
num++;
|
|
|
|
cv::Mat ref_cur_combine_temp;
|
|
int radius = 20;
|
|
cv::hconcat(img_cur, img_ref, ref_cur_combine_temp);
|
|
cv::cvtColor(ref_cur_combine_temp, ref_cur_combine_temp, CV_GRAY2BGR);
|
|
|
|
getImagePatch(img_cur, pc, patch_buffer.data(), 0);
|
|
|
|
float error_est = 0.0;
|
|
{
|
|
double cur_inv_expo = (!state->inv_expo_time_per_cam.empty()) ? state->inv_expo_time_per_cam[0] : 1.0;
|
|
Eigen::Map<Eigen::VectorXf> patch_warp(visual_submap->warp_patch[i].data(), patch_size_total);
|
|
Eigen::Map<Eigen::VectorXf> patch_buf(patch_buffer.data(), patch_size_total);
|
|
Eigen::VectorXf diff = ref_ftr->inv_expo_time_ * patch_warp - cur_inv_expo * patch_buf;
|
|
error_est = diff.squaredNorm();
|
|
}
|
|
std::string ref_est = "ref_est " + std::to_string(1.0 / ref_ftr->inv_expo_time_);
|
|
double cur_inv_expo_str = (!state->inv_expo_time_per_cam.empty()) ? state->inv_expo_time_per_cam[0] : 1.0;
|
|
std::string cur_est = "cur_est " + std::to_string(1.0 / cur_inv_expo_str);
|
|
std::string cur_optimize = "cur_est " + std::to_string(error_est);
|
|
|
|
cv::putText(ref_cur_combine_temp, ref_est, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - 40, ref_ftr->px_[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4,
|
|
cv::Scalar(0, 255, 0), 1, 8, 0);
|
|
cv::putText(ref_cur_combine_temp, cur_est, cv::Point2f(pc[0] - 40, pc[1] + 40), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0);
|
|
cv::putText(ref_cur_combine_temp, cur_optimize, cv::Point2f(pc[0] - 40, pc[1] + 80), cv::FONT_HERSHEY_COMPLEX, 0.4, cv::Scalar(0, 255, 0), 1, 8, 0);
|
|
|
|
cv::rectangle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols - radius, ref_ftr->px_[1] - radius),
|
|
cv::Point2f(ref_ftr->px_[0] + img_cur.cols + radius, ref_ftr->px_[1] + radius), cv::Scalar(0, 0, 255), 1);
|
|
cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc[0] - radius, pc[1] - radius), cv::Point2f(pc[0] + radius, pc[1] + radius), cv::Scalar(0, 255, 0),
|
|
1);
|
|
cv::rectangle(ref_cur_combine_temp, cv::Point2f(pc_prior[0] - radius, pc_prior[1] - radius), cv::Point2f(pc_prior[0] + radius, pc_prior[1] + radius),
|
|
cv::Scalar(255, 255, 255), 1);
|
|
cv::circle(ref_cur_combine_temp, cv::Point2f(ref_ftr->px_[0] + img_cur.cols, ref_ftr->px_[1]), 1, cv::Scalar(0, 0, 255), -1, 8);
|
|
cv::circle(ref_cur_combine_temp, cv::Point2f(pc[0], pc[1]), 1, cv::Scalar(0, 255, 0), -1, 8);
|
|
cv::circle(ref_cur_combine_temp, cv::Point2f(pc_prior[0], pc_prior[1]), 1, cv::Scalar(255, 255, 255), -1, 8);
|
|
cv::imwrite(dir + std::to_string(new_frame_->id_) + "_" + std::to_string(ref_ftr->id_) + "_" + std::to_string(num) + ".png", ref_cur_combine_temp);
|
|
|
|
std::vector<std::vector<pixel_member>> pixel_warp_matrix;
|
|
for (int y = 0; y < patch_size_dbg; ++y)
|
|
{
|
|
vector<pixel_member> pixel_warp_vec;
|
|
for (int x = 0; x < patch_size_dbg; ++x)
|
|
{
|
|
Vector2f px_patch(x - patch_size_dbg / 2, y - patch_size_dbg / 2);
|
|
px_patch *= (1 << search_level);
|
|
const Vector2f px_ref(px_patch + ref_ftr->px_.cast<float>());
|
|
uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]);
|
|
|
|
const Vector2f px(A_cur_ref.cast<float>() * px_patch + pc.cast<float>());
|
|
if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) continue;
|
|
else
|
|
{
|
|
pixel_member pixel_warp;
|
|
pixel_warp.pixel_pos << px[0], px[1];
|
|
pixel_warp.pixel_value = pixel_value;
|
|
pixel_warp_vec.push_back(pixel_warp);
|
|
}
|
|
}
|
|
pixel_warp_matrix.push_back(pixel_warp_vec);
|
|
}
|
|
|
|
float x_min = 1000, y_min = 1000, x_max = 0, y_max = 0;
|
|
for (int r = 0; r < (int)pixel_warp_matrix.size(); r++)
|
|
{
|
|
vector<pixel_member> pixel_warp_row = pixel_warp_matrix[r];
|
|
for (int c = 0; c < (int)pixel_warp_row.size(); c++)
|
|
{
|
|
float x_temp = pixel_warp_row[c].pixel_pos[0];
|
|
float y_temp = pixel_warp_row[c].pixel_pos[1];
|
|
if (x_temp < x_min) x_min = x_temp;
|
|
if (y_temp < y_min) y_min = y_temp;
|
|
if (x_temp > x_max) x_max = x_temp;
|
|
if (y_temp > y_max) y_max = y_temp;
|
|
}
|
|
}
|
|
int x_min_i = floor(x_min);
|
|
int y_min_i = floor(y_min);
|
|
int x_max_i = ceil(x_max);
|
|
int y_max_i = ceil(y_max);
|
|
Matrix2f A_cur_ref_Inv = A_cur_ref.inverse().cast<float>();
|
|
for (int px = x_min_i; px < x_max_i; px++)
|
|
{
|
|
for (int py = y_min_i; py < y_max_i; py++)
|
|
{
|
|
Eigen::Vector2f pc_temp(px, py);
|
|
Vector2f px_patch = A_cur_ref_Inv * (pc_temp - pc.cast<float>());
|
|
if (px_patch[0] > (-patch_size_dbg / 2 * (1 << search_level)) && px_patch[0] < (patch_size_dbg / 2 * (1 << search_level)) &&
|
|
px_patch[1] > (-patch_size_dbg / 2 * (1 << search_level)) && px_patch[1] < (patch_size_dbg / 2 * (1 << search_level)))
|
|
{
|
|
const Vector2f px_ref(px_patch + ref_ftr->px_.cast<float>());
|
|
uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]);
|
|
it_normal[width * py + px] = pixel_value;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
for (int i = 0; i < (int)visual_submap->voxel_points.size(); i++)
|
|
{
|
|
VisualPoint *pt = visual_submap->voxel_points[i];
|
|
if (!pt->is_normal_initialized_) continue;
|
|
|
|
Feature *ref_ftr = pt->ref_patch;
|
|
V2D pc(new_frame_->w2c(pt->pos_, 0));
|
|
|
|
Matrix2d A_cur_ref;
|
|
getWarpMatrixAffine(*cams[0], ref_ftr->px_, ref_ftr->f_, (ref_ftr->pos() - pt->pos_).norm(), new_frame_->T_f_w_[0] * ref_ftr->T_f_w_.inverse(), 0, 0,
|
|
patch_size_half, A_cur_ref);
|
|
int search_level = getBestSearchLevel(A_cur_ref.inverse(), 2);
|
|
double D = A_cur_ref.determinant();
|
|
if (D > 3) continue;
|
|
|
|
cv::Mat img_cur = new_frame_->imgs_[0];
|
|
cv::Mat img_ref = ref_ftr->img_;
|
|
for (int y = 0; y < patch_size_dbg; ++y)
|
|
{
|
|
for (int x = 0; x < patch_size_dbg; ++x)
|
|
{
|
|
Vector2f px_patch(x - patch_size_dbg / 2, y - patch_size_dbg / 2);
|
|
px_patch *= (1 << search_level);
|
|
const Vector2f px_ref(px_patch + ref_ftr->px_.cast<float>());
|
|
uint8_t pixel_value = (uint8_t)vk::interpolateMat_8u(img_ref, px_ref[0], px_ref[1]);
|
|
|
|
const Vector2f px(A_cur_ref.cast<float>() * px_patch + pc.cast<float>());
|
|
if (px[0] < 0 || px[1] < 0 || px[0] >= img_cur.cols - 1 || px[1] >= img_cur.rows - 1) continue;
|
|
else
|
|
{
|
|
int col = int(px[0]);
|
|
int row = int(px[1]);
|
|
it[width * row + col] = pixel_value;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
cv::Mat ref_cur_combine, ref_cur_combine_normal, ref_cur_combine_error;
|
|
|
|
cv::hconcat(result, new_frame_->imgs_[0], ref_cur_combine);
|
|
cv::hconcat(result_normal, new_frame_->imgs_[0], ref_cur_combine_normal);
|
|
|
|
cv::cvtColor(ref_cur_combine, ref_cur_combine, CV_GRAY2BGR);
|
|
cv::cvtColor(ref_cur_combine_normal, ref_cur_combine_normal, CV_GRAY2BGR);
|
|
cv::absdiff(img_photometric_error, result_normal, img_photometric_error);
|
|
cv::hconcat(img_photometric_error, new_frame_->imgs_[0], ref_cur_combine_error);
|
|
|
|
cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_.png", ref_cur_combine);
|
|
cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_photometric.png", ref_cur_combine_error);
|
|
cv::imwrite(dir + std::to_string(new_frame_->id_) + "_0_normal.png", ref_cur_combine_normal);
|
|
}
|
|
|
|
void VIOManager::precomputeReferencePatches(int level)
|
|
{
|
|
double t1 = omp_get_wtime();
|
|
if (total_points == 0) return;
|
|
|
|
const int num_cams = (int)cams.size();
|
|
const int H_DIM = total_points * patch_size_total;
|
|
|
|
H_sub_inv.resize(H_DIM, 6);
|
|
H_sub_inv.setZero();
|
|
|
|
for (int i = 0; i < total_points; i++)
|
|
{
|
|
VisualPoint *pt = visual_submap->voxel_points[i];
|
|
if (!pt) continue;
|
|
|
|
int cam_idx = visual_submap->camera_ids[i];
|
|
if (cam_idx < 0 || cam_idx >= num_cams) continue;
|
|
|
|
double fx_i = cams[cam_idx]->fx();
|
|
double fy_i = cams[cam_idx]->fy();
|
|
|
|
Feature *ref_ftr = pt->ref_patch;
|
|
if (!ref_ftr) continue;
|
|
|
|
cv::Mat &img = ref_ftr->img_;
|
|
if (img.empty()) continue;
|
|
|
|
double depth = (pt->pos_ - ref_ftr->pos()).norm();
|
|
V3D pf = ref_ftr->f_ * depth;
|
|
|
|
V2D pc = ref_ftr->px_;
|
|
M3D R_ref_w = ref_ftr->T_f_w_.rotationMatrix();
|
|
|
|
MD(2, 3) Jdpi;
|
|
computeProjectionJacobian(cam_idx, pf, Jdpi);
|
|
|
|
M3D p_hat;
|
|
p_hat << SKEW_SYM_MATRX(pt->pos_);
|
|
|
|
int search_level = visual_submap->search_levels[i];
|
|
int pyramid_level = level + search_level;
|
|
int scale = (1 << pyramid_level);
|
|
float inv_scale = 1.0f / scale;
|
|
|
|
float u_ref = pc[0];
|
|
float v_ref = pc[1];
|
|
int u_ref_i = (int)std::floor(u_ref / scale) * scale;
|
|
int v_ref_i = (int)std::floor(v_ref / scale) * scale;
|
|
float subpix_u_ref = (u_ref - u_ref_i) / scale;
|
|
float subpix_v_ref = (v_ref - v_ref_i) / scale;
|
|
float w_ref_tl = (1.0f - subpix_u_ref) * (1.0f - subpix_v_ref);
|
|
float w_ref_tr = subpix_u_ref * (1.0f - subpix_v_ref);
|
|
float w_ref_bl = (1.0f - subpix_u_ref) * subpix_v_ref;
|
|
float w_ref_br = subpix_u_ref * subpix_v_ref;
|
|
|
|
int row_offset_pt = i * patch_size_total;
|
|
|
|
for (int px_y = 0; px_y < patch_size; px_y++)
|
|
{
|
|
int row_img = v_ref_i + px_y * scale - patch_size_half * scale;
|
|
if (row_img < 1 || row_img >= img.rows - 1) continue;
|
|
|
|
int col_start = (u_ref_i - patch_size_half * scale);
|
|
if (col_start < 1 || col_start >= img.cols - 1) continue;
|
|
|
|
uint8_t *img_ptr = (uint8_t *)img.data + row_img * img.cols + col_start;
|
|
|
|
for (int px_x = 0; px_x < patch_size; px_x++, img_ptr += scale)
|
|
{
|
|
if ((col_start + px_x * scale) < 1 || (col_start + px_x * scale) >= (img.cols - 1)) continue;
|
|
|
|
float du = 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * img.cols + scale] +
|
|
w_ref_br * img_ptr[scale * img.cols + scale * 2]) -
|
|
(w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * img.cols - scale] +
|
|
w_ref_br * img_ptr[scale * img.cols]));
|
|
float dv = 0.5f * ((w_ref_tl * img_ptr[scale * img.cols] + w_ref_tr * img_ptr[scale + scale * img.cols] +
|
|
w_ref_bl * img_ptr[scale * img.cols * 2] + w_ref_br * img_ptr[scale * img.cols * 2 + scale]) -
|
|
(w_ref_tl * img_ptr[-scale * img.cols] + w_ref_tr * img_ptr[-scale * img.cols + scale] + w_ref_bl * img_ptr[0] +
|
|
w_ref_br * img_ptr[scale]));
|
|
|
|
MD(1, 2) Jimg;
|
|
Jimg << du, dv;
|
|
Jimg *= inv_scale;
|
|
|
|
MD(1, 3) J_dphi = Jimg * Jdpi * (R_ref_w * p_hat);
|
|
MD(1, 3) J_dp = -Jimg * Jdpi * R_ref_w;
|
|
|
|
MD(1, 3) JdR = J_dphi * Jdphi_dR_vec[cam_idx] + J_dp * Jdp_dR_vec[cam_idx];
|
|
MD(1, 3) Jdt = J_dp * Jdp_dt_vec[cam_idx];
|
|
|
|
int row_cur = row_offset_pt + px_y * patch_size + px_x;
|
|
if (row_cur >= 0 && row_cur < H_DIM) { H_sub_inv.block<1, 6>(row_cur, 0) << JdR, Jdt; }
|
|
}
|
|
}
|
|
}
|
|
|
|
has_ref_patch_cache = true;
|
|
compute_jacobian_time += (omp_get_wtime() - t1);
|
|
}
|
|
|
|
void VIOManager::updateStateInverse(const std::vector<cv::Mat> &imgs, int level)
|
|
{
|
|
if (total_points == 0) return;
|
|
StatesGroup old_state = (*state);
|
|
const int num_cams = static_cast<int>(imgs.size());
|
|
const int H_DIM = total_points * patch_size_total;
|
|
|
|
if ((int)state->inv_expo_time_per_cam.size() != (int)cams.size()) { state->inv_expo_time_per_cam.resize(cams.size(), 1.0); }
|
|
|
|
VectorXd z(H_DIM);
|
|
z.setZero();
|
|
MatrixXd H_sub(H_DIM, 6);
|
|
H_sub.setZero();
|
|
|
|
bool EKF_end = false;
|
|
float last_error = std::numeric_limits<float>::max();
|
|
int n_meas = 0;
|
|
|
|
std::vector<float> error_per_cam(num_cams, 0.0f);
|
|
std::vector<int> n_meas_per_cam(num_cams, 0);
|
|
std::vector<float> last_error_per_cam(num_cams, 0.0f);
|
|
|
|
compute_jacobian_time = 0.0;
|
|
update_ekf_time = 0.0;
|
|
M3D P_wi_hat;
|
|
P_wi_hat << SKEW_SYM_MATRX(state->pos_end);
|
|
if (!has_ref_patch_cache) { precomputeReferencePatches(level); }
|
|
for (int iter = 0; iter < max_iterations; iter++)
|
|
{
|
|
double t1 = omp_get_wtime();
|
|
n_meas = 0;
|
|
float error = 0.0f;
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
error_per_cam[c] = 0.0f;
|
|
n_meas_per_cam[c] = 0;
|
|
}
|
|
M3D Rwi(state->rot_end);
|
|
V3D Pwi(state->pos_end);
|
|
|
|
for (int cam_idx = 0; cam_idx < num_cams; cam_idx++)
|
|
{
|
|
Rcw_vec[cam_idx] = Rci_vec[cam_idx] * Rwi.transpose();
|
|
Pcw_vec[cam_idx] = -Rci_vec[cam_idx] * Rwi.transpose() * Pwi + Pci_vec[cam_idx];
|
|
Jdp_dt_vec[cam_idx] = Rci_vec[cam_idx] * Rwi.transpose();
|
|
}
|
|
|
|
#ifdef MP_EN
|
|
omp_set_num_threads(MP_PROC_NUM);
|
|
#pragma omp parallel for schedule(dynamic, 4) reduction(+:error, n_meas)
|
|
#endif
|
|
for (int i_pt = 0; i_pt < total_points; i_pt++)
|
|
{
|
|
float patch_error = 0.0f;
|
|
const int scale = (1 << level);
|
|
|
|
VisualPoint *pt = visual_submap->voxel_points[i_pt];
|
|
if (!pt) continue;
|
|
|
|
int cam_idx = visual_submap->camera_ids[i_pt];
|
|
if (cam_idx < 0 || cam_idx >= num_cams) continue;
|
|
if (imgs[cam_idx].empty()) continue;
|
|
|
|
const cv::Mat &img_cur = imgs[cam_idx];
|
|
double cur_inv_expo = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0;
|
|
|
|
V3D pf = Rcw_vec[cam_idx] * pt->pos_ + Pcw_vec[cam_idx];
|
|
if (pf.z() < 1e-6) continue;
|
|
V2D pc = cams[cam_idx]->world2cam(pf);
|
|
int u_ref_i = static_cast<int>(std::floor(pc[0] / scale)) * scale;
|
|
int v_ref_i = static_cast<int>(std::floor(pc[1] / scale)) * scale;
|
|
float subpix_u_ref = (pc[0] - u_ref_i) / scale;
|
|
float subpix_v_ref = (pc[1] - v_ref_i) / scale;
|
|
float w_ref_tl = (1.f - subpix_u_ref) * (1.f - subpix_v_ref);
|
|
float w_ref_tr = subpix_u_ref * (1.f - subpix_v_ref);
|
|
float w_ref_bl = (1.f - subpix_u_ref) * subpix_v_ref;
|
|
float w_ref_br = subpix_u_ref * subpix_v_ref;
|
|
const std::vector<float> &P_patch = visual_submap->warp_patch[i_pt];
|
|
double inv_ref_expo = visual_submap->inv_expo_list[i_pt];
|
|
int row_offset_pt = i_pt * patch_size_total;
|
|
for (int px_y = 0; px_y < patch_size; px_y++)
|
|
{
|
|
int row_img = v_ref_i + px_y * scale - patch_size_half * scale;
|
|
if (row_img < 1 || row_img >= (img_cur.rows - 1)) continue;
|
|
int col_start = u_ref_i - patch_size_half * scale;
|
|
if (col_start < 1 || col_start >= (img_cur.cols - 1)) continue;
|
|
uint8_t *img_ptr = (uint8_t *)img_cur.data + row_img * img_cur.cols + col_start;
|
|
|
|
for (int px_x = 0; px_x < patch_size; px_x++, img_ptr += scale)
|
|
{
|
|
int cur_col = col_start + px_x * scale;
|
|
if (cur_col < 1 || cur_col >= (img_cur.cols - 1)) continue;
|
|
float du = 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * img_cur.cols + scale] +
|
|
w_ref_br * img_ptr[scale * img_cur.cols + scale * 2]) -
|
|
(w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * img_cur.cols - scale] +
|
|
w_ref_br * img_ptr[scale * img_cur.cols]));
|
|
float dv = 0.5f * ((w_ref_tl * img_ptr[scale * img_cur.cols] + w_ref_tr * img_ptr[scale + scale * img_cur.cols] +
|
|
w_ref_bl * img_ptr[scale * img_cur.cols * 2] + w_ref_br * img_ptr[scale * img_cur.cols * 2 + scale]) -
|
|
(w_ref_tl * img_ptr[-scale * img_cur.cols] + w_ref_tr * img_ptr[-scale * img_cur.cols + scale] + w_ref_bl * img_ptr[0] +
|
|
w_ref_br * img_ptr[scale]));
|
|
|
|
MD(1, 2) Jimg;
|
|
Jimg << du, dv;
|
|
Jimg = Jimg * cur_inv_expo;
|
|
Jimg = Jimg * (1.0f / scale);
|
|
MD(2, 3) Jdpi;
|
|
computeProjectionJacobian(cam_idx, pf, Jdpi);
|
|
M3D p_hat;
|
|
p_hat << SKEW_SYM_MATRX(pf);
|
|
MD(1, 3) J_dphi = Jimg * Jdpi * p_hat;
|
|
MD(1, 3) J_dp = -Jimg * Jdpi;
|
|
MD(1, 3) JdR_local = J_dphi * Jdphi_dR_vec[cam_idx] + J_dp * Jdp_dR_vec[cam_idx];
|
|
MD(1, 3) Jdt_local = J_dp * Jdp_dt_vec[cam_idx];
|
|
int row_idx = row_offset_pt + px_y * patch_size + px_x;
|
|
if (row_idx < 0 || row_idx >= H_DIM) continue;
|
|
MD(1, 3) J_dR_ref = H_sub_inv.block<1, 3>(row_idx, 0);
|
|
MD(1, 3) J_dt_ref = H_sub_inv.block<1, 3>(row_idx, 3);
|
|
MD(1, 3) JdR_final = J_dR_ref * Rwi + J_dt_ref * P_wi_hat * Rwi;
|
|
MD(1, 3) Jdt_final = J_dt_ref * Rwi;
|
|
H_sub.block<1, 6>(row_idx, 0) << JdR_final, Jdt_final;
|
|
float cur_val = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * img_cur.cols] +
|
|
w_ref_br * img_ptr[scale * img_cur.cols + scale];
|
|
|
|
int patch_idx = px_y * patch_size + px_x;
|
|
double res = cur_inv_expo * cur_val - inv_ref_expo * P_patch[patch_size_total * level + patch_idx];
|
|
z(row_idx) = res;
|
|
patch_error += res * res;
|
|
n_meas++;
|
|
}
|
|
}
|
|
|
|
visual_submap->errors[i_pt] = patch_error;
|
|
#pragma omp atomic
|
|
error += patch_error;
|
|
}
|
|
|
|
for (int i_pt = 0; i_pt < total_points; i_pt++)
|
|
{
|
|
int cam_idx = visual_submap->camera_ids[i_pt];
|
|
if (cam_idx >= 0 && cam_idx < num_cams)
|
|
{
|
|
error_per_cam[cam_idx] += visual_submap->errors[i_pt];
|
|
n_meas_per_cam[cam_idx] += patch_size_total;
|
|
}
|
|
}
|
|
|
|
if (n_meas > 0) error /= n_meas;
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
if (n_meas_per_cam[c] > 0) error_per_cam[c] /= n_meas_per_cam[c];
|
|
}
|
|
|
|
compute_jacobian_time += omp_get_wtime() - t1;
|
|
double t3 = omp_get_wtime();
|
|
if (error <= last_error && error > 0)
|
|
{
|
|
old_state = (*state);
|
|
last_error = error;
|
|
for (int c = 0; c < num_cams; c++) { last_error_per_cam[c] = error_per_cam[c]; }
|
|
|
|
std::vector<double> cov_per_cam(num_cams, img_point_cov);
|
|
if (enable_dynamic_covariance_)
|
|
{
|
|
for (int c = 0; c < num_cams; c++) { cov_per_cam[c] = calculateCoVarianceScalePerCam(c, prev_avg_error_per_cam_[c], prev_n_meas_per_cam_[c]); }
|
|
}
|
|
|
|
MatrixXd H_weighted = H_sub;
|
|
VectorXd z_weighted = z;
|
|
for (int i_pt = 0; i_pt < total_points; i_pt++)
|
|
{
|
|
int cam_idx = visual_submap->camera_ids[i_pt];
|
|
if (cam_idx < 0 || cam_idx >= num_cams) continue;
|
|
double weight = 1.0 / std::sqrt(cov_per_cam[cam_idx]);
|
|
int row_start = i_pt * patch_size_total;
|
|
for (int k = 0; k < patch_size_total; k++)
|
|
{
|
|
int row_idx = row_start + k;
|
|
if (row_idx < H_DIM)
|
|
{
|
|
H_weighted.row(row_idx) *= weight;
|
|
z_weighted(row_idx) *= weight;
|
|
}
|
|
}
|
|
}
|
|
|
|
auto H_weighted_T = H_weighted.transpose();
|
|
H_T_H.setZero();
|
|
G.setZero();
|
|
H_T_H.block<6, 6>(0, 0) = H_weighted_T * H_weighted;
|
|
|
|
MD(DIM_STATE, DIM_STATE) K_1 = (H_T_H + state->cov.inverse()).inverse();
|
|
auto HTz = H_weighted_T * z_weighted;
|
|
auto vec = (*state_propagat) - (*state);
|
|
G.block<DIM_STATE, 6>(0, 0) = K_1.block<DIM_STATE, 6>(0, 0) * H_T_H.block<6, 6>(0, 0);
|
|
MD(DIM_STATE, 1) solution = -K_1.block<DIM_STATE, 6>(0, 0) * HTz + vec - G.block<DIM_STATE, 6>(0, 0) * vec.block<6, 1>(0, 0);
|
|
|
|
(*state) += solution;
|
|
|
|
if (exposure_estimate_en)
|
|
{
|
|
std::vector<double> expo_H(num_cams, 0.0);
|
|
std::vector<double> expo_b(num_cams, 0.0);
|
|
|
|
for (int i_pt = 0; i_pt < total_points; i_pt++)
|
|
{
|
|
int cam_idx = visual_submap->camera_ids[i_pt];
|
|
if (cam_idx < 0 || cam_idx >= num_cams) continue;
|
|
|
|
double inv_ref_expo = visual_submap->inv_expo_list[i_pt];
|
|
const std::vector<float> &P_patch = visual_submap->warp_patch[i_pt];
|
|
int row_offset_pt = i_pt * patch_size_total;
|
|
|
|
for (int px_idx = 0; px_idx < patch_size_total; px_idx++)
|
|
{
|
|
int row_idx = row_offset_pt + px_idx;
|
|
if (row_idx >= H_DIM) continue;
|
|
|
|
double residual = z_weighted(row_idx);
|
|
double ref_val = P_patch[patch_size_total * level + px_idx];
|
|
double cur_inv_expo = state->inv_expo_time_per_cam[cam_idx];
|
|
double cur_val = (residual + inv_ref_expo * ref_val) / (cur_inv_expo + 1e-10);
|
|
|
|
double J_expo = cur_val;
|
|
expo_H[cam_idx] += J_expo * J_expo;
|
|
expo_b[cam_idx] += J_expo * residual;
|
|
}
|
|
}
|
|
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
if (expo_H[c] > 1e-6)
|
|
{
|
|
double delta_expo = -expo_b[c] / expo_H[c];
|
|
state->inv_expo_time_per_cam[c] += delta_expo;
|
|
}
|
|
}
|
|
}
|
|
|
|
auto rot_add = solution.block<3, 1>(0, 0);
|
|
auto t_add = solution.block<3, 1>(3, 0);
|
|
|
|
if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.0f < 0.001f)) EKF_end = true;
|
|
}
|
|
else
|
|
{
|
|
(*state) = old_state;
|
|
EKF_end = true;
|
|
}
|
|
update_ekf_time += omp_get_wtime() - t3;
|
|
|
|
if (iter == max_iterations - 1 || EKF_end) break;
|
|
}
|
|
|
|
prev_avg_error_ = last_error;
|
|
prev_n_meas_ = n_meas;
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
if (c < (int)prev_avg_error_per_cam_.size())
|
|
{
|
|
prev_avg_error_per_cam_[c] = last_error_per_cam[c];
|
|
prev_n_meas_per_cam_[c] = n_meas_per_cam[c];
|
|
}
|
|
}
|
|
}
|
|
|
|
void VIOManager::updateState(const std::vector<cv::Mat> &imgs, int level)
|
|
{
|
|
if (total_points == 0) return;
|
|
StatesGroup old_state = (*state);
|
|
const int num_cams = (int)cams.size();
|
|
int base_measurement_size = total_points * patch_size_total * num_cams;
|
|
int cross_cam_measurements = total_points * patch_size_total * num_cams * (num_cams - 1) / 2;
|
|
int total_measurements = base_measurement_size + cross_cam_measurements;
|
|
|
|
VectorXd z(total_measurements);
|
|
z.setZero();
|
|
MatrixXd H_sub(total_measurements, 6 + num_cams);
|
|
H_sub.setZero();
|
|
|
|
bool EKF_end = false;
|
|
float last_error = std::numeric_limits<float>::max();
|
|
int n_meas = 0;
|
|
|
|
std::vector<float> error_per_cam(num_cams, 0.0f);
|
|
std::vector<int> n_meas_per_cam(num_cams, 0);
|
|
std::vector<float> last_error_per_cam(num_cams, 0.0f);
|
|
std::vector<int> row_start_per_cam(num_cams, 0);
|
|
|
|
successful_cross_camera_tracks = 0;
|
|
|
|
for (int iteration = 0; iteration < max_iterations; iteration++)
|
|
{
|
|
double t1 = omp_get_wtime();
|
|
M3D Rwi(state->rot_end);
|
|
V3D Pwi(state->pos_end);
|
|
|
|
if ((int)state->inv_expo_time_per_cam.size() != (int)cams.size()) { state->inv_expo_time_per_cam.resize(cams.size(), 1.0); }
|
|
float error = 0.0f;
|
|
n_meas = 0;
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
error_per_cam[c] = 0.0f;
|
|
n_meas_per_cam[c] = 0;
|
|
}
|
|
int cross_cam_row_offset = base_measurement_size;
|
|
|
|
struct CrossCameraPair
|
|
{
|
|
VisualPoint *pt;
|
|
int cam_a;
|
|
int cam_b;
|
|
};
|
|
std::vector<CrossCameraPair> cross_camera_pairs;
|
|
cross_camera_pairs.reserve(total_points);
|
|
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
row_start_per_cam[c] = c * total_points * patch_size_total;
|
|
if (!imgs[c].empty())
|
|
{
|
|
Rcw_vec[c] = Rci_vec[c] * Rwi.transpose();
|
|
Pcw_vec[c] = -Rci_vec[c] * Rwi.transpose() * Pwi + Pci_vec[c];
|
|
Jdp_dt_vec[c] = Rci_vec[c] * Rwi.transpose();
|
|
}
|
|
}
|
|
|
|
#ifdef MP_EN
|
|
omp_set_num_threads(MP_PROC_NUM);
|
|
#pragma omp parallel for schedule(dynamic, 4) reduction(+:error, n_meas)
|
|
#endif
|
|
for (int i_pt = 0; i_pt < total_points; i_pt++)
|
|
{
|
|
float patch_error = 0.0f;
|
|
|
|
VisualPoint *pt = visual_submap->voxel_points[i_pt];
|
|
if (!pt) continue;
|
|
|
|
int cam_idx = visual_submap->camera_ids[i_pt];
|
|
if (cam_idx < 0 || cam_idx >= num_cams) continue;
|
|
if (imgs[cam_idx].empty()) continue;
|
|
|
|
const cv::Mat &img_cur = imgs[cam_idx];
|
|
|
|
V3D pf = Rcw_vec[cam_idx] * pt->pos_ + Pcw_vec[cam_idx];
|
|
if (pf.z() < 1e-6) continue;
|
|
|
|
V2D pc = cams[cam_idx]->world2cam(pf);
|
|
MD(2, 3) Jdpi;
|
|
computeProjectionJacobian(cam_idx, pf, Jdpi);
|
|
M3D p_hat;
|
|
p_hat << SKEW_SYM_MATRX(pf);
|
|
int scale = (1 << level);
|
|
float inv_scale = 1.0f / scale;
|
|
|
|
float u_ref = pc[0];
|
|
float v_ref = pc[1];
|
|
int u_ref_i = floorf(u_ref / scale) * scale;
|
|
int v_ref_i = floorf(v_ref / scale) * scale;
|
|
float subpix_u_ref = (u_ref - u_ref_i) * inv_scale;
|
|
float subpix_v_ref = (v_ref - v_ref_i) * inv_scale;
|
|
float w_ref_tl = (1.f - subpix_u_ref) * (1.f - subpix_v_ref);
|
|
float w_ref_tr = subpix_u_ref * (1.f - subpix_v_ref);
|
|
float w_ref_bl = (1.f - subpix_u_ref) * subpix_v_ref;
|
|
float w_ref_br = subpix_u_ref * subpix_v_ref;
|
|
const std::vector<float> &P = visual_submap->warp_patch[i_pt];
|
|
double inv_ref_expo = visual_submap->inv_expo_list[i_pt];
|
|
float cur_inv_expo = (cam_idx < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[cam_idx] : 1.0;
|
|
if ((int)P.size() <= patch_size_total * level) continue;
|
|
|
|
int row_offset_pt = row_start_per_cam[cam_idx] + i_pt * patch_size_total;
|
|
for (int px_x = 0; px_x < patch_size; px_x++)
|
|
{
|
|
int row_img = v_ref_i + px_x * scale - patch_size_half * scale;
|
|
if (row_img < 1 || row_img >= (img_cur.rows - 1)) continue;
|
|
uint8_t *img_ptr = (uint8_t *)img_cur.data + row_img * width + (u_ref_i - patch_size_half * scale);
|
|
|
|
for (int px_y = 0; px_y < patch_size; px_y++, img_ptr += scale)
|
|
{
|
|
int col_img = (u_ref_i - patch_size_half * scale) + px_y * scale;
|
|
if (col_img < 1 || col_img >= (img_cur.cols - 1)) continue;
|
|
float du = 0.5f * ((w_ref_tl * img_ptr[scale] + w_ref_tr * img_ptr[scale * 2] + w_ref_bl * img_ptr[scale * width + scale] +
|
|
w_ref_br * img_ptr[scale * width + scale * 2]) -
|
|
(w_ref_tl * img_ptr[-scale] + w_ref_tr * img_ptr[0] + w_ref_bl * img_ptr[scale * width - scale] +
|
|
w_ref_br * img_ptr[scale * width]));
|
|
float dv = 0.5f * ((w_ref_tl * img_ptr[scale * width] + w_ref_tr * img_ptr[scale + scale * width] + w_ref_bl * img_ptr[scale * width * 2] +
|
|
w_ref_br * img_ptr[scale * width * 2 + scale]) -
|
|
(w_ref_tl * img_ptr[-scale * width] + w_ref_tr * img_ptr[-scale * width + scale] + w_ref_bl * img_ptr[0] +
|
|
w_ref_br * img_ptr[scale]));
|
|
MD(1, 2) Jimg;
|
|
Jimg << du, dv;
|
|
Jimg *= (float)cur_inv_expo;
|
|
Jimg *= inv_scale;
|
|
MD(1, 3) Jdphi = Jimg * Jdpi * p_hat;
|
|
MD(1, 3) Jdp = -Jimg * Jdpi;
|
|
MD(1, 3) JdR = Jdphi * Jdphi_dR_vec[cam_idx] + Jdp * Jdp_dR_vec[cam_idx];
|
|
MD(1, 3) Jdt = Jdp * Jdp_dt_vec[cam_idx];
|
|
float cur_val =
|
|
w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[scale] + w_ref_bl * img_ptr[scale * width] + w_ref_br * img_ptr[scale * width + scale];
|
|
int idx_patch = px_x * patch_size + px_y;
|
|
float ref_val = P[patch_size_total * level + idx_patch];
|
|
|
|
float res;
|
|
if (exposure_estimate_en) { res = cur_inv_expo * cur_val - inv_ref_expo * ref_val; }
|
|
else { res = ref_val - cur_val; }
|
|
float corrected_res = res;
|
|
|
|
if (pt->ref_patch && isRealCrossCameraPoint(pt, cam_idx))
|
|
{
|
|
V2D px_pos(pc);
|
|
float source_intensity = applyCameraPhotoCorrection(inv_ref_expo * ref_val, pt->ref_patch->cam_id_, pt->ref_patch->px_);
|
|
float target_intensity = applyCameraPhotoCorrection(cur_inv_expo * cur_val, cam_idx, px_pos);
|
|
corrected_res = source_intensity - target_intensity;
|
|
}
|
|
|
|
float w = 1.0f;
|
|
float abs_res = std::abs(corrected_res);
|
|
if (abs_res > outlier_threshold) { w = 0.0f; }
|
|
else
|
|
{
|
|
float ratio = corrected_res / outlier_threshold;
|
|
float temp = 1.0f - ratio * ratio;
|
|
w = temp * temp;
|
|
}
|
|
|
|
float sqrt_w = sqrtf(w);
|
|
int row_here = row_offset_pt + idx_patch;
|
|
z(row_here) = sqrt_w * corrected_res;
|
|
H_sub(row_here, 0) = sqrt_w * JdR(0);
|
|
H_sub(row_here, 1) = sqrt_w * JdR(1);
|
|
H_sub(row_here, 2) = sqrt_w * JdR(2);
|
|
H_sub(row_here, 3) = sqrt_w * Jdt(0);
|
|
H_sub(row_here, 4) = sqrt_w * Jdt(1);
|
|
H_sub(row_here, 5) = sqrt_w * Jdt(2);
|
|
|
|
if (exposure_estimate_en)
|
|
{
|
|
float expo_jacobian = cur_val;
|
|
int expo_col = 6 + cam_idx;
|
|
H_sub(row_here, expo_col) = sqrt_w * expo_jacobian;
|
|
}
|
|
patch_error += w * (corrected_res * corrected_res);
|
|
n_meas++;
|
|
}
|
|
}
|
|
visual_submap->errors[i_pt] = patch_error;
|
|
error += patch_error;
|
|
#pragma omp critical
|
|
{
|
|
error_per_cam[cam_idx] += patch_error;
|
|
n_meas_per_cam[cam_idx] += patch_size_total;
|
|
}
|
|
|
|
if (enable_cross_camera_tracking)
|
|
{
|
|
for (int other_cam = cam_idx + 1; other_cam < num_cams; other_cam++)
|
|
{
|
|
if (pt->cross_cam_data_.currently_visible.test(cam_idx) && pt->cross_cam_data_.currently_visible.test(other_cam))
|
|
{
|
|
#pragma omp critical
|
|
{ cross_camera_pairs.push_back({pt, cam_idx, other_cam}); }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
successful_cross_camera_tracks = 0;
|
|
for (const auto &pair : cross_camera_pairs)
|
|
{
|
|
addCrossCameraConsistencyConstraint(pair.pt, pair.cam_a, pair.cam_b, H_sub, z, level, cross_cam_row_offset);
|
|
successful_cross_camera_tracks++;
|
|
}
|
|
|
|
if (n_meas > 0) { error /= (float)n_meas; }
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
if (n_meas_per_cam[c] > 0) error_per_cam[c] /= n_meas_per_cam[c];
|
|
}
|
|
|
|
compute_jacobian_time += (omp_get_wtime() - t1);
|
|
double t3 = omp_get_wtime();
|
|
|
|
if (error <= last_error)
|
|
{
|
|
old_state = (*state);
|
|
last_error = error;
|
|
for (int c = 0; c < num_cams; c++) { last_error_per_cam[c] = error_per_cam[c]; }
|
|
|
|
std::vector<double> cov_per_cam(num_cams, img_point_cov);
|
|
if (enable_dynamic_covariance_)
|
|
{
|
|
for (int c = 0; c < num_cams; c++) { cov_per_cam[c] = calculateCoVarianceScalePerCam(c, prev_avg_error_per_cam_[c], prev_n_meas_per_cam_[c]); }
|
|
}
|
|
|
|
MatrixXd H_weighted = H_sub;
|
|
VectorXd z_weighted = z;
|
|
int cam_meas_size = total_points * patch_size_total;
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
double weight = 1.0 / std::sqrt(cov_per_cam[c]);
|
|
int row_start = row_start_per_cam[c];
|
|
int row_end = row_start + cam_meas_size;
|
|
for (int row = row_start; row < row_end && row < total_measurements; row++)
|
|
{
|
|
H_weighted.row(row) *= weight;
|
|
z_weighted(row) *= weight;
|
|
}
|
|
}
|
|
|
|
auto H_weighted_T = H_weighted.transpose();
|
|
H_T_H.setZero();
|
|
G.setZero();
|
|
|
|
int calib_dim = 6 + num_cams;
|
|
MatrixXd H_T_H_expo(calib_dim, calib_dim);
|
|
H_T_H_expo = H_weighted_T * H_weighted;
|
|
|
|
auto HTz = H_weighted_T * z_weighted;
|
|
auto vec = (*state_propagat) - (*state);
|
|
|
|
MD(6, 6) K_pose = (H_T_H_expo.block<6, 6>(0, 0) + state->cov.block<6, 6>(0, 0).inverse()).inverse();
|
|
VectorXd solution_pose = -K_pose * HTz.head<6>() + vec.head<6>();
|
|
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
int expo_idx = 6 + c;
|
|
double H_ii = H_T_H_expo(expo_idx, expo_idx);
|
|
if (H_ii > 1e-6)
|
|
{
|
|
double expo_before = state->inv_expo_time_per_cam[c];
|
|
double delta_expo = -HTz(expo_idx) / H_ii;
|
|
double expo_after = expo_before + delta_expo;
|
|
|
|
const double min_inv_expo = 0.001;
|
|
const double max_inv_expo = 100.0;
|
|
if (expo_after < min_inv_expo) expo_after = min_inv_expo;
|
|
else if (expo_after > max_inv_expo) expo_after = max_inv_expo;
|
|
|
|
state->inv_expo_time_per_cam[c] = expo_after;
|
|
}
|
|
}
|
|
|
|
MD(DIM_STATE, 1) solution = MD(DIM_STATE, 1)::Zero();
|
|
solution.head<6>() = solution_pose;
|
|
(*state) += solution;
|
|
|
|
auto rot_add = solution.block<3, 1>(0, 0);
|
|
auto t_add = solution.block<3, 1>(3, 0);
|
|
|
|
if ((rot_add.norm() * 57.3f < 0.001f) && (t_add.norm() * 100.f < 0.001f)) { EKF_end = true; }
|
|
}
|
|
else
|
|
{
|
|
(*state) = old_state;
|
|
EKF_end = true;
|
|
}
|
|
|
|
update_ekf_time += (omp_get_wtime() - t3);
|
|
if (iteration == max_iterations - 1 || EKF_end) break;
|
|
}
|
|
|
|
prev_avg_error_ = last_error;
|
|
prev_n_meas_ = n_meas;
|
|
for (int c = 0; c < num_cams; c++)
|
|
{
|
|
if (c < (int)prev_avg_error_per_cam_.size())
|
|
{
|
|
prev_avg_error_per_cam_[c] = last_error_per_cam[c];
|
|
prev_n_meas_per_cam_[c] = n_meas_per_cam[c];
|
|
}
|
|
}
|
|
}
|
|
|
|
void VIOManager::updateFrameState(StatesGroup state)
|
|
{
|
|
M3D Rwi(state.rot_end);
|
|
V3D Pwi(state.pos_end);
|
|
if (new_frame_->T_f_w_.size() != cams.size()) { new_frame_->T_f_w_.resize(cams.size(), SE3<double>()); }
|
|
for (int cam_idx = 0; cam_idx < (int)cams.size(); cam_idx++)
|
|
{
|
|
M3D Rcw = Rci_vec[cam_idx] * Rwi.transpose();
|
|
V3D Pcw = -Rci_vec[cam_idx] * Rwi.transpose() * Pwi + Pci_vec[cam_idx];
|
|
new_frame_->T_f_w_[cam_idx] = SE3<double>(Eigen::Quaterniond(Rcw).normalized().toRotationMatrix(), Pcw); // avoid R is not orthogonal
|
|
}
|
|
}
|
|
|
|
void VIOManager::plotTrackedPoints()
|
|
{
|
|
if (visual_submap->voxel_points.empty() || imgs_rgb.empty()) return;
|
|
int num_cams = std::min(cams.size(), imgs_rgb.size());
|
|
if (num_cams == 0) return;
|
|
|
|
int grid_rows, grid_cols;
|
|
if (num_cams <= 1) { grid_rows = grid_cols = 1; }
|
|
else
|
|
{
|
|
grid_rows = (int)ceil(sqrt((double)num_cams));
|
|
grid_cols = (int)ceil((double)num_cams / grid_rows);
|
|
if ((grid_rows - 1) * (grid_cols + 1) >= num_cams && (grid_rows - 1) > 0 && abs((grid_rows - 1) - (grid_cols + 1)) < abs(grid_rows - grid_cols))
|
|
{
|
|
grid_rows--;
|
|
grid_cols++;
|
|
}
|
|
}
|
|
|
|
const double SCALE_FACTOR = 0.25;
|
|
int img_width = imgs_rgb[0].cols;
|
|
int img_height = imgs_rgb[0].rows;
|
|
int display_width = static_cast<int>(img_width * SCALE_FACTOR);
|
|
int display_height = static_cast<int>(img_height * SCALE_FACTOR);
|
|
|
|
cv::Mat display = cv::Mat(grid_rows * display_height, grid_cols * display_width, CV_8UC3, cv::Scalar(0, 0, 0));
|
|
cv::Scalar normal_color(0, 255, 0);
|
|
|
|
for (int cam_idx = 0; cam_idx < num_cams; cam_idx++)
|
|
{
|
|
cv::Mat cam_img_resized;
|
|
cv::resize(imgs_rgb[cam_idx], cam_img_resized, cv::Size(display_width, display_height));
|
|
|
|
int row = cam_idx / grid_cols;
|
|
int col = cam_idx % grid_cols;
|
|
cv::Rect roi(col * display_width, row * display_height, display_width, display_height);
|
|
|
|
for (size_t i = 0; i < visual_submap->voxel_points.size(); i++)
|
|
{
|
|
VisualPoint *pt = visual_submap->voxel_points[i];
|
|
if (!pt) continue;
|
|
|
|
int pt_cam_id = visual_submap->camera_ids[i];
|
|
if (pt_cam_id != cam_idx) continue;
|
|
|
|
V3D pt_cam = new_frame_->w2f(pt->pos_, cam_idx);
|
|
if (pt_cam[2] > 0)
|
|
{
|
|
V2D pc = new_frame_->w2c(pt->pos_, cam_idx);
|
|
int x = static_cast<int>(pc[0] * SCALE_FACTOR);
|
|
int y = static_cast<int>(pc[1] * SCALE_FACTOR);
|
|
if (x >= 0 && x < display_width && y >= 0 && y < display_height) { cv::circle(cam_img_resized, cv::Point(x, y), 1, normal_color, -1); }
|
|
}
|
|
}
|
|
|
|
cam_img_resized.copyTo(display(roi));
|
|
}
|
|
|
|
panorama_image = display;
|
|
}
|
|
|
|
V3F VIOManager::getInterpolatedPixel(cv::Mat img, V2D pc)
|
|
{
|
|
const float u_ref = pc[0];
|
|
const float v_ref = pc[1];
|
|
const int u_ref_i = floorf(pc[0]);
|
|
const int v_ref_i = floorf(pc[1]);
|
|
const float subpix_u_ref = (u_ref - u_ref_i);
|
|
const float subpix_v_ref = (v_ref - v_ref_i);
|
|
const float w_ref_tl = (1.0 - subpix_u_ref) * (1.0 - subpix_v_ref);
|
|
const float w_ref_tr = subpix_u_ref * (1.0 - subpix_v_ref);
|
|
const float w_ref_bl = (1.0 - subpix_u_ref) * subpix_v_ref;
|
|
const float w_ref_br = subpix_u_ref * subpix_v_ref;
|
|
uint8_t *img_ptr = (uint8_t *)img.data + ((v_ref_i)*width + (u_ref_i)) * 3;
|
|
float B = w_ref_tl * img_ptr[0] + w_ref_tr * img_ptr[0 + 3] + w_ref_bl * img_ptr[width * 3] + w_ref_br * img_ptr[width * 3 + 0 + 3];
|
|
float G = w_ref_tl * img_ptr[1] + w_ref_tr * img_ptr[1 + 3] + w_ref_bl * img_ptr[1 + width * 3] + w_ref_br * img_ptr[width * 3 + 1 + 3];
|
|
float R = w_ref_tl * img_ptr[2] + w_ref_tr * img_ptr[2 + 3] + w_ref_bl * img_ptr[2 + width * 3] + w_ref_br * img_ptr[width * 3 + 2 + 3];
|
|
V3F pixel(B, G, R);
|
|
return pixel;
|
|
}
|
|
|
|
void VIOManager::dumpDataForColmap()
|
|
{
|
|
static int cnt = 1;
|
|
std::ostringstream ss;
|
|
ss << std::setw(5) << std::setfill('0') << cnt;
|
|
std::string cnt_str = ss.str();
|
|
if (cams.empty())
|
|
{
|
|
std::cerr << "No cameras available for COLMAP export" << std::endl;
|
|
return;
|
|
}
|
|
for (size_t cam_idx = 0; cam_idx < cams.size(); ++cam_idx)
|
|
{
|
|
std::string image_path = std::string(ROOT_DIR) + "Log/Colmap/images/" + cnt_str + "_cam" + std::to_string(cam_idx) + ".png";
|
|
if (cam_idx >= imgs_rgb.size() || imgs_rgb[cam_idx].empty()) { continue; }
|
|
|
|
cv::Mat img_rgb = imgs_rgb[cam_idx];
|
|
cv::Mat img_rgb_undistort;
|
|
vk::PinholeCamera *pinhole_cam_ptr = dynamic_cast<vk::PinholeCamera *>(cams[cam_idx]);
|
|
if (!pinhole_cam_ptr) { img_rgb_undistort = img_rgb.clone(); }
|
|
else { pinhole_cam_ptr->undistortImage(img_rgb, img_rgb_undistort); }
|
|
cv::imwrite(image_path, img_rgb_undistort);
|
|
}
|
|
static bool cameras_written = false;
|
|
if (!cameras_written)
|
|
{
|
|
fout_camera.open(DEBUG_FILE_DIR("Colmap/sparse/0/cameras.txt"), std::ios::out);
|
|
fout_camera << "# Camera list with one line of data per camera:\n";
|
|
fout_camera << "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n";
|
|
for (size_t cam_idx = 0; cam_idx < cams.size(); ++cam_idx)
|
|
{
|
|
fout_camera << (cam_idx + 1) << " PINHOLE " << cams[cam_idx]->width() << " " << cams[cam_idx]->height() << " " << std::fixed << std::setprecision(6)
|
|
<< cams[cam_idx]->fx() << " " << cams[cam_idx]->fy() << " " << cams[cam_idx]->cx() << " " << cams[cam_idx]->cy() << std::endl;
|
|
}
|
|
fout_camera.close();
|
|
cameras_written = true;
|
|
}
|
|
if (!fout_colmap.is_open())
|
|
{
|
|
fout_colmap.open(DEBUG_FILE_DIR("Colmap/sparse/0/images.txt"), std::ios::out);
|
|
fout_colmap << "# Image list with two lines of data per image:\n";
|
|
fout_colmap << "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n";
|
|
fout_colmap << "# POINTS2D[] as (X, Y, POINT3D_ID)\n";
|
|
}
|
|
for (size_t cam_idx = 0; cam_idx < cams.size(); ++cam_idx)
|
|
{
|
|
if (cam_idx >= new_frame_->T_f_w_.size()) continue;
|
|
SE3<double> cam_pose = new_frame_->T_f_w_[cam_idx];
|
|
Eigen::Quaterniond q(cam_pose.rotationMatrix());
|
|
Eigen::Vector3d t = cam_pose.translation();
|
|
int image_id = cnt * 100 + static_cast<int>(cam_idx);
|
|
std::string image_name = cnt_str + "_cam" + std::to_string(cam_idx) + ".png";
|
|
fout_colmap << image_id << " " << std::fixed << std::setprecision(6) << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " << t.x() << " "
|
|
<< t.y() << " " << t.z() << " " << (cam_idx + 1) << " " << image_name << std::endl;
|
|
fout_colmap << "0.0 0.0 -1.0" << std::endl;
|
|
}
|
|
|
|
cnt++;
|
|
}
|
|
|
|
void VIOManager::initializeCameraPhotoParams()
|
|
{
|
|
camera_photo_params.resize(cams.size());
|
|
for (size_t i = 0; i < cams.size(); i++)
|
|
{
|
|
camera_photo_params[i].exposure_factor = 1.0;
|
|
camera_photo_params[i].vignetting = {0.0, 0.0, 0.0};
|
|
camera_photo_params[i].parameters_initialized = false;
|
|
}
|
|
}
|
|
|
|
float VIOManager::applyCameraPhotoCorrection(float intensity, int cam_id, const V2D &pixel_pos)
|
|
{
|
|
if (cam_id < 0 || cam_id >= (int)camera_photo_params.size()) { return intensity; }
|
|
float w = cams[cam_id]->width();
|
|
float h = cams[cam_id]->height();
|
|
|
|
float dx = (pixel_pos[0] / w) - 0.5f;
|
|
float dy = (pixel_pos[1] / h) - 0.5f;
|
|
float r2 = dx * dx + dy * dy;
|
|
float vignette_factor = 1.0f;
|
|
if (!camera_photo_params[cam_id].vignetting.empty())
|
|
{
|
|
vignette_factor = 1.0f + camera_photo_params[cam_id].vignetting[0] * r2 + camera_photo_params[cam_id].vignetting[1] * r2 * r2 +
|
|
camera_photo_params[cam_id].vignetting[2] * r2 * r2 * r2;
|
|
}
|
|
return (intensity * vignette_factor) * camera_photo_params[cam_id].exposure_factor;
|
|
}
|
|
|
|
void VIOManager::cleanupOldVisualPoints()
|
|
{
|
|
if (feat_map.empty()) return;
|
|
|
|
int current_frame_id = new_frame_ ? new_frame_->id_ : 0;
|
|
|
|
std::vector<VOXEL_LOCATION> voxels_to_delete;
|
|
voxels_to_delete.reserve(feat_map.size() / 10);
|
|
int total_points_before = 0;
|
|
int total_points_deleted = 0;
|
|
|
|
for (auto &kv : feat_map)
|
|
{
|
|
VOXEL_POINTS *voxel_pts = kv.second;
|
|
if (!voxel_pts) continue;
|
|
|
|
auto &points = voxel_pts->voxel_points;
|
|
total_points_before += points.size();
|
|
|
|
points.erase(std::remove_if(points.begin(), points.end(),
|
|
[this, current_frame_id, &total_points_deleted](VisualPoint *pt) {
|
|
if (!pt) return true;
|
|
|
|
int last_seen = pt->cross_cam_data_.last_seen_frame_id;
|
|
bool too_old = (last_seen > 0) && ((current_frame_id - last_seen) > max_point_age_frames);
|
|
|
|
if (pt->is_converged_ && pt->obs_.size() > 5)
|
|
{
|
|
too_old = (last_seen > 0) && ((current_frame_id - last_seen) > max_point_age_frames * 2);
|
|
}
|
|
|
|
if (too_old)
|
|
{
|
|
delete pt;
|
|
total_points_deleted++;
|
|
return true;
|
|
}
|
|
return false;
|
|
}),
|
|
points.end());
|
|
|
|
if (points.empty()) { voxels_to_delete.push_back(kv.first); }
|
|
}
|
|
|
|
for (auto &loc : voxels_to_delete)
|
|
{
|
|
auto it = feat_map.find(loc);
|
|
if (it != feat_map.end())
|
|
{
|
|
delete it->second;
|
|
feat_map.erase(it);
|
|
}
|
|
}
|
|
|
|
if (total_points_deleted > 0)
|
|
{
|
|
std::cout << "[VIO Memory] Cleaned up " << total_points_deleted << " old points (" << (100.0 * total_points_deleted / std::max(1, total_points_before))
|
|
<< "%), " << voxels_to_delete.size() << " empty voxels. Remaining: " << (total_points_before - total_points_deleted) << " points in "
|
|
<< feat_map.size() << " voxels" << std::endl;
|
|
}
|
|
|
|
last_cleanup_frame_id = current_frame_id;
|
|
}
|
|
|
|
void VIOManager::cleanupVisualMapByTimestamp(double oldest_kept_timestamp)
|
|
{
|
|
if (feat_map.empty() || oldest_kept_timestamp < 0) { return; }
|
|
|
|
int total_voxels_before = feat_map.size();
|
|
int deleted_voxels = 0;
|
|
|
|
for (auto it = feat_map.begin(); it != feat_map.end();)
|
|
{
|
|
if (!it->second)
|
|
{
|
|
it = feat_map.erase(it);
|
|
continue;
|
|
}
|
|
|
|
double voxel_timestamp = it->second->creation_timestamp_;
|
|
if (voxel_timestamp < 0)
|
|
{
|
|
++it;
|
|
continue;
|
|
}
|
|
|
|
if (voxel_timestamp < oldest_kept_timestamp)
|
|
{
|
|
delete it->second;
|
|
it = feat_map.erase(it);
|
|
deleted_voxels++;
|
|
}
|
|
else { ++it; }
|
|
}
|
|
|
|
if (deleted_voxels > 0)
|
|
{
|
|
std::cout << "[VIO Sliding] Deleted " << deleted_voxels << " visual voxels (" << (100.0 * deleted_voxels / std::max(1, total_voxels_before))
|
|
<< "%), remaining: " << feat_map.size() << " voxels" << std::endl;
|
|
}
|
|
}
|
|
|
|
void VIOManager::cleanupOldFrames()
|
|
{
|
|
while ((int)frame_history_.size() > max_frame_history)
|
|
{
|
|
FramePtr old_frame = frame_history_.front();
|
|
frame_history_.pop_front();
|
|
if (old_frame) { old_frame->imgs_shared_.clear(); }
|
|
}
|
|
}
|
|
|
|
void VIOManager::addCrossCameraConsistencyConstraint(VisualPoint *pt, int source_cam_id, int target_cam_id, MatrixXd &H_sub, VectorXd &z, int level,
|
|
int &row_offset)
|
|
{
|
|
if (!pt || !enable_cross_camera_tracking || source_cam_id < 0 || source_cam_id >= (int)cams.size() || target_cam_id < 0 ||
|
|
target_cam_id >= (int)cams.size() || source_cam_id == target_cam_id)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (source_cam_id >= (int)new_frame_->imgs_.size() || target_cam_id >= (int)new_frame_->imgs_.size() || new_frame_->imgs_[source_cam_id].empty() ||
|
|
new_frame_->imgs_[target_cam_id].empty())
|
|
{
|
|
return;
|
|
}
|
|
|
|
V2D source_px = new_frame_->w2c(pt->pos_, source_cam_id);
|
|
V2D target_px = new_frame_->w2c(pt->pos_, target_cam_id);
|
|
|
|
if (!cams[source_cam_id]->isInFrame(source_px.cast<int>(), border) || !cams[target_cam_id]->isInFrame(target_px.cast<int>(), border)) { return; }
|
|
|
|
float source_patch[256];
|
|
float target_patch[256];
|
|
memset(source_patch, 0, sizeof(source_patch));
|
|
memset(target_patch, 0, sizeof(target_patch));
|
|
|
|
getImagePatch(new_frame_->imgs_[source_cam_id], source_px, source_patch, level);
|
|
getImagePatch(new_frame_->imgs_[target_cam_id], target_px, target_patch, level);
|
|
|
|
if (source_cam_id >= (int)camera_photo_params.size() || target_cam_id >= (int)camera_photo_params.size()) { return; }
|
|
|
|
float mean_source = 0, mean_target = 0;
|
|
for (int i = 0; i < patch_size_total; i++)
|
|
{
|
|
mean_source += source_patch[i];
|
|
mean_target += target_patch[i];
|
|
}
|
|
mean_source /= patch_size_total;
|
|
mean_target /= patch_size_total;
|
|
|
|
// Slowly adapt each non-reference camera's exposure_factor toward camera 0
|
|
// (treated as the exposure reference) whenever both patches are
|
|
// well-exposed, so applyCameraPhotoCorrection can normalize brightness
|
|
// across cameras for the cross-camera residual below.
|
|
if (mean_source > 20 && mean_source < 235 && mean_target > 20 && mean_target < 235)
|
|
{
|
|
double ratio = mean_target / mean_source;
|
|
double alpha = 0.005;
|
|
|
|
if (source_cam_id == 0 && target_cam_id > 0)
|
|
{
|
|
camera_photo_params[target_cam_id].exposure_factor = (1 - alpha) * camera_photo_params[target_cam_id].exposure_factor + alpha * ratio;
|
|
}
|
|
else if (target_cam_id == 0 && source_cam_id > 0)
|
|
{
|
|
camera_photo_params[source_cam_id].exposure_factor = (1 - alpha) * camera_photo_params[source_cam_id].exposure_factor + alpha / ratio;
|
|
}
|
|
}
|
|
|
|
double source_to_target_factor = camera_photo_params[target_cam_id].exposure_factor / camera_photo_params[source_cam_id].exposure_factor;
|
|
|
|
double source_inv_expo = (source_cam_id < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[source_cam_id] : 1.0;
|
|
double target_inv_expo = (target_cam_id < (int)state->inv_expo_time_per_cam.size()) ? state->inv_expo_time_per_cam[target_cam_id] : 1.0;
|
|
|
|
V3D p_source = new_frame_->w2f(pt->pos_, source_cam_id);
|
|
V3D p_target = new_frame_->w2f(pt->pos_, target_cam_id);
|
|
if (p_source.z() < 1e-6 || p_target.z() < 1e-6) { return; }
|
|
|
|
MD(2, 3) Jdpi_source, Jdpi_target;
|
|
computeProjectionJacobian(source_cam_id, p_source, Jdpi_source);
|
|
computeProjectionJacobian(target_cam_id, p_target, Jdpi_target);
|
|
|
|
M3D p_hat_source, p_hat_target;
|
|
p_hat_source << SKEW_SYM_MATRX(p_source);
|
|
p_hat_target << SKEW_SYM_MATRX(p_target);
|
|
|
|
float scale = 1 << level;
|
|
float inv_scale = 1.0f / scale;
|
|
|
|
cv::Mat &source_img = new_frame_->imgs_[source_cam_id];
|
|
cv::Mat &target_img = new_frame_->imgs_[target_cam_id];
|
|
|
|
for (int px_y = 0; px_y < patch_size; px_y++)
|
|
{
|
|
for (int px_x = 0; px_x < patch_size; px_x++)
|
|
{
|
|
int patch_idx = px_y * patch_size + px_x;
|
|
|
|
float source_intensity, target_intensity;
|
|
if (exposure_estimate_en)
|
|
{
|
|
source_intensity = source_inv_expo * source_patch[patch_idx];
|
|
target_intensity = target_inv_expo * target_patch[patch_idx];
|
|
}
|
|
else
|
|
{
|
|
source_intensity = source_patch[patch_idx];
|
|
target_intensity = target_patch[patch_idx] / source_to_target_factor;
|
|
}
|
|
float residual = source_intensity - target_intensity;
|
|
float abs_res = std::abs(residual);
|
|
|
|
float weight = (abs_res <= outlier_threshold)
|
|
? (1.0f - (residual / outlier_threshold) * (residual / outlier_threshold)) *
|
|
(1.0f - (residual / outlier_threshold) * (residual / outlier_threshold))
|
|
: 0.0f;
|
|
float sqrt_weight = std::sqrt(weight);
|
|
|
|
int u_source = std::floor(source_px[0]);
|
|
int v_source = std::floor(source_px[1]);
|
|
float du_source = 0, dv_source = 0;
|
|
if (u_source - 1 >= 0 && u_source + 1 < source_img.cols && v_source - 1 >= 0 && v_source + 1 < source_img.rows)
|
|
{
|
|
du_source = (float)(source_img.at<uint8_t>(v_source, u_source + 1) - source_img.at<uint8_t>(v_source, u_source - 1)) * 0.5f;
|
|
dv_source = (float)(source_img.at<uint8_t>(v_source + 1, u_source) - source_img.at<uint8_t>(v_source - 1, u_source)) * 0.5f;
|
|
}
|
|
|
|
int u_target = std::floor(target_px[0]);
|
|
int v_target = std::floor(target_px[1]);
|
|
float du_target = 0, dv_target = 0;
|
|
if (u_target - 1 >= 0 && u_target + 1 < target_img.cols && v_target - 1 >= 0 && v_target + 1 < target_img.rows)
|
|
{
|
|
du_target = (float)(target_img.at<uint8_t>(v_target, u_target + 1) - target_img.at<uint8_t>(v_target, u_target - 1)) * 0.5f;
|
|
dv_target = (float)(target_img.at<uint8_t>(v_target + 1, u_target) - target_img.at<uint8_t>(v_target - 1, u_target)) * 0.5f;
|
|
}
|
|
|
|
MD(1, 2) Jimg_source;
|
|
Jimg_source << du_source, dv_source;
|
|
Jimg_source *= (float)source_inv_expo;
|
|
Jimg_source *= inv_scale;
|
|
|
|
MD(1, 3) Jdphi_source = Jimg_source * Jdpi_source * p_hat_source;
|
|
MD(1, 3) Jdp_source = -Jimg_source * Jdpi_source;
|
|
MD(1, 3) JdR_source = Jdphi_source * Jdphi_dR_vec[source_cam_id] + Jdp_source * Jdp_dR_vec[source_cam_id];
|
|
MD(1, 3) Jdt_source = Jdp_source * Jdp_dt_vec[source_cam_id];
|
|
|
|
MD(1, 2) Jimg_target;
|
|
Jimg_target << du_target, dv_target;
|
|
Jimg_target *= (float)target_inv_expo;
|
|
Jimg_target *= inv_scale;
|
|
|
|
MD(1, 3) Jdphi_target = Jimg_target * Jdpi_target * p_hat_target;
|
|
MD(1, 3) Jdp_target = -Jimg_target * Jdpi_target;
|
|
MD(1, 3) JdR_target = Jdphi_target * Jdphi_dR_vec[target_cam_id] + Jdp_target * Jdp_dR_vec[target_cam_id];
|
|
MD(1, 3) Jdt_target = Jdp_target * Jdp_dt_vec[target_cam_id];
|
|
|
|
MD(1, 3) JdR_coupled = JdR_source - JdR_target;
|
|
MD(1, 3) Jdt_coupled = Jdt_source - Jdt_target;
|
|
|
|
int curr_row = row_offset + patch_idx;
|
|
if (curr_row >= H_sub.rows() || curr_row >= z.size()) continue;
|
|
|
|
H_sub(curr_row, 0) = sqrt_weight * JdR_coupled(0);
|
|
H_sub(curr_row, 1) = sqrt_weight * JdR_coupled(1);
|
|
H_sub(curr_row, 2) = sqrt_weight * JdR_coupled(2);
|
|
H_sub(curr_row, 3) = sqrt_weight * Jdt_coupled(0);
|
|
H_sub(curr_row, 4) = sqrt_weight * Jdt_coupled(1);
|
|
H_sub(curr_row, 5) = sqrt_weight * Jdt_coupled(2);
|
|
|
|
if (exposure_estimate_en)
|
|
{
|
|
float expo_jac_source = source_patch[patch_idx];
|
|
float expo_jac_target = -target_patch[patch_idx];
|
|
|
|
int source_expo_col = 6 + source_cam_id;
|
|
int target_expo_col = 6 + target_cam_id;
|
|
H_sub(curr_row, source_expo_col) = sqrt_weight * expo_jac_source;
|
|
H_sub(curr_row, target_expo_col) = sqrt_weight * expo_jac_target;
|
|
}
|
|
z(curr_row) = sqrt_weight * residual;
|
|
}
|
|
}
|
|
|
|
row_offset += patch_size_total;
|
|
}
|
|
|
|
void VIOManager::processFrame(const std::vector<cv::Mat> &imgs, vector<pointWithVar> &pg, const unordered_map<VOXEL_LOCATION, VoxelOctoTree *> &feat_map,
|
|
double frame_timestamp)
|
|
{
|
|
current_timestamp_ = frame_timestamp;
|
|
|
|
if (imgs.size() != cams.size())
|
|
{
|
|
std::cerr << "[ VIO ] Error: input imgs.size() != cams.size(). is " << imgs.size() << " " << cams.size() << std::endl;
|
|
return;
|
|
}
|
|
imgs_cp.resize(cams.size());
|
|
imgs_rgb.resize(cams.size());
|
|
for (size_t i = 0; i < cams.size(); i++)
|
|
{
|
|
if (imgs[i].empty())
|
|
{
|
|
std::cout << "Camera " << i << " image is empty, skipping." << std::endl;
|
|
continue;
|
|
}
|
|
if (width != imgs[i].cols || height != imgs[i].rows)
|
|
{
|
|
cv::Mat resized;
|
|
cv::resize(imgs[i], resized, cv::Size(width, height), 0, 0, cv::INTER_LINEAR);
|
|
imgs_cp[i] = resized.clone();
|
|
imgs_rgb[i] = resized.clone();
|
|
}
|
|
else
|
|
{
|
|
imgs_cp[i] = imgs[i].clone();
|
|
imgs_rgb[i] = imgs[i].clone();
|
|
}
|
|
if (imgs_cp[i].channels() == 3) { cv::cvtColor(imgs_cp[i], imgs_cp[i], cv::COLOR_BGR2GRAY); }
|
|
}
|
|
|
|
new_frame_.reset(new Frame(cams, imgs_cp, frame_timestamp));
|
|
updateFrameState(*state);
|
|
|
|
// Omni-LIVO declares/implements resetGrid() but never calls it anywhere in
|
|
// its own processFrame() — an upstream bug that leaves total_points (and
|
|
// the grid/map_dist/etc. buffers) uninitialized garbage on frames where
|
|
// retrieveFromVisualSparseMap() early-returns (feat_map still empty, e.g.
|
|
// the very first VIO frame of a run), causing an out-of-bounds access into
|
|
// visual_submap->voxel_points downstream. The original single-camera
|
|
// FAST-LIVO2 code called resetGrid() here; restored for multi-camera too.
|
|
resetGrid();
|
|
|
|
frame_history_.push_back(new_frame_);
|
|
cleanupOldFrames();
|
|
|
|
double t1 = omp_get_wtime();
|
|
retrieveFromVisualSparseMap(imgs_cp, pg, feat_map);
|
|
double t2 = omp_get_wtime();
|
|
computeJacobianAndUpdateEKF(imgs_cp);
|
|
double t3 = omp_get_wtime();
|
|
|
|
generateVisualMapPoints(imgs_cp, pg);
|
|
double t4 = omp_get_wtime();
|
|
plotTrackedPoints();
|
|
if (plot_flag) projectPatchFromRefToCur(feat_map);
|
|
double t5 = omp_get_wtime();
|
|
updateVisualMapPoints(imgs_cp);
|
|
double t6 = omp_get_wtime();
|
|
updateReferencePatch(feat_map);
|
|
|
|
if (new_frame_ && (new_frame_->id_ - last_cleanup_frame_id) >= cleanup_interval_frames) { cleanupOldVisualPoints(); }
|
|
|
|
double t7 = omp_get_wtime();
|
|
|
|
if (colmap_output_en) dumpDataForColmap();
|
|
|
|
frame_count++;
|
|
ave_total = ave_total * (frame_count - 1) / frame_count + (t7 - t1 - (t5 - t4)) / frame_count;
|
|
|
|
printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
|
|
printf("\033[1;34m| VIO Time (MultiCam) |\033[0m\n");
|
|
printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
|
|
printf("\033[1;34m| %-29s | %-27zu |\033[0m\n", "Sparse Map Size", feat_map.size());
|
|
printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
|
|
printf("\033[1;34m| %-29s | %-27s |\033[0m\n", "Algorithm Stage", "Time (secs)");
|
|
printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "retrieveFromVisualSparseMap", t2 - t1);
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "computeJacobianAndUpdateEKF", t3 - t2);
|
|
printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> computeJacobian", compute_jacobian_time);
|
|
printf("\033[1;32m| %-27s | %-27lf |\033[0m\n", "-> updateEKF", update_ekf_time);
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "generateVisualMapPoints", t4 - t3);
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateVisualMapPoints", t6 - t5);
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "updateReferencePatch", t7 - t6);
|
|
printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Current Total Time", t7 - t1 - (t5 - t4));
|
|
printf("\033[1;32m| %-29s | %-27lf |\033[0m\n", "Average Total Time", ave_total);
|
|
printf("\033[1;34m+-------------------------------------------------------------+\033[0m\n");
|
|
}
|