38 #ifndef PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
39 #define PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
41 #include <pcl/common/time.h>
42 #include <pcl/common/utils.h>
43 #include <pcl/common/io.h>
44 #include <pcl/common/utils.h>
53 template <
typename Po
intInT,
typename IntensityT>
inline void
57 track_height_ = height;
61 template <
typename Po
intInT,
typename IntensityT>
inline void
64 if (keypoints->
size () <= keypoints_nbr_)
65 keypoints_ = keypoints;
70 for (std::size_t i = 0; i < keypoints_nbr_; ++i)
76 keypoints_status_->indices.
resize (keypoints_->size (), 0);
80 template <
typename Po
intInT,
typename IntensityT>
inline void
83 assert ((input_ || ref_) &&
"[pcl::tracking::PyramidalKLTTracker] CALL setInputCloud FIRST!");
86 keypoints->
reserve (keypoints_nbr_);
87 for (std::size_t i = 0; i < keypoints_nbr_; ++i)
90 uv.
u = points->indices[i] % input_->width;
91 uv.
v = points->indices[i] / input_->width;
94 setPointsToTrack (keypoints);
98 template <
typename Po
intInT,
typename IntensityT>
bool
104 PCL_ERROR (
"[pcl::tracking::%s::initCompute] PCLBase::Init failed.\n",
105 tracker_name_.c_str ());
109 if (!input_->isOrganized ())
111 PCL_ERROR (
"[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!",
112 tracker_name_.c_str ());
116 if (!keypoints_ || keypoints_->empty ())
118 PCL_ERROR (
"[pcl::tracking::%s::initCompute] No keypoints aborting!",
119 tracker_name_.c_str ());
129 if ((track_height_ * track_width_)%2 == 0)
131 PCL_ERROR (
"[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n",
132 tracker_name_.c_str (), track_width_, track_height_);
136 if (track_height_ < 3 || track_width_ < 3)
138 PCL_ERROR (
"[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n",
139 tracker_name_.c_str (), track_width_, track_height_);
143 track_width_2_ = track_width_ / 2;
144 track_height_2_ = track_height_ / 2;
148 PCL_ERROR (
"[pcl::tracking::%s::initCompute] Number of pyramid levels should be at least 2!",
149 tracker_name_.c_str ());
155 PCL_ERROR (
"[pcl::tracking::%s::initCompute] Number of pyramid levels should not exceed 5!",
156 tracker_name_.c_str ());
170 template <
typename Po
intInT,
typename IntensityT>
void
189 float *row0 =
new float [src.
width + 2];
190 float *row1 =
new float [src.
width + 2];
191 float *trow0 = row0; ++trow0;
192 float *trow1 = row1; ++trow1;
193 const float* src_ptr = &(src.
points[0]);
195 for (
int y = 0; y < height; y++)
197 const float* srow0 = src_ptr + (y > 0 ? y-1 : height > 1 ? 1 : 0) * width;
198 const float* srow1 = src_ptr + y * width;
199 const float* srow2 = src_ptr + (y < height-1 ? y+1 : height > 1 ? height-2 : 0) * width;
200 float* grad_x_row = &(grad_x.
points[y * width]);
201 float* grad_y_row = &(grad_y.
points[y * width]);
204 for (
int x = 0; x < width; x++)
206 trow0[x] = (srow0[x] + srow2[x])*3 + srow1[x]*10;
207 trow1[x] = srow2[x] - srow0[x];
211 int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width-2 : 0;
212 trow0[-1] = trow0[x0]; trow0[width] = trow0[x1];
213 trow1[-1] = trow1[x0]; trow1[width] = trow1[x1];
216 for (
int x = 0; x < width; x++)
218 grad_x_row[x] = trow0[x+1] - trow0[x-1];
219 grad_y_row[x] = (trow1[x+1] + trow1[x-1])*3 + trow1[x]*10;
225 template <
typename Po
intInT,
typename IntensityT>
void
229 FloatImage smoothed (input->width, input->height);
230 convolve (input, smoothed);
232 int width = (smoothed.
width +1) / 2;
233 int height = (smoothed.
height +1) / 2;
234 std::vector<int> ii (width);
235 for (
int i = 0; i < width; ++i)
239 #pragma omp parallel for \
241 shared(down, height, output, smoothed, width) \
243 num_threads(threads_)
244 for (
int j = 0; j < height; ++j)
247 for (
int i = 0; i < width; ++i)
248 (*down) (i,j) = smoothed (ii[i],jj);
255 template <
typename Po
intInT,
typename IntensityT>
void
261 downsample (input, output);
264 derivatives (*output, *grad_x, *grad_y);
265 output_grad_x = grad_x;
266 output_grad_y = grad_y;
270 template <
typename Po
intInT,
typename IntensityT>
void
274 convolveRows (input, *tmp);
275 convolveCols (tmp, output);
279 template <
typename Po
intInT,
typename IntensityT>
void
282 int width = input->width;
283 int height = input->height;
284 int last = input->width - kernel_size_2_;
287 #pragma omp parallel for \
289 shared(input, height, last, output, w, width) \
290 num_threads(threads_)
291 for (
int j = 0; j < height; ++j)
293 for (
int i = kernel_size_2_; i < last; ++i)
296 for (
int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
297 result+= kernel_[k] * (*input) (l,j);
299 output (i,j) =
static_cast<float> (result);
302 for (
int i = last; i < width; ++i)
303 output (i,j) = output (w, j);
305 for (
int i = 0; i < kernel_size_2_; ++i)
306 output (i,j) = output (kernel_size_2_, j);
311 template <
typename Po
intInT,
typename IntensityT>
void
314 output =
FloatImage (input->width, input->height);
316 int width = input->width;
317 int height = input->height;
318 int last = input->height - kernel_size_2_;
321 #pragma omp parallel for \
323 shared(input, h, height, last, output, width) \
324 num_threads(threads_)
325 for (
int i = 0; i < width; ++i)
327 for (
int j = kernel_size_2_; j < last; ++j)
330 for (
int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l)
331 result += kernel_[k] * (*input) (i,l);
332 output (i,j) =
static_cast<float> (result);
335 for (
int j = last; j < height; ++j)
336 output (i,j) = output (i,h);
338 for (
int j = 0; j < kernel_size_2_; ++j)
339 output (i,j) = output (i, kernel_size_2_);
344 template <
typename Po
intInT,
typename IntensityT>
void
346 std::vector<FloatImageConstPtr>& pyramid,
350 pyramid.resize (step * nb_levels_);
354 #pragma omp parallel for \
357 num_threads(threads_)
358 for (
int i = 0; i < static_cast<int> (input->size ()); ++i)
359 tmp->points[i] = intensity_ (input->points[i]);
363 previous->height + 2*track_height_));
372 derivatives (*img, *g_x, *g_y);
375 previous->height + 2*track_height_));
376 pcl::copyPointCloud (*g_x, *grad_x, track_height_, track_height_, track_width_, track_width_,
381 previous->height + 2*track_height_));
382 pcl::copyPointCloud (*g_y, *grad_y, track_height_, track_height_, track_width_, track_width_,
386 for (
int level = 1; level < nb_levels_; ++level)
392 downsample (previous, current, g_x, g_y);
395 current->height + 2*track_height_));
396 pcl::copyPointCloud (*current, *image, track_height_, track_height_, track_width_, track_width_,
398 pyramid[level*step] = image;
400 pcl::copyPointCloud (*g_x, *gradx, track_height_, track_height_, track_width_, track_width_,
402 pyramid[level*step + 1] = gradx;
404 pcl::copyPointCloud (*g_y, *grady, track_height_, track_height_, track_width_, track_width_,
406 pyramid[level*step + 2] = grady;
413 template <
typename Po
intInT,
typename IntensityT>
void
417 const Eigen::Array2i& location,
418 const Eigen::Array4f& weight,
419 Eigen::ArrayXXf& win,
420 Eigen::ArrayXXf& grad_x_win,
421 Eigen::ArrayXXf& grad_y_win,
422 Eigen::Array3f &covariance)
const
424 const int step = img.
width;
425 covariance.setZero ();
427 for (
int y = 0; y < track_height_; y++)
429 const float* img_ptr = &(img.
points[0]) + (y + location[1])*step + location[0];
430 const float* grad_x_ptr = &(grad_x.
points[0]) + (y + location[1])*step + location[0];
431 const float* grad_y_ptr = &(grad_y.
points[0]) + (y + location[1])*step + location[0];
433 float* win_ptr = win.data () + y*win.cols ();
434 float* grad_x_win_ptr = grad_x_win.data () + y*grad_x_win.cols ();
435 float* grad_y_win_ptr = grad_y_win.data () + y*grad_y_win.cols ();
437 for (
int x =0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr)
439 *win_ptr++ = img_ptr[x]*weight[0] + img_ptr[x+1]*weight[1] + img_ptr[x+step]*weight[2] + img_ptr[x+step+1]*weight[3];
440 float ixval = grad_x_ptr[0]*weight[0] + grad_x_ptr[1]*weight[1] + grad_x_ptr[step]*weight[2] + grad_x_ptr[step+1]*weight[3];
441 float iyval = grad_y_ptr[0]*weight[0] + grad_y_ptr[1]*weight[1] + grad_y_ptr[step]*weight[2] + grad_y_ptr[step+1]*weight[3];
443 *grad_x_win_ptr++ = ixval;
444 *grad_y_win_ptr++ = iyval;
446 covariance[0] += ixval*ixval;
447 covariance[1] += ixval*iyval;
448 covariance[2] += iyval*iyval;
454 template <
typename Po
intInT,
typename IntensityT>
void
456 const Eigen::ArrayXXf& prev_grad_x,
457 const Eigen::ArrayXXf& prev_grad_y,
459 const Eigen::Array2i& location,
460 const Eigen::Array4f& weight,
461 Eigen::Array2f &b)
const
463 const int step = next.
width;
465 for (
int y = 0; y < track_height_; y++)
467 const float* next_ptr = &(next.
points[0]) + (y + location[1])*step + location[0];
468 const float* prev_ptr = prev.data () + y*prev.cols ();
469 const float* prev_grad_x_ptr = prev_grad_x.data () + y*prev_grad_x.cols ();
470 const float* prev_grad_y_ptr = prev_grad_y.data () + y*prev_grad_y.cols ();
472 for (
int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr)
474 float diff = next_ptr[x]*weight[0] + next_ptr[x+1]*weight[1]
475 + next_ptr[x+step]*weight[2] + next_ptr[x+step+1]*weight[3] - prev_ptr[x];
476 b[0] += *prev_grad_x_ptr * diff;
477 b[1] += *prev_grad_y_ptr * diff;
483 template <
typename Po
intInT,
typename IntensityT>
void
486 const std::vector<FloatImageConstPtr>& prev_pyramid,
487 const std::vector<FloatImageConstPtr>& pyramid,
490 std::vector<int>& status,
491 Eigen::Affine3f& motion)
const
493 std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f> > next_pts (prev_keypoints->
size ());
494 Eigen::Array2f half_win ((track_width_-1)*0.5f, (track_height_-1)*0.5f);
496 const int nb_points = prev_keypoints->
size ();
497 for (
int level = nb_levels_ - 1; level >= 0; --level)
499 const FloatImage& prev = *(prev_pyramid[level*3]);
501 const FloatImage& grad_x = *(prev_pyramid[level*3+1]);
502 const FloatImage& grad_y = *(prev_pyramid[level*3+2]);
504 Eigen::ArrayXXf prev_win (track_height_, track_width_);
505 Eigen::ArrayXXf grad_x_win (track_height_, track_width_);
506 Eigen::ArrayXXf grad_y_win (track_height_, track_width_);
507 float ratio (1./(1 << level));
508 for (
int ptidx = 0; ptidx < nb_points; ptidx++)
510 Eigen::Array2f prev_pt (prev_keypoints->
points[ptidx].u * ratio,
511 prev_keypoints->
points[ptidx].v * ratio);
512 Eigen::Array2f next_pt;
513 if (level == nb_levels_ -1)
516 next_pt = next_pts[ptidx]*2.f;
518 next_pts[ptidx] = next_pt;
520 Eigen::Array2i iprev_point;
522 iprev_point[0] = std::floor (prev_pt[0]);
523 iprev_point[1] = std::floor (prev_pt[1]);
525 if (iprev_point[0] < -track_width_ || (std::uint32_t) iprev_point[0] >= grad_x.
width ||
526 iprev_point[1] < -track_height_ || (std::uint32_t) iprev_point[1] >= grad_y.
height)
533 float a = prev_pt[0] - iprev_point[0];
534 float b = prev_pt[1] - iprev_point[1];
535 Eigen::Array4f weight;
536 weight[0] = (1.f - a)*(1.f - b);
537 weight[1] = a*(1.f - b);
538 weight[2] = (1.f - a)*b;
539 weight[3] = 1 - weight[0] - weight[1] - weight[2];
541 Eigen::Array3f covar = Eigen::Array3f::Zero ();
542 spatialGradient (prev, grad_x, grad_y, iprev_point, weight, prev_win, grad_x_win, grad_y_win, covar);
544 float det = covar[0]*covar[2] - covar[1]*covar[1];
545 float min_eigenvalue = (covar[2] + covar[0] - std::sqrt ((covar[0]-covar[2])*(covar[0]-covar[2]) + 4.f*covar[1]*covar[1]))/2.f;
547 if (min_eigenvalue < min_eigenvalue_threshold_ || det < std::numeric_limits<float>::epsilon ())
556 Eigen::Array2f prev_delta (0, 0);
557 for (
unsigned int j = 0; j < max_iterations_; j++)
559 Eigen::Array2i inext_pt = next_pt.floor ().cast<
int> ();
561 if (inext_pt[0] < -track_width_ || (std::uint32_t) inext_pt[0] >= next.
width ||
562 inext_pt[1] < -track_height_ || (std::uint32_t) inext_pt[1] >= next.
height)
569 a = next_pt[0] - inext_pt[0];
570 b = next_pt[1] - inext_pt[1];
571 weight[0] = (1.f - a)*(1.f - b);
572 weight[1] = a*(1.f - b);
573 weight[2] = (1.f - a)*b;
574 weight[3] = 1 - weight[0] - weight[1] - weight[2];
576 Eigen::Array2f beta = Eigen::Array2f::Zero ();
577 mismatchVector (prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
579 Eigen::Vector2f delta ((covar[1]*beta[1] - covar[2]*beta[0])*det, (covar[1]*beta[0] - covar[0]*beta[1])*det);
581 next_pt[0] += delta[0]; next_pt[1] += delta[1];
582 next_pts[ptidx] = next_pt + half_win;
584 if (delta.squaredNorm () <= epsilon_)
587 if (j > 0 && std::abs (delta[0] + prev_delta[0]) < 0.01 &&
588 std::abs (delta[1] + prev_delta[1]) < 0.01 )
590 next_pts[ptidx][0] -= delta[0]*0.5f;
591 next_pts[ptidx][1] -= delta[1]*0.5f;
599 if (level == 0 && !status[ptidx])
601 Eigen::Array2f next_point = next_pts[ptidx] - half_win;
602 Eigen::Array2i inext_point;
604 inext_point[0] = std::floor (next_point[0]);
605 inext_point[1] = std::floor (next_point[1]);
607 if (inext_point[0] < -track_width_ || (std::uint32_t) inext_point[0] >= next.
width ||
608 inext_point[1] < -track_height_ || (std::uint32_t) inext_point[1] >= next.
height)
615 n.
u = next_pts[ptidx][0];
616 n.
v = next_pts[ptidx][1];
619 inext_point[0] = std::floor (next_pts[ptidx][0]);
620 inext_point[1] = std::floor (next_pts[ptidx][1]);
621 iprev_point[0] = std::floor (prev_keypoints->
points[ptidx].u);
622 iprev_point[1] = std::floor (prev_keypoints->
points[ptidx].v);
623 const PointInT& prev_pt = prev_input->points[iprev_point[1]*prev_input->width + iprev_point[0]];
624 const PointInT& next_pt = input->points[inext_point[1]*input->width + inext_point[0]];
625 transformation_computer.
add (prev_pt.getVector3fMap (), next_pt.getVector3fMap (), 1.0);
634 template <
typename Po
intInT,
typename IntensityT>
void
640 std::vector<FloatImageConstPtr> pyramid;
643 keypoints->
reserve (keypoints_->size ());
644 std::vector<int> status (keypoints_->size (), 0);
645 track (ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_);
648 ref_pyramid_ = pyramid;
649 keypoints_ = keypoints;
650 keypoints_status_->indices = status;
typename PointCloudIn::ConstPtr PointCloudInConstPtr
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
shared_ptr< PointCloud< PointT > > Ptr
void convolveRows(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image rows.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void convolveCols(const FloatImageConstPtr &input, FloatImage &output) const
Convolve image columns.
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...
FloatImage::ConstPtr FloatImageConstPtr
void resize(std::size_t n)
Resize the cloud.
void downsample(const FloatImageConstPtr &input, FloatImageConstPtr &output) const
downsample input
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
bool initCompute() override
This method should get called before starting the actual computation.
FloatImage::Ptr FloatImagePtr
void convolve(const FloatImageConstPtr &input, FloatImage &output) const
Separately convolve image with decomposable convolution kernel.
std::uint32_t width
The point cloud width (if organized as an image-structure).
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
A 2D point structure representing pixel image coordinates.
void setTrackingWindowSize(int width, int height)
set the tracking window size
void setPointsToTrack(const pcl::PointIndicesConstPtr &points)
Provide a pointer to points to track.
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
std::uint32_t height
The point cloud height (if organized as an image-structure).
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void derivatives(const FloatImage &src, FloatImage &grad_x, FloatImage &grad_y) const
compute Scharr derivatives of a source cloud.
void computeTracking() override
Abstract tracking method.
void reserve(std::size_t n)
shared_ptr< const PointCloud< PointT > > ConstPtr
virtual void computePyramids(const PointCloudInConstPtr &input, std::vector< FloatImageConstPtr > &pyramid, pcl::InterpolationType border_type) const
Compute the pyramidal representation of an image.