Visual Servoing Platform
version 3.5.0 under development (2022-02-15)
|
#include <visp3/sensor/vpOccipitalStructure.h>
Public Types | |
enum | vpOccipitalStructureStream { visible, depth, infrared, imu } |
Public Member Functions | |
vpOccipitalStructure () | |
~vpOccipitalStructure () | |
void | acquire (vpImage< unsigned char > &gray, bool undistorted=false, double *ts=NULL) |
void | acquire (vpImage< vpRGBa > &rgb, bool undistorted=false, double *ts=NULL) |
void | acquire (vpImage< vpRGBa > *rgb, vpImage< vpRGBa > *depth, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL, bool undistorted=false, double *ts=NULL) |
void | acquire (vpImage< unsigned char > *gray, vpImage< vpRGBa > *depth, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL, bool undistorted=false, double *ts=NULL) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud=NULL, unsigned char *const data_infrared=NULL, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL, bool undistorted=true, double *ts=NULL) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud, unsigned char *const data_infrared=NULL, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL, bool undistorted=true, double *ts=NULL) |
void | acquire (unsigned char *const data_image, unsigned char *const data_depth, std::vector< vpColVector > *const data_pointCloud, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pointcloud, unsigned char *const data_infrared=NULL, vpColVector *acceleration_data=NULL, vpColVector *gyroscope_data=NULL, bool undistorted=true, double *ts=NULL) |
void | getIMUVelocity (vpColVector *imu_vel, double *ts) |
void | getIMUAcceleration (vpColVector *imu_acc, double *ts) |
void | getIMUData (vpColVector *imu_vel, vpColVector *imu_acc, double *ts=NULL) |
bool | open (const ST::CaptureSessionSettings &settings) |
void | close () |
ST::StructureCoreCameraType | getCameraType () const |
ST::CaptureSessionUSBVersion | getUSBVersion () const |
std::string | getSerialNumber () const |
ST::CaptureSession & | getCaptureSession () |
ST::CaptureSessionSettings & | getCaptureSessionSettings () |
unsigned int | getWidth (vpOccipitalStructureStream stream_type) |
unsigned int | getHeight (vpOccipitalStructureStream stream_type) |
float | getDepth (int x, int y) |
vpPoint | unprojectPoint (int row, int col) |
vpHomogeneousMatrix | getTransform (const vpOccipitalStructureStream from, const vpOccipitalStructureStream to) |
ST::Intrinsics | getIntrinsics (const vpOccipitalStructureStream stream_type) const |
vpCameraParameters | getCameraParameters (const vpOccipitalStructureStream stream_type, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithoutDistortion) |
void | saveDepthImageAsPointCloudMesh (std::string &filename) |
Protected Member Functions | |
void | getPointcloud (std::vector< vpColVector > &pointcloud) |
void | getPointcloud (pcl::PointCloud< pcl::PointXYZ >::Ptr &pointcloud) |
void | getColoredPointcloud (pcl::PointCloud< pcl::PointXYZRGB >::Ptr &pointcloud) |
Protected Attributes | |
bool | m_init |
float | m_invalidDepthValue |
float | m_maxZ |
ST::CaptureSession | m_captureSession |
ST::CaptureSessionSettings | m_captureSessionSettings |
SessionDelegate | m_delegate |
vpCameraParameters | m_visible_camera_parameters |
vpCameraParameters | m_depth_camera_parameters |
This class provides a wrapper over the Occipital Structure SDK library https://structure.io/developers. It allows to capture data from the Occipital Structure Core camera.
The usage of vpOccipitalStructure class is enabled when libStructure 3rd party is successfully installed. The following tutorials explain how to proceed:
Moreover, if Point Cloud Library (PCL) 3rd party is installed, we also propose interfaces to retrieve point cloud as pcl::PointCloud<pcl::PointXYZ> or pcl::PointCloud<pcl::PointXYZRGB> data structures.
sample-structure-core.cpp
that uses vpOccipitalStructure class.To acquire images from the Structure Core color camera and convert them into grey level images, a good starting is to use the following code that corresponds to the content of `sample-structure-core.cpp
:
If you want to acquire color images, in the previous sample replace:
by
If you are interested in the point cloud and if ViSP is build with PCL support, you can start from the following example where we use PCL library to visualize the point cloud
References to ST::CaptureSession
and ST::CaptureSessionSettings
can be retrieved with (sc.open() must be called before
):
Definition at line 266 of file vpOccipitalStructure.h.
Enumerator | |
---|---|
visible | Visible stream. |
depth | Depth stream. |
infrared | Infrared stream. |
imu | IMU stream. |
Definition at line 269 of file vpOccipitalStructure.h.
vpOccipitalStructure::vpOccipitalStructure | ( | ) |
Default constructor.
Definition at line 58 of file vpOccipitalStructure.cpp.
vpOccipitalStructure::~vpOccipitalStructure | ( | ) |
Default destructor that stops the streaming.
Definition at line 67 of file vpOccipitalStructure.cpp.
References close().
void vpOccipitalStructure::acquire | ( | vpImage< unsigned char > & | gray, |
bool | undistorted = false , |
||
double * | ts = NULL |
||
) |
Acquire greyscale image from Structure Core device.
gray | : Greyscale image. |
undistorted | : Set to true to get undistorted grayscale image. |
ts | : Image timestamp or NULL if not wanted. |
Definition at line 75 of file vpOccipitalStructure.cpp.
References vpImage< Type >::bitmap, SessionDelegate::cv_sampleLock, m_delegate, SessionDelegate::m_sampleLock, and SessionDelegate::m_visibleFrame.
void vpOccipitalStructure::acquire | ( | vpImage< vpRGBa > & | rgb, |
bool | undistorted = false , |
||
double * | ts = NULL |
||
) |
Acquire color image from Structure Core device.
rgb | : RGB image. |
undistorted | : Set to true to get undistorted image. |
ts | : Image timestamp or NULL if not wanted. |
Definition at line 100 of file vpOccipitalStructure.cpp.
References vpImage< Type >::bitmap, SessionDelegate::cv_sampleLock, vpImageConvert::GreyToRGBa(), SessionDelegate::m_cameraType, m_delegate, SessionDelegate::m_sampleLock, SessionDelegate::m_visibleFrame, and vpImageConvert::RGBToRGBa().
void vpOccipitalStructure::acquire | ( | vpImage< vpRGBa > * | rgb, |
vpImage< vpRGBa > * | depth, | ||
vpColVector * | acceleration_data = NULL , |
||
vpColVector * | gyroscope_data = NULL , |
||
bool | undistorted = false , |
||
double * | ts = NULL |
||
) |
Acquire rgb image and IMU data from Structure Core device.
rgb | : RGB image or NULL if not wanted. |
depth | : Depth image or NULL if not wanted. |
acceleration_data | : Acceleration data or NULL if not wanted. |
gyroscope_data | : Gyroscope data or NULL if not wanted. |
undistorted | : Set to true to get undistorted image. |
ts | : Image timestamp or NULL if not wanted. |
Definition at line 141 of file vpOccipitalStructure.cpp.
References vpImage< Type >::bitmap, SessionDelegate::cv_sampleLock, vpImageConvert::GreyToRGBa(), SessionDelegate::m_accelerometerEvent, SessionDelegate::m_cameraType, m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_sampleLock, SessionDelegate::m_visibleFrame, and vpImageConvert::RGBToRGBa().
void vpOccipitalStructure::acquire | ( | vpImage< unsigned char > * | gray, |
vpImage< vpRGBa > * | depth, | ||
vpColVector * | acceleration_data = NULL , |
||
vpColVector * | gyroscope_data = NULL , |
||
bool | undistorted = false , |
||
double * | ts = NULL |
||
) |
Acquire grayscale image, depth and IMU data from Structure Core device.
gray | : Gray image or NULL if not wanted. |
depth | : Depth image or NULL if not wanted. |
acceleration_data | : Acceleration data or NULL if not wanted. |
gyroscope_data | : Gyroscope data or NULL if not wanted. |
undistorted | : Set to true to get undistorted image. |
ts | : Image timestamp or NULL if not wanted. |
Definition at line 203 of file vpOccipitalStructure.cpp.
References vpImage< Type >::bitmap, SessionDelegate::cv_sampleLock, SessionDelegate::m_accelerometerEvent, m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_sampleLock, and SessionDelegate::m_visibleFrame.
void vpOccipitalStructure::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud = NULL , |
||
unsigned char *const | data_infrared = NULL , |
||
vpColVector * | acceleration_data = NULL , |
||
vpColVector * | gyroscope_data = NULL , |
||
bool | undistorted = true , |
||
double * | ts = NULL |
||
) |
Acquire data from Structure Core device.
data_image | : Visible image buffer or NULL if not wanted. |
data_depth | : Depth image buffer in millimeters or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
data_infrared | : Infrared image buffer or NULL if not wanted. |
acceleration_data | : Acceleration data or NULL if not wanted. |
gyroscope_data | : Gyroscope data or NULL if not wanted. |
undistorted | : Set to true if you want undistorted monochrome device image. |
ts | : Data timestamp or NULL if not wanted. |
Definition at line 253 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, getPointcloud(), vpImageConvert::GreyToRGBa(), SessionDelegate::m_accelerometerEvent, SessionDelegate::m_cameraType, m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_infraredFrame, SessionDelegate::m_sampleLock, SessionDelegate::m_visibleFrame, and vpImageConvert::RGBToRGBa().
void vpOccipitalStructure::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
pcl::PointCloud< pcl::PointXYZ >::Ptr & | pointcloud, | ||
unsigned char *const | data_infrared = NULL , |
||
vpColVector * | acceleration_data = NULL , |
||
vpColVector * | gyroscope_data = NULL , |
||
bool | undistorted = true , |
||
double * | ts = NULL |
||
) |
Acquire data from Structure Core device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
pointcloud | : Point cloud (in PCL format and without texture information) pointer or NULL if not wanted. |
data_infrared | : Infrared image buffer or NULL if not wanted. |
acceleration_data | : Acceleration data or NULL if not wanted. |
gyroscope_data | : Gyroscope data or NULL if not wanted. |
undistorted | : Set to true if you want undistorted monochrome device image. |
ts | : Data timestamp or NULL if not wanted. |
Definition at line 322 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, getPointcloud(), vpImageConvert::GreyToRGBa(), SessionDelegate::m_accelerometerEvent, SessionDelegate::m_cameraType, m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_infraredFrame, SessionDelegate::m_sampleLock, SessionDelegate::m_visibleFrame, and vpImageConvert::RGBToRGBa().
void vpOccipitalStructure::acquire | ( | unsigned char *const | data_image, |
unsigned char *const | data_depth, | ||
std::vector< vpColVector > *const | data_pointCloud, | ||
pcl::PointCloud< pcl::PointXYZRGB >::Ptr & | pointcloud, | ||
unsigned char *const | data_infrared = NULL , |
||
vpColVector * | acceleration_data = NULL , |
||
vpColVector * | gyroscope_data = NULL , |
||
bool | undistorted = true , |
||
double * | ts = NULL |
||
) |
Acquire data from Structure Core device.
data_image | : Color image buffer or NULL if not wanted. |
data_depth | : Depth image buffer or NULL if not wanted. |
data_pointCloud | : Point cloud vector pointer or NULL if not wanted. |
pointcloud | : Point cloud (in PCL format) pointer or NULL if not wanted. |
data_infrared | : Infrared image buffer or NULL if not wanted. |
acceleration_data | : Acceleration data or NULL if not wanted. |
gyroscope_data | : Gyroscope data or NULL if not wanted. |
undistorted | : Set to true if you want undistorted monochrome device image. |
ts | : Data timestamp or NULL if not wanted. |
Definition at line 392 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, getColoredPointcloud(), getPointcloud(), vpImageConvert::GreyToRGBa(), SessionDelegate::m_accelerometerEvent, SessionDelegate::m_cameraType, m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_infraredFrame, SessionDelegate::m_sampleLock, SessionDelegate::m_visibleFrame, and vpImageConvert::RGBToRGBa().
void vpOccipitalStructure::close | ( | ) |
libStructure documentation:
Unlike the start functions, this function runs synchronously and will block until the device has successfully stopped streaming. Once successful, the captureSessionEventDidOccur delegate will receive CaptureSessionEventId::Ready. If an error occurs, the captureSessionEventDidOccur delegate will receive CaptureSessionEventId::Error or the specific error case enum.
Definition at line 627 of file vpOccipitalStructure.cpp.
References m_captureSession, and m_init.
Referenced by open(), and ~vpOccipitalStructure().
vpCameraParameters vpOccipitalStructure::getCameraParameters | ( | const vpOccipitalStructureStream | stream_type, |
vpCameraParameters::vpCameraParametersProjType | proj_type = vpCameraParameters::perspectiveProjWithoutDistortion |
||
) |
Get intrinsic parameters of input stream type.
stream_type | : Type of stream (visible, depth). |
proj_type | : Perspective projection model type; with or without distorsion. |
Definition at line 865 of file vpOccipitalStructure.cpp.
References m_delegate, m_depth_camera_parameters, SessionDelegate::m_depthFrame, m_visible_camera_parameters, SessionDelegate::m_visibleFrame, vpCameraParameters::perspectiveProjWithDistortion, and vpCameraParameters::perspectiveProjWithoutDistortion.
|
inline |
Get camera type: Color or Monochrome.
Definition at line 314 of file vpOccipitalStructure.h.
|
inline |
Definition at line 318 of file vpOccipitalStructure.h.
|
inline |
Definition at line 319 of file vpOccipitalStructure.h.
References vpCameraParameters::perspectiveProjWithoutDistortion.
|
protected |
Definition at line 998 of file vpOccipitalStructure.cpp.
References vpMeterPixelConversion::convertPoint(), m_delegate, SessionDelegate::m_depthFrame, m_invalidDepthValue, m_maxZ, m_visible_camera_parameters, and SessionDelegate::m_visibleFrame.
Referenced by acquire().
float vpOccipitalStructure::getDepth | ( | int | x, |
int | y | ||
) |
Returns depth in millimeters at (x,y) if it exists, NAN otherwise.
x | : Pixel x-location. |
y | : Pixel y-location |
Definition at line 701 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, m_delegate, SessionDelegate::m_depthFrame, and SessionDelegate::m_sampleLock.
unsigned int vpOccipitalStructure::getHeight | ( | vpOccipitalStructureStream | stream_type | ) |
Returns height of given stream image.
stream_type | : Type of stream (visible, depth, infrared). |
Definition at line 670 of file vpOccipitalStructure.cpp.
References m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_infraredFrame, and SessionDelegate::m_visibleFrame.
void vpOccipitalStructure::getIMUAcceleration | ( | vpColVector * | imu_acc, |
double * | ts | ||
) |
Get linear acceleration from IMU's accelerometer.
imu_acc | : IMU 3-dim angular velocity vector from gyro. |
ts | : Timestamp. |
Definition at line 513 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, SessionDelegate::m_accelerometerEvent, m_delegate, SessionDelegate::m_sampleLock, and vpColVector::resize().
void vpOccipitalStructure::getIMUData | ( | vpColVector * | imu_vel, |
vpColVector * | imu_acc, | ||
double * | ts = NULL |
||
) |
Get IMU data (angular velocities and linear acceleration) from Structure Core device.
imu_vel | : IMU 3-dim angular velocity vector from gyro. |
imu_acc | : IMU 3-dim linear acceleration vector. |
ts | : Timestamp. |
Definition at line 555 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, SessionDelegate::m_accelerometerEvent, m_delegate, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_sampleLock, and vpColVector::resize().
void vpOccipitalStructure::getIMUVelocity | ( | vpColVector * | imu_vel, |
double * | ts | ||
) |
Get angular velocities from IMU's gyroscope.
imu_vel | : IMU 3-dim angular velocity vector from gyro. |
ts | : Timestamp. |
Definition at line 474 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, m_delegate, SessionDelegate::m_gyroscopeEvent, SessionDelegate::m_sampleLock, and vpColVector::resize().
ST::Intrinsics vpOccipitalStructure::getIntrinsics | ( | const vpOccipitalStructureStream | stream_type | ) | const |
Get intrinsic parameters corresponding to the stream. This function has to be called after open().
stream_type | : Stream for which the camera intrinsic parameters are returned. |
Definition at line 823 of file vpOccipitalStructure.cpp.
References depth, infrared, m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_infraredFrame, SessionDelegate::m_visibleFrame, and visible.
|
protected |
Definition at line 896 of file vpOccipitalStructure.cpp.
References m_delegate, SessionDelegate::m_depthFrame, m_invalidDepthValue, and m_maxZ.
Referenced by acquire().
|
protected |
Definition at line 940 of file vpOccipitalStructure.cpp.
References m_delegate, SessionDelegate::m_depthFrame, m_invalidDepthValue, and m_maxZ.
|
inline |
Definition at line 317 of file vpOccipitalStructure.h.
vpHomogeneousMatrix vpOccipitalStructure::getTransform | ( | const vpOccipitalStructureStream | from, |
const vpOccipitalStructureStream | to | ||
) |
Get the extrinsic transformation from one stream to another. This function has to be called after open().
from,to | : Streams for which the camera extrinsic parameters are returned. |
Definition at line 743 of file vpOccipitalStructure.cpp.
References depth, imu, infrared, m_captureSession, m_delegate, SessionDelegate::m_depthFrame, and visible.
|
inline |
Definition at line 316 of file vpOccipitalStructure.h.
unsigned int vpOccipitalStructure::getWidth | ( | vpOccipitalStructureStream | stream_type | ) |
Returns width of given stream image.
stream_type | : Type of stream (visible, depth, infrared). |
Definition at line 640 of file vpOccipitalStructure.cpp.
References m_delegate, SessionDelegate::m_depthFrame, SessionDelegate::m_infraredFrame, and SessionDelegate::m_visibleFrame.
bool vpOccipitalStructure::open | ( | const ST::CaptureSessionSettings & | settings | ) |
Open access to the Structure SDK CaptureSession and starts streaming.
settings | : CaptureSession settings. |
Definition at line 587 of file vpOccipitalStructure.cpp.
References close(), SessionDelegate::cv_sampleLock, m_captureSession, m_captureSessionSettings, m_delegate, m_init, and SessionDelegate::m_sampleLock.
void vpOccipitalStructure::saveDepthImageAsPointCloudMesh | ( | std::string & | filename | ) |
Converts the depth frame into a 3D point cloud using intrinsic calibration then writes out the result as PLY mesh at the provided path.
filename | : PLY file name. |
Definition at line 855 of file vpOccipitalStructure.cpp.
References m_delegate, and SessionDelegate::m_depthFrame.
vpPoint vpOccipitalStructure::unprojectPoint | ( | int | row, |
int | col | ||
) |
Returns 3D coordinates of point whose projection is at (row, col).
row | : Pixel row. |
col | : Pixel column. |
Definition at line 721 of file vpOccipitalStructure.cpp.
References SessionDelegate::cv_sampleLock, m_delegate, SessionDelegate::m_depthFrame, and SessionDelegate::m_sampleLock.
|
protected |
Definition at line 343 of file vpOccipitalStructure.h.
Referenced by close(), getTransform(), and open().
|
protected |
Definition at line 344 of file vpOccipitalStructure.h.
Referenced by open().
|
protected |
Definition at line 345 of file vpOccipitalStructure.h.
Referenced by acquire(), getCameraParameters(), getColoredPointcloud(), getDepth(), getHeight(), getIMUAcceleration(), getIMUData(), getIMUVelocity(), getIntrinsics(), getPointcloud(), getTransform(), getWidth(), open(), saveDepthImageAsPointCloudMesh(), and unprojectPoint().
|
protected |
Definition at line 346 of file vpOccipitalStructure.h.
Referenced by getCameraParameters().
|
protected |
Definition at line 339 of file vpOccipitalStructure.h.
|
protected |
Definition at line 340 of file vpOccipitalStructure.h.
Referenced by getColoredPointcloud(), and getPointcloud().
|
protected |
Definition at line 341 of file vpOccipitalStructure.h.
Referenced by getColoredPointcloud(), and getPointcloud().
|
protected |
Definition at line 346 of file vpOccipitalStructure.h.
Referenced by getCameraParameters(), and getColoredPointcloud().