Point Cloud Library (PCL)
1.14.1-dev
|
KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm. More...
#include </__w/1/s/gpu/kinfu_large_scale/include/pcl/gpu/kinfu_large_scale/kinfu.h>
Public Types | |
using | PixelRGB = pcl::gpu::kinfuLS::PixelRGB |
Pixel type for rendered image. More... | |
using | View = DeviceArray2D< PixelRGB > |
using | DepthMap = DeviceArray2D< unsigned short > |
using | PointType = pcl::PointXYZ |
using | NormalType = pcl::Normal |
Public Member Functions | |
void | performLastScan () |
bool | isFinished () |
KinfuTracker (const Eigen::Vector3f &volumeSize, const float shiftingDistance, int rows=480, int cols=640) | |
Constructor. More... | |
void | setDepthIntrinsics (float fx, float fy, float cx=-1, float cy=-1) |
Sets Depth camera intrinsics. More... | |
void | setInitialCameraPose (const Eigen::Affine3f &pose) |
Sets initial camera pose relative to volume coordinate space. More... | |
void | setDepthTruncationForICP (float max_icp_distance=0.f) |
Sets truncation threshold for depth image for ICP step only! This helps to filter measurements that are outside tsdf volume. More... | |
void | setIcpCorespFilteringParams (float distThreshold, float sineOfAngle) |
Sets ICP filtering parameters. More... | |
void | setCameraMovementThreshold (float threshold=0.001f) |
Sets integration threshold. More... | |
void | initColorIntegration (int max_weight=-1) |
Performs initialization for color integration. More... | |
int | cols () |
Returns cols passed to ctor. More... | |
int | rows () |
Returns rows passed to ctor. More... | |
bool | operator() (const DepthMap &depth) |
Processes next frame. More... | |
bool | operator() (const DepthMap &depth, const View &colors) |
Processes next frame (both depth and color integration). More... | |
Eigen::Affine3f | getCameraPose (int time=-1) const |
Returns camera pose at given time, default the last pose. More... | |
Eigen::Affine3f | getLastEstimatedPose () const |
std::size_t | getNumberOfPoses () const |
Returns number of poses including initial. More... | |
const TsdfVolume & | volume () const |
Returns TSDF volume storage. More... | |
TsdfVolume & | volume () |
Returns TSDF volume storage. More... | |
const ColorVolume & | colorVolume () const |
Returns color volume storage. More... | |
ColorVolume & | colorVolume () |
Returns color volume storage. More... | |
void | getImage (View &view) const |
Renders 3D scene to display to human. More... | |
void | getLastFrameCloud (DeviceArray2D< PointType > &cloud) const |
Returns point cloud abserved from last camera pose. More... | |
void | getLastFrameNormals (DeviceArray2D< NormalType > &normals) const |
Returns point cloud abserved from last camera pose. More... | |
tsdf_buffer * | getCyclicalBufferStructure () |
Returns pointer to the cyclical buffer structure. More... | |
void | extractAndSaveWorld () |
Extract the world and save it. More... | |
bool | icpIsLost () |
Returns true if ICP is currently lost. More... | |
void | reset () |
Performs the tracker reset to initial state. More... | |
void | setDisableICP () |
bool | hasShifted () const |
Return whether the last update resulted in a shift. More... | |
KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm.
using pcl::gpu::kinfuLS::KinfuTracker::DepthMap = DeviceArray2D<unsigned short> |
pcl::gpu::kinfuLS::KinfuTracker::KinfuTracker | ( | const Eigen::Vector3f & | volumeSize, |
const float | shiftingDistance, | ||
int | rows = 480 , |
||
int | cols = 640 |
||
) |
Constructor.
[in] | volumeSize | physical size of the volume represented by the tdsf volume. In meters. |
[in] | shiftingDistance | when the camera target point is farther than shiftingDistance from the center of the volume, shiting occurs. In meters. |
[in] | rows | height of depth image |
[in] | cols | width of depth image |
ColorVolume& pcl::gpu::kinfuLS::KinfuTracker::colorVolume | ( | ) |
Returns color volume storage.
const ColorVolume& pcl::gpu::kinfuLS::KinfuTracker::colorVolume | ( | ) | const |
Returns color volume storage.
int pcl::gpu::kinfuLS::KinfuTracker::cols | ( | ) |
Returns cols passed to ctor.
void pcl::gpu::kinfuLS::KinfuTracker::extractAndSaveWorld | ( | ) |
Extract the world and save it.
Eigen::Affine3f pcl::gpu::kinfuLS::KinfuTracker::getCameraPose | ( | int | time = -1 | ) | const |
Returns camera pose at given time, default the last pose.
[in] | time | Index of frame for which camera pose is returned. |
|
inline |
void pcl::gpu::kinfuLS::KinfuTracker::getImage | ( | View & | view | ) | const |
Renders 3D scene to display to human.
[out] | view | output array with image |
Eigen::Affine3f pcl::gpu::kinfuLS::KinfuTracker::getLastEstimatedPose | ( | ) | const |
void pcl::gpu::kinfuLS::KinfuTracker::getLastFrameCloud | ( | DeviceArray2D< PointType > & | cloud | ) | const |
Returns point cloud abserved from last camera pose.
[out] | cloud | output array for points |
void pcl::gpu::kinfuLS::KinfuTracker::getLastFrameNormals | ( | DeviceArray2D< NormalType > & | normals | ) | const |
Returns point cloud abserved from last camera pose.
[out] | normals | output array for normals |
std::size_t pcl::gpu::kinfuLS::KinfuTracker::getNumberOfPoses | ( | ) | const |
Returns number of poses including initial.
|
inline |
|
inline |
void pcl::gpu::kinfuLS::KinfuTracker::initColorIntegration | ( | int | max_weight = -1 | ) |
Performs initialization for color integration.
Must be called before calling color integration.
[in] | max_weight | max weighe for color integration. -1 means default weight. |
bool pcl::gpu::kinfuLS::KinfuTracker::operator() | ( | const DepthMap & | depth | ) |
Processes next frame.
[in] | depth | next frame with values in millimeters |
Processes next frame (both depth and color integration).
Please call initColorIntegration before invpoking this.
[in] | depth | next depth frame with values in millimeters |
[in] | colors | next RGB frame |
|
inline |
void pcl::gpu::kinfuLS::KinfuTracker::reset | ( | ) |
Performs the tracker reset to initial state.
It's used if camera tracking fails.
int pcl::gpu::kinfuLS::KinfuTracker::rows | ( | ) |
Returns rows passed to ctor.
void pcl::gpu::kinfuLS::KinfuTracker::setCameraMovementThreshold | ( | float | threshold = 0.001f | ) |
Sets integration threshold.
TSDF volume is integrated iff a camera movement metric exceeds the threshold value. The metric represents the following: M = (rodrigues(Rotation).norm() + alpha*translation.norm())/2, where alpha = 1.f (hardcoded constant)
[in] | threshold | a value to compare with the metric. Suitable values are ~0.001 |
void pcl::gpu::kinfuLS::KinfuTracker::setDepthIntrinsics | ( | float | fx, |
float | fy, | ||
float | cx = -1 , |
||
float | cy = -1 |
||
) |
Sets Depth camera intrinsics.
[in] | fx | focal length x |
[in] | fy | focal length y |
[in] | cx | principal point x |
[in] | cy | principal point y |
void pcl::gpu::kinfuLS::KinfuTracker::setDepthTruncationForICP | ( | float | max_icp_distance = 0.f | ) |
Sets truncation threshold for depth image for ICP step only! This helps to filter measurements that are outside tsdf volume.
Pass zero to disable the truncation.
[in] | max_icp_distance | Maximal distance, higher values are reset to zero (means no measurement). |
|
inline |
void pcl::gpu::kinfuLS::KinfuTracker::setIcpCorespFilteringParams | ( | float | distThreshold, |
float | sineOfAngle | ||
) |
Sets ICP filtering parameters.
[in] | distThreshold | distance. |
[in] | sineOfAngle | sine of angle between normals. |
void pcl::gpu::kinfuLS::KinfuTracker::setInitialCameraPose | ( | const Eigen::Affine3f & | pose | ) |
Sets initial camera pose relative to volume coordinate space.
[in] | pose | Initial camera pose |
TsdfVolume& pcl::gpu::kinfuLS::KinfuTracker::volume | ( | ) |
Returns TSDF volume storage.
const TsdfVolume& pcl::gpu::kinfuLS::KinfuTracker::volume | ( | ) | const |
Returns TSDF volume storage.