geolib2
Public Member Functions | Protected Member Functions | Protected Attributes | Static Protected Attributes | List of all members
geo::DepthCamera Class Reference

#include <DepthCamera.h>

Public Member Functions

double cx () const
 
double cy () const
 
 DepthCamera ()
 
 DepthCamera (const image_geometry::PinholeCameraModel &cam_model)
 
 DepthCamera (const sensor_msgs::CameraInfo &cam_info)
 
 DepthCamera (uint width, uint height, double fx, double fy, double cx, double cy, double tx, double ty)
 
double fx () const
 
double fy () const
 
double getFocalLengthX () const
 
double getFocalLengthY () const
 
double getOpticalCenterX () const
 
double getOpticalCenterY () const
 
double getOpticalTranslationX () const
 
double getOpticalTranslationY () const
 
int height () const
 
void initFromCamModel (const image_geometry::PinholeCameraModel &cam_model)
 Set camera parameters from pinhole camera model. More...
 
bool initialized () const
 Indicates whether the camera parameters are set. Using the camera when not initialized is useless. More...
 
template<typename T = double>
geo::Vec3T< T > project2Dto3D (int x, int y) const
 
double project2Dto3DX (int x) const
 
double project2Dto3DY (int y) const
 
template<typename Tin = double, typename Tout = double>
cv::Point_< Tout > project3Dto2D (const geo::Vec3T< Tin > &p) const
 
RasterizeResult rasterize (const Shape &shape, const Pose3D &cam_pose, const Pose3D &obj_pose, cv::Mat &image, PointerMap &pointer_map=EMPTY_POINTER_MAP, void *pointer=0, TriangleMap &triangle_map=EMPTY_TRIANGLE_MAP) const
 
RasterizeResult rasterize (const Shape &shape, const Pose3D &pose, cv::Mat &image, PointerMap &pointer_map=EMPTY_POINTER_MAP, void *pointer=0, TriangleMap &triangle_map=EMPTY_TRIANGLE_MAP) const
 rasterize: render a 3D shape onto a 2D image More...
 
void render (const RenderOptions &opt, RenderResult &res) const
 
double Tx () const
 
double Ty () const
 
int width () const
 
virtual ~DepthCamera ()
 

Protected Member Functions

template<typename Tin = double, typename Tout = double>
void drawTriangle (const geo::Vec3T< Tin > &p1, const geo::Vec3T< Tin > &p2, const geo::Vec3T< Tin > &p3, const RenderOptions &opt, RenderResult &res, uint i_triangle) const
 
template<typename T = double>
void drawTriangle2D (const geo::Vec3T< T > &p1, const geo::Vec3T< T > &p2, const geo::Vec3T< T > &p3, const RenderOptions &opt, RenderResult &res, uint i_triangle) const
 
void drawTrianglePart (int y_start, int y_end, float x_start, float x_start_delta, float x_end, float x_end_delta, float d_start, float d_start_delta, float d_end, float d_end_delta, const RenderOptions &opt, RenderResult &res, uint i_triangle) const
 
template<typename T = double>
void sort (const geo::Vec3T< T > *&p_min, const geo::Vec3T< T > *&p_mid, const geo::Vec3T< T > *&p_max, uchar dim) const
 

Protected Attributes

image_geometry::PinholeCameraModel cam_model_
 

Static Protected Attributes

static constexpr const double near_clip_z_ = -0.1
 

Detailed Description

Model of a depth camera which may be used to either convert points in an image to points in 3D space simulate a depth camera and render shapes in the image

Frame conventions: the frame of the camera is defined with the z-axis pointing into the camera, the x-axis matches the x -axis of the image, and the y-axis matches the negative y-axis of the image.

Definition at line 159 of file DepthCamera.h.

Constructor & Destructor Documentation

◆ DepthCamera() [1/4]

geo::DepthCamera::DepthCamera ( )

Definition at line 26 of file DepthCamera.cpp.

◆ DepthCamera() [2/4]

geo::DepthCamera::DepthCamera ( uint  width,
uint  height,
double  fx,
double  fy,
double  cx,
double  cy,
double  tx,
double  ty 
)

Definition at line 30 of file DepthCamera.cpp.

◆ DepthCamera() [3/4]

geo::DepthCamera::DepthCamera ( const image_geometry::PinholeCameraModel &  cam_model)

Definition at line 81 of file DepthCamera.cpp.

◆ DepthCamera() [4/4]

geo::DepthCamera::DepthCamera ( const sensor_msgs::CameraInfo &  cam_info)

Definition at line 86 of file DepthCamera.cpp.

◆ ~DepthCamera()

geo::DepthCamera::~DepthCamera ( )
virtual

Definition at line 93 of file DepthCamera.cpp.

Member Function Documentation

◆ cx()

double geo::DepthCamera::cx ( ) const
inline

Definition at line 225 of file DepthCamera.h.

◆ cy()

double geo::DepthCamera::cy ( ) const
inline

Definition at line 228 of file DepthCamera.h.

◆ drawTriangle()

template<typename Tin , typename Tout >
void geo::DepthCamera::drawTriangle ( const geo::Vec3T< Tin > &  p1,
const geo::Vec3T< Tin > &  p2,
const geo::Vec3T< Tin > &  p3,
const RenderOptions opt,
RenderResult res,
uint  i_triangle 
) const
protected

Definition at line 254 of file DepthCamera.cpp.

◆ drawTriangle2D()

template<typename T >
void geo::DepthCamera::drawTriangle2D ( const geo::Vec3T< T > &  p1,
const geo::Vec3T< T > &  p2,
const geo::Vec3T< T > &  p3,
const RenderOptions opt,
RenderResult res,
uint  i_triangle 
) const
protected

Definition at line 269 of file DepthCamera.cpp.

◆ drawTrianglePart()

void geo::DepthCamera::drawTrianglePart ( int  y_start,
int  y_end,
float  x_start,
float  x_start_delta,
float  x_end,
float  x_end_delta,
float  d_start,
float  d_start_delta,
float  d_end,
float  d_end_delta,
const RenderOptions opt,
RenderResult res,
uint  i_triangle 
) const
protected

Definition at line 322 of file DepthCamera.cpp.

◆ fx()

double geo::DepthCamera::fx ( ) const
inline

Definition at line 219 of file DepthCamera.h.

◆ fy()

double geo::DepthCamera::fy ( ) const
inline

Definition at line 222 of file DepthCamera.h.

◆ getFocalLengthX()

double geo::DepthCamera::getFocalLengthX ( ) const
inline

Definition at line 220 of file DepthCamera.h.

◆ getFocalLengthY()

double geo::DepthCamera::getFocalLengthY ( ) const
inline

Definition at line 223 of file DepthCamera.h.

◆ getOpticalCenterX()

double geo::DepthCamera::getOpticalCenterX ( ) const
inline

Definition at line 226 of file DepthCamera.h.

◆ getOpticalCenterY()

double geo::DepthCamera::getOpticalCenterY ( ) const
inline

Definition at line 229 of file DepthCamera.h.

◆ getOpticalTranslationX()

double geo::DepthCamera::getOpticalTranslationX ( ) const
inline

Definition at line 232 of file DepthCamera.h.

◆ getOpticalTranslationY()

double geo::DepthCamera::getOpticalTranslationY ( ) const
inline

Definition at line 235 of file DepthCamera.h.

◆ height()

int geo::DepthCamera::height ( ) const
inline

Definition at line 237 of file DepthCamera.h.

◆ initFromCamModel()

void geo::DepthCamera::initFromCamModel ( const image_geometry::PinholeCameraModel &  cam_model)

Set camera parameters from pinhole camera model.

Parameters
cam_modelpinhole camera model

Definition at line 96 of file DepthCamera.cpp.

◆ initialized()

bool geo::DepthCamera::initialized ( ) const
inline

Indicates whether the camera parameters are set. Using the camera when not initialized is useless.

Returns
initiliazed or not initialized

Definition at line 245 of file DepthCamera.h.

◆ project2Dto3D()

template<typename T = double>
geo::Vec3T<T> geo::DepthCamera::project2Dto3D ( int  x,
int  y 
) const
inline

convert points in an image to points in 3D space

Parameters
xx index of the 2d point in the image
yy index of the 2d point in the image
Returns
: (semi) unit vector indicating the direction of the beam corresponding to the pixel.

Definition at line 215 of file DepthCamera.h.

◆ project2Dto3DX()

double geo::DepthCamera::project2Dto3DX ( int  x) const
inline

Definition at line 204 of file DepthCamera.h.

◆ project2Dto3DY()

double geo::DepthCamera::project2Dto3DY ( int  y) const
inline

Definition at line 206 of file DepthCamera.h.

◆ project3Dto2D()

template<typename Tin = double, typename Tout = double>
cv::Point_<Tout> geo::DepthCamera::project3Dto2D ( const geo::Vec3T< Tin > &  p) const
inline

Definition at line 200 of file DepthCamera.h.

◆ rasterize() [1/2]

RasterizeResult geo::DepthCamera::rasterize ( const Shape shape,
const Pose3D cam_pose,
const Pose3D obj_pose,
cv::Mat &  image,
PointerMap pointer_map = EMPTY_POINTER_MAP,
void *  pointer = 0,
TriangleMap triangle_map = EMPTY_TRIANGLE_MAP 
) const

Definition at line 107 of file DepthCamera.cpp.

◆ rasterize() [2/2]

RasterizeResult geo::DepthCamera::rasterize ( const Shape shape,
const Pose3D pose,
cv::Mat &  image,
PointerMap pointer_map = EMPTY_POINTER_MAP,
void *  pointer = 0,
TriangleMap triangle_map = EMPTY_TRIANGLE_MAP 
) const

rasterize: render a 3D shape onto a 2D image

Parameters
shape3D shape to be rendered
posepose of the shape with respect to the camera
imageimage to render the result to
pointer_mappointer map to store an identifier of the shape
pointeridentifier of the shape
triangle_maptriangle map to store the index of a triangle in the mesh
Returns

Definition at line 114 of file DepthCamera.cpp.

◆ render()

void geo::DepthCamera::render ( const RenderOptions opt,
RenderResult res 
) const

Definition at line 129 of file DepthCamera.cpp.

◆ sort()

template<typename T >
void geo::DepthCamera::sort ( const geo::Vec3T< T > *&  p_min,
const geo::Vec3T< T > *&  p_mid,
const geo::Vec3T< T > *&  p_max,
uchar  dim 
) const
protected

Definition at line 368 of file DepthCamera.cpp.

◆ Tx()

double geo::DepthCamera::Tx ( ) const
inline

Definition at line 231 of file DepthCamera.h.

◆ Ty()

double geo::DepthCamera::Ty ( ) const
inline

Definition at line 234 of file DepthCamera.h.

◆ width()

int geo::DepthCamera::width ( ) const
inline

Definition at line 239 of file DepthCamera.h.

Member Data Documentation

◆ cam_model_

image_geometry::PinholeCameraModel geo::DepthCamera::cam_model_
protected

Definition at line 251 of file DepthCamera.h.

◆ near_clip_z_

constexpr const double geo::DepthCamera::near_clip_z_ = -0.1
staticconstexprprotected

Definition at line 249 of file DepthCamera.h.


The documentation for this class was generated from the following files: