729 lines
23 KiB
C++
729 lines
23 KiB
C++
/**
|
|
BSD 3-Clause License
|
|
|
|
This file is part of the Basalt project.
|
|
https://gitlab.com/VladyslavUsenko/basalt.git
|
|
|
|
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
|
|
* Redistributions of source code must retain the above copyright notice, this
|
|
list of conditions and the following disclaimer.
|
|
|
|
* Redistributions in binary form must reproduce the above copyright notice,
|
|
this list of conditions and the following disclaimer in the documentation
|
|
and/or other materials provided with the distribution.
|
|
|
|
* Neither the name of the copyright holder nor the names of its
|
|
contributors may be used to endorse or promote products derived from
|
|
this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
|
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
|
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
|
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
|
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
|
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
|
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
|
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <basalt/optimization/accumulator.h>
|
|
#include <basalt/utils/keypoints.h>
|
|
#include <basalt/utils/nfr.h>
|
|
#include <basalt/utils/tracks.h>
|
|
#include <basalt/vi_estimator/nfr_mapper.h>
|
|
|
|
#include <basalt/hash_bow/hash_bow.h>
|
|
|
|
namespace basalt {
|
|
|
|
NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
|
|
: config(config),
|
|
lambda(1e-10),
|
|
min_lambda(1e-32),
|
|
max_lambda(1000),
|
|
lambda_vee(2) {
|
|
this->calib = calib;
|
|
this->obs_std_dev = config.mapper_obs_std_dev;
|
|
this->huber_thresh = config.mapper_obs_huber_thresh;
|
|
|
|
hash_bow_database.reset(new HashBow<256>(config.mapper_bow_num_bits));
|
|
}
|
|
|
|
void NfrMapper::addMargData(MargData::Ptr& data) {
|
|
processMargData(*data);
|
|
bool valid = extractNonlinearFactors(*data);
|
|
|
|
if (valid) {
|
|
for (const auto& kv : data->frame_poses) {
|
|
PoseStateWithLin p(kv.second.getT_ns(), kv.second.getPose());
|
|
|
|
frame_poses[kv.first] = p;
|
|
}
|
|
|
|
for (const auto& kv : data->frame_states) {
|
|
if (data->kfs_all.count(kv.first) > 0) {
|
|
auto state = kv.second;
|
|
PoseStateWithLin p(state.getState().t_ns, state.getState().T_w_i);
|
|
frame_poses[kv.first] = p;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void NfrMapper::processMargData(MargData& m) {
|
|
BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
|
|
|
|
// std::cout << "rank " << m.abs_H.fullPivLu().rank() << " size "
|
|
// << m.abs_H.cols() << std::endl;
|
|
|
|
AbsOrderMap aom_new;
|
|
std::set<int> idx_to_keep;
|
|
std::set<int> idx_to_marg;
|
|
|
|
for (const auto& kv : m.aom.abs_order_map) {
|
|
if (kv.second.second == POSE_SIZE) {
|
|
for (size_t i = 0; i < POSE_SIZE; i++)
|
|
idx_to_keep.emplace(kv.second.first + i);
|
|
aom_new.abs_order_map.emplace(kv);
|
|
aom_new.total_size += POSE_SIZE;
|
|
} else if (kv.second.second == POSE_VEL_BIAS_SIZE) {
|
|
if (m.kfs_all.count(kv.first) > 0) {
|
|
for (size_t i = 0; i < POSE_SIZE; i++)
|
|
idx_to_keep.emplace(kv.second.first + i);
|
|
for (size_t i = POSE_SIZE; i < POSE_VEL_BIAS_SIZE; i++)
|
|
idx_to_marg.emplace(kv.second.first + i);
|
|
|
|
aom_new.abs_order_map[kv.first] =
|
|
std::make_pair(aom_new.total_size, POSE_SIZE);
|
|
aom_new.total_size += POSE_SIZE;
|
|
|
|
PoseStateWithLin p = m.frame_states.at(kv.first);
|
|
m.frame_poses[kv.first] = p;
|
|
m.frame_states.erase(kv.first);
|
|
} else {
|
|
for (size_t i = 0; i < POSE_VEL_BIAS_SIZE; i++)
|
|
idx_to_marg.emplace(kv.second.first + i);
|
|
m.frame_states.erase(kv.first);
|
|
}
|
|
} else {
|
|
std::cerr << "Unknown size" << std::endl;
|
|
std::abort();
|
|
}
|
|
|
|
// std::cout << kv.first << " " << kv.second.first << " " <<
|
|
// kv.second.second
|
|
// << std::endl;
|
|
}
|
|
|
|
Eigen::MatrixXd marg_H_new;
|
|
Eigen::VectorXd marg_b_new;
|
|
BundleAdjustmentBase::marginalizeHelper(m.abs_H, m.abs_b, idx_to_keep,
|
|
idx_to_marg, marg_H_new, marg_b_new);
|
|
|
|
// std::cout << "new rank " << marg_H_new.fullPivLu().rank() << " size "
|
|
// << marg_H_new.cols() << std::endl;
|
|
|
|
m.abs_H = marg_H_new;
|
|
m.abs_b = marg_b_new;
|
|
m.aom = aom_new;
|
|
|
|
BASALT_ASSERT(m.aom.total_size == size_t(m.abs_H.cols()));
|
|
|
|
// save image data
|
|
{
|
|
for (const auto& v : m.opt_flow_res) {
|
|
img_data[v->t_ns] = v->input_images;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool NfrMapper::extractNonlinearFactors(MargData& m) {
|
|
size_t asize = m.aom.total_size;
|
|
// std::cout << "asize " << asize << std::endl;
|
|
|
|
Eigen::FullPivHouseholderQR<Eigen::MatrixXd> qr(m.abs_H);
|
|
if (qr.rank() != m.abs_H.cols()) return false;
|
|
|
|
Eigen::MatrixXd cov_old = qr.solve(Eigen::MatrixXd::Identity(asize, asize));
|
|
|
|
int64_t kf_id = *m.kfs_to_marg.cbegin();
|
|
int kf_start_idx = m.aom.abs_order_map.at(kf_id).first;
|
|
|
|
auto state_kf = m.frame_poses.at(kf_id);
|
|
|
|
Sophus::SE3d T_w_i_kf = state_kf.getPose();
|
|
|
|
Eigen::Vector3d pos = T_w_i_kf.translation();
|
|
Eigen::Vector3d yaw_dir_body =
|
|
T_w_i_kf.so3().inverse() * Eigen::Vector3d::UnitX();
|
|
|
|
Sophus::Matrix<double, 3, POSE_SIZE> d_pos_d_T_w_i;
|
|
Sophus::Matrix<double, 1, POSE_SIZE> d_yaw_d_T_w_i;
|
|
Sophus::Matrix<double, 2, POSE_SIZE> d_rp_d_T_w_i;
|
|
|
|
absPositionError(T_w_i_kf, pos, &d_pos_d_T_w_i);
|
|
yawError(T_w_i_kf, yaw_dir_body, &d_yaw_d_T_w_i);
|
|
rollPitchError(T_w_i_kf, T_w_i_kf.so3(), &d_rp_d_T_w_i);
|
|
|
|
{
|
|
Eigen::MatrixXd J;
|
|
J.setZero(POSE_SIZE, asize);
|
|
J.block<3, POSE_SIZE>(0, kf_start_idx) = d_pos_d_T_w_i;
|
|
J.block<1, POSE_SIZE>(3, kf_start_idx) = d_yaw_d_T_w_i;
|
|
J.block<2, POSE_SIZE>(4, kf_start_idx) = d_rp_d_T_w_i;
|
|
|
|
Sophus::Matrix6d cov_new = J * cov_old * J.transpose();
|
|
|
|
// std::cout << "cov_new\n" << cov_new << std::endl;
|
|
|
|
RollPitchFactor rpf;
|
|
rpf.t_ns = kf_id;
|
|
rpf.R_w_i_meas = T_w_i_kf.so3();
|
|
|
|
if (!config.mapper_no_factor_weights) {
|
|
rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse();
|
|
} else {
|
|
rpf.cov_inv.setIdentity();
|
|
}
|
|
|
|
roll_pitch_factors.emplace_back(rpf);
|
|
}
|
|
|
|
for (int64_t other_id : m.kfs_all) {
|
|
if (m.frame_poses.count(other_id) == 0 || other_id == kf_id) {
|
|
continue;
|
|
}
|
|
|
|
auto state_o = m.frame_poses.at(other_id);
|
|
|
|
Sophus::SE3d T_w_i_o = state_o.getPose();
|
|
Sophus::SE3d T_kf_o = T_w_i_kf.inverse() * T_w_i_o;
|
|
|
|
int o_start_idx = m.aom.abs_order_map.at(other_id).first;
|
|
|
|
Sophus::Matrix6d d_res_d_T_w_i, d_res_d_T_w_j;
|
|
relPoseError(T_kf_o, T_w_i_kf, T_w_i_o, &d_res_d_T_w_i, &d_res_d_T_w_j);
|
|
|
|
Eigen::MatrixXd J;
|
|
J.setZero(POSE_SIZE, asize);
|
|
J.block<POSE_SIZE, POSE_SIZE>(0, kf_start_idx) = d_res_d_T_w_i;
|
|
J.block<POSE_SIZE, POSE_SIZE>(0, o_start_idx) = d_res_d_T_w_j;
|
|
|
|
Sophus::Matrix6d cov_new = J * cov_old * J.transpose();
|
|
RelPoseFactor rpf;
|
|
rpf.t_i_ns = kf_id;
|
|
rpf.t_j_ns = other_id;
|
|
rpf.T_i_j = T_kf_o;
|
|
rpf.cov_inv.setIdentity();
|
|
|
|
if (!config.mapper_no_factor_weights) {
|
|
cov_new.ldlt().solveInPlace(rpf.cov_inv);
|
|
}
|
|
|
|
// std::cout << "rpf.cov_inv\n" << rpf.cov_inv << std::endl;
|
|
|
|
rel_pose_factors.emplace_back(rpf);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void NfrMapper::optimize(int num_iterations) {
|
|
AbsOrderMap aom;
|
|
|
|
for (const auto& kv : frame_poses) {
|
|
aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE);
|
|
aom.total_size += POSE_SIZE;
|
|
}
|
|
|
|
for (int iter = 0; iter < num_iterations; iter++) {
|
|
auto t1 = std::chrono::high_resolution_clock::now();
|
|
|
|
double rld_error;
|
|
Eigen::vector<RelLinData> rld_vec;
|
|
linearizeHelper(rld_vec, lmdb.getObservations(), rld_error);
|
|
|
|
// SparseHashAccumulator<double> accum;
|
|
// accum.reset(aom.total_size);
|
|
|
|
// for (auto& rld : rld_vec) {
|
|
// rld.invert_keypoint_hessians();
|
|
|
|
// Eigen::MatrixXd rel_H;
|
|
// Eigen::VectorXd rel_b;
|
|
// linearizeRel(rld, rel_H, rel_b);
|
|
|
|
// linearizeAbs(rel_H, rel_b, rld, aom, accum);
|
|
// }
|
|
|
|
MapperLinearizeAbsReduce<SparseHashAccumulator<double>> lopt(aom,
|
|
&frame_poses);
|
|
tbb::blocked_range<Eigen::vector<RelLinData>::iterator> range(
|
|
rld_vec.begin(), rld_vec.end());
|
|
tbb::blocked_range<Eigen::vector<RollPitchFactor>::const_iterator> range1(
|
|
roll_pitch_factors.begin(), roll_pitch_factors.end());
|
|
tbb::blocked_range<Eigen::vector<RelPoseFactor>::const_iterator> range2(
|
|
rel_pose_factors.begin(), rel_pose_factors.end());
|
|
|
|
tbb::parallel_reduce(range, lopt);
|
|
tbb::parallel_reduce(range1, lopt);
|
|
tbb::parallel_reduce(range2, lopt);
|
|
|
|
double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error;
|
|
|
|
std::cout << "[LINEARIZE] iter " << iter
|
|
<< " before_update_error: vision: " << rld_error
|
|
<< " rel_error: " << lopt.rel_error
|
|
<< " roll_pitch_error: " << lopt.roll_pitch_error
|
|
<< " total: " << error_total << std::endl;
|
|
|
|
lopt.accum.iterative_solver = true;
|
|
lopt.accum.print_info = true;
|
|
|
|
lopt.accum.setup_solver();
|
|
const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
|
|
|
|
bool converged = false;
|
|
bool step = false;
|
|
int max_iter = 10;
|
|
|
|
while (!step && max_iter > 0 && !converged) {
|
|
Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
|
|
for (int i = 0; i < Hdiag_lambda.size(); i++)
|
|
Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
|
|
|
|
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
|
|
double max_inc = inc.array().abs().maxCoeff();
|
|
if (max_inc < 1e-5) converged = true;
|
|
|
|
backup();
|
|
|
|
// apply increment to poses
|
|
for (auto& kv : frame_poses) {
|
|
int idx = aom.abs_order_map.at(kv.first).first;
|
|
BASALT_ASSERT(!kv.second.isLinearized());
|
|
kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
|
|
}
|
|
|
|
// Update points
|
|
tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
|
|
auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
|
|
for (size_t i = r.begin(); i != r.end(); ++i) {
|
|
const auto& rld = rld_vec[i];
|
|
updatePoints(aom, rld, inc);
|
|
}
|
|
};
|
|
tbb::parallel_for(keys_range, update_points_func);
|
|
|
|
double after_update_vision_error = 0;
|
|
double after_rel_error = 0;
|
|
double after_roll_pitch_error = 0;
|
|
|
|
computeError(after_update_vision_error);
|
|
computeRelPose(after_rel_error);
|
|
computeRollPitch(after_roll_pitch_error);
|
|
|
|
double after_error_total =
|
|
after_update_vision_error + after_rel_error + after_roll_pitch_error;
|
|
|
|
double f_diff = (error_total - after_error_total);
|
|
double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
|
|
|
|
std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
|
|
|
|
double step_quality = f_diff / l_diff;
|
|
|
|
if (step_quality < 0) {
|
|
std::cout << "\t[REJECTED] lambda:" << lambda
|
|
<< " step_quality: " << step_quality
|
|
<< " max_inc: " << max_inc
|
|
<< " vision_error: " << after_update_vision_error
|
|
<< " rel_error: " << after_rel_error
|
|
<< " roll_pitch_error: " << after_roll_pitch_error
|
|
<< " total: " << after_error_total << std::endl;
|
|
lambda = std::min(max_lambda, lambda_vee * lambda);
|
|
lambda_vee *= 2;
|
|
|
|
restore();
|
|
} else {
|
|
std::cout << "\t[ACCEPTED] lambda:" << lambda
|
|
<< " step_quality: " << step_quality
|
|
<< " max_inc: " << max_inc
|
|
<< " vision_error: " << after_update_vision_error
|
|
<< " rel_error: " << after_rel_error
|
|
<< " roll_pitch_error: " << after_roll_pitch_error
|
|
<< " total: " << after_error_total << std::endl;
|
|
|
|
lambda = std::max(
|
|
min_lambda,
|
|
lambda *
|
|
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
|
|
lambda_vee = 2;
|
|
|
|
step = true;
|
|
}
|
|
|
|
max_iter--;
|
|
|
|
if (after_error_total > error_total) {
|
|
std::cout << "increased error after update!!!" << std::endl;
|
|
}
|
|
}
|
|
|
|
auto t2 = std::chrono::high_resolution_clock::now();
|
|
auto elapsed =
|
|
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
|
|
|
|
std::cout << "iter " << iter << " time : " << elapsed.count()
|
|
<< "(us), num_states " << frame_states.size() << " num_poses "
|
|
<< frame_poses.size() << std::endl;
|
|
|
|
if (converged) break;
|
|
|
|
// std::cerr << "LT\n" << LT << std::endl;
|
|
// std::cerr << "z_p\n" << z_p.transpose() << std::endl;
|
|
// std::cerr << "inc\n" << inc.transpose() << std::endl;
|
|
}
|
|
}
|
|
|
|
Eigen::map<int64_t, PoseStateWithLin>& NfrMapper::getFramePoses() {
|
|
return frame_poses;
|
|
}
|
|
|
|
void NfrMapper::computeRelPose(double& rel_error) {
|
|
rel_error = 0;
|
|
|
|
for (const RelPoseFactor& rpf : rel_pose_factors) {
|
|
const Sophus::SE3d& pose_i = frame_poses.at(rpf.t_i_ns).getPose();
|
|
const Sophus::SE3d& pose_j = frame_poses.at(rpf.t_j_ns).getPose();
|
|
|
|
Sophus::Vector6d res = relPoseError(rpf.T_i_j, pose_i, pose_j);
|
|
|
|
rel_error += res.transpose() * rpf.cov_inv * res;
|
|
}
|
|
}
|
|
|
|
void NfrMapper::computeRollPitch(double& roll_pitch_error) {
|
|
roll_pitch_error = 0;
|
|
|
|
for (const RollPitchFactor& rpf : roll_pitch_factors) {
|
|
const Sophus::SE3d& pose = frame_poses.at(rpf.t_ns).getPose();
|
|
|
|
Sophus::Vector2d res = rollPitchError(pose, rpf.R_w_i_meas);
|
|
|
|
roll_pitch_error += res.transpose() * rpf.cov_inv * res;
|
|
}
|
|
}
|
|
|
|
void NfrMapper::detect_keypoints() {
|
|
std::vector<int64_t> keys;
|
|
for (const auto& kv : img_data) {
|
|
if (frame_poses.count(kv.first) > 0) {
|
|
keys.emplace_back(kv.first);
|
|
}
|
|
}
|
|
|
|
auto t1 = std::chrono::high_resolution_clock::now();
|
|
|
|
tbb::parallel_for(
|
|
tbb::blocked_range<size_t>(0, keys.size()),
|
|
[&](const tbb::blocked_range<size_t>& r) {
|
|
for (size_t j = r.begin(); j != r.end(); ++j) {
|
|
auto kv = img_data.find(keys[j]);
|
|
if (kv->second.get()) {
|
|
for (size_t i = 0; i < kv->second->img_data.size(); i++) {
|
|
TimeCamId tcid(kv->first, i);
|
|
KeypointsData& kd = feature_corners[tcid];
|
|
|
|
if (!kv->second->img_data[i].img.get()) continue;
|
|
|
|
const Image<const uint16_t> img =
|
|
kv->second->img_data[i].img->Reinterpret<const uint16_t>();
|
|
|
|
detectKeypointsMapping(img, kd,
|
|
config.mapper_detection_num_points);
|
|
computeAngles(img, kd, true);
|
|
computeDescriptors(img, kd);
|
|
|
|
std::vector<bool> success;
|
|
calib.intrinsics[tcid.cam_id].unproject(kd.corners, kd.corners_3d,
|
|
success);
|
|
|
|
hash_bow_database->compute_bow(kd.corner_descriptors, kd.hashes,
|
|
kd.bow_vector);
|
|
|
|
hash_bow_database->add_to_database(tcid, kd.bow_vector);
|
|
|
|
// std::cout << "bow " << kd.bow_vector.size() << " desc "
|
|
// << kd.corner_descriptors.size() << std::endl;
|
|
}
|
|
}
|
|
}
|
|
});
|
|
|
|
auto t2 = std::chrono::high_resolution_clock::now();
|
|
|
|
auto elapsed1 =
|
|
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
|
|
|
|
std::cout << "Processed " << feature_corners.size() << " frames."
|
|
<< std::endl;
|
|
|
|
std::cout << "Detection time: " << elapsed1.count() * 1e-6 << "s."
|
|
<< std::endl;
|
|
}
|
|
|
|
void NfrMapper::match_stereo() {
|
|
// Pose of camera 1 (right) w.r.t camera 0 (left)
|
|
const Sophus::SE3d T_0_1 = calib.T_i_c[0].inverse() * calib.T_i_c[1];
|
|
|
|
// Essential matrix
|
|
Eigen::Matrix4d E;
|
|
computeEssential(T_0_1, E);
|
|
|
|
std::cout << "Matching " << img_data.size() << " stereo pairs..."
|
|
<< std::endl;
|
|
|
|
int num_matches = 0;
|
|
int num_inliers = 0;
|
|
|
|
for (const auto& kv : img_data) {
|
|
const TimeCamId tcid1(kv.first, 0), tcid2(kv.first, 1);
|
|
|
|
MatchData md;
|
|
md.T_i_j = T_0_1;
|
|
|
|
const KeypointsData& kd1 = feature_corners[tcid1];
|
|
const KeypointsData& kd2 = feature_corners[tcid2];
|
|
|
|
matchDescriptors(kd1.corner_descriptors, kd2.corner_descriptors, md.matches,
|
|
config.mapper_max_hamming_distance,
|
|
config.mapper_second_best_test_ratio);
|
|
|
|
num_matches += md.matches.size();
|
|
|
|
findInliersEssential(kd1, kd2, E, 1e-3, md);
|
|
|
|
if (md.inliers.size() > 16) {
|
|
num_inliers += md.inliers.size();
|
|
feature_matches[std::make_pair(tcid1, tcid2)] = md;
|
|
}
|
|
}
|
|
|
|
std::cout << "Matched " << img_data.size() << " stereo pairs with "
|
|
<< num_inliers << " inlier matches (" << num_matches << " total)."
|
|
<< std::endl;
|
|
}
|
|
|
|
void NfrMapper::match_all() {
|
|
std::vector<TimeCamId> keys;
|
|
std::unordered_map<TimeCamId, size_t> id_to_key_idx;
|
|
|
|
for (const auto& kv : feature_corners) {
|
|
id_to_key_idx[kv.first] = keys.size();
|
|
keys.push_back(kv.first);
|
|
}
|
|
|
|
auto t1 = std::chrono::high_resolution_clock::now();
|
|
|
|
struct match_pair {
|
|
size_t i;
|
|
size_t j;
|
|
double score;
|
|
};
|
|
|
|
tbb::concurrent_vector<match_pair> ids_to_match;
|
|
|
|
tbb::blocked_range<size_t> keys_range(0, keys.size());
|
|
auto compute_pairs = [&](const tbb::blocked_range<size_t>& r) {
|
|
for (size_t i = r.begin(); i != r.end(); ++i) {
|
|
const TimeCamId& tcid = keys[i];
|
|
const KeypointsData& kd = feature_corners.at(tcid);
|
|
|
|
std::vector<std::pair<TimeCamId, double>> results;
|
|
|
|
hash_bow_database->querry_database(kd.bow_vector,
|
|
config.mapper_num_frames_to_match,
|
|
results, &tcid.frame_id);
|
|
|
|
// std::cout << "Closest frames for " << tcid << ": ";
|
|
for (const auto& otcid_score : results) {
|
|
// std::cout << otcid_score.first << "(" << otcid_score.second << ")
|
|
// ";
|
|
if (otcid_score.first.frame_id != tcid.frame_id &&
|
|
otcid_score.second > config.mapper_frames_to_match_threshold) {
|
|
match_pair m;
|
|
m.i = i;
|
|
m.j = id_to_key_idx.at(otcid_score.first);
|
|
m.score = otcid_score.second;
|
|
|
|
ids_to_match.emplace_back(m);
|
|
}
|
|
}
|
|
// std::cout << std::endl;
|
|
}
|
|
};
|
|
|
|
tbb::parallel_for(keys_range, compute_pairs);
|
|
// compute_pairs(keys_range);
|
|
|
|
auto t2 = std::chrono::high_resolution_clock::now();
|
|
|
|
std::cout << "Matching " << ids_to_match.size() << " image pairs..."
|
|
<< std::endl;
|
|
|
|
std::atomic<int> total_matched = 0;
|
|
|
|
tbb::blocked_range<size_t> range(0, ids_to_match.size());
|
|
auto match_func = [&](const tbb::blocked_range<size_t>& r) {
|
|
int matched = 0;
|
|
|
|
for (size_t j = r.begin(); j != r.end(); ++j) {
|
|
const TimeCamId& id1 = keys[ids_to_match[j].i];
|
|
const TimeCamId& id2 = keys[ids_to_match[j].j];
|
|
|
|
const KeypointsData& f1 = feature_corners[id1];
|
|
const KeypointsData& f2 = feature_corners[id2];
|
|
|
|
MatchData md;
|
|
|
|
matchDescriptors(f1.corner_descriptors, f2.corner_descriptors, md.matches,
|
|
70, 1.2);
|
|
|
|
if (int(md.matches.size()) > config.mapper_min_matches) {
|
|
matched++;
|
|
|
|
findInliersRansac(f1, f2, config.mapper_ransac_threshold,
|
|
config.mapper_min_matches, md);
|
|
}
|
|
|
|
if (!md.inliers.empty()) feature_matches[std::make_pair(id1, id2)] = md;
|
|
}
|
|
total_matched += matched;
|
|
};
|
|
|
|
tbb::parallel_for(range, match_func);
|
|
// match_func(range);
|
|
|
|
auto t3 = std::chrono::high_resolution_clock::now();
|
|
|
|
auto elapsed1 =
|
|
std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
|
|
auto elapsed2 =
|
|
std::chrono::duration_cast<std::chrono::microseconds>(t3 - t2);
|
|
|
|
//
|
|
int num_matches = 0;
|
|
int num_inliers = 0;
|
|
|
|
for (const auto& kv : feature_matches) {
|
|
num_matches += kv.second.matches.size();
|
|
num_inliers += kv.second.inliers.size();
|
|
}
|
|
|
|
std::cout << "Matched " << ids_to_match.size() << " image pairs with "
|
|
<< num_inliers << " inlier matches (" << num_matches << " total)."
|
|
<< std::endl;
|
|
|
|
std::cout << "DB query " << elapsed1.count() * 1e-6 << "s. matching "
|
|
<< elapsed2.count() * 1e-6
|
|
<< "s. Geometric verification attemts: " << total_matched << "."
|
|
<< std::endl;
|
|
}
|
|
|
|
void NfrMapper::build_tracks() {
|
|
TrackBuilder trackBuilder;
|
|
// Build: Efficient fusion of correspondences
|
|
trackBuilder.Build(feature_matches);
|
|
// Filter: Remove tracks that have conflict
|
|
trackBuilder.Filter(config.mapper_min_track_length);
|
|
// Export tree to usable data structure
|
|
trackBuilder.Export(feature_tracks);
|
|
|
|
// info
|
|
size_t inlier_match_count = 0;
|
|
for (const auto& it : feature_matches) {
|
|
inlier_match_count += it.second.inliers.size();
|
|
}
|
|
|
|
size_t total_track_obs_count = 0;
|
|
for (const auto& it : feature_tracks) {
|
|
total_track_obs_count += it.second.size();
|
|
}
|
|
|
|
std::cout << "Built " << feature_tracks.size() << " feature tracks from "
|
|
<< inlier_match_count << " matches. Average track length is "
|
|
<< total_track_obs_count / (double)feature_tracks.size() << "."
|
|
<< std::endl;
|
|
}
|
|
|
|
void NfrMapper::setup_opt() {
|
|
const double min_triang_distance2 = config.mapper_min_triangulation_dist *
|
|
config.mapper_min_triangulation_dist;
|
|
|
|
for (const auto& kv : feature_tracks) {
|
|
if (kv.second.size() < 2) continue;
|
|
|
|
// Take first observation as host
|
|
auto it = kv.second.begin();
|
|
TimeCamId tcid_h = it->first;
|
|
|
|
FeatureId feat_id_h = it->second;
|
|
Eigen::Vector2d pos_2d_h = feature_corners.at(tcid_h).corners[feat_id_h];
|
|
Eigen::Vector4d pos_3d_h;
|
|
calib.intrinsics[tcid_h.cam_id].unproject(pos_2d_h, pos_3d_h);
|
|
|
|
it++;
|
|
|
|
for (; it != kv.second.end(); it++) {
|
|
TimeCamId tcid_o = it->first;
|
|
|
|
FeatureId feat_id_o = it->second;
|
|
Eigen::Vector2d pos_2d_o = feature_corners.at(tcid_o).corners[feat_id_o];
|
|
Eigen::Vector4d pos_3d_o;
|
|
calib.intrinsics[tcid_o.cam_id].unproject(pos_2d_o, pos_3d_o);
|
|
|
|
Sophus::SE3d T_w_h = frame_poses.at(tcid_h.frame_id).getPose() *
|
|
calib.T_i_c[tcid_h.cam_id];
|
|
Sophus::SE3d T_w_o = frame_poses.at(tcid_o.frame_id).getPose() *
|
|
calib.T_i_c[tcid_o.cam_id];
|
|
|
|
Sophus::SE3d T_h_o = T_w_h.inverse() * T_w_o;
|
|
|
|
if (T_h_o.translation().squaredNorm() < min_triang_distance2) continue;
|
|
|
|
Eigen::Vector4d pos_3d =
|
|
triangulate(pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_h_o);
|
|
|
|
if (!pos_3d.array().isFinite().all() || pos_3d[3] <= 0 || pos_3d[3] > 2.0)
|
|
continue;
|
|
|
|
KeypointPosition pos;
|
|
pos.kf_id = tcid_h;
|
|
pos.dir = StereographicParam<double>::project(pos_3d);
|
|
pos.id = pos_3d[3];
|
|
|
|
lmdb.addLandmark(kv.first, pos);
|
|
|
|
for (const auto& obs_kv : kv.second) {
|
|
KeypointObservation ko;
|
|
ko.kpt_id = kv.first;
|
|
ko.pos = feature_corners.at(obs_kv.first).corners[obs_kv.second];
|
|
|
|
lmdb.addObservation(obs_kv.first, ko);
|
|
// obs[tcid_h][obs_kv.first].emplace_back(ko);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace basalt
|