Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Eric Is Stupid
public class FlapControl
{
bool ifFlip = false;
#region some costant values
const int IMG_WIDTH = 640;
const int IMG_HEIGHT = 480;
const int IMG_SIZE = IMG_WIDTH * IMG_HEIGHT;
const int STEP = 3;
#endregion
#region basic variables
List<double> m_abc;
String m_dir = "none";
bool m_ifGroundCalibrate = false;
bool m_ifHumanCalibrate = false;
bool m_ifHuman = false;
double m_minDist = 1.5;
double m_maxDist = 3.0;
double m_limitOffsetHead = 2;
double m_limitOffsetFoot = -2;
double m_legL;
List<int> m_humanCenter;
List<double> m_gd;
List<int> m_feetLowerPixel = new List<int>(0);
double m_angleLeg;
double m_speedLeg;
public double m_lastReliableDetection;
public List<double> m_lastFive = new List<double>(0);
public List<long> m_lastFiveTime = new List<long>(0);
public List<double> m_angleHistory = new List<double>(0);
public bool m_ifLoadGD = false;
#endregion
#region face detection for direction determination (Face mush at left!)
#endregion
#region filter background
int extract_floor_plane(short[] shortDepth)
{
short[] depth = shortDepth;
List<double> ptPlane = new List<double>(shortDepth.Length);
for (int u = 0; u < IMG_WIDTH; u += 10)
{
for (int v = 0; v < IMG_HEIGHT; v += 10)
{
if (Math.Abs(u - IMG_WIDTH / 2) < IMG_WIDTH / 8 && v > IMG_HEIGHT - IMG_HEIGHT / 8)
{
List<double> xyz = depth_to_xyz(u, v, depth[u + v * IMG_WIDTH]);
if (xyz[1] >= 0 && xyz[2] < 0 && xyz[0] == xyz[0])
{
ptPlane.Add(xyz[0]);
ptPlane.Add(xyz[1]);
ptPlane.Add(xyz[2]);
}
}
}
}
List<double> planePara = get_plane_from_point_set(ptPlane);
m_gd = planePara;
m_ifGroundCalibrate = true;
StreamWriter outfilep = new StreamWriter("plane.txt");
StreamWriter outfiled = new StreamWriter("depth.txt");
for (int i = 0; i < 3; i++ )
{
outfilep.WriteLine(planePara[i].ToString());
}
outfilep.Close();
//short[] dd = change_arr_direction(shortDepth, 1);
for (int i = 0; i < shortDepth.Length; i++)
{
outfiled.WriteLine(shortDepth[i].ToString());
}
outfiled.Close();
return 0;
}
short[] filter_background(short[] shortDepth)
{
double fx_d = 594.21434211923247;
double fy_d = 591.04053696870778;
double cx_d = 339.30780975300314;
double cy_d = 242.73913761751615;
short[] depth = shortDepth;
int t = 0;
for (int u = 0; u < IMG_WIDTH; u++)
{
for (int v = 0; v < IMG_HEIGHT; v++)
{
if (depth[v * IMG_WIDTH + u] < m_minDist * 1000 || depth[v * IMG_WIDTH + u] > m_maxDist * 1000)
{
depth[v * IMG_WIDTH + u] = 0; t++;
}
else
{
double x = (u - cx_d) * (1.0 * depth[v * IMG_WIDTH + u]) / fx_d / 1000;
double y = 1.0 * depth[v * IMG_WIDTH + u] / 1000;
double z = -(v - cy_d) * (1.0 * depth[v * IMG_WIDTH + u]) / fy_d / 1000;
double A = m_gd[0];
double B = m_gd[1];
double C = m_gd[2];
m_abc = m_gd;
//A = 0.00547722535782924;
//B = 0.340717078392629;
//C = -1.1487329093282;
if (z < A * x + B * y + C + 0.06)
//if (z < -0.95)
{
depth[v * IMG_WIDTH + u] = 0; t++;
}
}
}
}
short[] res = depth;
return res;
}
short[] refine_filter_scale(short[] shortDepth)
{
short[] res = new short[IMG_SIZE];
double mindist = 1000;
double maxdist = 0;
double minx = 1000;
double maxx = -1000;
short[] depth = shortDepth;
for (int u = 0; u < IMG_WIDTH; u++)
{
for (int v = 0; v < IMG_HEIGHT; v++)
{
int id = v * IMG_WIDTH + u;
if (depth[id] > 0)
{
List<double> xyz = depth_to_xyz(u, v, depth[id]);
double x = xyz[0];
double y = xyz[1];
double z = xyz[2];
if (x < minx) { minx = x; }
if (x > maxx) { maxx = x; }
if (y < mindist) { mindist = y; }
if (y > maxdist) { maxdist = y; }
}
}
}
m_limitOffsetHead = maxx + 0.15;
m_limitOffsetFoot = minx - 0.15;
m_minDist = mindist - 0.1;
m_maxDist = maxdist + 0.1;
res = depth;
return res;
}
#endregion
#region human part detection
List<int> detect_human_center(short[] shortDepth)
{
List<int> res = new List<int>(4) { 0, 0, 0, 0 };
int ut = 0;
int t = 0;
short[] depth = shortDepth;
for (int u = 0; u < IMG_WIDTH; u++)
{
for (int v = 0; v < IMG_HEIGHT; v++)
{
if (depth[v * IMG_WIDTH + u] > 0)
{
ut += u;
t++;
}
}
}
ut /= t;
t = 0;
int vt = 0;
for (int v = 0; v < IMG_HEIGHT; v++)
{
if (depth[v * IMG_WIDTH + ut] > 0)
{
vt += v;
t++;
}
}
vt /= t;
int ve = 0;
for (int v = IMG_HEIGHT - 1; v >= 0; v--)
{
if (depth[v * IMG_WIDTH + ut] > 0)
{
ve = v;
break;
}
}
res[0] = ut;
res[1] = vt;
res[2] = ut;
res[3] = ve;
return res;
}
List<int> detect_body_upper_end(short[] depthHuman, int uc, int vc)
{
List<int> res = new List<int>(2) { 0, 0 };
int STEP = 3;
bool f = true;
int[] uf = { -STEP, -STEP, 0, 0, STEP, STEP, STEP };
int[] vf = { -STEP, 0, -STEP, STEP, 0, -STEP, STEP };
int ct = 0;
int uend = 0;
int vend = 0;
int CUBE = 3;
while (f && ct < 1000)
{
f = false;
for (int n = 0; n < uf.Length; n++)
{
int ur = uc + uf[n];
int vr = vc + vf[n];
int t = 0;
for (int i = -CUBE / 2; i <= CUBE / 2; i++)
{
for (int j = -CUBE / 2; j <= CUBE / 2; j++)
{
if (depthHuman[(vr + j) * IMG_WIDTH + ur + i] > 0)
{
t++;
}
}
}
if (t == CUBE * CUBE)
{
uc = ur;
vc = vr;
uend = ur;
vend = vr;
f = true;
break;
}
}
ct++;
}
res[0] = uend;
res[1] = vend;
return res;
}
List<int> detect_body_flat_end(short[] depthHuman, int uc, int vc)
{
List<int> res = new List<int>(2) { 0, 0 };
int STEP = 3;
bool f = true;
int[] uf = { -STEP, -STEP, 0, 0, STEP, STEP, STEP };
int[] vf = { 0, STEP, STEP, -STEP, STEP, -STEP, 0 };
int ct = 0;
int uend = 0;
int vend = 0;
int CUBE = 3;
while (f && ct < 1000)
{
f = false;
for (int n = 0; n < uf.Length; n++)
{
int ur = uc + uf[n];
int vr = vc + vf[n];
int t = 0;
for (int i = -CUBE / 2; i <= CUBE / 2; i++)
{
for (int j = -CUBE / 2; j <= CUBE / 2; j++)
{
if (depthHuman[(vr + j) * IMG_WIDTH + ur + i] > 0)
{
t++;
}
}
}
if (t == CUBE * CUBE)
{
uc = ur;
vc = vr;
uend = ur;
vend = vr;
f = true;
break;
}
}
ct++;
}
res[0] = uend;
res[1] = vend;
return res;
}
List<int> detect_body_lower_end(short[] depthHuman, int uc, int vc)
{
List<int> res = new List<int>(2) { 0, 0 };
int STEP = 3;
bool f = true;
int[] uf = { -STEP, -STEP, 0, 0, STEP, STEP, STEP };
int[] vf = { STEP, 0, STEP, -STEP, 0, STEP, -STEP };
int ct = 0;
int uend = 0;
int vend = 0;
int CUBE = 3;
while (f && ct < 1000)
{
f = false;
for (int n = 0; n < uf.Length; n++)
{
int ur = uc + uf[n];
int vr = vc + vf[n];
int t = 0;
for (int i = -CUBE / 2; i <= CUBE / 2; i++)
{
for (int j = -CUBE / 2; j <= CUBE / 2; j++)
{
if (depthHuman[(vr + j) * IMG_WIDTH + ur + i] > 0)
{
t++;
}
}
}
if (t == CUBE * CUBE)
{
uc = ur;
vc = vr;
uend = ur;
vend = vr;
f = true;
break;
}
}
ct++;
}
res[0] = uend;
res[1] = vend;
return res;
}
double get_leg_length(short[] shortDepth, int uc, int vc) // or imgDepth with commented code
{
double res = 0;
int uter = 0;
int vter = 0;
bool b = false;
for (int u = 0; u < IMG_WIDTH - 10; u++)
{
for (int v = IMG_HEIGHT - 1; v >= 5; v--)
{
int t = 0;
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 5; j++)
{
if (shortDepth[(v - j) * IMG_WIDTH + u + i] > 0)
{
t++;
}
}
}
if (t == 50)
{
uter = u + 5;
vter = v - 2;
b = true;
}
if (b) break;
}
if (b) break;
}
List<double> xyzc = depth_to_xyz(uc, vc, shortDepth[uc + vc * IMG_WIDTH]);
List<double> xyzter = depth_to_xyz(uter, vter, shortDepth[uter + vter * IMG_WIDTH]);
double length = Math.Sqrt(Math.Pow(xyzter[0] - xyzc[0], 2) + Math.Pow(xyzter[1] - xyzc[1], 2));
//cout << "Leg Length: " << length << endl;
res = length;
//*********************************************
return res;
}
List<double> get_flap_angle(short[] shortDepth, int uc, int vc, int ue, int ve)
{
List<double> res = new List<double>(2) { 0, 0 };
List<double> xyzc = depth_to_xyz(uc, vc, shortDepth[uc + vc * IMG_WIDTH]);
List<double> xyze = depth_to_xyz(ue, ve, shortDepth[ue + ve * IMG_WIDTH]);
/*********/
double u1 = xyze[0] - xyzc[0];
double u2 = xyze[1] - xyzc[1];
double u3 = xyze[2] - xyzc[2];
double A = m_gd[0];
double B = m_gd[1];
double C = -1;
double a = Math.Asin(Math.Abs(A * u1 + B * u2 + C * u3) / ((A * A + B * B + C / C) * (u1 * u1 + u2 * u2 + u3 * u3)));
if (a > 3.1415926 / 2)
{
a -= 3.1415926 / 2;
}
if (a < -3.1415926 / 2)
{
a += 3.1415926 / 2;
}
/***end***/
double angle = Math.Atan((xyze[2] - xyzc[2]) / (Math.Sqrt(Math.Pow(xyze[0] - xyzc[0], 2) + Math.Pow(xyze[1] - xyzc[1], 2))));
double legL = Math.Sqrt(Math.Pow(xyze[0] - xyzc[0], 2) + Math.Pow(xyze[1] - xyzc[1], 2) + Math.Pow(xyze[2] - xyzc[2], 2));
if (legL < 0.5 * m_legL)
{
angle = m_lastReliableDetection;
}
else
{
m_lastReliableDetection = angle;
}
res[0] = angle * 180 / 3.1415926;
res[1] = legL;
return res;
}
List<int> detect_upper_feet(short[] depthHuman)
{
List<int> res = new List<int>(2) { 0, 0 };
int UW = 32;
int VW = 24;
int US = UW / 8;
int VS = VW / 8;
List<int> uvs = new List<int>(2);
for (int u = 0; u <= IMG_WIDTH - UW; u += US)
{
for (int v = 0; v <= IMG_HEIGHT - VW; v += VS)
{
float r1 = 0;
for (int i = 0; i < VW; i++)
{
r1 += depthHuman[(v + i) * IMG_WIDTH + u + UW - 1] > 0 ? 1 : 0;
}
r1 /= VW;
float r2 = 0;
for (int i = 0; i < VW; i++)
{
r2 += depthHuman[(v + i) * IMG_WIDTH + u] > 0 ? 1 : 0;
}
r2 /= VW;
float r3 = 0;
for (int i = 0; i < UW; i++)
{
r3 += depthHuman[v * IMG_WIDTH + u + i] > 0 ? 1 : 0;
}
r3 /= UW;
float r4 = 0;
for (int i = 0; i < UW; i++)
{
r1 += depthHuman[(v + VW - 1) * IMG_WIDTH + u + i] > 0 ? 1 : 0;
}
r4 /= UW;
if (r1 >= 0 && r2 == 0 && r3 == 0 && r4 > 0 && v + VW / 2 < m_feetLowerPixel[1])
{
uvs[0] = u + UW;
uvs[1] = v + VW / 2;
}
}
}
int um = IMG_WIDTH;
int vm = 0;
for (int i = 0; i < uvs.Count / 2; i++)
{
if (uvs[2 * i] < um && uvs[2 * i] < IMG_WIDTH)
{
um = uvs[2 * i];
vm = uvs[2 * i + 1];
}
}
int xf = 0;
int yf = 0;
int tf = 0;
for (int u = 0; u < UW; u++)
{
for (int v = 0; v < VW; v++)
{
if (depthHuman[(vm + v) * IMG_WIDTH + um + u] > 0)
{
xf += um + u;
yf += vm + v;
tf++;
}
}
}
xf /= tf;
yf /= tf;
res[0] = xf;
res[1] = yf;
return res;
}
List<int> detect_raised_feet(short[] depthHuman, int uc, int vc)
{
List<int> res = new List<int>(2) { 0, 0 };
int uter = 0;
int vter = 0;
bool b = false;
for (int v = 2; v < vc; v++)
{
for (int u = 2; u < uc; u++)
{
int t = 0;
for (int i = -2; i < 3; i++)
{
for (int j = -2; j < 3; j++)
{
if (depthHuman[(v - j) * IMG_WIDTH + u + i] > 0)
{
t++;
}
}
}
if (t == 25)
{
uter = u;
vter = v;
b = true;
}
if (b) break;
}
if (b) break;
}
res[0] = uter;
res[1] = vter;
return res;
}
double get_flap_speed(List<double> lastFive, List<long> lastFiveTime)
{
double res = 0;
double path = (lastFive[lastFive.Count - 1] - lastFive[0]);
long time = (lastFiveTime[lastFiveTime.Count - 1] - lastFiveTime[0]);
double speed = path / time * 1000;
res = speed;
return res;
}
#endregion
#region task
public int ground_calibration(short[] shortDepth)
{
short[] depthArr = shortDepth;
depthArr = shift_arr(depthArr);
if (ifFlip == true)
{
depthArr = flip_array_vertical(shortDepth);
}
extract_floor_plane(depthArr);
m_ifGroundCalibrate = true;
return 0;
}
public int human_calibration(short[] shortDepth)
{
if (m_ifLoadGD == false)
{
load_gd("plane.txt");
}
int res = 0;
short[] depthArr = shortDepth;
depthArr = shift_arr(depthArr);
if (ifFlip == true)
{
depthArr = flip_array_vertical(shortDepth);
}
depthArr = filter_background(depthArr);
///------ get human center and leg length
m_humanCenter = detect_human_center(depthArr);
m_legL = get_leg_length(depthArr, m_humanCenter[0], m_humanCenter[1]);
detect_body_flat_end(depthArr, m_humanCenter[0], m_humanCenter[1]);
m_ifHumanCalibrate = true;
return res;
}
public int task(short[] shortDepth)
{
DateTime now1 = DateTime.Now;
short[] depthArr = shortDepth;
depthArr = shift_arr(depthArr);
if (ifFlip == true)
{
depthArr = flip_array_vertical(shortDepth);
}
depthArr = filter_background(depthArr);
///------ detect body ends
List<int> uvend = detect_body_upper_end(depthArr, m_humanCenter[0], m_humanCenter[1]);
List<double> angle = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend[0], uvend[1]);
List<int> uvend2 = detect_raised_feet(depthArr, m_humanCenter[0], m_humanCenter[1]);
List<double> angle2 = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend2[0], uvend2[1]);
//------ compute angle
double angleInput = angle[0];
double legLInput = angle[1];
if (m_lastReliableDetection > 30)
{
angleInput = angle2[0];
legLInput = angle2[1];
}
double integratedAngle = kalman_filter(angleInput, m_lastFive, m_legL, legLInput);
collect_latest_angles(integratedAngle);
m_angleLeg = integratedAngle;
m_speedLeg = get_flap_speed(m_lastFive, m_lastFiveTime);
//------ record and show angle
m_angleHistory.Add(m_angleLeg);
DateTime now2 = DateTime.Now;
return 0;
}
public short[] unique_task(short[] shortDepth)
{
DateTime now1 = DateTime.Now;
short[] depthArr = shortDepth;
depthArr = shift_arr(depthArr);
if (m_ifGroundCalibrate == false)
{
extract_floor_plane(depthArr);
}
depthArr = get_person_map(depthArr);
if (m_ifHuman == true)
{
m_humanCenter = detect_human_center(depthArr);
m_legL = get_leg_length(depthArr, m_humanCenter[0], m_humanCenter[1]);
depthArr = filter_background(depthArr);
List<int> uvend = detect_body_upper_end(depthArr, m_humanCenter[0], m_humanCenter[1]);
List<double> angle = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend[0], uvend[1]);
List<int> uvend2 = detect_raised_feet(depthArr, m_humanCenter[0], m_humanCenter[1]);
List<double> angle2 = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend2[0], uvend2[1]);
//------ compute angle
double angleInput = angle[0];
double legLInput = angle[1];
if (m_lastReliableDetection > 30)
{
angleInput = angle2[0];
legLInput = angle2[1];
}
double integratedAngle = kalman_filter(angleInput, m_lastFive, m_legL, legLInput);
collect_latest_angles(integratedAngle);
m_angleLeg = integratedAngle;
m_speedLeg = get_flap_speed(m_lastFive, m_lastFiveTime);
//------ record and show angle
m_angleHistory.Add(m_angleLeg);
}
DateTime now2 = DateTime.Now;
short[] res = depthArr;
return res;
}
public double get_angle()
{
return m_angleLeg;
}
public double get_speed()
{
return m_speedLeg;
}
#endregion
#region assistive functions
public short[] get_person_map(short[] depth)
{
short[] res = depth;
int XX = 20;
int YY = 20;
double xx = 4.0 / 20;
double yy = 4.0 / 20;
int[,] map = new int[YY, XX];
double fx_d = 594.21434211923247F;
double fy_d = 591.04053696870778F;
double cx_d = 339.30780975300314F;
double cy_d = 242.73913761751615F;
int u, v;
double A = m_gd[0];
double B = m_gd[1];
double C = m_gd[2];
for (int i = 0; i < IMG_SIZE; i++)
{
v = i / IMG_WIDTH;
u = i % IMG_WIDTH;
double df = depth[i] * 1.0 / 1000;
double x = (u - cx_d) * df / fx_d;
double z = -(v - cy_d) * df / fy_d;
double y = df;
if ( x > -2 && x < 2 && y > 1 && y < 4 && z > A * x + B * y + C + 0.10)
{
map[(int)(y / yy), (int)((x + 2) / xx)] = 1;
}
}
//for (int y = 0; y < 40; y++)
//{
// for (int x = 0; x < 40; x++)
// {
// Console.Write(map[y, x].ToString());
// }
// Console.Write("\n");
//}
//Console.Write("\n");
ConnectedComponent cc = new ConnectedComponent();
int[,] lmap = cc.connect_component(map, YY, XX);
int[] mt = new int[YY*XX];
for (int y = 0; y < YY; y++)
{
for (int x = 0; x < XX; x++)
{
mt[lmap[y, x]]++;
//Console.Write(lmap[y, x].ToString());
}
//Console.Write("\n");
}
int maxidx = 1;
int max = mt[1];
for (int i = 1; i < YY * XX; i++)
{
if (mt[i] > max)
{
maxidx = i;
max = mt[i];
}
}
if (max < 10)
{
m_ifHuman = false;
Console.WriteLine(max);
}
else
{
m_ifHuman = true;
}
for (int i = 0; i < IMG_SIZE; i++)
{
v = i / IMG_WIDTH;
u = i % IMG_WIDTH;
double df = depth[i] * 1.0 / 1000;
double x = (u - cx_d) * df / fx_d;
double z = -(v - cy_d) * df / fy_d;
double y = df;
if (x > -2 && x < 2 && y > 1 && y < 4 && z > A * x + B * y + C + 0.06)
{
if (lmap[(int)(y / yy), (int)((x + 2) / xx)] != maxidx)
{
res[i] = 0;
}
}
else
{
res[i] = 0;
}
}
return res;
}
List<double> depth_to_xyz(int u, int v, short depth)
{
List<double> res = new List<double>(3) { 0, 0, 0 };
double df = depth * 1.0 / 1000;
double fx_d = 594.21434211923247F;
double fy_d = 591.04053696870778F;
double cx_d = 339.30780975300314F;
double cy_d = 242.73913761751615F;
res[0] = (u - cx_d) * df / fx_d;
res[2] = -(v - cy_d) * df / fy_d;
res[1] = df;
return res;
}
List<double> get_plane_from_point_set(List<double> pt)
{
List<double> res = new List<double>(3) { 0, 0, 0 };
double xy = 0;
double xz = 0;
double yz = 0;
double x2 = 0;
double y2 = 0;
double x = 0;
double y = 0;
double z = 0;
double n = pt.Count / 3;
for (int i = 0; i < pt.Count / 3; i++)
{
double xf = pt[3 * i];
double yf = pt[3 * i + 1];
double zf = pt[3 * i + 2];
x += xf;
y += yf;
z += zf;
//cout << xf << " " << yf << " " << zf << endl;
x2 += xf * xf;
y2 += yf * yf;
xy += xf * yf;
xz += xf * zf;
yz += yf * zf;
}
double[,] m1Arr = new double[3, 3] {{x2, xy, x},
{xy, y2, y},
{x, y, n}};
double[,] m1ArrInv = MatrixInv.InvertMatrix(m1Arr, 3);
//for (int i = 0; i < 3; i++)
//{
// for (int j = 0; j < 3; j++)
// {
// Console.Write(m1Arr[i, j].ToString() + " ");
// }
// Console.WriteLine(" ");
//}
//for (int i = 0; i < 3; i++)
//{
// for (int j = 0; j < 3; j++)
// {
// Console.Write(m1ArrInv[i, j].ToString() + " ");
// }
// Console.WriteLine(" ");
//}
double[,] m1i = m1ArrInv;
double[,] m2 = new double[3, 1] {{xz},
{yz},
{z}};
res[0] = m1i[0, 0] * m2[0, 0] + m1i[0, 1] * m2[1, 0] + m1i[0, 2] * m2[2, 0];
res[1] = m1i[1, 0] * m2[0, 0] + m1i[1, 1] * m2[1, 0] + m1i[1, 2] * m2[2, 0];
res[2] = m1i[2, 0] * m2[0, 0] + m1i[2, 1] * m2[1, 0] + m1i[2, 2] * m2[2, 0];
Console.WriteLine("gd:" + res[0].ToString() + " " + res[1].ToString() + " " + res[2].ToString());
return res;
}
int collect_latest_angles(double angle)
{
m_lastFive.Add(angle);
DateTime now = DateTime.Now;
int time = now.Second * 1000 + now.Millisecond;
m_lastFiveTime.Add(time);
if (m_lastFive.Count > 5)
{
m_lastFive.RemoveAt(0);
m_lastFiveTime.RemoveAt(0);
}
return m_lastFive.Count;
}
double kalman_filter(double angle, List<double> lastFive, double legLength, double observeLength)
{
double res;
if (m_lastFive.Count >= 5)
{
double probObserve = 1 - Math.Pow(Math.Abs(observeLength / legLength - 1), 3);
double speed = (lastFive[4] - lastFive[3]) + (lastFive[4] - lastFive[3]) - (lastFive[3] - lastFive[2]);
double predictAngle = lastFive[4] + speed;
double integratedAngle = probObserve * angle + (1 - probObserve) * predictAngle;
res = integratedAngle;
}
else
{
res = angle;
}
return res;
}
public short[] flip_array_vertical(short[] shortDepth)
{
short[] res = new short[IMG_SIZE];
for (int u = 0; u < IMG_WIDTH; u++)
{
for (int v = 0; v < IMG_HEIGHT; v++)
{
int id1 = v * IMG_WIDTH + u;
int id2 = (IMG_HEIGHT - v - 1) * IMG_WIDTH + u;
res[id1] = shortDepth[id2];
}
}
return res;
}
public short[] change_arr_direction(short[] a, int dir)
{
short[] r = new short[IMG_SIZE];
if (dir == 0)
return a;
if (dir > 0)
{
for (int u = 0; u < IMG_WIDTH; u++)
{
for (int v = 0; v < IMG_HEIGHT; v++)
{
r[v + u * IMG_HEIGHT] = a[u + v * IMG_WIDTH];
}
}
}
if (dir < 0)
{
for (int v = 0; v < IMG_HEIGHT; v++)
{
for (int u = 0; u < IMG_WIDTH; u++)
{
r[u + v * IMG_WIDTH] = a[v + u * IMG_HEIGHT];
}
}
}
return r;
}
short[] shift_arr(short[] depth)
{
short[] r = new short[depth.Length];
for (int i = 0; i < depth.Length; i++)
{
r[i] = (short)(depth[i] >> 3);
}
return r;
}
int load_gd(string fileName)
{
string[] lines = System.IO.File.ReadAllLines(fileName);
m_gd[0] = Convert.ToDouble(lines[0]);
m_gd[1] = Convert.ToDouble(lines[1]);
m_gd[2] = Convert.ToDouble(lines[2]);
m_ifLoadGD = true;
return 0;
}
#endregion
}
class MyContainer
{
public double[][] m_double;
public int[] m_int;
}
class MatrixInv
{
/*
* Perform LUP decomposition on a matrix A.
* Return L and U as a single matrix(double[][]) and P as an array of ints.
* We implement the code to compute LU "in place" in the matrix A.
* In order to make some of the calculations more straight forward and to
* match Cormen's et al. pseudocode the matrix A should have its first row and first columns
* to be all 0.
* */
public static MyContainer LUPDecomposition(double[][] A)
{
int n = A.Length - 1;
/*
* pi represents the permutation matrix. We implement it as an array
* whose value indicates which column the 1 would appear. We use it to avoid
* dividing by zero or small numbers.
* */
int[] pi = new int[n + 1];
double p = 0;
int kp = 0;
int pik = 0;
int pikp = 0;
double aki = 0;
double akpi = 0;
//Initialize the permutation matrix, will be the identity matrix
for (int j = 0; j <= n; j++)
{
pi[j] = j;
}
for (int k = 0; k <= n; k++)
{
/*
* In finding the permutation matrix p that avoids dividing by zero
* we take a slightly different approach. For numerical stability
* We find the element with the largest
* absolute value of those in the current first column (column k). If all elements in
* the current first column are zero then the matrix is singluar and throw an
* error.
* */
p = 0;
for (int i = k; i <= n; i++)
{
if (Math.Abs(A[i][k]) > p)
{
p = Math.Abs(A[i][k]);
kp = i;
}
}
if (p == 0)
{
throw new Exception("singular matrix");
}
/*
* These lines update the pivot array (which represents the pivot matrix)
* by exchanging pi[k] and pi[kp].
* */
pik = pi[k];
pikp = pi[kp];
pi[k] = pikp;
pi[kp] = pik;
/*
* Exchange rows k and kpi as determined by the pivot
* */
for (int i = 0; i <= n; i++)
{
aki = A[k][i];
akpi = A[kp][i];
A[k][i] = akpi;
A[kp][i] = aki;
}
/*
* Compute the Schur complement
* */
for (int i = k + 1; i <= n; i++)
{
A[i][k] = A[i][k] / A[k][k];
for (int j = k + 1; j <= n; j++)
{
A[i][j] = A[i][j] - (A[i][k] * A[k][j]);
}
}
}
MyContainer res = new MyContainer();
res.m_double = A;
res.m_int = pi;
return res;
}
/*
* Given L,U,P and b solve for x.
* Input the L and U matrices as a single matrix LU.
* Return the solution as a double[].
* LU will be a n+1xm+1 matrix where the first row and columns are zero.
* This is for ease of computation and consistency with Cormen et al.
* pseudocode.
* The pi array represents the permutation matrix.
* */
public static double[] LUPSolve(double[][] LU, int[] pi, double[] b)
{
int n = LU.Length - 1;
double[] x = new double[n + 1];
double[] y = new double[n + 1];
double suml = 0;
double sumu = 0;
double lij = 0;
/*
* Solve for y using formward substitution
* */
for (int i = 0; i <= n; i++)
{
suml = 0;
for (int j = 0; j <= i - 1; j++)
{
/*
* Since we've taken L and U as a singular matrix as an input
* the value for L at index i and j will be 1 when i equals j, not LU[i][j], since
* the diagonal values are all 1 for L.
* */
if (i == j)
{
lij = 1;
}
else
{
lij = LU[i][j];
}
suml = suml + (lij * y[j]);
}
y[i] = b[pi[i]] - suml;
}
//Solve for x by using back substitution
for (int i = n; i >= 0; i--)
{
sumu = 0;
for (int j = i + 1; j <= n; j++)
{
sumu = sumu + (LU[i][j] * x[j]);
}
x[i] = (y[i] - sumu) / LU[i][i];
}
return x;
}
/*
* Given an nXn matrix A, solve n linear equations to find the inverse of A.
* */
public static double[,] InvertMatrix(double[,] Ainput, int ndim)
{
int n = ndim;
double[][] A = new double[n][];
for (int i = 0; i < n; i++)
{
A[i] = new double[n];
for (int j = 0; j < n; j++)
{
A[i][j] = Ainput[i, j];
}
}
//e will represent each column in the identity matrix
double[] e;
//x will hold the inverse matrix to be returned
double[][] x = new double[n][];
double[,] res = new double[n, n];
for (int i = 0; i < n; i++)
{
x[i] = new double[A[i].Length];
}
/*
* solve will contain the vector solution for the LUP decomposition as we solve
* for each vector of x. We will combine the solutions into the double[][] array x.
* */
double[] solve;
//Get the LU matrix and P matrix (as an array)
MyContainer results = LUPDecomposition(A);
double[][] LU = results.m_double;
int[] P = results.m_int;
/*
* Solve AX = e for each column ei of the identity matrix using LUP decomposition
* */
for (int i = 0; i < n; i++)
{
e = new double[A[i].Length];
e[i] = 1;
solve = LUPSolve(LU, P, e);
for (int j = 0; j < solve.Length; j++)
{
x[j][i] = solve[j];
res[j, i] = x[j][i];
}
}
return res;
}
}
public class ConnectedComponent
{
int xSize = 10;
int ySize = 10;
int[,] I;// = new byte[ySize, xSize]; //Original Image
int[,] Lfirst;// = new int [ySize,xSize]; //Labels after first step
int[,] Lmerge;// = new int [ySize,xSize]; //Labels after merging
int[,] Lsorted;// = new int [ySize,xSize]; //Labels after sorting
bool ifeight = true;
Int32 i, x, y, label, no_of_merge_passes;
public int[,] connect_component(int[,] map, int yy, int xx)
{
xSize = xx;
ySize = yy;
I = map;
Lfirst = new int[yy, xx];
Lmerge = new int[yy, xx];
Lsorted = new int[yy, xx];
fill_Lfirst();
fill_Lmerge();
fill_Lsorted();
//Random random = new Random();
//for (y = 0; y < ySize; y++)
//{
// for (x = 0; x < xSize; x++)
// {
// Int32 noise = random.Next() % 3 - 1;
// noise += I[y, x];
// if (noise < 0) I[y, x] = 0;
// else if (noise > 9) I[y, x] = 9;
// else I[y, x] = (int)noise;
// }
//}
//for (y = 0; y < ySize; y++)
//{
// for (x = 0; x < xSize; x++)
// {
// Lfirst[y, x] = Lmerge[y, x] = Lsorted[y, x] = I[y, x] = 0;
// }
//}
//for (y = 0; y < ySize; y++)
//{
// for (x = 0; x < xSize; x++)
// {
// Console.Write(Lsorted[y, x]);
// }
// Console.Write("\n");
//}
//Console.Write("\n");
return Lsorted;
}
private void fill_Lfirst()
{
Lfirst[0, 0] = label = 1;
for (x = 1; x < xSize; x++)
if (I[0, x] == I[0, x - 1]) Lfirst[0, x] = Lfirst[0, x - 1]; //label from left
else Lfirst[0, x] = ++label;
for (y = 1; y < ySize; y++)
{
if (I[y, 0] == I[y - 1, 0]) Lfirst[y, 0] = Lfirst[y - 1, 0]; //label from upstairs
else Lfirst[y, 0] = ++label;
for (x = 1; x < xSize; x++)
if (I[y, x] == I[y - 1, x]) Lfirst[y, x] = Lfirst[y - 1, x]; //label from upstairs
else if (I[y, x] == I[y, x - 1]) Lfirst[y, x] = Lfirst[y, x - 1]; //label from left
else Lfirst[y, x] = ++label;
}
}
private void fill_Lmerge()
{
for (y = 0; y < ySize; y++)
for (x = 0; x < xSize; x++)
Lmerge[y, x] = Lfirst[y, x]; //just copy
no_of_merge_passes = 0;
do
{
i = 0; no_of_merge_passes++;
for (y = 0; y < ySize; y++)
for (x = 0; x < xSize; x++)
{
if (x < xSize - 1 && I[y, x] == I[y, x + 1]) //merge with right neighbor ?
if (Lmerge[y, x] != Lmerge[y, x + 1])
{
Lmerge[y, x] = Lmerge[y, x + 1] =
Math.Min(Lmerge[y, x], Lmerge[y, x + 1]); i++;
}
if (y < ySize - 1 && I[y, x] == I[y + 1, x]) //merge with lower neighbor ?
if (Lmerge[y, x] != Lmerge[y + 1, x])
{
Lmerge[y, x] = Lmerge[y + 1, x] =
Math.Min(Lmerge[y, x], Lmerge[y + 1, x]); i++;
}
if (x > 0 && I[y, x] == I[y, x - 1]) //merge with left neighbor ?
if (Lmerge[y, x] != Lmerge[y, x - 1])
{
Lmerge[y, x] = Lmerge[y, x - 1] =
Math.Min(Lmerge[y, x], Lmerge[y, x - 1]); i++;
}
if (y > 0 && I[y, x] == I[y - 1, x]) //merge with upper neighbor ?
if (Lmerge[y, x] != Lmerge[y - 1, x])
{
Lmerge[y, x] = Lmerge[y - 1, x] =
Math.Min(Lmerge[y, x], Lmerge[y - 1, x]); i++;
}
if (ifeight) //8-Neighborhood ?
{
if (x < xSize - 1 && y < ySize - 1 && I[y, x] == I[y + 1, x + 1]) //merge with right lower neighbor ?
if (Lmerge[y, x] != Lmerge[y + 1, x + 1])
{
Lmerge[y, x] = Lmerge[y + 1, x + 1] =
Math.Min(Lmerge[y, x], Lmerge[y + 1, x + 1]); i++;
}
if (x > 0 && y < ySize - 1 && I[y, x] == I[y + 1, x - 1]) //merge with left lower neighbor ?
if (Lmerge[y, x] != Lmerge[y + 1, x - 1])
{
Lmerge[y, x] = Lmerge[y + 1, x - 1] =
Math.Min(Lmerge[y, x], Lmerge[y + 1, x - 1]); i++;
}
if (x > 0 && y > 0 && I[y, x] == I[y - 1, x - 1]) //merge with left upper neighbor ?
if (Lmerge[y, x] != Lmerge[y - 1, x - 1])
{
Lmerge[y, x] = Lmerge[y - 1, x - 1] =
Math.Min(Lmerge[y, x], Lmerge[y - 1, x - 1]); i++;
}
if (x < xSize - 1 && y > 0 && I[y, x] == I[y - 1, x + 1]) //merge with right upper neighbor ?
if (Lmerge[y, x] != Lmerge[y - 1, x + 1])
{
Lmerge[y, x] = Lmerge[y - 1, x + 1] =
Math.Min(Lmerge[y, x], Lmerge[y - 1, x + 1]); i++;
}
}
}
} while (i > 0);
}
private void fill_Lsorted()
{
int[,] histogram = new int[label + 1, 2]; //histogram with 2 lines
for (y = 0; y < ySize; y++)
for (x = 0; x < xSize; x++)
histogram[Lmerge[y, x], 0]++; //first line filling
int max, imax = 0, newlabel = 1;
do //sorted new labels from 1 = biggest, 2 = second etc.
{
max = 0;
for (i = 1; i <= label; i++)
{
if (histogram[i, 1] > 0) continue; //already sorted
if (histogram[i, 0] > max) { max = histogram[i, 0]; imax = i; }
}
if (max > 0) histogram[imax, 1] = newlabel++;
} while (max > 0);
for (y = 0; y < ySize; y++) //apply new labels
for (x = 0; x < xSize; x++)
Lsorted[y, x] = histogram[Lmerge[y, x], 1] - 1;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment