www.pudn.com > imageFeatures_Ver3_0_source.zip > IFLocation.cpp
// IFLocation.cpp: ImageFeatures class CLocation
/*
** Copyright (C) 1994, 2003 Tyler C. Folsom
**
** Permission to use, copy, modify, and distribute this software and its
** documentation for any purpose and without fee is hereby granted, provided
** that the above copyright notice appear in all copies and that both that
** copyright notice and this permission notice appear in supporting
** documentation. This software is provided "as is" without express or
** implied warranty.
*/
#include "stdafx.h"
#include "IFLocation.h"
#include "quad_dis.h"
#include "profile.h"
#define FL4TH ((float) 0.25)
#define FLHALF ((float) 0.5)
#define FL0 ((float) 0)
#define FL1 ((float) 1)
#define FL2 ((float) 2)
#define FL3 ((float) 3)
#define FL4 ((float) 4)
#define FL5 ((float) 5)
#define FL6 ((float) 6)
/* Solve for angles within 0.1 degree */
#define ANGLE_CONVEGENCE ((float) 0.00174)
#define FACTOR ((float)1.6)
#define EVEN_ORDER 2
#define ODD_ORDER 3
#define G2_ORN (EVEN_ORDER+1)
#define H2_ORN (4)
#define G1_ORN 2
#define H1_ORN 3
#define WAY_SMALL ((float) -1.0e20)
static float deriv_g2h2( float *gl, float theta);
static float g1h1( float *gl, float theta, float *d1, float *d2);
/*------------------------------------------------------------------------*/
// constructor specifying (x,y) sampling location and
// smaller filter diameter. Numbers are in pixels.
CLocation::CLocation( int x, int y, int diam )
{
Initialize( x, y, diam );
}
/*------------------------------------------------------------------------*/
// Initialization specifying (x,y) sampling location and
// smaller filter diameter. Numbers are in pixels.
void CLocation::Initialize( int x, int y, int diam )
{
m_x_rf = x;
m_y_rf = y;
if (diam > 0)
{
m_diam = diam;
m_usingOdd = false;
}
else
{
m_diam = -diam;
m_usingOdd = true;
}
if ((m_diam & 1) == 0)
m_big_diam = 2 * m_diam;
else /* keep odd filters centered at same point */
m_big_diam = 2 * m_diam + 1;
m_usingSmall = true;
}
//----------------------------------------------------------------------------------------
// copy constructor
CLocation::CLocation(const CLocation &right)
{
Copy( right );
}
//----------------------------------------------------------------------------------------
// copy method
void CLocation::Copy(const CLocation &right)
{
m_x_rf = right.m_x_rf;
m_y_rf = right.m_y_rf;
m_usingSmall = right.m_usingSmall;
m_usingOdd = right.m_usingOdd;
m_diam = right.m_diam;
m_big_diam = right.m_big_diam;
int size = right.corrEven.GetSize();
corrEven.SetSize(size);
for (int i = 0; i < size; i++)
{
corrEven.SetAt( i, right.corrEven[i]);
}
size = right.corrOdd.GetSize();
corrOdd.SetSize(size);
for (i = 0; i < size; i++)
{
corrOdd.SetAt( i, right.corrOdd[i]);
}
size = right.corrBigEven.GetSize();
corrBigEven.SetSize(size);
for (i = 0; i < size; i++)
{
corrBigEven.SetAt( i, right.corrBigEven[i]);
}
size = right.corrBigOdd.GetSize();
corrBigOdd.SetSize(size);
for (i = 0; i < size; i++)
{
corrBigOdd.SetAt( i, right.corrBigOdd[i]);
}
}
/*------------------------------------------------------------------------*/
const CLocation &CLocation::operator=( const CLocation &right )
{
if (&right != this)
{
Copy( right );
}
return *this;
}
/*------------------------------------------------------------------------*/
// store correlation "result" in slot n of even/odd big/small filter
void CLocation::SetResult( int n, float result)
{
if (m_usingSmall)
{
if (m_usingOdd)
{
corrOdd.SetAtGrow( n, result);
}
else // small even
{
corrEven.SetAtGrow( n, result);
}
}
else // big filters
{
if (m_usingOdd)
{
corrBigOdd.SetAtGrow( n, result);
}
else // big even
{
corrBigEven.SetAtGrow( n, result);
}
}
}
/*------------------------------------------------------------------------*/
// return correlation from slot n of even/odd big/small filter
float CLocation::GetResult( int n)
{
if (m_usingSmall)
{
if (m_usingOdd)
{
return (corrOdd[n]);
}
else // small even
{
return (corrEven[n]);
}
}
else // big filters
{
if (m_usingOdd)
{
return (corrBigOdd[n]);
}
else // big even
{
return (corrBigEven[n]);
}
}
}
/*------------------------------------------------------------------------*/
// returns a rectangle defining the subimage of the location
RECT CLocation::GetRect()
{
RECT bounds;
int low_half;
int up_half;
if (m_usingSmall)
{
low_half = m_diam / 2;
up_half = (m_diam+1) / 2;
}
else // big filters
{
low_half = m_big_diam / 2;
up_half = (m_big_diam+1) / 2;
}
bounds.left = m_x_rf - low_half;
bounds.top = m_y_rf - low_half;
bounds.right = m_x_rf + up_half;
bounds.bottom = m_y_rf + up_half;
return bounds;
}
/*------------------------------------------------------------------------*/
// returns an upper bound on the strength
float CLocation::GetStrengthBound()
{
float bound = 0;
for (int i = 0; i < corrOdd.GetSize(); i++)
bound += FL_ABS(corrOdd[i]);
for (i = 0; i < corrEven.GetSize(); i++)
bound += FL_ABS(corrEven[i]);
return( bound );
}
/*------------------------------------------------------------------------*/
// Steer a filter type to the indicated angle (radians)
float CLocation::Steer( float angle )
{
PROFILE("Steer");
float steered = 0;
if (m_usingSmall)
{
if (m_usingOdd)
{
steered = (ODD_ANGLES == 4)?
steer_45( corrOdd, angle ): // 4 orientations
steer_90( corrOdd, angle ); // 2 orientations
}
else // small even: 3 orientations
{
steered = steer_60( corrEven, angle );
}
}
else // big filters
{
if (m_usingOdd)
{
steered = (ODD_ANGLES == 4)?
steer_45( corrBigOdd, angle ): // 4 orientations
steer_90( corrBigOdd, angle ); // 2 orientations
}
else // big even: 3 orientations
{
steered = steer_60( corrBigEven, angle );
}
}
return steered;
}
/*------------------------------------------------------------------------*/
/*
steer_60: Given input filters sampled at 60 degrees, interpolate
to the filter response at angle theta.
input: sampled - response of filters at 0, 60, 120 degrees.
theta - angle for desired response in radians.
returned value: the interpolated filter response at theta.
*/
float CLocation::steer_60(
CArray& sampled,
float theta)
{
float angle;
float steered;
int i_angle;
angle = (float) (PI_1 / G2_ORN);
steered = (float) 0.0;
for (i_angle = 0; i_angle < G2_ORN; i_angle++)
steered += sampled[i_angle] * ((float) 1.0 + (float) 2.0 *
(float) cos( (double) (2.0 * theta - PI_2 * (float)i_angle /
(float) G2_ORN)));
steered /= G2_ORN;
return (steered);
}
/*------------------------------------------------------------------------*/
/*
steer_45: Given input filters sampled at 45 degrees, interpolate
to the filter response at angle theta.
input: sampled - response of filters at 0, 45, 90, 135 degrees.
theta - angle for desired response in radians.
returned value: the interpolated filter response at theta.
*/
float CLocation::steer_45(
CArray& sampled,
float theta)
{
int angle;
float steered;
steered = (float) 0.0;
for (angle = 0; angle < H2_ORN; angle++)
steered += sampled[angle] * (float) (
cos( (double) theta - PI_1 * (float)angle / (float) H2_ORN) +
cos( theta * 3.0 - PI_3 * (float) angle / (float) H2_ORN));
steered /= (float) 2.0;
return (steered);
}
/*------------------------------------------------------------------------*/
/*
steer_90: Given input filters sampled at 90 degrees, interpolate
to the filter response at angle theta.
input: sampled - response of filters at 0, 90 degrees.
theta - angle for desired response in radians.
returned value: the interpolated filter response at theta.
*/
float CLocation::steer_90(
CArray& sampled,
float theta)
{
return ( (float) (sampled[0] * cos( (double) theta)
+ sampled[1] * sin( (double) theta)) );
}
/*------------------------------------------------------------------------*/
// Finds the angle (radians) of the feature based on filter correlations
/*
GetAngle: Given input filters sampled at 60 and 90 degrees, find the angle
of the dominant response.
input: corrEven - response of even filters at 0, 60, 120 degrees.
corrOdd - response of odd filters at 0 and 90 degrees.
output: dom_resp - squared response of the filter at the dominant angle.
*steeredEven - interpolated even response at the dominant angle.
*steeredOdd - interpolated odd response at the dominant angle.
returned value: angle of dominant response in radians.
There are two methods here. The older, obsolete, method is based on using
corrOdd - response of odd filters at 0, 45, 90, 135 degrees.
The solve() and deriv_g2h2() routines find the angle where the derivative is zero.
The newer, more efficient, method uses solve_max() and g1h1() to find the
angle that gives maximum response. For documentation, see the header for
g1h1() and Folsom & Pinter, "Primitive Features by Steering, Quadrature
and Scale", IEEE Trans PAMI, Nov 1998, pp. 1161-1173.
*/
float CLocation::GetAngle(
float *dom_resp,
float *steeredEven,
float *steeredOdd,
bool noSteering) // no steering if true
{
PROFILE("CLocation::GetAngle");
float gl[7]; /* gamma and lamda coefficients */
float coef[8]; /* combinations of gamma and lambda for trig terms*/
float angles[4]; /* 4 solutions */
float dom2;
float dom1;
float theta1, theta;
float theta_rad;
float max_mag, mag_sq;
if (m_x_rf == 77 && m_y_rf == 87)
{
max_mag = 10; // break here to debug
}
max_mag = WAY_SMALL;
/* Gamma1, gamma2, gamma3 */
gl[0] = (corrEven[0] + corrEven[1] + corrEven[2]) / FL3;
gl[1] = (FL2 * corrEven[0] - corrEven[1] - corrEven[2]) / FL3;
gl[2] = (corrEven[1] - corrEven[2]) / ROOT_3;
if (ODD_ANGLES == 4)
{
/* Lambda1, .. lambda3 */
gl[3] = (corrOdd[0] + (corrOdd[1] - corrOdd[3]) / ROOT_2) / FL2;
gl[4] = (corrOdd[2] + (corrOdd[1] + corrOdd[3]) / ROOT_2) / FL2;
gl[5] = (corrOdd[0] - (corrOdd[1] - corrOdd[3]) / ROOT_2) / FL2;
gl[6] = (-corrOdd[2] + (corrOdd[1] + corrOdd[3]) / ROOT_2) / FL2;
coef[0] = FL2 * (FL2*gl[0]*gl[2] + gl[3]*gl[4] - gl[4]*gl[5] + gl[3]*gl[6]);
coef[1] = -(FL4*gl[0]*gl[1] + gl[3]*gl[3] - gl[4]*gl[4] + FL2*gl[3]*gl[5]
+ FL2*gl[4]*gl[6]);
coef[2] = FL4 * (gl[1]*gl[2] + gl[4]*gl[5] + gl[3]*gl[6]);
coef[3] = FL2 * (-gl[1]*gl[1] + gl[2]*gl[2] - FL2*gl[3]*gl[5]
+ FL2*gl[4]*gl[6]);
coef[4] = FL6 * gl[5]*gl[6];
coef[5] = FL3*(-gl[5]*gl[5] + gl[6]*gl[6]);
/* try arbitrary initial guess for theta */
for (theta = FL0; theta < PI_1;)
{
theta1 = theta;
if (!noSteering)
{ // Find the strongest angle near this guess
solve (coef, &theta1, deriv_g2h2);
}
dom1 = steer_45 (corrOdd, theta1);
dom2 = steer_60 (corrEven, theta1);
mag_sq = dom1 * dom1 + dom2 * dom2;
if (mag_sq >= max_mag)
{
max_mag = mag_sq;
theta_rad = theta1;
*steeredEven = dom2;
*steeredOdd = dom1;
}
if (noSteering)
{ // skip if not looking for the angle.
break;
}
/* start next guess about 1 degree beyond previous root. */
theta = theta1 + (float) 0.02;
}
*dom_resp = max_mag;
return (theta_rad);
}
else
{
if (noSteering)
{
theta = 0;
*steeredOdd = corrOdd[0];
*steeredEven = corrEven[0];
}
else
{
/* Alpha 1, alpha 2 */
gl[3] = corrOdd[0];
gl[4] = corrOdd[1];
/* [4 ...7] derivative coefficients for cos 2x, sin 2x, cos 4x, sin 4x */
coef[5] = -(FL4*gl[0]*gl[1] + gl[3]*gl[3] - gl[4]*gl[4]);
coef[7] = FL2 * (-gl[1]*gl[1] + gl[2]*gl[2]);
/* [0...1] coefficients for cos 2x, sin 2x, cos 4x, sin 4x */
coef[0] = -FLHALF * coef[5];
coef[1] = FL2*gl[0]*gl[2] + gl[3]*gl[4];
coef[4] = FL2 * coef[1];
coef[2] = -FL4TH * coef[7];
coef[3] = gl[1] * gl[2];
coef[6] = FL4 * coef[3];
/* Can find explicit solution when gl[2]*gl[3] == gl[1]*gl[4] */
// if (FL_ABS(gl[2]*gl[3] - gl[1]*gl[4]) < EPSILON * GetStrengthBound())
// set initial guesses at solutions for odd only or even only
angles[0] = atan2(gl[4], gl[3]);
angles[1] = angles[0] + (float) PI_HALF;
angles[2] = atan2(gl[2], gl[1]) * FLHALF;
angles[3] = angles[2] + (float) PI_HALF;
for (int i = 0; i < 4; i++)
{
float value = solve_max (coef, &angles[i], g1h1);
if (value > max_mag)
{
max_mag = value;
theta = angles[i];
}
}
*steeredOdd = steer_90 (corrOdd, theta);
*steeredEven = steer_60 (corrEven, theta);
} // steering
} // 2 orientations for odd
*dom_resp = *steeredOdd * *steeredOdd + *steeredEven * *steeredEven;
while (theta > PI_1)
theta -= PI_1;
while (theta < 0)
theta += PI_1;
return theta;
}
/*------------------------------------------------------------------------*/
/* [obsolete; only used when ODD_ANGLES == 4]
solve: Given sampled input filters, and an initial estimate of the angle
of the dominant response, solve numerically for this angle.
input: sampled - coefficients from response of filters.
deriv - the expression whose zero crossing we are looking for.
modified input: theta - angle of dominant response (radians).
returned value: none.
*/
void CLocation::solve(
float *sampled,
float *theta,
float (*deriv)(float *, float))
{
PROFILE("solve");
float theta1, theta2, value1, value2;
float th_low, th_hi, val_low, val_hi, d_th, del;
int j;
/* first bracket the root */
theta1 = *theta;
value1 = (*deriv) (sampled, theta1);
theta2 = theta1 + (float) 0.1; /* 0.1 = about 5 degrees */
value2 = (*deriv) (sampled, theta2);
while (value1 * value2 > (float) 0.0)
{
theta2 += FACTOR * (theta2 - theta1);
value2 = (*deriv) (sampled, theta2);
}
if (value1 == FL0)
{
*theta = theta1;
return;
}
if (value2 == FL0)
{
*theta = theta2;
return;
}
/* now values at theta1 and theta2 have opposite signs. */
if (value1 < (float) 0.0)
{
th_low = theta1;
val_low = value1;
th_hi = theta2;
val_hi = value2;
}
else
{
th_low = theta2;
val_low = value2;
th_hi = theta1;
val_hi = value1;
}
d_th = th_hi - th_low;
for (j = 0; j < MAX_ITER; j++)
{
theta1 = th_low + d_th * val_low / (val_low - val_hi);
value1 = (*deriv) (sampled, theta1);
if (value1 < (float) 0.0)
{
del = th_low - theta1;
th_low = theta1;
val_low = value1;
}
else
{
del = th_hi - theta1;
th_hi = theta1;
val_hi = value1;
}
d_th = th_hi - th_low;
if (FL_ABS(del) < (float) 0.001 || value1 == (float) 0.0)
break; /* converged */
} /* failed to converge if loop terminates normally */
*theta = theta1;
return;
}
/*------------------------------------------------------------------------*/
/* [obsolete; only used when ODD_ANGLES == 4]
deriv_g2h2: Given 3 input filters sampled at 60 degrees and 4 filters
at 45 degrees, and a steering angle, compute the derivative of the
squared magnitude of the filter at this angle.
input: gl - 0,1,2: coefficients of constant, cos2x, sin2x for g2.
3,4,5,6: coefs of cosx, sinx, cos3x, sin3x for h2 steering.
theta - angle of dominant response (radians).
returned value: derivative of squared magnitude of filter response.
*/
float deriv_g2h2(
float *coef,
float theta)
{
float sin_2th, cos_2th, sin_4th, cos_4th, sin_6th, cos_6th, value;
double dtheta;
dtheta = (double) theta;
sin_2th = (float) sin( dtheta * 2.0);
cos_2th = (float) cos( dtheta * 2.0);
sin_4th = (float) sin( dtheta * 4.0);
cos_4th = (float) cos( dtheta * 4.0);
sin_6th = (float) sin( dtheta * 6.0);
cos_6th = (float) cos( dtheta * 6.0);
value = coef[0] * cos_2th;
value += coef[1] * sin_2th;
value += coef[2] * cos_4th;
value += coef[3] * sin_4th;
value += coef[4] * cos_6th;
value += coef[5] * sin_6th;
return (value);
}
/*------------------------------------------------------------------------*/
/*
solve_max: Given sampled input filters, and an initial estimate of the angle
of the dominant response, solve numerically for this angle.
input: sampled - coefficients from response of filters.
trig - the expression whose maximum we are looking for.
modified input: theta - angle of dominant response (radians).
returned value: value of function at theta (less a constant)
*/
float CLocation::solve_max(
float *sampled,
float *theta,
float (*trig)(float *, float, float *, float *))
{
PROFILE("solve_max");
float theta1, theta2, value1, value2;
float th_low, th_hi, val_low, val_hi, d_low, d_hi;
float deriv1, deriv2, delta;
bool bracketed = false;
/* Try to bracket the maximum */
theta1 = *theta;
value1 = (*trig) (sampled, theta1, &deriv1, &deriv2);
if (FL_ABS(deriv1) < EPSILON && deriv2 < 0)
{
return value1;
}
while (!bracketed)
{
d_low = deriv1;
if (FL_ABS(deriv2) < EPSILON)
{
delta = (deriv1 > 0? (float) 0.1: (float) -0.1); /* 0.1 = about 5 degrees */
}
else
{ /* Newton-Raphson method for finding the zero of deriv1 */
delta = deriv1 / FL_ABS(deriv2);
if (delta < -PI8TH)
delta = -PI8TH;
if (delta > PI8TH)
delta = PI8TH;
if (FL_ABS(delta) < (float) 0.02) /* about 1 degree */
delta = (delta < 0? (float) -0.02: (float) 0.02);
}
theta2 = theta1 + delta; // In the direction in which the derivative increases
value2 = (*trig) (sampled, theta2, &deriv1, &deriv2);
if (FL_ABS(deriv1) < EPSILON && deriv2 < 0)
{
while (theta2 >= PI_1)
theta2 -= PI_1;
while (theta2 < 0)
theta2 += PI_1;
*theta = theta2;
return value2;
}
if (value2 < value1 ||
(theta2 > theta1 && deriv1 < 0) ||
(theta2 < theta1 && deriv1 > 0))
{
bracketed = true;
}
else
{ // we are still increasing from the starting point
theta1 = theta2;
value1 = value2;
}
} // bracketed
if (theta1 < theta2)
{
th_low = theta1;
th_hi = theta2;
val_low = value1;
val_hi = value2;
d_hi = deriv1;
}
else
{
th_low = theta2;
th_hi = theta1;
val_low = value2;
val_hi = value1;
d_hi = d_low;
d_low = deriv1;
}
while (th_hi - th_low > ANGLE_CONVEGENCE)
{
theta1 = theta2;
if (FL_ABS(deriv2) < EPSILON)
{
delta = (deriv1 > 0? (float) 0.1: (float) -0.1); /* 0.1 = about 5 degrees */
}
else
{ /* Newton-Raphson method for finding the zero of deriv1 */
delta = deriv1 / FL_ABS(deriv2);
if (FL_ABS(delta) < ANGLE_CONVEGENCE / 2)
delta = delta < 0? -ANGLE_CONVEGENCE / 2: ANGLE_CONVEGENCE / 2;
}
theta2 = theta1 + delta; // In the direction in which the derivative increases
if (theta2 <= th_low)
{
theta2 = th_low + FL4TH * (th_hi - th_low);
}
if (theta2 >= th_hi)
{
theta2 = th_hi - FL4TH * (th_hi - th_low);
}
value2 = (*trig) (sampled, theta2, &deriv1, &deriv2);
if (FL_ABS(deriv1) < EPSILON && deriv2 < 0)
{
break;
}
/* A positive derivative replaces the old lower bound,
except possibly when derivatives were positive at both bounds.
In this case the old upper bound is replaced by an angle with a
lower value.
*/
if (d_hi > 0 && value2 <= val_hi)
{ // bracketed region has a max and a min
th_hi = theta2;
d_hi = deriv1;
val_hi = value2;
}
else if (d_low < 0 && value2 <= val_low)
{ // bracketed region has a min and a max
th_low = theta2;
d_low = deriv1;
val_low = value2;
}
else if (deriv1 > 0)
{
th_low = theta2;
d_low = deriv1;
val_low = value2;
}
else /* negative or zero derivative replaces old upper bound */
{
th_hi = theta2;
d_hi = deriv1;
val_hi = value2;
}
}
while (theta2 < 0)
theta2 += PI_1;
while (theta2 >= PI_1 - EPSILON)
theta2 -= PI_1;
if (theta2 < 0)
theta2 = 0;
*theta = theta2;
return value2;
}
/*------------------------------------------------------------------------*/
/*
g1h1: Given coefficients from 3 input filters sampled at 60 degrees
and 2 filters at 90 degrees, and a steering angle, compute the
squared magnitude of the filter at this angle and its derivatives.
Since the function is trigonometric, all derivatives are well behaved.
input: coef - 0,1,2,3: coefficients of cos2x, sin2x, cos4x, sin4x.
4,5,6,7: coefs derivative of cos2x, sin2x, cos4x, sin4x.
theta - angle of dominant response (radians).
returned value: squared magnitude of filter response (less a constant).
output: *deriv - first derivative
*deriv2 - second derivative
The expression that we are maximizing is the squared response of steering
the even and odd filters.
The odd steering equation is
G(theta) = G0 * cos(theta) + G90 * sin(theta)
where G0 = Alpha1 = gl[3] = corrOdd[0] is vertical odd filter response
and G90 = Alpha2 = gl[4] = corrOdd[1] is horizontal odd filter response.
Thus
G(theta)^2 = 0.5 * (G0^2 + G90^2) + 0.5 * (G0^2 - G90^2) * cos(2*theta)
+ G0 * G90 * sin(2*theta)
The even steering equation is
H(theta) = (H0 + H60 + H120)/3 + (2*H0 - H60 - H120) * cos(2*theta)/3
+ (H60 - H120) * sin(2*theta) / sqrt(3)
where H0 = corrEven[0] is vertical even filter response
H60 = corrEven[1] is even filter response at 60 degrees
H120 = corrEven[2] is even filter response at 120 degrees
H(theta) = gl[0] + gl[1] * cos(2*theta) + gl[2] * sin(2*theta)
where gl[0] = Gamma1 = (H0 + H60 + H120)/3
gl[1] = Gamma2 = (2*H0 - H60 - H120)/3
gl[2] = Gamma3 = (H60 - H120) / sqrt(3)
H(theta)^2 = gl[0]^2 + 0.5*gl[1]^2 + 0.5*gl[2]^2
+ 2 * gl[0] * gl[1] * cos(2*theta) + 2 * gl[0] * gl[2] * sin(2*theta)
+ 0.5*(gl[1]^2 - gl[2]^2) * cos(4*theta) + gl[1]*gl[2] * sin(4*theta)
Combining the two equations gives
G(theta)^2 + H(theta)^2
= 0.5 * (gl[3]^2 + gl[4]^2) + 0.5 * (gl[3]^2 - gl[4]^2) * cos(2*theta)
+ gl[3] * gl[4] * sin(2*theta)
+ gl[0]^2 + 0.5*gl[1]^2 + 0.5*gl[2]^2
+ 2 * gl[0] * gl[1] * cos(2*theta) + 2 * gl[0] * gl[2] * sin(2*theta)
+ 0.5*(gl[1]^2 - gl[2]^2) * cos(4*theta) + gl[1]*gl[2] * sin(4*theta)
G(theta)^2 + H(theta)^2
= 0.5 * (gl[3]^2 + gl[4]^2) + gl[0]^2 + 0.5*gl[1]^2 + 0.5*gl[2]^2
+ (0.5 * (gl[3]^2 - gl[4]^2) + 2 * gl[0] * gl[1]) * cos(2*theta)
+ (gl[3] * gl[4] + 2 * gl[0] * gl[2])* sin(2*theta)
+ 0.5*(gl[1]^2 - gl[2]^2) * cos(4*theta) + gl[1]*gl[2] * sin(4*theta)
G(theta)^2 + H(theta)^2
= 0.5 * (gl[3]^2 + gl[4]^2) + gl[0]^2 + 0.5*gl[1]^2 + 0.5*gl[2]^2
+ coef[0] * cos(2*theta) + coef[1] * sin(2*theta)
+ coef[2] * cos(4*theta) + coef[3] * sin(4*theta)
The constant term is ignored, since it is irrelevant to maximizing the response.
Thus the returned value can be negative.
The first derivative of this expression is:
-2 * coef[0] * sin(2*theta) + 2 * coef[1] * cos(2*theta)
-4 * coef[2] * sin(4*theta) + 4 * coef[3] * cos(4*theta)
= coef[4] * cos(2*theta) + coef[5] * sin(2*theta)
+ coef[6] * cos(4*theta) + coef[7] * sin(4*theta)
The second derivative is:
-4 * coef[0] * cos(2*theta) - 4 * coef[1] * sin(2*theta)
-16 * coef[2] * cos(4*theta) -16 * coef[3] * sin(4*theta)
*/
float g1h1(
float *coef,
float theta,
float *deriv,
float *deriv2)
{
float sin_2th, cos_2th, sin_4th, cos_4th;
double dtheta;
float funct;
dtheta = 2.0 * theta;
sin_2th = (float) sin( dtheta);
cos_2th = (float) cos( dtheta);
sin_4th = FL2 * sin_2th * cos_2th;
cos_4th = cos_2th * cos_2th - sin_2th * sin_2th;
*deriv = coef[4] * cos_2th
+ coef[5] * sin_2th
+ coef[6] * cos_4th
+ coef[7] * sin_4th;
funct = coef[2] * cos_4th
+ coef[3] * sin_4th;
*deriv2 = -12 * funct; // pick up another -4 * these terms below.
funct += coef[0] * cos_2th
+ coef[1] * sin_2th;
*deriv2 -= FL4 * funct;
return funct;
}