Program Listing for File camera-3d-lidar.h

↰ Return to documentation for file (aslam_cv2/aslam_cv_cameras/include/aslam/cameras/camera-3d-lidar.h)

#ifndef ASLAM_CAMERAS_CAMERA_3D_LIDAR_H_
#define ASLAM_CAMERAS_CAMERA_3D_LIDAR_H_

#include <aslam/cameras/camera.h>
#include <aslam/cameras/distortion.h>
#include <aslam/common/crtp-clone.h>
#include <aslam/common/macros.h>
#include <aslam/common/types.h>

namespace aslam {

// Forward declarations.
class MappedUndistorter;
class NCamera;

class Camera3DLidar : public aslam::Cloneable<Camera, Camera3DLidar> {
  friend class NCamera;

  enum { kNumOfParams = 4 };

 public:
  ASLAM_POINTER_TYPEDEFS(Camera3DLidar);

  enum { CLASS_SERIALIZATION_VERSION = 1 };
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  enum Parameters {
    kHorizontalResolutionRad = 0,
    kVerticalResolutionRad = 1,
    kVerticalCenterRad = 2,
    kHorizontalCenterRad = 3
  };


 public:
  Camera3DLidar();

  Camera3DLidar(const Camera3DLidar& other) = default;
  void operator=(const Camera3DLidar&) = delete;

 public:
  Camera3DLidar(
      const Eigen::VectorXd& intrinsics, const uint32_t image_width,
      const uint32_t image_height);

  virtual ~Camera3DLidar(){};

  friend std::ostream& operator<<(
      std::ostream& out, const Camera3DLidar& camera);



  virtual bool backProject3(
      const Eigen::Ref<const Eigen::Vector2d>& keypoint,
      Eigen::Vector3d* out_point_3d) const;

  template <typename DerivedKeyPoint, typename DerivedPoint3d>
  inline const ProjectionResult evaluateProjectionResult(
      const Eigen::MatrixBase<DerivedKeyPoint>& keypoint,
      const Eigen::MatrixBase<DerivedPoint3d>& point_3d) const;



  // Get the overloaded non-virtual project3Functional(..) from base into scope.
  using Camera::project3Functional;

  template <
      typename ScalarType, typename DistortionType, typename MIntrinsics,
      typename MDistortion>
  const ProjectionResult project3Functional(
      const Eigen::Matrix<ScalarType, 3, 1>& point_3d,
      const Eigen::MatrixBase<MIntrinsics>& intrinsics_external,
      const Eigen::MatrixBase<MDistortion>& distortion_coefficients_external,
      Eigen::Matrix<ScalarType, 2, 1>* out_keypoint) const;

  virtual const ProjectionResult project3Functional(
      const Eigen::Ref<const Eigen::Vector3d>& point_3d,
      const Eigen::VectorXd* intrinsics_external,
      const Eigen::VectorXd* distortion_coefficients_external,
      Eigen::Vector2d* out_keypoint,
      Eigen::Matrix<double, 2, 3>* out_jacobian_point,
      Eigen::Matrix<double, 2, Eigen::Dynamic>* out_jacobian_intrinsics,
      Eigen::Matrix<double, 2, Eigen::Dynamic>* out_jacobian_distortion) const;



  virtual Eigen::Vector2d createRandomKeypoint() const;

  virtual Eigen::Vector3d createRandomVisiblePoint(double depth) const;

  void getBorderRays(Eigen::MatrixXd& rays) const;


 public:

  double verticalResolution() const {
    return intrinsics_[Parameters::kVerticalResolutionRad];
  };
  double horizontalResolution() const {
    return intrinsics_[Parameters::kHorizontalResolutionRad];
  };
  double horizontalCenter() const {
    return intrinsics_[Parameters::kHorizontalCenterRad];
  };
  double verticalCenter() const {
    return intrinsics_[Parameters::kVerticalCenterRad];
  };

  inline static constexpr int parameterCount() {
    return kNumOfParams;
  }

  inline virtual int getParameterSize() const {
    return kNumOfParams;
  }

  static bool areParametersValid(const Eigen::VectorXd& parameters);

  virtual bool intrinsicsValid(const Eigen::VectorXd& intrinsics) const;

  virtual void printParameters(
      std::ostream& out, const std::string& text) const;


  template <typename DistortionType = NullDistortion>
  static Camera3DLidar::Ptr createTestCamera() {
    return Camera3DLidar::Ptr(
        std::move(createTestCameraUnique<DistortionType>()));
  }

  // Simulates OS-1 64-beam lidar.
  template <typename DistortionType = NullDistortion>
  static Camera3DLidar::UniquePtr createTestCameraUnique() {
    Eigen::VectorXd intrinsics(4, 1);
    intrinsics[Parameters::kHorizontalResolutionRad] = 2. * M_PI / 1024.;
    intrinsics[Parameters::kVerticalResolutionRad] =
        0.009203703;  // 0.527333333 deg
    intrinsics[Parameters::kVerticalCenterRad] = 0.289916642;  // 16.611 deg
    intrinsics[Parameters::kHorizontalCenterRad] = 0;

    Camera3DLidar::UniquePtr camera = aligned_unique<Camera3DLidar>(
        intrinsics, 1024u /*width*/, 64u /*height*/);
    CameraId id;
    generateId(&id);
    camera->setId(id);
    return std::move(camera);
  }

  int64_t rollingShutterDelayNanoSeconds(
      const Eigen::Vector2d& keypoint) const {
    // Don't check validity. This allows points to wander in and out
    // of the frame during optimization
    return static_cast<int64_t>(keypoint(0)) * line_delay_nanoseconds_;
  }

  int64_t rollingShutterDelayNanoSeconds() const {
    return this->imageWidth() * line_delay_nanoseconds_;
  }

  uint32_t getNumberOfLines() const {
    if (line_delay_nanoseconds_ > 0) {
      return this->imageWidth();
    }
    return 1u;
  }

  LineDelayMode getLineDelayMode() const {
    return LineDelayMode::kColumns;
  }

 private:
  static const double kSquaredMinimumDepth;

  bool isValidImpl() const override;
  void setRandomImpl() override;
  bool isEqualImpl(const Sensor& other, const bool verbose) const override;
};

}  // namespace aslam

#include "aslam/cameras/camera-3d-lidar-inl.h"

#endif  // ASLAM_CAMERAS_CAMERA_3D_LIDAR_H_