#include <algorithm>
#include <numeric>
#include <iostream>
#include <set>
#include <queue>
#include <utility>
#include <unordered_map>
#include <unordered_set>
#include <limits>
#include <cmath>
#include <ctime>
#include <cassert>

#include <boost/random.hpp>
#include <boost/functional/hash.hpp>

#include "Factor.h"
#include "DisjointSet.h"
#include "DisjointSetBT.h"
#include "RandomSource.h"
#include "VAcyclicDecomposition.h"

namespace Grante {

const unsigned int VAcyclicDecomposition::sa_steps = 20;
const double VAcyclicDecomposition::sa_t0 = 5.0;
const double VAcyclicDecomposition::sa_tfinal = 0.1;

VAcyclicDecomposition::VAcyclicDecomposition(const FactorGraph* fg)
	: fg(fg), fgu(fg) {
}

double VAcyclicDecomposition::ComputeDecompositionGreedy(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed) {
	return (ComputeDecomposition(factor_weights, factor_is_removed,
		1, 1.0e-6, 1.0e-6));
}

double VAcyclicDecomposition::ComputeDecompositionSA(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed) {
	return (ComputeDecomposition(factor_weights, factor_is_removed,
		sa_steps, sa_t0, sa_tfinal));
}

// Compute solution based on iterative set packing heuristic
double VAcyclicDecomposition::ComputeDecompositionSP(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed) {
	// Throughout the algorithm, we keep a partition of the factor graph
	// variables
	const std::vector<Factor*>& factors = fg->Factors();
	DisjointSet wset(fg->Cardinalities().size());
	std::set<unsigned int> E;
	for (unsigned int iter = 1; true; ++iter) {
		// Go through all the factors of the model and decide whether they are
		// still active.  A factor is inactive if any of the following
		// conditions are met:
		//
		// 1. All its adjacent variables are already mapped to one component,
		// 2. There are two or more factors joining the same components.
		//    If we would allow these factors to be active, the components
		//    could be merged, violating v-acyclicity.
		//
		// If 1. and 2. are false, the factor is 'active'.
		std::unordered_map<std::set<unsigned int>, unsigned int,
			boost::hash<std::set<unsigned int> > > EF;
		std::vector<bool> factor_active(factors.size(), true);
		for (unsigned int fi = 0; fi < factors.size(); ++fi) {
			const Factor* fac = factors[fi];
			const std::vector<unsigned int>& fac_vars = fac->Variables();

			// Build mapped edge set
			E.clear();
			for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi)
				E.insert(wset.FindSet(fac_vars[fvi]));
			// -> All mapped to one component?
			if (E.size() == 1) {
				factor_active[fi] = false;
				continue;
			}

			// Check whether the same component link exist already -> delete
			std::unordered_map<std::set<unsigned int>,
				unsigned int>::const_iterator efi = EF.find(E);
			if (efi != EF.end()) {
				factor_active[fi] = false;
				factor_active[efi->second] = false;
				continue;
			}

			// Is active, add to dupe checking list
			EF[E] = fi;
		}

		// Solve set packing problem
		std::vector<std::unordered_set<unsigned int> > S;
		std::vector<double> S_weights;
		std::vector<unsigned int> S_fi;
		std::vector<bool> S_is_selected;
		for (std::unordered_map<std::set<unsigned int>,
			unsigned int>::const_iterator efi = EF.begin(); efi != EF.end();
			++efi) {
			if (factor_active[efi->second] == false)
				continue;

#if 0
			std::cout << "VASP iter " << iter << ", adding:";
			for (std::set<unsigned int>::const_iterator
				esi = efi->first.begin(); esi != efi->first.end(); ++esi)
				std::cout << " " << *esi;
			std::cout << std::endl;
#endif

			// Add, negative cost to benefit
			S.push_back(std::unordered_set<unsigned int>(
				efi->first.begin(), efi->first.end()));
			S_weights.push_back(factor_weights[efi->second]);
			S_fi.push_back(efi->second);
		}
		// No further merging possible -> break
		if (S.empty())
			break;
		Grante::VAcyclicDecomposition::ComputeSetPacking(
			S, S_weights, S_is_selected, 5);	// TODO: make 5 configurable

		// Perform merging
		unsigned int merged = 0;
		for (unsigned int si = 0; si < S.size(); ++si) {
			if (S_is_selected[si] == false)
				continue;

			const std::vector<unsigned int>& fac_vars =
				factors[S_fi[si]]->Variables();
			assert(fac_vars.size() >= 2);
			for (unsigned int fvi = 1; fvi < fac_vars.size(); ++fvi) {
				wset.Link(wset.FindSet(fac_vars[0]),
					wset.FindSet(fac_vars[fvi]));
			}
			merged += 1;
		}
		// This can happen due to symmetry, where the Lagrangian relaxation
		// method fails to identify a solution
		if (merged == 0)
			break;
	}

	// Reconstruct final solution: every factor not mapped to one component is
	// removed.
	factor_is_removed.resize(factors.size());
	std::fill(factor_is_removed.begin(), factor_is_removed.end(), true);
	double obj = 0.0;
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const Factor* fac = factors[fi];
		const std::vector<unsigned int>& fac_vars = fac->Variables();

		// Build mapped edge set
		E.clear();
		for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi)
			E.insert(wset.FindSet(fac_vars[fvi]));

		// All mapped to one component? -> factor is kept
		if (E.size() == 1) {
			factor_is_removed[fi] = false;
			obj += factor_weights[fi];
		}
	}
	return (obj);
}

double VAcyclicDecomposition::ComputeSetPacking(
	const std::vector<std::unordered_set<unsigned int> >& S,
	const std::vector<double>& S_weights,
	std::vector<bool>& S_is_selected, unsigned int lr_max_iter) {
	assert(S.size() == S_weights.size());

	// Random number generation
	boost::mt19937 rgen(RandomSource::GetGlobalRandomSeed());
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > randu(rgen, rdestu);

	// Fast vertex->edgeset map, VE[v]: list of set indices
	std::unordered_map<unsigned int, std::set<unsigned int> > VE;
	// Lagrange multipliers for constraints: \sum_{e, v \in e} x(e) <= 1.
	std::unordered_map<unsigned int, double> V_mu;
	std::unordered_map<unsigned int, double> V_mu_subg;
	for (unsigned int si = 0; si < S.size(); ++si) {
		for (std::unordered_set<unsigned int>::const_iterator
			ssi = S[si].begin(); ssi != S[si].end(); ++ssi) {
			VE[*ssi].insert(si);

			// Minimally random initialization to prevent symmetry problems:
			// if many costs have the same weight, and we initialize all
			// multipliers to zero, then we are at a dual degenerate ridge of
			// the cost function, where the subgradient is symmetric: we never
			// manage to break the symmetry, producing trivial solutions.
			V_mu[*ssi] = 1.0e-3*randu();
			V_mu_subg[*ssi] = 0.0;
		}
	}

	// Current, possibly infeasible solution
	std::vector<bool> cur_lr_solution(S.size(), false);
	std::vector<bool> cur_feas_solution(S.size(), false);
	double upper_bound = std::numeric_limits<double>::infinity();
	double lower_bound = -std::numeric_limits<double>::infinity();

	// Perform Lagrangian relaxation iterations
	for (unsigned int lr_iter = 1; lr_iter < lr_max_iter; ++lr_iter) {
		// Solve for x_e
		double cur_obj = 0.0;
		double feas_obj = 0.0;
		for (unsigned int si = 0; si < S.size(); ++si) {
			double si_obj = S_weights[si];
			for (std::unordered_set<unsigned int>::const_iterator
				ssi = S[si].begin(); ssi != S[si].end(); ++ssi) {
				si_obj += V_mu[*ssi];
			}
			cur_lr_solution[si] = (si_obj > 0.0) ? true : false;
			cur_obj += cur_lr_solution[si] ? si_obj : 0.0;

			cur_feas_solution[si] = cur_lr_solution[si];
			feas_obj += cur_feas_solution[si] ? S_weights[si] : 0.0;
		}
		// - \sum_v \mu_v
		for (std::unordered_map<unsigned int, double>::const_iterator
			vmi = V_mu.begin(); vmi != V_mu.end(); ++vmi) {
			cur_obj -= vmi->second;
		}

		// cur_obj provides a global upper bound on the solution
		if (cur_obj < upper_bound)
			upper_bound = cur_obj;

		// Check feasibility.  If solution is infeasible, produce a primal
		// feasible solution
		double cslackness = 0.0;
		double feasible = true;
		double subg_norm = 0.0;
		for (std::unordered_map<unsigned int, double>::iterator
			vmi = V_mu.begin(); vmi != V_mu.end(); ++vmi) {
			double mu_v_sd = -1.0;
			const std::set<unsigned int>& eset = VE[vmi->first];
			for (std::set<unsigned int>::const_iterator ei = eset.begin();
				ei != eset.end(); ++ei) {
				mu_v_sd += cur_lr_solution[*ei] ? 1.0 : 0.0;
			}
			cslackness += vmi->second * mu_v_sd;

			// Constraint is violated, adjust multiplier
			if (mu_v_sd > 0.0) {
				feasible = false;

				// Remove sets until feasible
				double infeas_m = mu_v_sd;
				for (std::set<unsigned int>::const_iterator ei = eset.begin();
					ei != eset.end(); ++ei) {
					// Sufficient number of edges removed? -> done
					if (infeas_m <= 1.0e-8)
						break;

					if (cur_lr_solution[*ei] == false)
						continue;

					infeas_m -= 1.0;

					// Was the set removed already? -> nothing to do
					if (cur_feas_solution[*ei] == false)
						continue;

					// Remove edge
					cur_feas_solution[*ei] = false;
					feas_obj -= S_weights[*ei];
				}
				assert(infeas_m <= 1.0e-8);
			}

			// Adjust Lagrange multiplier by projected gradient method
			subg_norm += mu_v_sd * mu_v_sd;
			V_mu_subg[vmi->first] = mu_v_sd;
		}

		// Compute step size: Polyak
		double beta_m = 1.0;
		double beta = (1.0 + beta_m) /
			(beta_m + static_cast<double>(lr_iter));
		double alpha = 0.0;
		if (subg_norm >= 1.0e-8) {
			alpha = (beta * (cur_obj - feas_obj)) / subg_norm;
		}
		for (std::unordered_map<unsigned int, double>::iterator
			vmi = V_mu_subg.begin(); vmi != V_mu_subg.end(); ++vmi) {
			// TODO: use subg_norm for LR mult update
			V_mu[vmi->first] -= alpha * vmi->second;
			V_mu[vmi->first] = std::min(0.0, V_mu[vmi->first]);
		}

		// Update lower bound and solution
		if (feas_obj > lower_bound) {
			lower_bound = feas_obj;
			S_is_selected = cur_feas_solution;
		}
#if 0
		std::cout << "set packing LR iter " << lr_iter
			<< ", lb " << lower_bound << ", ub " << upper_bound
			<< ", |subg| " << std::sqrt(subg_norm)
			<< std::endl;
#endif

		// Sufficient optimality condition:
		// primal feasible and \mu'v(y(\mu)) = cslackness = 0.
		if (feasible && std::fabs(cslackness) <= 1.0e-8) {
			S_is_selected = cur_lr_solution;
			return (cur_obj);
		}
	}
	return (lower_bound);
}

double VAcyclicDecomposition::ComputeDecomposition(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed,
	unsigned int csa_steps, double csa_t0, double csa_tfinal) {
	const std::vector<Factor*>& factors = fg->Factors();
	assert(factor_weights.size() == factors.size());

	// Initialization: all factors removed
	std::unordered_set<unsigned int> removed_factors(factors.size());
	for (unsigned int fi = 0; fi < factors.size(); ++fi)
		removed_factors.insert(fi);

	// Components
	std::vector<unsigned int> node_to_comp;
	const std::vector<unsigned int>& card = fg->Cardinalities();
	node_to_comp.reserve(card.size());
	std::vector<std::unordered_set<unsigned int> > comps(card.size());
	for (unsigned int ni = 0; ni < card.size(); ++ni) {
		node_to_comp[ni] = ni;
		comps[ni].insert(ni);
	}

	// Objective: weights of all included factors (zero)
	double obj = 0.0;

	// Best solution so far
	std::unordered_set<unsigned int>
		best_removed_factors(removed_factors);
	double best_obj = obj;

	// Random number generators: factor index
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(
		reinterpret_cast<size_t>(fg) ^ std::time(0))+14);
	boost::uniform_int<unsigned int> rdestd(0,
		static_cast<boost::uint32_t>(factors.size()-1));
	boost::variate_generator<boost::mt19937,
		boost::uniform_int<unsigned int> > rand_fi(rgen, rdestd);

	// Random number generator: Metropolis chain
	boost::mt19937 rgen2(static_cast<const boost::uint32_t>(
		reinterpret_cast<size_t>(fg) ^ std::time(0))+13);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > rand_m(rgen2, rdestu);

	// Calculate exponential multiplier alpha such that T(sa_steps)=Tfinal.
	double alpha = std::exp(std::log(csa_tfinal / csa_t0) /
		static_cast<double>(csa_steps));
	for (unsigned int k = 1; k <= csa_steps; ++k) {
		// Exponential schedule
		double temperature = csa_t0 * std::pow(alpha, static_cast<double>(k));
#if 0
		std::cout << "iter " << k << ", temp " << temperature
			<< ", best obj " << best_obj << std::endl;
#endif

		// One epoch
		for (unsigned int fi_d = 0; fi_d < factors.size(); ++fi_d) {
			unsigned int fi = rand_fi();
			// Special case of one pass: linear
			if (csa_steps == 1)
				fi = fi_d;

			double delta_E = std::numeric_limits<double>::signaling_NaN();
			bool is_in_G = (removed_factors.count(fi) == 0);
			if (is_in_G) {
				// Factor is currently in G, can be removed.
				delta_E = -factor_weights[fi];
			} else {
				// Factor is currently removed, check whether it can be added.
				if (IsComponentBridge(node_to_comp, comps, fi) == false)
					continue;	// not possible, reject

				delta_E = factor_weights[fi];
			}
			// Reject
			if (delta_E <= 0.0 && rand_m() >= std::exp(delta_E/temperature))
				continue;

			// Accept
			if (is_in_G) {
				// Split components adjacent to the factor
				SplitComponents(removed_factors, node_to_comp, comps, fi);
			} else {
				// Merge components adjacent to the factor
				MergeComponents(node_to_comp, comps, factors[fi]);
				removed_factors.erase(fi);	// its in the graph now
			}
			obj += delta_E;

			// Keep track of best solution
			if (obj > best_obj) {
				best_obj = obj;
				best_removed_factors = removed_factors;
			}
		}
	}

	// Return list of removed factors
	factor_is_removed.resize(factors.size());
	std::fill(factor_is_removed.begin(), factor_is_removed.end(), false);
	for (std::unordered_set<unsigned int>::const_iterator
		ri = best_removed_factors.begin(); ri != best_removed_factors.end();
		++ri) {
		factor_is_removed[*ri] = true;
	}
	return (best_obj);
}

bool VAcyclicDecomposition::IsComponentBridge(
	std::vector<unsigned int>& node_to_comp,
	std::vector<std::unordered_set<unsigned int> >& comps,
	unsigned int factor_index) const {
	const Factor* fac = fg->Factors()[factor_index];
	const std::vector<unsigned int>& fvars = fac->Variables();

	// 1. Collect adjacent factor sets for each component
	std::vector<std::unordered_set<unsigned int> >
		comp_facset(fvars.size());;
	for (unsigned int fvi = 0; fvi < fvars.size(); ++fvi) {
		const std::unordered_set<unsigned int>& cur_vars =
			comps[node_to_comp[fvars[fvi]]];
		assert(cur_vars.empty() == false);
		for (std::unordered_set<unsigned int>::const_iterator
			cvi = cur_vars.begin(); cvi != cur_vars.end(); ++cvi) {
			const std::set<unsigned int>& cur_facset =
				fgu.AdjacentFactors(*cvi);
			comp_facset[fvi].insert(cur_facset.begin(), cur_facset.end());
		}
		// Do not consider the factor of interest
		comp_facset[fvi].erase(factor_index);
	}

	// 2. Find factors that would link the components
	for (unsigned int c1 = 0; c1 < comp_facset.size(); ++c1) {
		const std::unordered_set<unsigned int>& c1_fset = comp_facset[c1];
		for (unsigned int c2 = c1 + 1; c2 < comp_facset.size(); ++c2) {
			for (std::unordered_set<unsigned int>::const_iterator
				c2i = comp_facset[c2].begin(); c2i != comp_facset[c2].end();
				++c2i) {
				// Check whether another factor between the components exists.
				// If so, factor_index is no bridge.
				if (c1_fset.count(*c2i) > 0)
					return (false);
			}
		}
	}
	return (true);
}

void VAcyclicDecomposition::SplitComponents(
	std::unordered_set<unsigned int>& removed_factors,
	std::vector<unsigned int>& node_to_comp,
	std::vector<std::unordered_set<unsigned int> >& comps,
	unsigned int fac_index) const {
	// Obtain variable indices (these are all in the same component)
	const Factor* fac = fg->Factors()[fac_index];
	const std::vector<unsigned int>& fvar = fac->Variables();
	for (unsigned int fvi = 0; fvi < fvar.size(); ++fvi) {
		assert(node_to_comp[fvar[fvi]] == node_to_comp[fvar[0]]);
	}

	// Save joined component, remove factor from graph
#if 0
	std::unordered_map<unsigned int> comp_old;
	comp_old.swap(comps[node_to_comp[fvar[0]]]);
#endif
	comps[node_to_comp[fvar[0]]].clear();
	removed_factors.insert(fac_index);

	// Relabel tree rooted in factor-adjacent node to a new component
	// FIXME: this seems to be very slow
	for (unsigned int fvi = 0; fvi < fvar.size(); ++fvi) {
		unsigned int vi = fvar[fvi];	// var and component index
		std::unordered_set<unsigned int>& comp_vi = comps[vi];
		assert(comp_vi.empty());

		// store (var_idx, came_from_factor_index) in queue
		std::queue<std::pair<unsigned int, unsigned int> > var_q;
		var_q.push(std::pair<unsigned int, unsigned int>(vi, fac_index));

		// Recurse on tree-structured partial component, relabeling in the
		// process
		while (var_q.empty() == false) {
			const std::pair<unsigned int, unsigned int>& cur = var_q.front();

			// Relabel this node
			comp_vi.insert(cur.first);
			node_to_comp[cur.first] = vi;

			// Insert adjacent nodes
			const std::set<unsigned int>& cur_facset =
				fgu.AdjacentFactors(cur.first);
			for (std::set<unsigned int>::const_iterator
				fi = cur_facset.begin(); fi != cur_facset.end(); ++fi) {
				if (*fi == cur.second)
					continue;	// this is the factor we came from

				// If factor is removed, skip
				if (removed_factors.count(*fi) > 0)
					continue;

				// Factor is valid, add all its variables except this one
				const Factor* cur_factor = fg->Factors()[*fi];
				const std::vector<unsigned int>& cur_facvar =
					cur_factor->Variables();
				for (std::vector<unsigned int>::const_iterator
					cfvi = cur_facvar.begin(); cfvi != cur_facvar.end();
					++cfvi) {
					if (*cfvi == cur.first)
						continue;	// do not add ourselves again

					// Put variable into the queue
					var_q.push(std::pair<unsigned int, unsigned int>(
						*cfvi, *fi));
				}
			}
			var_q.pop();
		}
	}
}

void VAcyclicDecomposition::MergeComponents(
	std::vector<unsigned int>& node_to_comp,
	std::vector<std::unordered_set<unsigned int> >& comps,
	const Factor* fac) const {
	// Obtain component indices (these are all be disjoint)
	const std::vector<unsigned int>& fvar = fac->Variables();
	std::vector<unsigned int> comp_indices(fvar.size());
	for (unsigned int fvi = 0; fvi < fvar.size(); ++fvi)
		comp_indices[fvi] = node_to_comp[fvar[fvi]];

	// Map them all to the first component
	unsigned int target = comp_indices[0];
	for (unsigned int ci = 1; ci < comp_indices.size(); ++ci) {
		// 1. relabel all nodes to the new component
		for (std::unordered_set<unsigned int>::const_iterator
			si = comps[comp_indices[ci]].begin();
			si != comps[comp_indices[ci]].end(); ++si) {
			node_to_comp[*si] = target;
		}
		// 2. merge component sets
		comps[target].insert(comps[comp_indices[ci]].begin(),
			comps[comp_indices[ci]].end());
		comps[comp_indices[ci]].clear();
	}
}

double VAcyclicDecomposition::ComputeDecompositionExact(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed, double opt_eps) {
	// Call reverse search enumeration function
	ReverseSearch rsearch(this, factor_weights, opt_eps);
	double obj = rsearch.Search(factor_is_removed);

	return (obj);
}

VAcyclicDecomposition::ReverseSearch::ReverseSearch(VAcyclicDecomposition* vac,
	const std::vector<double>& factor_weights, double opt_eps)
	: factor_count(vac->fg->Factors().size()), factor_weights(factor_weights),
		vac(vac), best_global(0.0),
		best_factor_is_removed(vac->fg->Factors().size(), true),
		opt_eps(opt_eps) {
}

double VAcyclicDecomposition::ReverseSearch::Search(
	std::vector<bool>& factor_is_removed_out) {
	std::list<unsigned int> factor_in;	// start with empty set
	std::list<unsigned int> factor_cand;
	std::set<unsigned int> factor_out;
	for (unsigned int fi = 0; fi < factor_count; ++fi) {
		factor_cand.push_back(fi);
		factor_out.insert(fi);
	}

	examined = 0;
	std::cout << "SEARCH BEGIN" << std::endl;
	DisjointSetBT dset(factor_count);
	Recurse(0.0, dset, factor_in, factor_cand, factor_out);
	std::cout << "SEARCH END" << std::endl << std::endl;

	factor_is_removed_out = best_factor_is_removed;

	return (best_global);
}

// This has polynomial delay at O(F log F + F N(F)), where the output
// operation is a an enumeration of a vac structure.  So in effect if the
// maximum factor scope size N(F) is small, the runtime is an almost linear
// function in the number of factors of the graph.
std::set<unsigned int>::const_iterator
VAcyclicDecomposition::ReverseSearch::Recurse(double obj, DisjointSetBT& dset,
	std::list<unsigned int>& factor_in, std::list<unsigned int>& factor_cand,
	std::set<unsigned int>& factor_out)
{
	examined += 1;
	if (examined % 1000000 == 1) {
		std::cout << "Recurse(obj=" << obj << ", factor_in (" << factor_in.size()
			<< "), factor_cand (" << factor_cand.size() << "), "
			<< "factor_out (" << factor_out.size() << "))"
			<< " best: " << best_global << std::endl;
	}

	// 1. Determine whether this tree is still vac.
	// When we find that it is not vac, return.
	//
	// Complexity: O(F log log F)

	// We need to check all factors not in factor_in
	for (std::set<unsigned int>::const_iterator foi = factor_out.begin();
		foi != factor_out.end(); ++foi) {
		const Factor* fac = vac->fg->Factors()[*foi];
		const std::vector<unsigned int>& fvars = fac->Variables();

		for (unsigned int fvi1 = 0; fvi1 < fvars.size(); ++fvi1) {
			unsigned int root1 = dset.Find(fvars[fvi1]);
			for (unsigned int fvi2 = fvi1 + 1; fvi2 < fvars.size(); ++fvi2) {
				unsigned int root2 = dset.Find(fvars[fvi2]);
#if 0
				std::cout << "   fi " << *foi << ", " << fvars[fvi1]
					<< "--" << fvars[fvi2] << " map to roots "
					<< root1 << "--" << root2 << std::endl;
#endif

				if (root1 == root2) {
#if 0
					std::cout << "   * not vac, returning" << std::endl;
#endif
					return (foi);
				}
			}
		}
	}

	// 2. Current set is vac, so update objective
	//
	// Complexity: O(1)
	if (obj > best_global) {
		best_global = obj;
		best_factor_is_removed.resize(factor_count);
		std::fill(best_factor_is_removed.begin(),
			best_factor_is_removed.end(), true);
		for (std::list<unsigned int>::const_iterator fil = factor_in.begin();
			fil != factor_in.end(); ++fil) {
			best_factor_is_removed[*fil] = false;
		}
		std::cout << "   found new best vac with obj " << obj << std::endl;
	}

	// 3. Compute bound and if it turns out that we cannot beat the best
	// solution so far, stop recursion
	//
	// Complexity: O(F)
	double obj_upper_bound = obj;
	for (std::list<unsigned int>::const_iterator fci = factor_cand.begin();
		fci != factor_cand.end(); ++fci) {
		obj_upper_bound += std::max(0.0, factor_weights[*fci]);
	}
#if 0
	if (examined % 10000 == 1) {
		std::cout << "   upper bound at this node: " << obj_upper_bound << std::endl;
	}
#endif

	if (obj_upper_bound <= best_global + opt_eps)
		return (factor_out.end());	// cannot beat best global one

	// 4. For all remaining factors, add and recurse
	//
	// Complexity: O(F N(F) + F log F) + recursion
	//  i) Sort by weight
	factor_cand.sort(
		[this](unsigned int v1, unsigned int v2) -> bool {
			return (this->factor_weights[v1] > this->factor_weights[v2]);
		});

	// ii) Recurse for each, greedily
	std::list<unsigned int>::iterator fii = factor_cand.begin();
	while (fii != factor_cand.end()) {
		// Skip factors that would not contribute anything to the objective
		unsigned int fi = *fii;
		if (factor_weights[fi] <= 0.0) {
			++fii;
			continue;
		}

		const Factor* fac = vac->fg->Factors()[fi];
		unsigned int union_count = AddFactor(dset, fac);

		// Prepare recursion
		factor_in.push_back(fi);	// more factor into retained factor set
		factor_out.erase(fi);

		// Note that here we change the list factor_cand, so that we realize
		// the reverse search ordering and avoid enumerating duplicates
		fii = factor_cand.erase(fii);	// erase factor from candidate list
		std::list<unsigned int> remaining_cand(fii, factor_cand.end());

		// Recurse
		std::set<unsigned int>::iterator conflict_iter =
			Recurse(obj + factor_weights[fi], dset, factor_in,
				remaining_cand, factor_out);

		// Handling discovered vac conflicts in the remaining candidate set
		if (conflict_iter != factor_out.end()) {
			unsigned int conflict_fi = *conflict_iter;

#if 0
			if (std::find(factor_cand.begin(), factor_cand.end(), conflict_fi)
				!= factor_cand.end()) {
				std::cout << "   removed conflict" << std::endl;
			}
#endif
			factor_cand.remove_if([conflict_fi](unsigned int k) -> bool {
				return (conflict_fi == k);
			});
			fii = factor_cand.begin();
		}

		factor_out.insert(fi);
		factor_in.pop_back();

		// Undo
		for (unsigned int uc = 0; uc < union_count; ++uc)
			dset.Deunion();
	}
	return (factor_out.end());
}

unsigned int VAcyclicDecomposition::ReverseSearch::AddFactor(DisjointSetBT& dset,
	const Factor* fac) {
	const std::vector<unsigned int>& fvars = fac->Variables();
	unsigned int union_count = 0;

	// Union all variables in the factor scope N(F)
	unsigned int root = dset.Find(fvars[0]);
	for (unsigned int fvi = 1; fvi < fvars.size(); ++fvi) {
		unsigned int root2 = dset.Find(fvars[fvi]);
		assert(root != root2);
		root = dset.Union(root, root2);
		union_count += 1;
	}

	return (union_count);
}

}


/*
 * Copyright [2019] [Christopher Syben]
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Links the cone-beam projector layer from python to the actual kernel implementation. Implemented according to Tensorflow API.
 * Implementation partially adapted from CONRAD
 * PYRO-NN is developed as an Open Source project under the Apache License, Version 2.0.
*/
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/shape_inference.h"
#include "helper_headers/helper_geometry_cpu.h"
#include "helper_headers/helper_eigen.h"
#include <Eigen/QR>
#include <typeinfo>
using namespace tensorflow; // NOLINT(build/namespaces)
using shape_inference::ShapeHandle;

#define CUDA_OPERATOR_KERNEL "ConeProjection3D"
REGISTER_OP(CUDA_OPERATOR_KERNEL)
    .Input("volume: float")
    .Attr("volume_shape: shape")
    .Attr("projection_shape: shape")
    .Attr("volume_origin : tensor")
    .Attr("volume_spacing : tensor")
    .Attr("projection_matrices : tensor")
    .Attr("hardware_interp : bool = false")
    .Attr("step_size: float = 1.0")
    .Attr("projection_multiplier : float")
    .Output("output: float")
    .SetShapeFn( []( ::tensorflow::shape_inference::InferenceContext* c )
    {
      TensorShapeProto sp;
      ShapeHandle sh;
      ShapeHandle batch;
      ShapeHandle out;
      auto status = c->GetAttr( "projection_shape", &sp );
      status.Update( c->MakeShapeFromShapeProto( sp, &sh ) );
      c->Subshape(c->input(0),0,1,&batch);
      c->Concatenate(batch, sh, &out);
      c->set_output( 0, out );
      return status;
    } )
    .Doc(R"doc(
Computes the 3D cone forward projection of the input based on the given the trajectory

output: A Tensor.
  output = A_cone * x
)doc");

void Cone_Projection_Kernel_Launcher(const float *volume_ptr, float *out, const float *inv_AR_matrix, const float *src_points, const int number_of_projections,
                                    const int volume_width, const int volume_height, const int volume_depth,
                                    const float volume_spacing_x, const float volume_spacing_y, const float volume_spacing_z,
                                    const int detector_width, const int detector_height, const float step_size);

void Cone_Projection_Kernel_Tex_Interp_Launcher(const float *volume_ptr, float *out, const float *inv_AR_matrix, const float *src_points, const int number_of_projections,
                                                const int volume_width, const int volume_height, const int volume_depth,
                                                const float volume_spacing_x, const float volume_spacing_y, const float volume_spacing_z,
                                                const int detector_width, const int detector_height, const float step_size);

class ConeProjection3DOp : public OpKernel
{
    TensorShape volume_shape;
    int volume_width, volume_height, volume_depth;

    TensorShape projection_shape;
    int detector_size_x, detector_size_y, number_of_projections;

    float volume_origin_x, volume_origin_y, volume_origin_z;

    float volume_spacing_x, volume_spacing_y, volume_spacing_z;

    float step_size;
    bool hardware_interp;

    float projection_multiplier;

    Eigen::Tensor<float, 3, Eigen::RowMajor> projection_matrices;
    //TensorShape projection_matrices_shape;

    Eigen::Tensor<float, 3, Eigen::RowMajor> inv_AR_matrix;
    Eigen::Tensor<float, 2, Eigen::RowMajor> src_points;

  public:

    explicit ConeProjection3DOp(OpKernelConstruction *context) : OpKernel(context)
    {
        //get volume shape from attributes
        OP_REQUIRES_OK(context, context->GetAttr("volume_shape", &volume_shape));
        volume_depth = volume_shape.dim_size(0);
        volume_height = volume_shape.dim_size(1);
        volume_width = volume_shape.dim_size(2);
        //get detector shape from attributes
        OP_REQUIRES_OK(context, context->GetAttr("projection_shape", &projection_shape));
        number_of_projections = projection_shape.dim_size(0);
        detector_size_y = projection_shape.dim_size(1);
        detector_size_x = projection_shape.dim_size(2);
        //get volume origin from attributes
        Tensor volume_origin_tensor;
        OP_REQUIRES_OK(context, context->GetAttr("volume_origin", &volume_origin_tensor));
        auto volume_origin_eigen = volume_origin_tensor.tensor<float, 1>();
        volume_origin_z = volume_origin_eigen(0);
        volume_origin_y = volume_origin_eigen(1);
        volume_origin_x = volume_origin_eigen(2);

        //get volume spacing from attributes
        Tensor volume_spacing_tensor;
        OP_REQUIRES_OK(context, context->GetAttr("volume_spacing", &volume_spacing_tensor));
        auto volume_spacing_eigen = volume_spacing_tensor.tensor<float, 1>();
        volume_spacing_z = volume_spacing_eigen(0);
        volume_spacing_y = volume_spacing_eigen(1);
        volume_spacing_x = volume_spacing_eigen(2);

        //get ray vectors from attributes
        Tensor projection_matrices_tensor;
        OP_REQUIRES_OK(context, context->GetAttr("projection_matrices", &projection_matrices_tensor));
        auto projection_matrices_eigen = projection_matrices_tensor.tensor<float, 3>();
        // projection_matrices_shape = projection_matrices_tensor.shape();
        //Init src_point and inv_ar_matrix tensors
        //get stepsize
        OP_REQUIRES_OK(context, context->GetAttr("step_size", &step_size));

        //get hardware interpolation flag
        OP_REQUIRES_OK(context, context->GetAttr("hardware_interp", &hardware_interp));

        //Projectionmultiplier for backprojection as gradient
        OP_REQUIRES_OK(context, context->GetAttr("projection_multiplier", &projection_multiplier));

        src_points = Eigen::Tensor<float, 2, Eigen::RowMajor>(number_of_projections,3);
        inv_AR_matrix = Eigen::Tensor<float, 3, Eigen::RowMajor>(number_of_projections,3,3);

        /*********************************************************************************************************************************************************************
         *
         *  P = [M | -MC]
         *  M = KR
         * 1. Extract Source Position (C) from P using SVD to calc right null space
         * 2. Calculate M^-1 and multiply with a 3x3 Matrix containing 1/voxel_spacing on the diagonal matrix
         * 3. Put src_points and inv_ar_matrix into the CUDA Kernel, like Cone-Projector from Conrad
         *
         * WARNING: The following code is not created under memory and runtime performance point of view.
         *          A better conversion from Tensorflow Tensor to Eigen::Tensor and Eigen::Matrizes are probably neccessary !!!!
         * ********************************************************************************************************************************************************************/

        Eigen::Matrix3f scaling_matrix(3,3);
        scaling_matrix.setZero();
        scaling_matrix(0,0) = 1.0/volume_spacing_x;
        scaling_matrix(1,1) = 1.0/volume_spacing_y;
        scaling_matrix(2,2) = 1.0/volume_spacing_z;
        src_points.setZero();
        inv_AR_matrix.setZero();

        //for each projection
        for (int n = 0; n < number_of_projections; n++)
        {
            Eigen::Matrix<float,3,4,Eigen::RowMajor> proj_mat(3,4);
            proj_mat << projection_matrices_eigen(n,0,0), projection_matrices_eigen(n,0,1), projection_matrices_eigen(n,0,2), projection_matrices_eigen(n,0,3),
                        projection_matrices_eigen(n,1,0), projection_matrices_eigen(n,1,1), projection_matrices_eigen(n,1,2), projection_matrices_eigen(n,1,3),
                        projection_matrices_eigen(n,2,0), projection_matrices_eigen(n,2,1) ,projection_matrices_eigen(n,2,2), projection_matrices_eigen(n,2,3);

            auto c = (Geometry::getCameraCenter(proj_mat) * -1).eval();

            src_points(n,0) = -((volume_origin_x * scaling_matrix(0,0)) + c(0) * scaling_matrix(0,0));
            src_points(n,1) = -((volume_origin_y * scaling_matrix(1,1)) + c(1) * scaling_matrix(1,1));
            src_points(n,2) = -((volume_origin_z * scaling_matrix(2,2)) + c(2) * scaling_matrix(2,2));

            Eigen::Matrix<float,3,3, Eigen::RowMajor> inverted_scaled_result = (scaling_matrix * proj_mat.block<3,3>(0,0).inverse()).eval();

            //TODO: dont copy element-wise use Eigen::Map to map eigen::matrix to eigen::tensor
            for(int j = 0; j < inverted_scaled_result.cols();++j){
                for(int i = 0; i < inverted_scaled_result.rows(); ++i){
                    inv_AR_matrix(n,j,i) = inverted_scaled_result(j,i);
                }
            }
        }
    }

    /*
    https://github.com/tensorflow/tensorflow/issues/5902
    tensorflow::GPUBFCAllocator* allocator = new tensorflow::GPUBFCAllocator(0, sizeof(float) * height * width * 3);
    tensorflow::Tensor input_tensor = tensorflow::Tensor(allocator, tensorflow::DataType::DT_FLOAT, tensorflow::TensorShape( { 1, height, width, 3 }));
    <copy output data from program A into the GPU memory allocated by input_tensor using a GPU->GPU copy>

    https://stackoverflow.com/questions/39797095/tensorflow-custom-allocator-and-accessing-data-from-tensor

    */

    void Compute(OpKernelContext *context) override
    {
        // Grab the input tensor
        const Tensor &input_tensor = context->input(0);
        auto input = input_tensor.flat<float>();
        // Create an output tensor
        TensorShape out_shape = TensorShape(
          {input_tensor.shape().dim_size(0), projection_shape.dim_size(0), projection_shape.dim_size(1), projection_shape.dim_size(2)});
        Tensor *output_tensor = nullptr;

        // Check Batch size. Batch > 1 is not supported currently.
        OP_REQUIRES(context, input_tensor.shape().dim_size(0) == 1,
                errors::InvalidArgument("Batch dimension is mandatory ! Batch size > 1 is not supported in the current PYRO-NN-layers."));

        OP_REQUIRES_OK(context, context->allocate_output(0, out_shape,
                                                         &output_tensor));

        auto output = output_tensor->template flat<float>();

        if(hardware_interp){
            Cone_Projection_Kernel_Tex_Interp_Launcher(input.data(), output.data(), inv_AR_matrix.data(), src_points.data(), number_of_projections,
                                        volume_width, volume_height, volume_depth, volume_spacing_x, volume_spacing_y, volume_spacing_z,
                                        detector_size_x, detector_size_y, step_size);
        }
        else{
            //TODO:
            // allocate inv_ar_matrix, src_points with tensorflow context as temp memory.
            // Call the cuda kernel launcher
            Cone_Projection_Kernel_Launcher(input.data(), output.data(), inv_AR_matrix.data(), src_points.data(), number_of_projections,
                                        volume_width, volume_height, volume_depth, volume_spacing_x, volume_spacing_y, volume_spacing_z,
                                        detector_size_x, detector_size_y,step_size);
        }
    }
};

REGISTER_KERNEL_BUILDER(Name(CUDA_OPERATOR_KERNEL).Device(DEVICE_GPU), ConeProjection3DOp);


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_unary_edge.h"
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include<pybind11/pybind11.h>
#include<pybind11/numpy.h>

namespace py = pybind11;

using namespace std;
using namespace cv;
using namespace g2o;

class EdgeProjection : public g2o::BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

  EdgeProjection(const Eigen::Matrix<double, 4, 4> &K,const Eigen::Matrix<double, 4, 9> &Cor,const int index ) : _K(K),_Cor(Cor),_index(index) {}

  virtual void computeError() override {
    // P3d----vertex0,T----vertex1
    const VertexSBAPointXYZ *v0 = static_cast<const VertexSBAPointXYZ *> (_vertices[0]);
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
    Eigen::Vector3d Di(v0->estimate());
    Eigen::Vector4d Di_linear ;
    Di_linear << Di(0),Di(1),Di(2),1.0;
    Eigen::Matrix4d diagDi(Di_linear.asDiagonal());
    // Eigen::Matrix<double, 4, 4> T;
    g2o::SE3Quat T_quat=v1->estimate();
    g2o::Matrix4D T= T_quat.to_homogeneous_matrix();
    Eigen::Matrix<double, 4, 9> pos_pixel = _K  * T *diagDi * _Cor;
    Eigen::Matrix<double, 2, 1> optimize;
    pos_pixel.row(0) = pos_pixel.row(0).cwiseQuotient(pos_pixel.row(2));
    pos_pixel.row(1) = pos_pixel.row(1).cwiseQuotient(pos_pixel.row(2));
    // cout << "_measurement"<< endl<<_measurement<< endl;
    optimize << pos_pixel(0,_index),pos_pixel(1,_index);
    // cout << "optimize"<< endl<<optimize<< endl;
    Vector2D obs(_measurement);
    _error = obs- optimize;
    // cout << "_error"<< endl<<_error<< endl;
  }
  // virtual void linearizeOplus(){};
  virtual void linearizeOplus() override  {
    VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
    SE3Quat T(vj->estimate());
    // vi是维度向量Di
    VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
    Eigen::Vector3d Di = vi->estimate();
    // 把维度转化成车辆的角点
    Eigen::Vector3d xyz (Di(0,0)*_Cor(0,_index),Di(1,0)*_Cor(1,_index),Di(2,0)*_Cor(2,_index));
    Eigen::Vector3d xyz_trans = T.map(xyz);
    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double z = xyz_trans[2];
    double z_2 = z*z;

    Eigen::Matrix<double,2,3,Eigen::ColMajor> tmp;
    tmp(0,0) = _K(0,0);
    tmp(0,1) = 0;
    tmp(0,2) = -x/z*_K(0,0);

    tmp(1,0) = 0;
    tmp(1,1) = _K(0,0);
    tmp(1,2) = -y/z*_K(0,0);
    // e_cp对维度求导
    for(int vertex=0;vertex<9;vertex++){
      Eigen::Vector3d corner_col((_Cor.col(vertex)).topRows(3));
      Eigen::Matrix3d diagCor_col(corner_col.asDiagonal());
      _jacobianOplusXi +=  -1./z * tmp * T.rotation().toRotationMatrix() * diagCor_col;
    }
    _jacobianOplusXi = _jacobianOplusXi / 9;
    // e_cp对位姿李代数求导
    _jacobianOplusXj(0,0) =  x*y/z_2 *_K(0,0);
    _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *_K(0,0);
    _jacobianOplusXj(0,2) = y/z *_K(0,0);
    _jacobianOplusXj(0,3) = -1./z *_K(0,0);
    _jacobianOplusXj(0,4) = 0;
    _jacobianOplusXj(0,5) = x/z_2 *_K(0,0);

    _jacobianOplusXj(1,0) = (1+y*y/z_2) *_K(0,0);
    _jacobianOplusXj(1,1) = -x*y/z_2 *_K(0,0);
    _jacobianOplusXj(1,2) = -x/z *_K(0,0);
    _jacobianOplusXj(1,3) = 0;
    _jacobianOplusXj(1,4) = -1./z *_K(0,0);
    _jacobianOplusXj(1,5) = y/z_2 *_K(0,0);
}
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

private:

  Eigen::Matrix<double, 4, 9> _Cor;
  Eigen::Matrix<double, 4, 4> _K;
  int _index;
};

/**
    input1:[w h l ry3d cx3d cy3d cz]
    input2:[x0,y0,x1,y1,...]
    input3:[f,cx,cy]
*/
py::array_t<double> optimize(py::array_t<double>& input1, py::array_t<double>& input2,py::array_t<double>& input3,
                             py::array_t<double>& score,py::array_t<double>& hm_score) {
    // 获取input的信息
    py::buffer_info buf1 = input1.request();
    py::buffer_info buf2 = input2.request();
    py::buffer_info buf3 = input3.request();
    py::buffer_info buf_score = score.request();
    py::buffer_info buf_hm_score = hm_score.request();
    if (buf1.ndim !=1 || buf2.ndim !=1 || buf3.ndim !=1 || buf_score.ndim !=1 || buf_hm_score.ndim !=1 ){
        throw std::runtime_error("Number of dimensions must be one");
    }
    if (buf1.size !=7){
        throw std::runtime_error("Input 3d bbox must match the format [w h l ry3d cx3d cy3d cz]");
    }
    if (buf2.size !=18){
        throw std::runtime_error("Input keypoints must match the format [x0,y0,x1,y1,...]");
    }
    if (buf3.size !=3){
        throw std::runtime_error("Input camera intrinsics must match the format [f,cx,cy]");
    }
    if (buf_score.size !=1){
        throw std::runtime_error("Input 2D bbox center score");
    }
    if (buf_hm_score.size !=9){
        throw std::runtime_error("Input 9 keypoints score");
    }
    //获取numpy.ndarray 数据指针
    double* ptr1 = (double*)buf1.ptr;
    double* ptr2 = (double*)buf2.ptr;
    double* ptr3 = (double*)buf3.ptr;
    double* ptr_score = (double*)buf_score.ptr;
    double* ptr_hm_score = (double*)buf_hm_score.ptr;
    double w_3d = ptr1[0], h_3d = ptr1[1], l_3d = ptr1[2];
    double ry3d = ptr1[3];
    double cx_3d = ptr1[4], cy_3d = ptr1[5], c_z = ptr1[6];
    Eigen::Matrix<double, 4, 9> Cor;//l,h,w
    Cor<< 1./2, 1./2, -1./2, -1./2, 1./2, 1./2, -1./2, -1./2, 0,
        0,0,0,0,-1,-1,-1,-1,-1./2,
        1./2, -1./2, -1./2, 1./2, 1./2, -1./2, -1./2, 1./2, 0,
        1,1,1,1,1,1,1,1,1;
    cout<<"优化前:"<<endl;
    cout << "角度 = " << ry3d << endl;
    cout<<"3D bbox中心点坐标：\t cx:"<< cx_3d << "\t cy:" <<
    cy_3d<< "\t cz:"  << c_z  <<endl;
    cout<<"3D bbox长宽高lhw : \t L: "<< l_3d<<"\t H: "<<h_3d<<"\t W: "<<w_3d<<endl;
    Eigen::Matrix<double, 18, 1> keypoints_2d;
    keypoints_2d.fill(0);
    for(int i=0;i<18;i++){
        keypoints_2d[i] = ptr2[i];
    }

    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3

    //Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverCSparse<Block::PoseMatrixType>());

    //Block* solver_ptr = new Block ( linearSolver );
    //std::unique_ptr<Block> solver_ptr ( new Block ( linearSolver));
    std::unique_ptr<Block> solver_ptr ( new Block ( std::move(linearSolver)));     // 矩阵块求解器

    //g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::move(solver_ptr));

    g2o::SparseOptimizer optimizer;

    optimizer.setAlgorithm ( solver );

    // pose对应转移矩阵
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    // 用theta对应的旋转矩阵初始化R_mat,c3d初始化平移向量t
    Eigen::Matrix3d R_mat;
    Eigen::Vector3d t_3d;
    // 内参矩阵
    Eigen::Matrix4d K_Mat ;
    K_Mat << ptr3[0], 0,         ptr3[1],  0,
            0,        ptr3[0],   ptr3[2],  0,
            0,        0,         1.0,      0,
            0,        0,         0,        1.0;
    R_mat<< +cos(ry3d), 0, +sin(ry3d),
            0, 1, 0,
            -sin(ry3d), 0, cos(ry3d);
    t_3d<< cx_3d, cy_3d, c_z;
    // 把转移矩阵添加至图顶点
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            t_3d
                        ));
    optimizer.addVertex ( pose );

    // 把Di当成第二个顶点
    g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
    point->setId ( 1 );
    point->setEstimate ( Eigen::Vector3d (l_3d,h_3d,w_3d));
    point->setMarginalized ( true );
    optimizer.addVertex ( point );

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        ptr3[0], Eigen::Vector2d (ptr3[1], ptr3[2] ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    int index = 1;
    for(int i=0;i<9;i++){
        EdgeProjection *edge = new EdgeProjection(K_Mat,Cor,i);
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( 1 ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d (keypoints_2d(i),keypoints_2d(i+9)));
        edge->setParameterId ( 0,0 );
        Eigen::Vector2d confidence(ptr_hm_score[i],ptr_score[0]);
        Eigen::Matrix2d conviance_matrix(confidence.asDiagonal());
        edge->setInformation ( conviance_matrix );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"优化过程耗时: "<<time_used.count() <<" 秒."<<endl;
    cout<<"优化后:"<<endl;
    Eigen::Isometry3d T( pose->estimate() );
    Eigen::Vector3d euler_angles =pose->estimate().rotation().toRotationMatrix().eulerAngles( 0, 1, 2 );
    cout << "角度 = " << euler_angles(1) << endl;
    cout<<"3D bbox中心点坐标：\t cx:"<< T.matrix()(0,3) << "\t cy:" <<
    T.matrix()(1,3)<< "\t cz:"  <<T.matrix()(2,3)  <<endl;
    cout<<"3D bbox长宽高lhw: \t L: "<< point->estimate()(0)<<"\t H: "<<point->estimate()(1)<<"\tW: "<<point->estimate()(2)<<endl;

    auto result = py::array_t<double>(7);
    py::buffer_info buf4 = result.request();
    double* ptr4 = (double*)buf4.ptr;
    ptr4[0] = point->estimate()(2);//w
    ptr4[1] = point->estimate()(1);//h
    ptr4[2] = point->estimate()(0);//l
    ptr4[3] = euler_angles(1);     //ry3d
    ptr4[4] = T.matrix()(0,3);     //cx
    ptr4[5] = T.matrix()(1,3);     //cy
    ptr4[6] = T.matrix()(2,3);     //cz

    return result;
}

PYBIND11_MODULE(energy, m) {

    m.doc() = "optimize using numpy and g2o!";

    m.def("optimize", &optimize);
}