Created
August 27, 2014 15:55
-
-
Save jagwire/e7e6db362580f1859557 to your computer and use it in GitHub Desktop.
Eric Is Stupid
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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