{

switch (quantized)

{

case 1: return 0;

case 2: return 1;

case 4: return 2;

case 8: return 3;

case 16: return 4;

case 32: return 5;

case 64: return 6;

case 128: return 7;

default:

CV_Error(CV_StsBadArg, "Invalid value of quantized parameter");

return -1; //avoid warning

}

{

FileNodeIterator fni = fn.begin();

fni >> x >> y >> label;

{

int min_x = std::numeric_limits<int>::max();

int min_y = std::numeric_limits<int>::max();

int max_x = std::numeric_limits<int>::min();

int max_y = std::numeric_limits<int>::min();

// First pass: find min/max feature x,y over all pyramid levels and modalities

for (int i = 0; i < (int)templates.size(); ++i)

{

Template& templ = templates[i];

for (int j = 0; j < (int)templ.features.size(); ++j)

{

int x = templ.features[j].x << templ.pyramid_level;

int y = templ.features[j].y << templ.pyramid_level;

min_x = std::min(min_x, x);

min_y = std::min(min_y, y);

max_x = std::max(max_x, x);

max_y = std::max(max_y, y);

}

}

/// @todo Why require even min_x, min_y?

if (min_x % 2 == 1) --min_x;

if (min_y % 2 == 1) --min_y;

// Second pass: set width/height and shift all feature positions

for (int i = 0; i < (int)templates.size(); ++i)

{

Template& templ = templates[i];

templ.width = (max_x - min_x) >> templ.pyramid_level;

templ.height = (max_y - min_y) >> templ.pyramid_level;

int offset_x = min_x >> templ.pyramid_level;

int offset_y = min_y >> templ.pyramid_level;

for (int j = 0; j < (int)templ.features.size(); ++j)

{

templ.features[j].x -= offset_x;

templ.features[j].y -= offset_y;

}

}

return Rect(min_x, min_y, max_x - min_x, max_y - min_y);

{

width = fn["width"];

height = fn["height"];

pyramid_level = fn["pyramid_level"];

FileNode features_fn = fn["features"];

features.resize(features_fn.size());

FileNodeIterator it = features_fn.begin(), it_end = features_fn.end();

for (int i = 0; it != it_end; ++it, ++i)

{

features[i].read(*it);

}

{

fs << "width" << width;

fs << "height" << height;

fs << "pyramid_level" << pyramid_level;

fs << "features" << "[";

for (int i = 0; i < (int)features.size(); ++i)

{

features[i].write(fs);

}

fs << "]"; // features

std::vector<Feature>& features,

size_t num_features, float distance)

{

features.clear();

float distance_sq = CV_SQR(distance);

int i = 0;

while (features.size() < num_features)

{

Candidate c = candidates[i];

// Add if sufficient distance away from any previously chosen feature

bool keep = true;

for (int j = 0; (j < (int)features.size()) && keep; ++j)

{

Feature f = features[j];

keep = CV_SQR(c.f.x - f.x) + CV_SQR(c.f.y - f.y) >= distance_sq;

}

if (keep)

features.push_back(c.f);

if (++i == (int)candidates.size())

{

// Start back at beginning, and relax required distance

i = 0;

distance -= 1.0f;

distance_sq = CV_SQR(distance);

}

}

{

if (modality_type == "ColorGradient")

return new ColorGradient();

else if (modality_type == "DepthNormal")

return new DepthNormal();

else

return NULL;

{

std::string type = fn["type"];

Ptr<Modality> modality = create(type);

modality->read(fn);

return modality;

{

std::vector<Vec3b> lut(8);

lut[0] = Vec3b( 0, 0, 255);

lut[1] = Vec3b( 0, 170, 255);

lut[2] = Vec3b( 0, 255, 170);

lut[3] = Vec3b( 0, 255, 0);

lut[4] = Vec3b(170, 255, 0);

lut[5] = Vec3b(255, 170, 0);

lut[6] = Vec3b(255, 0, 0);

lut[7] = Vec3b(255, 0, 170);

dst = Mat::zeros(quantized.size(), CV_8UC3);

for (int r = 0; r < dst.rows; ++r)

{

const uchar* quant_r = quantized.ptr(r);

Vec3b* dst_r = dst.ptr<Vec3b>(r);

for (int c = 0; c < dst.cols; ++c)

{

uchar q = quant_r[c];

if (q)

dst_r[c] = lut[getLabel(q)];

}

}

Mat& angle, float threshold)

{

magnitude.create(src.size(), CV_32F);

// Allocate temporary buffers

Size size = src.size();

Mat sobel_3dx; // per-channel horizontal derivative

Mat sobel_3dy; // per-channel vertical derivative

Mat sobel_dx(size, CV_32F); // maximum horizontal derivative

Mat sobel_dy(size, CV_32F); // maximum vertical derivative

Mat sobel_ag; // final gradient orientation (unquantized)

Mat smoothed;

// Compute horizontal and vertical image derivatives on all color channels separately

static const int KERNEL_SIZE = 7;

// For some reason cvSmooth/cv::GaussianBlur, cvSobel/cv::Sobel have different defaults for border handling...

GaussianBlur(src, smoothed, Size(KERNEL_SIZE, KERNEL_SIZE), 0, 0, BORDER_REPLICATE);

Sobel(smoothed, sobel_3dx, CV_16S, 1, 0, 3, 1.0, 0.0, BORDER_REPLICATE);

Sobel(smoothed, sobel_3dy, CV_16S, 0, 1, 3, 1.0, 0.0, BORDER_REPLICATE);

short * ptrx = (short *)sobel_3dx.data;

short * ptry = (short *)sobel_3dy.data;

float * ptr0x = (float *)sobel_dx.data;

float * ptr0y = (float *)sobel_dy.data;

float * ptrmg = (float *)magnitude.data;

const int length1 = static_cast<const int>(sobel_3dx.step1());

const int length2 = static_cast<const int>(sobel_3dy.step1());

const int length3 = static_cast<const int>(sobel_dx.step1());

const int length4 = static_cast<const int>(sobel_dy.step1());

const int length5 = static_cast<const int>(magnitude.step1());

const int length0 = sobel_3dy.cols * 3;

for (int r = 0; r < sobel_3dy.rows; ++r)

{

int ind = 0;

for (int i = 0; i < length0; i += 3)

{

// Use the gradient orientation of the channel whose magnitude is largest

int mag1 = CV_SQR(ptrx[i]) + CV_SQR(ptry[i]);

int mag2 = CV_SQR(ptrx[i + 1]) + CV_SQR(ptry[i + 1]);

int mag3 = CV_SQR(ptrx[i + 2]) + CV_SQR(ptry[i + 2]);

if (mag1 >= mag2 && mag1 >= mag3)

{

ptr0x[ind] = ptrx[i];

ptr0y[ind] = ptry[i];

ptrmg[ind] = (float)mag1;

}

else if (mag2 >= mag1 && mag2 >= mag3)

{

ptr0x[ind] = ptrx[i + 1];

ptr0y[ind] = ptry[i + 1];

ptrmg[ind] = (float)mag2;

}

else

{

ptr0x[ind] = ptrx[i + 2];

ptr0y[ind] = ptry[i + 2];

ptrmg[ind] = (float)mag3;

}

++ind;

}

ptrx += length1;

ptry += length2;

ptr0x += length3;

ptr0y += length4;

ptrmg += length5;

}

// Calculate the final gradient orientations

phase(sobel_dx, sobel_dy, sobel_ag, true);

hysteresisGradient(magnitude, angle, sobel_ag, CV_SQR(threshold));

Mat& angle, float threshold)

{

// Quantize 360 degree range of orientations into 16 buckets

// Note that [0, 11.25), [348.75, 360) both get mapped in the end to label 0,

// for stability of horizontal and vertical features.

Mat_<unsigned char> quantized_unfiltered;

angle.convertTo(quantized_unfiltered, CV_8U, 16.0 / 360.0);

// Zero out top and bottom rows

/// @todo is this necessary, or even correct?

memset(quantized_unfiltered.ptr(), 0, quantized_unfiltered.cols);

memset(quantized_unfiltered.ptr(quantized_unfiltered.rows - 1), 0, quantized_unfiltered.cols);

// Zero out first and last columns

for (int r = 0; r < quantized_unfiltered.rows; ++r)

{

quantized_unfiltered(r, 0) = 0;

quantized_unfiltered(r, quantized_unfiltered.cols - 1) = 0;

}

// Mask 16 buckets into 8 quantized orientations

for (int r = 1; r < angle.rows - 1; ++r)

{

uchar* quant_r = quantized_unfiltered.ptr<uchar>(r);

for (int c = 1; c < angle.cols - 1; ++c)

{

quant_r[c] &= 7;

}

}

// Filter the raw quantized image. Only accept pixels where the magnitude is above some

// threshold, and there is local agreement on the quantization.

quantized_angle = Mat::zeros(angle.size(), CV_8U);

for (int r = 1; r < angle.rows - 1; ++r)

{

float* mag_r = magnitude.ptr<float>(r);

for (int c = 1; c < angle.cols - 1; ++c)

{

if (mag_r[c] > threshold)

{

// Compute histogram of quantized bins in 3x3 patch around pixel

int histogram[8] = {0, 0, 0, 0, 0, 0, 0, 0};

uchar* patch3x3_row = &quantized_unfiltered(r-1, c-1);

histogram[patch3x3_row[0]]++;

histogram[patch3x3_row[1]]++;

histogram[patch3x3_row[2]]++;

patch3x3_row += quantized_unfiltered.step1();

histogram[patch3x3_row[0]]++;

histogram[patch3x3_row[1]]++;

histogram[patch3x3_row[2]]++;

patch3x3_row += quantized_unfiltered.step1();

histogram[patch3x3_row[0]]++;

histogram[patch3x3_row[1]]++;

histogram[patch3x3_row[2]]++;

// Find bin with the most votes from the patch

int max_votes = 0;

int index = -1;

for (int i = 0; i < 8; ++i)

{

if (max_votes < histogram[i])

{

index = i;

max_votes = histogram[i];

}

}

// Only accept the quantization if majority of pixels in the patch agree

static const int NEIGHBOR_THRESHOLD = 5;

if (max_votes >= NEIGHBOR_THRESHOLD)

quantized_angle.at<uchar>(r, c) = uchar(1 << index);

}

}

}

{

ColorGradientPyramid(const Mat& src, const Mat& mask,

float weak_threshold, size_t num_features,

float strong_threshold);

virtual void quantize(Mat& dst) const;

virtual bool extractTemplate(Template& templ) const;

virtual void pyrDown();

/// Recalculate angle and magnitude images

void update();

Mat src;

Mat mask;

int pyramid_level;

Mat angle;

Mat magnitude;

float weak_threshold;

size_t num_features;

float strong_threshold;

float _weak_threshold, size_t _num_features,

float _strong_threshold)

: src(_src),

mask(_mask),

pyramid_level(0),

weak_threshold(_weak_threshold),

num_features(_num_features),

strong_threshold(_strong_threshold)

{

update();

{

// Some parameters need to be adjusted

num_features /= 2; /// @todo Why not 4?

++pyramid_level;

// Downsample the current inputs

Size size(src.cols / 2, src.rows / 2);

Mat next_src;

cv::pyrDown(src, next_src, size);

src = next_src;

if (!mask.empty())

{

Mat next_mask;

resize(mask, next_mask, size, 0.0, 0.0, CV_INTER_NN);

mask = next_mask;

}

update();

{

dst = Mat::zeros(angle.size(), CV_8U);

angle.copyTo(dst, mask);

{

// Want features on the border to distinguish from background

Mat local_mask;

if (!mask.empty())

{

erode(mask, local_mask, Mat(), Point(-1,-1), 1, BORDER_REPLICATE);

subtract(mask, local_mask, local_mask);

}

// Create sorted list of all pixels with magnitude greater than a threshold

std::vector<Candidate> candidates;

bool no_mask = local_mask.empty();

float threshold_sq = CV_SQR(strong_threshold);

for (int r = 0; r < magnitude.rows; ++r)

{

const uchar* angle_r = angle.ptr<uchar>(r);

const float* magnitude_r = magnitude.ptr<float>(r);

const uchar* mask_r = no_mask ? NULL : local_mask.ptr<uchar>(r);

for (int c = 0; c < magnitude.cols; ++c)

{

if (no_mask || mask_r[c])

{

uchar quantized = angle_r[c];

if (quantized > 0)

{

float score = magnitude_r[c];

if (score > threshold_sq)

{

candidates.push_back(Candidate(c, r, getLabel(quantized), score));

}

}

}

}

}

// We require a certain number of features

if (candidates.size() < num_features)

return false;

// NOTE: Stable sort to agree with old code, which used std::list::sort()

std::stable_sort(candidates.begin(), candidates.end());

// Use heuristic based on surplus of candidates in narrow outline for initial distance threshold

float distance = static_cast<float>(candidates.size() / num_features + 1);

selectScatteredFeatures(candidates, templ.features, num_features, distance);

// Size determined externally, needs to match templates for other modalities

templ.width = -1;

templ.height = -1;

templ.pyramid_level = pyramid_level;

return true;

: weak_threshold(10.0f),

num_features(63),

strong_threshold(55.0f)

{

: weak_threshold(_weak_threshold),

num_features(_num_features),

strong_threshold(_strong_threshold)

{

const Mat& mask) const

{

return new ColorGradientPyramid(src, mask, weak_threshold, num_features, strong_threshold);

{

std::string type = fn["type"];

CV_Assert(type == CG_NAME);

weak_threshold = fn["weak_threshold"];

num_features = int(fn["num_features"]);

strong_threshold = fn["strong_threshold"];

{

fs << "type" << CG_NAME;

fs << "weak_threshold" << weak_threshold;

fs << "num_features" << int(num_features);

fs << "strong_threshold" << strong_threshold;

{

#ifdef __QNX__

long absdelta = (delta > 0) ? delta : -delta;

long f = absdelta < threshold ? 1 : 0;

#else

long f = std::abs(delta) < threshold ? 1 : 0;

#endif

const long fi = f * i;

const long fj = f * j;

A[0] += fi * i;

A[1] += fi * j;

A[3] += fj * j;

b[0] += fi * delta;

b[1] += fj * delta;

int difference_threshold)

{

dst = Mat::zeros(src.size(), CV_8U);

IplImage src_ipl = src;

IplImage* ap_depth_data = &src_ipl;

IplImage dst_ipl = dst;

IplImage* dst_ipl_ptr = &dst_ipl;

IplImage** m_dep = &dst_ipl_ptr;

unsigned short * lp_depth = (unsigned short *)ap_depth_data->imageData;

unsigned char * lp_normals = (unsigned char *)m_dep[0]->imageData;

const int l_W = ap_depth_data->width;

const int l_H = ap_depth_data->height;

const int l_r = 5; // used to be 7

const int l_offset0 = -l_r - l_r * l_W;

const int l_offset1 = 0 - l_r * l_W;

const int l_offset2 = +l_r - l_r * l_W;

const int l_offset3 = -l_r;

const int l_offset4 = +l_r;

const int l_offset5 = -l_r + l_r * l_W;

const int l_offset6 = 0 + l_r * l_W;

const int l_offset7 = +l_r + l_r * l_W;

const int l_offsetx = GRANULARITY / 2;

const int l_offsety = GRANULARITY / 2;

for (int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)

{

unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);

unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);

for (int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)

{

long l_d = lp_line[0];

if (l_d < distance_threshold)

{

// accum

long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;

long l_b[2]; l_b[0] = l_b[1] = 0;

accumBilateral(lp_line[l_offset0] - l_d, -l_r, -l_r, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset1] - l_d, 0, -l_r, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset2] - l_d, +l_r, -l_r, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset3] - l_d, -l_r, 0, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset4] - l_d, +l_r, 0, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset5] - l_d, -l_r, +l_r, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset6] - l_d, 0, +l_r, l_A, l_b, difference_threshold);

accumBilateral(lp_line[l_offset7] - l_d, +l_r, +l_r, l_A, l_b, difference_threshold);

// solve

long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];

long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];

long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];

/// @todo Magic number 1150 is focal length? This is something like

/// f in SXGA mode, but in VGA is more like 530.

float l_nx = static_cast<float>(1150 * l_ddx);

float l_ny = static_cast<float>(1150 * l_ddy);

float l_nz = static_cast<float>(-l_det * l_d);

float l_sqrt = sqrtf(l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);

if (l_sqrt > 0)

{

float l_norminv = 1.0f / (l_sqrt);

l_nx *= l_norminv;

l_ny *= l_norminv;

l_nz *= l_norminv;

//*lp_norm = fabs(l_nz)*255;

int l_val1 = static_cast<int>(l_nx * l_offsetx + l_offsetx);

int l_val2 = static_cast<int>(l_ny * l_offsety + l_offsety);

int l_val3 = static_cast<int>(l_nz * GRANULARITY + GRANULARITY);

*lp_norm = NORMAL_LUT[l_val3][l_val2][l_val1];

}

else

{

*lp_norm = 0; // Discard shadows from depth sensor

}

}

else

{

*lp_norm = 0; //out of depth

}

++lp_line;

++lp_norm;

}

}

cvSmooth(m_dep[0], m_dep[0], CV_MEDIAN, 5, 5);

{

DepthNormalPyramid(const Mat& src, const Mat& mask,

int distance_threshold, int difference_threshold, size_t num_features,

int extract_threshold);

virtual void quantize(Mat& dst) const;

virtual bool extractTemplate(Template& templ) const;

virtual void pyrDown();

Mat mask;

int pyramid_level;

Mat normal;

size_t num_features;

int extract_threshold;

int distance_threshold, int difference_threshold, size_t _num_features,

int _extract_threshold)

: mask(_mask),

pyramid_level(0),

num_features(_num_features),

extract_threshold(_extract_threshold)

{

quantizedNormals(src, normal, distance_threshold, difference_threshold);

{

// Some parameters need to be adjusted

num_features /= 2; /// @todo Why not 4?

extract_threshold /= 2;

++pyramid_level;

// In this case, NN-downsample the quantized image

Mat next_normal;

Size size(normal.cols / 2, normal.rows / 2);

resize(normal, next_normal, size, 0.0, 0.0, CV_INTER_NN);

normal = next_normal;

if (!mask.empty())

{

Mat next_mask;

resize(mask, next_mask, size, 0.0, 0.0, CV_INTER_NN);

mask = next_mask;

}

{

dst = Mat::zeros(normal.size(), CV_8U);

normal.copyTo(dst, mask);

{

// Features right on the object border are unreliable

Mat local_mask;

if (!mask.empty())

{

erode(mask, local_mask, Mat(), Point(-1,-1), 2, BORDER_REPLICATE);

}

// Compute distance transform for each individual quantized orientation

Mat temp = Mat::zeros(normal.size(), CV_8U);

Mat distances[8];

for (int i = 0; i < 8; ++i)

{

temp.setTo(1 << i, local_mask);

bitwise_and(temp, normal, temp);

// temp is now non-zero at pixels in the mask with quantized orientation i

distanceTransform(temp, distances[i], CV_DIST_C, 3);

}

// Count how many features taken for each label

int label_counts[8] = {0, 0, 0, 0, 0, 0, 0, 0};

// Create sorted list of candidate features

std::vector<Candidate> candidates;

bool no_mask = local_mask.empty();

for (int r = 0; r < normal.rows; ++r)

{

const uchar* normal_r = normal.ptr<uchar>(r);

const uchar* mask_r = no_mask ? NULL : local_mask.ptr<uchar>(r);

for (int c = 0; c < normal.cols; ++c)

{

if (no_mask || mask_r[c])

{

uchar quantized = normal_r[c];

if (quantized != 0 && quantized != 255) // background and shadow

{

int label = getLabel(quantized);

// Accept if distance to a pixel belonging to a different label is greater than

// some threshold. IOW, ideal feature is in the center of a large homogeneous

// region.

float score = distances[label].at<float>(r, c);

if (score >= extract_threshold)

{

candidates.push_back( Candidate(c, r, label, score) );

++label_counts[label];

}

}

}

}

}

// We require a certain number of features

if (candidates.size() < num_features)

return false;

// Prefer large distances, but also want to collect features over all 8 labels.

// So penalize labels with lots of candidates.

for (size_t i = 0; i < candidates.size(); ++i)

{

Candidate& c = candidates[i];

c.score /= (float)label_counts[c.f.label];

}

std::stable_sort(candidates.begin(), candidates.end());

// Use heuristic based on object area for initial distance threshold

float area = no_mask ? (float)normal.total() : (float)countNonZero(local_mask);

float distance = sqrtf(area) / sqrtf((float)num_features) + 1.5f;

selectScatteredFeatures(candidates, templ.features, num_features, distance);

// Size determined externally, needs to match templates for other modalities

templ.width = -1;

templ.height = -1;

templ.pyramid_level = pyramid_level;

return true;

: distance_threshold(2000),

difference_threshold(50),

num_features(63),

extract_threshold(2)

{

int _extract_threshold)

: distance_threshold(_distance_threshold),

difference_threshold(_difference_threshold),

num_features(_num_features),

extract_threshold(_extract_threshold)

{

const Mat& mask) const

{

return new DepthNormalPyramid(src, mask, distance_threshold, difference_threshold,

num_features, extract_threshold);

{

std::string type = fn["type"];

CV_Assert(type == DN_NAME);

distance_threshold = fn["distance_threshold"];

difference_threshold = fn["difference_threshold"];

num_features = int(fn["num_features"]);

extract_threshold = fn["extract_threshold"];

{

fs << "type" << DN_NAME;

fs << "distance_threshold" << distance_threshold;

fs << "difference_threshold" << difference_threshold;

fs << "num_features" << int(num_features);

fs << "extract_threshold" << extract_threshold;

uchar * dst, const int dst_stride,

const int width, const int height)

{

#if CV_SSE2

volatile bool haveSSE2 = checkHardwareSupport(CV_CPU_SSE2);

#if CV_SSE3

volatile bool haveSSE3 = checkHardwareSupport(CV_CPU_SSE3);

#endif

bool src_aligned = reinterpret_cast<unsigned long long>(src) % 16 == 0;

#endif

for (int r = 0; r < height; ++r)

{

int c = 0;

#if CV_SSE2

// Use aligned loads if possible

if (haveSSE2 && src_aligned)

{

for ( ; c < width - 15; c += 16)

{

const __m128i* src_ptr = reinterpret_cast<const __m128i*>(src + c);

__m128i* dst_ptr = reinterpret_cast<__m128i*>(dst + c);

*dst_ptr = _mm_or_si128(*dst_ptr, *src_ptr);

}

}

#if CV_SSE3

// Use LDDQU for fast unaligned load

else if (haveSSE3)

{

for ( ; c < width - 15; c += 16)

{

__m128i val = _mm_lddqu_si128(reinterpret_cast<const __m128i*>(src + c));

__m128i* dst_ptr = reinterpret_cast<__m128i*>(dst + c);

*dst_ptr = _mm_or_si128(*dst_ptr, val);

}

}

#endif

// Fall back to MOVDQU

else if (haveSSE2)

{

for ( ; c < width - 15; c += 16)

{

__m128i val = _mm_loadu_si128(reinterpret_cast<const __m128i*>(src + c));

__m128i* dst_ptr = reinterpret_cast<__m128i*>(dst + c);

*dst_ptr = _mm_or_si128(*dst_ptr, val);

}

}

#endif

for ( ; c < width; ++c)

dst[c] |= src[c];

// Advance to next row

src += src_stride;

dst += dst_stride;

}

{