diff --git a/include/basalt/calibration/calibration_helper.h b/include/basalt/calibration/calibration_helper.h index 3ba7070..b441417 100644 --- a/include/basalt/calibration/calibration_helper.h +++ b/include/basalt/calibration/calibration_helper.h @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include +#include #include #include diff --git a/include/basalt/hash_bow/hash_bow.h b/include/basalt/hash_bow/hash_bow.h index a2936f0..93d53f1 100644 --- a/include/basalt/hash_bow/hash_bow.h +++ b/include/basalt/hash_bow/hash_bow.h @@ -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; } } diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 53b0d5a..4b6008f 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -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 #include @@ -71,8 +71,6 @@ inline bool file_exists(const std::string &name) { return f.good(); } -typedef std::pair 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 { - size_t operator()(const basalt::TimeCamId &x) const { - size_t seed = 0; - hash_combine(seed, std::hash()(x.first)); - hash_combine(seed, std::hash()(x.second)); - return seed; - } -}; - -template <> -struct hash> { - size_t operator()( - const std::pair &x) const { - size_t seed = 0; - hash_combine(seed, std::hash()(x.first.first)); - hash_combine(seed, std::hash()(x.first.second)); - hash_combine(seed, std::hash()(x.second.first)); - hash_combine(seed, std::hash()(x.second.second)); - return seed; - } -}; -} // namespace std - -#endif // DATASET_IO_H diff --git a/include/basalt/utils/common_types.h b/include/basalt/utils/common_types.h index dc983dc..72e41a8 100644 --- a/include/basalt/utils/common_types.h +++ b/include/basalt/utils/common_types.h @@ -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 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; using HashBowVector = std::vector>; @@ -249,6 +271,11 @@ using TrackProjections = namespace cereal { +template +void serialize(Archive& ar, basalt::TimeCamId& c) { + ar(c.frame_id, c.cam_id); +} + template 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 { + size_t operator()(const basalt::TimeCamId& x) const { + size_t seed = 0; + hash_combine(seed, std::hash()(x.frame_id)); + hash_combine(seed, std::hash()(x.cam_id)); + return seed; + } +}; + +template <> +struct hash> { + size_t operator()( + const std::pair& x) const { + size_t seed = 0; + hash_combine(seed, std::hash()(x.first.frame_id)); + hash_combine(seed, std::hash()(x.first.cam_id)); + hash_combine(seed, std::hash()(x.second.frame_id)); + hash_combine(seed, std::hash()(x.second.cam_id)); + return seed; + } +}; +} // namespace std + +namespace tbb { + +template <> +struct tbb_hash : public std::hash {}; + +template <> +struct tbb_hash> + : public std::hash> {}; + +} // namespace tbb diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index c96b8f0..9a2fcf7 100644 --- a/include/basalt/utils/imu_types.h +++ b/include/basalt/utils/imu_types.h @@ -45,6 +45,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h index 7cc0a72..c4326ef 100644 --- a/include/basalt/vi_estimator/ba_base.h +++ b/include/basalt/vi_estimator/ba_base.h @@ -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( 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( diff --git a/include/basalt/vi_estimator/nfr_mapper.h b/include/basalt/vi_estimator/nfr_mapper.h index f15be69..62758b6 100644 --- a/include/basalt/vi_estimator/nfr_mapper.h +++ b/include/basalt/vi_estimator/nfr_mapper.h @@ -57,7 +57,6 @@ class HashBow; class NfrMapper : public BundleAdjustmentBase { public: using Ptr = std::shared_ptr; - using TimeCamId = std::pair; using Matches = tbb::concurrent_unordered_map< std::pair, MatchData, tbb::tbb_hash>, diff --git a/src/calibration/calibraiton_helper.cpp b/src/calibration/calibraiton_helper.cpp index 0a72e5e..b23cdc1 100644 --- a/src/calibration/calibraiton_helper.cpp +++ b/src/calibration/calibraiton_helper.cpp @@ -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); diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index 7164b22..e118c9e 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -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 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 &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 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); diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index 8141478..70095f4 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -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 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; diff --git a/src/calibration/vignette.cpp b/src/calibration/vignette.cpp index 1f31784..84b21a6 100644 --- a/src/calibration/vignette.cpp +++ b/src/calibration/vignette.cpp @@ -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.; } } } diff --git a/src/mapper.cpp b/src/mapper.cpp index 11aa30f..81c4dde 100644 --- a/src/mapper.cpp +++ b/src/mapper.cpp @@ -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(); diff --git a/src/mapper_sim.cpp b/src/mapper_sim.cpp index aa47a19..5b824a6 100644 --- a/src/mapper_sim.cpp +++ b/src/mapper_sim.cpp @@ -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(); diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index 29c2625..25b97ac 100644 --- a/src/mapper_sim_naive.cpp +++ b/src/mapper_sim_naive.cpp @@ -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); diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index 0d93f5b..a99afe8 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -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(i * POSE_SIZE) = rld.d_rel_d_h[i] * inc.segment(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; diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 8c769c2..327bcef 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -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(); - 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(); 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); } } } diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 874831d..c0fcb24 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -391,7 +391,7 @@ void NfrMapper::detect_keypoints() { computeDescriptors(img, kd); std::vector 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); diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index 7f25f2d..7e108a2 100644 --- a/src/vio_sim.cpp +++ b/src/vio_sim.cpp @@ -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);