void SiftMatrix::checkWithConfig() {
for(Data::iterator li = data.begin(); li!=data.end(); ++li) {
Layer &l = li->second;
for(unsigned o = 0; o<l.size(); o++)
for(unsigned o2 = 0; o2<l.size(); ++o2) {
DDModel::Node *u = config.ranking.GetRank(li->first)->order[o],
*v = config.ranking.GetRank(li->first)->order[o2];
dgassert(getCrossings(u,v,false)==weigh(uvcross(u,v,true,false)));
dgassert(getCrossings(u,v,true)==weigh(uvcross(u,v,false,true)));
}
}
}
float Candidate::WeightedScore(const PointCloud &pc, float epsilon,
float normalThresh) const
{
float score = 0;
#pragma omp parallel for schedule(static) reduction(+:score)
for(intptr_t i = 0; i < (intptr_t)m_indices->size(); ++i)
score += weigh(m_shape->Distance(pc[(*m_indices)[i]].pos), epsilon);
return score;
}
void SiftMatrix::recompute() {
for(Config::Ranks::iterator ri = config.ranking.begin(); ri!=config.ranking.end(); ++ri) {
Rank *rank = *ri;
unsigned r = config.ranking.IndexOfIter(ri);
unsigned wid = rank->order.size();
Layer &l = data[r];
l.resize(wid);
for(unsigned o = 0; o<wid; ++o) {
l[o].resize(wid);
for(unsigned o2 = 0; o2<wid; ++o2) {
CrossCount cr = make_pair(weigh(uvcross(rank->order[o],rank->order[o2],true,false)),
weigh(uvcross(rank->order[o],rank->order[o2],false,true)));
dgassert(cr.first>=0);
dgassert(cr.second>=0);
l[o][o2] = cr;
}
}
}
}
void main(void)
{
// Initialise system
setup();
// used for debugging.
TRISBbits.RB4 = 0;
PORTBbits.RB4 = 0;
// Loop until the system is turned off
while (req_state & POWER_ON)
{
int serial_data;
switch (cur_state)
{
case ST_WEIGH: weigh(); break;
case ST_COUNT_I:
case ST_COUNT_F: count(); break;
case ST_CALIBRATE: calibrate(); break;
}
serial_data = parseSerial();
if (serial_data != RS232_NO_DATA)
{
RS232writeByte(COMM_DEBUG);
RS232writeByte(cur_state);
RS232writeByte(disp_type);
//RS232writeByte(serial_data);
}
if (req_state != ST_NONE)
{
// int timeout = 0xFFFF;
// int serial_return;
cur_state = req_state;
// Send Change to GUI.
if (!st_chng_rs232_flag)
{
if (cur_state == ST_COUNT_F)
{
RS232sendData_b_i(COMM_CHANGE_STATE, cur_state, number_items);
}
else
{
RS232sendData_b(COMM_CHANGE_STATE, cur_state);
}
}
st_chng_rs232_flag = 0;
req_state = ST_NONE;
}
}
powerDown(); // Save state and power down.
}
void SiftMatrix::updateIns(DDModel::Node *v,DDModel::Node *x) {
bool toLeft = gd<DDNode>(x).order<gd<DDNode>(v).order;
for(DDModel::inedge_iter uxi = x->ins().begin(); uxi!=x->ins().end(); ++uxi) {
DDModel::Node *ux = (*uxi)->other(x);
for(DDModel::inedge_iter uvi = v->ins().begin(); uvi!=v->ins().end(); ++uvi) {
DDModel::Node *uv = (*uvi)->other(v);
if(gd<DDNode>(ux).order==gd<DDNode>(uv).order)
continue;
unsigned cost = weigh(Crossings(*uxi,*uvi));
dgassert(cost>=0);
if(toLeft) {
setCrossings(uv,ux,true,getCrossings(uv,ux,true) -cost);
setCrossings(ux,uv,true,getCrossings(ux,uv,true)+cost);
}
else {
setCrossings(ux,uv,true,getCrossings(ux,uv,true)-cost);
setCrossings(uv,ux,true,getCrossings(uv,ux,true)+cost);
}
}
}
}
