38#ifndef PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
39#define PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
41#include <pcl/common/io.h>
43#include <pcl/common/utils.h>
48template <
typename Po
intInT,
typename IntensityT>
53 track_height_ = height;
57template <
typename Po
intInT,
typename IntensityT>
62 if (keypoints->size() <= keypoints_nbr_)
63 keypoints_ = keypoints;
66 p->reserve(keypoints_nbr_);
67 for (std::size_t i = 0; i < keypoints_nbr_; ++i)
68 p->push_back((*keypoints)[i]);
72 keypoints_status_.reset(
new std::vector<int>);
73 keypoints_status_->resize(keypoints_->size(), 0);
77template <
typename Po
intInT,
typename IntensityT>
82 assert((input_ || ref_) &&
"[PyramidalKLTTracker] CALL setInputCloud FIRST!");
85 keypoints->reserve(keypoints_nbr_);
86 for (std::size_t i = 0; i < keypoints_nbr_; ++i) {
88 uv.u = points->indices[i] % input_->width;
89 uv.v = points->indices[i] / input_->width;
90 keypoints->push_back(
uv);
92 setPointsToTrack(keypoints);
96template <
typename Po
intInT,
typename IntensityT>
102 PCL_ERROR(
"[%s::initCompute] PCLBase::Init failed.\n", tracker_name_.c_str());
106 if (!input_->isOrganized()) {
108 "[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!\n",
109 tracker_name_.c_str());
113 if (!keypoints_ || keypoints_->empty()) {
114 PCL_ERROR(
"[pcl::tracking::%s::initCompute] No keypoints aborting!\n",
115 tracker_name_.c_str());
124 if ((track_height_ * track_width_) % 2 == 0) {
126 "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n",
127 tracker_name_.c_str(),
133 if (track_height_ < 3 || track_width_ < 3) {
135 "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n",
136 tracker_name_.c_str(),
142 track_width_2_ = track_width_ / 2;
143 track_height_2_ = track_height_ / 2;
145 if (nb_levels_ < 2) {
146 PCL_ERROR(
"[pcl::tracking::%s::initCompute] Number of pyramid levels should be "
148 tracker_name_.c_str());
152 if (nb_levels_ > 5) {
153 PCL_ERROR(
"[pcl::tracking::%s::initCompute] Number of pyramid levels should not "
155 tracker_name_.c_str());
169template <
typename Po
intInT,
typename IntensityT>
192 int height =
src.height, width =
src.width;
193 float* row0 =
new float[
src.width + 2];
194 float* row1 =
new float[
src.width + 2];
201 for (
int y = 0; y < height; y++) {
202 const float*
srow0 =
src_ptr + (y > 0 ? y - 1 : height > 1 ? 1 : 0) * width;
205 src_ptr + (y < height - 1 ? y + 1 : height > 1 ? height - 2 : 0) * width;
210 for (
int x = 0; x < width; x++) {
216 int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width - 2 : 0;
223 for (
int x = 0; x < width; x++) {
231template <
typename Po
intInT,
typename IntensityT>
239 int width = (
smoothed.width + 1) / 2;
240 int height = (
smoothed.height + 1) / 2;
241 std::vector<int>
ii(width);
242 for (
int i = 0; i < width; ++i)
247#pragma omp parallel for \
249 shared(down, height, output, smoothed, width) \
251 num_threads(threads_)
253 for (
int j = 0; j < height; ++j) {
255 for (
int i = 0; i < width; ++i)
263template <
typename Po
intInT,
typename IntensityT>
280template <
typename Po
intInT,
typename IntensityT>
291template <
typename Po
intInT,
typename IntensityT>
296 int width =
input->width;
297 int height =
input->height;
298 int last =
input->width - kernel_size_2_;
302#pragma omp parallel for \
304 shared(input, height, last, output, w, width) \
305 num_threads(threads_)
307 for (
int j = 0; j < height; ++j) {
308 for (
int i = kernel_size_2_; i <
last; ++i) {
310 for (
int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
316 for (
int i =
last; i < width; ++i)
319 for (
int i = 0; i < kernel_size_2_; ++i)
325template <
typename Po
intInT,
typename IntensityT>
332 int width =
input->width;
333 int height =
input->height;
334 int last =
input->height - kernel_size_2_;
338#pragma omp parallel for \
340 shared(input, h, height, last, output, width) \
341 num_threads(threads_)
343 for (
int i = 0; i < width; ++i) {
344 for (
int j = kernel_size_2_; j <
last; ++j) {
346 for (
int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l)
351 for (
int j =
last; j < height; ++j)
354 for (
int j = 0; j < kernel_size_2_; ++j)
360template <
typename Po
intInT,
typename IntensityT>
364 std::vector<FloatImageConstPtr>&
pyramid,
368 pyramid.resize(step * nb_levels_);
373#pragma omp parallel for \
376 num_threads(threads_)
379 (*
tmp)[i] = intensity_((*
input)[i]);
383 previous->height + 2 * track_height_));
401 previous->height + 2 * track_height_));
413 previous->height + 2 * track_height_));
424 for (
int level = 1; level < nb_levels_; ++level) {
432 current->height + 2 * track_height_));
443 new FloatImage(
g_x->width + 2 * track_width_,
g_x->height + 2 * track_height_));
454 new FloatImage(
g_y->width + 2 * track_width_,
g_y->height + 2 * track_height_));
470template <
typename Po
intInT,
typename IntensityT>
477 const Eigen::Array4f& weight,
478 Eigen::ArrayXXf&
win,
481 Eigen::Array3f& covariance)
const
483 const int step =
img.width;
484 covariance.setZero();
486 for (
int y = 0; y < track_height_; y++) {
497 img_ptr[x + step] * weight[2] +
img_ptr[x + step + 1] * weight[3];
514template <
typename Po
intInT,
typename IntensityT>
517 const Eigen::ArrayXXf& prev,
522 const Eigen::Array4f& weight,
523 Eigen::Array2f& b)
const
525 const int step = next.
width;
527 for (
int y = 0; y < track_height_; y++) {
529 const float*
prev_ptr = prev.data() + y * prev.cols();
544template <
typename Po
intInT,
typename IntensityT>
550 const std::vector<FloatImageConstPtr>&
pyramid,
554 Eigen::Affine3f&
motion)
const
556 std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f>>
next_pts(
558 Eigen::Array2f
half_win((track_width_ - 1) * 0.5f, (track_height_ - 1) * 0.5f);
561 for (
int level = nb_levels_ - 1; level >= 0; --level) {
567 Eigen::ArrayXXf
prev_win(track_height_, track_width_);
568 Eigen::ArrayXXf
grad_x_win(track_height_, track_width_);
569 Eigen::ArrayXXf
grad_y_win(track_height_, track_width_);
570 float ratio(1. / (1 << level));
575 if (level == nb_levels_ - 1)
598 Eigen::Array4f weight;
599 weight[0] = (1.f - a) * (1.f - b);
600 weight[1] = a * (1.f - b);
601 weight[2] = (1.f - a) * b;
602 weight[3] = 1 - weight[0] - weight[1] - weight[2];
604 Eigen::Array3f
covar = Eigen::Array3f::Zero();
605 spatialGradient(prev,
622 det < std::numeric_limits<float>::epsilon()) {
631 for (
unsigned int j = 0; j < max_iterations_; j++) {
643 weight[0] = (1.f - a) * (1.f - b);
644 weight[1] = a * (1.f - b);
645 weight[2] = (1.f - a) * b;
646 weight[3] = 1 - weight[0] - weight[1] - weight[2];
648 Eigen::Array2f
beta = Eigen::Array2f::Zero();
658 if (
delta.squaredNorm() <= epsilon_)
690 keypoints->push_back(n);
709template <
typename Po
intInT,
typename IntensityT>
716 std::vector<FloatImageConstPtr>
pyramid;
719 keypoints->reserve(keypoints_->size());
720 std::vector<int>
status(keypoints_->size(), 0);
721 track(ref_, input_, ref_pyramid_,
pyramid, keypoints_, keypoints,
status, motion_);
725 keypoints_ = keypoints;
726 *keypoints_status_ =
status;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
void mismatchVector(const Eigen::ArrayXXf &prev, const Eigen::ArrayXXf &prev_grad_x, const Eigen::ArrayXXf &prev_grad_y, const FloatImage &next, const Eigen::Array2i &location, const Eigen::Array4f &weights, Eigen::Array2f &b) const
void derivatives(const FloatImage &src, FloatImage &grad_x, FloatImage &grad_y) const
compute Scharr derivatives of a source cloud.
bool initCompute() override
This method should get called before starting the actual computation.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
void computeTracking() override
Abstract tracking method.
void setTrackingWindowSize(int width, int height)
set the tracking window size
virtual void computePyramids(const PointCloudInConstPtr &input, std::vector< FloatImageConstPtr > &pyramid, pcl::InterpolationType border_type) const
Compute the pyramidal representation of an image.
void downsample(const FloatImageConstPtr &input, FloatImageConstPtr &output) const
downsample input
virtual void track(const PointCloudInConstPtr &previous_input, const PointCloudInConstPtr ¤t_input, const std::vector< FloatImageConstPtr > &previous_pyramid, const std::vector< FloatImageConstPtr > ¤t_pyramid, const pcl::PointCloud< pcl::PointUV >::ConstPtr &previous_keypoints, pcl::PointCloud< pcl::PointUV >::Ptr ¤t_keypoints, std::vector< int > &status, Eigen::Affine3f &motion) const
virtual void spatialGradient(const FloatImage &img, const FloatImage &grad_x, const FloatImage &grad_y, const Eigen::Array2i &location, const Eigen::Array4f &weights, Eigen::ArrayXXf &win, Eigen::ArrayXXf &grad_x_win, Eigen::ArrayXXf &grad_y_win, Eigen::Array3f &covariance) const
extract the patch from the previous image, previous image gradients surrounding pixel alocation while...
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
Define methods for measuring time spent in code blocks.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
PointIndices::ConstPtr PointIndicesConstPtr
A 2D point structure representing pixel image coordinates.