Пример #1
0
	string_iterator& operator -- () {
		prior(it, range_start);
		return *this;
	}
Пример #2
0
int main(int argc, char **argv) {
   OptionParser opts;

   string mapFile, evidFile;

   int factor;

   opts.addOption(new StringOption("map", 
            "--map <filename>                 : map file",
            "../input/grid.bmp", mapFile, false));

   opts.addOption(new StringOption("evidence", 
            "--evidence <filename>            : evidence file",
            "", evidFile, true));

   opts.addOption(new IntOption("factor",
            "--factor <int>                   : scaling factor",
            1, factor, true));

   opts.parse(argc,argv);
   JetColorMap jet;
   RGBTRIPLE black = {0,0,0};

   RGBTRIPLE white = {255,255,255};

   RGBTRIPLE red;
   red.R = 255;
   red.G = 0;
   red.B = 0;

   RGBTRIPLE blue;
   blue.R = 0;
   blue.G = 0;
   blue.B = 255;

   RGBTRIPLE green;
   green.R = 0;
   green.G = 255;
   green.B = 0; 

   RGBTRIPLE initialColor;
   initialColor.R = 111; 
   initialColor.G = 49;
   initialColor.B = 152;
//   initialColor.G = 152;
//   initialColor.B = 49;


   RGBTRIPLE currentColor;
   currentColor.R = 181;
   currentColor.G = 165;
   currentColor.B = 213;
//   currentColor.G = 213;
//   currentColor.B = 165;


   RGBTRIPLE magenta;
   magenta.R = 255;
   magenta.G = 0;
   magenta.B = 255;

   RGBTRIPLE cyan;
   cyan.R = 0;
   cyan.G = 255;
   cyan.B = 255;

   RGBTRIPLE yellow;
   yellow.R = 255;
   yellow.G = 255;
   yellow.B = 0;

   BMPFile bmpFile(mapFile);

   Grid grid(bmpFile, black);

   
   Evidence testSet(evidFile, grid, factor);
 /* 
   if (1) { 
	   evid.split(trainSet, testSet, 0.8);
   }else{
	   evid.deterministicsplit(trainSet, testSet);
   }*/

#if 0 
   cout << "Creating Markov Model"<<endl;
   MarkovModel markmodel(grid, trainSet);

   double totalObj = 0.0;

   for (int i=0; i < testSet.size(); i++) {
      vector<pair<int, int> > path = testSet.at(i);
      cout << "Calling eval"<<endl;
      double obj = markmodel.eval(path);
      cout << "OBJ: "<<i<<" "<<obj<<endl;
	
      totalObj += obj;
   }

   cout << "TOTAL OBJ: "<<totalObj<<endl;

   cout << "AVERAGE OBJ: "<<totalObj/testSet.size()<<endl;
   return 0;
#endif
   vector<PosFeature> features;

   cout << "Constant Feature"<<endl;

   ConstantFeature constFeat(grid);
   features.push_back(constFeat);

   cout << "Obstacle Feature"<<endl;

   ObstacleFeature obsFeat(grid);
   features.push_back(obsFeat);

   for (int i=1; i < 5; i++) {
      cout << "Blur Feature "<<i<<endl;
      ObstacleBlurFeature blurFeat(grid, 5*i);
      features.push_back(blurFeat);
   }

   cout << "Creating feature array"<<endl;
   FeatureArray featArray2(features);

   cout << "Creating lower resolution feature array"<<endl;
   FeatureArray featArray(featArray2, factor);

   pair<int, int> dims = grid.dims();
   pair<int, int> lowDims((int)ceil((float)dims.first/factor),
         (int)ceil((float)dims.second/factor));

   vector<double> weights(features.size(), -0.0);
   weights.at(1) = -6.2;
   //for (int i=2; i < weights.size(); i++)
   //   weights.at(i) = -1.0;
   weights.at(0) = -2.23;//-2.23
   weights.at(2) = -0.35;
   weights.at(3) = -2.73;
   weights.at(4) = -0.92;
   weights.at(5) = -0.26;
   Parameters params(weights);

   OrderedWaveInferenceEngine engine(InferenceEngine::GRID8);

   vector<vector<double> > prior(dims.first,vector<double> (dims.second,0.0));
/*
   double divide = 1.0;
   vector<double> radiusWeight;
   for (int i=0; i < 20; i++) {
      radiusWeight.push_back(1.0/divide);
      divide*=2;
   }
   generatePrior(grid, trainSet, priorOrig, radiusWeight, factor);
 
   reducePrior(priorOrig, prior, factor);
*/

   vector<vector<vector<double> > > partition, backpartition;

   int time0 = time(0);

   BMPFile gridView(dims.first, dims.second);



   RewardMap rewards(featArray, params); 

   vector<double> sums(params.size(),0.00001);
      
   vector<vector<double> > occupancy;

   Predictor predictor(grid, rewards, engine); 
   
   predictor.setPrior(prior);


   cout << testSet.size() <<" Examples"<<endl;

   for (int i=0; i < testSet.size(); i++) {

      int index = 0;


      vector<pair<int, int> > traj = testSet.at(i);
      vector<double> times = testSet.getTimes(i); 
      pair<int, int> initial = traj.front();
	  pair<int,int> & botinGrid = testSet.at_bot(i); 
	  pair<double,double>& botinPoint = testSet.at_rbot(i);
	  pair<double,double>& end = testSet.at_raw(i).back();

      predictor.setStart(initial); 

      double thresh = -20.0;
	  double startTime = times.front();

      char buf[1024];
      sprintf(buf, "../output/pppredict%03d.dat", i);
      ofstream file(buf);

      for (double tick = startTime; index < traj.size(); tick+=0.4) {

         for ( ; index < traj.size() && times.at(index) < tick; index++); 

         if (index == traj.size() ) break;
 
         cout << "Evidence: "<<i<<"  timestep: "<<tick
            <<"   index: "<<index<<endl;
         predictor.predict(traj.at(index), occupancy);

         cout << "SIZE: "<<prior.size()<<endl;
		 vector<vector<double> >  pos 
            = predictor.getPosterior();

         gridView.addBelief(pos, -30.0, 0.0,jet);

         grid.addObstacles(gridView, black);
         gridView.addLabel(botinGrid,green);
         vector<pair<int, int> > subTraj;

         subTraj.insert(subTraj.end(), traj.begin(), traj.begin()+index);

         gridView.addVector(subTraj, red, factor);

         sprintf(buf, "../compare/pp%03d-%03f.bmp", i, tick-startTime); 
         gridView.write(buf);
		 //pair<double,double> values = predictor.check(traj.back());
		 double cost = 0.0;
		 for(int itr = 0;itr<index;itr++)
		   cost +=rewards.at(traj[itr].first,traj[itr].second);

		 cout<<i<<" Normalizer: "<<predictor.getNormalizer(traj.back())<<
			 " path cost: "<<cost<<" Probability:  "<<cost+predictor.getNormalizer(traj.back())<<endl;

         vector<vector<vector<double> > > timeOcc 
            = predictor.getTimeOccupancy();

		 vector<vector<double > > posterior  = predictor.getPosterior();
		 double maxV = -HUGE_VAL;
		 pair<int,int> predestGrid;
		 pair<double,double> predestPoint;

         for (int ii=0; ii< dims.first; ii++) { 
            for (int jj=0; jj < dims.second; jj++) {
			   if(posterior[ii][jj]>maxV){
				   predestGrid.first = ii;
				   predestGrid.second = jj;
			   }
               maxV  = max(maxV, posterior.at(ii).at(jj));
            }
         }
		 predestPoint = grid.grid2Real(predestGrid.first,predestGrid.second);
		 double dist = sqrt((end.first-predestPoint.first)*(end.first-predestPoint.first)
			 +(end.second-predestPoint.second)*(end.second-predestPoint.second));

		 double logloss = entropy(posterior);

		 cout<<"final belief: "<<posterior.at(traj.back().first).at(traj.back().second)
			 <<" max: "<<maxV
			 <<" logloss: "<<logloss<<endl; 
		 cout<<botinGrid.first<<" "<<botinGrid.second
			 <<" "<<predestGrid.first<<" "<<predestGrid.second<<endl;
		 file<<tick-startTime
			 <<" "<<logloss
			 <<" "<<posterior.at(botinGrid.first).at(botinGrid.second)
			 <<" "<<posterior.at(traj.back().first).at(traj.back().second)
			 <<" "<<maxV<<" "<<dist<<endl;

      } 
      file.close();
   }

}
bool is_straight_line_drawing(const Graph& g,
                              GridPositionMap drawing,
                              VertexIndexMap vm
                             )
{

    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::edge_iterator edge_iterator_t;
    typedef typename graph_traits<Graph>::edges_size_type e_size_t;
    typedef typename graph_traits<Graph>::vertices_size_type v_size_t;

    typedef std::size_t x_coord_t;
    typedef std::size_t y_coord_t;
    typedef boost::tuple<edge_t, x_coord_t, y_coord_t> edge_event_t;
    typedef typename std::vector< edge_event_t > edge_event_queue_t;

    typedef tuple<y_coord_t, y_coord_t, x_coord_t, x_coord_t> active_map_key_t;
    typedef edge_t active_map_value_t;
    typedef std::map< active_map_key_t, active_map_value_t > active_map_t;
    typedef typename active_map_t::iterator active_map_iterator_t;


    edge_event_queue_t edge_event_queue;
    active_map_t active_edges;

    edge_iterator_t ei, ei_end;
    for(tie(ei,ei_end) = edges(g); ei != ei_end; ++ei)
    {
        edge_t e(*ei);
        vertex_t s(source(e,g));
        vertex_t t(target(e,g));
        edge_event_queue.push_back
        (make_tuple(e,
                    static_cast<std::size_t>(drawing[s].x),
                    static_cast<std::size_t>(drawing[s].y)
                   )
        );
        edge_event_queue.push_back
        (make_tuple(e,
                    static_cast<std::size_t>(drawing[t].x),
                    static_cast<std::size_t>(drawing[t].y)
                   )
        );
    }

    // Order by edge_event_queue by first, then second coordinate
    // (bucket_sort is a stable sort.)
    bucket_sort(edge_event_queue.begin(), edge_event_queue.end(),
                property_map_tuple_adaptor<edge_event_t, 2>()
               );

    bucket_sort(edge_event_queue.begin(), edge_event_queue.end(),
                property_map_tuple_adaptor<edge_event_t, 1>()
               );

    typedef typename edge_event_queue_t::iterator event_queue_iterator_t;
    event_queue_iterator_t itr_end = edge_event_queue.end();
    for(event_queue_iterator_t itr = edge_event_queue.begin();
            itr != itr_end; ++itr
       )
    {
        edge_t e(get<0>(*itr));
        vertex_t source_v(source(e,g));
        vertex_t target_v(target(e,g));
        if (drawing[source_v].y > drawing[target_v].y)
            std::swap(source_v, target_v);

        active_map_key_t key(get(drawing, source_v).y,
                             get(drawing, target_v).y,
                             get(drawing, source_v).x,
                             get(drawing, target_v).x
                            );

        active_map_iterator_t a_itr = active_edges.find(key);
        if (a_itr == active_edges.end())
        {
            active_edges[key] = e;
        }
        else
        {
            active_map_iterator_t before, after;
            if (a_itr == active_edges.begin())
                before = active_edges.end();
            else
                before = prior(a_itr);
            after = boost::next(a_itr);

            if (before != active_edges.end())
            {

                edge_t f = before->second;
                vertex_t e_source(source(e,g));
                vertex_t e_target(target(e,g));
                vertex_t f_source(source(f,g));
                vertex_t f_target(target(f,g));

                if (intersects(drawing[e_source].x,
                               drawing[e_source].y,
                               drawing[e_target].x,
                               drawing[e_target].y,
                               drawing[f_source].x,
                               drawing[f_source].y,
                               drawing[f_target].x,
                               drawing[f_target].y
                              )
                   )
                    return false;
            }

            if (after != active_edges.end())
            {

                edge_t f = after->second;
                vertex_t e_source(source(e,g));
                vertex_t e_target(target(e,g));
                vertex_t f_source(source(f,g));
                vertex_t f_target(target(f,g));

                if (intersects(drawing[e_source].x,
                               drawing[e_source].y,
                               drawing[e_target].x,
                               drawing[e_target].y,
                               drawing[f_source].x,
                               drawing[f_source].y,
                               drawing[f_target].x,
                               drawing[f_target].y
                              )
                   )
                    return false;
            }

            active_edges.erase(a_itr);

        }
    }

    return true;

}
Пример #4
-1
std::vector<std::string>  prefix(std::string infix_str){
	
	std::vector<std::string> tmp=postfix(infix_str);
	std::vector<std::string> stack;
	for(int i=0;i<tmp.size();++i){
		if(!is_number(prior(tmp[i][0]))){
			int n=stack.size()-1;
			std::string tmp1=stack[n];
			std::string tmp2=stack[n-1];
			
			stack.pop_back();
			stack.pop_back();
			
			stack.push_back(tmp[i]+" "+tmp2+" "+tmp1);
			
		}
		else{
			stack.push_back(tmp[i]);
		}
	}
	return stack;
}
Пример #5
-1
void generateParam(peak_param *peak, particle_arr *oldPart, particle_t *newPa, prior_fct *prior)
{
  //-- WARNING: d-dependent
  if (newPa->d != 2) {
    printf("Need to implement in ABC.c/generateParam\n");
    exit(1);
  }
  
  gsl_rng *generator      = peak->generator;
  gsl_matrix *cov         = oldPart->cov;
  particle_t **oldPartArr = oldPart->array;
  double p, x1, x2, var2;
  
  particle_t *target;
  int i;
  do {
    i = 0;
    target = oldPartArr[0];
    p = gsl_ran_flat(generator, 0.0, 1.0);
    while (p > target->weight) {
      p -= target->weight;
      i++;
      target = oldPartArr[i];
    }
    
    //-- Compute pdf from multivariate Gaussian pdf
    x1   = gsl_ran_gaussian(generator, sqrt(gsl_matrix_get(cov, 0, 0)));
    var2 = gsl_matrix_get(cov, 1, 1) - SQ(gsl_matrix_get(cov, 1, 0)) / gsl_matrix_get(cov, 0, 0);
    x2   = gsl_ran_gaussian(generator, sqrt(var2));
    x2  += target->param[1] + x1 * gsl_matrix_get(cov, 1, 0) / gsl_matrix_get(cov, 0, 0);
    x1  += target->param[0];
    newPa->param[0] = x1;
    newPa->param[1] = x2;
  } while (!prior(newPa));
  
  return;
}
Пример #6
-1
char* TSolver::convtopol()   //converts symbol-function string to polish record
{if (S==NULL || !strlen(S)) seterr(E_VOID);
 char* r;
 if (Err!=E_NO)
   {ERROR:
    if (r!=NULL) free(r);
    free(S);
    return S=NULL;
   }
 int i,j=0;
 int p;
 int SL=strlen(S);
 r=(char*)calloc(SL*2+2,sizeof(char));
 r[0]='\0';
 cst_clear;
 for (i=0;S[i]!='\0';i++)
   {if (isnumc(S[i]) || isconst(S[i]) || (S[i]=='-' && (minusE || minusN)))
      {r[j++]=S[i];
       continue;
      }
    if (S[i]=='!')
      {addspc();
       r[j++]=S[i];
       addspc();
       continue;
      }
    p=prior(S[i]);
      {if (S[i]==')')
	 {addspc();
	  while ((!cst_free) && cst_end!='(')
	    {r[j++]=cpop();
	     r[j++]=' ';
	    }
	  cpop();
	  if ((!cst_free) && isfn(cst_end))
	    {r[j++]=cpop();
	     r[j++]=' ';
	    }
	  continue;
	 }

       if (S[i]==']')
	 {addspc();
	  while ((!cst_free) && cst_end!='[')
	    {r[j++]=cpop();
	     r[j++]=' ';
	    }
	  cpop();
	  r[j++]=f_abs;
          r[j++]=' ';
	  continue;
	 }
       if ((((!cst_free) && (p>=prior(cst_end)) && (prior(cst_end)>0)&&cst_end!='_'&&S[i]!='_') || S[i]==','))
	   {addspc();
	    while ((!cst_free) && p>=prior(cst_end) && prior(cst_end)>0)
	      {r[j++]=cpop();
	       r[j++]=' ';
	      }
	    if (S[i]==',') continue;
	   }
       cpush(S[i]);
       if (j>0) addspc();
      }
   }
 if (Err!=E_NO) goto ERROR;
 if (r[j-1]!=' ') r[j++]=' ';
 while (!cst_free)
  {r[j++]=cpop();
   r[j++]=' ';
  }
 if (r[j-1]!=' ') r[j++]=' ';
 r[j]='\0';
 free(S);
 S=strdbl(r);
 free(r);
#ifdef debug
 printf("%s\n",S);
#endif
 poled=1;
 return S;
}
Пример #7
-1
std::vector<double> TimeSmoother::FitModel(int parameterIndex,
		const std::vector<double>& dataPoints,
		const std::vector<int>& framesWithDataPoints) const {
	// 1. Project data points (from each frame) to model to get corresponding xi
	// Here the data points are the nodal parameters at each frame and linearly map to xi
	// 2. Construct P

	FourierBasis basis;
	int numRows = dataPoints.size();
	gmm::dense_matrix<double> P(numRows, NUMBER_OF_PARAMETERS);

//	std::cout << "\n\n\nnumRows = " << numRows << '\n';

	for (int i = 0; i < numRows; i++) {
		double xiDouble[1];
		xiDouble[0] = MapToXi(static_cast<double>(i) / numRows); //REVISE design
//		std::cout << "dataPoint(" << i << ") = " << dataPoints[i] << ", xi = "<< xiDouble[0] <<'\n';
		double psi[NUMBER_OF_PARAMETERS];
		basis.Evaluate(psi, xiDouble);
		for (int columnIndex = 0; columnIndex < NUMBER_OF_PARAMETERS;
				columnIndex++) {
			P(i, columnIndex) = psi[columnIndex];
			if (framesWithDataPoints[i]) {
				P(i, columnIndex) *= CAP_WEIGHT_GP; //TEST
			}
		}
	}

//	std::cout << "P = " << P << std::endl;

	// 3. Construct A
	// Note that G is the identity matrix. StS is read in from file.
	gmm::dense_matrix<double> A(NUMBER_OF_PARAMETERS, NUMBER_OF_PARAMETERS),
			temp(NUMBER_OF_PARAMETERS, NUMBER_OF_PARAMETERS);
	gmm::mult(gmm::transposed(P), P, temp);
	gmm::add(pImpl->S, temp, A);

//	std::cout << "A = " << A << std::endl;

	// 4. Construct rhs
	std::vector<double> prior(11), p(numRows), rhs(11);
	for (int i = 0; i < 11; i++) {
		prior[i] = pImpl->Priors(i, parameterIndex);
	}

//	std::cout << "prior: " << prior << std::endl;

	gmm::mult(P, prior, p);

//	std::transform(dataPoints.begin(), dataPoints.end(), p.begin(), p.begin(), std::minus<double>());

	//TEST
//	p[0] *= 10; //more weight for frame 0

	std::vector<double> dataLambda = dataPoints;
	for (unsigned int i = 0; i < dataLambda.size(); i++) {
		if (framesWithDataPoints[i]) {
			dataLambda[i] *= CAP_WEIGHT_GP;
		}
	}
	std::transform(dataLambda.begin(), dataLambda.end(), p.begin(), p.begin(),
			std::minus<double>());

	gmm::mult(transposed(P), p, rhs);

//	std::cout << "rhs: " << rhs << std::endl;

	// 5. Solve normal equation (direct solver) 
	std::vector<double> x(gmm::mat_nrows(A));
	gmm::lu_solve(A, x, rhs);

#ifndef NDEBUG
//	std::cout << "delta x (" << parameterIndex << ") " << x << std::endl;
#endif

	std::transform(x.begin(), x.end(), prior.begin(), x.begin(),
			std::plus<double>());
	return x;
}
Пример #8
-1
double trajOptimizerplus::eval(vector<double> &params) {

    cout << "IN EVAL "<<itrcount++<<" "<<params.size()<<endl;



    for (int i=0; i < params.size(); i++)
        cout << "PARAMS IN: "<<i<<" "<<params.at(i)<<endl;


    int factor = evidence.getFactor();

    pair<int, int> dims = grid.dims();
    int v_dim = seqFeat.num_V();

    /*
    pair<int, int> lowDims((int)ceil((float)dims.first/factor),
          (int)ceil((float)dims.second/factor));
    */
    vector<vector<vector<double> > >
    prior(dims.first, vector<vector<double> >(dims.second,
            vector<double> (v_dim,-HUGE_VAL)));

    double obj = 0.0;
    vector<double> gradient(params.size(), 0.0);
    vector<vector<vector<double> > > occupancy;
    vector<vector<double> > layerOccupancy;
    layerOccupancy.resize(dims.first,vector<double>(dims.second,-HUGE_VAL));
    vector<double> modelFeats, pathFeats;

    for (int i=0; i < evidence.size(); i++) {
        for (int j=0; j < params.size(); j++) {
            cout << "  "<<j<<" "<<params.at(j);
        }
        cout<<endl;

        cout << "Evidence #"<<i<<endl;
        vector<pair<int, int> >&  trajectory = evidence.at(i);
        vector<double>& velocityseq = evidence.at_v(i);
        pair<int,int>&  bot = evidence.at_bot(i);

        //  robot local blurres features
        for (int r=1; r <= NUMROBFEAT; r++) {
            cout << "Adding  Robot Feature "<<r<<endl;
            RobotLocalBlurFeature robblurFeat(grid,bot,10*r);
            //	RobotGlobalFeature robFeat(grid,bot);
            posFeatures.push_back(robblurFeat);
        }

        cout << "   Creating feature array"<<endl;
        FeatureArray featArray2(posFeatures);
        FeatureArray featArray(featArray2, factor);

        for (int rr=1; rr<= NUMROBFEAT; rr++)
            posFeatures.pop_back();

        // split different posfeatures and seqfeature weights
        vector<double> p_weights,s_weights;
        int itr = 0;
        for (; itr<featArray.size(); itr++)
            p_weights.push_back(params[itr]);
        for (; itr<params.size(); itr++)
            s_weights.push_back(params[itr]);

        //cout<<"Params"<<endl;
        Parameters p_parameters(p_weights), s_parameters(s_weights);
        /*    cout<<featArray.size()<<endl;
        	  cout<<params.size()<<endl;
        	  cout<<p_weights.size()<<endl;
        	  cout<<s_weights.size()<<endl;
        	  cout<<p_parameters.size()<<endl;
        	  cout<<s_parameters.size()<<endl;
        */
        //cout<<"Reward"<<endl;
        RewardMap rewards(featArray,seqFeat,p_parameters,s_parameters);
        DisSeqPredictor predictor(grid, rewards, engine);

        // sum of reward along the trajectory
        double cost = 0.0;
        //cout<< trajectory.size()<<endl;
        for (int j=0; j < trajectory.size(); j++) {
            //cout<<j<<" "<<trajectory.at(j).first<<" "<< trajectory.at(j).second<< " "<< seqFeat.getFeat(velocityseq.at(j))<<endl;
            cost+=rewards.at(trajectory.at(j).first, trajectory.at(j).second, seqFeat.getFeat(velocityseq.at(j)));
        }
        State initial(trajectory.front(),seqFeat.getFeat(velocityseq.front()));
        State destination(trajectory.back(),seqFeat.getFeat(velocityseq.back()));
        //for (int k=0;k<v_dim;k++)
        prior.at(destination.x()).at(destination.y()).at(destination.disV) = 0.0;

        cout << "Initial: "<<initial.x()<<"  "<<initial.y()<<"  "<<initial.disV<<endl;
        cout << "Destination: "<<destination.x()<<"  "
             <<destination.y()<<" "<<destination.disV<<endl;
        predictor.setStart(initial);
        predictor.setPrior(prior);

        double norm = predictor.forwardBackwardInference(initial, occupancy);

        for (int l=0; l<v_dim; l++) {
            BMPFile gridView(dims.first, dims.second);
            for (int x= 0; x<dims.first; x++) {
                for(int y=0; y<dims.second; y++) {
                    layerOccupancy.at(x).at(y) = occupancy.at(x).at(y).at(l);
                }
            }

            char buf[1024];
            /*
            RobotGlobalFeature robblurFeat(grid,bot);
            gridView.addBelief(robblurFeat.getMap(), 0.0, 25, white, red);
            gridView.addVector(trajectory, blue, factor);
            gridView.addLabel(bot,green);
            sprintf(buf, "../figures/feat%04d_%d.bmp",i,l);
            gridView.write(buf);
            */

            gridView.addBelief(layerOccupancy, -300.0, 5.0, white, red);
            //grid.addObstacles(gridView, black);
            gridView.addLabel(bot,green);
            gridView.addVector(trajectory, blue, factor);

            sprintf(buf, "../figures/train%04d_%d.bmp",i,l);
            gridView.write(buf);
        }


        /*
        for (int i=0; i < occupancy.size(); i++)
           for (int j=0; j < occupancy.at(i).size(); j++)
              if (occupancy.at(i).at(j) > -10)
                 cout << i <<" "<<j<<"    "<<occupancy.at(i).at(j)<<endl;
        */
        featArray.featureCounts(occupancy, modelFeats);

        featArray.featureCounts(trajectory, pathFeats);


        seqFeat.featureCounts_vec(occupancy,modelFeats);
        seqFeat.featureCounts_vec(velocityseq,pathFeats);

        for (int k=0; k < params.size(); k++) {
            double diff = pathFeats.at(k) - modelFeats.at(k);
            gradient.at(k) -= diff;
            cout <<" Gradient ("<< k << " -grad: "<< gradient.at(k) <<" -path: "<<
                 pathFeats.at(k)<<" -model: "<< modelFeats.at(k)<<")";
        }
        cout<<endl;
        cout << "OBJ: "<<cost-norm<< "  "<<cost<<"  "<<norm<<endl;
        obj += (cost - norm);
        /* obj is the path probability
         * cost is the sum of rewards: sum f(s,a)
         * norm is V(s_1->G), since here s_T = G, V(s_T->G) = 0*/
        prior.at(destination.x()).at(destination.y()).at(destination.disV)
            = -HUGE_VAL;
    }

    cout << "RETURN OBJ: "<<-obj<<endl;

    params = gradient;

    return -obj;
}
// [ref] ${MOCAPY_HOME}/examples/discrete_hmm_with_prior.cpp
void hmm_with_discrete_and_prior()
{
    // in Mocapy++, so far only the discrete node supports the use of a prior.
    //  [ref] Mocapy++ manual, pp. 15.

#if 1
	mocapy::mocapy_seed((uint)5556574);
#else
	mocapy::mocapy_seed((uint)std::time(NULL));
#endif

	// number of trainining sequences
	const int N = 100;

	// sequence lengths
	const int T = 100;

	// Gibbs sampling parameters
	int MCMC_BURN_IN = 10;

	//---------------------------------------------------------------
	// HMM hidden and observed node sizes
	const uint H_SIZE = 2;
	const uint O_SIZE = 2;
	const bool init_random = false;

	mocapy::CPD th0_cpd;
	th0_cpd.set_shape(2);
	th0_cpd.set_values(mocapy::vec(0.1, 0.9));

	mocapy::CPD th1_cpd;
	th1_cpd.set_shape(2, 2);
	th1_cpd.set_values(mocapy::vec(0.95, 0.05, 0.1, 0.9));

	mocapy::CPD to_cpd;
	to_cpd.set_shape(2, 2);
	to_cpd.set_values(mocapy::vec(0.1, 0.9, 0.8, 0.2));

	// The target DBN (This DBN generates the data)
	mocapy::Node *th0 = mocapy::NodeFactory::new_discrete_node(H_SIZE, "th0", init_random, th0_cpd);
	mocapy::Node *th1 = mocapy::NodeFactory::new_discrete_node(H_SIZE, "th1", init_random, th1_cpd);
	mocapy::Node *to = mocapy::NodeFactory::new_discrete_node(O_SIZE, "to", init_random, to_cpd);

	mocapy::DBN tdbn;
	tdbn.set_slices(mocapy::vec(th0, to), mocapy::vec(th1, to));

	tdbn.add_intra("th0", "to");
	tdbn.add_inter("th0", "th1");
	tdbn.construct();

	//---------------------------------------------------------------
	// The model DBN (this DBN will be trained)
	// For mh0, get the CPD from th0 and fix parameters
	mocapy::Node *mh0 = mocapy::NodeFactory::new_discrete_node(H_SIZE, "mh0", init_random, mocapy::CPD(), th0, true );
	mocapy::Node *mh1 = mocapy::NodeFactory::new_discrete_node(H_SIZE, "mh1", init_random);
	mocapy::Node *mo = mocapy::NodeFactory::new_discrete_node(O_SIZE, "mo", init_random);

    // a pseudo count prior for the discrete nodes
    //  [ref] Mocapy++ manual, pp. 24.
	mocapy::MDArray<double> pcounts;
	pcounts.set_shape(O_SIZE, H_SIZE);
	pcounts.set(0, 1, 1000);
	mocapy::PseudoCountPrior prior(pcounts);
	((mocapy::DiscreteNode *)mo)->get_densities()->set_prior(&prior);

	mocapy::DBN mdbn;
	mdbn.set_slices(mocapy::vec(mh0, mo), mocapy::vec(mh1, mo));

	mdbn.add_intra("mh0", "mo");
	mdbn.add_inter("mh0", "mh1");
	mdbn.construct();

	std::cout << "*** TARGET ***" << std::endl;
	std::cout << *th0 << std::endl;
	std::cout << *th1 << std::endl;
	std::cout << *to << std::endl;

	std::cout << "*** MODEL ***" << std::endl;
	std::cout << *mh0 << std::endl;
	std::cout << *mh1 << std::endl;
	std::cout << *mo << std::endl;

	//---------------------------------------------------------------
	std::vector<mocapy::Sequence> seq_list;
	std::vector<mocapy::MDArray<mocapy::eMISMASK> > mismask_list;

	std::cout << "Generating data" << std::endl;

	mocapy::MDArray<mocapy::eMISMASK> mismask;
	mismask.repeat(T, mocapy::vec(mocapy::MOCAPY_HIDDEN, mocapy::MOCAPY_OBSERVED));

	// Generate the data
	double sum_LL(0);
	for (int i = 0; i < N; ++i)
	{
		std::pair<mocapy::Sequence, double> seq_ll = tdbn.sample_sequence(T);
		sum_LL += seq_ll.second;
		seq_list.push_back(seq_ll.first);
		mismask_list.push_back(mismask);
	}
	std::cout << "Average LL: " << sum_LL/N << std::endl;

	//---------------------------------------------------------------
	mocapy::GibbsRandom mcmc = mocapy::GibbsRandom(&mdbn);
	mocapy::EMEngine em = mocapy::EMEngine(&mdbn, &mcmc, &seq_list, &mismask_list);

	std::cout << "Starting EM loop" << std::endl;
	double bestLL = -1000;
	uint it_no_improvement(0);
	uint i(0);

	// Start EM loop
	while (it_no_improvement < 100)
	{
		em.do_E_step(1, MCMC_BURN_IN, true);

		const double ll = em.get_loglik();
		std::cout << "LL= " << ll;

		if (ll > bestLL)
		{
			std::cout << " * saving model *" << std::endl;
			mdbn.save("./data/probabilistic_graphical_model/mocapy/discrete_hmm_with_prior.dbn");
			bestLL = ll;
			it_no_improvement = 0;
		}
		else
		{
			++it_no_improvement;
			std::cout << std::endl;
		}

		++i;
		em.do_M_step();
	}

	std::cout << "DONE" << std::endl;

	//---------------------------------------------------------------
	mdbn.load("./data/probabilistic_graphical_model/mocapy/discrete_hmm_with_prior.dbn");

	std::cout << "*** TARGET ***" << std::endl;
	std::cout << "th0: \n" << *th0 << std::endl;
	std::cout << "th1: \n" << *th1 << std::endl;
	std::cout << "to: \n" << *to << std::endl;

	std::cout << "*** MODEL ***" << std::endl;
	std::cout << "mh0: \n" << *mh0 << std::endl;
	std::cout << "mh1: \n" << *mh1 << std::endl;
	std::cout << "mo: \n" << *mo << std::endl;

	//---------------------------------------------------------------
	delete th0;
	delete th1;
	delete to;

	delete mh0;
	delete mh1;
	delete mo;
}
Пример #10
-1
  void planar_canonical_ordering(const Graph& g, 
                                 PlanarEmbedding embedding, 
                                 OutputIterator ordering, 
                                 VertexIndexMap vm)
  {
    
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
    typedef typename graph_traits<Graph>::edge_descriptor edge_t;
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator_t;
    typedef typename graph_traits<Graph>::adjacency_iterator
      adjacency_iterator_t;
    typedef typename std::pair<vertex_t, vertex_t> vertex_pair_t;
    typedef typename property_traits<PlanarEmbedding>::value_type 
      embedding_value_t;
    typedef typename embedding_value_t::const_iterator embedding_iterator_t;
    typedef iterator_property_map
      <typename std::vector<vertex_t>::iterator, VertexIndexMap> 
      vertex_to_vertex_map_t;
    typedef iterator_property_map
      <typename std::vector<std::size_t>::iterator, VertexIndexMap> 
      vertex_to_size_t_map_t;
    
    enum {PROCESSED, 
          UNPROCESSED, 
          ONE_NEIGHBOR_PROCESSED, 
          READY_TO_BE_PROCESSED};
    
    std::vector<vertex_t> processed_neighbor_vector(num_vertices(g));
    vertex_to_vertex_map_t processed_neighbor
      (processed_neighbor_vector.begin(), vm);

    std::vector<std::size_t> status_vector(num_vertices(g), UNPROCESSED);
    vertex_to_size_t_map_t status(status_vector.begin(), vm);

    std::list<vertex_t> ready_to_be_processed;
    
    vertex_t first_vertex = *vertices(g).first;
    vertex_t second_vertex;
    adjacency_iterator_t ai, ai_end;
    for(tie(ai,ai_end) = adjacent_vertices(first_vertex,g); ai != ai_end; ++ai)
      {
        if (*ai == first_vertex)
          continue;
        second_vertex = *ai;
        break;
      }

    ready_to_be_processed.push_back(first_vertex);
    status[first_vertex] = READY_TO_BE_PROCESSED;
    ready_to_be_processed.push_back(second_vertex);
    status[second_vertex] = READY_TO_BE_PROCESSED;

    while(!ready_to_be_processed.empty())
      {
        vertex_t u = ready_to_be_processed.front();
        ready_to_be_processed.pop_front();

        if (status[u] != READY_TO_BE_PROCESSED && u != second_vertex)
          continue;

        embedding_iterator_t ei, ei_start, ei_end;
        embedding_iterator_t next_edge_itr, prior_edge_itr;

        ei_start = embedding[u].begin();
        ei_end = embedding[u].end();
        prior_edge_itr = prior(ei_end);
        while(source(*prior_edge_itr, g) == target(*prior_edge_itr,g))
          prior_edge_itr = prior(prior_edge_itr);

        for(ei = ei_start; ei != ei_end; ++ei)
          {
            
            edge_t e(*ei); // e = (u,v)
            next_edge_itr = next(ei) == ei_end ? ei_start : next(ei);
            vertex_t v = source(e,g) == u ? target(e,g) : source(e,g);

            vertex_t prior_vertex = source(*prior_edge_itr, g) == u ? 
              target(*prior_edge_itr, g) : source(*prior_edge_itr, g);
            vertex_t next_vertex = source(*next_edge_itr, g) == u ? 
              target(*next_edge_itr, g) : source(*next_edge_itr, g);

            // Need prior_vertex, u, v, and next_vertex to all be
            // distinct. This is possible, since the input graph is
            // triangulated. It'll be true all the time in a simple
            // graph, but loops and parallel edges cause some complications.
            if (prior_vertex == v || prior_vertex == u)
              {
                prior_edge_itr = ei;
                continue;
              }

            //Skip any self-loops
            if (u == v)
                continue;
                                                                
            // Move next_edge_itr (and next_vertex) forwards
            // past any loops or parallel edges
            while (next_vertex == v || next_vertex == u)
              {
                next_edge_itr = next(next_edge_itr) == ei_end ?
                  ei_start : next(next_edge_itr);
                next_vertex = source(*next_edge_itr, g) == u ? 
                  target(*next_edge_itr, g) : source(*next_edge_itr, g);
              }


            if (status[v] == UNPROCESSED)
              {
                status[v] = ONE_NEIGHBOR_PROCESSED;
                processed_neighbor[v] = u;
              }
            else if (status[v] == ONE_NEIGHBOR_PROCESSED)
              {
                vertex_t x = processed_neighbor[v];
                //are edges (v,u) and (v,x) adjacent in the planar
                //embedding? if so, set status[v] = 1. otherwise, set
                //status[v] = 2.

                if ((next_vertex == x &&
                     !(first_vertex == u && second_vertex == x)
                     )
                    ||
                    (prior_vertex == x &&
                     !(first_vertex == x && second_vertex == u)
                     )
                    )
                  {
                    status[v] = READY_TO_BE_PROCESSED;
                  }
                else
                  {
                    status[v] = READY_TO_BE_PROCESSED + 1;
                  }                                                        
              }
            else if (status[v] > ONE_NEIGHBOR_PROCESSED)
              {
                //check the two edges before and after (v,u) in the planar
                //embedding, and update status[v] accordingly

                bool processed_before = false;
                if (status[prior_vertex] == PROCESSED)
                  processed_before = true;

                bool processed_after = false;
                if (status[next_vertex] == PROCESSED)
                  processed_after = true;

                if (!processed_before && !processed_after)
                    ++status[v];

                else if (processed_before && processed_after)
                    --status[v];

              }

            if (status[v] == READY_TO_BE_PROCESSED)
              ready_to_be_processed.push_back(v);

            prior_edge_itr = ei;

          }

        status[u] = PROCESSED;
        *ordering = u;
        ++ordering;
        
      }
    
  }
Пример #11
-1
double trajectoryOptimizer::eval(vector<double> &params, vector<double> &gradient) {

   cout << "IN EVAL   "<<params.size()<<endl;


   for (int i=0; i < params.size(); i++) 
      cout << "PARAMS IN: "<<i<<" "<<params.at(i)<<endl;

   int factor = evidence.getFactor();

  // cout << "FACTOR: "<<factor<<endl;
  
   FeatureArray featArray2(features);

   FeatureArray featArray(featArray2, factor);
   //cout<<"Dims featarray  "<<featArray.dims().first<<" "<<featArray.dims().second<<endl;

   Parameters parameters(params);

   //cout << "Calculating rewards"<<endl;

   RewardMap rewards(featArray, parameters); 

   pair<int, int> dims = grid.dims();

   BMPFile gridView(dims.first, dims.second);

   pair<int, int> lowDims((int)ceil((float)dims.first/factor),
         (int)ceil((float)dims.second/factor));

   //cout << "Computing prior"<<endl;
   vector<vector<double> > prior(lowDims.first, vector<double>(lowDims.second, 
            -HUGE_VAL)); 

   double obj = 0.0;
   gradient.clear();
   gradient.resize(params.size(), 0.0); 

   for (int i=0; i < evidence.size(); i++) {

      Predictor predictor(grid, rewards, engine); 

      cout << "Evidence #"<<i<<endl;
      vector<pair<int, int> > trajectory = evidence.at(i);

      double cost = 0.0;
      for (int j=0; j < trajectory.size(); j++){
		  double temp = rewards.at(trajectory.at(j).first, 
					  trajectory.at(j).second);
		  cost += temp;
	  }
	  

      pair<int, int> initial = trajectory.front();
      pair<int, int> destination = trajectory.back();

      prior.at(destination.first).at(destination.second) = 0.0;
#if 0
      cout << "Initial: "<<initial.first<<"  "<<initial.second<<endl;
      cout << "Destination: "<<destination.first<<"  "
         <<destination.second<<endl;
#endif
      predictor.setStart(initial);
      predictor.setPrior(prior);

      vector<vector<double> > occupancy;
      double norm = predictor.predict(initial, occupancy);

      gridView.addBelief(occupancy, -300.0, 0.0, white, red);


      gridView.addVector(trajectory, blue, factor);

      char buf[1024];
      sprintf(buf, "../figures/train%04d.bmp", i);
      gridView.write(buf);

      vector<double> modelFeats, pathFeats;

      //cout << "Computing feature counts"<<endl;

	  /*
      for (int i=0; i < occupancy.size(); i++)
         for (int j=0; j < occupancy.at(i).size(); j++) 
            if (occupancy.at(i).at(j) > -10)
               cout << i <<" "<<j<<"    "<<occupancy.at(i).at(j)<<endl; 
      */

      featArray.featureCounts(occupancy, modelFeats);

      featArray.featureCounts(trajectory, pathFeats);

      cout << "GRADIENT"<<endl;

      for (int k=0; k < params.size(); k++) {
         double diff = pathFeats.at(k) - modelFeats.at(k);
         gradient.at(k) -= diff;
         cout << k << ": " << gradient.at(k) << "    " << pathFeats.at(k)
            << " " << modelFeats.at(k) <<endl;
      }

      cout << "OBJ: "<<cost-norm<<endl;
      cout << "     "<<cost<<"  "<<norm<<endl;
      obj += (cost - norm);

      prior.at(destination.first).at(destination.second) = -HUGE_VAL; 
   }

   cout << "RETURN OBJ: "<<-obj<<endl;

   return -obj;
}
Пример #12
-1
 void MvnConjVarSampler::draw() {
   Ptr<MvnSuf> suf = model()->suf();
   model()->set_siginv(MvnVarSampler::draw_precision(
       rng(), suf->n() - 1, suf->center_sumsq(suf->ybar()), *prior()));
 }
Пример #13
-1
void fnIMIS(const size_t InitSamples, const size_t StepSamples, const size_t FinalResamples, const size_t MaxIter, const size_t NumParam, unsigned long int rng_seed, const char * runName)
{

  // Declare and configure GSL RNG
  gsl_rng * rng;
  const gsl_rng_type * T;

  gsl_rng_env_setup();
  T = gsl_rng_default;
  rng = gsl_rng_alloc (T);
  gsl_rng_set(rng, rng_seed);

  char strDiagnosticsFile[strlen(runName) + 15 +1];
  char strResampleFile[strlen(runName) + 12 +1];
  strcpy(strDiagnosticsFile, runName); strcat(strDiagnosticsFile, "Diagnostics.txt");
  strcpy(strResampleFile, runName); strcat(strResampleFile, "Resample.txt");
  FILE * diagnostics_file = fopen(strDiagnosticsFile, "w");
  fprintf(diagnostics_file, "Seeded RNG: %zu\n", rng_seed);
  fprintf(diagnostics_file, "Running IMIS. InitSamples: %zu, StepSamples: %zu, FinalResamples %zu, MaxIter %zu\n", InitSamples, StepSamples, FinalResamples, MaxIter);

  // Setup IMIS arrays
  gsl_matrix * Xmat = gsl_matrix_alloc(InitSamples + StepSamples*MaxIter, NumParam);
  double * prior_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
  double * likelihood_all = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
  double * imp_weight_denom = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));  // proportional to q(k) in stage 2c of Raftery & Bao
  double * gaussian_sum = (double*) calloc(InitSamples + StepSamples*MaxIter, sizeof(double));      // sum of mixture distribution for mode
  struct dst * distance = (struct dst *) malloc(sizeof(struct dst) * (InitSamples + StepSamples*MaxIter)); // Mahalanobis distance to most recent mode
  double * imp_weights = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));
  double * tmp_MVNpdf = (double*) malloc(sizeof(double) * (InitSamples + StepSamples*MaxIter));

  gsl_matrix * nearestX = gsl_matrix_alloc(StepSamples, NumParam);
  double center_all[MaxIter][NumParam];
  gsl_matrix * sigmaChol_all[MaxIter];
  gsl_matrix * sigmaInv_all[MaxIter];

  // Initial prior samples
  sample_prior(rng, InitSamples, Xmat);

  // Calculate prior covariance
  double prior_invCov_diag[NumParam];
  /*
    The paper describing the algorithm uses the full prior covariance matrix.
    This follows the code in the IMIS R package and diagonalizes the prior 
    covariance matrix to ensure invertibility.
  */
  for(size_t i = 0; i < NumParam; i++){
    gsl_vector_view tmpCol = gsl_matrix_subcolumn(Xmat, i, 0, InitSamples);
    prior_invCov_diag[i] = gsl_stats_variance(tmpCol.vector.data, tmpCol.vector.stride, InitSamples);
    prior_invCov_diag[i] = 1.0/prior_invCov_diag[i];
  }

  // IMIS steps
  fprintf(diagnostics_file, "Step Var(w_i)  MargLik    Unique Max(w_i)     ESS     Time\n");
  printf("Step Var(w_i)  MargLik    Unique Max(w_i)     ESS     Time\n");
  time_t time1, time2;
  time(&time1);
  size_t imisStep = 0, numImisSamples;
  for(imisStep = 0; imisStep < MaxIter; imisStep++){
    numImisSamples = (InitSamples + imisStep*StepSamples);
    
    // Evaluate prior and likelihood
    if(imisStep == 0){ // initial stage
      #pragma omp parallel for
      for(size_t i = 0; i < numImisSamples; i++){
        gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
        prior_all[i] = prior(&theta.vector);
        likelihood_all[i] = likelihood(&theta.vector);
      }
    } else {  // imisStep > 0
      #pragma omp parallel for
      for(size_t i = InitSamples + (imisStep-1)*StepSamples; i < numImisSamples; i++){
        gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, i);
        prior_all[i] = prior(&theta.vector);
        likelihood_all[i] = likelihood(&theta.vector);
      }
    }

    // Determine importance weights, find current maximum, calculate monitoring criteria

    #pragma omp parallel for
    for(size_t i = 0; i < numImisSamples; i++){
      imp_weight_denom[i] = (InitSamples*prior_all[i] + StepSamples*gaussian_sum[i])/(InitSamples + StepSamples * imisStep);
      imp_weights[i] = (prior_all[i] > 0)?likelihood_all[i]*prior_all[i]/imp_weight_denom[i]:0;
    }

    double sumWeights = 0.0;
    for(size_t i = 0; i < numImisSamples; i++){
      sumWeights += imp_weights[i];
    }

    double maxWeight = 0.0, varImpW = 0.0, entropy = 0.0, expectedUnique = 0.0, effSampSize = 0.0, margLik;
    size_t maxW_idx;
    #pragma omp parallel for reduction(+: varImpW, entropy, expectedUnique, effSampSize)
    for(size_t i = 0; i < numImisSamples; i++){
      imp_weights[i] /= sumWeights;
      varImpW += pow(numImisSamples * imp_weights[i] - 1.0, 2.0);
      entropy += imp_weights[i] * log(imp_weights[i]);
      expectedUnique += (1.0 - pow((1.0 - imp_weights[i]), FinalResamples));
      effSampSize += pow(imp_weights[i], 2.0);
    }

    for(size_t i = 0; i < numImisSamples; i++){
      if(imp_weights[i] > maxWeight){
        maxW_idx = i;
        maxWeight = imp_weights[i];
      }
    }
    for(size_t i = 0; i < NumParam; i++)
      center_all[imisStep][i] = gsl_matrix_get(Xmat, maxW_idx, i);

    varImpW /= numImisSamples;
    entropy = -entropy / log(numImisSamples);
    effSampSize = 1.0/effSampSize;
    margLik = log(sumWeights/numImisSamples);

    fprintf(diagnostics_file, "%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
    printf("%4zu %8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", imisStep, varImpW, margLik, expectedUnique, maxWeight, effSampSize, difftime(time(&time2), time1));
    time1 = time2;

    // Check for convergence
    if(expectedUnique > FinalResamples*(1.0 - exp(-1.0))){
      break;
    }

    // Calculate Mahalanobis distance to current mode
    GetMahalanobis_diag(Xmat, center_all[imisStep],  prior_invCov_diag, numImisSamples, NumParam, distance);

    // Find StepSamples nearest points
    // (Note: this was a major bottleneck when InitSamples and StepResamples are large. qsort substantially outperformed GSL sort options.)
    qsort(distance, numImisSamples, sizeof(struct dst), cmp_dst);

    #pragma omp parallel for
    for(size_t i = 0; i < StepSamples; i++){
      gsl_vector_const_view tmpX = gsl_matrix_const_row(Xmat, distance[i].idx);
      gsl_matrix_set_row(nearestX, i, &tmpX.vector);
    }

    // Calculate weighted covariance of nearestX

    // (a) Calculate weights for nearest points 1...StepSamples
    double weightsCov[StepSamples];
    #pragma omp parallel for
    for(size_t i = 0; i < StepSamples; i++){
      weightsCov[i] = 0.5*(imp_weights[distance[i].idx] + 1.0/numImisSamples); // cov_wt function will normalize the weights
    }

    // (b) Calculate weighted covariance
    sigmaChol_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
    covariance_weighted(nearestX, weightsCov, StepSamples, center_all[imisStep], NumParam, sigmaChol_all[imisStep]);

    // (c) Do Cholesky decomposition and inverse of covariance matrix
    gsl_linalg_cholesky_decomp(sigmaChol_all[imisStep]);
    for(size_t j = 0; j < NumParam; j++) // Note: GSL outputs a symmetric matrix rather than lower tri, so have to set upper tri to zero
      for(size_t k = j+1; k < NumParam; k++)
        gsl_matrix_set(sigmaChol_all[imisStep], j, k, 0.0);

    sigmaInv_all[imisStep] = gsl_matrix_alloc(NumParam, NumParam);
    gsl_matrix_memcpy(sigmaInv_all[imisStep], sigmaChol_all[imisStep]);

    gsl_linalg_cholesky_invert(sigmaInv_all[imisStep]);

    // Sample new inputs
    gsl_matrix_view newSamples = gsl_matrix_submatrix(Xmat, numImisSamples, 0, StepSamples, NumParam);
    GenerateRandMVnorm(rng, StepSamples, center_all[imisStep], sigmaChol_all[imisStep], NumParam, &newSamples.matrix);

    // Evaluate sampling probability from mixture distribution
    // (a) For newly sampled points, sum over all previous centers
    for(size_t pastStep = 0; pastStep < imisStep; pastStep++){
      GetMVNpdf(&newSamples.matrix, center_all[pastStep], sigmaInv_all[pastStep], sigmaChol_all[pastStep], StepSamples, NumParam, tmp_MVNpdf);
      #pragma omp parallel for
      for(size_t i = 0; i < StepSamples; i++)
        gaussian_sum[numImisSamples + i] += tmp_MVNpdf[i];
    }
    // (b) For all points, add weight for most recent center
    gsl_matrix_const_view Xmat_curr = gsl_matrix_const_submatrix(Xmat, 0, 0, numImisSamples + StepSamples, NumParam);
    GetMVNpdf(&Xmat_curr.matrix, center_all[imisStep], sigmaInv_all[imisStep], sigmaChol_all[imisStep], numImisSamples + StepSamples, NumParam, tmp_MVNpdf);
    #pragma omp parallel for
    for(size_t i = 0; i < numImisSamples + StepSamples; i++)
      gaussian_sum[i] += tmp_MVNpdf[i];
  } // loop over imisStep

  //// FINISHED IMIS ROUTINE
  fclose(diagnostics_file);
  
  // Resample posterior outputs
  int resampleIdx[FinalResamples];
  walker_ProbSampleReplace(rng, numImisSamples, imp_weights, FinalResamples, resampleIdx); // Note: Random sampling routine used in R sample() function.
  
  // Print results
  FILE * resample_file = fopen(strResampleFile, "w");
  for(size_t i = 0; i < FinalResamples; i++){
    for(size_t j = 0; j < NumParam; j++)
      fprintf(resample_file, "%.15e\t", gsl_matrix_get(Xmat, resampleIdx[i], j));
    gsl_vector_const_view theta = gsl_matrix_const_row(Xmat, resampleIdx[i]);
    fprintf(resample_file, "\n");
  }
  fclose(resample_file);
  
  /*  
  // This outputs Xmat (parameter matrix), centers, and covariance matrices to files for debugging
  FILE * Xmat_file = fopen("Xmat.txt", "w");
  for(size_t i = 0; i < numImisSamples; i++){
    for(size_t j = 0; j < NumParam; j++)
      fprintf(Xmat_file, "%.15e\t", gsl_matrix_get(Xmat, i, j));
    fprintf(Xmat_file, "%e\t%e\t%e\t%e\t%e\t\n", prior_all[i], likelihood_all[i], imp_weights[i], gaussian_sum[i], distance[i]);
  }
  fclose(Xmat_file);
  
  FILE * centers_file = fopen("centers.txt", "w");
  for(size_t i = 0; i < imisStep; i++){
  for(size_t j = 0; j < NumParam; j++)
  fprintf(centers_file, "%f\t", center_all[i][j]);
  fprintf(centers_file, "\n");
  }
  fclose(centers_file);

  FILE * sigmaInv_file = fopen("sigmaInv.txt", "w");
  for(size_t i = 0; i < imisStep; i++){
  for(size_t j = 0; j < NumParam; j++)
  for(size_t k = 0; k < NumParam; k++)
  fprintf(sigmaInv_file, "%f\t", gsl_matrix_get(sigmaInv_all[i], j, k));
  fprintf(sigmaInv_file, "\n");
  }
  fclose(sigmaInv_file);
  */

  // free memory allocated by IMIS
  for(size_t i = 0; i < imisStep; i++){
    gsl_matrix_free(sigmaChol_all[i]);
    gsl_matrix_free(sigmaInv_all[i]);
  }

  // release RNG
  gsl_rng_free(rng);
  gsl_matrix_free(Xmat);
  gsl_matrix_free(nearestX);

  free(prior_all);
  free(likelihood_all);
  free(imp_weight_denom);
  free(gaussian_sum);
  free(distance);
  free(imp_weights);
  free(tmp_MVNpdf);

  return;
}
Пример #14
-1
	string_iterator operator -- (int) {
		string_iterator temp = *this;
		prior(it, range_start);
		return temp;
	}
//------------------------------------------------------------------
ofUTF8Ptr ofUTF8::prior(const ofUTF8String& input, ofUTF8Ptr iter) {
    prior(iter, endPtr(input));
    return iter;
}
Пример #16
-1
RINGING_END_ANON_NAMESPACE



// Returns false if the touch is false
bool prover::add_row( const row &r )
{
  // This function is quite complicated to avoid doing more than one
  // O( ln N ) operation on the multimap.  The equal_range function call
  // will be O( ln N ); the insert function ought to be O(1) because a 
  // sensible hint is supplied (although the C++ standard doesn't require
  // the hint to be used).  The number of identical elements is also 
  // calculated from the range, rather than doing a O( ln N ) call to 
  // multimap::count.

  typedef pair< mmap::iterator, mmap::iterator > range;
  range rng = m.equal_range(r);
  size_t n = distance( rng.first, rng.second ) + 1; // effecively m.count(r)
  mmap::iterator i( m.insert( rng.first == m.begin() ? m.begin() 
			                             : prior( rng.first ),
			      mmap::value_type( r, ++lineno ) ) );

  if ( n > 1 )
    ++dups;

  if ( (int) n > max_occurs )
    {
      is_true = false; 
      if ( fi )
	{

	  for ( failinfo::iterator j = fi->begin(), e = fi->end(); j != e; ++j)
	    if ( j->_row == r )
	      {
		j->_lines.push_back( i->second );
		return false;
	      }

	  linedetail l;

	  // The C++ standard doesn't make any guarantee about where i
	  // is in relation to rng.first and rng.second -- it may fall in
	  // the range [rng.first, rng.second), but equally, it might get 
	  // inserted immediately before rng.first.  If we don't detect i 
	  // in this range, we explicitly add it by hand afterwards.
	  l._row = r;
	  bool added_i = false;
	  {
	    for ( mmap::iterator j = rng.first; j != rng.second; ++j )
	      {
		if ( j == i ) added_i = true;
		l._lines.push_back( j->second );
	      }
	  }
	  if (!added_i) 
	    l._lines.push_back( i->second );

	  fi->push_back( l );
	  return false;
	}
    }

  return is_true;
}
void
somatic_snv_caller_strand_grid::
position_somatic_snv_call(const extended_pos_info& normal_epi,
                          const extended_pos_info& tumor_epi,
                          const extended_pos_info* normal_epi_t2_ptr,
                          const extended_pos_info* tumor_epi_t2_ptr,
                          somatic_snv_genotype_grid& sgt) const {

    static const bool is_always_test(false);

    {
        const snp_pos_info& normal_pi(normal_epi.pi);
        const snp_pos_info& tumor_pi(tumor_epi.pi);

        if(normal_pi.ref_base=='N') return;
        sgt.ref_gt=base_to_id(normal_pi.ref_base);

        // check that a non-reference call meeting quality criteria even
        // exists:
        if(not is_always_test) {
            if(is_spi_allref(normal_pi,sgt.ref_gt) and is_spi_allref(tumor_pi,sgt.ref_gt)) return;
        }
    }

    // strawman model treats normal and tumor as independent, so
    // calculate separate lhoods:
    blt_float_t normal_lhood[DIGT_SGRID::SIZE];
    blt_float_t tumor_lhood[DIGT_SGRID::SIZE];

    const bool is_tier2(NULL != normal_epi_t2_ptr);

    static const unsigned n_tier(2);
    result_set tier_rs[n_tier];
    for(unsigned i(0); i<n_tier; ++i) {
        const bool is_include_tier2(i==1);
        if(is_include_tier2) {
            if(! is_tier2) continue;
            if(tier_rs[0].snv_qphred==0) {
                tier_rs[1].snv_qphred=0;
                continue;
            }
        }

        // get likelihood of each genotype
        //
        static const bool is_normal_het_bias(false);
        static const blt_float_t normal_het_bias(0.0);
        static const bool is_tumor_het_bias(false);
        static const blt_float_t tumor_het_bias(0.0);

        const extended_pos_info& nepi(is_include_tier2 ? *normal_epi_t2_ptr : normal_epi );
        const extended_pos_info& tepi(is_include_tier2 ? *tumor_epi_t2_ptr : tumor_epi );
        get_diploid_gt_lhood_spi(_opt,nepi.pi,is_normal_het_bias,normal_het_bias,normal_lhood);
        get_diploid_gt_lhood_spi(_opt,tepi.pi,is_tumor_het_bias,tumor_het_bias,tumor_lhood);

        get_diploid_het_grid_lhood_spi(nepi.pi,normal_lhood+DIGT::SIZE);
        get_diploid_het_grid_lhood_spi(tepi.pi,tumor_lhood+DIGT::SIZE);

        get_diploid_strand_grid_lhood_spi(nepi.pi,sgt.ref_gt,normal_lhood+DIGT_SGRID::PRESTRAND_SIZE);
        get_diploid_strand_grid_lhood_spi(tepi.pi,sgt.ref_gt,tumor_lhood+DIGT_SGRID::PRESTRAND_SIZE);

        // genomic site results:
        calculate_result_set_grid(normal_lhood,
                                  tumor_lhood,
                                  get_prior_set(sgt.ref_gt),
                                  _ln_som_match,_ln_som_mismatch,
                                  sgt.ref_gt,
                                  tier_rs[i]);

#if 0
#ifdef ENABLE_POLY
        // polymorphic site results:
        assert(0); // still needs to be adapted for 2-tier system:
        calculate_result_set(normal_lhood,tumor_lhood,
                             lnprior_polymorphic(sgt.ref_gt),sgt.ref_gt,sgt.poly);
#else
        sgt.poly.snv_qphred = 0;
#endif
#endif

#ifdef SOMATIC_DEBUG
        if((i==0) && (tier_rs[i].snv_qphred > 0)) {
            const somatic_snv_caller_strand_grid::prior_set& pset(get_prior_set(sgt.ref_gt));
            const blt_float_t lnmatch(_ln_som_match);
            const blt_float_t lnmismatch(_ln_som_mismatch);

            log_os << "DUMP ON\n";
            log_os << "tier1_qphred: " << tier_rs[0].snv_qphred << "\n";

            // instead of dumping the entire distribution, we sort the lhood,prior,and prob to print out the N top values of each:
            std::vector<double> lhood(DDIGT_SGRID::SIZE);
            std::vector<double> prior(DDIGT_SGRID::SIZE);
            std::vector<double> post(DDIGT_SGRID::SIZE);

            // first get raw lhood:
            //
            for(unsigned ngt(0); ngt<DIGT_SGRID::PRESTRAND_SIZE; ++ngt) {
                for(unsigned tgt(0); tgt<DIGT_SGRID::PRESTRAND_SIZE; ++tgt) {
                    const unsigned dgt(DDIGT_SGRID::get_state(ngt,tgt));
                    // unorm takes the role of the normal prior for the somatic case:
                    //            static const blt_float_t unorm(std::log(static_cast<blt_float_t>(DIGT_SGRID::PRESTRAND_SIZE)));

                    //blt_float_t prior;
                    //if(tgt==ngt) { prior=pset.normal[ngt]+lnmatch; }
                    //else         { prior=pset.somatic_marginal[ngt]+lnmismatch; }
                    blt_float_t pr;
                    if(tgt==ngt) { pr=pset.normal[ngt]+lnmatch; }
                    else         { pr=pset.somatic_marginal[ngt]+lnmismatch; }
                    prior[dgt] = pr;

                    lhood[dgt] = normal_lhood[ngt]+tumor_lhood[tgt];
                    post[dgt] = lhood[dgt] + prior[dgt];
                }
            }

            for(unsigned gt(DIGT_SGRID::PRESTRAND_SIZE); gt<DIGT_SGRID::SIZE; ++gt) {
                const unsigned dgt(DDIGT_SGRID::get_state(gt,gt));
                lhood[dgt] = normal_lhood[gt]+tumor_lhood[gt];
                prior[dgt] = pset.normal[gt]+lnmatch;
                post[dgt] = lhood[dgt] + prior[dgt];
            }

            std::vector<double> lhood2(lhood);
            sort_n_dump("lhood_prior",lhood,prior,sgt.ref_gt);
            sort_n_dump("post_lhood",post,lhood2,sgt.ref_gt);

            log_os << "DUMP OFF\n";
        }
#endif

    }

    if((tier_rs[0].snv_qphred==0) ||
       (is_tier2 && (tier_rs[1].snv_qphred==0))) return;

    sgt.snv_tier=0;
    sgt.snv_from_ntype_tier=0;
    if(is_tier2) {
        if(tier_rs[0].snv_qphred > tier_rs[1].snv_qphred) {
            sgt.snv_tier=1;
        }

        if(tier_rs[0].snv_from_ntype_qphred > tier_rs[1].snv_from_ntype_qphred) {
            sgt.snv_from_ntype_tier=1;
        }
    }

    sgt.rs=tier_rs[sgt.snv_from_ntype_tier];

    if(is_tier2 && (tier_rs[0].ntype != tier_rs[1].ntype)) {
        // catch NTYPE conflict states:
        sgt.rs.ntype = NTYPE::CONFLICT;
        sgt.rs.snv_from_ntype_qphred = 0;
    } else {
        // classify NTYPE:
        //

        // convert diploid genotype into more limited ntype set:
        //
        if       (sgt.rs.ntype==sgt.ref_gt) {
            sgt.rs.ntype=NTYPE::REF;
        } else if(DIGT::is_het(sgt.rs.ntype)) {
            sgt.rs.ntype=NTYPE::HET;
        } else {
            sgt.rs.ntype=NTYPE::HOM;
        }
    }

    sgt.rs.snv_qphred = tier_rs[sgt.snv_tier].snv_qphred;
    sgt.is_snv=((sgt.rs.snv_qphred != 0));
}
// Given the likelihood, go through the final computations to get the
// posterior and derived values.
//
static
void
calculate_result_set_grid(const blt_float_t* normal_lhood,
                          const blt_float_t* tumor_lhood,
                          const somatic_snv_caller_strand_grid::prior_set& pset,
                          const blt_float_t lnmatch,
                          const blt_float_t lnmismatch,
                          const unsigned /*ref_gt*/,
                          result_set& rs) {

    // a piece transplanted from 1150 to make a formal correction to
    // the priors which should have a low-impact on the results.
    // the prior below is incomplete
#ifdef DEBUG_ALTERNATE_PRIOR
    static const double neginf(-std::numeric_limits<double>::infinity());

    std::vector<double> prior(DDIGT_SGRID::SIZE);
    std::fill(prior.begin(),prior.end(),neginf);

    // this zero'd code is incomplete and abandoned for now...:
#if 0
    for(unsigned ngt(0); ngt<DIGT_SGRID::PRESTRAND_SIZE; ++ngt) {
        double base_prior(neginf);
        const bool is_noise(ngt>=STAR_DIINDEL::SIZE);
        if(is_noise) {
            base_prior=pset.normal[ngt];
        } else {
            base_prior=pset.nonoise[ngt];
        }
        for(unsigned tgt(0); tgt<DIGT_SGRID::PRESTRAND_SIZE; ++tgt) {
            const blt_float_t tgt_prior_mod( (tgt==ngt) ? lnmatch : lnmismatch );
            const unsigned dgt(DDIGT_SGRID::get_state(ngt,tgt));
            prior[dgt] = normal_genomic_lnprior[ngt]+tgt_prior_mod;
        }
    }

    for(unsigned gt(DIGT_SGRID::PRESTRAND_SIZE); gt<DIGT_SGRID::SIZE; ++gt) {
        const unsigned dgt(DDIGT_SGRID::get_state(gt,gt));
        prior[dgt] = normal_genomic_lnprior[gt]+lnmatch;
    }
#endif

    check_ln_distro(prior.begin(),
                    prior.end(),
                    "somatic snv full prior");
#endif

    // intentionally use higher float res for this structure:
    std::vector<double> pprob(DDIGT_SGRID::SIZE);

    // mult by prior distro to get unnormalized pprob for states in
    // the regular grid model:
    //
    for(unsigned ngt(0); ngt<DIGT_SGRID::PRESTRAND_SIZE; ++ngt) {
        for(unsigned tgt(0); tgt<DIGT_SGRID::PRESTRAND_SIZE; ++tgt) {
            const unsigned dgt(DDIGT_SGRID::get_state(ngt,tgt));

#if 0
            // the trusty old way...:
            const blt_float_t tgt_prior_mod( (tgt==ngt) ? lnmatch : lnmismatch );
            pprob[dgt] = normal_lhood[ngt]+tumor_lhood[tgt]+pset.normal[ngt]+tgt_prior_mod;
#else

            // unorm takes the role of the normal prior for the somatic case:
            //            static const blt_float_t unorm(std::log(static_cast<blt_float_t>(DIGT_SGRID::PRESTRAND_SIZE)));
            blt_float_t prior;
            if(tgt==ngt) {
                prior=pset.normal[ngt]+lnmatch;
            } else {
                prior=pset.somatic_marginal[ngt]+lnmismatch;
            }
            pprob[dgt] = normal_lhood[ngt]+tumor_lhood[tgt]+prior;

#endif
        }
    }

    // Now add the single-strand noise states. note that these states
    // are unique in that we don't look for mixtures of somatic
    // variation with these noise states, b/c single-strand
    // observations can almost exclusively be ruled out as noise:
    //
    for(unsigned gt(DIGT_SGRID::PRESTRAND_SIZE); gt<DIGT_SGRID::SIZE; ++gt) {
        const unsigned dgt(DDIGT_SGRID::get_state(gt,gt));
        pprob[dgt] = normal_lhood[gt]+tumor_lhood[gt]+pset.normal[gt]+lnmatch;
    }

    opt_normalize_ln_distro(pprob.begin(),pprob.end(),DDIGT_SGRID::is_nonsom.val.begin(),rs.max_gt);
    //normalize_ln_distro(pprob.begin(),pprob.end(),rs.max_gt);

    double nonsomatic_sum(0);
    for(unsigned gt(0); gt<DIGT_SGRID::SIZE; ++gt) {
        nonsomatic_sum += pprob[DDIGT_SGRID::get_state(gt,gt)];
    }
    rs.snv_qphred=error_prob_to_qphred(nonsomatic_sum);

    if(0==rs.snv_qphred) return;

#if 0
    // alternate way to calculate the joint:
    //
    double min_not_somfrom_sum(0);
    for(unsigned dgt(0); dgt<DIGT::SIZE; ++dgt) {
        double not_somfrom_sum(nonsomatic_sum);

        for(unsigned ngt(0); ngt<DIGT_SGRID::PRESTRAND_SIZE; ++ngt) {
            // we're looking for the joint prob when state dgt is true
            // in the normal, so skip this as a normal state here:
            //
            if(dgt==ngt) continue;

            for(unsigned tgt(0); tgt<DIGT_SGRID::PRESTRAND_SIZE; ++tgt) {
                // we've already started from the nonsomatic som, so we can skip the equal states:
                //
                if(ngt==tgt) continue;

                not_somfrom_sum += pprob[DDIGT_SGRID::get_state(ngt,tgt)];
            }
        }

        if((dgt==0) || (!_somfrom_sum<min_not_somfrom_sum)) {
            min_not_somfrom_sum=not_somfrom_sum;
            rs.snv_from_ntype_qphred=error_prob_to_qphred(not_somfrom_sum);
            rs.ntype=dgt;
        }
    }
#endif

#if 0
    // reset max_gt to the most likely state excluding normal noise states:
    //
    rs.max_gt=0;
    for(unsigned dgt(0); dgt<DIGT::SIZE; ++dgt) {
        for(unsigned tgt(0); tgt<DIGT_SGRID::PRESTRAND_SIZE; ++tgt) {
            const unsigned xgt(DDIGT_SGRID::get_state(dgt,tgt));
            if(pprob[xgt] > pprob[rs.max_gt]) rs.max_gt=xgt;
        }
    }
#endif

    // Calculate normal distribution alone so that we can classify this call:
    //
    // Polymorphic prior is used because in this situation we want to
    // be conservative about the reference classification --
    // ie. conditioned on only looking at putative somatic sites, we
    // require evidence to show that the normal is in fact reference
    // and not simply an unsampled copy of the somatic variation.
    //
    std::vector<double> normal_pprob(DIGT_SGRID::PRESTRAND_SIZE);
    for(unsigned ngt(0); ngt<DIGT_SGRID::PRESTRAND_SIZE; ++ngt) {
        normal_pprob[ngt] = normal_lhood[ngt]+pset.normal_poly[ngt];
    }

    unsigned max_norm_gt(0);
    normalize_ln_distro(normal_pprob.begin(),normal_pprob.end(),max_norm_gt);

    // find the probability of max_norm_gt:
    const double ngt_prob(prob_comp(normal_pprob.begin(),normal_pprob.end(),max_norm_gt));

    // (1-(1-a)(1-b)) -> a+b-(ab)
    double not_somfrom_sum(nonsomatic_sum+ngt_prob-(nonsomatic_sum*ngt_prob));

    rs.snv_from_ntype_qphred=error_prob_to_qphred(not_somfrom_sum);
    rs.ntype=max_norm_gt;
}
Пример #19
-11
double getL(double *L1) {
    int a,b,c,d,i,r;
    double L,la,la1,mu,mu1,d1,d2;
    L=prior(pa,L1);
    for(r=0; r<nr; r++) {
        a=res[r][0];
        b=res[r][1];
        c=res[r][2];
        d=res[r][3];
        la=fn(al[a]-be[b]+hh[0]);
        la1=fn1(al[a]-be[b]+hh[0]);
        //la2=fn2(al[a]-be[b]+hh[0]);
        mu=fn(al[b]-be[a]-hh[1]);
        mu1=fn1(al[b]-be[a]-hh[1]);
        //mu2=fn2(al[b]-be[a]-hh[1]);
        L+=-la+c*log(la)-lfac[c]-mu+d*log(mu)-lfac[d];
        //L+=-log(phg[c])-log(pag[d]);
        //if(it==5000)printf("%12g --> %d\n%12 --> %d\n",la,c,mu,d);
        L1[a]+=(-1+c/la)*la1;
        L1[b]+=(-1+d/mu)*mu1;
        L1[nt+a]-=(-1+d/mu)*mu1;
        L1[nt+b]-=(-1+c/la)*la1;
        L1[2*nt]+=(-1+c/la)*la1;
        L1[2*nt+1]-=(-1+d/mu)*mu1;
    }
    d1=d2=0;
    for(i=0; i<nt; i++) {
        d1+=L1[i];
        d2+=L1[nt+i];
    }
    for(i=0; i<nt; i++) {
        L1[i]-=d1/nt;
        L1[nt+i]-=d2/nt;
    }
    return L;
}
Пример #20
-31
typename enable_if<is_interval_map<Type>, bool>::type
contains(const Type& super, const typename Type::segment_type& sub_segment)
{
    typedef typename Type::interval_type  interval_type;
    typedef typename Type::const_iterator const_iterator;

    interval_type sub_interval = sub_segment.first;
    if(icl::is_empty(sub_interval)) 
        return true;

    std::pair<const_iterator, const_iterator> exterior = super.equal_range(sub_interval);
    if(exterior.first == exterior.second)
        return false;

    const_iterator last_overlap = prior(exterior.second);

    if(!(sub_segment.second == exterior.first->second) )
        return false;

    return
          icl::contains(hull(exterior.first->first, last_overlap->first), sub_interval)
      &&  Interval_Map::is_joinable(super, exterior.first, last_overlap);
}