ITG3200::ITG3200() {
spn("Init ITG-3200 starting...");
readI2C(GYRADDR, 0x00, 1, buffer); // Who am I?
sp("ITG-3200 ID = ");
spln((int) buffer[0]);
readI2C(GYRADDR, 0x15, 2, buffer);
// Sample rate divider is 1 + 1 = 2, so 1000 Hz / 2 = 500 Hz
buffer[0] = 1;
// Set FS_SEL = 3 as recommended on DS p. 24.
// Set DLPF_CFG = 3. This signifies a 1 kHz internal sample rate with a 42
// Hz LPF bandwidth, which should be low enough to filter out the motor
// vibrations.
buffer[1] = (3 << 3) | 3; // FS_SEL is on bits 4 and 3.
writeI2C(GYRADDR, 0x15, 2, buffer);
// Set to use X gyro as clock reference as recommended on DS p. 27.
buffer[0] = 1;
writeI2C(GYRADDR, 0x3e, 1, buffer);
spln("ITG-3200 configured!");
// Zero buffer.
for (int i=0; i<6; i++) {
buffer[i] = 0;
}
// Low-pass filter.
#ifdef GYRO_LPF_DEPTH
lpfIndex = 0;
for (int i=0; i<3; i++)
for (int j=0; j<GYRO_LPF_DEPTH; j++)
lpfVal[i][j] = 0;
#endif // GYRO_LPF_DEPTH
for (int i=0; i<3; i++) {
gZero[i] = 0;
gVec[i] = 0;
angle[i] = 0;
}
calibrated = false;
}
void InferenceEngineLoopyBP::initializePotentials(Beliefs& potentials, FeatureGenerator* fGen,
DataSequence *X, Model* m, iMatrix adjMat, int seqLabel, bool bUseStatePerNodes)
{
initializeBeliefs(potentials, X, m, adjMat);
int V = m->getNumberOfViews();
int T = X->length();
int nbNodes = V*T;
featureVector vecFeatures;
feature *f;
dVector *lambda = m->getWeights();
double val;
// Singleton potentials
for(int xi=0; xi<nbNodes; xi++) {
fGen->getFeatures(vecFeatures,X,m,xi,-1,seqLabel);
f = vecFeatures.getPtr();
for(int k=0; k<vecFeatures.size(); k++, f++) {
val = (*lambda)[f->globalId] * f->value;
potentials.belStates[xi][f->nodeState] += val;
}
}
// Pairwise potentials. Values are symmetrical: pot(xi,xj)==pot(xj,xi).
for(int xi=0; xi<nbNodes; xi++) {
for(int xj=xi+1; xj<nbNodes; xj++) {
if( !adjMat(xi,xj) ) continue;
fGen->getFeatures(vecFeatures,X,m,xj,xi,seqLabel);
f = vecFeatures.getPtr();
for(int k=0; k<vecFeatures.size(); k++, f++) {
val = (*lambda)[f->globalId] * f->value;
potentials.belEdges[adjMat(xi,xj)-1](f->prevNodeState, f->nodeState) += val;
}
}
}
// For MV-LDCRF
if( !bUseStatePerNodes ) return;
if( !m->isMultiViewMode() ) {
iMatrix* pStatesPerNode = m->getStateMatrix(X);
for(int xi=0; xi<nbNodes; xi++)
for(int h=0; h<m->getNumberOfStates(); h++)
if( pStatesPerNode->getValue(h,xi)==0 )
potentials.belStates[xi][h] = -INF_VALUE;
}
else {
std::vector<iMatrix> statesPerNodeMV;
for(int v=0; v<m->getNumberOfViews(); v++) {
iMatrix spn(m->getNumberOfStatesMV(v),X->length());
for(int t=0; t<X->length(); t++) {
for(int h=0; h<m->getNumberOfStatesMV(v); h++) {
spn(t,h) = m->getStatesPerLabelMV(v)(h,X->getStateLabels()->getValue(t));
}
}
statesPerNodeMV.push_back(spn);
}
for(int xi=0; xi<nbNodes; xi++) {
int v = xi/T; int t = xi%T;
for(int h=0; h<m->getNumberOfStatesMV(v); h++)
if(statesPerNodeMV[v](t,h)==0)
potentials.belStates[xi][h] = -INF_VALUE;
}
}
}
