Program Listing for File visual-nframe-simulator.h

Return to documentation for file (algorithms/simulation/include/simulation/visual-nframe-simulator.h)

#ifndef SIMULATION_VISUAL_NFRAME_SIMULATOR_H_
#define SIMULATION_VISUAL_NFRAME_SIMULATOR_H_

#include <memory>
#include <mutex>
#include <vector>

#include <aslam/cameras/ncamera.h>
#include <aslam/frames/visual-nframe.h>
#include <maplab-common/macros.h>
#include <vi-map/unique-id.h>

#include "simulation/generic-path-generator.h"

using test_trajectory_gen::VisualInertialPathGenerator;

namespace simulation {

class VisualNFrameSimulator {
 public:
  MAPLAB_POINTER_TYPEDEFS(VisualNFrameSimulator);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VisualNFrameSimulator() = default;
  virtual ~VisualNFrameSimulator() {}

  void simulateVisualNFrames(
      const Eigen::VectorXd& timestamps_seconds,
      const aslam::TransformationVector& T_G_Bs,
      const Eigen::Matrix3Xd& G_landmarks,
      const aslam::NCamera::Ptr& camera_rig, double keypoint_sigma_px,
      bool add_noise_to_keypoints, size_t num_bits_to_flip,
      aslam::VisualNFrame::PtrVector* nframe_list,
      aslam::TransformationVector* poses_without_keypoints);

  const aslam::VisualFrame::DescriptorsT& getGroundTruthDescriptors() const;

  const vi_map::LandmarkIdList& getGroundTruthLandmarkIds() const;

  const Eigen::VectorXd& getGroundTruthLandmarkScores() const;

  static constexpr uint32_t kDescriptorSizeBytes = 48;

  const std::vector<size_t>& getGroundTruthLandmarkObservationCount() {
    return ground_truth_landmark_observation_count_;
  }

 private:
  void projectLandmarksAndFillFrame(
      const Eigen::Matrix3Xd& G_landmarks, const aslam::Transformation& T_B_G,
      double keypoint_sigma_px, size_t num_bits_to_flip,
      const Eigen::Matrix2d* keypoint_std_dev, aslam::VisualNFrame* nframe,
      aslam::TransformationVector* poses_with_no_visible_landmarks);

  void generateGroundTruth(size_t num_landmarks);

  inline bool withinImageBoxWithBorder(
      Eigen::Block<Eigen::Matrix2Xd, 2, 1> keypoint,
      const aslam::Camera& camera) {
    static constexpr double kMinDistanceToBorderPx = 10.0;
    CHECK_LT(2 * kMinDistanceToBorderPx, camera.imageWidth());
    CHECK_LT(2 * kMinDistanceToBorderPx, camera.imageHeight());
    return (keypoint(0) >= kMinDistanceToBorderPx) &&
           (keypoint(1) >= kMinDistanceToBorderPx) &&
           (keypoint(0) < static_cast<double>(camera.imageWidth()) -
                              kMinDistanceToBorderPx) &&
           (keypoint(1) <
            static_cast<double>(camera.imageHeight()) - kMinDistanceToBorderPx);
  }

  aslam::VisualFrame::DescriptorsT ground_truth_landmark_descriptors_;
  vi_map::LandmarkIdList ground_truth_landmark_ids_;
  Eigen::VectorXd ground_truth_landmark_scores_;
  std::vector<size_t> ground_truth_landmark_observation_count_;
  std::mutex m_parallel_projection_;
};

}  // namespace simulation
#endif  // SIMULATION_VISUAL_NFRAME_SIMULATOR_H_