summaryrefslogtreecommitdiffstats
path: root/wiimote_ir_smoothing/C_3DPointSmoother.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'wiimote_ir_smoothing/C_3DPointSmoother.cpp')
-rw-r--r--wiimote_ir_smoothing/C_3DPointSmoother.cpp240
1 files changed, 240 insertions, 0 deletions
diff --git a/wiimote_ir_smoothing/C_3DPointSmoother.cpp b/wiimote_ir_smoothing/C_3DPointSmoother.cpp
new file mode 100644
index 0000000..529f9e8
--- /dev/null
+++ b/wiimote_ir_smoothing/C_3DPointSmoother.cpp
@@ -0,0 +1,240 @@
+#include <map>
+#include <iostream>
+#include <iomanip>
+
+#include "C_3DPointSmoother.h"
+
+using namespace std;
+
+void C_3DPointSmoother::SetSimpleMovingAverage(int f_iWindowSize)
+{
+ //reset smoothing stuff for Simple moving average smoothing
+ m_Method = SimpMovingAvg;
+ m_iWindowSize = f_iWindowSize;
+ m_iSampCount = 0;
+ m_St = Vect3D_t(0.0, 0.0, 0.0);
+ m_SampWindow.clear();
+}
+
+void C_3DPointSmoother::SetExponentialMovingAverage(double f_dExponent)
+{
+ m_Method = ExpMovingAvg;
+ m_dExponent = f_dExponent;
+ m_iSampCount = 0;
+ m_St = Vect3D_t(0.0, 0.0, 0.0);
+}
+
+Vect3D_t C_3DPointSmoother::SmoothSample_SimpMovingAvg(const Vect3D_t &f_Xt)
+{
+ //add sample to the window
+ m_SampWindow.push_front(f_Xt);
+
+ if (m_iSampCount < m_iWindowSize)
+ {
+ //we don't have enough cmopute the average of the points currently in the window
+ Vect3D_t l_sum;
+ for (std::list<Vect3D_t>::iterator it = m_SampWindow.begin(); it != m_SampWindow.end(); it++)
+ l_sum += *it;
+ return l_sum / (double)m_SampWindow.size();
+ }
+ else
+ {
+ //there are mote than m_iWindowSize samples, incrementally update smoothed sample
+ Vect3D_t l_Xtk = m_SampWindow.back(); //pop the last point from the (window) list
+ m_SampWindow.pop_back();
+ return m_St + ((f_Xt - l_Xtk)/(double)m_iWindowSize);
+ }
+}
+
+Vect3D_t C_3DPointSmoother::SmoothSample_ExpMovingAvg(const Vect3D_t &f_Xt)
+{
+ if (m_iSampCount == 0)
+ return f_Xt;
+ else
+ return m_St + m_dExponent * (f_Xt - m_St);
+}
+
+Vect3D_t C_3DPointSmoother::Smooth(const Vect3D_t &f_Xt)
+{
+ switch(m_Method)
+ {
+ case SimpMovingAvg:
+ m_St = SmoothSample_SimpMovingAvg(f_Xt);
+ break;
+
+ case ExpMovingAvg:
+ m_St = SmoothSample_ExpMovingAvg(f_Xt);
+ break;
+ }
+
+ m_iSampCount++;
+ return m_St;
+}
+
+
+
+#include "wiimote_state.h"
+
+bool FindSensorBarDots(wiimote_state::ir &f_IR, Vect3D_t f_Dots[2])
+{
+ RawDot l_rd[4];
+
+ int n = 0;
+ for(int i = 0; i < 4; i++)
+ {
+ if (f_IR.Dot[i].bFound)
+ {
+ l_rd[n].rx = f_IR.Dot[i].RawX;
+ l_rd[n].ry = f_IR.Dot[i].RawY;
+ n++;
+
+ }
+ }
+
+ ////print raw dots
+ //cout << "Raw IR dots:" << endl;
+ //for (int i=0; i<4; i++)
+ //{
+ // if (i < n)
+ // {
+ // cout << "\t" << l_rd[i] << SPACES << endl;
+ // }
+ // else
+ // {
+ // cout << "\t-- -- --" << SPACES << endl;
+ // }
+ //}
+
+ return FindSensorBarDots(l_rd, n, f_Dots);
+}
+
+
+bool FindSensorBarDots(RawDot *f_rd, int f_iNumdots, Vect3D_t f_Dot[2])
+{
+ //the ends of the sensorbar contain more 3 to 5 ir leds each. the wiimote
+ //occaisonally detects individual ir leds at the same sensorbar end
+ //the ir dots have to be mapped to the ends of the sensor bar, so when we
+ //have more than 2 dots we have to group them into two groups
+ int l_GroupId[4] = {-1, -1, -1, -1};
+
+ if (f_iNumdots < 2)
+ {
+ //not enough dots
+ return false;
+ }
+ else if (f_iNumdots == 2)
+ {
+ //two dots (easy case)
+ l_GroupId[0] = 0;
+ l_GroupId[1] = 1;
+ //sort the dots below
+ }
+ else //more than 2 dots...
+ {
+ //group dots that are close to each other and compute their average position
+ //first compute the squared distance of all dot pairs and then groupe them
+ //into two groups based on their proximities
+ typedef pair<int,int> intpair;
+ map<int, intpair> m_Distances; //the squared distances between the pair of dots
+
+ //iterate through all pairs and compute their squared distance
+ for(int i=0; i<f_iNumdots; i++)
+ {
+ for(int j=i+1; j<f_iNumdots; j++)
+ {
+ RawDot d = f_rd[i] - f_rd[j];
+ int dist2 = d.rx*d.rx + d.ry*d.ry;
+ m_Distances[dist2] = intpair(i,j);
+ }
+ }
+
+ //first assign the two dots that are furthest apart into two seperate groups then
+ //iterate through the distances from smallest to largest and assign pairs of
+ //dots closest to each other to a group. for 4 dots there are 6 pairs, for 3 dots 3 pairs
+ //continue until all points are grouped into two groups
+ int l_iDotsLeft = f_iNumdots; //number of dots not assigned to a group
+ //get the pair of dots that are furthest away
+ intpair l_dotpair = (--m_Distances.end())->second;
+ l_GroupId[l_dotpair.first] = 0;
+ l_GroupId[l_dotpair.second] = 1;
+ l_iDotsLeft -= 2;
+
+ for(map<int, intpair>::iterator it = m_Distances.begin();
+ it != m_Distances.end() && l_iDotsLeft > 0; it++)
+ {
+ //check if the dots in the pair are assigned a group id
+ intpair dots = it->second;
+
+ //we can only add a dot to a group of the other dot in the pair is already
+ //assigned a group id. If we find a pair of dots of which no dot is assigned
+ //a group id, then we ignore them. If the latter case occurs it means that
+ //the pair is closer to each other then the two initial dots, which means
+ //that we are probably not processing ir dots captured from the sensor bar
+ if (l_GroupId[dots.first] != -1 && l_GroupId[dots.second] == -1)
+ {
+ //second belongs to the same group assigned to first
+ l_GroupId[dots.second] = l_GroupId[dots.first];
+ l_iDotsLeft--;
+ }
+ else if (l_GroupId[dots.first] == -1 && l_GroupId[dots.second] != -1)
+ {
+ //first belongs to the same group assigned to second
+ l_GroupId[dots.first] = l_GroupId[dots.second];
+ l_iDotsLeft--;
+ }
+ }
+ }
+
+ //calculate the average dots
+ int l_iDotsPerGroup[2] = {0,0};
+
+ f_Dot[0] = f_Dot[1] = Vect3D_t(0.0, 0.0, 0.0);
+ for (int i=0; i<f_iNumdots; i++)
+ {
+ if (l_GroupId[i] > -1)
+ {
+ f_Dot[l_GroupId[i]] += Vect3D_t((double)f_rd[i].rx, (double)f_rd[i].ry, 0.0);
+ l_iDotsPerGroup[l_GroupId[i]]++;
+ }
+ }
+ f_Dot[0] /= (double)l_iDotsPerGroup[0];
+ f_Dot[1] /= (double)l_iDotsPerGroup[1];
+
+ bool swapped = false;
+
+ //sort the dots such that f_Dot[0] is the leftmost
+ if (f_Dot[0].x > f_Dot[1].x || (f_Dot[0].x == f_Dot[1].x && f_Dot[0].y > f_Dot[1].y))
+ {
+ //swap
+ Vect3D_t tmp = f_Dot[0];
+ f_Dot[0] = f_Dot[1];
+ f_Dot[1] = tmp;
+ swapped = true;
+ }
+
+ //print debug stuff
+ int gid = swapped ? 1 : 0;
+
+ cout.fill(' ');
+
+ cout << setw(32) << "Group 0:" << "Group 1:" << endl;
+ for (int i = 0; i < 4; i++)
+ {
+ if (l_GroupId[i] == gid && i < f_iNumdots)
+ {
+ cout << setw(32) << f_rd[i] << setw(32) << "------" << endl;
+ }
+ else if (i < f_iNumdots)
+ {
+ cout << setw(32) << "------" << setw(32) << f_rd[i] << endl;
+ }
+ else
+ {
+ cout << setw(32) << "------" << setw(32) << "------" << endl;
+ }
+ }
+ cout << endl << setw(32) << "Average:" << endl;
+ cout << setw(32) << f_Dot[0] << setw(32) << f_Dot[1] << endl << endl;
+
+ return true;
+} \ No newline at end of file