double beliefPenaltyCollocation(std::vector< Matrix<B_DIM> >& B, std::vector< Matrix<U_DIM> >& U, beliefPenaltyMPC_params &problem, beliefPenaltyMPC_output &output, beliefPenaltyMPC_info &info)
{
double penalty_coeff = cfg::initial_penalty_coeff;
//double trust_box_size = cfg::initial_trust_box_size;
int penalty_increases = 0;
Matrix<B_DIM> dynviol;
// penalty loop
while(penalty_increases < cfg::max_penalty_coeff_increases)
{
bool success = minimizeMeritFunction(B, U, problem, output, info, penalty_coeff);
double cntviol = 0;
for(int t = 0; t < T-1; ++t) {
dynviol = (B[t+1] - beliefDynamics(B[t], U[t]) );
for(int i = 0; i < B_DIM; ++i) {
cntviol += fabs(dynviol[i]);
}
}
success = success && (cntviol < cfg::cnt_tolerance);
LOG_DEBUG("Constraint violations: %2.10f",cntviol);
if (!success) {
penalty_increases++;
penalty_coeff = penalty_coeff*cfg::penalty_coeff_increase_ratio;
//trust_box_size = cfg::initial_trust_box_size;
}
else {
return computeCost(B, U);
}
}
return computeCost(B, U);
}
void NeuralNetwork::trainOn(arma::mat& input, const arma::mat& output, int numIterations, int iterationsBetweenReport)
{
if (input.n_cols != static_cast<unsigned int>(m_numNeuronsOnLayer[0]) ||
output.n_cols != static_cast<unsigned int>(m_numNeuronsOnLayer[m_numLayers - 1]))
throw InvalidInputException("File's input / output length doesn't match with the"
"number of neurons on input / output layer.");
if (m_featureNormalization)
normalizeFeatures(input);
double prevCost = computeCost(input, output, m_theta);
double crtCost = prevCost;
for (int iteration = 0; iteration < numIterations; ++iteration)
{
if (iterationsBetweenReport)
if (iteration % iterationsBetweenReport == 0 || iteration + 1 == numIterations)
std::cout << "Iteration: " << iteration << " | Cost: " << crtCost << std::endl;
if (crtCost > prevCost)
{
std::cout << "The cost is increasing. Choose a smaller learning rate." << std::endl;
return;
}
backprop(input, output);
prevCost = crtCost;
crtCost = computeCost(input, output, m_theta);
}
}
void ParallellizeBySquares2D::parallelize(){
// we must have the same number of blocks as processors
PLB_PRECONDITION(xTiles*yTiles == processorNumber);
plint totalCost = computeCost(originalBlocks, finestBoundingBox);
plint idealCostPerProcessor = totalCost/processorNumber;
pcout << "Total cost of computations = " << totalCost << std::endl;
pcout << "We are using " << processorNumber << " processors...\n";
pcout << "Ideal cost per processor = " << idealCostPerProcessor << std::endl;
std::vector<plint> totalCosts(processorNumber);
plint total = 0;
for (plint iProc=0; iProc<processorNumber; ++iProc){
plint blockCost = computeCost(originalBlocks,finestDivision[iProc]);
totalCosts[iProc] += blockCost;
mpiDistribution[iProc] = iProc;
total += blockCost;
}
pcout << "---- Costs Per Processor ----\n";
for (pluint i=0; i<totalCosts.size(); ++i){
pcout << i << " : " << totalCosts[i] << std::endl;
// check if everyone is doing something
if (totalCosts[i] == 0){
pcout << "\t >> processor " << i << " does not have work to do. Exiting.....\n";
std::exit(1);
}
}
pcout << "*******************************\n";
pcout << "Sum of all costs = " << total << std::endl;
pcout << "*******************************\n";
// convert the original blocks to the new blocks
recomputedBlocks.resize(originalBlocks.size());
finalMpiDistribution.resize(originalBlocks.size());
plint finestLevel= (plint)originalBlocks.size()-1;
for (plint iLevel=finestLevel; iLevel>=0; --iLevel) {
parallelizeLevel(iLevel, originalBlocks,finestDivision, mpiDistribution);
// Adapt the regions to the next-coarser level.
for (pluint iRegion=0; iRegion<finestDivision.size(); ++iRegion) {
finestDivision[iRegion] = finestDivision[iRegion].divideAndFitSmaller(2);
}
}
}
void InflationLayer::computeCaches()
{
if(cell_inflation_radius_ == 0)
return;
//based on the inflation radius... compute distance and cost caches
if(cell_inflation_radius_ != cached_cell_inflation_radius_)
{
if(cached_cell_inflation_radius_ > 0)
deleteKernels();
cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
cached_distances_ = new double*[cell_inflation_radius_ + 2];
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
cached_distances_[i] = new double[cell_inflation_radius_ + 2];
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{
cached_distances_[i][j] = sqrt(i * i + j * j);
}
}
cached_cell_inflation_radius_ = cell_inflation_radius_;
}
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
}
/**Fills `edges` with all of the edges in the provided mesh, computes their
cost (with computeCost), then sorts them. -CJ*/
void QuadricErrorSimplification::populateEdges(STTriangleMesh* m){
int i,j;
STVertex *v1,*v2;
for (i=0; i < m->mFaces.size(); i++){
for (j=0; j<3; j++){
v1=m->mFaces[i]->v[j];
v2=m->mFaces[i]->v[(j+1)%3];
Edge newEdge = Edge(v1,v2);
if (findEdge(this,&newEdge) == -1){
edges.push_back(new Edge(v1,v2));
}
}
}
int i1,i2;
STVertex w = STVertex(0,0,0,0,0);
STMatrix4* Q1;
STMatrix4* Q2;
for (i=0;i<edges.size();i++){
i1 = findVertex(m, edges[i]->vertex1);
i2 = findVertex(m, edges[i]->vertex2);
if (i1==-1){
printf("Error: QuadricErrorSimplification::populateEdges could not find vertex at %f,%f,%f\n",
edges[i]->vertex1->pt.x,edges[i]->vertex1->pt.y,edges[i]->vertex1->pt.z);
}
if (i2==-1){
printf("Error: QuadricErrorSimplification::populateEdges could not find vertex at %f,%f,%f\n",
edges[i]->vertex2->pt.x,edges[i]->vertex2->pt.y,edges[i]->vertex2->pt.z);
}
Q1 = qMatrixes[i1];
Q2 = qMatrixes[i2];
edges[i]->cost = computeCost(m, v1, v2, &w, Q1, Q2);
}
}
void computeCost(node *r)
{
r->reduce_cost = 0;
if (r->children_count==0)
{
return;
}
double mean_r = static_cast<double>(r->reds) / r->count_cum;
double mean_g = static_cast<double>(r->greens) / r->count_cum;
double mean_b = static_cast<double>(r->blues) / r->count_cum;
for (unsigned idx=0; idx < 8; ++idx)
{
if (r->children_[idx] != 0)
{
double dr,dg,db;
computeCost(r->children_[idx]);
dr = r->children_[idx]->reds / r->children_[idx]->count_cum - mean_r;
dg = r->children_[idx]->greens / r->children_[idx]->count_cum - mean_g;
db = r->children_[idx]->blues / r->children_[idx]->count_cum - mean_b;
r->reduce_cost += r->children_[idx]->reduce_cost;
r->reduce_cost += (dr*dr + dg*dg + db*db) * r->children_[idx]->count_cum;
}
}
}
void CRandom::perform()
{
savePreviousResult();
permute();
computeCost();
m_costStatsCalculator.update(m_cost);
restorePreviousResultIfItWasBetter();
}
/*
* Update context for start of inlining.
*/
void InliningDecider::accountForInlining(const Func* callee,
const RegionDesc& region) {
int cost = computeCost(region);
m_costStack.push_back(cost);
m_cost += cost;
m_callDepth += 1;
m_stackDepth += callee->maxStackCells();
}
void KdTree::orderLineSegmentsByCost (LineSegments &lineSegments, LineSegments::iterator begin, LineSegments::iterator end, int orderType, int depth, map<int, LineSegments> &orderedLineSegments)
{
double min_c = std::numeric_limits<double>::max();
int mid;
int i = 0;
for (LineSegments::iterator it = begin; it != end; ++it, ++i) {
double c = computeCost(lineSegments, begin, end, (*it)->p0, orderType);
if (c < min_c) {
min_c = c;
mid = i;
}
/*
c = computeCost(lineSegments, begin, end, (*it)->p1, orderType);
if (c < min_c) {
mid = i;
}
*/
}
// separate the line segments such that the first half is for the left sub-tree and the second half is for the right sub-tree
LineSegment *mid_l = *(begin + mid);
{
swap(*(end - 1), *(begin + mid));
LineSegments::iterator it = begin;
LineSegments::iterator r_index = end - 2;
for (; it != end - 1 && it <= r_index;) {
if (orderType == 0) {
if (XOrder((*it)->p0, mid_l->p0) == 1) {
it++;
} else {
swap(*it, *r_index);
r_index--;
}
} else {
if (YOrder((*it)->p0, mid_l->p0) == 1) {
it++;
} else {
swap(*it, *r_index);
r_index--;
}
}
}
swap(*it, *(end - 1));
mid = it - begin;
}
orderedLineSegments[depth].push_back(mid_l);
if (mid > 0) {
orderLineSegmentsByCost(lineSegments, begin, begin + mid, 1 - orderType, depth + 1, orderedLineSegments);
}
if (begin + mid + 1 != end) {
orderLineSegmentsByCost(lineSegments, begin + mid + 1, end, 1 - orderType, depth + 1, orderedLineSegments);
}
}
MockStrategy::MockStrategy(const Matrix & flow, const Matrix & distance) : IStrategy(flow, distance) {
if (0 == testId) {
m_result = {0, 1};
}
else if (1 == testId) {
m_result = {8, 3, 5, 2, 10, 6, 11, 1, 7, 9, 0, 4};
}
else if (2 == testId) {
m_result = {4, 6, 0, 9, 10, 2, 3, 1, 8, 5, 11, 7};
}
computeCost();
}
// Creates the best possible patch to match the given target
Heightmap bestPatch(const Heightmap *output, const Patch &target_patch, const ivec2 &target_offset, const vector<Patch> &candidates) {
// Create a comparator for PatchCandidate
auto cmp = [](const PatchCandidate &left, const PatchCandidate &right) { return left.cost > right.cost; };
// create and populate a priority queue for candidates
priority_queue<PatchCandidate, vector<PatchCandidate>, decltype(cmp)> target_candidates(cmp);
for (const Patch &p : candidates) {
// use patch if the the degree matches
if (p.degree == target_patch.degree)
target_candidates.emplace(&p, computeCost(p, target_patch, target_offset, output, 5.0, 2.0, 0.001, 1.0));
}
// if no patches were found with matching degree, use all patches
if (target_candidates.empty()) {
for (const Patch &p : candidates) {
target_candidates.emplace(&p, computeCost(p, target_patch, target_offset, output, 5.0, 2.0, 0.001, 1.0));
}
}
// compute cost (and graphcut) for best n (typically 5) candidates
float best_cost = 0;
Heightmap best_patch = graphcut(output, &(target_candidates.top().patch->data), target_offset, &best_cost);
for (int i = 0; i < 5 && !target_candidates.empty(); ++i) {
target_candidates.pop();
float cost = 0;
Heightmap g = graphcut(output, &(target_candidates.top().patch->data), target_offset, &cost);
if (cost < best_cost) {
best_patch = g;
best_cost = cost;
}
}
return best_patch;
}
void CostMap2D::propagateCosts(){
while(!queue_.empty()){
QueueElement* c = queue_.top();
queue_.pop();
unsigned char cost = computeCost(c->distance);
updateCellCost(c->ind, cost);
// If distance reached the inflation radius then skip further expansion
if(c->distance < inflationRadius_)
enqueueNeighbors(c->source, c->ind);
delete c;
}
}
void NeuralNetwork::checkGradients(arma::mat &input, arma::mat &output, std::vector<arma::mat>& gradients)
{
double e = 0.0001;
std::vector<arma::mat> numericalGrads;
for (unsigned int layer = 0; layer < gradients.size(); ++layer)
{
std::vector<arma::mat> thetas = m_theta;
arma::mat grads = arma::ones(m_theta[layer].n_rows, m_theta[layer].n_cols);
for (unsigned int i = 0; i < grads.n_rows; ++i)
{
for (unsigned int j = 0; j < grads.n_cols; ++j)
{
arma::mat thetaPlus = m_theta[layer];
arma::mat thetaMinnus = m_theta[layer];
thetaPlus(i, j) += e;
thetaMinnus(i, j) -= e;
thetas[layer] = thetaPlus;
double loss1 = computeCost(input, output, thetas);
thetas[layer] = thetaMinnus;
double loss2 = computeCost(input, output, thetas);
grads(i, j) = (loss1 - loss2) / (2 * e);
}
}
numericalGrads.push_back(grads);
}
for (unsigned int layer = 0; layer < gradients.size(); ++layer)
{
std::cout << "----------------------------------------" << std::endl;
std::cout << "Numerical computed gradients" << std::endl;
std::cout << numericalGrads[layer] << std::endl;
std::cout << "Analytical computed gradients" << std::endl;
std::cout << gradients[layer] << std::endl;
std::cout << "----------------------------------------" << std::endl;
}
}
void finiteDifferenceCostGrad(std::vector< Matrix<X_DIM> >& X, std::vector< Matrix<U_DIM> >& U, double& cost, Matrix<XU_DIM>& G, Matrix<XU_DIM,XU_DIM>& diag_hess) {
cost = computeCost(X,U);
double cost_plus, cost_minus;
int index = 0;
for(int t = 0; t < T; ++t) {
for(int i = 0; i < X_DIM+U_DIM; ++i) {
if (i < X_DIM) {
double original = X[t][i];
X[t][i] += step;
cost_plus = computeCostTruncated(X,U,t);
X[t][i] = original;
X[t][i] -= step;
cost_minus = computeCostTruncated(X,U,t);
X[t][i] = original;
} else if (t < TIMESTEPS - 1){
double original = U[t][i-X_DIM];
U[t][i-X_DIM] += step;
cost_plus = computeCostTruncated(X,U,t);
U[t][i-X_DIM] = original;
U[t][i-X_DIM] -= step;
cost_minus = computeCostTruncated(X,U,t);
U[t][i-X_DIM] = original;
} else {
break; // no U at T-1
}
G[index] = (cost_plus - cost_minus)/(2*step);
diag_hess[index] = (cost_plus + cost_minus - 2*cost)/(step*step);
index++;
}
}
}
void Pair::init()
{
// Boundary quadrics only needed for Vertex::init()...so clean up some space:
if (m_pQuadric)
{
delete m_pQuadric;
}
// We'll use this instance of the matrix for pair cost computations:
m_pQuadric = NULL;
Matrix4x4 Quadric;
computeQuadric(&Quadric);
computeCost(&Quadric);
}
// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
int BFReconstructionEngine::calculateCost(CostData::Pointer cost,
SinogramPtr sinogram,
GeometryPtr geometry,
RealVolumeType::Pointer ErrorSino,
QGGMRF::QGGMRF_Values* QGGMRF_Values)
{
Real_t cost_value = computeCost(sinogram, geometry, ErrorSino, QGGMRF_Values);
//std::cout << "cost_value: " << cost_value << std::endl;
int increase = cost->addCostValue(cost_value);
if(increase == 1)
{
return -1;
}
cost->writeCostValue(cost_value);
return 0;
}
static void computeCtcCosts(Bundle& bundle, const std::string& costFunctionName,
double costFunctionWeight,
const Matrix& inputActivations, const LabelVector& labels, const IndexVector& inputTimesteps)
{
auto costFunction = CostFunctionFactory::create(costFunctionName);
Bundle inputBundle;
inputBundle["outputActivations"] = MatrixVector({inputActivations});
inputBundle["referenceLabels"] = labels;
inputBundle["inputTimesteps"] = inputTimesteps;
costFunction->computeCost(inputBundle);
auto costs = inputBundle["costs"].get<Matrix>();
bundle["cost"] = static_cast<double>(reduce(costs, {}, matrix::Add())[0]) * costFunctionWeight;
}
static Matrix optimizeWithoutDerivative(const NeuralNetwork* network,
const Image& image, unsigned int neuron)
{
Matrix bestSoFar = generateRandomImage(network, image, 0, 0.00f);
float bestCost = computeCost(network, neuron, bestSoFar);
std::string solverType = util::KnobDatabase::getKnobValue(
"NeuronVisualizer::SolverType", "SimulatedAnnealingSolver");
auto solver =
optimizer::GeneralNondifferentiableSolverFactory::create(solverType);
assert(solver != nullptr);
CostFunction costFunction(network, neuron, bestCost);
bestCost = solver->solve(bestSoFar, costFunction);
delete solver;
return bestSoFar;
}
void MockStrategy::perform() {
computeCost(swapedIndxs);
std::swap(m_result[swapedIndxs.first], m_result[swapedIndxs.second]);
}
/*
* Use gradient-projection to solve an instance of
* the Variable Placement with Separation Constraints problem.
*/
unsigned GradientProjection::solve(
valarray<double> const &linearCoefficients,
valarray<double> &x) {
COLA_ASSERT(linearCoefficients.size()==x.size());
COLA_ASSERT(x.size()==denseSize);
COLA_ASSERT(numStaticVars>=denseSize);
COLA_ASSERT(sparseQ==nullptr ||
(sparseQ!=nullptr && (vars.size()==sparseQ->rowSize())) );
if(max_iterations==0) return 0;
bool converged=false;
solver = setupVPSC();
#ifdef MOSEK_AVAILABLE
if(solveWithMosek==Outer) {
float* ba=new float[vars.size()];
float* xa=new float[vars.size()];
for(unsigned i=0;i<vars.size();i++) {
ba[i]=-linearCoefficients[i];
}
mosek_quad_solve_sep(menv,ba,xa);
for(unsigned i=0;i<vars.size();i++) {
//printf("mosek result x[%d]=%f\n",i,xa[i]);
x[i]=xa[i];
}
delete [] ba;
delete [] xa;
return 1;
}
#endif
// it may be that we have to consider dummy vars, which the caller didn't know
// about. Thus vars.size() may not equal x.size()
unsigned n = vars.size();
valarray<double> b(n);
result.resize(n);
// load desired positions into vars, note that we keep desired positions
// already calculated for dummy vars
for (unsigned i=0;i<x.size();i++) {
COLA_ASSERT(!isNaN(x[i]));
COLA_ASSERT(isFinite(x[i]));
b[i]=i<linearCoefficients.size()?linearCoefficients[i]:0;
result[i]=x[i];
if(scaling) {
b[i]*=vars[i]->scale;
result[i]/=vars[i]->scale;
}
if(!vars[i]->fixedDesiredPosition) vars[i]->desiredPosition=result[i];
}
runSolver(result);
valarray<double> g(n); /* gradient */
valarray<double> previous(n); /* stored positions */
valarray<double> d(n); /* actual descent vector */
#ifdef CHECK_CONVERGENCE_BY_COST
double previousCost = DBL_MAX;
#endif
unsigned counter=0;
double stepSize;
for (; counter<max_iterations&&!converged; counter++) {
previous=result;
stepSize=0;
double alpha=computeSteepestDescentVector(b,result,g);
//printf("Iteration[%d]\n",counter);
// move to new unconstrained position
for (unsigned i=0; i<n; i++) {
// dividing by variable weight is a cheap trick to make these
// weights mean something in terms of the descent vector
double step=alpha*g[i]/vars[i]->weight;
result[i]+=step;
//printf(" after unconstrained step: x[%d]=%f\n",i,result[i]);
stepSize+=step*step;
COLA_ASSERT(!isNaN(result[i]));
COLA_ASSERT(isFinite(result[i]));
if(!vars[i]->fixedDesiredPosition) vars[i]->desiredPosition=result[i];
}
//project to constraint boundary
bool constrainedOptimum = false;
constrainedOptimum=runSolver(result);
stepSize=0;
for (unsigned i=0;i<n;i++) {
double step = previous[i]-result[i];
stepSize+=step*step;
}
//constrainedOptimum=false;
// beta seems, more often than not, to be >1!
if(constrainedOptimum) {
// The following step limits the step-size in the feasible
// direction
d = result - previous;
const double beta = 0.5*computeStepSize(g, d);
// beta > 1.0 takes us back outside the feasible region
// beta < 0 clearly not useful and may happen due to numerical imp.
//printf("beta=%f\n",beta);
if(beta>0&&beta<0.99999) {
stepSize=0;
for (unsigned i=0; i<n; i++) {
double step=beta*d[i];
result[i]=previous[i]+step;
stepSize+=step*step;
}
}
}
#ifdef CHECK_CONVERGENCE_BY_COST
/* This would be the slow way to detect convergence */
//if(counter%2) {
double cost = computeCost(b,result);
printf(" gp[%d] %.15f %.15f\n",counter,previousCost,cost);
//COLA_ASSERT(previousCost>cost);
if(fabs(previousCost - cost) < tolerance) {
converged = true;
}
previousCost = cost;
//}
#else
if(stepSize<tolerance) converged = true;
#endif
}
//printf("GP[%d] converged after %d iterations.\n",k,counter);
for(unsigned i=0;i<x.size();i++) {
x[i]=result[i];
if(scaling) {
x[i]*=vars[i]->scale;
}
}
destroyVPSC(solver);
return counter;
}
int ZoltanLibClass::precompute()
{
std::string str1("Isorropia::ZoltanLibClass::precompute ");
MPI_Comm mpicomm = MPI_COMM_WORLD;
#ifdef HAVE_MPI
MPI_Comm default_mpicomm = MPI_COMM_WORLD;
#endif // HAVE_MPI
int itype;
Library::precompute(); // assumes input_type_ is set
if (input_graph_.get() || input_matrix_.get())
{
if (input_type_ != hgraph2d_finegrain_input_){
computeCost(); // graph or hypergraph weights
}
}
if (input_type_ == graph_input_)
itype = ZoltanLib::QueryObject::graph_input_;
else if (input_type_ == hgraph_input_)
itype = ZoltanLib::QueryObject::hgraph_input_;
else if (input_type_ == hgraph2d_finegrain_input_)
itype = ZoltanLib::QueryObject::hgraph2d_finegrain_input_;
else if (input_type_ == geometric_input_)
itype = ZoltanLib::QueryObject::geometric_input_;
else if (input_type_ == simple_input_)
itype = ZoltanLib::QueryObject::simple_input_;
else if (input_type_ == hgraph_graph_input_) // hierarchical partitioning
itype = ZoltanLib::QueryObject::hgraph_graph_input_;
else if (input_type_ == hgraph_geometric_input_) // hierarchical partitioning
itype = ZoltanLib::QueryObject::hgraph_geometric_input_;
else if (input_type_ == graph_geometric_input_) // hierarchical partitioning
itype = ZoltanLib::QueryObject::graph_geometric_input_;
else if (input_type_ == hgraph_graph_geometric_input_) // hierarchical partitioning
itype = ZoltanLib::QueryObject::hgraph_graph_geometric_input_;
else
itype = ZoltanLib::QueryObject::unspecified_input_;
if (input_graph_.get() !=0 && input_coords_.get()!=0) //geometric and graph inputs
{
queryObject_ = Teuchos::rcp(new ZoltanLib::QueryObject(input_graph_, costs_, input_coords_, weights_, itype));
#ifdef HAVE_MPI
const Epetra_Comm &ecomm = input_graph_->RowMap().Comm();
try
{
const Epetra_MpiComm &empicomm = dynamic_cast<const Epetra_MpiComm &>(ecomm);
mpicomm = empicomm.Comm();
}
catch (std::exception& e)
{
// Serial Comm with MPI
MPI_Comm_split(default_mpicomm, ecomm.MyPID(), 0, &mpicomm);
}
#endif
}
else if (input_matrix_.get() !=0 && input_coords_.get()!=0) //geometric and matrix inputs
{
queryObject_ = Teuchos::rcp(new ZoltanLib::QueryObject(input_matrix_, costs_, input_coords_, weights_, itype));
#ifdef HAVE_MPI
const Epetra_Comm &ecomm = input_matrix_->RowMatrixRowMap().Comm();
try
{
const Epetra_MpiComm &empicomm = dynamic_cast<const Epetra_MpiComm &>(ecomm);
mpicomm = empicomm.Comm();
}
catch (std::exception& e)
{
// Serial Comm with MPI
MPI_Comm_split(default_mpicomm, ecomm.MyPID(), 0, &mpicomm);
}
#endif
}
else if (input_graph_.get() != 0) //graph inputs
{
queryObject_ = Teuchos::rcp(new ZoltanLib::QueryObject(input_graph_, costs_, itype));
#ifdef HAVE_MPI
const Epetra_Comm &ecomm = input_graph_->RowMap().Comm();
try
{
const Epetra_MpiComm &empicomm = dynamic_cast<const Epetra_MpiComm &>(ecomm);
mpicomm = empicomm.Comm();
}
catch (std::exception& e)
{
// Serial Comm with MPI
MPI_Comm_split(default_mpicomm, ecomm.MyPID(), 0, &mpicomm);
}
#endif
}
else if (input_matrix_.get() != 0) //matrix inputs
{
queryObject_ = Teuchos::rcp(new ZoltanLib::QueryObject(input_matrix_, costs_, itype));
#ifdef HAVE_MPI
const Epetra_Comm &ecomm = input_matrix_->RowMatrixRowMap().Comm();
try
{
const Epetra_MpiComm &empicomm = dynamic_cast<const Epetra_MpiComm &>(ecomm);
mpicomm = empicomm.Comm();
}
catch (std::exception& e)
{
// Serial Comm with MPI
MPI_Comm_split(default_mpicomm, ecomm.MyPID(), 0, &mpicomm);
}
#endif
}
else if (input_coords_.get() != 0) // coord inputs
{
queryObject_ = Teuchos::rcp(new ZoltanLib::QueryObject(input_coords_, weights_));
#ifdef HAVE_MPI
const Epetra_Comm &ecomm = input_coords_->Map().Comm();
try
{
const Epetra_MpiComm &empicomm = dynamic_cast<const Epetra_MpiComm &>(ecomm);
mpicomm = empicomm.Comm();
}
catch (std::exception& e)
{
// Serial Comm with MPI
MPI_Comm_split(default_mpicomm, ecomm.MyPID(), 0, &mpicomm);
}
#endif
}
else // BlockMap inputs
{
queryObject_ = Teuchos::rcp(new ZoltanLib::QueryObject(input_map_, itype));
#ifdef HAVE_MPI
const Epetra_Comm &ecomm = input_map_->Comm();
try
{
const Epetra_MpiComm &empicomm = dynamic_cast<const Epetra_MpiComm &>(ecomm);
mpicomm = empicomm.Comm();
}
catch (std::exception& e)
{
// Serial Comm with MPI
MPI_Comm_split(default_mpicomm, ecomm.MyPID(), 0, &mpicomm);
}
#endif
}
float version;
int argcTmp=0;
char *argvTmp[1];
std::string lb_method_str("LB_METHOD");
// create a Zoltan problem
argvTmp[0] = NULL;
Zoltan_Initialize(argcTmp, argvTmp, &version);
zz_ = new Zoltan(mpicomm);
if (zz_ == NULL){
throw Isorropia::Exception("Error creating Zoltan object");
return (-1);
}
//////////////////////////
// set problem parameters
//////////////////////////
std::string dbg_level_str("DEBUG_LEVEL");
if (!zoltanParamList_.isParameter(dbg_level_str))
{
zoltanParamList_.set(dbg_level_str, "0");
}
if (!zoltanParamList_.isParameter(lb_method_str)) //set default parameters
{
if (input_type_ == graph_input_)
{
zoltanParamList_.set(lb_method_str, "GRAPH");
}
else if (input_type_ == geometric_input_)
{
if (!zoltanParamList_.isParameter(lb_method_str)) //MMW: Don't think this if is needed
zoltanParamList_.set(lb_method_str, "RCB");
}
else if (input_type_ == simple_input_) //not sure this is needed
{
zoltanParamList_.set(lb_method_str, "BLOCK");
}
else if (input_type_ == hgraph_graph_input_ || input_type_ == hgraph_geometric_input_ ||
input_type_ == graph_geometric_input_ || input_type_ == hgraph_graph_geometric_input_ )
{
zoltanParamList_.set(lb_method_str, "HIER");
}
else
{
zoltanParamList_.set(lb_method_str, "HYPERGRAPH");
}
}
// Make LB_APPROACH = PARTITION the default in Isorropia
std::string lb_approach_str("LB_APPROACH");
if (!zoltanParamList_.isParameter(lb_approach_str)) {
zoltanParamList_.set(lb_approach_str, "PARTITION");
}
// For fine-grain hypergraph, we don't want obj or (hyper)edge weights
if (input_type_ == hgraph2d_finegrain_input_)
{
zoltanParamList_.set("OBJ_WEIGHT_DIM", "0");
zoltanParamList_.set("EDGE_WEIGHT_DIM", "0");
}
else if (input_type_ == geometric_input_)
{
// We always overwrite user choice.
// if (!zoltanParamList_.isParameter("OBJ_WEIGHT_DIM")) {
if (weights_.get())
{
zoltanParamList_.set("OBJ_WEIGHT_DIM", "1");
}
else
{
zoltanParamList_.set("OBJ_WEIGHT_DIM", "0");
}
//}
}
else if(input_type_ != simple_input_) //graph or hypergraph
{
if (queryObject_->haveVertexWeights())
{
if (!zoltanParamList_.isParameter("OBJ_WEIGHT_DIM"))
{
zoltanParamList_.set("OBJ_WEIGHT_DIM", "1");
}
}
if (queryObject_->haveGraphEdgeWeights() ||
queryObject_->haveHypergraphEdgeWeights())
{
if (!zoltanParamList_.isParameter("EDGE_WEIGHT_DIM"))
{
zoltanParamList_.set("EDGE_WEIGHT_DIM", "1");
}
}
}
// For fine-grain hypergraph, we will use (row, col) of nz for
// vertex GIDs. Don't need LIDs.
if (input_type_ == hgraph2d_finegrain_input_)
{
zoltanParamList_.set("NUM_GID_ENTRIES", "2");
zoltanParamList_.set("NUM_LID_ENTRIES", "1");
}
Teuchos::ParameterList::ConstIterator
iter = zoltanParamList_.begin(),
iter_end = zoltanParamList_.end();
for(; iter != iter_end; ++iter)
{
const std::string& name = iter->first;
const std::string& value = Teuchos::getValue<std::string>(iter->second);
zz_->Set_Param(name, value);
}
// Set the query functions
zz_->Set_Num_Obj_Fn(ZoltanLib::QueryObject::Number_Objects, (void *)queryObject_.get());
zz_->Set_Obj_List_Fn(ZoltanLib::QueryObject::Object_List, (void *)queryObject_.get());
int ierr;
num_obj_ = ZoltanLib::QueryObject::Number_Objects((void *)queryObject_.get(), &ierr);
if (input_type_ == hgraph2d_finegrain_input_)
{
zz_->Set_HG_Size_CS_Fn(ZoltanLib::QueryObject::HG_Size_CS, (void *)queryObject_.get());
zz_->Set_HG_CS_Fn(ZoltanLib::QueryObject::HG_CS, (void *)queryObject_.get());
}
if (input_type_ == hgraph_input_ || input_type_ == hgraph_graph_input_ ||
input_type_ == hgraph_geometric_input_ || input_type_ == hgraph_graph_geometric_input_)
{
zz_->Set_HG_Size_CS_Fn(ZoltanLib::QueryObject::HG_Size_CS, (void *)queryObject_.get());
zz_->Set_HG_CS_Fn(ZoltanLib::QueryObject::HG_CS, (void *)queryObject_.get());
zz_->Set_HG_Size_Edge_Wts_Fn(ZoltanLib::QueryObject::HG_Size_Edge_Weights,
(void *)queryObject_.get());
zz_->Set_HG_Edge_Wts_Fn(ZoltanLib::QueryObject::HG_Edge_Weights, (void *)queryObject_.get());
}
if (input_type_ == graph_input_ || input_type_ == hgraph_graph_input_ ||
input_type_ == graph_geometric_input_ || input_type_ == hgraph_graph_geometric_input_)
{
zz_->Set_Num_Edges_Multi_Fn(ZoltanLib::QueryObject::Number_Edges_Multi, (void *)queryObject_.get());
zz_->Set_Edge_List_Multi_Fn(ZoltanLib::QueryObject::Edge_List_Multi, (void *)queryObject_.get());
}
if (input_type_ == geometric_input_ || input_type_ == hgraph_geometric_input_ ||
input_type_ == graph_geometric_input_ || input_type_ == hgraph_graph_geometric_input_)
{
zz_->Set_Num_Geom_Fn(ZoltanLib::QueryObject::Number_Geom, (void *)queryObject_.get());
zz_->Set_Geom_Multi_Fn(ZoltanLib::QueryObject::Geom_Multi, (void *)queryObject_.get());
}
return (ierr);
}
void YARPDispMap::assignWeights(node * graph, int length, unsigned char * data1, unsigned char * data2,int * Sobel, int * tmpDisparity, float alpha, int totArcs, int totNodes)
{
int i,j;
// int graphSize = length * length;
float arcCost;
int nodedisp;
int nextdisp;
float occlWeight;
unsigned char left, pright, cright, nright;
for (j=0; j<totNodes; j++)
{
for (i=0; i<graph[j].no_of_nodes; i++)
{
nodedisp = graph[j].left-graph[j].right;
if(graph[j].foll_nodes[i]==totNodes-1)
{
occlWeight = _beta * abs(nodedisp);
graph[j].arcweight[i] = occlWeight;
}
else
{
left = data1[graph[graph[j].foll_nodes[i]].left];
cright = data2[graph[graph[j].foll_nodes[i]].right];
if (graph[graph[j].foll_nodes[i]].right>0)
pright = data2[graph[graph[j].foll_nodes[i]].right-1];
else
pright = cright;
if (graph[graph[j].foll_nodes[i]].right<length-1)
nright = data2[graph[graph[j].foll_nodes[i]].right+1];
else
nright = cright;
nextdisp = graph[graph[j].foll_nodes[i]].left-graph[graph[j].foll_nodes[i]].right;
occlWeight = _beta * abs(nodedisp-nextdisp);
#ifdef DEFORMATION
if ((graph[j].left==graph[graph[j].foll_nodes[i]].left)||(graph[j].right==graph[graph[j].foll_nodes[i]].right))
occlWeight /= 1.5f;
#endif
arcCost = computeCost(left,cright,nright,pright)+occlWeight;
graph[j].arcweight[i] = arcCost;
if (alpha> -0.5)
if (graph[graph[j].foll_nodes[i]].left!= 0)
if (graph[graph[j].foll_nodes[i]].left!= length-1)
{
graph[j].arcweight[i] += computeInterCost(alpha,Sobel[graph[graph[j].foll_nodes[i]].left],tmpDisparity[graph[graph[j].foll_nodes[i]].left],nextdisp);
}
}
}
}
//First node
for (i=0; i<graph[0].no_of_nodes; i++)
{
nodedisp = 0;
left = data1[graph[graph[0].foll_nodes[i]].left];
cright = data2[graph[graph[0].foll_nodes[i]].right];
if (graph[graph[0].foll_nodes[i]].right>0)
pright = data2[graph[graph[0].foll_nodes[i]].right-1];
else
pright = cright;
if (graph[graph[0].foll_nodes[i]].right<length-1)
nright = data2[graph[graph[0].foll_nodes[i]].right+1];
else
nright = cright;
nextdisp = (graph[graph[0].foll_nodes[i]].left-graph[graph[0].foll_nodes[i]].right);
occlWeight = _beta * abs(nodedisp-nextdisp);
graph[0].arcweight[i] = computeCost(left,cright,nright,pright)+occlWeight;
if (alpha> -0.5)
if (graph[graph[0].foll_nodes[i]].left!= 0)
if (graph[graph[0].foll_nodes[i]].left!= length-1)
{
graph[0].arcweight[i] += computeInterCost(alpha,Sobel[graph[graph[0].foll_nodes[i]].left],tmpDisparity[graph[graph[0].foll_nodes[i]].left],nextdisp);
}
}
}
void reduce()
{
computeCost(root_);
reducible_[0].push_back(root_);
// sort reducible by reduce_cost
for (unsigned i=0;i<InsertPolicy::MAX_LEVELS;++i)
{
std::sort(reducible_[i].begin(), reducible_[i].end(),node_cmp());
}
while (colors_ > max_colors_ && colors_ > 1)
{
while (leaf_level_ >0 && reducible_[leaf_level_-1].size() == 0)
{
--leaf_level_;
}
if (leaf_level_ <= 0)
{
return;
}
// select best of all reducible:
unsigned red_idx = leaf_level_-1;
unsigned bestv = static_cast<unsigned>((*reducible_[red_idx].begin())->reduce_cost);
for(unsigned i=red_idx; i>=InsertPolicy::MIN_LEVELS; i--)
{
if (!reducible_[i].empty())
{
node *nd = *reducible_[i].begin();
unsigned gch = 0;
for(unsigned idx=0; idx<8; idx++)
{
if (nd->children_[idx])
gch += nd->children_[idx]->children_count;
}
if (gch==0 && nd->reduce_cost < bestv)
{
bestv = static_cast<unsigned>(nd->reduce_cost);
red_idx = i;
}
}
}
typename std::deque<node*>::iterator pos = reducible_[red_idx].begin();
node * cur_node = *pos;
unsigned num_children = 0;
for (unsigned idx=0; idx < 8; ++idx)
{
if (cur_node->children_[idx] != 0)
{
cur_node->children_count--;
++num_children;
cur_node->count += cur_node->children_[idx]->count;
//todo: case of nonleaf children, if someday sorting by reduce_cost doesn't handle it
delete cur_node->children_[idx], cur_node->children_[idx]=0;
}
}
reducible_[red_idx].erase(pos);
if (num_children > 0 )
{
colors_ -= (num_children - 1);
}
}
}
void ApexQuadricSimplifier::collapseEdge(QuadricEdge& edge)
{
uint32_t vNr0 = edge.vertexNr[0];
uint32_t vNr1 = edge.vertexNr[1];
QuadricVertex* qv0 = mVertices[vNr0];
QuadricVertex* qv1 = mVertices[vNr1];
PX_ASSERT(qv0->bDeleted == 0);
PX_ASSERT(qv1->bDeleted == 0);
//FILE* f = NULL;
//fopen_s(&f, "c:\\collapse.txt", "a");
//fprintf_s(f, "Collapse Vertex %d -> %d\n", vNr1, vNr0);
// contract edge to the vertex0
qv0->pos = qv0->pos * (1.0f - edge.ratio) + qv1->pos * edge.ratio;
qv0->q += qv1->q;
// merge the edges
for (uint32_t i = 0; i < qv1->mEdges.size(); i++)
{
QuadricEdge& ei = mEdges[qv1->mEdges[i]];
uint32_t vi = ei.otherVertex(vNr1);
if (vi == vNr0)
{
continue;
}
// test whether we already have this neighbor
bool found = false;
for (uint32_t j = 0; j < qv0->mEdges.size(); j++)
{
QuadricEdge& ej = mEdges[qv0->mEdges[j]];
if (ej.otherVertex(vNr0) == vi)
{
found = true;
break;
}
}
if (found)
{
mVertices[vi]->removeEdge((int32_t)qv1->mEdges[i]);
ei.deleted = true;
if (ei.heapPos >= 0)
{
heapRemove((uint32_t)ei.heapPos, false);
}
#if TESTING
testHeap();
#endif
}
else
{
ei.replaceVertex(vNr1, vNr0);
qv0->mEdges.pushBack(qv1->mEdges[i]);
}
}
// remove common edge and update adjacent edges
for (int32_t i = (int32_t)qv0->mEdges.size() - 1; i >= 0; i--)
{
QuadricEdge& ei = mEdges[qv0->mEdges[(uint32_t)i]];
if (ei.otherVertex(vNr0) == vNr1)
{
qv0->mEdges.replaceWithLast((uint32_t)i);
}
else
{
computeCost(ei);
if (ei.heapPos >= 0)
{
heapUpdate((uint32_t)ei.heapPos);
}
#if TESTING
testHeap();
#endif
}
}
// delete collapsed triangles
for (int32_t i = (int32_t)qv0->mTriangles.size() - 1; i >= 0; i--)
{
uint32_t triangleIndex = qv0->mTriangles[(uint32_t)i];
QuadricTriangle& t = mTriangles[triangleIndex];
if (!t.deleted && t.containsVertex(vNr1))
{
mNumDeletedTriangles++;
t.deleted = true;
//fprintf_s(f, "Delete Triangle %d\n", triangleIndex);
PX_ASSERT(t.containsVertex(vNr0));
for (uint32_t j = 0; j < 3; j++)
{
mVertices[t.vertexNr[j]]->removeTriangle((int32_t)triangleIndex);
//fprintf_s(f, " v %d\n", t.vertexNr[j]);
}
}
}
// update triangles
for (uint32_t i = 0; i < qv1->mTriangles.size(); i++)
{
QuadricTriangle& t = mTriangles[qv1->mTriangles[i]];
if (t.deleted)
{
continue;
}
if (t.containsVertex(vNr1))
{
qv0->mTriangles.pushBack(qv1->mTriangles[i]);
}
t.replaceVertex(vNr1, vNr0);
}
mNumDeletedVertices += qv1->bDeleted == 1 ? 0 : 1;
qv1->bDeleted = 1;
edge.deleted = true;
//fclose(f);
#if TESTING
testMesh();
testHeap();
#endif
}
/**
* Local greedy surface area heuristic
*
* TODO : The accurate way of determining the candidate planes thus is
* to first clip the triangle t to the voxel V , and use the sides of
* the clipped triangle’s AABB B(t ∩ V )
*/
bool KdTreeNode::findBestSplit(int axis, float* bestSplit)
{
float cost, bestCost = FLT_MAX;
std::list<float> limits;
std::list<Object*>::iterator iterObj;
AABB * aabb;
for (iterObj = objects.begin(); iterObj != objects.end(); iterObj++)
{
aabb = (*iterObj)->getAABB();
if (axis == 0) { // X
limits.push_back(aabb->minX);
limits.push_back(aabb->maxX);
} else if (axis == 1) { // Y
limits.push_back(aabb->minY);
limits.push_back(aabb->maxY);
} else { // Z
limits.push_back(aabb->minZ);
limits.push_back(aabb->maxZ);
}
}
limits.sort();
limits.unique();
float thisMin, thisMax;
if (axis == 0) { // X
thisMin = this->aabb->minX;
thisMax = this->aabb->maxX;
} else if (axis == 1) { // Y
thisMin = this->aabb->minY;
thisMax = this->aabb->maxY;
} else { // Z
thisMin = this->aabb->minZ;
thisMax = this->aabb->maxZ;
}
//Logger::log(LOG_DEBUG)<<"limit list size = "<<limits.size()<<std::endl;
std::list<Object*> objectsNode;
std::list<Object*> objectsLeft;
std::list<Object*> objectsRight;
objectsNode.assign(objects.begin(), objects.end());
bool splitFound = false;
std::list<float>::iterator iterLimit;
for (iterLimit = limits.begin(); iterLimit != limits.end(); iterLimit++)
{
if (thisMin < *iterLimit && *iterLimit < thisMax) {
splitFound = true;
//Logger::log(LOG_DEBUG)<<"----------------------------"<<std::endl;
//Logger::log(LOG_DEBUG)<<"limit candidate = "<<*iterLimit<<std::endl;
createChildren(axis, *iterLimit);
splitObjects(axis, &objectsNode, &objectsLeft, &objectsRight);
cost = computeCost(&objectsLeft, &objectsRight);
//Logger::log(LOG_DEBUG)<<"cost = "<<cost<<std::endl;
if (cost < bestCost) {
bestCost = cost;
*bestSplit = *iterLimit;
}
//Logger::log(LOG_DEBUG)<<"best cost = "<<bestCost<<std::endl;
/*
objectsNode.clear();
objectsNode.splice(objectsNode.begin(), objectsRight);
*/
objectsLeft.clear();
objectsRight.clear();
if (left != NULL) delete left;
if (right != NULL) delete right;
}
}
//Logger::log(LOG_DEBUG)<<"bestSplit = "<<*bestSplit<<std::endl;
return splitFound;
}
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, unsigned char threshold, double maxZ, double zLB, double zUB,
double inflationRadius, double circumscribedRadius, double inscribedRadius, double weight,
double obstacleRange, double raytraceRange)
: ObstacleMapAccessor(0, 0, width, height, resolution),
maxZ_(maxZ), zLB_(zLB), zUB_(zUB),
inflationRadius_(toCellDistance(inflationRadius, (unsigned int) ceil(width * resolution), resolution)),
circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
weight_(std::max(0.0, std::min(weight, 1.0))), sq_obstacle_range_(obstacleRange * obstacleRange),
sq_raytrace_range_((raytraceRange / resolution) * (raytraceRange / resolution)),
staticData_(NULL), xy_markers_(NULL), kernelWidth_((circumscribedRadius_ * 2) + 1)
{
if(weight != weight_){
ROS_INFO("Warning - input weight %f is invalid and has been set to %f\n", weight, weight_);
}
unsigned int i, j;
staticData_ = new unsigned char[width_*height_];
xy_markers_ = new bool[width_*height_];
memset(xy_markers_, 0, width_ * height_* sizeof(bool));
// Set up a cache of distance values for a kernel around the robot
cachedDistances = new double*[inflationRadius_+1];
for (i=0; i<=inflationRadius_; i++) {
cachedDistances[i] = new double[inflationRadius_+1];
for (j=0; j<=i; j++) {
cachedDistances[i][j] = sqrt (pow(i, 2) + pow(j, 2));
cachedDistances[j][i] = cachedDistances[i][j];
}
}
// Allocate memory for a kernel matrix to be used for map updates aruond the robot
kernelData_ = new unsigned char[kernelWidth_ * kernelWidth_];
setCircumscribedCostLowerBound(computeCost(circumscribedRadius_));
// For the first pass, just clean up the data and get the set of original obstacles.
std::vector<unsigned int> updates;
for (unsigned int i=0;i<width_;i++){
for (unsigned int j=0;j<height_;j++){
unsigned int ind = MC_IND(i, j);
costData_[ind] = data[ind];
// If the source value is greater than the threshold but less than the NO_INFORMATION LIMIT
// then set it to the threshold.
// Workaround for miletone 1. We treat no information as a lethal obstacle.
if (/*costData_[ind] != NO_INFORMATION && */costData_[ind] >= threshold)
costData_[ind] = LETHAL_OBSTACLE;
// Lethal obstacles will have to be inflated. We take the approach that they will all be treated initially
// as dynamic obstacles, and will be faded out as such, but
if(costData_[ind] >= LETHAL_OBSTACLE){
enqueue(ind, i, j);
}
}
}
// Now propagate the costs derived from static data
propagateCosts();
// Instantiate static data
memcpy(staticData_, costData_, width_ * height_);
}
Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y,
double resolution, double origin_x, double origin_y, double inscribed_radius,
double circumscribed_radius, double inflation_radius, double max_obstacle_range,
double max_obstacle_height, double max_raytrace_range, double weight,
const std::vector<unsigned char>& static_data, unsigned char lethal_threshold, bool track_unknown_space, unsigned char unknown_cost_value) : size_x_(cells_size_x),
size_y_(cells_size_y), resolution_(resolution), origin_x_(origin_x), origin_y_(origin_y), static_map_(NULL),
costmap_(NULL), markers_(NULL), max_obstacle_range_(max_obstacle_range),
max_obstacle_height_(max_obstacle_height), max_raytrace_range_(max_raytrace_range), cached_costs_(NULL), cached_distances_(NULL),
inscribed_radius_(inscribed_radius), circumscribed_radius_(circumscribed_radius), inflation_radius_(inflation_radius),
weight_(weight), lethal_threshold_(lethal_threshold), track_unknown_space_(track_unknown_space), unknown_cost_value_(unknown_cost_value), inflation_queue_(){
//creat the costmap, static_map, and markers
costmap_ = new unsigned char[size_x_ * size_y_];
static_map_ = new unsigned char[size_x_ * size_y_];
markers_ = new unsigned char[size_x_ * size_y_];
memset(markers_, 0, size_x_ * size_y_ * sizeof(unsigned char));
//convert our inflations from world to cell distance
cell_inscribed_radius_ = cellDistance(inscribed_radius);
cell_circumscribed_radius_ = cellDistance(circumscribed_radius);
cell_inflation_radius_ = cellDistance(inflation_radius);
//set the cost for the circumscribed radius of the robot
circumscribed_cost_lb_ = computeCost(cell_circumscribed_radius_);
//based on the inflation radius... compute distance and cost caches
cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
cached_distances_ = new double*[cell_inflation_radius_ + 2];
for(unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i){
cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
cached_distances_[i] = new double[cell_inflation_radius_ + 2];
for(unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j){
cached_distances_[i][j] = sqrt(i*i + j*j);
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
}
}
if(!static_data.empty()){
ROS_ASSERT_MSG(size_x_ * size_y_ == static_data.size(), "If you want to initialize a costmap with static data, their sizes must match.");
//make sure the inflation queue is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_queue_.empty(), "The inflation queue must be empty at the beginning of inflation");
unsigned int index = 0;
unsigned char* costmap_index = costmap_;
std::vector<unsigned char>::const_iterator static_data_index = static_data.begin();
//initialize the costmap with static data
for(unsigned int i = 0; i < size_y_; ++i){
for(unsigned int j = 0; j < size_x_; ++j){
//check if the static value is above the unknown or lethal thresholds
if(track_unknown_space_ && unknown_cost_value_ > 0 && *static_data_index == unknown_cost_value_)
*costmap_index = NO_INFORMATION;
else if(*static_data_index >= lethal_threshold_)
*costmap_index = LETHAL_OBSTACLE;
else
*costmap_index = FREE_SPACE;
if(*costmap_index == LETHAL_OBSTACLE){
unsigned int mx, my;
indexToCells(index, mx, my);
enqueue(index, mx, my, mx, my, inflation_queue_);
}
++costmap_index;
++static_data_index;
++index;
}
}
//now... let's inflate the obstacles
inflateObstacles(inflation_queue_);
//we also want to keep a copy of the current costmap as the static map
memcpy(static_map_, costmap_, size_x_ * size_y_ * sizeof(unsigned char));
}
else{
//everything is unknown initially if we don't have a static map unless we aren't tracking unkown space in which case it is free
resetMaps();
}
}
void planPath(std::vector<Matrix<P_DIM> > l, beliefPenaltyMPC_params& problem, beliefPenaltyMPC_output& output, beliefPenaltyMPC_info& info, std::ofstream& f) {
initProblemParams(l);
util::Timer solveTimer, trajTimer;
double totalSolveTime = 0, trajTime = 0;
double totalTrajCost = 0;
std::vector<Matrix<B_DIM> > B_total(T*NUM_WAYPOINTS);
std::vector<Matrix<U_DIM> > U_total((T-1)*NUM_WAYPOINTS);
std::vector<Matrix<B_DIM> > B(T);
Matrix<U_DIM> uinit;
Matrix<X_DIM,1> xtmp;
Matrix<X_DIM,X_DIM> stmp;
for(int i=0; i < NUM_WAYPOINTS; ++i) {
LOG_INFO("Going to waypoint %d",i);
// goal is waypoint position + direct angle + landmarks
xGoal.insert(0, 0, waypoints[i]);
// want to be facing the next waypoint
if (i < NUM_WAYPOINTS - 1) {
xGoal[2] = atan2(waypoints[i+1][1] - waypoints[i][1], waypoints[i+1][0] - waypoints[i][0]);
} else {
xGoal[2] = atan2(xGoal[1] - x0[1], xGoal[0] - x0[0]);
}
xGoal.insert(C_DIM, 0, x0.subMatrix<L_DIM,1>(C_DIM,0));
util::Timer_tic(&trajTimer);
std::vector<Matrix<U_DIM> > U(T-1);
bool initTrajSuccess = initTraj(x0.subMatrix<C_DIM,1>(0,0), xGoal.subMatrix<C_DIM,1>(0,0), U, T);
if (!initTrajSuccess) {
LOG_ERROR("Failed to initialize trajectory, continuing anyways");
//exit(-1);
}
double initTrajTime = util::Timer_toc(&trajTimer);
trajTime += initTrajTime;
vec(x0, SqrtSigma0, B[0]);
for(int t=0; t < T-1; ++t) {
B[t+1] = beliefDynamics(B[t], U[t]);
}
//pythonDisplayTrajectory(B, U, waypoints, landmarks, T, false);
//double initTrajCost = computeCost(B, U);
//LOG_INFO("Initial trajectory cost: %4.10f", initTrajCost);
Timer_tic(&solveTimer);
double cost = 0;
int iter = 0;
while(true) {
try {
cost = beliefPenaltyCollocation(B, U, problem, output, info);
break;
}
catch (forces_exception &e) {
if (iter > 3) {
LOG_ERROR("Tried too many times, giving up");
pythonDisplayTrajectory(U, T, false);
logDataToFile(f, B_total, INFTY, INFTY, 1);
//exit(-1);
return;
}
LOG_ERROR("Forces exception, trying again");
iter++;
}
}
double solvetime = util::Timer_toc(&solveTimer);
totalSolveTime += solvetime;
vec(x0, SqrtSigma0, B[0]);
for (int t = 0; t < T-1; ++t) {
B[t+1] = beliefDynamics(B[t], U[t]);
unVec(B[t+1], xtmp, stmp);
}
for (int t = 0; t < T-1; ++t) {
B_total[t+T*i] = B[t];
U_total[t+(T-1)*i] = U[t];
}
B_total[T-1+T*i] = B[T-1];
totalTrajCost += computeCost(B,U);
// LOG_INFO("Initial cost: %4.10f", initTrajCost);
// LOG_INFO("Optimized cost: %4.10f", cost);
// LOG_INFO("Actual cost: %4.10f", computeCost(B,U));
// LOG_INFO("Solve time: %5.3f ms", solvetime*1000);
//pythonDisplayTrajectory(B, U, waypoints, landmarks, T, true);
unVec(B[T-1], x0, SqrtSigma0);
}
LOG_INFO("Total trajectory cost: %4.10f", totalTrajCost);
LOG_INFO("Total trajectory solve time: %5.3f ms", trajTime*1000);
LOG_INFO("Total solve time: %5.3f ms", totalSolveTime*1000);
logDataToFile(f, B_total, totalSolveTime*1000, trajTime*1000, 0);
pythonDisplayTrajectory(B_total, U_total, waypoints, landmarks, T*NUM_WAYPOINTS, false);
}
void ParallellizeByCubes3D::parallelize(){
// we must have the same number of blocks as processors
PLB_PRECONDITION(xTiles*yTiles*zTiles == processorNumber);
plint totalCost = computeCost(originalBlocks, finestBoundingBox);
plint idealCostPerProcessor = totalCost/processorNumber;
pcout << "Total cost of computations = " << totalCost << std::endl;
pcout << "We are using " << processorNumber << " processors...\n";
pcout << "Ideal cost per processor = " << idealCostPerProcessor << std::endl;
std::vector<plint> totalCosts(processorNumber);
// greedy load balancing part
// plint currentProcessor = 0;
// bool allAssigned = false;
// plint iBlock = 0;
// plint maxBlockCost = 0;
// while ( (currentProcessor<processorNumber) && !allAssigned) {
// plint blockCost = computeCost(originalBlocks, finestDivision[iBlock]);
// if (blockCost > maxBlockCost) maxBlockCost = blockCost;
// if ( totalCosts[currentProcessor] < idealCostPerProcessor ){
// totalCosts[currentProcessor] += blockCost;
// mpiDistribution[iBlock] = currentProcessor;
// ++iBlock;
// }
// else {
// currentProcessor++;
// }
// if (iBlock>=(plint)finestDivision.size()){
// allAssigned = true;
// }
// }
//
// if (maxBlockCost > idealCostPerProcessor){
// pcout << "There is a problem : maxBlockCost=" << maxBlockCost << " and ideal cost=" << idealCostPerProcessor
// << std::endl;
// }
plint total = 0;
for (plint iProc=0; iProc<processorNumber; ++iProc){
plint blockCost = computeCost(originalBlocks,finestDivision[iProc]);
totalCosts[iProc] += blockCost;
mpiDistribution[iProc] = iProc;
total += blockCost;
}
pcout << "---- Costs Per Processor ----\n";
for (pluint i=0; i<totalCosts.size(); ++i){
pcout << i << " : " << totalCosts[i] << std::endl;
// check if everyone is doing something
if (totalCosts[i] == 0){
pcout << "\t >> processor " << i << " does not have work to do. Exiting.....\n";
std::exit(1);
}
}
pcout << "*******************************\n";
pcout << "Sum of all costs = " << total << std::endl;
pcout << "*******************************\n";
// convert the original blocks to the new blocks
recomputedBlocks.resize(originalBlocks.size());
finalMpiDistribution.resize(originalBlocks.size());
plint finestLevel= (plint)originalBlocks.size()-1;
for (plint iLevel=finestLevel; iLevel>=0; --iLevel) {
parallelizeLevel(iLevel, originalBlocks,finestDivision, mpiDistribution);
// Adapt the regions to the next-coarser level.
for (pluint iRegion=0; iRegion<finestDivision.size(); ++iRegion) {
finestDivision[iRegion] = finestDivision[iRegion].divideAndFitSmaller(2);
}
}
}
void MockStrategy::performNormalComputeCost() {
computeCost();
}