Program Listing for File camera_model.h

↰ Return to documentation for file (voxblox/include/voxblox/utils/camera_model.h)

#ifndef VOXBLOX_UTILS_CAMERA_MODEL_H_
#define VOXBLOX_UTILS_CAMERA_MODEL_H_

#include <vector>

#include <glog/logging.h>
#include <kindr/minimal/quat-transformation.h>
#include <Eigen/Core>

#include "voxblox/core/common.h"

namespace voxblox {

class Plane {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Plane() : normal_(Point::Identity()), distance_(0) {}
  virtual ~Plane() {}

  void setFromPoints(const Point& p1, const Point& p2, const Point& p3);
  void setFromDistanceNormal(const Point& normal, double distance);

  bool isPointInside(const Point& point) const;

  Point normal() const { return normal_; }
  double distance() const { return distance_; }

 private:
  Point normal_;
  double distance_;
};

class CameraModel {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CameraModel() : initialized_(false) {}
  virtual ~CameraModel() {}

  void setIntrinsicsFromFocalLength(
      const Eigen::Matrix<FloatingPoint, 2, 1>& resolution, double focal_length,
      double min_distance, double max_distance);
  void setIntrinsicsFromFoV(double horizontal_fov, double vertical_fov,
                            double min_distance, double max_distance);
  void setExtrinsics(const Transformation& T_C_B);

  Transformation getCameraPose() const;
  Transformation getBodyPose() const;
  void setCameraPose(const Transformation& cam_pose);
  void setBodyPose(const Transformation& body_pose);

  void getAabb(Point* aabb_min, Point* aabb_max) const;
  bool isPointInView(const Point& point) const;

  const AlignedVector<Plane>& getBoundingPlanes() const {
    return bounding_planes_;
  }
  void getBoundingLines(AlignedVector<Point>* lines) const;

  void getFarPlanePoints(AlignedVector<Point>* points) const;

 private:
  void calculateBoundingPlanes();

  bool initialized_;

  Transformation T_C_B_;
  Transformation T_G_C_;

  AlignedVector<Point> corners_C_;

  AlignedVector<Plane> bounding_planes_;
  Point aabb_min_;
  Point aabb_max_;
};
}  // namespace voxblox

#endif  // VOXBLOX_UTILS_CAMERA_MODEL_H_