text:
/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef KAGOME_CORE_CONSENSUS_GRANDPA_VOTINGROUNDIMPL
#define KAGOME_CORE_CONSENSUS_GRANDPA_VOTINGROUNDIMPL

#include "consensus/grandpa/voting_round.hpp"

#include <boost/asio/basic_waitable_timer.hpp>
#include <boost/signals2.hpp>

#include "common/logger.hpp"
#include "consensus/grandpa/environment.hpp"
#include "consensus/grandpa/grandpa_config.hpp"
#include "consensus/grandpa/vote_crypto_provider.hpp"
#include "consensus/grandpa/vote_graph.hpp"
#include "consensus/grandpa/vote_tracker.hpp"

namespace kagome::consensus::grandpa {

  class Grandpa;

  class VotingRoundImpl : public VotingRound {
    using OnCompleted = boost::signals2::signal<void(const CompletedRound &)>;
    using OnCompletedSlotType = OnCompleted::slot_type;

   private:
    VotingRoundImpl(const std::shared_ptr<Grandpa> &grandpa,
                    const GrandpaConfig &config,
                    std::shared_ptr<Environment> env,
                    std::shared_ptr<VoteCryptoProvider> vote_crypto_provider,
                    std::shared_ptr<VoteTracker> prevotes,
                    std::shared_ptr<VoteTracker> precommits,
                    std::shared_ptr<VoteGraph> graph,
                    std::shared_ptr<Clock> clock,
                    std::shared_ptr<boost::asio::io_context> io_context);

   public:
    ~VotingRoundImpl() override = default;

    VotingRoundImpl(
        const std::shared_ptr<Grandpa> &grandpa,
        const GrandpaConfig &config,
        const std::shared_ptr<Environment> &env,
        const std::shared_ptr<VoteCryptoProvider> &vote_crypto_provider,
        const std::shared_ptr<VoteTracker> &prevotes,
        const std::shared_ptr<VoteTracker> &precommits,
        const std::shared_ptr<VoteGraph> &graph,
        const std::shared_ptr<Clock> &clock,
        const std::shared_ptr<boost::asio::io_context> &io_context,
        std::shared_ptr<const RoundState> previous_round_state);

    VotingRoundImpl(
        const std::shared_ptr<Grandpa> &grandpa,
        const GrandpaConfig &config,
        const std::shared_ptr<Environment> &env,
        const std::shared_ptr<VoteCryptoProvider> &vote_crypto_provider,
        const std::shared_ptr<VoteTracker> &prevotes,
        const std::shared_ptr<VoteTracker> &precommits,
        const std::shared_ptr<VoteGraph> &graph,
        const std::shared_ptr<Clock> &clock,
        const std::shared_ptr<boost::asio::io_context> &io_context,
        const std::shared_ptr<const VotingRound> &previous_round);

    enum class Stage {
      // Initial stage, round is just created
      INIT,

      // Beginner stage, round is just start to play
      START,

      // Stages for prevote mechanism
      START_PREVOTE,
      PREVOTE_RUNS,
      END_PREVOTE,

      // Stages for precommit mechanism
      START_PRECOMMIT,
      PRECOMMIT_RUNS,
      END_PRECOMMIT,

      // Stages for waiting finalisation
      START_WAITING,
      WAITING_RUNS,
      END_WAITING,

      // Final state. Round was finalized
      COMPLETED
    };

    // Start/stop round

    void play() override;
    void end() override;

    // Workflow of round

    void startPrevoteStage();
    void endPrevoteStage();
    void startPrecommitStage();
    void endPrecommitStage();
    void startWaitingStage();
    void endWaitingStage();

    // Actions of round

    /**
     * During the primary propose we:
     * 1. Check if we are the primary for the current round. If not execution of
     * the method is finished
     * 2. We can send primary propose only if the estimate from last round state
     * is greater than finalized. If we cannot send propose, method is finished
     * 3. Primary propose is the last rounds estimate.
     * 4. After all steps above are done we broadcast propose
     * 5. We store what we have broadcasted in primary_vote_ field
     */
    void doProposal() override;

    /**
     * 1. Waits until start_time_ + duration * 2 or round is completable
     * 2. Constructs prevote (\see constructPrevote) and broadcasts it
     */
    void doPrevote() override;

    /**
     * 1. Waits until start_time_ + duration * 4 or round is completable
     * 2. Constructs precommit (\see constructPrecommit) and broadcasts it
     */
    void doPrecommit() override;

    // Handlers of incoming messages

    /**
     * Triggered when we receive finalization message
     */
    void onFinalize(const Fin &f) override;

    /**
     * Invoked when we received a primary propose for the current round
     * Basically method just checks if received propose was produced by the
     * primary and if so, it is stored in primary_vote_ field
     */
    void onPrimaryPropose(const SignedMessage &primary_propose) override;

    /**
     * Triggered when we receive prevote for current round
     * \param prevote is stored in prevote tracker and vote graph
     * Then we try to update prevote ghost (\see updatePrevoteGhost) and round
     * state (\see update)
     */
    void onPrevote(const SignedMessage &prevote) override;

    /**
     * Triggered when we receive precommit for the current round
     * \param precommit is stored in precommit tracker and vote graph
     * Then we try to update round state and finalize
     */
    void onPrecommit(const SignedMessage &precommit) override;

    /**
     * Checks if current round is completable and finalized block differs from
     * the last round's finalized block. If so fin message is broadcasted to the
     * network
     * @return true if fin message was sent, false otherwise
     */
    bool tryFinalize() override;

    RoundNumber roundNumber() const override;

    std::shared_ptr<const RoundState> state() const override;

    /**
     * Round is completable when we have block (stored in
     * current_state_.finalized) for which we have supermajority on both
     * prevotes and precommits
     */
    bool completable() const;

   private:
    void constructCurrentState();

    /// Check if peer \param id is primary
    bool isPrimary(const Id &id) const;

    /// Check if current peer is primary
    bool isPrimary() const;

    /// Calculate threshold from the total weights of voters
    size_t getThreshold(const std::shared_ptr<VoterSet> &voters);

    /// Triggered when we receive \param signed_prevote for the current peer
    void onSignedPrevote(const SignedMessage &signed_prevote);

    /// Triggered when we receive \param signed_precommit for the current peer
    bool onSignedPrecommit(const SignedMessage &signed_precommit);

    /**
     * Invoked during each onSingedPrevote.
     * Updates current round's prevote ghost. New prevote-ghost is the highest
     * block with supermajority of prevotes
     */
    void updatePrevoteGhost();

    /// Update current state of the round. In particular we update:
    /// 1. If round is completable
    /// 2. If round has something ready to finalize
    /// 3. New best rounds estimate
    void update();

    // notify about new finalized round. False if new state does not differ from
    // old one
    outcome::result<void> notify(const RoundState &last_round_state);

    /// prepare justification for the provided \param estimate
    boost::optional<GrandpaJustification> finalizingPrecommits(
        const BlockInfo &estimate) const;

    /**
     * We will vote for the best chain containing primary_vote_ iff
     * the last round's prevote-GHOST included that block and
     * that block is a strict descendent of the last round-estimate that we are
     * aware of.
     * Otherwise we will vote for the best chain containing last round estimate
     */
    outcome::result<SignedMessage> constructPrevote(
        const RoundState &last_round_state) const;

    /**
     * Constructs precommit as following:
     * 1. If we have prevote ghost, then vote for it
     * 2. Otherwise vote for the last finalized block (base of the graph)
     */
    outcome::result<SignedMessage> constructPrecommit(
        const RoundState &last_round_state) const;

    /// Check if received \param vote has valid \param justification. If so
    /// \return true, false otherwise
    bool validate(const BlockInfo &vote,
                  const GrandpaJustification &justification) const;

   private:
    std::weak_ptr<Grandpa> grandpa_;

    Stage stage_ = Stage::INIT;
    bool isPrimary_ = false;

    std::shared_ptr<const RoundState> previous_round_state_;
    std::shared_ptr<RoundState> current_round_state_;

    std::function<void()> on_complete_handler_;

    std::shared_ptr<VoterSet> voter_set_;
    const RoundNumber round_number_;
    const Duration duration_;  // length of round (T in spec)
    TimePoint start_time_;

    const Id id_;  // id of current peer

    std::shared_ptr<Environment> env_;

    std::shared_ptr<VoteCryptoProvider> vote_crypto_provider_;
    size_t threshold_;  // supermajority threshold

    std::shared_ptr<VoteTracker> prevotes_;
    std::shared_ptr<VoteTracker> precommits_;
    std::shared_ptr<VoteGraph> graph_;

    std::shared_ptr<Clock> clock_;

    std::shared_ptr<boost::asio::io_context> io_context_;

    Timer timer_;

    common::Logger logger_;
    // equivocators arrays. Index in vector corresponds to the index of voter in
    // voterset, value corresponds to the weight of the voter
    std::vector<bool> prevote_equivocators_;
    std::vector<bool> precommit_equivocators_;

    boost::optional<PrimaryPropose> primary_vote_;
    bool completable_ = false;
  };
}  // namespace kagome::consensus::grandpa

#endif  // KAGOME_CORE_CONSENSUS_GRANDPA_VOTINGROUNDIMPL

max_stars_repo_name: FlorianFranzen/kagome

max_issues_repo_name: FlorianFranzen/kagome

max_forks_repo_name: FlorianFranzen/kagome


text:
#ifndef EFANNA_KDTREE_UB_INDEX_H_
#define EFANNA_KDTREE_UB_INDEX_H_
#include "algorithm/base_index.hpp"
#include <fstream>
#include <ctime>
#include <string.h>
#include <random>
#include <queue>
//#include <bitset>
//using std::bitset;
#include <boost/dynamic_bitset.hpp>

namespace efanna{
struct KDTreeUbIndexParams : public IndexParams
{
	KDTreeUbIndexParams(bool rnn_used, int tree_num_total, int merge_level = 4, int epoches = 4, int check = 25, int myL = 30, int building_use_k = 10, int tree_num_build = 0, int myS = 10)
	{
		reverse_nn_used = rnn_used;
		init_index_type = KDTREE_UB;
		K = building_use_k;
		build_epoches = epoches;
		S = myS;
		ValueType treev;
		treev.int_val = tree_num_total;
		extra_params.insert(std::make_pair("trees",treev));
		ValueType treeb;
		treeb.int_val = tree_num_build > 0 ? tree_num_build : tree_num_total;
		extra_params.insert(std::make_pair("treesb",treeb));
		ValueType merge_levelv;
		merge_levelv.int_val = merge_level;
		extra_params.insert(std::make_pair("ml",merge_levelv));
		L = myL;
		Check_K = check;
	}
};
template <typename DataType>
class KDTreeUbIndex : public InitIndex<DataType>
{
public:

	typedef InitIndex<DataType> BaseClass;
	KDTreeUbIndex(const Matrix<DataType>& dataset, const Distance<DataType>* d, const IndexParams& params = KDTreeUbIndexParams(true,4)) :
		BaseClass(dataset,d,params)
	{
		std::cout<<"kdtree ub initial"<<std::endl;
		ExtraParamsMap::const_iterator it = params_.extra_params.find("trees");
		if(it != params_.extra_params.end()){
			TreeNum = (it->second).int_val;
#ifdef INFO
			std::cout << "Using kdtree to build "<< TreeNum << " trees in total" << std::endl;
#endif
		}
		else{
			TreeNum = 4;
#ifdef INFO
			std::cout << "Using kdtree to build "<< TreeNum << " trees in total" << std::endl;
#endif
		}
		SP.tree_num = TreeNum;

		it = params_.extra_params.find("treesb");
		if(it != params_.extra_params.end()){
			TreeNumBuild = (it->second).int_val;
#ifdef INFO
			std::cout << "Building kdtree graph with "<< TreeNumBuild <<" trees"<< std::endl;
#endif
		}
		else{
			TreeNumBuild = TreeNum;
#ifdef INFO
			std::cout << "Building kdtree graph with "<< TreeNumBuild <<" trees"<< std::endl;
#endif
		}

		it = params_.extra_params.find("ml");
		if(it != params_.extra_params.end()){
			ml = (it->second).int_val;
#ifdef INFO
			std::cout << "Building kdtree initial index with merge level "<< ml  << std::endl;
#endif
		}
		else{
			ml = -1;
#ifdef INFO
			std::cout << "Building kdtree initial index with max merge level "<< std::endl;
#endif
		}
		max_deepth = 0x0fffffff;
		error_flag = false;
	}

	void buildIndexImpl(){
#ifdef INFO
		clock_t s,f;
		s = clock();
#endif
		initGraph();

#ifdef INFO
		f = clock();
#endif

		std::cout << "initial graph finised"<< std::endl;
#ifdef INFO
		std::cout << "initial graph using time: "<< (f-s)*1.0/CLOCKS_PER_SEC<<" seconds"<< std::endl;
#endif

		if(error_flag){
			std::cout << "merge level deeper than tree, max merge deepth is" << max_deepth-1<<std::endl;
			return;
		}
		refineGraph();
	}
	struct Node
	{
		int DivDim;
		DataType DivVal;
		size_t StartIdx, EndIdx;
		unsigned treeid;
		Node* Lchild, * Rchild;

		~Node() {
			if (Lchild!=NULL) Lchild->~Node();
			if (Rchild!=NULL) Rchild->~Node();
		}

	};

	void loadIndex(char* filename){
		read_data(filename);
	}
	void saveIndex(char* filename){

		size_t points_num = features_.get_rows();
		size_t feature_dim = features_.get_cols();
		save_data(filename, params_.K, points_num, feature_dim);
	}
	//algorithms copy and rewrite from flann
	void loadTrees(char* filename){
		std::ifstream in(filename, std::ios::binary|std::ios::in);
		if(!in.is_open()){std::cout<<"open file error"<<std::endl;exit(-10087);}
		unsigned int K,tree_num;
		size_t dim,num;

		//read file head
		in.read((char*)&(K),sizeof(unsigned int));
		in.read((char*)&(tree_num),sizeof(unsigned int));
		in.read((char*)&(num),sizeof(size_t));
		in.read((char*)&(dim),sizeof(size_t));

		SP.tree_num = tree_num;

		//read trees

		tree_roots_.clear();
		for(unsigned int i=0;i<tree_num;i++){// for each tree
			int node_num, node_size;
			in.read((char*)&(node_num),sizeof(int));
			in.read((char*)&(node_size),sizeof(int));

			std::vector<struct Node *> tree_nodes;
			for(int j=0;j<node_num;j++){
				struct Node *tmp = new struct Node();
				in.read((char*)&(tmp->DivDim),sizeof(tmp->DivDim));
				in.read((char*)&(tmp->DivVal),sizeof(tmp->DivVal));
				in.read((char*)&(tmp->StartIdx),sizeof(tmp->StartIdx));
				in.read((char*)&(tmp->EndIdx),sizeof(tmp->EndIdx));
				in.read((char*)&(tmp->Lchild),sizeof(tmp->Lchild));
				in.read((char*)&(tmp->Rchild),sizeof(tmp->Rchild));
				tmp->Lchild = NULL;
				tmp->Rchild = NULL;
				tmp->treeid = i;
				tree_nodes.push_back(tmp);

			}
			//std::cout<<"build "<<i<<std::endl;
			struct Node *root = DepthFirstBuildTree(tree_nodes);
			if(root==NULL){ exit(-11); }
			tree_roots_.push_back(root);
		}

		//read index range
		LeafLists.clear();
		for(unsigned int i=0;i<tree_num;i++){

			std::vector<unsigned> leaves;
			for(unsigned int j=0;j<num; j++){
				unsigned leaf;
				in.read((char*)&(leaf),sizeof(int));
				leaves.push_back(leaf);
			}
			LeafLists.push_back(leaves);
		}
		in.close();
	}
	void saveTrees(char* filename){
		unsigned int K = params_.K;
		size_t num = features_.get_rows();
		size_t dim = features_.get_cols();
		std::fstream out(filename, std::ios::binary|std::ios::out);
		if(!out.is_open()){std::cout<<"open file error"<<std::endl;exit(-10086);}
		unsigned int tree_num = tree_roots_.size();

		//write file head
		out.write((char *)&(K), sizeof(unsigned int));
		out.write((char *)&(tree_num), sizeof(unsigned int));
		out.write((char *)&(num), sizeof(size_t)); //feature point number
		out.write((char *)&(dim), sizeof(size_t)); //feature dim

		//write trees
		typename std::vector<Node *>::iterator it;//int cnt=0;
		for(it=tree_roots_.begin(); it!=tree_roots_.end(); it++){
			//write tree nodes with depth first trace

			size_t offset_node_num = out.tellp();

			out.seekp(sizeof(int),std::ios::cur);

			unsigned int node_size = sizeof(struct Node);
			out.write((char *)&(node_size), sizeof(int));

			unsigned int node_num = DepthFirstWrite(out, *it);

			out.seekg(offset_node_num,std::ios::beg);

			out.write((char *)&(node_num), sizeof(int));

			out.seekp(0,std::ios::end);
			//std::cout<<"tree: "<<cnt++<<" written, node: "<<node_num<<" at offset " << offset_node_num <<std::endl;
		}

		if(LeafLists.size()!=tree_num){ std::cout << "leaf_size!=tree_num" << std::endl; exit(-6); }

		for(unsigned int i=0; i<tree_num; i++){
			for(unsigned int j=0;j<num;j++){
				out.write((char *)&(LeafLists[i][j]), sizeof(int));
			}
		}
		out.close();
	}
	void loadGraph(char* filename){
		std::ifstream in(filename,std::ios::binary);
		unsigned N;

		in.seekg(0,std::ios::end);
		std::ios::pos_type ss = in.tellg();
		size_t fsize = (size_t)ss;
		int dim;
		in.seekg(0,std::ios::beg);
		in.read((char*)&dim, sizeof(int));
		N = fsize / (dim+1) / 4;

		in.seekg(0,std::ios::beg);

		gs.resize(N);
		//M.resize(N);
		//norms.resize(N);
		for(unsigned i=0; i < N; i++){
			unsigned k;
			//DataType norm;
			in.read((char*)&k, sizeof(unsigned));
			//in.read((char*)&m, sizeof(unsigned));
			//in.read((char*)&norm, sizeof(DataType));
			//norms[i] = norm;
			//M[i] = m;
			gs[i].resize(k);

			for(unsigned j=0; j<k; j++){
				unsigned id;
				in.read((char*)&id, sizeof(unsigned));
				gs[i][j] = id;
			}
		}
		in.close();
	}
	/*
    void saveGraph(char* filename){
     std::ofstream out(filename,std::ios::binary);

     int dim = params_.K;//int meansize = 0;
     for(size_t i = 0; i < knn_graph.size(); i++){
       typename CandidateHeap::reverse_iterator it = knn_graph[i].rbegin();
       out.write((char*)&dim, sizeof(int));//meansize += knn_graph[i].size();
       for(size_t j =0; j < params_.K && it!= knn_graph[i].rend(); j++,it++ ){
         int id = it->row_id;
         out.write((char*)&id, sizeof(int));
       }
     }//meansize /= knn_graph.size();
     //std::cout << "size mean " << meansize << std::endl;
     out.close();
    }
	 */
	void saveGraph(char* filename){
		std::ofstream out(filename,std::ios::binary);
		unsigned N = gs.size();
		//out.write((char*)&N, sizeof(int));
		for(unsigned i=0; i < N; i++){
			unsigned k = gs[i].size();
			//unsigned m = M[i];
			//DataType norm = norms[i];
			out.write((char*)&k, sizeof(unsigned));
			//out.write((char*)&m, sizeof(unsigned));
			//out.write((char*)&norm, sizeof(DataType));
			for(unsigned j = 0; j < k; j++){
				unsigned id = gs[i][j];
				out.write((char*)&id, sizeof(unsigned));
			}
		}
		out.close();
	}
	//for nn search

	void SearchQueryToLeaf(Node* node, const DataType* q, unsigned dep, std::vector<Node*>& node_pool){
		if(node->Lchild != NULL && node->Rchild !=NULL){
			if(q[node->DivDim] < node->DivVal){
				SearchQueryToLeaf(node->Lchild, q, dep, node_pool);
				if(node_pool.size() < dep)
					SearchQueryToLeaf(node->Rchild, q, dep, node_pool);
			}
			else{
				SearchQueryToLeaf(node->Rchild, q, dep, node_pool);
				if(node_pool.size() < dep)
					SearchQueryToLeaf(node->Lchild, q, dep, node_pool);
			}
		}
		else
			node_pool.push_back(node);
	}

	void getSearchNodeList(Node* node, const DataType* q, unsigned int lsize, std::vector<Node*>& vn){
		if(vn.size() >= lsize)
			return;

		if(node->Lchild != NULL && node->Rchild !=NULL){
			if(q[node->DivDim] < node->DivVal){
				getSearchNodeList(node->Lchild, q, lsize,  vn );
				getSearchNodeList(node->Rchild, q, lsize, vn);
			}else{
				getSearchNodeList(node->Rchild, q, lsize, vn);
				getSearchNodeList(node->Lchild, q, lsize, vn);
			}
		}else
			vn.push_back(node);
	}

	void getNeighbors(size_t searchK, const Matrix<DataType>& query){
		switch(SP.search_method){
		case 0:
			getNeighbors_nnexp(searchK, query);
			break;
		case 1:
			getNeighbors_kgraph(searchK, query);
			break;
		default:
			std::cout<<"no such searching method"<<std::endl;
		}

	}

	void getNeighbors_nnexp(size_t K, const Matrix<DataType>& query){
#ifdef INFO
		std::cout<<"using tree num "<< SP.tree_num<<std::endl;
#endif
		if(SP.tree_num > tree_roots_.size()){
			std::cout<<"wrong tree number"<<std::endl;return;
		}

		nn_results.clear();
		nn_results.resize(query.get_rows());
		unsigned dim = features_.get_cols();

		int resultSize = SP.extend_to;
		if (K > (unsigned)SP.extend_to)
			resultSize = K;

#pragma omp parallel for
		for(unsigned int cur = 0; cur < query.get_rows(); cur++){
			boost::dynamic_bitset<> tbflag(features_.get_rows(), false);
			boost::dynamic_bitset<> newflag(features_.get_rows(), true);
			tbflag.reset();
			newflag.set();

			std::vector<std::vector<Node*>> NodeCandi;
			NodeCandi.resize(SP.tree_num);

			const DataType* q_row = query.get_row(cur);
			_mm_prefetch((char *)q_row, _MM_HINT_T0);
			unsigned int lsize = SP.search_init_num*2 / (5*SP.tree_num) + 1;
			for(unsigned int i = 0; i < SP.tree_num; i++){
				getSearchNodeList(tree_roots_[i], q_row, lsize, NodeCandi[i]);
			}
			std::vector<int> pool(SP.search_init_num);
			unsigned int p = 0;
			for(unsigned int ni = 0; ni < lsize; ni++){
				for(unsigned int i = 0; i < NodeCandi.size(); i++){
					Node* leafn = NodeCandi[i][ni];
					for(size_t j = leafn->StartIdx; j < leafn->EndIdx && p < (unsigned int)SP.search_init_num; j++){
						size_t nn = LeafLists[i][j];
						if(tbflag.test(nn))continue;
						tbflag.set(nn);
						pool[p++]=(nn);
					}
					if(p >= (unsigned int)SP.search_init_num) break;
				}
				if(p >= (unsigned int)SP.search_init_num) break;
			}
			int base_n = features_.get_rows();
			while(p < (unsigned int)SP.search_init_num){
				unsigned int nn = rand() % base_n;
				if(tbflag.test(nn))continue;
				tbflag.set(nn);
				pool[p++]=(nn);
			}

			std::vector<std::pair<float,size_t>> result;
			//for(unsigned int i=0; i<pool.size();i++){
			//  _mm_prefetch((char *)features_.get_row(pool[i]), _MM_HINT_T0);
			//}
			unsigned cache_blocksz = 80;
			for(unsigned int i=0; i*cache_blocksz<pool.size();i++){
				unsigned s = i*cache_blocksz;
				unsigned t = s + cache_blocksz > pool.size() ? pool.size() : s+cache_blocksz;
				unsigned s_ = s;
				while(s<t){
					_mm_prefetch((char *)features_.get_row(pool[s]), _MM_HINT_T0);
					s++;
				}
				while(s_<t){
					result.push_back(std::make_pair(distance_->compare(q_row, features_.get_row(pool[s_]), dim),pool[s_]));
					s_++;
				}
			}
			std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
			result.resize(resultSize);
			pool.clear();
			for(int j = 0; j < resultSize; j++)
				pool.push_back(result[j].second);

			int iter=0;
			std::vector<int> ids;
			while(iter++ < SP.search_epoches){
				ids.clear();
				for(unsigned j = 0; j < SP.extend_to ; j++){
					if(newflag.test( pool[j] )){
						newflag.reset(pool[j]);

						for(unsigned neighbor=0; neighbor < gs[pool[j]].size(); neighbor++){
							unsigned id = gs[pool[j]][neighbor];

							if(tbflag.test(id))continue;
							else tbflag.set(id);

							ids.push_back(id);
						}
					}
				}
				//for(unsigned int j=0; j<ids.size();j++){
				//_mm_prefetch((char *)features_.get_row(ids[j]), _MM_HINT_T0);
				//}
				for(size_t j = 0; j * cache_blocksz< ids.size(); j++){
					unsigned s = j * cache_blocksz;
					unsigned t = s + cache_blocksz > ids.size() ? ids.size() : s+cache_blocksz;
					unsigned s_ = s;
					while(s<t){
						_mm_prefetch((char *)features_.get_row(ids[s]), _MM_HINT_T0);
						s++;
					}
					while(s_<t){
						result.push_back(std::make_pair(distance_->compare(q_row, features_.get_row(ids[s_]), dim),ids[s_]));
						s_++;
					}
					//result.push_back(std::make_pair(distance_->compare(q_row, features_.get_row(ids[j]), dim),ids[j]));
				}
				std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
				result.resize(resultSize);
				pool.clear();
				for(int j = 0; j < resultSize; j++)
					pool.push_back(result[j].second);
			}

			if(K<SP.extend_to)
				pool.resize(K);

			//nn_results.push_back(pool);
			std::vector<int>& res = nn_results[cur];
			for(unsigned i = 0; i < K ;i++)
				res.push_back(pool[i]);
		}
	}

	void getNeighbors_kgraph(size_t searchK, const Matrix<DataType>& query){
#ifdef INFO
		std::cout<<"using tree num "<< SP.tree_num<<std::endl;
#endif
		if(SP.tree_num > tree_roots_.size()){
			std::cout<<"wrong tree number"<<std::endl;return;
		}

		nn_results.clear();
		nn_results.resize(query.get_rows());
		unsigned dim = features_.get_cols();
		unsigned int lsize = SP.search_init_num*2 / (5*SP.tree_num) + 1;

		bool bSorted = true;
		unsigned pool_size = SP.search_epoches * SP.extend_to;
		if (pool_size >= (unsigned)SP.search_init_num){
			SP.search_init_num = pool_size;
			bSorted = false;
		}

#pragma omp parallel for
		for(unsigned int cur = 0; cur < query.get_rows(); cur++){
			std::mt19937 rng(1998);
			boost::dynamic_bitset<> flags(features_.get_rows(), false);

			std::vector<std::vector<Node*> > Vnl;
			Vnl.resize(SP.tree_num);
			const DataType* q_row = query.get_row(cur);
			_mm_prefetch((char *)q_row, _MM_HINT_T0);
			for(unsigned int i = 0; i < SP.tree_num; i++){
				getSearchNodeList(tree_roots_[i], q_row, lsize, Vnl[i]);
			}

			std::vector<int> pool(SP.search_init_num);
			unsigned int p = 0;
			for(unsigned int ni = 0; ni < lsize; ni++){
				for(unsigned int i = 0; i < Vnl.size(); i++){
					Node* leafn = Vnl[i][ni];
					for(size_t j = leafn->StartIdx; j < leafn->EndIdx && p < (unsigned int)SP.search_init_num; j++){
						size_t nn = LeafLists[i][j];
						if(flags.test(nn))continue;
						flags.set(nn);
						pool[p++]=(nn);
					}
					if(p >= (unsigned int)SP.search_init_num) break;
				}
				if(p >= (unsigned int)SP.search_init_num) break;
			}
			int base_n = features_.get_rows();
			while(p < (unsigned int)SP.search_init_num){
				unsigned int nn = rand() % base_n;
				if(flags.test(nn))continue;
				flags.set(nn);
				pool[p++]=(nn);
			}

			std::vector<std::pair<float,size_t>> result;
			unsigned cache_blocksz = 80;
			for(unsigned int i=0; i*cache_blocksz<pool.size();i++){
				unsigned s = i*cache_blocksz;
				unsigned t = s + cache_blocksz > pool.size() ? pool.size() : s+cache_blocksz;
				unsigned s_ = s;
				while(s<t){
					_mm_prefetch((char *)features_.get_row(pool[s]), _MM_HINT_T0);
					s++;
				}
				while(s_<t){
					result.push_back(std::make_pair(distance_->compare(q_row, features_.get_row(pool[s_]), dim),pool[s_]));
					s_++;
				}
			}
			if(bSorted){
				std::partial_sort(result.begin(), result.begin() + pool_size, result.end());
				result.resize(pool_size);
			}

			flags.reset();
			std::vector<Point> knn(searchK + SP.extend_to +1);
			std::vector<Point> results;
			for (unsigned iter = 0; iter < (unsigned)SP.search_epoches; ++iter) {

				unsigned L = 0;
				for(unsigned j=0; j < (unsigned)SP.extend_to ; j++){
					if(!flags.test(result[iter*SP.extend_to+j].second)){
						flags.set(result[iter*SP.extend_to+j].second);
						knn[L].id = result[iter*SP.extend_to+j].second;
						knn[L].dist = result[iter*SP.extend_to+j].first;
						knn[L].flag = true;
						L++;
					}
				}
				if(~bSorted){
					std::sort(knn.begin(), knn.begin() + L);
				}

				unsigned k =  0;
				while (k < L) {
					unsigned nk = L;
					if (knn[k].flag) {
						knn[k].flag = false;
						unsigned n = knn[k].id;

						//unsigned maxM = M[n];
						unsigned maxM = SP.extend_to;
						//if ((unsigned)SP.extend_to > maxM) maxM = SP.extend_to;
						auto const &neighbors = gs[n];
						if (maxM > neighbors.size()) {
							maxM = neighbors.size();
						}

						for(unsigned m = 0; m < maxM; ++m){
							_mm_prefetch((char *)features_.get_row(neighbors[m]), _MM_HINT_T0);
						}
						for (unsigned m = 0; m < maxM; ++m) {
							unsigned id = neighbors[m];
							//BOOST_VERIFY(id < graph.size());
							if (flags[id]) continue;
							flags[id] = true;

							DataType dist = distance_->compare(q_row, features_.get_row(id), dim);

							Point nn(id, dist);
							unsigned r = InsertIntoKnn(&knn[0], L, nn);
							//BOOST_VERIFY(r <= L);
							//if (r > L) continue;
							if (L + 1 < knn.size()) ++L;
							if (r < nk) {
								nk = r;
							}
						}
					}
					if (nk <= k) {
						k = nk;
					}
					else {
						++k;
					}
				}
				if (L > searchK) L = searchK;

				if (results.empty()) {
					results.reserve(searchK + 1);
					results.resize(L + 1);
					std::copy(knn.begin(), knn.begin() + L, results.begin());
				} else {
					for (unsigned int l = 0; l < L; ++l) {
						unsigned r = InsertIntoKnn(&results[0], results.size() - 1, knn[l]);
						if (r < results.size()  && results.size() < (searchK + 1)) {
							results.resize(results.size() + 1);
						}
					}
				}
			}

			std::vector<int>& res = nn_results[cur];
			for(size_t i = 0; i < searchK && i < results.size();i++)
				res.push_back(results[i].id);
		}
	}

	int DepthFirstWrite(std::fstream& out, struct Node *root){
		if(root==NULL) return 0;
		int left_cnt = DepthFirstWrite(out, root->Lchild);
		int right_cnt = DepthFirstWrite(out, root->Rchild);

		//std::cout << root->StartIdx <<":" << root->EndIdx<< std::endl;
		out.write((char *)&(root->DivDim), sizeof(root->DivDim));
		out.write((char *)&(root->DivVal), sizeof(root->DivVal));
		out.write((char *)&(root->StartIdx), sizeof(root->StartIdx));
		out.write((char *)&(root->EndIdx), sizeof(root->EndIdx));
		out.write((char *)&(root->Lchild), sizeof(root->Lchild));
		out.write((char *)&(root->Rchild), sizeof(root->Rchild));
		return (left_cnt + right_cnt + 1);
	}
	struct Node* DepthFirstBuildTree(std::vector<struct Node *>& tree_nodes){
		std::vector<Node*> root_serial;
		typename std::vector<struct Node*>::iterator it = tree_nodes.begin();
		for( ; it!=tree_nodes.end(); it++){
			Node* tmp = *it;
			size_t rsize = root_serial.size();
			if(rsize<2){
				root_serial.push_back(tmp);
				//continue;
			}
			else{
				Node *last1 = root_serial[rsize-1];
				Node *last2 = root_serial[rsize-2];
				if(last1->EndIdx == tmp->EndIdx && last2->StartIdx == tmp->StartIdx){
					tmp->Rchild = last1;
					tmp->Lchild = last2;
					root_serial.pop_back();
					root_serial.pop_back();
				}
				root_serial.push_back(tmp);
			}

		}
		if(root_serial.size()!=1){
			std::cout << "Error constructing trees" << std::endl;
			return NULL;
		}
		return root_serial[0];
	}
	void read_data(char *filename){
		std::ifstream in(filename, std::ios::binary|std::ios::in);
		if(!in.is_open()){std::cout<<"open file error"<<std::endl;exit(-10087);}
		unsigned int K,tree_num;
		size_t dim,num;

		//read file head
		in.read((char*)&(K),sizeof(unsigned int));
		in.read((char*)&(tree_num),sizeof(unsigned int));
		in.read((char*)&(num),sizeof(size_t));
		in.read((char*)&(dim),sizeof(size_t));

		SP.tree_num = tree_num;

		//read trees

		tree_roots_.clear();
		for(unsigned int i=0;i<tree_num;i++){// for each tree
			int node_num, node_size;
			in.read((char*)&(node_num),sizeof(int));
			in.read((char*)&(node_size),sizeof(int));

			std::vector<struct Node *> tree_nodes;
			for(int j=0;j<node_num;j++){
				struct Node *tmp = new struct Node();
				in.read((char*)&(tmp->DivDim),sizeof(tmp->DivDim));
				in.read((char*)&(tmp->DivVal),sizeof(tmp->DivVal));
				in.read((char*)&(tmp->StartIdx),sizeof(tmp->StartIdx));
				in.read((char*)&(tmp->EndIdx),sizeof(tmp->EndIdx));
				in.read((char*)&(tmp->Lchild),sizeof(tmp->Lchild));
				in.read((char*)&(tmp->Rchild),sizeof(tmp->Rchild));
				tmp->Lchild = NULL;
				tmp->Rchild = NULL;
				tree_nodes.push_back(tmp);

			}
			//std::cout<<"build "<<i<<std::endl;
			struct Node *root = DepthFirstBuildTree(tree_nodes);
			if(root==NULL){ exit(-11); }
			tree_roots_.push_back(root);
		}

		//read index range
		LeafLists.clear();
		for(unsigned int i=0;i<tree_num;i++){

			std::vector<unsigned> leaves;
			for(unsigned int j=0;j<num; j++){
				unsigned leaf;
				in.read((char*)&(leaf),sizeof(int));
				leaves.push_back(leaf);
			}
			LeafLists.push_back(leaves);
		}

		//read knn graph
		knn_graph.clear();
		for(size_t i = 0; i < num; i++){
			CandidateHeap heap;
			for(size_t j =0; j < K ; j++ ){
				int id;
				in.read((char*)&id, sizeof(int));
				Candidate<DataType> can(id, -1);
				heap.insert(can);
			}
			knn_graph.push_back(heap);
		}
		in.close();
	}
	void save_data(char *filename, unsigned int K, size_t num, size_t dim){
		std::fstream out(filename, std::ios::binary|std::ios::out);
		if(!out.is_open()){std::cout<<"open file error"<<std::endl;exit(-10086);}
		unsigned int tree_num = tree_roots_.size();

		//write file head
		out.write((char *)&(K), sizeof(unsigned int));
		out.write((char *)&(tree_num), sizeof(unsigned int));
		out.write((char *)&(num), sizeof(size_t)); //feature point number
		out.write((char *)&(dim), sizeof(size_t)); //feature dim

		//write trees
		typename std::vector<Node *>::iterator it;//int cnt=0;
		for(it=tree_roots_.begin(); it!=tree_roots_.end(); it++){
			//write tree nodes with depth first trace

			size_t offset_node_num = out.tellp();

			out.seekp(sizeof(int),std::ios::cur);

			unsigned int node_size = sizeof(struct Node);
			out.write((char *)&(node_size), sizeof(int));

			unsigned int node_num = DepthFirstWrite(out, *it);

			out.seekg(offset_node_num,std::ios::beg);

			out.write((char *)&(node_num), sizeof(int));

			out.seekp(0,std::ios::end);
			//std::cout<<"tree: "<<cnt++<<" written, node: "<<node_num<<" at offset " << offset_node_num <<std::endl;
		}

		if(LeafLists.size()!=tree_num){ std::cout << "leaf_size!=tree_num" << std::endl; exit(-6); }

		for(unsigned int i=0; i<tree_num; i++){
			for(unsigned int j=0;j<num;j++){
				out.write((char *)&(LeafLists[i][j]), sizeof(int));
			}
		}

		//write knn-graph

		if(knn_graph.size()!=num){std::cout << "Error:" << std::endl; exit(-1);}
		for(size_t i = 0; i < knn_graph.size(); i++){
			typename CandidateHeap::reverse_iterator it = knn_graph[i].rbegin();
			for(size_t j =0; j < K && it!= knn_graph[i].rend(); j++,it++ ){
				int id = it->row_id;
				out.write((char*)&id, sizeof(int));
			}
		}

		out.close();
	}
	/*
    Node* divideTree(std::mt19937& rng, int* indices, size_t count, size_t offset){
      Node* node = new Node();
      if(count <= params_.TNS){
        node->DivDim = -1;
        node->Lchild = NULL;
        node->Rchild = NULL;
        node->StartIdx = offset;
        node->EndIdx = offset + count;
        //add points

        for(size_t i = 0; i < count; i++){
          for(size_t j = i+1; j < count; j++){
            DataType dist = distance_->compare(
                features_.get_row(indices[i]), features_.get_row(indices[j]), features_.get_cols());

            if(knn_graph[indices[i]].size() < params_.S || dist < knn_graph[indices[i]].begin()->distance){
              Candidate<DataType> c1(indices[j], dist);
              knn_graph[indices[i]].insert(c1);
              if(knn_graph[indices[i]].size() > params_.S)knn_graph[indices[i]].erase(knn_graph[indices[i]].begin());
            }
            else if(nhoods[indices[i]].nn_new.size() < params_.S * 2)nhoods[indices[i]].nn_new.push_back(indices[j]);
            if(knn_graph[indices[j]].size() < params_.S || dist < knn_graph[indices[j]].begin()->distance){
              Candidate<DataType> c2(indices[i], dist);
              knn_graph[indices[j]].insert(c2);
              if(knn_graph[indices[j]].size() > params_.S)knn_graph[indices[j]].erase(knn_graph[indices[j]].begin());
            }
            else if(nhoods[indices[j]].nn_new.size() < params_.S * 2)nhoods[indices[j]].nn_new.push_back(indices[i]);
          }
        }

      }else{
        int idx;
        int cutdim;
        DataType cutval;
        meanSplit(rng, indices, count, idx, cutdim, cutval);

        node->DivDim = cutdim;
        node->DivVal = cutval;
        node->StartIdx = offset;
        node->EndIdx = offset + count;
        node->Lchild = divideTree(rng, indices, idx, offset);
        node->Rchild = divideTree(rng, indices+idx, count-idx, offset+idx);
      }

      return node;
    }

    Node* divideTreeOnly(std::mt19937& rng, unsigned* indices, size_t count, size_t offset){
      Node* node = new Node();
      if(count <= params_.TNS){
        node->DivDim = -1;
        node->Lchild = NULL;
        node->Rchild = NULL;
        node->StartIdx = offset;
        node->EndIdx = offset + count;
        //add points

      }else{
        unsigned idx;
        unsigned cutdim;
        DataType cutval;
        meanSplit(rng, indices, count, idx, cutdim, cutval);

        node->DivDim = cutdim;
        node->DivVal = cutval;
        node->StartIdx = offset;
        node->EndIdx = offset + count;
        node->Lchild = divideTreeOnly(rng, indices, idx, offset);
        node->Rchild = divideTreeOnly(rng, indices+idx, count-idx, offset+idx);
      }

      return node;
    }
	 */

	void meanSplit(std::mt19937& rng, unsigned* indices, unsigned count, unsigned& index, unsigned& cutdim, DataType& cutval){
		size_t veclen_ = features_.get_cols();
		DataType* mean_ = new DataType[veclen_];
		DataType* var_ = new DataType[veclen_];
		memset(mean_,0,veclen_*sizeof(DataType));
		memset(var_,0,veclen_*sizeof(DataType));

		/* Compute mean values.  Only the first SAMPLE_NUM values need to be
          sampled to get a good estimate.
		 */
		unsigned cnt = std::min((unsigned)SAMPLE_NUM+1, count);
		for (unsigned j = 0; j < cnt; ++j) {
			const DataType* v = features_.get_row(indices[j]);
			for (size_t k=0; k<veclen_; ++k) {
				mean_[k] += v[k];
			}
		}
		DataType div_factor = DataType(1)/cnt;
		for (size_t k=0; k<veclen_; ++k) {
			mean_[k] *= div_factor;
		}

		/* Compute variances (no need to divide by count). */

		for (unsigned j = 0; j < cnt; ++j) {
			const DataType* v = features_.get_row(indices[j]);
			for (size_t k=0; k<veclen_; ++k) {
				DataType dist = v[k] - mean_[k];
				var_[k] += dist * dist;
			}
		}

		/* Select one of the highest variance indices at random. */
		cutdim = selectDivision(rng, var_);

		cutval = mean_[cutdim];

		unsigned lim1, lim2;

		planeSplit(indices, count, cutdim, cutval, lim1, lim2);
		//cut the subtree using the id which best balances the tree
		if (lim1>count/2) index = lim1;
		else if (lim2<count/2) index = lim2;
		else index = count/2;

		/* If either list is empty, it means that all remaining features
		 * are identical. Split in the middle to maintain a balanced tree.
		 */
		if ((lim1==count)||(lim2==0)) index = count/2;
		delete[] mean_;
		delete[] var_;
	}
	void planeSplit(unsigned* indices, unsigned count, unsigned cutdim, DataType cutval, unsigned& lim1, unsigned& lim2){
		/* Move vector indices for left subtree to front of list. */
		int left = 0;
		int right = count-1;
		for (;; ) {
			while (left<=right && features_.get_row(indices[left])[cutdim]<cutval) ++left;
			while (left<=right && features_.get_row(indices[right])[cutdim]>=cutval) --right;
			if (left>right) break;
			std::swap(indices[left], indices[right]); ++left; --right;
		}
		lim1 = left;//lim1 is the id of the leftmost point <= cutval
		right = count-1;
		for (;; ) {
			while (left<=right && features_.get_row(indices[left])[cutdim]<=cutval) ++left;
			while (left<=right && features_.get_row(indices[right])[cutdim]>cutval) --right;
			if (left>right) break;
			std::swap(indices[left], indices[right]); ++left; --right;
		}
		lim2 = left;//lim2 is the id of the leftmost point >cutval
	}
	int selectDivision(std::mt19937& rng, DataType* v){
		int num = 0;
		size_t topind[RAND_DIM];

		//Create a list of the indices of the top RAND_DIM values.
		for (size_t i = 0; i < features_.get_cols(); ++i) {
			if ((num < RAND_DIM)||(v[i] > v[topind[num-1]])) {
				// Put this element at end of topind.
				if (num < RAND_DIM) {
					topind[num++] = i;            // Add to list.
				}
				else {
					topind[num-1] = i;         // Replace last element.
				}
				// Bubble end value down to right location by repeated swapping. sort the varience in decrease order
				int j = num - 1;
				while (j > 0  &&  v[topind[j]] > v[topind[j-1]]) {
					std::swap(topind[j], topind[j-1]);
					--j;
				}
			}
		}
		// Select a random integer in range [0,num-1], and return that index.
		int rnd = rng()%num;
		return (int)topind[rnd];
	}
	void getMergeLevelNodeList(Node* node, size_t treeid, int deepth){
		if(node->Lchild != NULL && node->Rchild != NULL && deepth < ml){
			deepth++;
			getMergeLevelNodeList(node->Lchild, treeid, deepth);
			getMergeLevelNodeList(node->Rchild, treeid, deepth);
		}else if(deepth == ml){
			mlNodeList.push_back(std::make_pair(node,treeid));
		}else{
			error_flag = true;
			if(deepth < max_deepth)max_deepth = deepth;
		}
	}
	Node* SearchToLeaf(Node* node, size_t id){
		if(node->Lchild != NULL && node->Rchild !=NULL){
			if(features_.get_row(id)[node->DivDim] < node->DivVal)
				return SearchToLeaf(node->Lchild, id);
			else
				return SearchToLeaf(node->Rchild, id);
		}
		else
			return node;
	}int cc = 0;
	void mergeSubGraphs(size_t treeid, Node* node){
		if(node->Lchild != NULL && node->Rchild != NULL){
			mergeSubGraphs(treeid, node->Lchild);
			mergeSubGraphs(treeid, node->Rchild);

			size_t numL = node->Lchild->EndIdx - node->Lchild->StartIdx;
			size_t numR = node->Rchild->EndIdx - node->Rchild->StartIdx;
			size_t start,end;
			Node * root;
			if(numL < numR){
				root = node->Rchild;
				start = node->Lchild->StartIdx;
				end = node->Lchild->EndIdx;
			}else{
				root = node->Lchild;
				start = node->Rchild->StartIdx;
				end = node->Rchild->EndIdx;
			}

			for(;start < end; start++){

				size_t feature_id = LeafLists[treeid][start];

				Node* leaf = SearchToLeaf(root, feature_id);
				for(size_t i = leaf->StartIdx; i < leaf->EndIdx; i++){
					size_t tmpfea = LeafLists[treeid][i];
					DataType dist = distance_->compare(
							features_.get_row(tmpfea), features_.get_row(feature_id), features_.get_cols());

					{LockGuard g(*nhoods[tmpfea].lock);
					if(knn_graph[tmpfea].size() < params_.S || dist < knn_graph[tmpfea].begin()->distance){
						Candidate<DataType> c1(feature_id, dist);
						knn_graph[tmpfea].insert(c1);
						if(knn_graph[tmpfea].size() > params_.S)knn_graph[tmpfea].erase(knn_graph[tmpfea].begin());

					}
					else if(nhoods[tmpfea].nn_new.size() < params_.S * 2){

						nhoods[tmpfea].nn_new.push_back(feature_id);

					}
					}
					{LockGuard g(*nhoods[feature_id].lock);
					if(knn_graph[feature_id].size() < params_.S || dist < knn_graph[feature_id].begin()->distance){
						Candidate<DataType> c1(tmpfea, dist);
						knn_graph[feature_id].insert(c1);
						if(knn_graph[feature_id].size() > params_.S)knn_graph[feature_id].erase(knn_graph[feature_id].begin());

					}
					else if(nhoods[feature_id].nn_new.size() < params_.S * 2){

						nhoods[feature_id].nn_new.push_back(tmpfea);

					}
					}
				}
			}
		}
	}

	typedef std::set<Candidate<DataType>, std::greater<Candidate<DataType>> > CandidateHeap;

protected:
	enum
	{
		/**
		 * To improve efficiency, only SAMPLE_NUM random values are used to
		 * compute the mean and variance at each level when building a tree.
		 * A value of 100 seems to perform as well as using all values.
		 */
		SAMPLE_NUM = 100,
		/**
		 * Top random dimensions to consider
		 *
		 * When creating random trees, the dimension on which to subdivide is
		 * selected at random from among the top RAND_DIM dimensions with the
		 * highest variance.  A value of 5 works well.
		 */
		RAND_DIM=5
	};

	int TreeNum;
	int TreeNumBuild;
	int ml;   //merge_level
	int max_deepth;
	int veclen_;
	//DataType* var_;
	omp_lock_t rootlock;
	bool error_flag;
	//DataType* mean_;
	std::vector<Node*> tree_roots_;
	std::vector< std::pair<Node*,size_t> > mlNodeList;
	std::vector<std::vector<unsigned>> LeafLists;
	USING_BASECLASS_SYMBOLS

	//kgraph code

	static void GenRandom (std::mt19937& rng, unsigned *addr, unsigned size, unsigned N) {
		for (unsigned i = 0; i < size; ++i) {
			addr[i] = rng() % (N - size);
		}
		std::sort(addr, addr + size);
		for (unsigned i = 1; i < size; ++i) {
			if (addr[i] <= addr[i-1]) {
				addr[i] = addr[i-1] + 1;
			}
		}
		unsigned off = rng() % N;
		for (unsigned i = 0; i < size; ++i) {
			addr[i] = (addr[i] + off) % N;
		}
	}

	void DFSbuild(Node* node, std::mt19937& rng, unsigned* indices, unsigned count, unsigned offset){
		//omp_set_lock(&rootlock);
		//std::cout<<node->treeid<<":"<<offset<<":"<<count<<std::endl;
		//omp_unset_lock(&rootlock);
		if(count <= params_.TNS){
			node->DivDim = -1;
			node->Lchild = NULL;
			node->Rchild = NULL;
			node->StartIdx = offset;
			node->EndIdx = offset + count;
			//add points

		}else{
			unsigned idx;
			unsigned cutdim;
			DataType cutval;
			meanSplit(rng, indices, count, idx, cutdim, cutval);
			node->DivDim = cutdim;
			node->DivVal = cutval;
			node->StartIdx = offset;
			node->EndIdx = offset + count;
			Node* nodeL = new Node(); Node* nodeR = new Node();
			node->Lchild = nodeL;
			nodeL->treeid = node->treeid;
			DFSbuild(nodeL, rng, indices, idx, offset);
			node->Rchild = nodeR;
			nodeR->treeid = node->treeid;
			DFSbuild(nodeR, rng, indices+idx, count-idx, offset+idx);
		}
	}

	void DFStest(unsigned level, unsigned dim, Node* node){
		if(node->Lchild !=NULL){
			DFStest(++level, node->DivDim, node->Lchild);
			//if(level > 15)
			std::cout<<"dim: "<<node->DivDim<<"--cutval: "<<node->DivVal<<"--S: "<<node->StartIdx<<"--E: "<<node->EndIdx<<" TREE: "<<node->treeid<<std::endl;
			if(node->Lchild->Lchild ==NULL){
				std::vector<unsigned>& tmp = LeafLists[node->treeid];
				for(unsigned i = node->Rchild->StartIdx; i < node->Rchild->EndIdx; i++)
					std::cout<<features_.get_row(tmp[i])[node->DivDim]<<" ";
				std::cout<<std::endl;
			}
		}
		else if(node->Rchild !=NULL){
			DFStest(++level, node->DivDim, node->Rchild);
		}
		else{
			std::cout<<"dim: "<<dim<<std::endl;
			std::vector<unsigned>& tmp = LeafLists[node->treeid];
			for(unsigned i = node->StartIdx; i < node->EndIdx; i++)
				std::cout<<features_.get_row(tmp[i])[dim]<<" ";
			std::cout<<std::endl;
		}
	}
	void buildTrees(){
		unsigned N = features_.get_rows();
		unsigned seed = 1998;
		std::mt19937 rng(seed);
		nhoods.resize(N);
		g.resize(N);
		boost::dynamic_bitset<> visited(N, false);
		knn_graph.resize(N);
		for (auto &nhood: nhoods) {
			//nhood.nn_new.resize(params_.S * 2);
			nhood.pool.resize(params_.L+1);
			nhood.radius = std::numeric_limits<float>::max();
		}

		//build tree
		std::vector<int> indices(N);
		LeafLists.resize(TreeNum);
		std::vector<Node*> ActiveSet;
		std::vector<Node*> NewSet;
		for(unsigned i = 0; i < (unsigned)TreeNum; i++){
			Node* node = new Node;
			node->DivDim = -1;
			node->Lchild = NULL;
			node->Rchild = NULL;
			node->StartIdx = 0;
			node->EndIdx = N;
			node->treeid = i;
			tree_roots_.push_back(node);
			ActiveSet.push_back(node);
		}
#pragma omp parallel for
		for(unsigned i = 0; i < N; i++)indices[i] = i;
#pragma omp parallel for
		for(unsigned i = 0; i < (unsigned)TreeNum; i++){
			std::vector<unsigned>& myids = LeafLists[i];
			myids.resize(N);
			std::copy(indices.begin(), indices.end(),myids.begin());
			std::random_shuffle(myids.begin(), myids.end());
		}
		omp_init_lock(&rootlock);
		while(!ActiveSet.empty() && ActiveSet.size() < 1100){
#pragma omp parallel for
			for(unsigned i = 0; i < ActiveSet.size(); i++){
				Node* node = ActiveSet[i];
				unsigned mid;
				unsigned cutdim;
				DataType cutval;
				std::mt19937 rng(seed ^ omp_get_thread_num());
				std::vector<unsigned>& myids = LeafLists[node->treeid];

				meanSplit(rng, &myids[0]+node->StartIdx, node->EndIdx - node->StartIdx, mid, cutdim, cutval);

				node->DivDim = cutdim;
				node->DivVal = cutval;
				//node->StartIdx = offset;
				//node->EndIdx = offset + count;
				Node* nodeL = new Node(); Node* nodeR = new Node();
				nodeR->treeid = nodeL->treeid = node->treeid;
				nodeL->StartIdx = node->StartIdx;
				nodeL->EndIdx = node->StartIdx+mid;
				nodeR->StartIdx = nodeL->EndIdx;
				nodeR->EndIdx = node->EndIdx;
				node->Lchild = nodeL;
				node->Rchild = nodeR;
				omp_set_lock(&rootlock);
				if(mid>params_.S)NewSet.push_back(nodeL);
				if(nodeR->EndIdx - nodeR->StartIdx > params_.S)NewSet.push_back(nodeR);
				omp_unset_lock(&rootlock);
			}
			ActiveSet.resize(NewSet.size());
			std::copy(NewSet.begin(), NewSet.end(),ActiveSet.begin());
			NewSet.clear();
		}
#pragma omp parallel for
		for(unsigned i = 0; i < ActiveSet.size(); i++){
			Node* node = ActiveSet[i];
			//omp_set_lock(&rootlock);
			//std::cout<<i<<":"<<node->EndIdx-node->StartIdx<<std::endl;
			//omp_unset_lock(&rootlock);
			std::mt19937 rng(seed ^ omp_get_thread_num());
			std::vector<unsigned>& myids = LeafLists[node->treeid];
			DFSbuild(node, rng, &myids[0]+node->StartIdx, node->EndIdx-node->StartIdx, node->StartIdx);
		}
	}
    void outputVisitBucketNum(){}

	void initGraph(){
		//initial
		unsigned N = features_.get_rows();
		unsigned seed = 1998;
		std::mt19937 rng(seed);
		nhoods.resize(N);
		g.resize(N);
		boost::dynamic_bitset<> visited(N, false);
		knn_graph.resize(N);
		for (auto &nhood: nhoods) {
			//nhood.nn_new.resize(params_.S * 2);
			nhood.pool.resize(params_.L+1);
			nhood.radius = std::numeric_limits<float>::max();
		}

		//build tree
		std::vector<int> indices(N);
		LeafLists.resize(TreeNum);
		std::vector<Node*> ActiveSet;
		std::vector<Node*> NewSet;
		for(unsigned i = 0; i < (unsigned)TreeNum; i++){
			Node* node = new Node;
			node->DivDim = -1;
			node->Lchild = NULL;
			node->Rchild = NULL;
			node->StartIdx = 0;
			node->EndIdx = N;
			node->treeid = i;
			tree_roots_.push_back(node);
			ActiveSet.push_back(node);
		}
#pragma omp parallel for
		for(unsigned i = 0; i < N; i++)indices[i] = i;
#pragma omp parallel for
		for(unsigned i = 0; i < (unsigned)TreeNum; i++){
			std::vector<unsigned>& myids = LeafLists[i];
			myids.resize(N);
			std::copy(indices.begin(), indices.end(),myids.begin());
			std::random_shuffle(myids.begin(), myids.end());
		}
		omp_init_lock(&rootlock);
		while(!ActiveSet.empty() && ActiveSet.size() < 1100){
#pragma omp parallel for
			for(unsigned i = 0; i < ActiveSet.size(); i++){
				Node* node = ActiveSet[i];
				unsigned mid;
				unsigned cutdim;
				DataType cutval;
				std::mt19937 rng(seed ^ omp_get_thread_num());
				std::vector<unsigned>& myids = LeafLists[node->treeid];

				meanSplit(rng, &myids[0]+node->StartIdx, node->EndIdx - node->StartIdx, mid, cutdim, cutval);

				node->DivDim = cutdim;
				node->DivVal = cutval;
				//node->StartIdx = offset;
				//node->EndIdx = offset + count;
				Node* nodeL = new Node(); Node* nodeR = new Node();
				nodeR->treeid = nodeL->treeid = node->treeid;
				nodeL->StartIdx = node->StartIdx;
				nodeL->EndIdx = node->StartIdx+mid;
				nodeR->StartIdx = nodeL->EndIdx;
				nodeR->EndIdx = node->EndIdx;
				node->Lchild = nodeL;
				node->Rchild = nodeR;
				omp_set_lock(&rootlock);
				if(mid>params_.S)NewSet.push_back(nodeL);
				if(nodeR->EndIdx - nodeR->StartIdx > params_.S)NewSet.push_back(nodeR);
				omp_unset_lock(&rootlock);
			}
			ActiveSet.resize(NewSet.size());
			std::copy(NewSet.begin(), NewSet.end(),ActiveSet.begin());
			NewSet.clear();
		}
#pragma omp parallel for
		for(unsigned i = 0; i < ActiveSet.size(); i++){
			Node* node = ActiveSet[i];
			//omp_set_lock(&rootlock);
			//std::cout<<i<<":"<<node->EndIdx-node->StartIdx<<std::endl;
			//omp_unset_lock(&rootlock);
			std::mt19937 rng(seed ^ omp_get_thread_num());
			std::vector<unsigned>& myids = LeafLists[node->treeid];
			DFSbuild(node, rng, &myids[0]+node->StartIdx, node->EndIdx-node->StartIdx, node->StartIdx);
		}
		//DFStest(0,0,tree_roots_[0]);
		//build tree completed

		for(size_t i = 0; i < (unsigned)TreeNumBuild; i++){
			getMergeLevelNodeList(tree_roots_[i], i ,0);
		}

#pragma omp parallel for
		for(size_t i = 0; i < mlNodeList.size(); i++){
			mergeSubGraphs(mlNodeList[i].second, mlNodeList[i].first);
		}

#pragma omp parallel
		{
#ifdef _OPENMP
			std::mt19937 rng(seed ^ omp_get_thread_num());
#else
			std::mt19937 rng(seed);
#endif
			std::vector<unsigned> random(params_.S + 1);

#pragma omp for
			for (unsigned n = 0; n < N; ++n) {
				auto &nhood = nhoods[n];
				Points &pool = nhood.pool;
				if(nhood.nn_new.size()<params_.S*2){
					nhood.nn_new.resize(params_.S*2);
					GenRandom(rng, &nhood.nn_new[0], nhood.nn_new.size(), N);
				}

				GenRandom(rng, &random[0], random.size(), N);
				nhood.L = params_.S;
				nhood.Range = params_.S;
				while(knn_graph[n].size() < params_.S){
					unsigned rand_id = rng() % N;
					DataType dist = distance_->compare(
							features_.get_row(n), features_.get_row(rand_id), features_.get_cols());
					Candidate<DataType> c(rand_id,dist);
					knn_graph[n].insert(c);
				}

				//omp_set_lock(&rootlock);
				//if(knn_graph[n].size() < nhood.L)std::cout<<n<<":"<<knn_graph[n].size()<<std::endl;
				//omp_unset_lock(&rootlock);
				unsigned i = 0;
				typename CandidateHeap::reverse_iterator it = knn_graph[n].rbegin();
				for (unsigned l = 0; l < nhood.L; ++l) {
					if (random[i] == n) ++i;
					auto &nn = nhood.pool[l];
					nn.id = it->row_id;//random[i++];
					nhood.nn_new[l] = it->row_id;
					nn.dist = it->distance;//distance_->compare(features_.get_row(n), features_.get_row(nn.id), features_.get_cols());
					nn.flag = true;it++;
					//if(it == knn_graph[n].rend())break;
				}
				sort(pool.begin(), pool.begin() + nhood.L);
			}
		}
		knn_graph.clear();
#ifdef INFO
		std::cout<<"initial completed"<<std::endl;
#endif
	}

};

}
#endif

ext: hpp

max_stars_repo_name: Tengke-Xiong/efanna

max_issues_repo_name: Tengke-Xiong/efanna

max_forks_repo_name: ZJULearning/efanna


text:
//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#include <boost/multiprecision/mpfr.hpp>
#include <boost/math/tools/luroth_expansion.hpp>
#include <boost/math/constants/constants.hpp>

using boost::math::constants::pi;
using boost::math::tools::luroth_expansion;
using boost::multiprecision::mpfr_float;

int main() {
    using Real = mpfr_float;
    mpfr_float::default_precision(1024);
    auto luroth = luroth_expansion(pi<Real>());
    std::cout << luroth << "\n";
}

ext: cpp

max_stars_repo_name: anarthal/boost-unix-mirror

max_issues_repo_name: anarthal/boost-unix-mirror


text:
#include <boost/noncopyable.hpp>
#include <boost/move/move.hpp>
#include <assert.h>
#include <iostream>
#include <string>
#include <string.h>

class descriptor_owner : private boost::noncopyable{
	void* descriptor_;

public:
	explicit descriptor_owner(const char* params) {};
	~descriptor_owner() {};
};

class descriptor_owner1 {
	void* descriptor_;
	 BOOST_MOVABLE_BUT_NOT_COPYABLE(descriptor_owner1)

public:
	descriptor_owner1()
		: descriptor_(NULL)
	{}

	explicit descriptor_owner1(const char* params)
		: descriptor_(strdup(params))
	{}

	descriptor_owner1(BOOST_RV_REF(descriptor_owner1) param)
		: descriptor_(param.descriptor_) {
		param.descriptor_ = NULL;
	}

	descriptor_owner1& operator=(BOOST_RV_REF(descriptor_owner1) param) {
		clear();
		std::swap(descriptor_, param.descriptor_);
		return *this;
	}

	void clear() {
		free(descriptor_);
		descriptor_ = NULL;
	}

	bool empty() const {
		return !descriptor_;
	}

	~descriptor_owner1() {
		clear();
	}
};

descriptor_owner1 construct_descriptor2() {
	return descriptor_owner1("Construct using this string");
}

void foo_rv() {
	std::cout << "C++11\n";
	descriptor_owner1 desc;
	desc = construct_descriptor2();
	assert(!desc.empty());
}

max_stars_repo_name: dexpota/boost-api-examples

max_issues_repo_name: dexpota/boost-api-examples

max_forks_repo_name: dexpota/boost-api-examples


text:
/* Copyright (c) 2021, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
 * platform" software is 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.
 */

// ROS includes
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/CameraInfo.h>
#include <ff_msgs/VisualLandmarks.h>
#include <ff_msgs/EkfState.h>
#include <eigen_conversions/eigen_msg.h>
#include <std_msgs/Float64MultiArray.h>

// PCL includes
#include <pcl/io/ply_io.h>

// OpenCV includes
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>

// Eigen includes
#include <Eigen/Geometry>
#include <Eigen/Core>

// FreeFlyer includes
#include <ff_util/ff_names.h>
#include <ff_common/utils.h>
#include <camera/camera_model.h>
#include <config_reader/config_reader.h>

// isaac includes
#include <dense_map_utils.h>
#include <texture_processing.h>

// System includes
#include <unistd.h>
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <thread>
#include <mutex>

namespace dense_map {

class StreamingMapper {
 public:
  StreamingMapper();
  virtual ~StreamingMapper();
  void Initialize(ros::NodeHandle& nh);
  void ProcessingLoop();

 private:
  void ReadParams(ros::NodeHandle const& nh);

  void publishTexturedMesh(mve::TriangleMesh::ConstPtr mesh, std::shared_ptr<BVHTree> bvh_tree,
                           double max_iso_times_exposure, double iso, double exposure, int processed_camera_count,
                           cv::Mat const& image, double image_timestamp, camera::CameraModel const& cam,
                           std::vector<double>& smallest_cost_per_face, std::string const& out_prefix);

  void TextureCamSimPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose);
  void TextureCamSimInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& info);
  void LandmarkCallback(ff_msgs::VisualLandmarks::ConstPtr const& vl);
  void EkfStateCallback(ff_msgs::EkfState::ConstPtr const& ekf_state);
  void EkfPoseCallback(geometry_msgs::PoseStamped::ConstPtr const& ekf_pose);
  void SciCamExifCallback(std_msgs::Float64MultiArray::ConstPtr const& exif);
  void CompressedTextureCallback(const sensor_msgs::CompressedImageConstPtr& msg);
  void UncompressedTextureCallback(const sensor_msgs::ImageConstPtr& msg);
  void AddTextureCamPose(double nav_cam_timestamp, Eigen::Affine3d const& nav_cam_pose);
  void WipeOldImages();

  std::shared_ptr<image_transport::ImageTransport> image_transport;
  ros::Subscriber compressed_texture_image_sub;
  image_transport::Subscriber uncompressed_texture_image_sub;
  ros::Subscriber texture_cam_pose_sub;
  ros::Subscriber texture_cam_info_sub;
  ros::Subscriber landmark_sub;
  ros::Subscriber ekf_state_sub;
  ros::Subscriber ekf_pose_sub;
  ros::Subscriber sci_cam_exif_sub;
  ros::Publisher texture_img_pub;
  ros::Publisher texture_obj_pub;
  ros::Publisher texture_mtl_pub;

  // Parameters
  config_reader::ConfigReader config_params;

  camera::CameraParameters m_texture_cam_params;
  sensor_msgs::Image texture_obj_msg, texture_mtl_msg;

  double navcam_to_texture_cam_timestamp_offset;
  Eigen::MatrixXd texture_cam_to_navcam_trans;
  Eigen::MatrixXd navcam_to_body_trans;

  std::string nav_cam_pose_topic, ekf_state_topic, ekf_pose_topic, texture_cam_topic, sci_cam_exif_topic,
    texture_cam_type, mesh_file;
  bool sim_mode, save_to_disk, use_single_texture;

  // For meshing
  mve::TriangleMesh::Ptr mesh;
  std::shared_ptr<mve::MeshInfo> mesh_info;
  std::shared_ptr<tex::Graph> graph;  // TODO(oalexan1): Is this necessary?
  std::shared_ptr<BVHTree> bvh_tree;

  // Each callback must have a lock
  std::mutex nav_cam_pose_lock, texture_cam_pose_lock, texture_cam_info_lock, texture_cam_image_lock, sci_cam_exif_lock;

  // Data indexed by timestamp
  std::map<double, Eigen::Affine3d> nav_cam_localization_poses;
  std::map<double, Eigen::Affine3d> nav_cam_landmark_poses;
  std::map<double, Eigen::Affine3d> texture_cam_poses;
  std::map<double, cv::Mat> texture_cam_images;
  std::map<double, std::vector<double> > sci_cam_exif;

  // For processing pictures as the camera moves along
  double dist_between_processed_cams, max_iso_times_exposure;
  Eigen::Vector3d last_processed_cam_ctr;
  std::vector<double> smallest_cost_per_face;
  int processed_camera_count;
  double num_exclude_boundary_pixels;

  // The info for each face which allows one later to project an image onto
  // the face and compute the texture.
  std::vector<dense_map::FaceInfo> face_projection_info;

  tex::Model texture_model;
  std::vector<dense_map::IsaacTextureAtlas::Ptr> texture_atlases;
  cv::Mat model_texture;

  // The pixel size to use to sample the image
  double pixel_size;

  // Number of threads to use
  int num_threads;

  std::shared_ptr<std::thread> thread;
};

StreamingMapper::StreamingMapper()
    : m_texture_cam_params(Eigen::Vector2i(0, 0), Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0)),
      navcam_to_texture_cam_timestamp_offset(0.0),
      texture_cam_to_navcam_trans(Eigen::MatrixXd::Identity(4, 4)),
      navcam_to_body_trans(Eigen::MatrixXd::Identity(4, 4)),
      last_processed_cam_ctr(Eigen::Vector3d(std::numeric_limits<double>::quiet_NaN(), 0.0, 0.0)),
      processed_camera_count(0) {}

StreamingMapper::~StreamingMapper(void) { thread->join(); }

void StreamingMapper::Initialize(ros::NodeHandle& nh) {
  std::vector<std::string> args;
  bool main_thread = true;
  ff_common::InitFreeFlyerApplication(args, main_thread);

  // Set the config path to ISAAC
  char* path;
  if ((path = getenv("ISAAC_CONFIG_DIR")) == NULL) ROS_FATAL("Could not find the config path.");
  config_params.SetPath(path);

  config_params.AddFile("dense_map/streaming_mapper.config");
  if (!config_params.ReadFiles()) ROS_FATAL("Failed to read config files.");
  // Read the parameters defined in the launch file
  StreamingMapper::ReadParams(nh);

  // Read configuration data
  if (!sim_mode) {
    Eigen::MatrixXd hazcam_to_navcam_trans;
    Eigen::MatrixXd scicam_to_hazcam_trans;
    Eigen::Affine3d hazcam_depth_to_image_transform;
    double navcam_to_hazcam_timestamp_offset, scicam_to_hazcam_timestamp_offset;

    camera::CameraParameters nav_cam_params(Eigen::Vector2i(0, 0), Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0));
    camera::CameraParameters haz_cam_params(Eigen::Vector2i(0, 0), Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0));
    camera::CameraParameters sci_cam_params(Eigen::Vector2i(0, 0), Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 0));

    {
      // Note the lock, because m_texture_cam_params is a shared resource
      const std::lock_guard<std::mutex> lock(texture_cam_info_lock);
      dense_map::readConfigFile("navcam_to_hazcam_timestamp_offset", "scicam_to_hazcam_timestamp_offset",
                                "hazcam_to_navcam_transform", "scicam_to_hazcam_transform", "nav_cam_transform",
                                "hazcam_depth_to_image_transform", navcam_to_hazcam_timestamp_offset,
                                scicam_to_hazcam_timestamp_offset, hazcam_to_navcam_trans, scicam_to_hazcam_trans,
                                navcam_to_body_trans, hazcam_depth_to_image_transform, nav_cam_params, haz_cam_params,
                                sci_cam_params);

      // Transform to convert from given camera to nav camera coordinates
      if (texture_cam_type == "nav_cam") {
        texture_cam_to_navcam_trans = Eigen::MatrixXd::Identity(4, 4);
        navcam_to_texture_cam_timestamp_offset = 0.0;
        m_texture_cam_params = nav_cam_params;
      } else if (texture_cam_type == "haz_cam") {
        texture_cam_to_navcam_trans = hazcam_to_navcam_trans;
        navcam_to_texture_cam_timestamp_offset = navcam_to_hazcam_timestamp_offset;
        m_texture_cam_params = haz_cam_params;
      } else if (texture_cam_type == "sci_cam") {
        texture_cam_to_navcam_trans = hazcam_to_navcam_trans * scicam_to_hazcam_trans;
        navcam_to_texture_cam_timestamp_offset = navcam_to_hazcam_timestamp_offset - scicam_to_hazcam_timestamp_offset;
        m_texture_cam_params = sci_cam_params;
      } else {
        LOG(FATAL) << "Invalid texture cam type: " << texture_cam_type;
      }

#if 0
      std::cout << "hazcam_to_navcam_trans\n" << hazcam_to_navcam_trans << "\n";
      std::cout << "scicam_to_hazcam_trans\n" << scicam_to_hazcam_trans << "\n";
      std::cout << "texture_cam_to_navcam_trans\n" << texture_cam_to_navcam_trans << "\n";
      std::cout << "navcam_to_hazcam_timestamp_offset: "
                << navcam_to_hazcam_timestamp_offset
                << "\n";
      std::cout << "scicam_to_hazcam_timestamp_offset: "
                << scicam_to_hazcam_timestamp_offset
                << "\n";
      std::cout << "hazcam_depth_to_image_transform\n"
                << hazcam_depth_to_image_transform.matrix()
                << "\n";

      std::cout << "navcam_to_texture_cam_timestamp_offset: "
                << navcam_to_texture_cam_timestamp_offset << "\n";
      std::cout << "texture cam focal vector: "
                << m_texture_cam_params.GetFocalVector().transpose() << "\n";
#endif
    }
  }

  // Set up the publishers
  std::string mapper_img_topic = "/ism/" + texture_cam_type + "/img";
  std::string mapper_obj_topic = "/ism/" + texture_cam_type + "/obj";
  std::string mapper_mtl_topic = "/ism/" + texture_cam_type + "/mtl";
  ROS_INFO_STREAM("Publishing topics: " << mapper_img_topic << ' ' << mapper_obj_topic << ' ' << mapper_mtl_topic);
  texture_img_pub = nh.advertise<sensor_msgs::Image>(mapper_img_topic, 1);
  texture_obj_pub = nh.advertise<sensor_msgs::Image>(mapper_obj_topic, 1);
  texture_mtl_pub = nh.advertise<sensor_msgs::Image>(mapper_mtl_topic, 1);

  // Set up the subscribers
  if (sim_mode) {
    // Subscribe to the simulated texture cam image pose and intrinsics.
    // Keep a bunch of them in the queue.
    // The name of these topics are TOPIC_HAZ_CAM_SIM_POSE, etc., in ff_names.h.
    std::string texture_cam_sim_pose_topic = "/sim/" + texture_cam_type + "/pose";
    std::string texture_cam_sim_info_topic = "/sim/" + texture_cam_type + "/info";
    ROS_INFO_STREAM("Subscribing to  " << texture_cam_sim_pose_topic);
    ROS_INFO_STREAM("Subscribing to  " << texture_cam_sim_info_topic);
    texture_cam_pose_sub =
      nh.subscribe(texture_cam_sim_pose_topic, 10, &StreamingMapper::TextureCamSimPoseCallback, this);
    texture_cam_info_sub =
      nh.subscribe(texture_cam_sim_info_topic, 10, &StreamingMapper::TextureCamSimInfoCallback, this);
  } else {
    // Get the nav cam pose topic or the ekf state topic, or the ekf
    // pose topic, from which the other camera poses are deduced.
    if (!nav_cam_pose_topic.empty()) {
      ROS_INFO_STREAM("Subscribing to nav cam pose topic: " << nav_cam_pose_topic);
      landmark_sub = nh.subscribe(nav_cam_pose_topic, 10, &StreamingMapper::LandmarkCallback, this);
    } else if (!ekf_state_topic.empty()) {
      ROS_INFO_STREAM("Subscribing to ekf state topic: " << ekf_state_topic);
      ekf_state_sub = nh.subscribe(ekf_state_topic, 10, &StreamingMapper::EkfStateCallback, this);
    } else if (!ekf_pose_topic.empty()) {
      ROS_INFO_STREAM("Subscribing to ekf pose topic: " << ekf_pose_topic);
      ekf_pose_sub = nh.subscribe(ekf_pose_topic, 10, &StreamingMapper::EkfPoseCallback, this);
    }
  }

  sci_cam_exif_topic = "/hw/sci_cam_exif";
  ROS_INFO_STREAM("Subscribing to sci cam exif topic: " << sci_cam_exif_topic);
  sci_cam_exif_sub = nh.subscribe(sci_cam_exif_topic, 10, &StreamingMapper::SciCamExifCallback, this);

  // Subscribe to images. Keep just 2 in the queue as they can be big.
  image_transport.reset(new image_transport::ImageTransport(nh));

  std::string compressed = "/compressed";
  if (texture_cam_topic.length() >= compressed.length() &&
      texture_cam_topic.compare(texture_cam_topic.length() - compressed.length(), compressed.length(), compressed) ==
        0) {
    // texture_cam_topic ends with the string /compressed
    ROS_INFO_STREAM("Subscribing to compressed image topic: " << texture_cam_topic);
    compressed_texture_image_sub =
      nh.subscribe(texture_cam_topic, 2, &StreamingMapper::CompressedTextureCallback, this);
  } else {
    ROS_INFO_STREAM("Subscribing to uncompressed image topic: " << texture_cam_topic);
    uncompressed_texture_image_sub =
      image_transport->subscribe(texture_cam_topic, 2, &StreamingMapper::UncompressedTextureCallback, this);
  }

  // Load the mesh
  try {
    dense_map::loadMeshBuildTree(mesh_file, mesh, mesh_info, graph, bvh_tree);

    std::vector<unsigned int> const& faces = mesh->get_faces();
    int num_faces = faces.size();
    smallest_cost_per_face = std::vector<double>(num_faces, 1.0e+100);

    if (use_single_texture)
      dense_map::formModel(mesh, pixel_size, num_threads, face_projection_info, texture_atlases, texture_model);
  } catch (std::exception& e) {
    LOG(FATAL) << "Could not load mesh.\n" << e.what() << "\n";
  }

  // Start a new thread in which to do the processing
  thread.reset(new std::thread(&StreamingMapper::ProcessingLoop, this));
}

// TODO(oalexan1): Document all these.
void StreamingMapper::ReadParams(ros::NodeHandle const& nh) {
  if (!config_params.GetStr("nav_cam_pose_topic", &nav_cam_pose_topic))
    ROS_FATAL("Could not read the nav_cam_pose_topic parameter.");
  if (!config_params.GetStr("ekf_pose_topic", &ekf_pose_topic))
    ROS_FATAL("Could not read the ekf_pose_topic parameter.");
  if (!config_params.GetStr("ekf_state_topic", &ekf_state_topic))
    ROS_FATAL("Could not read the ekf_state_topic parameter.");
  if (!config_params.GetStr("texture_cam_topic", &texture_cam_topic))
    ROS_FATAL("Could not read the texture_cam_topic parameter.");
  if (!config_params.GetStr("texture_cam_type", &texture_cam_type))
    ROS_FATAL("Could not read the texture_cam_type parameter.");
  if (!config_params.GetStr("mesh_file", &mesh_file)) ROS_FATAL("Could not read the mesh_file parameter.");
  if (!config_params.GetReal("dist_between_processed_cams", &dist_between_processed_cams))
    ROS_FATAL("Could not read the dist_between_processed_cams parameter.");
  if (!config_params.GetReal("max_iso_times_exposure", &max_iso_times_exposure))
    ROS_FATAL("Could not read the max_iso_times_exposure parameter.");
  if (!config_params.GetReal("pixel_size", &pixel_size)) ROS_FATAL("Could not read the pixel_size parameter.");
  if (!config_params.GetReal("num_exclude_boundary_pixels", &num_exclude_boundary_pixels))
    ROS_FATAL("Could not read the num_exclude_boundary_pixels parameter.");
  if (!config_params.GetInt("num_threads", &num_threads)) ROS_FATAL("Could not read the num_threads parameter.");
  if (!nh.hasParam("sim_mode")) LOG(FATAL) << "sim_mode flag not specified.";

  // The sim_mode parameter is very important, it must be read early
  // on as a lot of logic depends on its value.
  nh.getParam("sim_mode", sim_mode);

  if (!config_params.GetBool("save_to_disk", &save_to_disk)) ROS_FATAL("Could not read the save_to_disk parameter.");

  if (!config_params.GetBool("use_single_texture", &use_single_texture))
    ROS_FATAL("Could not read the use_single_texture parameter.");

  ROS_INFO_STREAM("Texture camera type = " << texture_cam_type);
  ROS_INFO_STREAM("Mesh = " << mesh_file);
  if (!sim_mode) {
    int num = (!nav_cam_pose_topic.empty()) + (!ekf_state_topic.empty()) + (!ekf_pose_topic.empty());
    if (num != 1)
      LOG(FATAL) << "Must specify exactly only one of nav_cam_pose_topic, "
                 << "ekf_state_topic, ekf_pose_topic.";

    ROS_INFO_STREAM("dist_between_processed_cams = " << dist_between_processed_cams);
    ROS_INFO_STREAM("max_iso_times_exposure = " << max_iso_times_exposure);
    ROS_INFO_STREAM("use_single_texture = " << use_single_texture);
    ROS_INFO_STREAM("pixel_size = " << pixel_size);
    ROS_INFO_STREAM("num_threads = " << num_threads);
    ROS_INFO_STREAM("sim_mode = " << sim_mode);
    ROS_INFO_STREAM("save_to_disk = " << save_to_disk);
    ROS_INFO_STREAM("num_exclude_boundary_pixels = " << num_exclude_boundary_pixels);
    ROS_INFO_STREAM("ASTROBEE_ROBOT = " << getenv("ASTROBEE_ROBOT"));
  }

  if (texture_cam_type != "nav_cam" && texture_cam_type != "sci_cam" && texture_cam_type != "haz_cam" &&
      texture_cam_type != "heat_cam" && texture_cam_type != "acoustics_cam")
    LOG(FATAL) << "Invalid texture cam type: " << texture_cam_type;

  // For a camera like sci_cam, the word "sci" better be part of texture_cam_topic.
  std::string cam_name = texture_cam_type.substr(0, 3);
  if (texture_cam_topic.find(cam_name) == std::string::npos)
    LOG(FATAL) << "Texture topic " << texture_cam_topic << " is expected to contain the string " << cam_name;
}

void StreamingMapper::publishTexturedMesh(mve::TriangleMesh::ConstPtr mesh, std::shared_ptr<BVHTree> bvh_tree,
                                          double max_iso_times_exposure, double iso, double exposure,
                                          int processed_camera_count, cv::Mat const& image, double image_timestamp,
                                          camera::CameraModel const& cam, std::vector<double>& smallest_cost_per_face,
                                          std::string const& out_prefix) {
  omp_set_dynamic(0);                // Explicitly disable dynamic teams
  omp_set_num_threads(num_threads);  // Use this many threads for all consecutive
                                     // parallel regions

  // Colorize the image if grayscale. Careful here to avoid making a
  // copy if we don't need one.  Keep img_ptr pointing at the final
  // image.
  const cv::Mat* img_ptr = NULL;
  cv::Mat color_image;
  std::string mat_type = dense_map::matType(image);
  if (mat_type == "8UC3") {
    // Don't colorize
    img_ptr = ℑ
  } else if (mat_type == "8UC1") {
    // Do colorize
    cv::cvtColor(image, color_image, cv::COLOR_GRAY2BGR);
    img_ptr = &color_image;
  } else {
    LOG(FATAL) << "Unknown image type: " << mat_type;
  }

  util::WallTimer timer1;

  // Exposure adjustment
  cv::Mat scaled_image;
  int same_type = -1;
  double offset = 0.0;
  if (iso > 0.0 && exposure > 0.0) {
    dense_map::exposureCorrection(max_iso_times_exposure, iso, exposure, *img_ptr, scaled_image);
    img_ptr = &scaled_image;
  }

  // std::cout << "Exposure correction took " << timer1.get_elapsed()/1000.0 <<
  // " seconds\n";

  // Prepare the image for publishing
  sensor_msgs::ImagePtr msg;
  std::vector<Eigen::Vector3i> face_vec;
  std::map<int, Eigen::Vector2d> uv_map;
  if (!use_single_texture) {
    // Find the UV coordinates and the faces having them
    dense_map::projectTexture(mesh, bvh_tree, *img_ptr, cam, num_exclude_boundary_pixels, smallest_cost_per_face,
                              face_vec, uv_map);

    msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", *img_ptr).toImageMsg();
  } else {
    dense_map::projectTexture(mesh, bvh_tree, *img_ptr, cam, smallest_cost_per_face, pixel_size, num_threads,
                              face_projection_info, texture_atlases, texture_model, model_texture);
    // Note that the output image has an alpha channel
    msg = cv_bridge::CvImage(std_msgs::Header(), "bgra8", model_texture).toImageMsg();
  }

  // Publish the image, use the same timestamp as for the input image
  msg->header.stamp = ros::Time(image_timestamp);

  texture_img_pub.publish(msg);

  // Publish the obj string as a mono image with one row
  std::string obj_str;

  if (!use_single_texture || processed_camera_count == 0 || save_to_disk) {
    if (!use_single_texture)
      formObjCustomUV(mesh, face_vec, uv_map, out_prefix, obj_str);
    else
      formObj(texture_model, out_prefix, obj_str);

    int obj_len = obj_str.size();
    int width = std::sqrt(1.0 * obj_len);
    int height = obj_len / width + 1;
    if (width * height <= obj_len) LOG(FATAL) << "Not enough room allocated for the image.";

    texture_obj_msg.header.stamp = ros::Time(image_timestamp);
    texture_obj_msg.width = width;
    texture_obj_msg.height = height;
    texture_obj_msg.step = width;
    texture_obj_msg.encoding = "mono8";
    texture_obj_msg.is_bigendian = false;
    texture_obj_msg.data.resize(width * height);

    std::copy(reinterpret_cast<const char*>(&obj_str[0]),            // input beg
              reinterpret_cast<const char*>(&obj_str[0]) + obj_len,  // input end
              texture_obj_msg.data.begin());                         // destination
    // Pad with nulls
    for (int it = obj_len; it < width * height; it++) texture_obj_msg.data[it] = '\0';

    // When using a single texture, publish just once
    if (!use_single_texture || processed_camera_count == 0) texture_obj_pub.publish(texture_obj_msg);
  }

  // Publish the mtl string as a mono image with one row (this hack is temporary)
  std::string mtl_str;
  if (!use_single_texture || processed_camera_count == 0 || save_to_disk) {
    formMtl(out_prefix, mtl_str);
    int mtl_len = mtl_str.size();
    texture_mtl_msg.header.stamp = ros::Time(image_timestamp);
    texture_mtl_msg.width = mtl_len;
    texture_mtl_msg.height = 1;
    texture_mtl_msg.step = mtl_len;
    texture_mtl_msg.encoding = "mono8";
    texture_mtl_msg.is_bigendian = false;
    texture_mtl_msg.data.resize(mtl_len);
    std::copy(reinterpret_cast<const char*>(&mtl_str[0]),            // input beg
              reinterpret_cast<const char*>(&mtl_str[0]) + mtl_len,  // input end
              texture_mtl_msg.data.begin());                         // destination

    if (!use_single_texture || processed_camera_count == 0) texture_mtl_pub.publish(texture_mtl_msg);
  }

  if (save_to_disk) {
    std::string obj_file = out_prefix + ".obj";
    std::cout << "Writing: " << obj_file << std::endl;
    std::ofstream obj_handle(obj_file);
    obj_handle << obj_str;
    obj_handle.close();

    std::string mtl_file = out_prefix + ".mtl";
    std::cout << "Writing: " << mtl_file << std::endl;
    std::ofstream mtl_handle(mtl_file);
    mtl_handle << mtl_str;
    mtl_handle.close();

    std::string png_file = out_prefix + ".png";
    std::cout << "Writing: " << png_file << std::endl;
    if (!use_single_texture)
      cv::imwrite(png_file, *img_ptr);
    else
      cv::imwrite(png_file, model_texture);
  }
}

// Add the latest received texture cam pose to the storage. This is meant to be used
// only for simulated data. Else use the landmark and ekf callbacks.
void StreamingMapper::TextureCamSimPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose) {
  if (!sim_mode) return;

  // Add the latest pose. Use a lock.
  const std::lock_guard<std::mutex> lock(texture_cam_pose_lock);

  // Convert the pose to Eigen::Affine3d
  Eigen::Affine3d texture_cam_pose;
  tf::poseMsgToEigen(pose->pose, texture_cam_pose);
  texture_cam_poses[pose->header.stamp.toSec()] = texture_cam_pose;

  // std::cout << "Received the sim texture cam pose at time: " << pose->header.stamp.toSec()
  // << "\n";

  // Wipe the oldest poses. Keep a lot of them as sometimes they can come very often.
  while (texture_cam_poses.size() > 100) {
    texture_cam_poses.erase(texture_cam_poses.begin());
  }
}

void StreamingMapper::TextureCamSimInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& info) {
  if (!sim_mode) return;

  // Initialize m_texture_cam_params just once. Use a lock.
  if (m_texture_cam_params.GetFocalVector() == Eigen::Vector2d(0, 0)) {
    const std::lock_guard<std::mutex> lock(texture_cam_info_lock);

    int image_width = info->width;
    int image_height = info->height;
    double focal_length = info->K[0];
    double optical_center_x = info->K[2];
    double optical_center_y = info->K[5];

    // We support only fish eye lenses (one distortion parameter), or
    // lenses with no distortion (zero distortion parameters).  Note:
    // The fisheye distortion model in freeflyer does not seem to
    // agree to well with the wide angle camera distorted images
    // output by gazebo. Hence it is better to use images with no
    // distortion in the simulated images.
    Eigen::VectorXd distortion;
    if (info->D[0] != 0) {
      distortion.resize(1);
      distortion[0] = info->D[0];
    }

    // std::cout << "Received the camera info at time: " << info->header.stamp.toSec() << "\n";
    m_texture_cam_params =
      camera::CameraParameters(Eigen::Vector2i(image_width, image_height), Eigen::Vector2d(focal_length, focal_length),
                               Eigen::Vector2d(optical_center_x, optical_center_y), distortion);
  }
}

// Compute the texture cam pose and timestamp based on the nav cam pose
// and timestamp, and save it in the storage.
void StreamingMapper::AddTextureCamPose(double nav_cam_timestamp, Eigen::Affine3d const& nav_cam_pose) {
  if (nav_cam_pose.matrix() == Eigen::Matrix<double, 4, 4>::Identity()) {
    // Skip bad poses
    return;
  }

  // Must compensate for the fact that the nav cam, haz cam, and texture cam all
  // have some time delay among them
  double texture_cam_timestamp = nav_cam_timestamp + navcam_to_texture_cam_timestamp_offset;

  // Keep in mind that nav_cam_pose is the transform from the nav cam
  // to the world.  Hence the matrices are multiplied in the order as
  // below.
  Eigen::Affine3d texture_cam_pose;
  texture_cam_pose.matrix() = nav_cam_pose.matrix() * texture_cam_to_navcam_trans;
  {
    // Add the latest pose. Use a lock.
    const std::lock_guard<std::mutex> lock(texture_cam_pose_lock);
    texture_cam_poses[texture_cam_timestamp] = texture_cam_pose;

    // Wipe the oldest poses. Keep a lot of them, as sometimes, particularly for ekf,
    // they can come very fast
    while (texture_cam_poses.size() > 100) texture_cam_poses.erase(texture_cam_poses.begin());
  }
  // TODO(oalexan1): This will need more testing!
}

// This callback takes as input the nav_cam pose as output by localization_node.
void StreamingMapper::LandmarkCallback(ff_msgs::VisualLandmarks::ConstPtr const& vl) {
  if (sim_mode) return;

  double nav_cam_timestamp = vl->header.stamp.toSec();
  Eigen::Affine3d nav_cam_pose;
  tf::poseMsgToEigen(vl->pose, nav_cam_pose);

  StreamingMapper::AddTextureCamPose(nav_cam_timestamp, nav_cam_pose);
}

// This callback takes as input the robot pose as output by ekf on
// ekf_state_topic (it should be set to /gnc/ekf)
void StreamingMapper::EkfStateCallback(ff_msgs::EkfState::ConstPtr const& ekf_state) {
  if (sim_mode) return;

  double nav_cam_timestamp = ekf_state->header.stamp.toSec();
  Eigen::Affine3d body_pose;
  tf::poseMsgToEigen(ekf_state->pose, body_pose);

  // Convert from body pose (body to world transform) to nav cam pose (nav cam to world transform)
  // Use the equation: body_to_world = navcam_to_world * body_to_navcam, written equivalently as:
  // navcam_to_world = body_to_world * navcam_to_body
  Eigen::Affine3d nav_cam_pose;
  nav_cam_pose.matrix() = body_pose.matrix() * navcam_to_body_trans;
  StreamingMapper::AddTextureCamPose(nav_cam_timestamp, nav_cam_pose);
}

// This callback takes as input the robot pose as output by ekf on /loc/pose.
void StreamingMapper::EkfPoseCallback(geometry_msgs::PoseStamped::ConstPtr const& ekf_pose) {
  if (sim_mode) return;

  double nav_cam_timestamp = ekf_pose->header.stamp.toSec();
  Eigen::Affine3d body_pose;
  tf::poseMsgToEigen(ekf_pose->pose, body_pose);

  // Convert from body pose (body to world transform) to nav cam pose (nav cam to world transform)
  // Use the equation: body_to_world = navcam_to_world * body_to_navcam, written equivalently as:
  // navcam_to_world = body_to_world * navcam_to_body
  Eigen::Affine3d nav_cam_pose;
  nav_cam_pose.matrix() = body_pose.matrix() * navcam_to_body_trans;
  StreamingMapper::AddTextureCamPose(nav_cam_timestamp, nav_cam_pose);
}

// This callback takes as input the robot pose as output by ekf on /loc/pose.
void StreamingMapper::SciCamExifCallback(std_msgs::Float64MultiArray::ConstPtr const& exif) {
  if (sim_mode) return;

  double timestamp = exif->data[dense_map::TIMESTAMP];

  std::vector<double> exif_vec;
  for (int it = 0; it < dense_map::NUM_EXIF; it++) {
    exif_vec.push_back(exif->data[it]);
  }

  {
    // Use a lock to modify shared data
    const std::lock_guard<std::mutex> lock(sci_cam_exif_lock);
    sci_cam_exif[timestamp] = exif_vec;

    // Wipe the oldest exif data
    while (sci_cam_exif.size() > 10) {
      sci_cam_exif.erase(sci_cam_exif.begin());
    }
  }
}

void StreamingMapper::WipeOldImages() {
  // Wipe the oldest images. Keep just a few.
  size_t num_to_keep = 10;  // for sci_cam
  if (texture_cam_type == "nav_cam") {
    // For small images that get published often keep a bunch of them,
    // as sometimes there is a delay for when the pose arrives,
    // and if not enough images are kept they are wiped before
    // they can be used.
    // TODO(oalexan1): This is not robust enough.
    num_to_keep = 50;
  }
  if (texture_cam_type == "haz_cam") {
    num_to_keep = 100;
  }

  while (texture_cam_images.size() > num_to_keep) texture_cam_images.erase(texture_cam_images.begin());

  // There is a second wiping in this code in a different place. After
  // an image is processed it will be wiped together with any images
  // older than it. So the real number of images is not as many as it
  // looks here.
}

void StreamingMapper::CompressedTextureCallback(const sensor_msgs::CompressedImageConstPtr& msg) {
  // Use a lock to ensure no shared data is messed up.
  const std::lock_guard<std::mutex> lock(texture_cam_image_lock);

  double texture_cam_image_timestamp = msg->header.stamp.toSec();
  try {
    // Copy the data to local storage to ensure no strange behavior
    //  cv::Mat tmp_img = cv::imdecode(cv::Mat(msg->data), cv::IMREAD_COLOR);
    //  cv::Mat tmp_img =
    texture_cam_images[texture_cam_image_timestamp] = cv::imdecode(cv::Mat(msg->data), cv::IMREAD_COLOR);

    //  cv::namedWindow( "Display window", cv::WINDOW_AUTOSIZE );  // Create a window for display.
    //  cv::imshow( "Display window", tmp_img);                   // Show our image inside it.
    //  cv::waitKey(0);

    //  texture_cam_images[texture_cam_image_timestamp] = tmp_img;
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not decompress image!");
  }

  WipeOldImages();
}

// Record the texture cam image received
void StreamingMapper::UncompressedTextureCallback(const sensor_msgs::ImageConstPtr& msg) {
  // Use a lock to ensure no shared data is messed up.
  const std::lock_guard<std::mutex> lock(texture_cam_image_lock);

  double texture_cam_image_timestamp = msg->header.stamp.toSec();

  // std::cout << "Received image at time " << texture_cam_image_timestamp << std::endl;

  cv_bridge::CvImageConstPtr image_ptr;
  try {
    image_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
  }

  // Copy the data to local storage to ensure no strange behavior
  (image_ptr->image).copyTo(texture_cam_images[texture_cam_image_timestamp]);

  WipeOldImages();
}

// Iterate over the texture images. Overlay them on the 3D model and publish
// the result.
// Need to consider here if they are
// full res and compressed and color, or 1/4 res and grayscale and
// and uncompressed
//
void StreamingMapper::ProcessingLoop() {
  double last_attempted_texture_cam_timestamp = -1.0;

  bool success = false;

  while (ros::ok()) {
    if (!success) {
      // Sleep for a little if we had no luck so far
      double num_seconds = 0.2;
      usleep(static_cast<int>(1e+6 * num_seconds));
    } else {
      // We were just successful in processing. That should have taken a while,
      // so no need to sleep, and check for new data right away.
      success = false;
    }

    if (m_texture_cam_params.GetFocalVector() == Eigen::Vector2d(0, 0)) {
      // if the texture cam is not initialized yet, then wait
      continue;
    }

    // Safely copy the data we need (this data is small, so copying is cheap).

    std::map<double, Eigen::Affine3d> local_texture_cam_poses;
    {
      const std::lock_guard<std::mutex> lock(texture_cam_pose_lock);
      local_texture_cam_poses = texture_cam_poses;
    }

    camera::CameraParameters local_texture_cam_params(Eigen::Vector2i(0, 0), Eigen::Vector2d(0, 0),
                                                      Eigen::Vector2d(0, 0));
    {
      const std::lock_guard<std::mutex> lock(texture_cam_info_lock);
      local_texture_cam_params = m_texture_cam_params;
    }

    // Copy the exif data locally using a lock
    bool need_exif = (!sim_mode && texture_cam_type == "sci_cam");

    std::map<double, std::vector<double> > local_sci_cam_exif;
    if (need_exif) {
      const std::lock_guard<std::mutex> lock(sci_cam_exif_lock);
      local_sci_cam_exif = sci_cam_exif;
    }

    // TODO(oalexan1): Make the exif topic optional?

    double texture_cam_image_timestamp = -1.0;
    cv::Mat texture_cam_image;
    {
      // Iterate over the texture cam images we have, from newest to
      // oldest, and see for which of them we have enough data to
      // build the texture.
      const std::lock_guard<std::mutex> lock(texture_cam_image_lock);

      for (auto it = texture_cam_images.rbegin(); it != texture_cam_images.rend(); it++) {
        texture_cam_image_timestamp = it->first;

        // Do not do images older than the image we did previously
        if (texture_cam_image_timestamp <= last_attempted_texture_cam_timestamp) {
          break;
        }

        if (local_texture_cam_poses.empty()) {
          break;  // no poses yet
        }

        // Ensure that we can bracket the texture image timestamp between
        // the timestamps of the poses
        if (texture_cam_image_timestamp > local_texture_cam_poses.rbegin()->first) {
          continue;
        }
        if (texture_cam_image_timestamp < local_texture_cam_poses.begin()->first) {
          continue;
        }

        if (need_exif && local_sci_cam_exif.find(texture_cam_image_timestamp) == local_sci_cam_exif.end()) {
          // Skip if the exif info did not arrive yet
          continue;
        }

        // While copying could be expensive, how else can we
        // guaranteed that our data is still there when we need it,
        // unless we use a lock throughout?
        (it->second).copyTo(texture_cam_image);

        success = true;

        // Wipe this image and images older than this as we will never use them
        while (texture_cam_images.size() > 0 && texture_cam_images.begin()->first <= texture_cam_image_timestamp)
          texture_cam_images.erase(texture_cam_images.begin());

        // Stop given that we have found a good image to process
        break;
      }
    }  // end of lock

    if (!success) {
      continue;
    }

    util::WallTimer timer;

    // All is good with this timestamp. We may still not want to process it,
    // if the camera is too close now to the previously processed camera,
    // but at least mark it as good, so we won't have to examine it again.
    last_attempted_texture_cam_timestamp = texture_cam_image_timestamp;

    // Find the interpolated pose at the current image
    Eigen::Affine3d curr_texture_cam_pose;

    if (!dense_map::findInterpPose(texture_cam_image_timestamp, local_texture_cam_poses, curr_texture_cam_pose))
      LOG(FATAL) << "Could not bracket the timestamp. Should have worked at this stage.";

    camera::CameraModel cam(curr_texture_cam_pose.inverse(), local_texture_cam_params);

    Eigen::Vector3d cam_ctr = cam.GetPosition();
    double dist_to_prev_processed = (last_processed_cam_ctr - cam_ctr).norm();
    bool do_process = (std::isnan(dist_to_prev_processed) ||  // for the first camera to process
                       dist_to_prev_processed > dist_between_processed_cams);

    //  if (!std::isnan(dist_to_prev_processed))
    //    ROS_INFO_STREAM("Distance from current camera to last processed camera: "
    //                  << dist_to_prev_processed << " meters.");

    // Current camera is too close to previously processed camera, hence wait for the bot to move
    if (!do_process) {
      // ROS_INFO_STREAM("Won't process this camera image, as it is too close to previous one.");
      continue;
    }

    // May need to keep this comment long term
    // std::cout << "Processing the streaming camera image at time stamp "
    //           << texture_cam_image_timestamp << std::endl;

    double iso = -1.0, exposure = -1.0;
    if (need_exif) {
      auto exif_it = local_sci_cam_exif.find(texture_cam_image_timestamp);
      if (exif_it == local_sci_cam_exif.end()) {
        LOG(FATAL) << "Could not find the exif data.";
      }
      std::vector<double> const& exif = exif_it->second;
      iso = exif[dense_map::ISO];
      exposure = exif[dense_map::EXPOSURE_TIME];
    }

    // Publish the textured mesh in obj format
    std::ostringstream oss;
    oss << "processed_" << 1000 + processed_camera_count;  // add 1000 to list them nicely
    std::string out_prefix = oss.str();
    publishTexturedMesh(mesh, bvh_tree, max_iso_times_exposure, iso, exposure, processed_camera_count,
                        texture_cam_image, texture_cam_image_timestamp, cam, smallest_cost_per_face, out_prefix);

    // Save this for next time
    last_processed_cam_ctr = cam_ctr;
    processed_camera_count++;

    std::cout << "Total processing took " << timer.get_elapsed() / 1000.0 << " seconds\n";
  }
}

}  // end of namespace dense_map

int main(int argc, char** argv) {
  ros::init(argc, argv, "streaming_mapper");

  std::cout.precision(17);  // to be able to print timestamps accurately

  ros::NodeHandle nh("~");
  dense_map::StreamingMapper sm;
  sm.Initialize(nh);

  ros::spin();

  return 0;
}

ext: cc

max_stars_repo_name: oleg-alexandrov/isaac

max_issues_repo_name: oleg-alexandrov/isaac

max_forks_repo_name: oleg-alexandrov/isaac


text: #include <boost/mpl/aux_/preprocessed/bcc/apply_wrap.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino