converted TimeCamId to struct

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 <basalt/io/dataset_io.h>
+#include <basalt/utils/common_types.h>
 #include <basalt/calibration/calibration.hpp>
 
 #include <tbb/concurrent_unordered_map.h>

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;
       }
     }

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 <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

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<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

include/basalt/utils/imu_types.h

@@ -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>

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_h_idx = aom.abs_order_map.at(tcid_h.frame_id).first;
-      int abs_ti_idx = aom.abs_order_map.at(tcid_ti.first).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>(

include/basalt/vi_estimator/nfr_mapper.h

@@ -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>>,

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);

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<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_sum[tcid.cam_id][polar_bin] += error;
-            polar_num[tcid.second][polar_bin] += 1;
+            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_sum[tcid.cam_id][azimuth_bin] += error;
-            azimuth_num[tcid.second][azimuth_bin] += 1;
+            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())
+    if (invalid_timestamps.find(kv.first.frame_id) == invalid_timestamps.end())
-      calib_opt->addAprilgridMeasurement(kv.first.first, kv.first.second,
+      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);

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 tcid0(timestamp0_ns, 0);
-    TimeCamId tcid1 = std::make_pair(timestamp1_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())
+    if (invalid_timestamps.find(kv.first.frame_id) == invalid_timestamps.end())
-      calib_opt->addAprilgridMeasurement(kv.first.first, kv.first.second,
+      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;

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.;
       }
     }
   }

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();
 

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();
 

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);

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) {
+    if (tcid_h.frame_id != tcid_t.frame_id) {
-      int abs_h_idx = aom.abs_order_map.at(tcid_h.first).first;
+      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.first).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_h = getPoseStateWithLin(tcid_h.frame_id);
-        PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first);
+        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],
+            computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
-                           state_t.getPose(), calib.T_i_c[tcid_t.second]);
+                           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_h = getPoseStateWithLin(tcid_h.frame_id);
-              PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first);
+              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_h.getPoseLin(), calib.T_i_c[tcid_h.cam_id],
-                  state_t.getPoseLin(), calib.T_i_c[tcid_t.second], &d_rel_d_h,
+                  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_h.getPose(), calib.T_i_c[tcid_h.cam_id],
-                    state_t.getPose(), calib.T_i_c[tcid_t.second]);
+                    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;
 

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) {
+        if (num_points_connected.count(tcid_host.frame_id) == 0) {
-          num_points_connected[tcid_host.first] = 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]
+        const Eigen::Vector2d p1 = prev_opt_flow_res[tcido.frame_id]
-                                       ->observations[tcido.second]
+                                       ->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(tcidl.frame_id).getPose().inverse() *
-            getPoseStateWithLin(tcido.first).getPose();
+            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 ||
+        if (poses_to_marg.count(it2->first.frame_id) > 0 ||
-            states_to_marg_all.count(it2->first.first) > 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_h = getPoseStateWithLin(tcid_h.frame_id);
-        PoseStateWithLin state_t = getPoseStateWithLin(tcid_t.first);
+        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],
+            computeRelPose(state_h.getPose(), calib.T_i_c[tcid_h.cam_id],
-                           state_t.getPose(), calib.T_i_c[tcid_t.second]);
+                           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);
       }
     }
   }

src/vi_estimator/nfr_mapper.cpp

@@ -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 =
+      Sophus::SE3d T_w_h = frame_poses.at(tcid_h.frame_id).getPose() *
-          frame_poses.at(tcid_h.first).getPose() * calib.T_i_c[tcid_h.second];
+                           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);

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);