template <typename PointInT, typename IntensityT> void pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::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& weight, Eigen::Array2f &b) const { const int step = next.width; b.setZero (); for (int y = 0; y < track_height_; y++) { const float* next_ptr = &(next.points[0]) + (y + location[1])*step + location[0]; const float* prev_ptr = prev.data () + y*prev.cols (); const float* prev_grad_x_ptr = prev_grad_x.data () + y*prev_grad_x.cols (); const float* prev_grad_y_ptr = prev_grad_y.data () + y*prev_grad_y.cols (); for (int x = 0; x < track_width_; ++x, ++prev_grad_y_ptr, ++prev_grad_x_ptr) { float diff = next_ptr[x]*weight[0] + next_ptr[x+1]*weight[1] + next_ptr[x+step]*weight[2] + next_ptr[x+step+1]*weight[3] - prev_ptr[x]; b[0] += *prev_grad_x_ptr * diff; b[1] += *prev_grad_y_ptr * diff; } } }
template <typename PointInT, typename IntensityT> void pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::spatialGradient (const FloatImage& img, const FloatImage& grad_x, const FloatImage& grad_y, const Eigen::Array2i& location, const Eigen::Array4f& weight, Eigen::ArrayXXf& win, Eigen::ArrayXXf& grad_x_win, Eigen::ArrayXXf& grad_y_win, Eigen::Array3f &covariance) const { const int step = img.width; covariance.setZero (); for (int y = 0; y < track_height_; y++) { const float* img_ptr = &(img.points[0]) + (y + location[1])*step + location[0]; const float* grad_x_ptr = &(grad_x.points[0]) + (y + location[1])*step + location[0]; const float* grad_y_ptr = &(grad_y.points[0]) + (y + location[1])*step + location[0]; float* win_ptr = win.data () + y*win.cols (); float* grad_x_win_ptr = grad_x_win.data () + y*grad_x_win.cols (); float* grad_y_win_ptr = grad_y_win.data () + y*grad_y_win.cols (); for (int x =0; x < track_width_; ++x, ++grad_x_ptr, ++grad_y_ptr) { *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]; 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]; 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]; //!!! store those *grad_x_win_ptr++ = ixval; *grad_y_win_ptr++ = iyval; //covariance components covariance[0] += ixval*ixval; covariance[1] += ixval*iyval; covariance[2] += iyval*iyval; } } }