Loading...
Searching...
No Matches
kinfu.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#pragma once
#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/gpu/containers/device_array.h>
#include <pcl/gpu/kinfu/pixel_rgb.h>
#include <pcl/gpu/kinfu/tsdf_volume.h>
#include <pcl/gpu/kinfu/color_volume.h>
#include <pcl/gpu/kinfu/raycaster.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <Eigen/Core>
#include <vector>
// Focal lengths of RGB camera
#define KINFU_DEFAULT_RGB_FOCAL_X 525.f
#define KINFU_DEFAULT_RGB_FOCAL_Y 525.f
// Focal lengths of depth (i.e. NIR) camera
#define KINFU_DEFAULT_DEPTH_FOCAL_X 585.f
#define KINFU_DEFAULT_DEPTH_FOCAL_Y 585.f
namespace pcl
{
namespace gpu
{
/** \brief KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm
* \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
*/
class PCL_EXPORTS KinfuTracker
{
public:
/** \brief Pixel type for rendered image. */
using PixelRGB = pcl::gpu::PixelRGB;
using View = DeviceArray2D<PixelRGB>;
using DepthMap = DeviceArray2D<unsigned short>;
using PointType = pcl::PointXYZ;
using NormalType = pcl::Normal;
/** \brief Constructor
* \param[in] rows height of depth image
* \param[in] cols width of depth image
*/
KinfuTracker (int rows = 480, int cols = 640);
/** \brief Sets Depth camera intrinsics
* \param[in] fx focal length x
* \param[in] fy focal length y
* \param[in] cx principal point x
* \param[in] cy principal point y
*/
void
setDepthIntrinsics (float fx, float fy, float cx = -1, float cy = -1);
/** \brief Get Depth camera intrinsics
* \param[out] fx focal length x
* \param[out] fy focal length y
* \param[out] cx principal point x
* \param[out] cy principal point y
*/
void
getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy) const;
/** \brief Sets initial camera pose relative to volume coordinate space
* \param[in] pose Initial camera pose
*/
void
setInitalCameraPose (const Eigen::Affine3f& pose);
/** \brief 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.
* \param[in] max_icp_distance Maximal distance, higher values are reset to zero (means no measurement).
*/
void
setDepthTruncationForICP (float max_icp_distance = 0.f);
/** \brief Sets ICP filtering parameters.
* \param[in] distThreshold distance.
* \param[in] sineOfAngle sine of angle between normals.
*/
void
setIcpCorespFilteringParams (float distThreshold, float sineOfAngle);
/** \brief 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)
* \param[in] threshold a value to compare with the metric. Suitable values are ~0.001
*/
void
setCameraMovementThreshold(float threshold = 0.001f);
/** \brief Performs initialization for color integration. Must be called before calling color integration.
* \param[in] max_weight max weighe for color integration. -1 means default weight.
*/
void
initColorIntegration(int max_weight = -1);
/** \brief Returns cols passed to ctor */
int
cols ();
/** \brief Returns rows passed to ctor */
int
rows ();
/** \brief Processes next frame.
* \param[in] depth next frame with values in millimeters
* \param hint
* \return true if can render 3D view.
*/
bool operator() (const DepthMap& depth, Eigen::Affine3f* hint=nullptr);
/** \brief Processes next frame (both depth and color integration). Please call initColorIntegration before invpoking this.
* \param[in] depth next depth frame with values in millimeters
* \param[in] colors next RGB frame
* \return true if can render 3D view.
*/
bool operator() (const DepthMap& depth, const View& colors);
/** \brief Returns camera pose at given time, default the last pose
* \param[in] time Index of frame for which camera pose is returned.
* \return camera pose
*/
Eigen::Affine3f
getCameraPose (int time = -1) const;
/** \brief Returns number of poses including initial */
std::size_t
getNumberOfPoses () const;
/** \brief Returns TSDF volume storage */
const TsdfVolume& volume() const;
/** \brief Returns TSDF volume storage */
TsdfVolume& volume();
/** \brief Returns color volume storage */
const ColorVolume& colorVolume() const;
/** \brief Returns color volume storage */
ColorVolume& colorVolume();
/** \brief Renders 3D scene to display to human
* \param[out] view output array with image
*/
void
getImage (View& view) const;
/** \brief Returns point cloud abserved from last camera pose
* \param[out] cloud output array for points
*/
void
getLastFrameCloud (DeviceArray2D<PointType>& cloud) const;
/** \brief Returns point cloud abserved from last camera pose
* \param[out] normals output array for normals
*/
void
getLastFrameNormals (DeviceArray2D<NormalType>& normals) const;
/** \brief Disables ICP forever */
void disableIcp();
private:
/** \brief Number of pyramid levels */
enum { LEVELS = 3 };
/** \brief ICP Correspondences map type */
using CorespMap = DeviceArray2D<int>;
/** \brief Vertex or Normal Map type */
using MapArr = DeviceArray2D<float>;
using Matrix3frm = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
using Vector3f = Eigen::Vector3f;
/** \brief Height of input depth image. */
int rows_;
/** \brief Width of input depth image. */
int cols_;
/** \brief Frame counter */
int global_time_;
/** \brief Truncation threshold for depth image for ICP step */
float max_icp_distance_;
/** \brief Intrinsic parameters of depth camera. */
float fx_, fy_, cx_, cy_;
/** \brief Tsdf volume container. */
TsdfVolume::Ptr tsdf_volume_;
ColorVolume::Ptr color_volume_;
/** \brief Initial camera rotation in volume coo space. */
Matrix3frm init_Rcam_;
/** \brief Initial camera position in volume coo space. */
Vector3f init_tcam_;
/** \brief array with IPC iteration numbers for each pyramid level */
int icp_iterations_[LEVELS];
/** \brief distance threshold in correspondences filtering */
float distThres_;
/** \brief angle threshold in correspondences filtering. Represents max sine of angle between normals. */
float angleThres_;
/** \brief Depth pyramid. */
std::vector<DepthMap> depths_curr_;
/** \brief Vertex maps pyramid for current frame in global coordinate space. */
std::vector<MapArr> vmaps_g_curr_;
/** \brief Normal maps pyramid for current frame in global coordinate space. */
std::vector<MapArr> nmaps_g_curr_;
/** \brief Vertex maps pyramid for previous frame in global coordinate space. */
std::vector<MapArr> vmaps_g_prev_;
/** \brief Normal maps pyramid for previous frame in global coordinate space. */
std::vector<MapArr> nmaps_g_prev_;
/** \brief Vertex maps pyramid for current frame in current coordinate space. */
std::vector<MapArr> vmaps_curr_;
/** \brief Normal maps pyramid for current frame in current coordinate space. */
std::vector<MapArr> nmaps_curr_;
/** \brief Array of buffers with ICP correspondences for each pyramid level. */
std::vector<CorespMap> coresps_;
/** \brief Buffer for storing scaled depth image */
DeviceArray2D<float> depthRawScaled_;
/** \brief Temporary buffer for ICP */
DeviceArray2D<double> gbuf_;
/** \brief Buffer to store MLS matrix. */
DeviceArray<double> sumbuf_;
/** \brief Array of camera rotation matrices for each moment of time. */
std::vector<Matrix3frm> rmats_;
/** \brief Array of camera translations for each moment of time. */
std::vector<Vector3f> tvecs_;
/** \brief Camera movement threshold. TSDF is integrated iff a camera movement metric exceeds some value. */
float integration_metric_threshold_;
/** \brief ICP step is completely disabled. Only integration now. */
bool disable_icp_;
/** \brief Allocates all GPU internal buffers.
* \param[in] rows_arg
* \param[in] cols_arg
*/
void
allocateBufffers (int rows_arg, int cols_arg);
/** \brief Performs the tracker reset to initial state. It's used if case of camera tracking fail.
*/
void
reset ();
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
}
};
pcl::gpu::ColorVolume
ColorVolume class.
Definition color_volume.h:57
pcl::gpu::ColorVolume::Ptr
shared_ptr< ColorVolume > Ptr
Definition color_volume.h:60
pcl::gpu::DeviceArray2D
DeviceArray2D class
Definition device_array.h:188
pcl::gpu::DeviceArray
DeviceArray class
Definition device_array.h:54
pcl::gpu::KinfuTracker
KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm.
Definition kinfu.h:68
pcl::gpu::KinfuTracker::cols
int cols()
Returns cols passed to ctor.
pcl::gpu::KinfuTracker::getLastFrameCloud
void getLastFrameCloud(DeviceArray2D< PointType > &cloud) const
Returns point cloud abserved from last camera pose.
pcl::gpu::KinfuTracker::colorVolume
ColorVolume & colorVolume()
Returns color volume storage.
pcl::gpu::KinfuTracker::KinfuTracker
KinfuTracker(int rows=480, int cols=640)
Constructor.
pcl::gpu::KinfuTracker::getImage
void getImage(View &view) const
Renders 3D scene to display to human.
pcl::gpu::KinfuTracker::setInitalCameraPose
void setInitalCameraPose(const Eigen::Affine3f &pose)
Sets initial camera pose relative to volume coordinate space.
pcl::gpu::KinfuTracker::initColorIntegration
void initColorIntegration(int max_weight=-1)
Performs initialization for color integration.
pcl::gpu::KinfuTracker::setIcpCorespFilteringParams
void setIcpCorespFilteringParams(float distThreshold, float sineOfAngle)
Sets ICP filtering parameters.
pcl::gpu::KinfuTracker::disableIcp
void disableIcp()
Disables ICP forever.
pcl::gpu::KinfuTracker::getNumberOfPoses
std::size_t getNumberOfPoses() const
Returns number of poses including initial.
pcl::gpu::KinfuTracker::volume
const TsdfVolume & volume() const
Returns TSDF volume storage.
pcl::gpu::KinfuTracker::volume
TsdfVolume & volume()
Returns TSDF volume storage.
pcl::gpu::KinfuTracker::getDepthIntrinsics
void getDepthIntrinsics(float &fx, float &fy, float &cx, float &cy) const
Get Depth camera intrinsics.
pcl::gpu::KinfuTracker::getLastFrameNormals
void getLastFrameNormals(DeviceArray2D< NormalType > &normals) const
Returns point cloud abserved from last camera pose.
pcl::gpu::KinfuTracker::setCameraMovementThreshold
void setCameraMovementThreshold(float threshold=0.001f)
Sets integration threshold.
pcl::gpu::KinfuTracker::View
DeviceArray2D< PixelRGB > View
Definition kinfu.h:73
pcl::gpu::KinfuTracker::setDepthIntrinsics
void setDepthIntrinsics(float fx, float fy, float cx=-1, float cy=-1)
Sets Depth camera intrinsics.
pcl::gpu::KinfuTracker::getCameraPose
Eigen::Affine3f getCameraPose(int time=-1) const
Returns camera pose at given time, default the last pose.
pcl::gpu::KinfuTracker::colorVolume
const ColorVolume & colorVolume() const
Returns color volume storage.
pcl::gpu::KinfuTracker::rows
int rows()
Returns rows passed to ctor.
pcl::gpu::KinfuTracker::setDepthTruncationForICP
void setDepthTruncationForICP(float max_icp_distance=0.f)
Sets truncation threshold for depth image for ICP step only! This helps to filter measurements that a...
pcl::gpu::TsdfVolume
TsdfVolume class.
Definition tsdf_volume.h:56
pcl::gpu::TsdfVolume::Ptr
shared_ptr< TsdfVolume > Ptr
Definition tsdf_volume.h:58
point_types.h
Defines all the PCL implemented PointT point type structures.
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl
Definition convolution.h:46
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition point_types.hpp:823
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition point_types.hpp:351
pcl::gpu::PixelRGB
Input/output pixel format for KinfuTracker.
Definition pixel_rgb.h:47