Program Listing for File dense-mapping-common.h

Return to documentation for file (console-plugins/dense-mapping-constraints-plugin/include/dense-mapping/dense-mapping-common.h)

#ifndef DENSE_MAPPING_DENSE_MAPPING_COMMON_H_
#define DENSE_MAPPING_DENSE_MAPPING_COMMON_H_

#include <unordered_set>
#include <vector>

#include <aslam/common/memory.h>
#include <aslam/common/pose-types.h>
#include <aslam/common/unique-id.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <vi-map/vi-map.h>

namespace dense_mapping {

typedef Eigen::Matrix<double, 3, Eigen::Dynamic> PositionMatrix;
typedef Eigen::Matrix<int64_t, 1, Eigen::Dynamic> TimestampNsVector;

static std::unordered_set<backend::ResourceType, backend::ResourceTypeHash>
    kSupportedResourceTypes{
        backend::ResourceType::kPointCloudXYZ,
        backend::ResourceType::kPointCloudXYZI,
        backend::ResourceType::kPointCloudXYZRGBN,
        backend::ResourceType::kPointCloudXYZL
    };

struct AlignmentCandidate {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  vi_map::MissionId mission_id;
  aslam::SensorId sensor_id;
  int64_t timestamp_ns;
  backend::ResourceType resource_type;
  aslam::Transformation T_G_S_resource;

  pose_graph::VertexId closest_vertex_id;
  aslam::Transformation T_G_B_closest_vertex;

  bool isValid(const bool verbose = false) const;

  // Check if these two candidates originate from the same dense data. This does
  // not check the closest vertex transformation.
  bool isSameCandidate(
      const AlignmentCandidate& rhs, const bool verbose = false) const;
  bool operator==(const AlignmentCandidate& rhs) const;

  // Convenience function for logging.
  friend std::ostream& operator<<(
      std::ostream& out, const AlignmentCandidate& candidate);
};

struct AlignmentCandidatePair {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  AlignmentCandidate candidate_A;
  AlignmentCandidate candidate_B;

  // Initial guess for the relative transformation between these two sensor
  // frames. This can be either obtained through their relative position in the
  // pose graph or an algorithm for an intial rough alignment.
  aslam::Transformation T_SB_SA_init;

  // Final relative transformation between these two sensor frames after
  // alignment.
  bool success;
  aslam::Transformation T_SB_SA_final;
  aslam::TransformationCovariance T_SB_SA_final_covariance;

  bool isValid(const bool verbose = false) const;

  // Check if this candidate pair corresponds to the same underlying data. This
  // does not check if their initial alignment or computed alignment is the
  // same.
  bool isSameCandidatePair(
      const AlignmentCandidatePair& rhs, const bool verbose = false) const;
  bool operator==(const AlignmentCandidatePair& rhs) const;

  // Convenience function for logging.
  friend std::ostream& operator<<(
      std::ostream& out, const AlignmentCandidatePair& pair);
};

}  // namespace dense_mapping

namespace std {

template <>
struct hash<dense_mapping::AlignmentCandidate> {
  std::size_t operator()(
      const dense_mapping::AlignmentCandidate& candidate) const {
    // Fits into two bytes for sure.
    const uint64_t resource_type_number =
        static_cast<uint64_t>(candidate.resource_type);

    // Fits into 4 bytes for sure.
    const uint64_t uint_timestamp_ns =
        static_cast<uint64_t>(candidate.timestamp_ns);

    // Fill the rest with hashes from the vertex and sensor id.
    return (0xFFFFFFFFFFF00000 &
            (std::hash<pose_graph::VertexId>()(candidate.closest_vertex_id) ^
             std::hash<aslam::SensorId>()(candidate.sensor_id))) |
           (0x00000000000FFF00 & (uint_timestamp_ns << 16)) |
           (0x00000000000000FF & resource_type_number);
  }
};

template <>
struct hash<dense_mapping::AlignmentCandidatePair> {
  std::size_t operator()(
      const dense_mapping::AlignmentCandidatePair& pair) const {
    return std::hash<dense_mapping::AlignmentCandidate>()(pair.candidate_A) ^
           std::hash<dense_mapping::AlignmentCandidate>()(pair.candidate_B);
  }
};
}  // namespace std

namespace dense_mapping {

typedef AlignedUnorderedSet<AlignmentCandidatePair> AlignmentCandidatePairs;
typedef Aligned<std::vector, AlignmentCandidate> AlignmentCandidateList;
typedef AlignedUnorderedMap<vi_map::MissionId, AlignmentCandidateList>
    MissionToAlignmentCandidatesMap;

}  // namespace dense_mapping

#endif  // DENSE_MAPPING_DENSE_MAPPING_COMMON_H_