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;
}
