text:
#include <iostream>
#include <armadillo.h>
#include <digitRecognition.h>
//#include <mkl.h>
#include <iomanip>

using namespace arma;

//support functions
//cost = fmincg2(10000, nn_params2, k, n , num_labels, X, y, 0.01)
//real function fmincg2(length, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda)

void fmincg(double& finalcost, const int length,mat& nn_params,const int input_layer_size,const int hidden_layer_size,const int num_labels,mat& inputdata,mat& y,const double lambda){
// Copyright (C) 2001 and 2002 by Carl Edward Rasmussen. Date 2002-02-13
// (C) Copyright 1999, 2000 & 2001, Carl Edward Rasmussen
//
// Permission is granted for anyone to copy, use, or modify these
// programs and accompanying documents for purposes of research or
// education, provided this copyright notice is retained, and note is
// made of any changes that have been made.
//
// These programs and documents are distributed without any warranty,
// express or implied.  As the programs were written for research
// purposes only, they have not been tested to the degree that would be
// advisable in any important application.  All use of these programs is
// entirely at the user's own risk.
//
// [ml-class] Changes Made:
// 1) Function name and argument specifications
// 2) Output display
//
// [John Shahbazian] Changes Made:
// 1) Ported to C++ using the Armadillo (http://arma.sourceforge.net/) library
// 2) Change the cost function call to internal.  Replace the
//    'costfunction' function to whatever you would like.  It returns
//    the cost as the result, and the gradient is returned through the first
//    argument as an intent(inout) (e.g. 'gradient#').
// 3) Changed the variable names to be readable.
//    f1 = cost1
//    df1 = gradient1
//    s = search_direction
//    d1 = slope1
//    z1 = point1
//    X0 = backup_params
//    f0 = cost_backup
//    df0 = gradient_backup

	const double RHO = 0.01;
	const double SIG = 0.5;
	const double INT = 0.1;
	const double EXT = 3.0;
	const int MAXEVALS = 20;
	const double RATIO = 100;

	double mintemp, minstuff, M, A, B;
	double fX = 0.0;
	int success;
	int i=0;
	int ls_failed = 0;

	mat backup_params = nn_params;
	mat gradient2(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat gradient3(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat gradient_backup(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat tmp(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	double limit;
	double point1, point2, point3;
	double cost1, cost2, cost3, cost_backup;
	double slope1, slope2, slope3;
	mat search_direction(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	mat stemp(nn_params.n_rows,nn_params.n_cols,fill::zeros);

	double sqrtnumber;
	//mat sd_calc_1,sd_calc_2,sd_calc_3;
	//double sd_calc_4;
	//mat sd_calc_5(nn_params.n_rows,nn_params.n_cols,fill::zeros);

	cost1 = 10000.0;  //lower is better, so init with high
	mat gradient1(nn_params.n_rows,nn_params.n_cols,fill::zeros);
	costfunction(cost1, gradient1, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);
	//std::cout << "gradient1: " << gradient1(0,0) << endl;
	//pause();

	//i = i + (length<0);
	search_direction = -gradient1;

	mat slope_vector(1,1);
	slope_vector = -search_direction.t() * search_direction;
	slope1 = slope_vector(0,0);
	point1 = 1.0/(1.0 - slope1);
	//std::cout << "point1: " << point1 << endl;

	while(i < std::abs(length)){
		i = i + 1;
		//std::cout << "loop: " << i << endl;
		backup_params = nn_params;
		cost_backup = cost1;
		gradient_backup = gradient1;
		stemp = point1 * search_direction;
		nn_params = nn_params + stemp;
		//std::cout << "nn_params: " << nn_params.row(0) << endl;

		gradient2 = gradient1;

		cost2 = 10000.0;
		costfunction(cost2, gradient2, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);

		//i = i + (length<0);
		//i++;
		//std::cout << "i: " << i << endl;
		slope_vector = gradient2.t() * search_direction;
		slope2 = slope_vector(0,0);
		//std::cout << "slope2: " << slope2 << endl;

		cost3 = cost1;
		slope3 = slope1;
		point3 = -point1;
		if(length>0)M=MAXEVALS;
		else M = std::min(MAXEVALS,-length-i);
		success=0;
		limit=-1;

		while(1){ //3.66252   3.66252  0 -5.40439
			//std::cout << "cost2: " << cost2 << " cost1: " << cost1 << " point1: " << point1 << " slope1: " << slope1 << endl;
			//std::cout << "cost2 vs stuff: " << cost2 << "   " << (cost1 + (point1 * RHO * slope1)) << endl;
			//std::cout << "slope2 vs stuff: " << slope2 << "   " << (-SIG * slope1) << endl;
			//std::cout << "M: " << M << endl;
			//pause();
			while(( (cost2 > (cost1 + (point1 * RHO * slope1))) || (slope2 > (-SIG * slope1)) ) && (M > 0) ){
				//std::cout << "here*******************" << endl;
				limit = point1;
				if(cost2 > cost1)
                    point2 = point3 - (0.5 * slope3 * point3 * point3)/(slope3 * point3 + cost2 - cost3);  //quadratic fit
                else{
                    A = 6*(cost2 - cost3)/point3 + 3*(slope2 + slope3);           //cubic fit
                    B = 3*(cost3 - cost2) - point3 * (slope3 + 2*slope2);
                    point2 = (std::sqrt(B*B - A*slope2*point3*point3) - B)/A;
				}
                if(std::isnan(point2) || (!std::isfinite(point2)))
                    point2 = point3 / 2;                         // if we had a numerical problem then bisect
                point2 = std::max( std::min(point2, (INT * point3)), ((1.0 - INT) * point3));  //don't accept too close to limits
                point1  = point1 + point2;                       // update the step
                stemp = point2 * search_direction;
                nn_params = nn_params + stemp;

                costfunction(cost2, gradient2, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);

                M = M - 1.0;
				//std::cout << "i2: " << i << endl;
                //i = i + (length<0);                              // count epochs?!
				//std::cout << "i2: " << i << endl;
                slope_vector = gradient2.t() * search_direction;
                slope2 = slope_vector(0,0);                      //convert to scalar
                point3 = point3 - point2;                        // point3 is now relative to the location of point2

			}

            if ((cost2 > (cost1 + (point1*RHO*slope1)) ) || (slope2 > (-SIG * slope1) )){
				//std::cout << "Break -----------------" << endl;
				break;                                            // this is a failure

            }
            else if (slope2 > (SIG * slope1)){
				//std::cout << "Break -----------------" << endl;
                success = 1;
                break;                                            // success
			}
            else if (M == 0.0){
				//std::cout << "Break -----------------" << endl;
                break;                                            // failure
			}

            A = 6*(cost2 - cost3)/point3 + 3*(slope2 + slope3);  // make cubic extrapolation
            B = 3*(cost3 - cost2) - point3*(slope3 + 2*slope2);
            sqrtnumber = (B*B) - A*slope2*point3*point3;

			if((!std::isnormal(sqrtnumber)) || (sqrtnumber < 0.0) ){
				//std::cout << "sqrt ifs" << endl;
                if (limit < -0.5)                          // if we have no upper limit
                    point2 = point1  * (EXT - 1);                // the extrapolate the maximum amount
                else
                    point2 = (limit - point1) / 2;               // otherwise bisect
				//std::cout << "point2 sqrt ifs: " << point2 << endl;
            }
            else{
                point2 = (-slope2 * point3 * point3)/(B + std::sqrt(sqrtnumber));
                if ((limit > -0.5) && ((point2 + point1) > limit))          // extraplation beyond max?
                    point2 = (limit - point1)/2;                 // bisect
                else if ((limit < -0.5) && ((point2 + point1) > (point1 * EXT)))       // extrapolation beyond limit
                    point2 = point1 * (EXT - 1.0);               // set to extrapolation limit
                else if (point2 < (-point3 * INT))
                    point2 = -point3 * INT;
                else if ((limit > -0.5) && (point2 < (limit - point1)*(1.0 - INT)))   // too close to limit?
                    point2 = (limit - point1 ) * (1.0 - INT);
            }

            cost3 = cost2;
            slope3 = slope2;
            point3 = -point2;
            point1  = point1 + point2;

			//std::cout << "search_direction: " << endl << search_direction.rows(0, 10) << endl;
            stemp = point2 * search_direction;
			//std::cout << "point2: " << point2 << endl;

            nn_params = nn_params + stemp;                       // update current estimates
			//std::cout << "nn_paramsb: " << endl << nn_params.rows(0,10) << endl;

            costfunction(cost2, gradient2, nn_params, input_layer_size, hidden_layer_size, num_labels, inputdata, y, lambda);
			//std::cout << "cost2b: " << cost2 << endl;
            M = M - 1.0;
			//std::cout << "i: " << i << endl;
            //i = i + (length<0);                                  // count epochs?!
			//std::cout << "i: " << i << endl;
			slope_vector = gradient2.t() * search_direction;
            slope2 = slope_vector(0,0);                          //convert to scalar
			//std::cout << "slope2b: " << slope2 << endl;
			//pause();
		}

        if (success == 1){                                   // if line search succeeded
            cost1 = cost2;
            fX = cost1;
            std::cout << "Iteration: " << i << " | Cost: " << cost1 << endl;
            mat sd_calc_1 = gradient2.t() * gradient2;
            mat sd_calc_2 = gradient1.t() * gradient2;
            mat sd_calc_3 = gradient1.t() * gradient1;
            double sd_calc_4 = (sd_calc_1(0,0) - sd_calc_2(0,0)) / sd_calc_3(0,0);
            mat sd_calc_5 = sd_calc_4 * search_direction;

            search_direction = sd_calc_5 - gradient2;
            tmp = gradient1;
            gradient1 = gradient2;
            gradient2 = tmp;                                     // swap derivatives
			slope_vector = gradient1.t() * search_direction;
            slope2 = slope_vector(0,0);                          //convert to scalar
            if(slope2 > 0.0){                                  // new slope must be negative
                search_direction = -gradient1;                   // otherwise use steepest direction
				slope_vector = -search_direction.t() * search_direction;
                slope2 = slope_vector(0,0);                      //convert to scalar
            }
			mintemp = slope1 / (slope2);// - std::numeric_limits<double>::lowest());  //std::numeric_limits<double>::lowest() is min value double precision float //TODO: figure out why the min number is needed
            minstuff = std::min(RATIO, mintemp);
            point1  = point1 * minstuff;                         // slope ratio but max RATIO
            slope1 = slope2;
            ls_failed = 0;                                        // this line search did not fail
        }
        else{
            nn_params = backup_params;
            cost1 = cost_backup;
            gradient1 = gradient_backup;                         // restore point from before failed line search
            if (ls_failed == 1 || (i > std::abs(length)))       // line search failed twice in a row
				//std::cout << "Break -----------------" << endl;
                break;                                            // or we ran out of time, so we give up
            tmp = gradient1;
            gradient1 = gradient2;
            gradient2 = tmp;                                    // swap derivatives
            search_direction = -gradient1;                      // try steepest
            slope_vector = -search_direction.t() * search_direction;
            slope1 = slope_vector(0,0);                         // convert to scalar
            point1  = 1.0 / (1.0 - slope1);
            ls_failed = 1;                                      // this line search failed
        }

	}

	finalcost = fX;		//return finalcost
}

void costfunction(double& cost, mat& gradient, mat& nn_params,const int input_layer_size,const int hidden_layer_size,const int num_labels, const mat& inputdata, const mat& y,const double lambda){
	//std::cout << "in costfunction..." << endl;

	mat Theta1 = nn_params.submat(0, 0, hidden_layer_size*(input_layer_size + 1) - 1, 0);
	Theta1.reshape(hidden_layer_size, input_layer_size + 1);
	//std::cout << "Theta1 rows: " << Theta1.n_rows << ", cols: " << Theta1.n_cols << endl;

	mat Theta2 = nn_params.submat(hidden_layer_size*(input_layer_size + 1), 0, hidden_layer_size*(input_layer_size + 1) - 1 + num_labels*(hidden_layer_size + 1) - 1, 0);
	Theta2.reshape(num_labels, hidden_layer_size + 1);
	//std::cout << "Theta2 rows: " << Theta2.n_rows << ", cols: " << Theta2.n_cols << endl;

	//std::cout << "Theta2: " << endl << Theta2.col(0) << endl;
	//pause();

	//constexpr int training_size = 4000;    //m
	//constexpr int input_layer_size = 784;  //k
	//constexpr int hidden_layer_size = 500;  //n
	// l  y_train size

	int l = y.n_rows;
	int m = inputdata.n_rows;
	//int k = input_layer_size;

	//create the  0 1 0 0 0 0 0 0 0 0 style representative matrix
	mat y_representative(l, num_labels,fill::zeros);
	for (unsigned int i=0;i<=(y.n_rows - 1);i++ ){
		int row_value = y(i,0);
		y_representative(i,row_value) = 1.0;
	}
	//std::cout << "y_rep: " << y_representative.row(0) << endl;

	//setup a_2
	mat a_1(inputdata.n_rows,inputdata.n_cols + 1,fill::ones);
	a_1.submat(0,1,a_1.n_rows-1,a_1.n_cols-1) = inputdata;
	mat z_2(a_1.n_rows,Theta1.n_rows,fill::zeros);

	//std::cout << "a_1: " << a_1(0, 0) << endl;

	//std::cout << "Theta1: " << Theta1(1, 2) << endl;

	//std::cout << "Theta1.t(): " << theta1t.row(0) << endl;

	z_2 = a_1 * Theta1.t();
	//std::cout << "z_2: " << z_2.row(0) << endl;
	//pause();

	mat a_2=z_2;
	mat a_2_ones(z_2.n_rows,z_2.n_cols+1,fill::ones);

	sigmoid(a_2,a_2);
	//std::cout << "a_2: " << a_2.row(0) << endl;
	//pause();

	a_2_ones.submat(0,1,a_2_ones.n_rows-1,a_2_ones.n_cols-1)=a_2;
	//std::cout << "a_2_ones: " << a_2_ones.n_rows << endl;
	//std::cout << "a_2_ones: " << a_2_ones.row(0) << endl;
	//pause();

	mat theta2t = Theta2.t();
	//std::cout << "theta2t: " << theta2t.row(0) << endl;
	//pause();

	//setup a_3
	mat z_3(a_2_ones.n_rows,Theta2.n_rows,fill::zeros);
	z_3 = a_2_ones * Theta2.t();
	//std::cout << "z_3: " << z_3.row(0) << endl;
	//pause();

	mat a_3 = z_3;
	//std::cout << "a_3: " << a_3(0, 0) << endl;
	sigmoid(a_3,a_3);

	//mat test1;

	//test1 << 1.0 << 2.0 << 3.0 << endr
	//	  << 4.0 << 5.0 << 6.0 << endr;

	//mat test2 = test1;
	//mat test3 = test1;
	//mat test2l = test2.transform([](double val){ return (log(val)); });

	//test3 = test1 % test2l;
	//std::cout << test3 << endl;
	//pause();

	//test2l = log(test2);
	//test3 = test1 % test2l;
	//std::cout << test3 << endl;
	//pause();

	//calculate cost
	mat summation(z_3.n_rows,z_3.n_cols,fill::zeros);

//	mat a_3l1 = a_3;
//	a_3l1.transform([](double val){ return (log(val)); });

	//std::cout << "log(a_3): " << log(a_3.row(0)) << endl;
	//std::cout << "transform log(a_3): " << a_3l1.row(0) << endl;
	//pause();

//	mat a_3l2 = a_3;
//	a_3l2.transform([](double val){ return (log(1 - val)); });

	summation = -y_representative % log(a_3) - ((1 - y_representative) % log(1-a_3));
	//std::cout << "summation: " << summation.row(0) << endl;
	//pause();
	cost = accu(summation);
	//std::cout << "Cost: " << cost << " m: " << m << endl;
	cost /= m;
	//std::cout << "Cost: " << cost << endl;
	//pause();

	//setup delta_3
	mat delta_3 = y_representative;
	delta_3 = a_3 - y_representative;
	//std::cout << "delta_3: " << delta_3.row(0) << endl;
	//pause();

	//setup sigmoid of z_2
	mat z_2_ones(z_2.n_rows, z_2.n_cols + 1, fill::ones);
	z_2_ones.submat(0,1,z_2_ones.n_rows-1,z_2_ones.n_cols-1) = z_2;
	//std::cout << "z_2_ones: " << z_2_ones.row(0) << endl;
	//pause();

	sigmoidGradient(z_2_ones,z_2_ones);

	//std::cout << "sigmoid_z_2: " << z_2_ones.row(0) << endl;
	//pause();

	//setup delta_2
	mat delta_2(delta_3.n_rows,Theta2.n_cols);
	delta_2 = (delta_3 * Theta2) % z_2_ones;

	//std::cout << "delta_2: " << delta_2.row(0) << endl;
	//pause();

	//setup and combine gradients
	mat gradient_2(delta_3.n_cols,a_2_ones.n_cols,fill::zeros);
	mat gradient_1(delta_2.n_cols-1,a_1.n_cols,fill::zeros);

	gradient_2 = (delta_3.t() * a_2_ones) / m;
	//std::cout << "gradient_2: " << gradient_2.row(0) << endl;
	//pause();

	gradient_1 = (delta_2.cols(1,delta_2.n_cols-1).t() * a_1) / m;
	//std::cout << "gradient_1: " << gradient_1.row(0) << endl;
	//pause();
	gradient_2.cols(1,gradient_2.n_cols-1) += Theta2.cols(1,Theta2.n_cols-1) * (lambda/m);
	gradient_1.cols(1,gradient_1.n_cols-1) += Theta1.cols(1,Theta1.n_cols-1) * (lambda/m);
	gradient_1.reshape(gradient_1.n_rows*gradient_1.n_cols,1);
	gradient_2.reshape(gradient_2.n_rows*gradient_2.n_cols,1);
	gradient = join_cols(gradient_1,gradient_2);
	//std::cout << "gradient: " << gradient.col(0) << endl;
	//pause();

	//std::cout << "leaving costfunction..." << endl;
}

void predict(const mat& Theta1,const mat& Theta2,const mat& inputdata, mat& predictions){
    mat x_holder(inputdata.n_rows,inputdata.n_cols+1,fill::ones);
    mat pre_h1(x_holder.n_rows,Theta1.n_rows);
    mat h1_ones(x_holder.n_rows,Theta1.n_rows+1,fill::ones);
    mat h2(x_holder.n_rows,Theta2.n_rows,fill::zeros);

	//std::cout << "inputdata: " << inputdata.n_rows << " " << inputdata.n_cols << endl;
	// std::cout << "x_holder: " << x_holder.n_rows << " " << x_holder.n_cols << endl;
    x_holder.submat(0,1,x_holder.n_rows-1,x_holder.n_cols-1) = inputdata;

    // std::cout << "Theta1: " << Theta1.n_rows << " " << Theta1.n_cols << endl;

    pre_h1 = x_holder * Theta1.t();

	// std::cout << "pre_h1: " << pre_h1.n_rows << " " << pre_h1.n_cols << endl;

    sigmoid(pre_h1,pre_h1);
    h1_ones.submat(0,1,h1_ones.n_rows-1,h1_ones.n_cols-1) = pre_h1;

    // std::cout << "h1_ones: " << h1_ones.n_rows << " " << h1_ones.n_cols << endl;
    // std::cout << "Theta2: " << Theta2.n_rows << " " << Theta2.n_cols << endl;

    h2 = h1_ones * Theta2.t();
    sigmoid(h2,h2);

    predictions = arma::max(h2,0);

}

void sigmoid(const mat& input, mat& output){
    output = exp(-input);
    output.transform([](double val){return (1.0/(1.0 + val));});
}

void sigmoidGradient(const mat& input, mat& output){
    output = exp(-input);
    output.transform([](double val){return (1.0/(1.0 + val));});
    output.transform([](double val){return (val * (1.0 - val));});
}

// void pauseJNS(){
// 	std::cout << std::endl << "Press ENTER to continue...";
// 	std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
// }

max_stars_repo_name: jshahbazi/mnist-cpp

max_issues_repo_name: jshahbazi/mnist-cpp

max_forks_repo_name: jshahbazi/mnist-cpp


text:
/**
 * @file test_potential.cpp
 * @brief This file defines the test cases for the potential calculation.
 *
 * @copyright Copyright (c) 2022
 *
 */

#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <cstddef>
#include <eigen3/Eigen/Core>
#include <iterator>
#include <memory>
#include <string>
#include <vector>

#include "proseco_planning/agent/cost_model/costModel.h"
#include "proseco_planning/agent/desire.h"
#include "proseco_planning/agent/vehicle.h"
#include "proseco_planning/config/computeOptions.h"
#include "proseco_planning/config/configuration.h"
#include "proseco_planning/config/defaultConfiguration.h"
#include "proseco_planning/config/scenarioOptions.h"

using namespace proseco_planning;

BOOST_AUTO_TEST_SUITE(potential)

BOOST_AUTO_TEST_CASE(laneCenterDeviation) {
  Config::create(config::scenarioSimple, config::optionsSimple);

  int lane{1};

  Vehicle vehicle(config::vehicle);
  vehicle.m_positionX = 0;
  vehicle.m_velocityX = 35.0;
  vehicle.setLane(lane);
  Desire desire = Desire(config::Desire(35.0, 0, lane, 0));

  std::vector<float> v1(50, 1.0);
  Eigen::MatrixXd w1 = config::CostModel::convertVectorToEigenMatrix(v1, 10, 5);
  std::vector<float> v2(5, 1.0);
  Eigen::MatrixXd w2 = config::CostModel::convertVectorToEigenMatrix(v2, 5, 1);
  // Set cost Parameter and generate cost model
  auto configCostModel = config::CostModel("costContinuous", -50, 20, 100, 30, -1, 0, -500, -500,
                                           -500, -10, 0, 1, 1, 1, 1, 1, 1, 1, w1, w2);

  auto costModel = CostModel::createCostModel(configCostModel);

  const unsigned int steps = 11;
  std::vector<float> potentialLaneCenterOffset(steps);
  for (size_t offset = 0; offset < potentialLaneCenterOffset.size(); ++offset) {
    vehicle.m_positionY               = sOpt().road.lane_width * (lane + offset / 10.f);
    potentialLaneCenterOffset[offset] = costModel->updateStatePotential(desire, vehicle);
  }

  std::vector<float> potentialLaneCenterOffsetReverse(potentialLaneCenterOffset.size());
  std::reverse_copy(std::begin(potentialLaneCenterOffset), std::end(potentialLaneCenterOffset),
                    std::begin(potentialLaneCenterOffsetReverse));
  for (size_t i = 0; i < potentialLaneCenterOffset.size(); ++i) {
    BOOST_REQUIRE_SMALL(potentialLaneCenterOffsetReverse[i] - potentialLaneCenterOffset[i],
                        float(0.001));
  }
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp


text:
/**
 * grove: ScoreRelocaliser.cpp
 * Copyright (c) Torr Vision Group, University of Oxford, 2017. All rights reserved.
 */

#include "relocalisation/interface/ScoreRelocaliser.h"
using namespace ORUtils;
using namespace tvgutil;

#include <boost/filesystem.hpp>
namespace bf = boost::filesystem;

#include <orx/base/MemoryBlockFactory.h>
using namespace orx;

#include "clustering/ExampleClustererFactory.h"
#include "features/FeatureCalculatorFactory.h"
#include "ransac/PreemptiveRansacFactory.h"
#include "relocalisation/shared/ScoreGTRelocaliser_Shared.h"

namespace grove {

//#################### CONSTRUCTORS ####################

ScoreRelocaliser::ScoreRelocaliser(const SettingsContainer_CPtr& settings, const std::string& settingsNamespace, DeviceType deviceType)
: m_backed(false),
  m_deviceType(deviceType),
  m_maxX(static_cast<float>(INT_MIN)),
  m_maxY(static_cast<float>(INT_MIN)),
  m_maxZ(static_cast<float>(INT_MIN)),
  m_minX(static_cast<float>(INT_MAX)),
  m_minY(static_cast<float>(INT_MAX)),
  m_minZ(static_cast<float>(INT_MAX)),
  m_settings(settings),
  m_settingsNamespace(settingsNamespace)
{
  // Determine the top-level parameters for the relocaliser.
  m_enableDebugging = m_settings->get_first_value<bool>(settingsNamespace + "enableDebugging", false);
  m_maxRelocalisationsToOutput = m_settings->get_first_value<uint32_t>(settingsNamespace + "maxRelocalisationsToOutput", 1);

  // Determine the reservoir-related parameters.
  m_maxReservoirsToUpdate = m_settings->get_first_value<uint32_t>(settingsNamespace + "maxReservoirsToUpdate", 256);  // Update the modes associated with this number of reservoirs for each train/update call.
  m_reservoirCapacity = m_settings->get_first_value<uint32_t>(settingsNamespace + "reservoirCapacity", 1024);
  m_rngSeed = m_settings->get_first_value<uint32_t>(settingsNamespace + "rngSeed", 42);

  // Determine the clustering-related parameters (the defaults are tentative values that seem to work).
  m_clustererSigma = m_settings->get_first_value<float>(settingsNamespace + "clustererSigma", 0.1f);
  m_clustererTau = m_settings->get_first_value<float>(settingsNamespace + "clustererTau", 0.05f);
  m_maxClusterCount = m_settings->get_first_value<uint32_t>(settingsNamespace + "maxClusterCount", ScorePrediction::Capacity);
  m_minClusterSize = m_settings->get_first_value<uint32_t>(settingsNamespace + "minClusterSize", 20);

  // Check that the maximum number of clusters to store in each leaf is within range.
  if(m_maxClusterCount > ScorePrediction::Capacity)
  {
    throw std::invalid_argument(settingsNamespace + "maxClusterCount > ScorePrediction::Capacity");
  }

  // Allocate the internal images.
  MemoryBlockFactory& mbf = MemoryBlockFactory::instance();
  m_descriptorsImage = mbf.make_image<DescriptorType>();
  m_groundTruthPredictionsImage = mbf.make_image<ScorePrediction>();
  m_keypointsImage = mbf.make_image<ExampleType>();
  m_predictionsImage = mbf.make_image<ScorePrediction>();

  // Instantiate the sub-components.
  m_featureCalculator = FeatureCalculatorFactory::make_da_rgbd_patch_feature_calculator(deviceType);
  m_preemptiveRansac = PreemptiveRansacFactory::make_preemptive_ransac(settings, settingsNamespace + "PreemptiveRansac.", deviceType);
}

//#################### DESTRUCTOR ####################

ScoreRelocaliser::~ScoreRelocaliser() {}

//#################### PUBLIC MEMBER FUNCTIONS ####################

void ScoreRelocaliser::finish_training()
{
  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  boost::lock_guard<boost::recursive_mutex> lock(m_mutex);

  // First update all of the clusters.
  update_all_clusters();

  // Then kill the contents of the reservoirs (we won't need them any more).
  m_relocaliserState->exampleReservoirs.reset();
  m_relocaliserState->lastExamplesAddedStartIdx = 0;
  m_relocaliserState->reservoirUpdateStartIdx = 0;

  // Finally, release the example clusterer.
  m_exampleClusterer.reset();
}

void ScoreRelocaliser::get_best_poses(std::vector<PoseCandidate>& poseCandidates) const
{
  m_preemptiveRansac->get_best_poses(poseCandidates);
}

Keypoint3DColourImage_CPtr ScoreRelocaliser::get_keypoints_image() const
{
  return m_keypointsImage;
}

ScorePredictionsImage_CPtr ScoreRelocaliser::get_predictions_image() const
{
  return m_predictionsImage;
}

ORUChar4Image_CPtr ScoreRelocaliser::get_visualisation_image(const std::string& key) const
{
  if(key == "gtpoints") return m_groundTruthPixelsToPointsImage;
  else if(key == "points") return m_pixelsToPointsImage;
  else return ORUChar4Image_CPtr();
}

void ScoreRelocaliser::load_from_disk(const std::string& inputFolder)
{
  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  // Otherwise, load its internal state from disk.
  m_relocaliserState->load_from_disk(inputFolder);
}

std::vector<Relocaliser::Result> ScoreRelocaliser::relocalise(const ORUChar4Image *colourImage, const ORFloatImage *depthImage, const Vector4f& depthIntrinsics) const
{
  boost::lock_guard<boost::recursive_mutex> lock(m_mutex);

  std::vector<Result> results;

  // Iff we have enough valid depth values, try to estimate the camera pose:
  if(m_preemptiveRansac->count_valid_depths(depthImage) > m_preemptiveRansac->get_min_nb_required_points())
  {
    // Step 1: Extract keypoints from the RGB-D image and compute descriptors for them.
    // FIXME: We only need to compute the descriptors if we're using the forest.
    m_featureCalculator->compute_keypoints_and_features(colourImage, depthImage, depthIntrinsics, m_keypointsImage.get(), m_descriptorsImage.get());

    // Step 2: Create a single SCoRe prediction (a single set of clusters) for each keypoint.
    make_predictions(colourImage);

    // Step 3: Perform P-RANSAC to try to estimate the camera pose.
    boost::optional<PoseCandidate> poseCandidate = m_preemptiveRansac->estimate_pose(m_keypointsImage, m_predictionsImage);

    // Step 4: If we succeeded in estimating a camera pose:
    if(poseCandidate)
    {
      // Add the pose to the results.
      Result result;
      result.pose.SetInvM(poseCandidate->cameraPose);
      result.quality = RELOCALISATION_GOOD;
      result.score = poseCandidate->energy;
      results.push_back(result);

      // If we're outputting multiple poses:
      if(m_maxRelocalisationsToOutput > 1)
      {
        // Get all of the candidates that survived the initial culling process during P-RANSAC.
        std::vector<PoseCandidate> candidates;
        m_preemptiveRansac->get_best_poses(candidates);

        // Add the best candidates to the results (skipping the first one, since it's the same one returned by estimate_pose above).
        const size_t maxElements = std::min<size_t>(candidates.size(), m_maxRelocalisationsToOutput);
        for(size_t i = 1; i < maxElements; ++i)
        {
          Result result;
          result.pose.SetInvM(candidates[i].cameraPose);
          result.quality = RELOCALISATION_GOOD;
          result.score = candidates[i].energy;
          results.push_back(result);
        }
      }
    }
  }

  // If debugging is enabled, update the visualisation images.
  if(m_enableDebugging)
  {
    make_visualisation_images(depthImage, results);
  }

  // If we're using the ground truth camera trajectory, increment the ground truth frame index.
  if(m_groundTruthTrajectory && m_groundTruthFrameIndex < m_groundTruthTrajectory->size())
  {
    ++m_groundTruthFrameIndex;
  }

  return results;
}

void ScoreRelocaliser::reset()
{
  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  boost::lock_guard<boost::recursive_mutex> lock(m_mutex);

  // Set up the clusterer if it isn't currently allocated (note that it can be deallocated by finish_training, so this can't just be moved to the constructor).
  if(!m_exampleClusterer)
  {
    m_exampleClusterer = ExampleClustererFactory<ExampleType,ClusterType,PredictionType::Capacity>::make_clusterer(
      m_clustererSigma, m_clustererTau, m_maxClusterCount, m_minClusterSize, m_deviceType
    );
  }

  // If the relocaliser's state already exists, reset it; if not, allocate it.
  if(m_relocaliserState) m_relocaliserState->reset();
  else m_relocaliserState.reset(new ScoreRelocaliserState(m_reservoirCount, m_reservoirCapacity, m_deviceType, m_rngSeed));

  // Reset the ground truth frame index.
  m_groundTruthFrameIndex = 0;
}

void ScoreRelocaliser::save_to_disk(const std::string& outputFolder) const
{
  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  // First make sure that the output folder exists.
  bf::create_directories(outputFolder);

  // Then save the relocaliser's internal state to disk.
  m_relocaliserState->save_to_disk(outputFolder);
}

void ScoreRelocaliser::set_backing_relocaliser(const ScoreRelocaliser_Ptr& backingRelocaliser)
{
  m_relocaliserState = backingRelocaliser->m_relocaliserState;
  m_backed = true;
}

void ScoreRelocaliser::set_ground_truth_trajectory(const std::vector<ORUtils::SE3Pose>& groundTruthTrajectory)
{
  m_groundTruthTrajectory = groundTruthTrajectory;
}

void ScoreRelocaliser::train(const ORUChar4Image *colourImage, const ORFloatImage *depthImage,
                             const Vector4f& depthIntrinsics, const ORUtils::SE3Pose& cameraPose)
{
  boost::lock_guard<boost::recursive_mutex> lock(m_mutex);

  // If debugging is enabled, update the maximum and minimum x, y and z coordinates visited by the camera during training.
  if(m_enableDebugging)
  {
    m_maxX = std::max(m_maxX, cameraPose.GetT().x);
    m_maxY = std::max(m_maxY, cameraPose.GetT().y);
    m_maxZ = std::max(m_maxZ, cameraPose.GetT().z);
    m_minX = std::min(m_minX, cameraPose.GetT().x);
    m_minY = std::min(m_minY, cameraPose.GetT().y);
    m_minZ = std::min(m_minZ, cameraPose.GetT().z);
  }

  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  // If we haven't reset since the last time finish_training was called, throw.
  if(!m_relocaliserState->exampleReservoirs)
  {
    throw std::runtime_error("Error: finish_training() has been called; the relocaliser cannot be trained again until reset() is called");
  }

  // Call the hook function (a function that should be overridden by derived classes to perform the actual training).
  train_sub(colourImage, depthImage, depthIntrinsics, cameraPose);

  // If there are any reservoirs:
  if(m_reservoirCount > 0)
  {
    // Cluster some of the reservoirs.
    const uint32_t nbReservoirsToUpdate = compute_nb_reservoirs_to_update();
    m_exampleClusterer->cluster_examples(
      m_relocaliserState->exampleReservoirs->get_reservoirs(), m_relocaliserState->exampleReservoirs->get_reservoir_sizes(),
      m_relocaliserState->reservoirUpdateStartIdx, nbReservoirsToUpdate, m_relocaliserState->predictionsBlock
    );

    // Store the index of the first reservoir that was just updated so that we can tell when there are no more clusters to update.
    m_relocaliserState->lastExamplesAddedStartIdx = m_relocaliserState->reservoirUpdateStartIdx;

    // Update the index of the first reservoir to subject to clustering during the next train/update call.
    update_reservoir_start_idx();
  }
}

void ScoreRelocaliser::update()
{
  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  boost::lock_guard<boost::recursive_mutex> lock(m_mutex);

  if(!m_relocaliserState->exampleReservoirs)
  {
    throw std::runtime_error("Error: finish_training() has been called; the relocaliser cannot be updated again until reset() is called");
  }

  // If we are back to the first reservoir that was updated when the last batch of examples were added to the
  // reservoirs, there is no need to perform further updates, since we would get the same clusters. Note that
  // this check only works if m_maxReservoirsToUpdate remains constant throughout the whole program.
  if(m_relocaliserState->reservoirUpdateStartIdx == m_relocaliserState->lastExamplesAddedStartIdx) return;

  // Otherwise, cluster the next batch of reservoirs, and update the index of the first reservoir to subject to
  // clustering during the next train/update call.
  const uint32_t updateCount = compute_nb_reservoirs_to_update();
  m_exampleClusterer->cluster_examples(
    m_relocaliserState->exampleReservoirs->get_reservoirs(), m_relocaliserState->exampleReservoirs->get_reservoir_sizes(),
    m_relocaliserState->reservoirUpdateStartIdx, updateCount, m_relocaliserState->predictionsBlock
  );

  update_reservoir_start_idx();
}

void ScoreRelocaliser::update_all_clusters()
{
  // If this relocaliser is "backed" by another one, early out.
  if(m_backed) return;

  boost::lock_guard<boost::recursive_mutex> lock(m_mutex);

  // Repeatedly call update until we get back to the batch of reservoirs that was updated last time train() was called.
  while(m_relocaliserState->reservoirUpdateStartIdx != m_relocaliserState->lastExamplesAddedStartIdx)
  {
    update();
  }
}

//#################### PROTECTED MEMBER FUNCTIONS ####################

void ScoreRelocaliser::make_visualisation_images(const ORFloatImage *depthImage, const std::vector<Result>& results) const
{
  if(m_groundTruthTrajectory && m_groundTruthFrameIndex < m_groundTruthTrajectory->size())
  {
    // Update the normal pixel to points image, using the ground truth world to camera transformation.
    const Matrix4f& worldToCamera = (*m_groundTruthTrajectory)[m_groundTruthFrameIndex].GetM();
    update_pixels_to_points_image(worldToCamera, m_predictionsImage, m_pixelsToPointsImage);

    // If requested, also update the ground truth pixel to points image.
    if(m_settings->get_first_value<bool>(m_settingsNamespace + "makeGroundTruthPointsImage", false))
    {
      const Matrix4f& cameraToWorld = (*m_groundTruthTrajectory)[m_groundTruthFrameIndex].GetInvM();
      set_ground_truth_predictions_for_keypoints(m_keypointsImage, cameraToWorld, m_groundTruthPredictionsImage);
      update_pixels_to_points_image(worldToCamera, m_groundTruthPredictionsImage, m_groundTruthPixelsToPointsImage);
    }
  }
  else if(!results.empty())
  {
    // Update the normal pixel to points image, using the "best" relocalised pose as the world to camera transformation.
    // Note: We use this pose as a default, even though it may later be either refined by ICP or discarded in favour of a different pose.
    update_pixels_to_points_image(results[0].pose, m_predictionsImage, m_pixelsToPointsImage);
  }
}

void ScoreRelocaliser::set_ground_truth_predictions_for_keypoints(const Keypoint3DColourImage_CPtr& keypointsImage, const Matrix4f& cameraToWorld,
                                                                  ScorePredictionsImage_Ptr& outputPredictions) const
{
  const Vector2i imgSize = keypointsImage->noDims;

  // Make sure that the output predictions image has the right size (this is a no-op after the first time).
  outputPredictions->ChangeDims(imgSize);

  // If necessary, copy the keypoints image across to the CPU.
  if(m_deviceType == DEVICE_CUDA) keypointsImage->UpdateHostFromDevice();

  const Keypoint3DColour *keypointsPtr = keypointsImage->GetData(MEMORYDEVICE_CPU);
  ScorePrediction *outputPredictionsPtr = outputPredictions->GetData(MEMORYDEVICE_CPU);

#ifdef WITH_OPENMP
  #pragma omp parallel for
#endif
  for(int y = 0; y < imgSize.y; ++y)
  {
    for(int x = 0; x < imgSize.x; ++x)
    {
      set_ground_truth_prediction_for_keypoint(x, y, imgSize, keypointsPtr, cameraToWorld, outputPredictionsPtr);
    }
  }

  // If necessary, copy the output predictions back across to the GPU.
  if(m_deviceType == DEVICE_CUDA) outputPredictions->UpdateDeviceFromHost();
}

//#################### PRIVATE MEMBER FUNCTIONS ####################

uint32_t ScoreRelocaliser::compute_nb_reservoirs_to_update() const
{
  // Either the standard number of reservoirs to update, or the number remaining before the end of the memory block.
  return std::min(m_maxReservoirsToUpdate, m_reservoirCount - m_relocaliserState->reservoirUpdateStartIdx);
}

void ScoreRelocaliser::update_pixels_to_points_image(const ORUtils::SE3Pose& worldToCamera, const ScorePredictionsImage_Ptr& predictionsImage,
                                                     ORUChar4Image_Ptr& pixelsToPointsImage) const
{
  // Ensure that the keypoints and SCoRe predictions are available on the CPU.
  m_keypointsImage->UpdateHostFromDevice();
  predictionsImage->UpdateHostFromDevice();

  // If the pixels to points image hasn't been allocated yet, allocate it now.
  if(!pixelsToPointsImage) pixelsToPointsImage.reset(new ORUChar4Image(m_keypointsImage->noDims, true, true));

  // For each pixel:
  Vector4u *p = pixelsToPointsImage->GetData(MEMORYDEVICE_CPU);
  for(int i = 0, pixelCount = static_cast<int>(pixelsToPointsImage->dataSize); i < pixelCount; ++i, ++p)
  {
    p->r = p->g = p->b = 0;
    p->a = 255;

    // If the pixel has a valid keypoint, look up the position of the cluster (if any) in the corresponding prediction that is closest to it.
    const ExampleType& keypoint = m_keypointsImage->GetData(MEMORYDEVICE_CPU)[i];
    if(!keypoint.valid) continue;
    const PredictionType& prediction = predictionsImage->GetData(MEMORYDEVICE_CPU)[i];
    const int closestModeIdx = find_closest_mode(worldToCamera.GetInvM() * keypoint.position, prediction);
    if(closestModeIdx == -1) continue;
    const Vector3f& clusterPos = prediction.elts[closestModeIdx].position;

    // Colour the pixel in the pixels to points image based on the cluster's position in world space.
    float scale = 2.0f;
    float centre = 0.5f;
    float offset = centre - 1.0f / (scale * 2.0f);
    float xCoeff = ((CLAMP(clusterPos.x,m_minX,m_maxX) - m_minX) / (m_maxX - m_minX)) / scale + offset;
    float yCoeff = ((CLAMP(clusterPos.y,m_minY,m_maxY) - m_minY) / (m_maxY - m_minY)) / scale + offset;
    float zCoeff = ((CLAMP(clusterPos.z,m_minZ,m_maxZ) - m_minZ) / (m_maxZ - m_minZ)) / scale + offset;
    p->r = static_cast<unsigned char>(255 * xCoeff);
    p->g = static_cast<unsigned char>(255 * yCoeff);
    p->b = static_cast<unsigned char>(255 * zCoeff);
    p->a = 255;
  }

  // Update the GPU copy of the pixels to points image (if it exists).
  pixelsToPointsImage->UpdateDeviceFromHost();
}

void ScoreRelocaliser::update_reservoir_start_idx()
{
  m_relocaliserState->reservoirUpdateStartIdx += m_maxReservoirsToUpdate;

  // If we go past the end of the list of reservoirs, loop back round.
  if(m_relocaliserState->reservoirUpdateStartIdx >= m_reservoirCount)
  {
    m_relocaliserState->reservoirUpdateStartIdx = 0;
  }
}

}

max_stars_repo_name: torrvision/spaint

max_issues_repo_name: torrvision/spaint

max_forks_repo_name: torrvision/spaint


text:
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <list>
#include <vector>
#include <string>
#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <vikit/pinhole_camera.h>
#include <vikit/abstract_camera.h>
#include <vikit/atan_camera.h>
#include <vikit/math_utils.h>
#include <vikit/file_reader.h>
#include <vikit/blender_utils.h>
#include <vikit/timer.h>
#include <iostream>
#include <svo/feature_detection.h>
#include <svo/depth_filter.h>
#include <svo/frame.h>
#include <svo/feature.h>
#include <svo/point.h>
#include <svo/config.h>
#include "test_utils.h"

namespace {

struct ConvergedSeed {
  int x_, y_;
  double depth_, error_;
  ConvergedSeed(int x, int y, double depth, double error) :
    x_(x), y_(y), depth_(depth), error_(error)
  {}
};

/// DepthFilter Test-Fixture
class DepthFilterTest {
 public:
  DepthFilterTest();
  virtual ~DepthFilterTest();
  void depthFilterCb(svo::Point* point, double depth_sigma2);
  void testReconstruction(std::string dataset_dir, std::string experiment_name);

  std::list<ConvergedSeed> results_;
  std::vector<double> errors_;
  size_t n_converged_seeds_;
  vk::PinholeCamera* cam_;
  svo::DepthFilter* depth_filter_;
  svo::FramePtr frame_ref_;
  svo::FramePtr frame_cur_;
  cv::Mat depth_ref_;
};

DepthFilterTest::DepthFilterTest() :
    n_converged_seeds_(0),
    cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)),
    depth_filter_(NULL)
{
  errors_.reserve(1000);
}

DepthFilterTest::~DepthFilterTest()
{
  delete cam_;
}

void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2)
{
  double depth = (frame_ref_->pos() - point->pos_).norm();
  double error = fabs(depth - depth_ref_.at<float>(point->obs_.front()->px[1],
                                                   point->obs_.front()->px[0]));
  results_.push_back(ConvergedSeed(
      point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error));
  errors_.push_back(error);
  delete point->obs_.front();
  delete point;
  ++n_converged_seeds_;
}

void DepthFilterTest::testReconstruction(
    std::string dataset_dir,
    std::string experiment_name)
{
  vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
  std::vector<vk::blender_utils::file_format::ImageNameAndPose> sequence;
  sequence_file_reader.skipComments();
  if(!sequence_file_reader.next())
    std::runtime_error("Failed to open sequence file");
  sequence_file_reader.readAllEntries(sequence);
  std::cout << "RUN EXPERIMENT: read " << sequence.size() << " dataset entries." << std::endl;
  std::vector<vk::blender_utils::file_format::ImageNameAndPose>::iterator it = sequence.begin();
  vk::Timer t;
  std::list<size_t> n_converged_per_iteration;

  svo::feature_detection::DetectorPtr feature_detector(
      new svo::feature_detection::FastDetector(
          cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()));
  svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2);
  depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb);
  depth_filter_->options_.verbose = true;

  for(int i=0; it != sequence.end() && i<20; ++it, ++i)
  {
    std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png");
    printf("reading image: '%s'\n", img_name.c_str());
    cv::Mat img(cv::imread(img_name, 0));
    assert(!img.empty());

    Sophus::SE3 T_w_f(it->q_, it->t_);
    if(i == 0)
    {
      // create reference frame and load ground truth depthmap
      frame_ref_ = boost::make_shared<svo::Frame>(cam_, img, 0.0);
      frame_ref_->T_f_w_ = T_w_f.inverse();
      depth_filter_->addKeyframe(frame_ref_, 2, 0.5);
      vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_);
      continue;
    }

    n_converged_seeds_ = 0;
    frame_cur_ = boost::make_shared<svo::Frame>(cam_, img, 0.0);
    frame_cur_->T_f_w_ = T_w_f.inverse();
    depth_filter_->addFrame(frame_cur_);
    n_converged_per_iteration.push_back(n_converged_seeds_);
  }
  printf("Experiment '%s' took %f ms\n", experiment_name.c_str(), t.stop()*1000);

  // compute mean, median and variance of error in converged area
  {
    printf("# converged:  \t %zu (ref: 287)\n", errors_.size());
    double sum_error = 0;
    std::for_each(errors_.begin(), errors_.end(), [&](double& e){sum_error+=e;});
    printf("mean error:   \t %f cm (ref: 0.080357)\n", sum_error*100/errors_.size());
    std::vector<double>::iterator it = errors_.begin()+0.5*errors_.size();
    std::nth_element(errors_.begin(), it, errors_.end());
    printf("50-percentile: \t %f cm (ref: 0.062042)\n", *it*100);
    it = errors_.begin()+0.8*errors_.size();
    std::nth_element(errors_.begin(), it, errors_.end());
    printf("80-percentile: \t %f cm (ref: 0.124526)\n", *it*100);
    it = errors_.begin()+0.95*errors_.size();
    std::nth_element(errors_.begin(), it, errors_.end());
    printf("95-percentile: \t %f cm (ref: 0.200417)\n", *it*100);
  }

  // trace error
  std::string trace_dir(svo::test_utils::getTraceDir());
  std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt");
  std::ofstream ofs(trace_name.c_str());
  for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
    ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl;
  ofs.close();

  // trace convergence rate
  trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt";
  ofs.open(trace_name.c_str());
  for(std::list<size_t>::iterator it=n_converged_per_iteration.begin();
      it!=n_converged_per_iteration.end(); ++it)
    ofs << *it << std::endl;
  ofs.close();

  // write ply file for pointcloud visualization in Meshlab
  trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply";
  ofs.open(trace_name.c_str());
  ofs << "ply" << std::endl
      << "format ascii 1.0" << std::endl
	  << "element vertex " << results_.size() << std::endl
	  << "property float x" << std::endl
	  << "property float y" << std::endl
	  << "property float z" << std::endl
	  << "property uchar blue" << std::endl
	  << "property uchar green" << std::endl
	  << "property uchar red" << std::endl
	  << "end_header" << std::endl;

  for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
  {
	cv::Vec3b c = frame_ref_->img_pyr_[0].at<cv::Vec3b>(i->y_, i->x_);
	Eigen::Vector3d p = cam_->cam2world(i->x_, i->y_)*i->depth_;
	ofs << p[0] << " " << p[1] << " " << p[2] << " "
	    << (int) c[0] << " " << (int) c[1] << " " << (int) c[2] << std::endl;
  }
}

}  // namespace

int main(int argc, char** argv)
{
  DepthFilterTest test;
  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
  std::string experiment_name("sin2_tex2_h1_v8_d");
  test.testReconstruction(dataset_dir, experiment_name);
  return 0;
}

max_stars_repo_name: vatanaksoytezer/zephyr

max_issues_repo_name: VatanTezer/zephyr

max_forks_repo_name: VatanTezer/zephyr


text:
///////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) Tavendo GmbH
//
// Boost Software License - Version 1.0 - August 17th, 2003
//
// Permission is hereby granted, free of charge, to any person or organization
// obtaining a copy of the software and accompanying documentation covered by
// this license (the "Software") to use, reproduce, display, distribute,
// execute, and transmit the Software, and to prepare derivative works of the
// Software, and to permit third-parties to whom the Software is furnished to
// do so, all subject to the following:
//
// The copyright notices in the Software and this entire statement, including
// the above license grant, this restriction and the following disclaimer,
// must be included in all copies of the Software, in whole or in part, and
// all derivative works of the Software, unless such copies or derivative
// works are solely in the form of machine-executable object code generated by
// a source language processor.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
///////////////////////////////////////////////////////////////////////////////

#include "wamp_message.hpp"
#include "wamp_message_type.hpp"

#include <boost/lexical_cast.hpp>
#include <stdexcept>
#include <tuple>

namespace autobahn {

inline wamp_invocation_impl::wamp_invocation_impl()
    : m_zone()
    , m_arguments(EMPTY_ARGUMENTS)
    , m_kw_arguments(EMPTY_KW_ARGUMENTS)
    , m_send_result_fn()
    , m_request_id(0)
    , m_progressive_results_expected(false)
{
}

inline const std::string& wamp_invocation_impl::uri() const
{
    return m_uri;
}

inline std::size_t wamp_invocation_impl::number_of_arguments() const
{
    return m_arguments.type == msgpack::type::ARRAY ? m_arguments.via.array.size : 0;
}

inline std::size_t wamp_invocation_impl::number_of_kw_arguments() const
{
    return m_kw_arguments.type == msgpack::type::MAP ? m_kw_arguments.via.map.size : 0;
}

template <typename T>
inline T wamp_invocation_impl::argument(std::size_t index) const
{
    if (m_arguments.type != msgpack::type::ARRAY || m_arguments.via.array.size <= index) {
        throw std::out_of_range("no argument at index " + boost::lexical_cast<std::string>(index));
    }
    return m_arguments.via.array.ptr[index].as<T>();
}

template <typename List>
inline List wamp_invocation_impl::arguments() const
{
    return m_arguments.as<List>();
}

template <typename List>
inline void wamp_invocation_impl::get_arguments(List& args) const
{
    m_arguments.convert(args);
}

template <typename... T>
inline void wamp_invocation_impl::get_each_argument(T&... args) const
{
    auto args_tuple = std::make_tuple(std::ref(args)...);
    m_arguments.convert(args_tuple);
}

template <typename T>
inline T wamp_invocation_impl::kw_argument(const std::string& key) const
{
    if (m_kw_arguments.type != msgpack::type::MAP) {
        throw msgpack::type_error();
    }
    for (std::size_t i = 0; i < m_kw_arguments.via.map.size; ++i) {
        const msgpack::object_kv& kv = m_kw_arguments.via.map.ptr[i];
        if (kv.key.type == msgpack::type::STR && key.size() == kv.key.via.str.size
                && key.compare(0, key.size(), kv.key.via.str.ptr, kv.key.via.str.size) == 0)
        {
            return kv.val.as<T>();
        }
    }
    throw std::out_of_range(key + " keyword argument doesn't exist");
}

template <typename T>
inline T wamp_invocation_impl::kw_argument(const char* key) const
{
    if (m_kw_arguments.type != msgpack::type::MAP) {
        throw msgpack::type_error();
    }
    std::size_t key_size = strlen(key);
    for (std::size_t i = 0; i < m_kw_arguments.via.map.size; ++i) {
        const msgpack::object_kv& kv = m_kw_arguments.via.map.ptr[i];
        if (kv.key.type == msgpack::type::STR && key_size == kv.key.via.str.size
                && memcmp(key, kv.key.via.str.ptr, key_size) == 0)
        {
            return kv.val.as<T>();
        }
    }
    throw std::out_of_range(std::string(key) + " keyword argument doesn't exist");
}

template <typename T>
inline T wamp_invocation_impl::kw_argument_or(const std::string& key, const T& fallback) const
{
    if (m_kw_arguments.type != msgpack::type::MAP) {
        throw msgpack::type_error();
    }
    for (std::size_t i = 0; i < m_kw_arguments.via.map.size; ++i) {
        const msgpack::object_kv& kv = m_kw_arguments.via.map.ptr[i];
        if (kv.key.type == msgpack::type::STR && key.size() == kv.key.via.str.size
                && key.compare(0, key.size(), kv.key.via.str.ptr, kv.key.via.str.size) == 0)
        {
            return kv.val.as<T>();
        }
    }
    return fallback;
}

template <typename T>
inline T wamp_invocation_impl::kw_argument_or(const char* key, const T& fallback) const
{
    if (m_kw_arguments.type != msgpack::type::MAP) {
        throw msgpack::type_error();
    }
    std::size_t key_size = strlen(key);
    for (std::size_t i = 0; i < m_kw_arguments.via.map.size; ++i) {
        const msgpack::object_kv& kv = m_kw_arguments.via.map.ptr[i];
        if (kv.key.type == msgpack::type::STR && key_size == kv.key.via.str.size
                && memcmp(key, kv.key.via.str.ptr, key_size) == 0)
        {
            return kv.val.as<T>();
        }
    }
    return fallback;
}

template <typename Map>
inline Map wamp_invocation_impl::kw_arguments() const
{
    return m_kw_arguments.as<Map>();
}

template <typename Map>
inline void wamp_invocation_impl::get_kw_arguments(Map& kw_args) const
{
    m_kw_arguments.convert(kw_args);
}

inline bool wamp_invocation_impl::progressive_results_expected() const
{
    return m_progressive_results_expected;
}

inline void wamp_invocation_impl::empty_result()
{
    throw_if_not_sendable();

    // [YIELD, INVOCATION.Request|id, Options|dict]
    auto message = std::make_shared<wamp_message>(3);
    message->set_field(0, static_cast<int>(message_type::YIELD));
    message->set_field(1, m_request_id);
    message->set_field(2, std::map<int, int>() /* No details */);

    m_send_result_fn(message);
    m_send_result_fn = send_result_fn();
}

template<typename List>
inline void wamp_invocation_impl::send_result(const List& arguments, wamp_invocation_impl::result_type resultType)
{
    throw_if_not_sendable();
    if (resultType == intermediary && !m_progressive_results_expected)
    {
        //Discard intermediate results.  Other option is to throw, since method could check if progressive results are expected
        return;
    }
    // [YIELD, INVOCATION.Request|id, Options|dict, Arguments|list]
    auto message = std::make_shared<wamp_message>(4);
    message->set_field(0, static_cast<int>(message_type::YIELD));
    message->set_field(1, m_request_id);
    if (resultType == intermediary)
    {
        message->set_field(2, std::map<std::string, bool>{ {"progress", true} });
    }
    else
    {
        message->set_field(2, std::map<int, int>() /* No details */);
    }
    message->set_field(3, arguments);

    m_send_result_fn(message);
    if (resultType != intermediary)
    {
        //Final result clears send function
        m_send_result_fn = send_result_fn();
    }
}

template<typename List, typename Map>
inline void wamp_invocation_impl::send_result(
        const List& arguments, const Map& kw_arguments, wamp_invocation_impl::result_type resultType)
{
    throw_if_not_sendable();
    if (resultType == intermediary && !m_progressive_results_expected)
    {
        //Discard intermediate results.  Other option is to throw, since method could check if progressive results are expected
        return;
    }

    // [YIELD, INVOCATION.Request|id, Options|dict, Arguments|list, ArgumentsKw|dict]
    auto message = std::make_shared<wamp_message>(5);
    message->set_field(0, static_cast<int>(message_type::YIELD));
    message->set_field(1, m_request_id);

    if (resultType == intermediary)
    {
        message->set_field(2, std::map<std::string, bool>{ {"progress", true} });
    }
    else
    {
        message->set_field(2, std::map<int, int>() /* No details */);
    }

    message->set_field(3, arguments);
    message->set_field(4, kw_arguments);

    m_send_result_fn(message);
    if (resultType != intermediary)
    {
        //Final result clears send function
        m_send_result_fn = send_result_fn();
    }
}

template <typename List>
inline void wamp_invocation_impl::progress(const List& arguments)
{
    send_result<List>(arguments, intermediary);
}

template <typename List, typename Map>
inline void wamp_invocation_impl::progress(const List& arguments, const Map& kw_arguments)
{
    send_result<List, Map>(arguments, kw_arguments, intermediary);
}

template <typename List>
inline void wamp_invocation_impl::result(const List& arguments)
{
    send_result<List>(arguments, final);
}

template <typename List, typename Map>
inline void wamp_invocation_impl::result(const List& arguments, const Map& kw_arguments)
{
    send_result<List, Map>(arguments, kw_arguments, final);
}

inline void wamp_invocation_impl::error(const std::string& error_uri)
{
    throw_if_not_sendable();

    // [ERROR, INVOCATION, INVOCATION.Request|id, Details|dict, Error|uri]
    auto message = std::make_shared<wamp_message>(5);
    message->set_field(0, static_cast<int>(message_type::ERROR));
    message->set_field(1, static_cast<int>(message_type::INVOCATION));
    message->set_field(2, m_request_id);
    message->set_field(3, std::map<int, int>() /* No details */);
    message->set_field(4, error_uri);

    m_send_result_fn(message);
    m_send_result_fn = send_result_fn();
}

template <typename List>
inline void wamp_invocation_impl::error(const std::string& error_uri, const List& arguments)
{
    throw_if_not_sendable();

    // [ERROR, INVOCATION, INVOCATION.Request|id, Details|dict, Error|uri, Arguments|list]
    auto message = std::make_shared<wamp_message>(6);
    message->set_field(0, static_cast<int>(message_type::ERROR));
    message->set_field(1, static_cast<int>(message_type::INVOCATION));
    message->set_field(2, m_request_id);
    message->set_field(3, std::map<int, int>() /* No details */);
    message->set_field(4, error_uri);
    message->set_field(5, arguments);

    m_send_result_fn(message);
    m_send_result_fn = send_result_fn();
}

template <typename List, typename Map>
inline void wamp_invocation_impl::error(
        const std::string& error_uri,
        const List& arguments, const Map& kw_arguments)
{
    throw_if_not_sendable();

    // [ERROR, INVOCATION, INVOCATION.Request|id, Details|dict, Error|uri, Arguments|list, ArgumentsKw|dict]
    auto message = std::make_shared<wamp_message>(7);
    message->set_field(0, static_cast<int>(message_type::ERROR));
    message->set_field(1, static_cast<int>(message_type::INVOCATION));
    message->set_field(2, m_request_id);
    message->set_field(3, std::map<int, int>() /* No details */);
    message->set_field(4, error_uri);
    message->set_field(5, arguments);
    message->set_field(6, kw_arguments);

    m_send_result_fn(message);
    m_send_result_fn = send_result_fn();
}

inline void wamp_invocation_impl::set_send_result_fn(send_result_fn&& send_result)
{
    m_send_result_fn = std::move(send_result);
}

inline void wamp_invocation_impl::set_details(const msgpack::object& details)
{
    m_uri = value_for_key_or<std::string>(details, "procedure", std::string());
    m_progressive_results_expected = value_for_key_or<bool>(details, "receive_progress", false);
}

inline void wamp_invocation_impl::set_request_id(std::uint64_t request_id)
{
    m_request_id = request_id;
}

inline void wamp_invocation_impl::set_zone(msgpack::zone&& zone)
{
    m_zone = std::move(zone);
}

inline void wamp_invocation_impl::set_arguments(const msgpack::object& arguments)
{
    m_arguments = arguments;
}

inline void wamp_invocation_impl::set_kw_arguments(const msgpack::object& kw_arguments)
{
    m_kw_arguments = kw_arguments;
}

inline bool wamp_invocation_impl::sendable() const
{
    return static_cast<bool>(m_send_result_fn);
}

inline void wamp_invocation_impl::throw_if_not_sendable() const
{
    if (!sendable()) {
        throw std::runtime_error("tried to call result() or error() but wamp_invocation "
                "is not sendable (double call?)");
    }
}

} // namespace autobahn

ext: ipp

max_stars_repo_name: BrianBland/autobahn-cpp

max_issues_repo_name: BrianBland/autobahn-cpp

max_forks_repo_name: BrianBland/autobahn-cpp


text:
// Copyright (c) 2011 Sirikata Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can
// be found in the LICENSE file.

#include "ManualObjectQueryProcessor.hpp"
#include <sirikata/core/network/Frame.hpp>
#include "Protocol_Prox.pbj.hpp"
#include "Protocol_Loc.pbj.hpp"
#include "Protocol_Frame.pbj.hpp"

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#define QPLOG(lvl, msg) SILOG(manual-query-processor, lvl, msg)

namespace Sirikata {
namespace OH {
namespace Manual {

using std::tr1::placeholders::_1;
using std::tr1::placeholders::_2;
using std::tr1::placeholders::_3;

namespace {

// Helpers for encoding requests
String initRequest() {
    using namespace boost::property_tree;
    try {
        ptree pt;
        pt.put("action", "init");
        std::stringstream data_json;
        write_json(data_json, pt);
        return data_json.str();
    }
    catch(json_parser::json_parser_error exc) {
        QPLOG(detailed, "Failed to encode 'init' request.");
        return "";
    }
}

String destroyRequest() {
    using namespace boost::property_tree;
    try {
        ptree pt;
        pt.put("action", "destroy");
        std::stringstream data_json;
        write_json(data_json, pt);
        return data_json.str();
    }
    catch(json_parser::json_parser_error exc) {
        QPLOG(detailed, "Failed to encode 'destroy' request.");
        return "";
    }
}

String refineRequest(const std::vector<ObjectReference>& aggs) {
    using namespace boost::property_tree;
    try {
        ptree nodes;
        for(uint32 i = 0; i < aggs.size(); i++) {
            nodes.put( aggs[i].toString(), aggs[i].toString() );
        }

        ptree pt;
        pt.put("action", "refine");
        pt.put_child("nodes", nodes);
        std::stringstream data_json;
        write_json(data_json, pt);
        return data_json.str();
    }
    catch(json_parser::json_parser_error exc) {
        QPLOG(detailed, "Failed to encode 'refine' request.");
        return "";
    }
}

String coarsenRequest(const std::vector<ObjectReference>& aggs) {
    using namespace boost::property_tree;
    try {
        ptree nodes;
        for(uint32 i = 0; i < aggs.size(); i++) {
            nodes.put( aggs[i].toString(), aggs[i].toString() );
        }

        ptree pt;
        pt.put("action", "coarsen");
        pt.put_child("nodes", nodes);
        std::stringstream data_json;
        write_json(data_json, pt);
        return data_json.str();
    }
    catch(json_parser::json_parser_error exc) {
        QPLOG(detailed, "Failed to encode 'coarsen' request.");
        return "";
    }
}

void addAdditionResult(Sirikata::Protocol::Prox::ProximityUpdate& obj_results, const SpaceObjectReference& observed, const SequencedPresenceProperties& props) {
    Sirikata::Protocol::Prox::IObjectAddition addition = obj_results.add_addition();
    addition.set_object(observed.object().getAsUUID());

    addition.set_seqno(props.maxSeqNo());

    Sirikata::Protocol::ITimedMotionVector motion = addition.mutable_location();
    TimedMotionVector3f loc = props.location();
    motion.set_t(loc.updateTime());
    motion.set_position(loc.position());
    motion.set_velocity(loc.velocity());

    TimedMotionQuaternion orient = props.orientation();
    Sirikata::Protocol::ITimedMotionQuaternion msg_orient = addition.mutable_orientation();
    msg_orient.set_t(orient.updateTime());
    msg_orient.set_position(orient.position());
    msg_orient.set_velocity(orient.velocity());

    addition.set_bounds( props.bounds() );
    const String& mesh = props.mesh().toString();
    if (mesh.size() > 0)
        addition.set_mesh(mesh);
    const String& phy = props.physics();
    if (phy.size() > 0)
        addition.set_physics(phy);

    // FIXME parent?
    // FIXME should we expose whether the object is an aggregate?
}

void addRemovalResult(Sirikata::Protocol::Prox::ProximityUpdate& obj_results, const SpaceObjectReference& observed, uint64 seqno, bool permanent) {
    Sirikata::Protocol::Prox::IObjectRemoval removal = obj_results.add_removal();
    removal.set_object(observed.object().getAsUUID());
    removal.set_seqno(seqno);
    removal.set_type(
        permanent ?
        Sirikata::Protocol::Prox::ObjectRemoval::Permanent :
        Sirikata::Protocol::Prox::ObjectRemoval::Transient
    );
}

}

ManualObjectQueryProcessor* ManualObjectQueryProcessor::create(ObjectHostContext* ctx, const String& args) {
    return new ManualObjectQueryProcessor(ctx);
}

ManualObjectQueryProcessor::ManualObjectQueryProcessor(ObjectHostContext* ctx)
 : mContext(ctx)
{
}

ManualObjectQueryProcessor::~ManualObjectQueryProcessor() {
}

void ManualObjectQueryProcessor::start() {
    mContext->objectHost->SpaceNodeSessionManager::addListener(static_cast<SpaceNodeSessionListener*>(this));
    mContext->objectHost->ObjectNodeSessionProvider::addListener(static_cast<ObjectNodeSessionListener*>(this));
}

void ManualObjectQueryProcessor::stop() {
    mContext->objectHost->ObjectNodeSessionProvider::removeListener(static_cast<ObjectNodeSessionListener*>(this));
    mContext->objectHost->SpaceNodeSessionManager::removeListener(static_cast<SpaceNodeSessionListener*>(this));

    mObjectState.clear();
    mServerQueries.clear();
}

void ManualObjectQueryProcessor::onSpaceNodeSession(const OHDP::SpaceNodeID& id, OHDPSST::Stream::Ptr sn_stream) {
    QPLOG(detailed, "New space node session " << id);
    assert(mServerQueries.find(id) == mServerQueries.end());
    mServerQueries.insert(
        ServerQueryMap::value_type(
            id,
            ServerQueryStatePtr(new ServerQueryState(mContext, sn_stream))
        )
    );

    sn_stream->listenSubstream(OBJECT_PORT_LOCATION,
        std::tr1::bind(&ManualObjectQueryProcessor::handleLocationSubstream, this,
            id, _1, _2
        )
    );
}

void ManualObjectQueryProcessor::onSpaceNodeSessionEnded(const OHDP::SpaceNodeID& id) {
    mServerQueries.erase(id);
}

void ManualObjectQueryProcessor::onObjectNodeSession(const SpaceID& space, const ObjectReference& oref, const OHDP::NodeID& id) {
    QPLOG(detailed, "New object-space node session: " << oref << " connected to " << space << "-" << id);

    OHDP::SpaceNodeID snid(space, id);
    SpaceObjectReference sporef(space, oref);

    ObjectStateMap::iterator obj_it = mObjectState.find(sporef);

    // First, clear out old state
    if (obj_it != mObjectState.end() && obj_it->second.node != OHDP::NodeID::null()) {
        OHDP::SpaceNodeID old_snid(obj_it->first.space(), obj_it->second.node);
        ServerQueryMap::iterator old_serv_it = mServerQueries.find(old_snid);
        assert(old_serv_it != mServerQueries.end());
        decrementServerQuery(old_serv_it);
    }

    // Update state
    // Object no longer has a session, it's no longer relevant
    if (id == OHDP::NodeID::null()) {
        mObjectState.erase(obj_it);
        return;
    }
    // Otherwise it's new/migrating
    if (obj_it == mObjectState.end())
        obj_it = mObjectState.insert( ObjectStateMap::value_type(sporef, ObjectState()) ).first;
    obj_it->second.node = id;
    // Figure out if we need to do anything on the
    ServerQueryMap::iterator serv_it = mServerQueries.find(snid);
    assert(serv_it != mServerQueries.end());
    incrementServerQuery(serv_it);
}

void ManualObjectQueryProcessor::incrementServerQuery(ServerQueryMap::iterator serv_it) {
    bool is_new = (serv_it->second->nconnected == 0);
    serv_it->second->nconnected++;
    updateServerQuery(serv_it, is_new);
}

void ManualObjectQueryProcessor::updateServerQuery(ServerQueryMap::iterator serv_it, bool is_new) {
    if (is_new) {
        QPLOG(detailed, "Initializing server query to " << serv_it->first);
        sendInitRequest(serv_it);
    }
}

void ManualObjectQueryProcessor::decrementServerQuery(ServerQueryMap::iterator serv_it) {
    serv_it->second->nconnected--;
    if (!serv_it->second->canRemove()) return;

    QPLOG(detailed, "Destroying server query to " << serv_it->first);
    sendDestroyRequest(serv_it);
}

String ManualObjectQueryProcessor::connectRequest(HostedObjectPtr ho, const SpaceObjectReference& sporef, const String& query) {
    ObjectStateMap::iterator obj_it = mObjectState.find(sporef);
    // Very likely brand new here
    if (obj_it == mObjectState.end())
        obj_it = mObjectState.insert( ObjectStateMap::value_type(sporef, ObjectState()) ).first;
    obj_it->second.who = ho;
    obj_it->second.query = query;

    // Return an empty query -- we don't want any query passed on to the
    // serve. We aggregate and register the OH query separately.
    return "";
}

void ManualObjectQueryProcessor::updateQuery(HostedObjectPtr ho, const SpaceObjectReference& sporef, const String& new_query) {
    ObjectStateMap::iterator it = mObjectState.find(sporef);

    if (new_query.empty()) { // Cancellation
        // Remove from server query
        // FIXME
        // Clear object state if possible
        if (it->second.canRemove())
            mObjectState.erase(it);
    }
    else { // Init or update
        // Track state
        it->second.who = ho;
        it->second.query = new_query;
        // Update server query
        // FIXME
    }
}

// Proximity

void ManualObjectQueryProcessor::sendInitRequest(ServerQueryMap::iterator serv_it) {
    Protocol::Prox::QueryRequest request;
    request.set_query_parameters(initRequest());
    if (request.query_parameters().empty()) return;
    std::string init_msg_str = serializePBJMessage(request);

    String framed = Network::Frame::write(init_msg_str);
    sendProxMessage(serv_it, framed);
}

void ManualObjectQueryProcessor::sendDestroyRequest(ServerQueryMap::iterator serv_it) {
    if (!serv_it->second->prox_stream) return;

    Protocol::Prox::QueryRequest request;
    request.set_query_parameters(destroyRequest());
    if (request.query_parameters().empty()) return;
    std::string destroy_msg_str = serializePBJMessage(request);
    String framed = Network::Frame::write(destroy_msg_str);
    sendProxMessage(serv_it, framed);
}

void ManualObjectQueryProcessor::sendRefineRequest(ServerQueryMap::iterator serv_it, const ObjectReference& agg) {
    std::vector<ObjectReference> aggs;
    aggs.push_back(agg);
    sendRefineRequest(serv_it, aggs);
}

void ManualObjectQueryProcessor::sendRefineRequest(ServerQueryMap::iterator serv_it, const std::vector<ObjectReference>& aggs) {
    Protocol::Prox::QueryRequest request;
    request.set_query_parameters(refineRequest(aggs));
    if (request.query_parameters().empty()) return;
    std::string msg_str = serializePBJMessage(request);
    String framed = Network::Frame::write(msg_str);
    sendProxMessage(serv_it, framed);
}

void ManualObjectQueryProcessor::sendCoarsenRequest(ServerQueryMap::iterator serv_it, const ObjectReference& agg) {
    std::vector<ObjectReference> aggs;
    aggs.push_back(agg);
    sendCoarsenRequest(serv_it, aggs);
}

void ManualObjectQueryProcessor::sendCoarsenRequest(ServerQueryMap::iterator serv_it, const std::vector<ObjectReference>& aggs) {
    Protocol::Prox::QueryRequest request;
    request.set_query_parameters(coarsenRequest(aggs));
    if (request.query_parameters().empty()) return;
    std::string msg_str = serializePBJMessage(request);
    String framed = Network::Frame::write(msg_str);
    sendProxMessage(serv_it, framed);
}

void ManualObjectQueryProcessor::sendProxMessage(ServerQueryMap::iterator serv_it, const String& msg) {
    serv_it->second->outstanding.push(msg);

    // Possibly trigger writing

    // Already in progress
    if (serv_it->second->writing) return;

    // Without a stream, we need to first create the stream
    if (!serv_it->second->prox_stream) {
        serv_it->second->writing = true;

        serv_it->second->base_stream->createChildStream(
            std::tr1::bind(&ManualObjectQueryProcessor::handleCreatedProxSubstream, this, serv_it->first, _1, _2),
            NULL, 0,
            OBJECT_PORT_PROXIMITY, OBJECT_PORT_PROXIMITY
        );
        return;
    }

    // Otherwise, we're clear for starting writing
    writeSomeProxData(serv_it->second);
}

void ManualObjectQueryProcessor::writeSomeProxData(ServerQueryStatePtr data) {
    assert(data->prox_stream);
    data->writing = true;

    while(!data->outstanding.empty()) {
        String& msg = data->outstanding.front();
        int bytes_written = data->prox_stream->write((const uint8*)msg.data(), msg.size());
        if (bytes_written < 0) {
            break;
        }
        else if (bytes_written < (int)msg.size()) {
            msg = msg.substr(bytes_written);
            break;
        }
        else {
            data->outstanding.pop();
        }
    }

    if (data->outstanding.empty()) {
        data->writing = false;
    }
    else {
        if (mContext->stopped()) return;

        static Duration retry_rate = Duration::milliseconds((int64)1);
        mContext->mainStrand->post(
            retry_rate,
            std::tr1::bind(&ManualObjectQueryProcessor::writeSomeProxData, this, data)
        );
    }
}

void ManualObjectQueryProcessor::handleCreatedProxSubstream(const OHDP::SpaceNodeID& snid, int success, OHDPSST::Stream::Ptr prox_stream) {
    if (success != SST_IMPL_SUCCESS)
        QPLOG(error, "Failed to create proximity substream to " << snid);

    // Save the stream for additional writing
    ServerQueryMap::iterator serv_it = mServerQueries.find(snid);
    // We may have lost the session since we requested the connection
    if (serv_it == mServerQueries.end()) return;
    serv_it->second->prox_stream = prox_stream;

    // Register to get data
    String* prevdata = new String();
    prox_stream->registerReadCallback(
        std::tr1::bind(&ManualObjectQueryProcessor::handleProximitySubstreamRead, this,
            snid, prox_stream, prevdata, _1, _2
        )
    );

    // We created the stream to start writing, so start that now
    assert(serv_it->second->writing == true);
    writeSomeProxData(serv_it->second);
}

void ManualObjectQueryProcessor::handleProximitySubstreamRead(const OHDP::SpaceNodeID& snid, OHDPSST::Stream::Ptr prox_stream, String* prevdata, uint8* buffer, int length) {
    if (mContext->stopped()) {
        QPLOG(detailed, "Ignoring proximity update after system stop requested.");
        return;
    }

    prevdata->append((const char*)buffer, length);

    while(true) {
        std::string msg = Network::Frame::parse(*prevdata);

        // If we don't have a full message, just wait for more
        if (msg.empty()) return;

        // Otherwise, try to handle it
        handleProximityMessage(snid, msg);
    }

    // FIXME we should be getting a callback on stream close so we can clean up!
    //prox_stream->registerReadCallback(0);
}

void ManualObjectQueryProcessor::handleProximityMessage(const OHDP::SpaceNodeID& snid, const String& payload) {
    ServerQueryMap::iterator serv_it = mServerQueries.find(snid);
    if (serv_it == mServerQueries.end()) {
        QPLOG(debug, "Received proximity message without query. Query may have recently been destroyed.");
        return;
    }
    ServerQueryStatePtr& query_state = serv_it->second;

    Sirikata::Protocol::Prox::ProximityResults contents;
    bool parse_success = contents.ParseFromString(payload);
    if (!parse_success) {
        QPLOG(error, "Failed to decode proximity message");
        return;
    }

    // Proximity messages are handled just by updating our state
    // tracking the objects, adding or removing the objects or
    // updating their properties.
    QPLOG(detailed, "Received proximity message with " << contents.update_size() << " updates");
    for(int32 idx = 0; idx < contents.update_size(); idx++) {
        Sirikata::Protocol::Prox::ProximityUpdate update = contents.update(idx);

        // TODO(ewencp) this is temporary code to actually get
        // results to objects, without actually doing real querying. We
        // shouldn't even do this in here, we should trigger some code in a
        // query handler which will then trigger the results for the object.
        // Note that we generate a new update so that we are sure to use the
        // most up-to-date loc information.
        Sirikata::Protocol::Prox::ProximityUpdate obj_update;

        for(int32 aidx = 0; aidx < update.addition_size(); aidx++) {
            Sirikata::Protocol::Prox::ObjectAddition addition = update.addition(aidx);
            ProxProtocolLocUpdate add(addition);

            ObjectReference observed_oref(addition.object());
            SpaceObjectReference observed(snid.space(), observed_oref);

            bool is_agg = (addition.has_type() && addition.type() == Sirikata::Protocol::Prox::ObjectAddition::Aggregate);

            // Store the data
            query_state->objects->objectAdded(
                observed_oref, is_agg,
                add.locationWithLocalTime(mContext->objectHost, snid.space()), add.location_seqno(),
                add.orientationWithLocalTime(mContext->objectHost, snid.space()), add.orientation_seqno(),
                add.bounds(), add.bounds_seqno(),
                Transfer::URI(add.meshOrDefault()), add.mesh_seqno(),
                add.physicsOrDefault(), add.physics_seqno()
            );

            // Replay orphans
            query_state->orphans.invokeOrphanUpdates(snid, observed, this);

            // TODO(ewencp) temporary code to get results out. See note above.
            addAdditionResult(obj_update, observed, query_state->objects->properties(observed_oref));

            // TODO(ewencp) this is a temporary way to trigger refinement -- we
            // just always refine whenver we see. Obviously this is inefficient
            // and won't scale, but it's useful to get some tests running.
            if (addition.has_type() && addition.type() == Sirikata::Protocol::Prox::ObjectAddition::Aggregate)
                sendRefineRequest(serv_it, observed_oref);
        }

        for(int32 ridx = 0; ridx < update.removal_size(); ridx++) {
            Sirikata::Protocol::Prox::ObjectRemoval removal = update.removal(ridx);
            ObjectReference observed_oref(removal.object());
            SpaceObjectReference observed(snid.space(), observed_oref);
            // Backup data for orphans and then destroy
            query_state->orphans.addUpdateFromExisting(observed, query_state->objects->properties(observed_oref));
            query_state->objects->objectRemoved(
                observed_oref
            );

            // TODO(ewencp) temporary code to get results out. See note above.
            addRemovalResult(
                obj_update, observed,
                removal.seqno(),
                (removal.has_type() && (removal.type() == Sirikata::Protocol::Prox::ObjectRemoval::Permanent))
            );
        }

        // TODO(ewencp) temporary to get results to objects. See note above.
        // FIXME this also isn't even right. we need to track which objects
        // should get results from this query.
        for(ObjectStateMap::const_iterator obj_it = mObjectState.begin(); obj_it != mObjectState.end(); obj_it++) {
            HostedObjectPtr ho = obj_it->second.who.lock();
            if (!ho) continue;
            const SpaceObjectReference& sporef = obj_it->first;
            deliverProximityUpdate(ho, sporef, obj_update);
        }
    }

}

// Location

void ManualObjectQueryProcessor::handleLocationSubstream(const OHDP::SpaceNodeID& snid, int err, OHDPSST::Stream::Ptr s) {
    s->registerReadCallback(
        std::tr1::bind(
            &ManualObjectQueryProcessor::handleLocationSubstreamRead, this,
            snid, s, new std::stringstream(), _1, _2
        )
    );
}

void ManualObjectQueryProcessor::handleLocationSubstreamRead(const OHDP::SpaceNodeID& snid, OHDPSST::Stream::Ptr s, std::stringstream* prevdata, uint8* buffer, int length) {
    prevdata->write((const char*)buffer, length);
    if (handleLocationMessage(snid, prevdata->str())) {
        // FIXME we should be getting a callback on stream close instead of
        // relying on this parsing as an indicator
        delete prevdata;
        // Clear out callback so we aren't responsible for any remaining
        // references to s, and close the stream
        s->registerReadCallback(0);
        s->close(false);
    }
}

namespace {
// Helper for applying an update to
void applyLocUpdate(const ObjectReference& objid, OHLocationServiceCachePtr loccache, const LocUpdate& lu, ObjectHost* oh, const SpaceID& space) {
    if (lu.has_location())
        loccache->locationUpdated(objid, lu.locationWithLocalTime(oh, space), lu.location_seqno());
    if (lu.has_orientation())
        loccache->orientationUpdated(objid, lu.orientationWithLocalTime(oh, space), lu.orientation_seqno());
    if (lu.has_bounds())
        loccache->boundsUpdated(objid, lu.bounds(), lu.bounds_seqno());
    if (lu.has_mesh())
        loccache->meshUpdated(objid, Transfer::URI(lu.meshOrDefault()), lu.mesh_seqno());
    if (lu.has_physics())
        loccache->physicsUpdated(objid, lu.physicsOrDefault(), lu.physics_seqno());
}
}

bool ManualObjectQueryProcessor::handleLocationMessage(const OHDP::SpaceNodeID& snid, const std::string& payload) {
    Sirikata::Protocol::Frame frame;
    bool parse_success = frame.ParseFromString(payload);
    if (!parse_success) return false;
    Sirikata::Protocol::Loc::BulkLocationUpdate contents;
    contents.ParseFromString(frame.payload());

    ServerQueryMap::iterator serv_it = mServerQueries.find(snid);
    if (serv_it == mServerQueries.end()) {
        QPLOG(debug, "Received location message without query. Query may have recently been destroyed.");
        return true; // Was successfully parsed, but not handled
    }
    ServerQueryStatePtr& query_state = serv_it->second;

    for(int32 idx = 0; idx < contents.update_size(); idx++) {
        Sirikata::Protocol::Loc::LocationUpdate update = contents.update(idx);
        ObjectReference observed_oref(update.object());
        SpaceObjectReference observed(snid.space(), observed_oref);

        // Because of prox/loc ordering, we may or may not have a record of the
        // object yet.
        if (!query_state->objects->tracking(observed_oref)) {
            query_state->orphans.addOrphanUpdate(observed, update);
        }
        else {
            LocProtocolLocUpdate llu(update);
            applyLocUpdate(observed_oref, query_state->objects, llu, mContext->objectHost, snid.space());
        }
    }

    return true;
}

void ManualObjectQueryProcessor::onOrphanLocUpdate(const OHDP::SpaceNodeID& observer, const LocUpdate& lu) {
    ServerQueryMap::iterator serv_it = mServerQueries.find(observer);
    if (serv_it == mServerQueries.end()) {
        QPLOG(debug, "Received proximity message without query. Query may have recently been destroyed.");
        return;
    }
    ServerQueryStatePtr& query_state = serv_it->second;

    SpaceObjectReference observed(observer.space(), lu.object());
    assert(query_state->objects->tracking(lu.object()));
    applyLocUpdate(lu.object(), query_state->objects, lu, mContext->objectHost, observer.space());
}

} // namespace Manual
} // namespace OH
} // namespace Sirikata

max_stars_repo_name: pathorn/sirikata

max_issues_repo_name: pathorn/sirikata

max_forks_repo_name: pathorn/sirikata


text:
/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     max_likelihood_csp.cpp
 * \author   Collin Johnson
 *
 * Definition of MaxLikelihoodCSP.
 */

#include <hssh/local_topological/area_detection/labeling/max_likelihood_csp.h>
#include <hssh/local_topological/area_detection/labeling/mcmc_sampling.h>
#include <hssh/local_topological/area_detection/labeling/boundary.h>
#include <hssh/local_topological/area_detection/labeling/hypothesis_graph.h>
#include <hssh/local_topological/area_detection/labeling/small_scale_star_builder.h>
#include <system/debug_communicator.h>
#include <utils/algorithm_ext.h>
#include <utils/serialized_file_io.h>
#include <boost/range/iterator_range.hpp>
#include <cassert>

namespace vulcan
{
namespace hssh
{

using HypToIdMap = std::unordered_map<AreaHypothesis*, AlignmentGraph::Id>;

void create_fixed_type_constraint(AreaHypothesis* area,
                                  HypToIdMap& hypIds,
                                  HypothesisType fixedtype,
                                  AlignmentGraph& network);
void create_constraints_for_area(AreaHypothesis* area,
                                 HypToIdMap& hypIds,
                                 const SmallScaleStarBuilder& starBuilder,
                                 AlignmentGraph& network);
void create_star_based_constraints(AreaHypothesis* area,
                                   HypToIdMap& hypIds,
                                   const SmallScaleStarBuilder& starBuilder,
                                   AlignmentGraph& network);
void create_adjacency_constraints(AreaHypothesis* area, HypToIdMap& hypIds, AlignmentGraph& network);
void create_all_neighbors_constraint(AreaHypothesis* area, HypToIdMap& hypIds, AlignmentGraph& network);
void create_exit_area_constraint(AreaHypothesis* area,
                                 const std::vector<AreaHypothesis*>& mutableAreas,
                                 HypToIdMap& hypIds,
                                 AlignmentGraph& network);

MaxLikelihoodCSP::MaxLikelihoodCSP(const std::shared_ptr<SmallScaleStarBuilder>& starBuilder,
                                   const MCMCSamplingParams& mcmcParams)
: starBuilder_(starBuilder)
, mcmcParams_(mcmcParams)
{
}

CSPSolution MaxLikelihoodCSP::solve(const std::vector<AreaHypothesis*>& fixedAreas,
                                    const std::vector<AreaHypothesis*>& mutableAreas,
                                    AreaHypothesis* exitedArea,
                                    AreaHypothesis* enteredArea,
                                    const BoundaryClassifier& boundaryClassifier)
{
    CSPSolution solution = runMCMC(fixedAreas, mutableAreas, exitedArea, enteredArea, boundaryClassifier, true);

    std::cout << "Max-likelihood solution: " << solution << '\n';

    // If a fixed area is failing, then just make them mutable and try again
    if(solution.errorCode() == CSPSolution::fixed_area_failing_constraints)
    {
        // First fallback to just not do the initial merge, which can overly constrain the problem
        solution = runMCMC(fixedAreas, mutableAreas, exitedArea, enteredArea, boundaryClassifier, false);

        std::cout << "Fixed-area initial recovery solution: " << solution << '\n';
    }

    // If a fixed area is failing, then just make them mutable and try again
    if(solution.errorCode() == CSPSolution::fixed_area_failing_constraints)
    {
        std::vector<AreaHypothesis*> allAreas;
        allAreas.insert(allAreas.end(), fixedAreas.begin(), fixedAreas.end());
        allAreas.insert(allAreas.end(), mutableAreas.begin(), mutableAreas.end());

        std::vector<AreaHypothesis*> noAreas;

        solution = runMCMC(noAreas, allAreas, exitedArea, enteredArea, boundaryClassifier, true);

        std::cout << "Fixed-area recovery solution: " << solution << '\n';
    }

    return solution;
}

void MaxLikelihoodCSP::sendDebug(system::DebugCommunicator& communicator)
{
//     communicator.sendDebug(debugInfo_);
    std::string path = getenv("VULCAN_BIN");
    path += "/csp_debug_info.log";
    utils::save_serializable_to_file(path, debugInfo_);
}

CSPSolution MaxLikelihoodCSP::runMCMC(const std::vector<AreaHypothesis*>& fixedAreas,
                                      const std::vector<AreaHypothesis*>& mutableAreas,
                                      AreaHypothesis* exitedArea,
                                      AreaHypothesis* enteredArea,
                                      const BoundaryClassifier& boundaryClassifier,
                                      bool doInitialMerge)
{
    // Create the network
    MCMCSampling network(mcmcParams_, &boundaryClassifier);

    // Add all areas to the network
    std::unordered_map<AreaHypothesis*, AlignmentGraph::Id> hypIds;
    for(auto hyp : fixedAreas)
    {
        if(hyp != exitedArea)
        {
            assert(hyp);
            assert(hyp->getType() != HypothesisType::kArea);
            hypIds[hyp] = network.addFixedArea(hyp, hyp->getType(), false);  // don't need to be connected to graph

            std::cout << "Adding fixed:" << hyp->rectangleBoundary() << ' ' << hyp->getType() << '\n';
        }
    }

    if(exitedArea)
    {
        hypIds[exitedArea] = network.addFixedArea(exitedArea, exitedArea->getType(), true); // must be connected

        std::cout << "Adding exited:" << exitedArea->rectangleBoundary() << ' ' << exitedArea->getType() << '\n';
    }

    if(enteredArea)
    {
        hypIds[enteredArea] = network.addArea(enteredArea, enteredArea->getType());
        std::cout << "Adding entered:" << enteredArea->rectangleBoundary() << '\n';
    }

    for(auto hyp : mutableAreas)
    {
        if(hyp != enteredArea)
        {
            assert(hyp);
            auto areaType = hyp->getType();
            hypIds[hyp] = network.addArea(hyp, HypothesisType::kArea);
            assert(areaType == hyp->getType());

            std::cout << "Adding mutable:" << hyp->rectangleBoundary() << '\n';
        }
    }

    // Once areas are added to the network, create the constraints for each of the three types of areas
    // Fixed areas only have a fixed type constraint. This constraint is done to ensure that the type of prior
    // areas doesn't affect the current graph. These fixed areas might have been chopped in half or otherwise changed
    // as a result of the shrinking of the map on exiting an area
    for(auto hyp : fixedAreas)
    {
        create_fixed_type_constraint(hyp, hypIds, hyp->getType(), network);

        std::cout << "Adding fixed constraint:" << hyp->rectangleBoundary() << ' ' << hyp->getType() << '\n';
    }

    // The exited area can't have its type changed, but it also needs to have the full set of constraints because it
    // separates the fixed areas from the mutable areas and thus is the graph separator between them.
    if(exitedArea)
    {
        create_fixed_type_constraint(exitedArea, hypIds, exitedArea->getType(), network);
        create_constraints_for_area(exitedArea, hypIds, *starBuilder_, network);
        create_exit_area_constraint(exitedArea, mutableAreas, hypIds, network);

        std::cout << "Adding exit constraint: " << exitedArea->rectangleBoundary() << ' ' << exitedArea->getType()
            << '\n';
    }

    if(enteredArea)
    {
        create_constraints_for_area(enteredArea, hypIds, *starBuilder_, network);
    }

    // Mutable areas need to have the full set of constraints
    for(auto hyp : mutableAreas)
    {
        create_constraints_for_area(hyp, hypIds, *starBuilder_, network);
    }

    debugInfo_.iterations.clear();
    debugInfo_.extents.clear();
    return network.solve(doInitialMerge, &debugInfo_);
}

void create_fixed_type_constraint(AreaHypothesis* area,
                                  HypToIdMap& hypIds,
                                  HypothesisType fixedType,
                                  AlignmentGraph& network)
{
    AlignmentConstraint::AdjVec adjacent(area->numBoundaries());
    std::transform(area->beginBoundary(),
                   area->endBoundary(),
                   adjacent.begin(),
                   [&area, &hypIds](const AreaHypothesisBoundary* boundary) {
        return ConstraintAdjacentArea{hypIds[area->adjacentArea(boundary->getGateway())],
            boundary->getGateway().id()};
    });

    network.addConstraint(AlignmentConstraint::CreateFixedConstraint(adjacent, hypIds[area], fixedType));
}

void create_constraints_for_area(AreaHypothesis* area,
                                 HypToIdMap& hypIds,
                                 const SmallScaleStarBuilder& starBuilder,
                                 AlignmentGraph& network)
{
    create_star_based_constraints(area, hypIds, starBuilder, network);
    create_adjacency_constraints(area, hypIds, network);
    create_all_neighbors_constraint(area, hypIds, network);
}

void create_star_based_constraints(AreaHypothesis* area,
                                   HypToIdMap& hypIds,
                                   const SmallScaleStarBuilder& starBuilder,
                                   AlignmentGraph& network)
{
    // For each area, find all unaligned gateway pairs
    // The non-through-paths are unaligned constraints.
    AlignmentGraph::Id insideId = hypIds[area];
    AlignmentConstraint::AdjVec adjacent;

    auto unalignedGateways = area->pairwiseUnalignedGateways();

    for(auto& u : unalignedGateways)
    {
        adjacent.clear();

        AlignmentGraph::Id firstId = hypIds[area->adjacentArea(*u.first)];
        AlignmentGraph::Id secondId = hypIds[area->adjacentArea(*u.second)];

        adjacent.push_back({firstId, u.first->id()});
        adjacent.push_back({secondId, u.second->id()});
        network.addConstraint(AlignmentConstraint::CreateUnalignedConstraint(adjacent, insideId));
    }

    // For each area, also find the aligned gateway pairs
    auto alignedGateways = area->pairwiseAlignedGateways();

    for(auto& u : alignedGateways)
    {
        adjacent.clear();

        AlignmentGraph::Id firstId = hypIds[area->adjacentArea(*u.first)];
        AlignmentGraph::Id secondId = hypIds[area->adjacentArea(*u.second)];

        adjacent.push_back({firstId, u.first->id()});
        adjacent.push_back({secondId, u.second->id()});
        network.addConstraint(AlignmentConstraint::CreateAlignedConstraint(adjacent, insideId));
    }
}

void create_adjacency_constraints(AreaHypothesis* area, HypToIdMap& hypIds, AlignmentGraph& network)
{
    AlignmentGraph::Id insideId = hypIds[area];
    AlignmentConstraint::AdjVec endpoints;

    for(auto& boundary : boost::make_iterator_range(area->beginBoundary(), area->endBoundary()))
    {
        ConstraintAdjacentArea adj;
        adj.id = hypIds[area->adjacentArea(boundary->getGateway())];
        adj.gatewayId = boundary->getGateway().id();

        if(area->isEndGateway(adj.gatewayId))
        {
            endpoints.push_back(adj);
        }
        else
        {
            network.addConstraint(AlignmentConstraint::CreateAdjacentConstraint(adj, insideId));
        }
    }

    if(!endpoints.empty())
    {
        network.addConstraint(AlignmentConstraint::CreateEndpointsConstraint(endpoints, insideId));
    }
    else
    {
        std::cerr << "WARNING: MaxLikelihoodCSP: No endpoints for area: " << area->extent().rectangleBoundary()
            << '\n';
    }
}

void create_all_neighbors_constraint(AreaHypothesis* area, HypToIdMap& hypIds, AlignmentGraph& network)
{
    AlignmentGraph::Id insideId = hypIds[area];
    AlignmentConstraint::AdjVec adjacent(area->numBoundaries());

    std::transform(area->beginBoundary(),
                   area->endBoundary(),
                   adjacent.begin(),
                   [&area, &hypIds](const AreaHypothesisBoundary* boundary) {
        return ConstraintAdjacentArea{hypIds[area->adjacentArea(boundary->getGateway())],
                                      boundary->getGateway().id()};
    });

    network.addConstraint(AlignmentConstraint::CreateAllNeighborsConstraint(adjacent, insideId));
}

void create_exit_area_constraint(AreaHypothesis* area,
                                 const std::vector<AreaHypothesis*>& mutableAreas,
                                 HypToIdMap& hypIds,
                                 AlignmentGraph& network)
{
    // Go through the boundary for the constraint. All adjacent areas that are mutable will get added to the adjacent
    // areas to be considered
    AlignmentGraph::Id insideId = hypIds[area];
    AlignmentConstraint::AdjVec adjacent;

    for(auto& bnd : boost::make_iterator_range(area->beginBoundary(), area->endBoundary()))
    {
        if(utils::contains(mutableAreas, bnd->getOtherHypothesis(*area)))
        {
            ConstraintAdjacentArea constraint{hypIds[bnd->getOtherHypothesis(*area)], bnd->getGateway().id()};
            adjacent.emplace_back(constraint);
        }
    }

    network.addConstraint(AlignmentConstraint::CreateExitConstraint(adjacent, insideId, area->getType()));
}

} // namespace hssh
} // namespace vulcan