Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
pyramidal_klt.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2014-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#ifndef PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
39#define PCL_TRACKING_IMPL_PYRAMIDAL_KLT_HPP
40
41#include <pcl/common/io.h>
42#include <pcl/common/time.h>
43#include <pcl/common/utils.h>
44
45namespace pcl {
46namespace tracking {
47///////////////////////////////////////////////////////////////////////////////////////////////
48template <typename PointInT, typename IntensityT>
49inline void
51{
52 track_width_ = width;
53 track_height_ = height;
54}
55
56///////////////////////////////////////////////////////////////////////////////////////////////
57template <typename PointInT, typename IntensityT>
58inline void
61{
62 if (keypoints->size() <= keypoints_nbr_)
63 keypoints_ = keypoints;
64 else {
66 p->reserve(keypoints_nbr_);
67 for (std::size_t i = 0; i < keypoints_nbr_; ++i)
68 p->push_back((*keypoints)[i]);
69 keypoints_ = p;
70 }
71
72 keypoints_status_.reset(new std::vector<int>);
73 keypoints_status_->resize(keypoints_->size(), 0);
74}
75
76///////////////////////////////////////////////////////////////////////////////////////////////
77template <typename PointInT, typename IntensityT>
78inline void
80 const pcl::PointIndicesConstPtr& points)
81{
82 assert((input_ || ref_) && "[PyramidalKLTTracker] CALL setInputCloud FIRST!");
83
85 keypoints->reserve(keypoints_nbr_);
86 for (std::size_t i = 0; i < keypoints_nbr_; ++i) {
87 pcl::PointUV uv;
88 uv.u = points->indices[i] % input_->width;
89 uv.v = points->indices[i] / input_->width;
90 keypoints->push_back(uv);
91 }
92 setPointsToTrack(keypoints);
93}
94
95///////////////////////////////////////////////////////////////////////////////////////////////
96template <typename PointInT, typename IntensityT>
97bool
99{
100 // std::cout << ">>> [PyramidalKLTTracker::initCompute]" << std::endl;
102 PCL_ERROR("[%s::initCompute] PCLBase::Init failed.\n", tracker_name_.c_str());
103 return (false);
104 }
105
106 if (!input_->isOrganized()) {
107 PCL_ERROR(
108 "[pcl::tracking::%s::initCompute] Need an organized point cloud to proceed!\n",
109 tracker_name_.c_str());
110 return (false);
111 }
112
113 if (!keypoints_ || keypoints_->empty()) {
114 PCL_ERROR("[pcl::tracking::%s::initCompute] No keypoints aborting!\n",
115 tracker_name_.c_str());
116 return (false);
117 }
118
119 // This is the first call
120 if (!ref_) {
121 ref_ = input_;
122 // std::cout << "First run!!!" << std::endl;
123
124 if ((track_height_ * track_width_) % 2 == 0) {
125 PCL_ERROR(
126 "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be odd!\n",
127 tracker_name_.c_str(),
128 track_width_,
129 track_height_);
130 return (false);
131 }
132
133 if (track_height_ < 3 || track_width_ < 3) {
134 PCL_ERROR(
135 "[pcl::tracking::%s::initCompute] Tracking window (%dx%d) must be >= 3x3!\n",
136 tracker_name_.c_str(),
137 track_width_,
138 track_height_);
139 return (false);
140 }
141
142 track_width_2_ = track_width_ / 2;
143 track_height_2_ = track_height_ / 2;
144
145 if (nb_levels_ < 2) {
146 PCL_ERROR("[pcl::tracking::%s::initCompute] Number of pyramid levels should be "
147 "at least 2!\n",
148 tracker_name_.c_str());
149 return (false);
150 }
151
152 if (nb_levels_ > 5) {
153 PCL_ERROR("[pcl::tracking::%s::initCompute] Number of pyramid levels should not "
154 "exceed 5!\n",
155 tracker_name_.c_str());
156 return (false);
157 }
158
159 computePyramids(ref_, ref_pyramid_, pcl::BORDER_REFLECT_101);
160 return (true);
161 }
162
163 initialized_ = true;
164
165 return (true);
166}
167
168///////////////////////////////////////////////////////////////////////////////////////////////
169template <typename PointInT, typename IntensityT>
170void
172 FloatImage& grad_x,
173 FloatImage& grad_y) const
174{
175 // std::cout << ">>> derivatives" << std::endl;
176 ////////////////////////////////////////////////////////
177 // Use Shcarr operator to compute derivatives. //
178 // Vertical kernel +3 +10 +3 = [1 0 -1]T * [3 10 3] //
179 // 0 0 0 //
180 // -3 -10 -3 //
181 // Horizontal kernel +3 0 -3 = [3 10 3]T * [1 0 -1] //
182 // +10 0 -10 //
183 // +3 0 -3 //
184 ////////////////////////////////////////////////////////
185 if (grad_x.size() != src.size() || grad_x.width != src.width ||
186 grad_x.height != src.height)
187 grad_x = FloatImage(src.width, src.height);
188 if (grad_y.size() != src.size() || grad_y.width != src.width ||
189 grad_y.height != src.height)
190 grad_y = FloatImage(src.width, src.height);
191
192 int height = src.height, width = src.width;
193 float* row0 = new float[src.width + 2];
194 float* row1 = new float[src.width + 2];
195 float* trow0 = row0;
196 ++trow0;
197 float* trow1 = row1;
198 ++trow1;
199 const float* src_ptr = &(src[0]);
200
201 for (int y = 0; y < height; y++) {
202 const float* srow0 = src_ptr + (y > 0 ? y - 1 : height > 1 ? 1 : 0) * width;
203 const float* srow1 = src_ptr + y * width;
204 const float* srow2 =
205 src_ptr + (y < height - 1 ? y + 1 : (height > 1 ? height - 2 : 0)) * width;
206 float* grad_x_row = &(grad_x[y * width]);
207 float* grad_y_row = &(grad_y[y * width]);
208
209 // do vertical convolution
210 for (int x = 0; x < width; x++) {
211 trow0[x] = (srow0[x] + srow2[x]) * 3 + srow1[x] * 10;
212 trow1[x] = srow2[x] - srow0[x];
213 }
214
215 // make border
216 int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width - 2 : 0;
217 trow0[-1] = trow0[x0];
218 trow0[width] = trow0[x1];
219 trow1[-1] = trow1[x0];
220 trow1[width] = trow1[x1];
221
222 // do horizontal convolution and store results
223 for (int x = 0; x < width; x++) {
224 grad_x_row[x] = trow0[x + 1] - trow0[x - 1];
225 grad_y_row[x] = (trow1[x + 1] + trow1[x - 1]) * 3 + trow1[x] * 10;
226 }
227 }
228 delete[] row0;
229 delete[] row1;
230}
231
232///////////////////////////////////////////////////////////////////////////////////////////////
233template <typename PointInT, typename IntensityT>
234void
236 FloatImageConstPtr& output) const
237{
238 FloatImage smoothed(input->width, input->height);
239 convolve(input, smoothed);
240
241 int width = (smoothed.width + 1) / 2;
242 int height = (smoothed.height + 1) / 2;
243 std::vector<int> ii(width);
244 for (int i = 0; i < width; ++i)
245 ii[i] = 2 * i;
246
247 FloatImagePtr down(new FloatImage(width, height));
248 // clang-format off
249#pragma omp parallel for \
250 default(none) \
251 shared(down, height, output, smoothed, width) \
252 firstprivate(ii) \
253 num_threads(threads_)
254 // clang-format on
255 for (int j = 0; j < height; ++j) {
256 int jj = 2 * j;
257 for (int i = 0; i < width; ++i)
258 (*down)(i, j) = smoothed(ii[i], jj);
259 }
260
261 output = down;
262}
263
264///////////////////////////////////////////////////////////////////////////////////////////////
265template <typename PointInT, typename IntensityT>
266void
268 const FloatImageConstPtr& input,
269 FloatImageConstPtr& output,
270 FloatImageConstPtr& output_grad_x,
271 FloatImageConstPtr& output_grad_y) const
272{
273 downsample(input, output);
274 FloatImagePtr grad_x(new FloatImage(input->width, input->height));
275 FloatImagePtr grad_y(new FloatImage(input->width, input->height));
276 derivatives(*output, *grad_x, *grad_y);
277 output_grad_x = grad_x;
278 output_grad_y = grad_y;
279}
280
281///////////////////////////////////////////////////////////////////////////////////////////////
282template <typename PointInT, typename IntensityT>
283void
285 const FloatImageConstPtr& input, FloatImage& output) const
286{
287 FloatImagePtr tmp(new FloatImage(input->width, input->height));
288 convolveRows(input, *tmp);
289 convolveCols(tmp, output);
290}
291
292///////////////////////////////////////////////////////////////////////////////////////////////
293template <typename PointInT, typename IntensityT>
294void
296 const FloatImageConstPtr& input, FloatImage& output) const
297{
298 int width = input->width;
299 int height = input->height;
300 int last = input->width - kernel_size_2_;
301 int w = last - 1;
302
303 // clang-format off
304#pragma omp parallel for \
305 default(none) \
306 shared(input, height, last, output, w, width) \
307 num_threads(threads_)
308 // clang-format on
309 for (int j = 0; j < height; ++j) {
310 for (int i = kernel_size_2_; i < last; ++i) {
311 double result = 0;
312 for (int k = kernel_last_, l = i - kernel_size_2_; k > -1; --k, ++l)
313 result += kernel_[k] * (*input)(l, j);
314
315 output(i, j) = static_cast<float>(result);
316 }
317
318 for (int i = last; i < width; ++i)
319 output(i, j) = output(w, j);
320
321 for (int i = 0; i < kernel_size_2_; ++i)
322 output(i, j) = output(kernel_size_2_, j);
323 }
324}
325
326///////////////////////////////////////////////////////////////////////////////////////////////
327template <typename PointInT, typename IntensityT>
328void
330 FloatImage& output) const
331{
332 output = FloatImage(input->width, input->height);
333
334 int width = input->width;
335 int height = input->height;
336 int last = input->height - kernel_size_2_;
337 int h = last - 1;
338
339 // clang-format off
340#pragma omp parallel for \
341 default(none) \
342 shared(input, h, height, last, output, width) \
343 num_threads(threads_)
344 // clang-format on
345 for (int i = 0; i < width; ++i) {
346 for (int j = kernel_size_2_; j < last; ++j) {
347 double result = 0;
348 for (int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l)
349 result += kernel_[k] * (*input)(i, l);
350 output(i, j) = static_cast<float>(result);
351 }
352
353 for (int j = last; j < height; ++j)
354 output(i, j) = output(i, h);
355
356 for (int j = 0; j < kernel_size_2_; ++j)
357 output(i, j) = output(i, kernel_size_2_);
358 }
359}
360
361///////////////////////////////////////////////////////////////////////////////////////////////
362template <typename PointInT, typename IntensityT>
363void
365 const PointCloudInConstPtr& input,
366 std::vector<FloatImageConstPtr>& pyramid,
367 pcl::InterpolationType border_type) const
368{
369 int step = 3;
370 pyramid.resize(step * nb_levels_);
371
372 FloatImageConstPtr previous;
373 FloatImagePtr tmp(new FloatImage(input->width, input->height));
374 // clang-format off
375#pragma omp parallel for \
376 default(none) \
377 shared(input, tmp) \
378 num_threads(threads_)
379 // clang-format on
380 for (int i = 0; i < static_cast<int>(input->size()); ++i)
381 (*tmp)[i] = intensity_((*input)[i]);
382 previous = tmp;
383
384 FloatImagePtr img(new FloatImage(previous->width + 2 * track_width_,
385 previous->height + 2 * track_height_));
386
388 *img,
389 track_height_,
390 track_height_,
391 track_width_,
392 track_width_,
393 border_type,
394 0.f);
395 pyramid[0] = img;
396
397 // compute first level gradients
398 FloatImagePtr g_x(new FloatImage(input->width, input->height));
399 FloatImagePtr g_y(new FloatImage(input->width, input->height));
400 derivatives(*img, *g_x, *g_y);
401 // copy to bigger clouds
402 FloatImagePtr grad_x(new FloatImage(previous->width + 2 * track_width_,
403 previous->height + 2 * track_height_));
405 *grad_x,
406 track_height_,
407 track_height_,
408 track_width_,
409 track_width_,
411 0.f);
412 pyramid[1] = grad_x;
413
414 FloatImagePtr grad_y(new FloatImage(previous->width + 2 * track_width_,
415 previous->height + 2 * track_height_));
417 *grad_y,
418 track_height_,
419 track_height_,
420 track_width_,
421 track_width_,
423 0.f);
424 pyramid[2] = grad_y;
425
426 for (int level = 1; level < nb_levels_; ++level) {
427 // compute current level and current level gradients
428 FloatImageConstPtr current;
431 downsample(previous, current, g_x, g_y);
432 // copy to bigger clouds
433 FloatImagePtr image(new FloatImage(current->width + 2 * track_width_,
434 current->height + 2 * track_height_));
435 pcl::copyPointCloud(*current,
436 *image,
437 track_height_,
438 track_height_,
439 track_width_,
440 track_width_,
441 border_type,
442 0.f);
443 pyramid[level * step] = image;
444 FloatImagePtr gradx(
445 new FloatImage(g_x->width + 2 * track_width_, g_x->height + 2 * track_height_));
447 *gradx,
448 track_height_,
449 track_height_,
450 track_width_,
451 track_width_,
453 0.f);
454 pyramid[level * step + 1] = gradx;
455 FloatImagePtr grady(
456 new FloatImage(g_y->width + 2 * track_width_, g_y->height + 2 * track_height_));
458 *grady,
459 track_height_,
460 track_height_,
461 track_width_,
462 track_width_,
464 0.f);
465 pyramid[level * step + 2] = grady;
466 // set the new level
467 previous = current;
468 }
469}
470
471///////////////////////////////////////////////////////////////////////////////////////////////
472template <typename PointInT, typename IntensityT>
473void
475 const FloatImage& img,
476 const FloatImage& grad_x,
477 const FloatImage& grad_y,
478 const Eigen::Array2i& location,
479 const Eigen::Array4f& weight,
480 Eigen::ArrayXXf& win,
481 Eigen::ArrayXXf& grad_x_win,
482 Eigen::ArrayXXf& grad_y_win,
483 Eigen::Array3f& covariance) const
484{
485 const int step = img.width;
486 covariance.setZero();
487
488 for (int y = 0; y < track_height_; y++) {
489 const float* img_ptr = &(img[0]) + (y + location[1]) * step + location[0];
490 const float* grad_x_ptr = &(grad_x[0]) + (y + location[1]) * step + location[0];
491 const float* grad_y_ptr = &(grad_y[0]) + (y + location[1]) * step + location[0];
492
493 float* win_ptr = win.data() + y * win.cols();
494 float* grad_x_win_ptr = grad_x_win.data() + y * grad_x_win.cols();
495 float* grad_y_win_ptr = grad_y_win.data() + y * grad_y_win.cols();
496
497 for (int x = 0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr) {
498 *win_ptr++ = img_ptr[x] * weight[0] + img_ptr[x + 1] * weight[1] +
499 img_ptr[x + step] * weight[2] + img_ptr[x + step + 1] * weight[3];
500 float ixval = grad_x_ptr[0] * weight[0] + grad_x_ptr[1] * weight[1] +
501 grad_x_ptr[step] * weight[2] + grad_x_ptr[step + 1] * weight[3];
502 float iyval = grad_y_ptr[0] * weight[0] + grad_y_ptr[1] * weight[1] +
503 grad_y_ptr[step] * weight[2] + grad_y_ptr[step + 1] * weight[3];
504 //!!! store those
505 *grad_x_win_ptr++ = ixval;
506 *grad_y_win_ptr++ = iyval;
507 // covariance components
508 covariance[0] += ixval * ixval;
509 covariance[1] += ixval * iyval;
510 covariance[2] += iyval * iyval;
511 }
512 }
513}
514
515///////////////////////////////////////////////////////////////////////////////////////////////
516template <typename PointInT, typename IntensityT>
517void
519 const Eigen::ArrayXXf& prev,
520 const Eigen::ArrayXXf& prev_grad_x,
521 const Eigen::ArrayXXf& prev_grad_y,
522 const FloatImage& next,
523 const Eigen::Array2i& location,
524 const Eigen::Array4f& weight,
525 Eigen::Array2f& b) const
526{
527 const int step = next.width;
528 b.setZero();
529 for (int y = 0; y < track_height_; y++) {
530 const float* next_ptr = &(next[0]) + (y + location[1]) * step + location[0];
531 const float* prev_ptr = prev.data() + y * prev.cols();
532 const float* prev_grad_x_ptr = prev_grad_x.data() + y * prev_grad_x.cols();
533 const float* prev_grad_y_ptr = prev_grad_y.data() + y * prev_grad_y.cols();
534
535 for (int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr) {
536 float diff = next_ptr[x] * weight[0] + next_ptr[x + 1] * weight[1] +
537 next_ptr[x + step] * weight[2] + next_ptr[x + step + 1] * weight[3] -
538 prev_ptr[x];
539 b[0] += *prev_grad_x_ptr * diff;
540 b[1] += *prev_grad_y_ptr * diff;
541 }
542 }
543}
544
545///////////////////////////////////////////////////////////////////////////////////////////////
546template <typename PointInT, typename IntensityT>
547void
549 const PointCloudInConstPtr& prev_input,
550 const PointCloudInConstPtr& input,
551 const std::vector<FloatImageConstPtr>& prev_pyramid,
552 const std::vector<FloatImageConstPtr>& pyramid,
553 const pcl::PointCloud<pcl::PointUV>::ConstPtr& prev_keypoints,
555 std::vector<int>& status,
556 Eigen::Affine3f& motion) const
557{
558 std::vector<Eigen::Array2f, Eigen::aligned_allocator<Eigen::Array2f>> next_pts(
559 prev_keypoints->size());
560 Eigen::Array2f half_win((track_width_ - 1) * 0.5f, (track_height_ - 1) * 0.5f);
561 pcl::TransformationFromCorrespondences transformation_computer;
562 const int nb_points = prev_keypoints->size();
563 for (int level = nb_levels_ - 1; level >= 0; --level) {
564 const FloatImage& prev = *(prev_pyramid[level * 3]);
565 const FloatImage& next = *(pyramid[level * 3]);
566 const FloatImage& grad_x = *(prev_pyramid[level * 3 + 1]);
567 const FloatImage& grad_y = *(prev_pyramid[level * 3 + 2]);
568
569 Eigen::ArrayXXf prev_win(track_height_, track_width_);
570 Eigen::ArrayXXf grad_x_win(track_height_, track_width_);
571 Eigen::ArrayXXf grad_y_win(track_height_, track_width_);
572 float ratio(1. / (1 << level));
573 for (int ptidx = 0; ptidx < nb_points; ptidx++) {
574 Eigen::Array2f prev_pt((*prev_keypoints)[ptidx].u * ratio,
575 (*prev_keypoints)[ptidx].v * ratio);
576 Eigen::Array2f next_pt;
577 if (level == nb_levels_ - 1)
578 next_pt = prev_pt;
579 else
580 next_pt = next_pts[ptidx] * 2.f;
581
582 next_pts[ptidx] = next_pt;
583
584 Eigen::Array2i iprev_point;
585 prev_pt -= half_win;
586 iprev_point[0] = std::floor(prev_pt[0]);
587 iprev_point[1] = std::floor(prev_pt[1]);
588
589 if (iprev_point[0] < -track_width_ ||
590 (std::uint32_t)iprev_point[0] >= grad_x.width ||
591 iprev_point[1] < -track_height_ ||
592 (std::uint32_t)iprev_point[1] >= grad_y.height) {
593 if (level == 0)
594 status[ptidx] = -1;
595 continue;
596 }
597
598 float a = prev_pt[0] - iprev_point[0];
599 float b = prev_pt[1] - iprev_point[1];
600 Eigen::Array4f weight;
601 weight[0] = (1.f - a) * (1.f - b);
602 weight[1] = a * (1.f - b);
603 weight[2] = (1.f - a) * b;
604 weight[3] = 1 - weight[0] - weight[1] - weight[2];
605
606 Eigen::Array3f covar = Eigen::Array3f::Zero();
607 spatialGradient(prev,
608 grad_x,
609 grad_y,
610 iprev_point,
611 weight,
612 prev_win,
613 grad_x_win,
614 grad_y_win,
615 covar);
616
617 float det = covar[0] * covar[2] - covar[1] * covar[1];
618 float min_eigenvalue = (covar[2] + covar[0] -
619 std::sqrt((covar[0] - covar[2]) * (covar[0] - covar[2]) +
620 4.f * covar[1] * covar[1])) /
621 2.f;
622
623 if (min_eigenvalue < min_eigenvalue_threshold_ ||
624 det < std::numeric_limits<float>::epsilon()) {
625 status[ptidx] = -2;
626 continue;
627 }
628
629 det = 1.f / det;
630 next_pt -= half_win;
631
632 Eigen::Array2f prev_delta(0, 0);
633 for (unsigned int j = 0; j < max_iterations_; j++) {
634 Eigen::Array2i inext_pt = next_pt.floor().cast<int>();
635
636 if (inext_pt[0] < -track_width_ || (std::uint32_t)inext_pt[0] >= next.width ||
637 inext_pt[1] < -track_height_ || (std::uint32_t)inext_pt[1] >= next.height) {
638 if (level == 0)
639 status[ptidx] = -1;
640 break;
641 }
642
643 a = next_pt[0] - inext_pt[0];
644 b = next_pt[1] - inext_pt[1];
645 weight[0] = (1.f - a) * (1.f - b);
646 weight[1] = a * (1.f - b);
647 weight[2] = (1.f - a) * b;
648 weight[3] = 1 - weight[0] - weight[1] - weight[2];
649 // compute mismatch vector
650 Eigen::Array2f beta = Eigen::Array2f::Zero();
651 mismatchVector(prev_win, grad_x_win, grad_y_win, next, inext_pt, weight, beta);
652 // optical flow resolution
653 Eigen::Vector2f delta((covar[1] * beta[1] - covar[2] * beta[0]) * det,
654 (covar[1] * beta[0] - covar[0] * beta[1]) * det);
655 // update position
656 next_pt[0] += delta[0];
657 next_pt[1] += delta[1];
658 next_pts[ptidx] = next_pt + half_win;
659
660 if (delta.squaredNorm() <= epsilon_)
661 break;
662
663 if (j > 0 && std::abs(delta[0] + prev_delta[0]) < 0.01 &&
664 std::abs(delta[1] + prev_delta[1]) < 0.01) {
665 next_pts[ptidx][0] -= delta[0] * 0.5f;
666 next_pts[ptidx][1] -= delta[1] * 0.5f;
667 break;
668 }
669 // update delta
670 prev_delta = delta;
671 }
672
673 // update tracked points
674 if (level == 0 && !status[ptidx]) {
675 Eigen::Array2f next_point = next_pts[ptidx] - half_win;
676 Eigen::Array2i inext_point;
677
678 inext_point[0] = std::floor(next_point[0]);
679 inext_point[1] = std::floor(next_point[1]);
680
681 if (inext_point[0] < -track_width_ ||
682 (std::uint32_t)inext_point[0] >= next.width ||
683 inext_point[1] < -track_height_ ||
684 (std::uint32_t)inext_point[1] >= next.height) {
685 status[ptidx] = -1;
686 continue;
687 }
688 // insert valid keypoint
689 pcl::PointUV n;
690 n.u = next_pts[ptidx][0];
691 n.v = next_pts[ptidx][1];
692 keypoints->push_back(n);
693 // add points pair to compute transformation
694 inext_point[0] = std::floor(next_pts[ptidx][0]);
695 inext_point[1] = std::floor(next_pts[ptidx][1]);
696 iprev_point[0] = std::floor((*prev_keypoints)[ptidx].u);
697 iprev_point[1] = std::floor((*prev_keypoints)[ptidx].v);
698 const PointInT& prev_pt =
699 (*prev_input)[iprev_point[1] * prev_input->width + iprev_point[0]];
700 const PointInT& next_pt =
701 (*input)[inext_point[1] * input->width + inext_point[0]];
702 transformation_computer.add(
703 prev_pt.getVector3fMap(), next_pt.getVector3fMap(), 1.0);
704 }
705 }
706 }
707 motion = transformation_computer.getTransformation();
708}
709
710///////////////////////////////////////////////////////////////////////////////////////////////
711template <typename PointInT, typename IntensityT>
712void
714{
715 if (!initialized_)
716 return;
717
718 std::vector<FloatImageConstPtr> pyramid;
719 computePyramids(input_, pyramid, pcl::BORDER_REFLECT_101);
721 keypoints->reserve(keypoints_->size());
722 std::vector<int> status(keypoints_->size(), 0);
723 track(ref_, input_, ref_pyramid_, pyramid, keypoints_, keypoints, status, motion_);
724 // swap reference and input
725 ref_ = input_;
726 ref_pyramid_ = pyramid;
727 keypoints_ = keypoints;
728 *keypoints_status_ = status;
729}
730
731} // namespace tracking
732} // namespace pcl
733
734#endif
PCL base class.
Definition pcl_base.h:70
PointCloud represents the base class in PCL for storing collections of 3D points.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void resize(std::size_t count)
Resizes the container to contain count elements.
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).
std::size_t size() const
void reserve(std::size_t n)
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Calculates a transformation based on corresponding 3D points.
Eigen::Affine3f getTransformation()
Calculate the transformation that will best transform the points into their correspondences.
void add(const Eigen::Vector3f &point, const Eigen::Vector3f &corresponding_point, float weight=1.0)
Add a new sample.
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 &current_input, const std::vector< FloatImageConstPtr > &previous_pyramid, const std::vector< FloatImageConstPtr > &current_pyramid, const pcl::PointCloud< pcl::PointUV >::ConstPtr &previous_keypoints, pcl::PointCloud< pcl::PointUV >::Ptr &current_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 allocation whil...
FloatImage::ConstPtr FloatImageConstPtr
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.
Definition io.hpp:142
InterpolationType
Definition io.h:255
@ BORDER_REFLECT_101
Definition io.h:258
@ BORDER_CONSTANT
Definition io.h:256
PointIndices::ConstPtr PointIndicesConstPtr
A 2D point structure representing pixel image coordinates.