ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

converted TimeCamId to struct

This commit is contained in:
Vladyslav Usenko 2019-07-12 13:26:10 +02:00
parent dd087bdbb8
commit 9b705e1c3c
18 changed files with 172 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/basalt/calibration/calibration_helper.h
View File

@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <basalt/io/dataset_io.h>
#include <basalt/utils/common_types.h>
#include <basalt/calibration/calibration.hpp>
#include <tbb/concurrent_unordered_map.h>

2
include/basalt/hash_bow/hash_bow.h
View File

@ -80,7 +80,7 @@ class HashBow {
for (auto it = range_it.first; it != range_it.second; ++it) {
// if there is a maximum query time select only the frames that have
// timestamp below max_t_ns
if (!max_t_ns || it->second.first.first < (*max_t_ns))
if (!max_t_ns || it->second.first.frame_id < (*max_t_ns))
scores[it->second.first] += kv.second * it->second.second;
}
}

38
include/basalt/io/dataset_io.h
View File

@ -32,8 +32,8 @@ 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.
*/
#ifndef DATASET_IO_H
#define DATASET_IO_H
#pragma once
#include <array>
#include <fstream>
@ -71,8 +71,6 @@ inline bool file_exists(const std::string &name) {
return f.good();
}
typedef std::pair<int64_t, size_t> TimeCamId;
struct ImageData {
ImageData() : exposure(0) {}
@ -212,35 +210,3 @@ void serialize(Archive &ar, basalt::AccelData &c) {
}
} // namespace cereal
namespace std {
inline void hash_combine(std::size_t &seed, std::size_t value) {
seed ^= value + 0x9e3779b9 + (seed << 6) + (seed >> 2);
}
template <>
struct hash<basalt::TimeCamId> {
size_t operator()(const basalt::TimeCamId &x) const {
size_t seed = 0;
hash_combine(seed, std::hash<int>()(x.first));
hash_combine(seed, std::hash<int>()(x.second));
return seed;
}
};
template <>
struct hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>> {
size_t operator()(
const std::pair<basalt::TimeCamId, basalt::TimeCamId> &x) const {
size_t seed = 0;
hash_combine(seed, std::hash<int>()(x.first.first));
hash_combine(seed, std::hash<int>()(x.first.second));
hash_combine(seed, std::hash<int>()(x.second.first));
hash_combine(seed, std::hash<int>()(x.second.second));
return seed;
}
};
} // namespace std
#endif // DATASET_IO_H

72
include/basalt/utils/common_types.h
View File

@ -58,12 +58,34 @@ using FrameId = int64_t;
using CamId = std::size_t;
/// pair of image timestamp and camera id identifies an image (imageId)
typedef std::pair<FrameId, CamId> TimeCamId;
struct TimeCamId {
TimeCamId() : frame_id(0), cam_id(0) {}
TimeCamId(const FrameId& frame_id, const CamId& cam_id)
: frame_id(frame_id), cam_id(cam_id) {}
FrameId frame_id;
CamId cam_id;
};
inline std::ostream& operator<<(std::ostream& os, const TimeCamId& tcid) {
os << tcid.first << "_" << tcid.second;
os << tcid.frame_id << "_" << tcid.cam_id;
return os;
}
inline bool operator<(const TimeCamId& o1, const TimeCamId& o2) {
if (o1.frame_id == o2.frame_id) return o1.cam_id < o2.cam_id;
return o1.frame_id < o2.frame_id;
}
inline bool operator==(const TimeCamId& o1, const TimeCamId& o2) {
return o1.frame_id == o2.frame_id && o1.cam_id == o2.cam_id;
}
inline bool operator!=(const TimeCamId& o1, const TimeCamId& o2) {
return o1.frame_id != o2.frame_id || o1.cam_id != o2.cam_id;
}
constexpr static const size_t FEATURE_HASH_MAX_SIZE = 32;
using FeatureHash = std::bitset<FEATURE_HASH_MAX_SIZE>;
using HashBowVector = std::vector<std::pair<FeatureHash, double>>;
@ -249,6 +271,11 @@ using TrackProjections =
namespace cereal {
template <class Archive>
void serialize(Archive& ar, basalt::TimeCamId& c) {
ar(c.frame_id, c.cam_id);
}
template <class Archive>
void serialize(Archive& ar, basalt::KeypointsData& c) {
ar(c.corners, c.corner_angles, c.corner_descriptors);
@ -259,3 +286,44 @@ void serialize(Archive& ar, basalt::MatchData& c) {
ar(c.T_i_j, c.matches, c.inliers);
}
} // namespace cereal
namespace std {
inline void hash_combine(std::size_t& seed, std::size_t value) {
seed ^= value + 0x9e3779b9 + (seed << 6) + (seed >> 2);
}
template <>
struct hash<basalt::TimeCamId> {
size_t operator()(const basalt::TimeCamId& x) const {
size_t seed = 0;
hash_combine(seed, std::hash<int>()(x.frame_id));
hash_combine(seed, std::hash<int>()(x.cam_id));
return seed;
}
};
template <>
struct hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>> {
size_t operator()(
const std::pair<basalt::TimeCamId, basalt::TimeCamId>& x) const {
size_t seed = 0;
hash_combine(seed, std::hash<int>()(x.first.frame_id));
hash_combine(seed, std::hash<int>()(x.first.cam_id));
hash_combine(seed, std::hash<int>()(x.second.frame_id));
hash_combine(seed, std::hash<int>()(x.second.cam_id));
return seed;
}
};
} // namespace std
namespace tbb {
template <>
struct tbb_hash<basalt::TimeCamId> : public std::hash<basalt::TimeCamId> {};
template <>
struct tbb_hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>>
: public std::hash<std::pair<basalt::TimeCamId, basalt::TimeCamId>> {};
} // namespace tbb

1
include/basalt/utils/imu_types.h
View File

@ -45,6 +45,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <basalt/imu/imu_types.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/utils/common_types.h>
#include <basalt/utils/sophus_utils.hpp>
#include <cereal/cereal.hpp>

9
include/basalt/vi_estimator/ba_base.h
View File

@ -265,8 +265,8 @@ class BundleAdjustmentBase {
const TimeCamId& tcid_h = rld.order[i].first;
const TimeCamId& tcid_ti = rld.order[i].second;
int abs_h_idx = aom.abs_order_map.at(tcid_h.first).first;
int abs_ti_idx = aom.abs_order_map.at(tcid_ti.first).first;
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
int abs_ti_idx = aom.abs_order_map.at(tcid_ti.frame_id).first;
accum.template addB<POSE_SIZE>(
abs_h_idx, rld.d_rel_d_h[i].transpose() *
@ -280,9 +280,10 @@ class BundleAdjustmentBase {
const TimeCamId& tcid_tj = rld.order[j].second;
int abs_tj_idx = aom.abs_order_map.at(tcid_tj.first).first;
int abs_tj_idx = aom.abs_order_map.at(tcid_tj.frame_id).first;
if (tcid_h.first == tcid_ti.first || tcid_h.first == tcid_tj.first)
if (tcid_h.frame_id == tcid_ti.frame_id ||
tcid_h.frame_id == tcid_tj.frame_id)
continue;
accum.template addH<POSE_SIZE, POSE_SIZE>(

1
include/basalt/vi_estimator/nfr_mapper.h
View File

@ -57,7 +57,6 @@ class HashBow;
class NfrMapper : public BundleAdjustmentBase {
public:
using Ptr = std::shared_ptr<NfrMapper>;
using TimeCamId = std::pair<int64_t, std::size_t>;
using Matches = tbb::concurrent_unordered_map<
std::pair<TimeCamId, TimeCamId>, MatchData,
tbb::tbb_hash<std::pair<TimeCamId, TimeCamId>>,

4
src/calibration/calibraiton_helper.cpp
View File

@ -128,7 +128,7 @@ void CalibHelper::detectCorners(
// ccd_bad.corners.size()
// << " rejected)" << std::endl;
TimeCamId tcid = std::make_pair(timestamp_ns, i);
TimeCamId tcid(timestamp_ns, i);
calib_corners.emplace(tcid, ccd_good);
calib_corners_rejected.emplace(tcid, ccd_bad);
@ -160,7 +160,7 @@ void CalibHelper::initCamPoses(
CalibInitPoseData cp;
computeInitialPose(calib, tcid.second,
computeInitialPose(calib, tcid.cam_id,
aprilgrid_corner_pos_3d, ccd, cp);
calib_init_poses.emplace(tcid, cp);

26
src/calibration/cam_calib.cpp
View File

@ -305,7 +305,7 @@ void CamCalib::computeProjections() {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j];
for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) {
TimeCamId tcid = std::make_pair(timestamp_ns, i);
TimeCamId tcid(timestamp_ns, i);
ProjectedCornerData rc, rv;
Eigen::vector<Eigen::Vector2d> polar_azimuthal_angle;
@ -339,15 +339,15 @@ void CamCalib::computeProjections() {
size_t polar_bin =
180 * polar_azimuthal_angle[id][0] / (M_PI * ANGLE_BIN_SIZE);
polar_sum[tcid.second][polar_bin] += error;
polar_num[tcid.second][polar_bin] += 1;
polar_sum[tcid.cam_id][polar_bin] += error;
polar_num[tcid.cam_id][polar_bin] += 1;
size_t azimuth_bin =
180 / ANGLE_BIN_SIZE + (180.0 * polar_azimuthal_angle[id][1]) /
(M_PI * ANGLE_BIN_SIZE);
azimuth_sum[tcid.second][azimuth_bin] += error;
azimuth_num[tcid.second][azimuth_bin] += 1;
azimuth_sum[tcid.cam_id][azimuth_bin] += error;
azimuth_num[tcid.cam_id][azimuth_bin] += 1;
}
}
}
@ -450,7 +450,7 @@ void CamCalib::initCamIntrinsics() {
const std::vector<basalt::ImageData> &img_vec =
vio_dataset->get_image_data(timestamp_ns);
TimeCamId tcid = std::make_pair(timestamp_ns, j);
TimeCamId tcid(timestamp_ns, j);
if (calib_corners.find(tcid) != calib_corners.end()) {
CalibCornerData cid = calib_corners.at(tcid);
@ -556,7 +556,7 @@ void CamCalib::initCamExtrinsics() {
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
TimeCamId tcid0 = std::make_pair(timestamp_ns, 0);
TimeCamId tcid0(timestamp_ns, 0);
if (calib_init_poses.find(tcid0) == calib_init_poses.end()) continue;
@ -565,7 +565,7 @@ void CamCalib::initCamExtrinsics() {
bool success = true;
for (size_t j = 1; j < vio_dataset->get_num_cams(); j++) {
TimeCamId tcid = std::make_pair(timestamp_ns, j);
TimeCamId tcid(timestamp_ns, j);
auto cd = calib_init_poses.find(tcid);
if (cd != calib_init_poses.end() && cd->second.num_inliers > 0) {
@ -604,12 +604,12 @@ void CamCalib::initOptimization() {
std::set<uint64_t> invalid_timestamps;
for (const auto &kv : calib_corners) {
if (kv.second.corner_ids.size() < MIN_CORNERS)
invalid_timestamps.insert(kv.first.first);
invalid_timestamps.insert(kv.first.frame_id);
}
for (const auto &kv : calib_corners) {
if (invalid_timestamps.find(kv.first.first) == invalid_timestamps.end())
calib_opt->addAprilgridMeasurement(kv.first.first, kv.first.second,
if (invalid_timestamps.find(kv.first.frame_id) == invalid_timestamps.end())
calib_opt->addAprilgridMeasurement(kv.first.frame_id, kv.first.cam_id,
kv.second.corners,
kv.second.corner_ids);
}
@ -618,7 +618,7 @@ void CamCalib::initOptimization() {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j];
for (size_t cam_id = 0; cam_id < calib_opt->calib->T_i_c.size(); cam_id++) {
TimeCamId tcid = std::make_pair(timestamp_ns, cam_id);
TimeCamId tcid(timestamp_ns, cam_id);
const auto cp_it = calib_init_poses.find(tcid);
if (cp_it != calib_init_poses.end()) {
@ -806,7 +806,7 @@ void CamCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) {
if (vio_dataset && frame_id < vio_dataset->get_image_timestamps().size()) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[frame_id];
TimeCamId tcid = std::make_pair(timestamp_ns, cam_id);
TimeCamId tcid(timestamp_ns, cam_id);
if (show_corners) {
glLineWidth(1.0);

18
src/calibration/cam_imu_calib.cpp
View File

@ -213,7 +213,7 @@ void CamImuCalib::computeProjections() {
continue;
for (size_t i = 0; i < calib_opt->calib->intrinsics.size(); i++) {
TimeCamId tcid = std::make_pair(timestamp_ns, i);
TimeCamId tcid(timestamp_ns, i);
ProjectedCornerData rc;
@ -323,8 +323,8 @@ void CamImuCalib::initCamImuTransform() {
int64_t timestamp0_ns = vio_dataset->get_image_timestamps()[i - 1];
int64_t timestamp1_ns = vio_dataset->get_image_timestamps()[i];
TimeCamId tcid0 = std::make_pair(timestamp0_ns, 0);
TimeCamId tcid1 = std::make_pair(timestamp1_ns, 0);
TimeCamId tcid0(timestamp0_ns, 0);
TimeCamId tcid1(timestamp1_ns, 0);
if (calib_init_poses.find(tcid0) == calib_init_poses.end()) continue;
if (calib_init_poses.find(tcid1) == calib_init_poses.end()) continue;
@ -421,12 +421,12 @@ void CamImuCalib::initOptimization() {
std::set<uint64_t> invalid_timestamps;
for (const auto &kv : calib_corners) {
if (kv.second.corner_ids.size() < MIN_CORNERS)
invalid_timestamps.insert(kv.first.first);
invalid_timestamps.insert(kv.first.frame_id);
}
for (const auto &kv : calib_corners) {
if (invalid_timestamps.find(kv.first.first) == invalid_timestamps.end())
calib_opt->addAprilgridMeasurement(kv.first.first, kv.first.second,
if (invalid_timestamps.find(kv.first.frame_id) == invalid_timestamps.end())
calib_opt->addAprilgridMeasurement(kv.first.frame_id, kv.first.cam_id,
kv.second.corners,
kv.second.corner_ids);
}
@ -442,7 +442,7 @@ void CamImuCalib::initOptimization() {
for (size_t j = 0; j < vio_dataset->get_image_timestamps().size(); ++j) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[j];
TimeCamId tcid = std::make_pair(timestamp_ns, 0);
TimeCamId tcid(timestamp_ns, 0);
const auto cp_it = calib_init_poses.find(tcid);
if (cp_it != calib_init_poses.end()) {
@ -809,7 +809,7 @@ void CamImuCalib::drawImageOverlay(pangolin::View &v, size_t cam_id) {
if (vio_dataset && frame_id < vio_dataset->get_image_timestamps().size()) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[frame_id];
TimeCamId tcid = std::make_pair(timestamp_ns, cam_id);
TimeCamId tcid(timestamp_ns, cam_id);
if (show_corners) {
glLineWidth(1.0);
@ -955,7 +955,7 @@ void CamImuCalib::recomputeDataLog() {
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
TimeCamId tcid = std::make_pair(timestamp_ns, 0);
TimeCamId tcid(timestamp_ns, 0);
const auto &it = calib_init_poses.find(tcid);
double t = timestamp_ns * 1e-9 - min_time;

20
src/calibration/vignette.cpp
View File

@ -95,7 +95,7 @@ void VignetteEstimator::compute_error(
const TimeCamId &tcid = kv.first;
const auto &points_2d_val = kv.second;
Eigen::Vector2d oc = optical_centers[tcid.second];
Eigen::Vector2d oc = optical_centers[tcid.cam_id];
BASALT_ASSERT(points_2d_val.size() ==
april_grid.aprilgrid_vignette_pos_3d.size());
@ -108,7 +108,7 @@ void VignetteEstimator::compute_error(
int64_t loc =
(points_2d_val[i].head<2>() - oc).norm() * 1e9; // in pixels * 1e9
double e =
irradiance[i] * vign_param[tcid.second].evaluate(loc)[0] - val;
irradiance[i] * vign_param[tcid.cam_id].evaluate(loc)[0] - val;
ve[i] = e;
error += e * e;
mean_residual += std::abs(e);
@ -151,7 +151,7 @@ void VignetteEstimator::opt_irradience() {
const TimeCamId &tcid = kv.first;
const auto &points_2d_val = kv.second;
Eigen::Vector2d oc = optical_centers[tcid.second];
Eigen::Vector2d oc = optical_centers[tcid.cam_id];
BASALT_ASSERT(points_2d_val.size() ==
april_grid.aprilgrid_vignette_pos_3d.size());
@ -162,7 +162,7 @@ void VignetteEstimator::opt_irradience() {
int64_t loc =
(points_2d_val[i].head<2>() - oc).norm() * 1e9; // in pixels * 1e9
new_irradiance[i] += val / vign_param[tcid.second].evaluate(loc)[0];
new_irradiance[i] += val / vign_param[tcid.cam_id].evaluate(loc)[0];
new_irradiance_count[i] += 1;
}
}
@ -193,7 +193,7 @@ void VignetteEstimator::opt_vign() {
// Eigen::Vector3d::UnitZ();
// if (-opt_axis_w[2] < angle_threshold) continue;
Eigen::Vector2d oc = optical_centers[tcid.second];
Eigen::Vector2d oc = optical_centers[tcid.cam_id];
BASALT_ASSERT(points_2d_val.size() ==
april_grid.aprilgrid_vignette_pos_3d.size());
@ -204,12 +204,12 @@ void VignetteEstimator::opt_vign() {
int64_t loc = (points_2d_val[i].head<2>() - oc).norm() * 1e9;
RdSpline<1, SPLINE_N>::JacobianStruct J;
vign_param[tcid.second].evaluate(loc, &J);
vign_param[tcid.cam_id].evaluate(loc, &J);
for (size_t k = 0; k < J.d_val_d_knot.size(); k++) {
new_vign_param[tcid.second][J.start_idx + k] +=
new_vign_param[tcid.cam_id][J.start_idx + k] +=
J.d_val_d_knot[k] * val / irradiance[i];
new_vign_param_count[tcid.second][J.start_idx + k] +=
new_vign_param_count[tcid.cam_id][J.start_idx + k] +=
J.d_val_d_knot[k];
}
}
@ -255,7 +255,7 @@ void VignetteEstimator::compute_data_log(pangolin::DataLog &vign_data_log) {
const TimeCamId &tcid = kv.first;
const auto &points_2d = kv.second;
Eigen::Vector2d oc = optical_centers[tcid.second];
Eigen::Vector2d oc = optical_centers[tcid.cam_id];
BASALT_ASSERT(points_2d.size() ==
april_grid.aprilgrid_vignette_pos_3d.size());
@ -263,7 +263,7 @@ void VignetteEstimator::compute_data_log(pangolin::DataLog &vign_data_log) {
for (size_t i = 0; i < points_2d.size(); i++) {
if (points_2d[i][2] >= 0) {
size_t loc = (points_2d[i].head<2>() - oc).norm();
num_proj_points[tcid.second][loc] += 1.;
num_proj_points[tcid.cam_id][loc] += 1.;
}
}
}

8
src/mapper.cpp
View File

@ -342,7 +342,7 @@ void draw_image_overlay(pangolin::View& v, size_t view_id) {
size_t frame_id = (view_id == 0) ? show_frame1 : show_frame2;
size_t cam_id = (view_id == 0) ? show_cam1 : show_cam2;
basalt::TimeCamId tcid = std::make_pair(image_t_ns[frame_id], cam_id);
basalt::TimeCamId tcid(image_t_ns[frame_id], cam_id);
float text_row = 20;
@ -389,7 +389,7 @@ void draw_image_overlay(pangolin::View& v, size_t view_id) {
size_t o_frame_id = (view_id == 0 ? show_frame2 : show_frame1);
size_t o_cam_id = (view_id == 0 ? show_cam2 : show_cam1);
basalt::TimeCamId o_tcid = std::make_pair(image_t_ns[o_frame_id], o_cam_id);
basalt::TimeCamId o_tcid(image_t_ns[o_frame_id], o_cam_id);
int idx = -1;
@ -563,11 +563,11 @@ void computeEdgeVis() {
for (const auto& kv1 : nrf_mapper->obs) {
for (const auto& kv2 : kv1.second) {
Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
.at(kv1.first.first)
.at(kv1.first.frame_id)
.getPose()
.translation();
Eigen::Vector3d p2 = nrf_mapper->getFramePoses()
.at(kv2.first.first)
.at(kv2.first.frame_id)
.getPose()
.translation();

4
src/mapper_sim.cpp
View File

@ -482,11 +482,11 @@ void computeEdgeVis() {
for (const auto& kv1 : nrf_mapper->obs) {
for (const auto& kv2 : kv1.second) {
Eigen::Vector3d p1 = nrf_mapper->getFramePoses()
.at(kv1.first.first)
.at(kv1.first.frame_id)
.getPose()
.translation();
Eigen::Vector3d p2 = nrf_mapper->getFramePoses()
.at(kv2.first.first)
.at(kv2.first.frame_id)
.getPose()
.translation();

2
src/mapper_sim_naive.cpp
View File

@ -397,7 +397,7 @@ int main(int argc, char** argv) {
void draw_image_overlay(pangolin::View& v, size_t cam_id) {
size_t frame_id = show_frame;
basalt::TimeCamId tcid = std::make_pair(kf_t_ns[frame_id], cam_id);
basalt::TimeCamId tcid(kf_t_ns[frame_id], cam_id);
if (show_obs) {
glLineWidth(1.0);

38
src/vi_estimator/ba_base.cpp
View File

@ -89,9 +89,9 @@ void BundleAdjustmentBase::updatePoints(const AbsOrderMap& aom,
const TimeCamId& tcid_h = rld.order[i].first;
const TimeCamId& tcid_t = rld.order[i].second;
if (tcid_h.first != tcid_t.first) {
int abs_h_idx = aom.abs_order_map.at(tcid_h.first).first;
int abs_t_idx = aom.abs_order_map.at(tcid_t.first).first;
if (tcid_h.frame_id != tcid_t.frame_id) {
int abs_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
int abs_t_idx = aom.abs_order_map.at(tcid_t.frame_id).first;
rel_inc.segment<POSE_SIZE>(i * POSE_SIZE) =
rld.d_rel_d_h[i] * inc.segment<POSE_SIZE>(abs_h_idx) +
@ -138,12 +138,12 @@ void BundleAdjustmentBase::computeError(double& error) const {
const TimeCamId& tcid_t = obs_kv.first;
if (tcid_h != tcid_t) {
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.first);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first);
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
Sophus::SE3d T_t_h_sophus =
computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.second],
state_t.getPose(), calib.T_i_c[tcid_t.second]);
computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
@ -169,7 +169,7 @@ void BundleAdjustmentBase::computeError(double& error) const {
}
}
},
calib.intrinsics[tcid_t.second].variant);
calib.intrinsics[tcid_t.cam_id].variant);
} else {
// target and host are the same
@ -197,7 +197,7 @@ void BundleAdjustmentBase::computeError(double& error) const {
}
}
},
calib.intrinsics[tcid_t.second].variant);
calib.intrinsics[tcid_t.cam_id].variant);
}
}
}
@ -237,14 +237,14 @@ void BundleAdjustmentBase::linearizeHelper(
// target and host are not the same
rld.order.emplace_back(std::make_pair(tcid_h, tcid_t));
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.first);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first);
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
Sophus::Matrix6d d_rel_d_h, d_rel_d_t;
Sophus::SE3d T_t_h_sophus = computeRelPose(
state_h.getPoseLin(), calib.T_i_c[tcid_h.second],
state_t.getPoseLin(), calib.T_i_c[tcid_t.second], &d_rel_d_h,
state_h.getPoseLin(), calib.T_i_c[tcid_h.cam_id],
state_t.getPoseLin(), calib.T_i_c[tcid_t.cam_id], &d_rel_d_h,
&d_rel_d_t);
rld.d_rel_d_h.emplace_back(d_rel_d_h);
@ -252,8 +252,8 @@ void BundleAdjustmentBase::linearizeHelper(
if (state_h.isLinearized() || state_t.isLinearized()) {
T_t_h_sophus = computeRelPose(
state_h.getPose(), calib.T_i_c[tcid_h.second],
state_t.getPose(), calib.T_i_c[tcid_t.second]);
state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
}
Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
@ -305,7 +305,7 @@ void BundleAdjustmentBase::linearizeHelper(
}
}
},
calib.intrinsics[tcid_t.second].variant);
calib.intrinsics[tcid_t.cam_id].variant);
rld.Hpppl.emplace_back(frld);
@ -348,7 +348,7 @@ void BundleAdjustmentBase::linearizeHelper(
}
}
},
calib.intrinsics[tcid_t.second].variant);
calib.intrinsics[tcid_t.cam_id].variant);
}
}
}
@ -408,7 +408,7 @@ void BundleAdjustmentBase::get_current_points(
Sophus::SE3d T_w_i;
int64_t id = kv_kpt.second.kf_id.first;
int64_t id = kv_kpt.second.kf_id.frame_id;
if (frame_states.count(id) > 0) {
PoseVelBiasStateWithLin state = frame_states.at(id);
T_w_i = state.getState().T_w_i;
@ -421,7 +421,7 @@ void BundleAdjustmentBase::get_current_points(
std::abort();
}
const Sophus::SE3d& T_i_c = calib.T_i_c[kv_kpt.second.kf_id.second];
const Sophus::SE3d& T_i_c = calib.T_i_c[kv_kpt.second.kf_id.cam_id];
// std::cerr << "T_w_i\n" << T_w_i.matrix() << std::endl;

46
src/vi_estimator/keypoint_vio.cpp
View File

@ -247,10 +247,10 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
obs[tcid_host][tcid_target].push_back(kobs);
if (num_points_connected.count(tcid_host.first) == 0) {
num_points_connected[tcid_host.first] = 0;
if (num_points_connected.count(tcid_host.frame_id) == 0) {
num_points_connected[tcid_host.frame_id] = 0;
}
num_points_connected[tcid_host.first]++;
num_points_connected[tcid_host.frame_id]++;
if (i == 0) connected0++;
} else {
@ -310,22 +310,22 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
.at(lm_id)
.translation()
.cast<double>();
const Eigen::Vector2d p1 = prev_opt_flow_res[tcido.first]
->observations[tcido.second]
const Eigen::Vector2d p1 = prev_opt_flow_res[tcido.frame_id]
->observations[tcido.cam_id]
.at(lm_id)
.translation()
.cast<double>();
Eigen::Vector4d p0_3d, p1_3d;
bool valid1 = calib.intrinsics[0].unproject(p0, p0_3d);
bool valid2 = calib.intrinsics[tcido.second].unproject(p1, p1_3d);
bool valid2 = calib.intrinsics[tcido.cam_id].unproject(p1, p1_3d);
if (!valid1 || !valid2) continue;
Sophus::SE3d T_i0_i1 =
getPoseStateWithLin(tcidl.first).getPose().inverse() *
getPoseStateWithLin(tcido.first).getPose();
getPoseStateWithLin(tcidl.frame_id).getPose().inverse() *
getPoseStateWithLin(tcido.frame_id).getPose();
Sophus::SE3d T_0_1 =
calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.second];
calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.cam_id];
if (T_0_1.translation().squaredNorm() < 0.03 * 0.03) continue;
@ -672,10 +672,10 @@ void KeypointVioEstimator::marginalize(
obs_to_lin;
for (auto it = obs.cbegin(); it != obs.cend();) {
if (kfs_to_marg.count(it->first.first) > 0) {
if (kfs_to_marg.count(it->first.frame_id) > 0) {
for (auto it2 = it->second.cbegin(); it2 != it->second.cend();
++it2) {
if (it2->first.first <= last_state_to_marg)
if (it2->first.frame_id <= last_state_to_marg)
obs_to_lin[it->first].emplace(*it2);
}
it = obs.erase(it);
@ -701,7 +701,7 @@ void KeypointVioEstimator::marginalize(
// remove points
for (auto it = kpts.cbegin(); it != kpts.cend();) {
if (kfs_to_marg.count(it->second.kf_id.first) > 0) {
if (kfs_to_marg.count(it->second.kf_id.frame_id) > 0) {
it = kpts.erase(it);
} else {
++it;
@ -808,8 +808,8 @@ void KeypointVioEstimator::marginalize(
for (auto it = obs.begin(); it != obs.end(); ++it) {
for (auto it2 = it->second.cbegin(); it2 != it->second.cend();) {
if (poses_to_marg.count(it2->first.first) > 0 ||
states_to_marg_all.count(it2->first.first) > 0) {
if (poses_to_marg.count(it2->first.frame_id) > 0 ||
states_to_marg_all.count(it2->first.frame_id) > 0) {
it2 = it->second.erase(it2);
} else {
++it2;
@ -1035,15 +1035,15 @@ void KeypointVioEstimator::computeProjections(
for (const auto& obs_kv : kv.second) {
const TimeCamId& tcid_t = obs_kv.first;
if (tcid_t.first != last_state_t_ns) continue;
if (tcid_t.frame_id != last_state_t_ns) continue;
if (tcid_h != tcid_t) {
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.first);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first);
PoseStateWithLin state_h = getPoseStateWithLin(tcid_h.frame_id);
PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.frame_id);
Sophus::SE3d T_t_h_sophus =
computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.second],
state_t.getPose(), calib.T_i_c[tcid_t.second]);
computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
state_t.getPose(), calib.T_i_c[tcid_t.cam_id]);
Eigen::Matrix4d T_t_h = T_t_h_sophus.matrix();
@ -1062,10 +1062,10 @@ void KeypointVioEstimator::computeProjections(
nullptr, &proj);
proj[3] = kpt_obs.kpt_id;
data[tcid_t.second].emplace_back(proj);
data[tcid_t.cam_id].emplace_back(proj);
}
},
calib.intrinsics[tcid_t.second].variant);
calib.intrinsics[tcid_t.cam_id].variant);
} else {
// target and host are the same
@ -1085,10 +1085,10 @@ void KeypointVioEstimator::computeProjections(
cam, res, nullptr, nullptr, &proj);
proj[3] = kpt_obs.kpt_id;
data[tcid_t.second].emplace_back(proj);
data[tcid_t.cam_id].emplace_back(proj);
}
},
calib.intrinsics[tcid_t.second].variant);
calib.intrinsics[tcid_t.cam_id].variant);
}
}
}

16
src/vi_estimator/nfr_mapper.cpp
View File

@ -391,7 +391,7 @@ void NfrMapper::detect_keypoints() {
computeDescriptors(img, kd);
std::vector<bool> success;
calib.intrinsics[tcid.second].unproject(kd.corners, kd.corners_3d,
calib.intrinsics[tcid.cam_id].unproject(kd.corners, kd.corners_3d,
success);
hash_bow_database->compute_bow(kd.corner_descriptors, kd.hashes,
@ -489,12 +489,12 @@ void NfrMapper::match_all() {
hash_bow_database->querry_database(kd.bow_vector,
config.mapper_num_frames_to_match,
results, &tcid.first);
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.first != tcid.first &&
if (otcid_score.first.frame_id != tcid.frame_id &&
otcid_score.second > config.mapper_frames_to_match_threshold) {
match_pair m;
m.i = i;
@ -612,7 +612,7 @@ void NfrMapper::setup_opt() {
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.second].unproject(pos_2d_h, pos_3d_h);
calib.intrinsics[tcid_h.cam_id].unproject(pos_2d_h, pos_3d_h);
it++;
@ -622,12 +622,12 @@ void NfrMapper::setup_opt() {
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.second].unproject(pos_2d_o, 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.first).getPose() * calib.T_i_c[tcid_h.second];
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.first).getPose() * calib.T_i_c[tcid_o.second];
frame_poses.at(tcid_o.cam_id).getPose() * calib.T_i_c[tcid_o.cam_id];
Eigen::Vector4d pos_3d = triangulate(
pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_w_h.inverse() * T_w_o);

2
src/vio_sim.cpp
View File

@ -424,7 +424,7 @@ int main(int argc, char** argv) {
void draw_image_overlay(pangolin::View& v, size_t cam_id) {
size_t frame_id = show_frame;
basalt::TimeCamId tcid = std::make_pair(gt_frame_t_ns[frame_id], cam_id);
basalt::TimeCamId tcid(gt_frame_t_ns[frame_id], cam_id);
if (show_obs) {
glLineWidth(1.0);