const float sqrt2_inv = 0.7071067811865475f; // 1.0 / sqrt(2)

/** @brief Adds local_cost(p, q) to cost_p.

bool add_local_cost(const Point& p, const Point& q, float& cost_p, const float cost_q) const

if ((cost_p += weight_non_edge_compute * non_edge_feature.at<uchar>(q)) < cost_q)

{

const bool isDiag = (p.x != q.x) && (p.y != q.y);

float fG = gradient_magnitude.at<float>(q);

if (!isDiag)

{

fG *= sqrt2_inv;

}

if ((cost_p += weight_gradient_magnitude * fG) < cost_q)

{

const Point2f diff((float)(q.x - p.x), (float)(q.y - p.y));

const Point2f Ip = gradient_direction(p);

const Point2f Iq = gradient_direction(q);

const Point2f Dp(Ip.y, -Ip.x); // D(p) - 90 degrees clockwise

const Point2f Dq(Iq.y, -Iq.x); // D(q) - 90 degrees clockwise

float dp = Dp.dot(diff); // dp(p, q)

float dq = Dq.dot(diff); // dq(p, q)

if (dp < 0)

{

dp = -dp; // ensure dp >= 0

dq = -dq;

}

if (isDiag)

{

dp *= sqrt2_inv; // normalize length of (q - p)

dq *= sqrt2_inv; // normalize length of (q - p)

}

int dp_i = cvFloor(dp * ACOS_TABLE_SIZE); // dp is in range 0..1

dp_i = std::min(ACOS_TABLE_SIZE, std::max(0, dp_i));

int dq_i = cvFloor(dq * ACOS_TABLE_SIZE); // dq is in range -1..1

dq_i = std::min(ACOS_TABLE_SIZE, std::max(-ACOS_TABLE_SIZE, dq_i));

const float fD = acos_table[dp_i + ACOS_TABLE_SIZE] + acos_table[dq_i + ACOS_TABLE_SIZE];

cost_p += weight_gradient_direction * fD;

}

}

return cost_p < cost_q;

float cost = cost_q;

if (add_local_cost(q, r, cost, cost_r))