BeliefValuePairPool.cpp

Go to the documentation of this file.

#include "BeliefValuePairPool.h"
#include <exception>
using namespace std;


REAL_VALUE BeliefValuePairPool::getValue(SharedPointer<Belief>& b) 
{
        DEBUG_TRACE(cout << "UpperBoundBVpair::getValue" << endl;);

        REAL_VALUE val, min_val;
        REAL_VALUE ratio, min_ratio;
        vector<SparseVector_Entry>::const_iterator  bi, bend;

        vector<SparseVector_Entry>::const_iterator  ci, cend;

        SparseVector tmp, bhat;
        REAL_VALUE cval;
        REAL_VALUE inner_cornerPoints_b;
        REAL_VALUE inner_cornerPoints_c;
        bool bdone = false;

        inner_cornerPoints_b = inner_prod( cornerPoints, *b );

        
        DEBUG_TRACE( cout << "cornerPoints"<< endl; );
        DEBUG_TRACE( cornerPoints.write(cout) << endl; );
        DEBUG_TRACE( cout << "b"<< endl; );
        DEBUG_TRACE( b->write(cout) << endl; );

        DEBUG_TRACE( cout << "inner_cornerPoints_b "<< inner_cornerPoints_b << endl; );

        min_val = inner_cornerPoints_b;

        for(list<SharedPointer<BeliefValuePair> >::iterator iter = points.begin(); iter!=points.end(); iter ++) 
        {

                if ((*iter)->disabled) 
                {
                        //cout << "skipping" << endl;
                        continue;
                }

                belief_vector& c = *((*iter)->b);
                cval = (*iter)->v;


                //cout << "c.size() " << c.size() << " cval " << cval << endl;

                DEBUG_TRACE( cout << "UpperBound::getValue:1 " << " pr->version: " << (*iter)->version << " cornerPointsVersion " << cornerPointsVersion << endl;);
                if((*iter)->version == cornerPointsVersion) 
                {
                        DEBUG_TRACE( cout << "UpperBound::getValue:2 " << endl; );
                        inner_cornerPoints_c = (*iter)->innver_cornerPoints;
                }
                else 
                {
                        DEBUG_TRACE( cout << "UpperBound::getValue:3 " << endl; );

                        inner_cornerPoints_c = inner_prod( cornerPoints, c );
                        (*iter)->innver_cornerPoints = inner_cornerPoints_c;
                        (*iter)->version = cornerPointsVersion;
                }

                //                      if (inner_cornerPoints_c <= cval)
                //                      {
                //                // FIX: should figure out a way to prune c here
                //                //cout << "  bad cval" << endl;
                //                              continue;
                //                      }

                min_ratio = 99e+20;

                bi = b->data.begin();
                bend = b->data.end();

                ci = c.data.begin();
                cend = c.data.end();

                for (; ci != cend; ci++) {
                        if (0.0 == ci->value) continue;

                        // advance until bi->index >= ci->index
                        while (1) {
                                if (bi == bend) {
                                        bdone = true;
                                        goto break_ci_loop;
                                }
                                if (bi->index >= ci->index) break;
                                bi++;
                        }

                        if (bi->index > ci->index) {
                                // we found a j such that b(j) = 0 and c(j) != 0, which implies
                                // min_ratio = 0, so this c is useless and should be skipped
                                //cout << "  skipping out" << endl;
                                goto continue_Points_loop;
                        }

                        ratio = bi->value / ci->value;
                        if (ratio < min_ratio) min_ratio = ratio;
                }
break_ci_loop:
                if (bdone) {
                        for (; ci != cend; ci++) {
                                if (0.0 != ci->value) goto continue_Points_loop;
                        }
                }

                val = inner_cornerPoints_b + min_ratio * ( cval - inner_cornerPoints_c );
#if 1
                if (min_ratio > 1) 
                {
                        if (min_ratio < 1 + MIN_RATIO_EPS) 
                        {
                                // round-off error, correct it down to 1
                                min_ratio = 1;
                        } 
                        else 
                        {
                                cout << "ERROR: min_ratio > 1 in upperBoundInternal!" << endl;
                                cout << "  (min_ratio-1)=" << (min_ratio-1) << endl;
                                cout << "  normb=" << b->norm_1() << endl;
                                cout << "  b=";
                                b->write(cout);
                                cout << endl;

                                cout << "  normc=" << c.norm_1() << endl;
                                cout << "  c=";
                                c.write(cout);
                                cout << endl;

                                exit(EXIT_FAILURE);
                        }
                }
#endif

                if (val < min_val) min_val = val;
continue_Points_loop:
                ;
        }

        return min_val;
}



REAL_VALUE BeliefValuePairPool::getValue_NoInterpolation(const belief_vector& b) 
{
        REAL_VALUE min_val;
        REAL_VALUE inner_cornerPoints_b;

        inner_cornerPoints_b = inner_prod( cornerPoints, b );
        min_val = inner_cornerPoints_b;

        return min_val;
}

// update helper functions
SharedPointer<BeliefValuePair> BeliefValuePairPool::addPoint(SharedPointer<belief_vector>&  b, REAL_VALUE val) 
{

        DEBUG_TRACE( cout << "BVPairPool::add" << endl; );
        DEBUG_TRACE( cout << "val " << val << endl; );
        DEBUG_TRACE( cout << "b   "; );
        DEBUG_TRACE( b->write(cout) << endl; );

        int wc = whichCornerPoint(b);
        if (-1 == wc) 
        {
                SharedPointer<BeliefValuePair>bvp = new BeliefValuePair();
                // new bvp will have "disabled" set to false
                bvp->b = b;
                bvp->v = val;

                points.push_back(bvp);
                return bvp;
        }
        else 
        {
                cornerPointsVersion++;
                cornerPoints(wc) = val;
                return NULL;
        }
}

// if b is a corner point e_i, return i.  else return -1
int BeliefValuePairPool::whichCornerPoint(const SharedPointer<belief_vector>&  b) const {
        int numStates = problem->getBeliefSize();
        int non_zero_count = 0;
        int non_zero_index = -1;
        FOR(i, numStates) {
                if (fabs((*b)(i)) > CORNER_EPS) {
                        if (non_zero_count == 0) {
                                non_zero_index = i;
                                non_zero_count++;
                        } else {
                                return -1;
                        }
                }
        }
        if (non_zero_count == 0) {
                return -1;
        }
        else {
                return non_zero_index;
        }
}
//private methods
//for debugging purposes
void BeliefValuePairPool::printCorners() const {
        printf("[");

        FOREACH(REAL_VALUE, di,  cornerPoints.data) {
                printf("%f, ", *di);
        }
        printf("]\n");
}//end printAlphal