template <typename PointInT, typename IntensityT> void pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::derivatives (const FloatImage& src, FloatImage& grad_x, FloatImage& grad_y) const { // std::cout << ">>> derivatives" << std::endl; //////////////////////////////////////////////////////// // Use Shcarr operator to compute derivatives. // // Vertical kernel +3 +10 +3 = [1 0 -1]T * [3 10 3] // // 0 0 0 // // -3 -10 -3 // // Horizontal kernel +3 0 -3 = [3 10 3]T * [1 0 -1] // // +10 0 -10 // // +3 0 -3 // //////////////////////////////////////////////////////// if (grad_x.size () != src.size () || grad_x.width != src.width || grad_x.height != src.height) grad_x = FloatImage (src.width, src.height); if (grad_y.size () != src.size () || grad_y.width != src.width || grad_y.height != src.height) grad_y = FloatImage (src.width, src.height); int height = src.height, width = src.width; float *row0 = new float [src.width + 2]; float *row1 = new float [src.width + 2]; float *trow0 = row0; ++trow0; float *trow1 = row1; ++trow1; const float* src_ptr = &(src.points[0]); for (int y = 0; y < height; y++) { const float* srow0 = src_ptr + (y > 0 ? y-1 : height > 1 ? 1 : 0) * width; const float* srow1 = src_ptr + y * width; const float* srow2 = src_ptr + (y < height-1 ? y+1 : height > 1 ? height-2 : 0) * width; float* grad_x_row = &(grad_x.points[y * width]); float* grad_y_row = &(grad_y.points[y * width]); // do vertical convolution for (int x = 0; x < width; x++) { trow0[x] = (srow0[x] + srow2[x])*3 + srow1[x]*10; trow1[x] = srow2[x] - srow0[x]; } // make border int x0 = width > 1 ? 1 : 0, x1 = width > 1 ? width-2 : 0; trow0[-1] = trow0[x0]; trow0[width] = trow0[x1]; trow1[-1] = trow1[x0]; trow1[width] = trow1[x1]; // do horizontal convolution and store results for (int x = 0; x < width; x++) { grad_x_row[x] = trow0[x+1] - trow0[x-1]; grad_y_row[x] = (trow1[x+1] + trow1[x-1])*3 + trow1[x]*10; } } }
static FloatImage CreateCPU(const Dim &dim) { FloatImageImpl *impl = new FloatImageImpl; impl->initCPU(dim); return FloatImage(impl); }
template <typename PointInT, typename IntensityT> void pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::convolveCols (const FloatImageConstPtr& input, FloatImage& output) const { output = FloatImage (input->width, input->height); int width = input->width; int height = input->height; int last = input->height - kernel_size_2_; int h = last -1; #ifdef _OPENMP #pragma omp parallel for shared (output) num_threads (threads_) #endif for (int i = 0; i < width; ++i) { for (int j = kernel_size_2_; j < last; ++j) { double result = 0; for (int k = kernel_last_, l = j - kernel_size_2_; k > -1; --k, ++l) result += kernel_[k] * (*input) (i,l); output (i,j) = static_cast<float> (result); } for (int j = last; j < height; ++j) output (i,j) = output (i,h); for (int j = 0; j < kernel_size_2_; ++j) output (i,j) = output (i, kernel_size_2_); } }
NiblackBinaryImage::NiblackBinaryImage(const GreyLevelImage& anImg, const int aLowThres, const int aHighThres, const int aMaskSize, const float aK, const float aPostThres) : BinaryImage(anImg.width(), anImg.height()) { // Pointer to mean image FloatImage* pMeanImg = 0; // Compute standard deviation and mean StandardDeviationImage stdImg(FloatImage(anImg), pMeanImg, aMaskSize); // Rows to exchange data GreyLevelImage::pointer bRow = new GreyLevelImage::value_type [_width]; GreyLevelImage::pointer gRow = new GreyLevelImage::value_type [_width]; float* mRow = new float [_width]; float* sRow = new float [_width]; // Binarize for (int i = 0 ; i < _height ; ++i) { // Read means pMeanImg->row(i, mRow); // Read deviations stdImg.row(i, sRow); // Read original anImg.row(i, gRow); // Pointers float* m = mRow; float* s = sRow; GreyLevelImage::pointer g = gRow; GreyLevelImage::pointer b = bRow; // Dynamic thresholding for (int j = 0 ; j < _width ; ++j, ++b, ++g) { if (*g < aLowThres) { *b = 1; } else if (*g > aHighThres) { *b = 0; } else if (*g < (GreyLevelImage::value_type) (*m++ + aK * *s++)) { *b = 1; } else { *b = 0; } } // Save line setRow(i, bRow); } // Post-processing if (aPostThres > 0) { // Copy reference image BinaryImage* contours = new BinaryImage(*this); // Extract connected components ConnectedComponents* compConnexes = new ConnectedComponents(*contours); // Prepare tables int labCnt = compConnexes->componentCnt(); // Tables for the sums of the means of the gradient float* gsum = new float [labCnt]; qgFill(gsum, labCnt, 0.f); // Tables for the numbers of points -- for the mean int* psum = new int [labCnt]; qgFill(psum, labCnt, 0); // Table of labels Component::label_type* labRow = new Component::label_type[_width]; // By construction, first and last pixels are always WHITE labRow[1] = 0; labRow[_width - 1] = 0; // Compute the module of Canny gradient CannyGradientImage* gradImg = new CannyGradientImage(anImg); GradientModuleImage gradModImg(*gradImg); delete gradImg; // Construct the image of the contours of the black components // which thus includes the interesting pixels ErodedBinaryImage* eroImg = new ErodedBinaryImage(*contours); (*contours) -= (*eroImg); delete eroImg; // Create a line of floats float* fRow = new float [_width]; // Pointer to the pixel map of the component image Component::label_type* pMapCCImg = (compConnexes->accessComponentImage()).pPixMap() + _width; for (int i = 1 ; i < (_height - 1) ; ++i) { // Get a line of labels from the component image // and set pixels from white components to 0 pMapCCImg += 2; PRIVATEgetBlackLabels(pMapCCImg, labRow); // Read the corresponding line in the contours contours->row(i, bRow); // Read the corresponding line in the module of the gradient gradModImg.row(i, fRow); Component::label_type* p = labRow; GreyLevelImage::pointer q = bRow; float* r = fRow; for (int j = 0 ; j < _width ; ++j, ++p, ++q, ++r) { if (*q != 0) // We are on a contour { gsum[(int)(*p)] += *r; psum[(int)(*p)] += 1; } } // END for j } // END for i delete contours; // Compute the means for (int i = 0 ; i < labCnt ; ++i) { if (psum[i] != 0) { gsum[i] /= psum[i]; } } // END for // Pointer to the pixel map of the component image pMapCCImg = (compConnexes->accessComponentImage()).pPixMap() + _width; // Delete fake black components for (int i = 1 ; i < _height - 1 ; ++i) { // Read the current line of components pMapCCImg += 2; PRIVATEgetBlackLabels(pMapCCImg, labRow); // Read the corresponding line in the binary image row(i, bRow); // Examine components and delete Component::label_type* p = labRow; GreyLevelImage::pointer pb = bRow; for (int j = 0 ; j < _width ; ++j, ++p, ++pb) { if (((*pb) != 0) && (gsum[(int)(*p)] < aPostThres)) { *pb = 0; } } // Save this line setRow(i, bRow); } // END for // Clean up delete [] fRow; delete [] psum; delete [] gsum; delete compConnexes; } // And clean up delete [] bRow; delete [] gRow; delete [] mRow; delete [] sRow; }