text:
///////////////////////////////////////////////////////////////////////////////
//
//  Copyright (C) 2008-2012  Artyom Beilis (Tonkikh) <artyomtnk@yahoo.com>
//
//  See accompanying file COPYING.TXT file for licensing details.
//
///////////////////////////////////////////////////////////////////////////////
#define CPPCMS_SOURCE
#include <cppcms/session_interface.h>
#include <cppcms/session_pool.h>
#include <cppcms/session_api.h>
#include <cppcms/util.h>
#include <cppcms/http_context.h>
#include <cppcms/http_request.h>
#include <cppcms/http_response.h>
#include <cppcms/http_cookie.h>
#include <cppcms/cppcms_error.h>
#include <cppcms/service.h>
#include <cppcms/json.h>
#include <cppcms/urandom.h>
#include <cppcms/base64.h>
#include <booster/log.h>

#include <iostream>

#include "cached_settings.h"
#include <set>
#include <sstream>
#include "string.h"

namespace cppcms {

	struct session_interface::_data {
		session_pool *pool;
		session_interface_cookie_adapter *adapter;
		_data() : pool(0), adapter(0) {}
	};

	struct session_interface::entry {
		std::string value;
		bool exposed;
		entry() : exposed(false) {}
		explicit entry(std::string const &s) : value(s), exposed(false) {}
		entry(std::string const &v,bool exp) : value(v) , exposed(exp) {}
		bool operator==(entry const &other) const
		{
			return value==other.value && exposed==other.exposed;
		}
		bool operator!=(entry const &other) const
		{
			return !(*this==other);
		}
	};

void session_interface::init()
{
	csrf_validation_ = cached_settings().security.csrf.enable;
	csrf_do_validation_ = cached_settings().security.csrf.automatic;
	timeout_val_def_=cached_settings().session.timeout;
	std::string s_how=cached_settings().session.expire;
	if(s_how=="fixed") {
		how_def_=fixed;
	}
	else if(s_how=="renew") {
		how_def_=renew;
	}
	else if(s_how=="browser") {
		how_def_=browser;
	}
	else {
		throw cppcms_error("Unsupported `session.expire' type `"+s_how+"'");
	}
}

session_interface::session_interface(session_pool &pool,session_interface_cookie_adapter &adapter) :
	context_(0),
	loaded_(0),
	reset_(0),
	csrf_checked_(0),
	csrf_do_validation_(0),
	csrf_validation_(0),
	d(new session_interface::_data())
{
	d->pool = &pool;
	d->adapter = &adapter;
	init();
	storage_=d->pool->get();
}

session_interface::session_interface(http::context &context) :
	context_(&context),
	loaded_(0),
	reset_(0),
	csrf_checked_(0),
	csrf_do_validation_(0),
	csrf_validation_(0),
	d(new session_interface::_data())
{
	init();
	storage_=context_->service().session_pool().get();
}

session_interface::~session_interface()
{
}

void session_interface::request_origin_validation_is_required(bool v)
{
	csrf_do_validation_ = v;
}

bool session_interface::validate_csrf_token(std::string const &token)
{
	std::string session_token = get("_csrf","");
	return session_token.empty() || session_token == token;
}

void session_interface::validate_request_origin()
{
	if(!context_)
		throw cppcms_error("request origin validation isn't possible without http::context");
	if(csrf_checked_)
		return;
	csrf_checked_ = 1;

	if(!csrf_validation_)
		return;
	if(!csrf_do_validation_)
		return;
	if(context_->request().request_method()!="POST")
		return;

	std::string token;

	typedef http::request::form_type::const_iterator iterator_type;
	std::pair<iterator_type,iterator_type> pair=context_->request().post().equal_range("_csrf");

	if(pair.first != pair.second && std::distance(pair.first,pair.second)==1)
		token = pair.first->second;
	else
		token = context_->request().getenv("HTTP_X_CSRFTOKEN");

	if(!validate_csrf_token(token)) {
		BOOSTER_WARNING("cppcms")	<<"CSRF validation failed"
						<<" IP="<< context_->request().remote_addr()
						<<" SCRIPT_NAME=" << context_->request().script_name()
						<<" PATH_INFO="<<context_->request().path_info();
		throw request_forgery_error();
	}
}

bool session_interface::load()
{
	if(loaded_)
		return false; // FIXME
	loaded_ = 1;
	if(!storage_.get())
		return false;
	data_.clear();
	data_copy_.clear();
	timeout_val_=timeout_val_def_;
	how_=how_def_;
	std::string ar;
	saved_=0;
	on_server_=0;
	if(!storage_->load(*this,ar,timeout_in_)) {
		return false;
	}
	load_data(data_,ar);
	data_copy_=data_;
	if(is_set("_t"))
		timeout_val_=get<int>("_t");
	if(is_set("_h"))
		how_=get<int>("_h");
	if(is_set("_s"))
		on_server_=get<int>("_s");
	return true;
}
bool session_interface::set_cookie_adapter_and_reload(session_interface_cookie_adapter &adapter)
{
	d->adapter = &adapter;
	loaded_ = 0;
	return load();
}

int session_interface::cookie_age()
{
	if(how_==browser)
		return 0;
	if(how_==renew || ( how_==fixed && new_session_ ))
		return timeout_val_;
	return timeout_in_ - time(NULL);
}

time_t session_interface::session_age()
{
	if(how_==browser || how_==renew || (how_==fixed && new_session_))
		return timeout_val_ + time(NULL);
	return timeout_in_;
}

namespace {
struct packed {
        uint32_t key_size  : 10;
        uint32_t exposed   :  1;
        uint32_t data_size : 21;
	packed() {}
	packed(unsigned ks,bool exp, unsigned ds)
	{
		if(ks >=1024)
			throw cppcms_error("session::save key too long");
		if(ds >= 1024 * 1024 * 2)
			throw cppcms_error("session::save value too long");
		key_size=ks;
		exposed = exp ? 1 : 0;
		data_size=ds;
	}
	packed(char const *start,char const *end)
	{
		if(start + 4 <= end ) {
			memcpy(this,start,4);
		}
		else
			throw cppcms_error("session::format violation -> pack");
	}
};

}

void session_interface::save_data(data_type const &data,std::string &s)
{
	s.clear();
	data_type::const_iterator p;
	for(p=data.begin();p!=data.end();++p) {
		packed header(p->first.size(),p->second.exposed,p->second.value.size());
		char *ptr=(char *)&header;
		s.append(ptr,ptr+sizeof(header));
		s.append(p->first.begin(),p->first.end());
		s.append(p->second.value.begin(),p->second.value.end());
	}
}

void session_interface::load_data(data_type &data,std::string const &s)
{
	data.clear();
	char const *begin=s.data(),*end=begin+s.size();
	while(begin < end) {
		packed p(begin,end);
		begin +=sizeof(p);
		if(end - begin >= int(p.key_size + p.data_size)) {
			std::string key(begin,begin+p.key_size);
			begin+=p.key_size;
			std::string val(begin,begin+p.data_size);
			begin+=p.data_size;
			entry &ent=data[key];
			ent.exposed = p.exposed;
			ent.value.swap(val);
		}
		else {
			throw cppcms_error("sessions::format violation data");
		}

	}

}

void session_interface::update_exposed(bool force)
{

	std::set<std::string> removed;
	for(data_type::iterator p=data_.begin();p!=data_.end();++p) {
		data_type::iterator p2=data_copy_.find(p->first);
		if(p->second.exposed && (force || p2==data_copy_.end() || !p2->second.exposed || p->second.value!=p2->second.value)){
			set_session_cookie(cookie_age(),p->second.value,p->first);
		}
		else if(!p->second.exposed && ((p2!=data_copy_.end() && p2->second.exposed) || force)) {
			removed.insert(p->first);
		}
	}
	for(data_type::iterator p=data_copy_.begin();p!=data_copy_.end();++p) {
		if(p->second.exposed && data_.find(p->first)==data_.end()) {
			removed.insert(p->first);
		}
	}

	if(cached_settings().session.cookies.remove_unknown_cookies) {
		std::string prefix = cached_settings().session.cookies.prefix + "_";
		if(!d->adapter) {
			typedef http::request::cookies_type cookies_type;
			cookies_type const &input_cookies = context_->request().cookies();

			for(cookies_type::const_iterator cp=input_cookies.begin();cp!=input_cookies.end();++cp) {
				if(cp->first.compare(0,prefix.size(),prefix)!=0)
					continue;
				std::string key = cp->first.substr(prefix.size());
				if(removed.find(key)!=removed.end())
					continue;
				data_type::iterator ptr;
				if((ptr = data_.find(key))==data_.end() || !ptr->second.exposed) {
					removed.insert(key);
				}
			}
		}
		else {
			std::set<std::string> cookies = d->adapter->get_cookie_names();
			for(std::set<std::string>::const_iterator cp=cookies.begin();cp!=cookies.end();++cp) {
				if(cp->compare(0,prefix.size(),prefix)!=0)
					continue;
				std::string key = cp->substr(prefix.size());
				if(removed.find(key)!=removed.end())
					continue;
				data_type::iterator ptr;
				if((ptr = data_.find(key))==data_.end() || !ptr->second.exposed) {
					removed.insert(key);
				}
			}
		}
	}

	for(std::set<std::string>::const_iterator p=removed.begin();p!=removed.end();++p)
		set_session_cookie(-1,"",*p);

}

std::string session_interface::generate_csrf_token()
{
	unsigned char binary[6];
	unsigned char text[16];
	urandom_device dev;
	dev.generate(binary,sizeof(binary));
	unsigned char *text_begin = text;
	unsigned char *text_end = b64url::encode(binary,binary+sizeof(binary),text_begin);
	return std::string(text_begin,text_end);
}

void session_interface::save()
{
	if(storage_.get()==NULL || !loaded_ || saved_)
		return;
	check();
	new_session_  = (data_copy_.empty() && !data_.empty()) || reset_;
	if(data_.empty()) {
		if(get_session_cookie()!="")
			storage_->clear(*this);
		update_exposed(true);
		return;
	}

	if(new_session_ && cached_settings().security.csrf.enable)
	{
		set("_csrf",generate_csrf_token());
		if(cached_settings().security.csrf.exposed)
			expose("_csrf");
	}

	time_t now = time(NULL);

	bool force_update=false;
	if(data_==data_copy_ && !new_session_) {
		if(how_==fixed) {
			return;
		}
		if(how_==renew || how_==browser) {
			int64_t delta=now + timeout_val_ - timeout_in_;
			if(delta < timeout_val_ * 0.1) {// Less then 10% -- no renew need
				return;
			}
		}
		force_update=true;
	}

	std::string ar;
	save_data(data_,ar);

	temp_cookie_.clear();
	storage_->save(*this,ar,session_age(),new_session_,on_server_);
	set_session_cookie(cookie_age(),temp_cookie_);
	temp_cookie_.clear();

	update_exposed(force_update);
	saved_=true;
}

void session_interface::check()
{
	if(storage_.get()==NULL)
		throw cppcms_error("Session storage backend is not loaded\n");
}

std::string &session_interface::operator[](std::string const &key)
{
	check();
	return data_[key].value;
}

void session_interface::erase(std::string const &key)
{
	check();
	data_.erase(key);
}

bool session_interface::is_set(std::string const &key)
{
	check();
	return data_.find(key)!=data_.end();
}

void session_interface::clear()
{
	check();
	data_.clear();
}

std::set<std::string> session_interface::key_set()
{
	check();
	std::set<std::string> r;
	for(data_type::const_iterator p=data_.begin();p!=data_.end();++p) {
		if(p->first.c_str()[0]=='_')
			continue;
		r.insert(p->first);
	}
	return r;
}

std::string session_interface::get(std::string const &key,std::string const &def)
{
	check();
	data_type::const_iterator p=data_.find(key);
	if(p==data_.end())
		return def;
	return p->second.value;
}

std::string session_interface::get(std::string const &key)
{
	check();
	data_type::const_iterator p=data_.find(key);
	if(p==data_.end())
		throw cppcms_error("Undefined session key "+key);
	return p->second.value;
}

void session_interface::set(std::string const &key,std::string const &v)
{
	check();
	data_[key].value=v;
}

void session_interface::clear_session_cookie()
{
	check();
	if(get_session_cookie()!="")
		set_session_cookie(-1,"");
}

bool session_interface::is_blocking()
{
	return storage_ && storage_->is_blocking();
}

void session_interface::set_session_cookie(int64_t age,std::string const &data,std::string const &key)
{
	if(data.empty())
		age=-1;
	std::string cookie_name=cached_settings().session.cookies.prefix;
	if(!key.empty()) {
		cookie_name+="_";
		cookie_name+=key;
	}
	std::string const &domain = cached_settings().session.cookies.domain;
	std::string const &path   = cached_settings().session.cookies.path;
	int time_shift = cached_settings().session.cookies.time_shift;
	bool use_age = cached_settings().session.cookies.use_age;
	bool use_exp = cached_settings().session.cookies.use_exp;

	bool secure = cached_settings().session.cookies.secure;

	http::cookie the_cookie(cookie_name,util::urlencode(data),path,domain);

	if(age < 0) {
		if(use_age)
			the_cookie.max_age(0);
		if(use_exp)
			the_cookie.expires(1);
	}
	else if(age == 0) {
		the_cookie.browser_age();
	}
	else {
		if(use_age)
			the_cookie.max_age(age);
		if(use_exp) {
			the_cookie.expires(age + time(0) + time_shift);
		}
	}

	the_cookie.secure(secure);

	if(d->adapter)
		d->adapter->set_cookie(the_cookie);
	else
		context_->response().set_cookie(the_cookie);
}

void session_interface::set_session_cookie(std::string const &data)
{
	check();
	temp_cookie_=data;
}

std::string session_interface::session_cookie_name()
{
	return cached_settings().session.cookies.prefix;
}

std::string session_interface::get_session_cookie()
{
	check();
	std::string const &name=cached_settings().session.cookies.prefix;
	if(d->adapter) {
		return d->adapter->get_session_cookie(name);
	}
	else {
		http::request::cookies_type const &cookies = context_->request().cookies();
		http::request::cookies_type::const_iterator p=cookies.find(name);
		if(p==cookies.end())
			return std::string();
		return p->second.value();
	}
}

bool session_interface::is_exposed(std::string const &key)
{
	data_type::iterator p=data_.find(key);
	if(p!=data_.end())
		return p->second.exposed;
	return false;
}

void session_interface::expose(std::string const &key,bool exp)
{
	data_[key].exposed=exp;
}

void session_interface::hide(std::string const &key)
{
	check();
	expose(key,false);
}

void session_interface::age(int t)
{
	check();
	timeout_val_=t;
	set("_t",t);
}

int session_interface::age()
{
	check();
	return timeout_val_;
}

void session_interface::default_age()
{
	check();
	erase("_t");
	timeout_val_=timeout_val_def_;
}

int session_interface::expiration()
{
	check();
	return how_;
}

void session_interface::expiration(int h)
{
	check();
	how_=h;
	set("_h",h);
}
void session_interface::default_expiration()
{
	check();
	erase("_h");
	how_=how_def_;
}

void session_interface::on_server(bool srv)
{
	check();
	on_server_=srv;
	set("_s",int(srv));
}

bool session_interface::on_server()
{
	check();
	return on_server_;
}

void session_interface::reset_session()
{
	reset_ = 1;
}

std::string session_interface::get_csrf_token()
{
	return get("_csrf","");
}

std::string session_interface::get_csrf_token_cookie_name()
{
	return cached_settings().session.cookies.prefix + "__csrf"; // one for suffix and one for _csrf
}

impl::cached_settings const &session_interface::cached_settings()
{
	if(context_)
		return context_->service().cached_settings();
	else
		return d->pool->cached_settings();
}

session_interface_cookie_adapter::~session_interface_cookie_adapter()
{
}

} // cppcms


text:
/*
 * encode_decode_unittest_helper.cpp
 *
 *  Created on: 07-05-2015
 *      Author: Karol Dzitkowski
 */

#include "compression_unittest_base.hpp"
#include "core/cuda_array.hpp"
#include "core/macros.h"

#include "util/copy/cuda_array_copy.hpp"

#include <boost/function.hpp>

namespace ddj
{

template<typename T>
bool CompressionUnittestBase::TestSize(
		boost::function<SharedCudaPtrVector<char> (SharedCudaPtr<T> data)> encodeFunction,
		boost::function<SharedCudaPtr<T> (SharedCudaPtrVector<char> data)> decodeFunction,
		SharedCudaPtr<T> data)
{
	auto encodedData = encodeFunction(data);
	auto decodedData = decodeFunction(encodedData);
	return data->size() == decodedData->size();
}

template<typename T>
bool CompressionUnittestBase::TestContent(
		boost::function<SharedCudaPtrVector<char> (SharedCudaPtr<T> data)> encodeFunction,
		boost::function<SharedCudaPtr<T> (SharedCudaPtrVector<char> data)> decodeFunction,
		SharedCudaPtr<T> data)
{
	auto encodedData = encodeFunction(data);
	auto decodedData = decodeFunction(encodedData);
//
//	HelperPrint::PrintSharedCudaPtr(data->copy(100), "data");
//	HelperPrint::PrintSharedCudaPtr(decodedData->copy(100), "decodedData");
//	printf("Size before compression: %lu bytes\n", data->size()*sizeof(T));
//	printf("Size after compression: %lu bytes\n", CudaArrayCopy().Concatenate(encodedData)->size());

	return CompareDeviceArrays(data->get(), decodedData->get(), data->size());
}

#define COMPRESSION_UNITTEST_BASE_SPEC(X) \
	template bool CompressionUnittestBase::TestSize<X>(								\
			boost::function<SharedCudaPtrVector<char> (SharedCudaPtr<X> data)>,		\
			boost::function<SharedCudaPtr<X> (SharedCudaPtrVector<char> data)>,		\
			SharedCudaPtr<X> data); 												\
	template bool CompressionUnittestBase::TestContent<X>(							\
			boost::function<SharedCudaPtrVector<char> (SharedCudaPtr<X> data)>, 	\
			boost::function<SharedCudaPtr<X> (SharedCudaPtrVector<char> data)>, 	\
			SharedCudaPtr<X> data);
FOR_EACH(COMPRESSION_UNITTEST_BASE_SPEC, char, short, double, float, int, long, long long, unsigned int)

} /* namespace ddj */

ext: cpp

max_stars_repo_licenses: MIT Unlicense

max_issues_repo_licenses: MIT Unlicense

max_forks_repo_licenses: MIT Unlicense


text:
//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under 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)
//
// Official repository: https://github.com/boostorg/beast
//

#ifndef BOOST_BEAST_WEBSOCKET_DETAIL_PRNG_HPP
#define BOOST_BEAST_WEBSOCKET_DETAIL_PRNG_HPP

#include <boost/beast/core/detail/config.hpp>
#include <boost/config.hpp>
#include <cstdint>
#include <random>

namespace boost {
namespace beast {
namespace websocket {
namespace detail {

using generator = std::uint32_t(*)();

//------------------------------------------------------------------------------

// Manually seed the prngs, must be called
// before acquiring a prng for the first time.
//
BOOST_BEAST_DECL
std::uint32_t const*
prng_seed(std::seed_seq* ss = nullptr);

// Acquire a PRNG using the TLS implementation if it
// is available, otherwise using the no-TLS implementation.
//
BOOST_BEAST_DECL
generator
make_prng(bool secure);

} // detail
} // websocket
} // beast
} // boost

#ifdef BOOST_BEAST_HEADER_ONLY
#include <boost/beast/websocket/detail/prng.ipp>
#endif

#endif

ext: hpp

max_stars_repo_name: kindlychung/mediasoup-sfu-cpp

max_issues_repo_name: kindlychung/mediasoup-sfu-cpp

max_forks_repo_name: kindlychung/mediasoup-sfu-cpp


text:
/*=============================================================================
    Copyright (c) 2014 Kohei Takahashi

    Distributed under 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)
==============================================================================*/
#ifndef FUSION_LIST_FORWARD_10262014_0528
#define FUSION_LIST_FORWARD_10262014_0528

#include <boost/config.hpp>
#include <boost/fusion/support/config.hpp>

#if defined(BOOST_NO_CXX11_VARIADIC_TEMPLATES) ||                              \
    (defined(__WAVE__) && defined(BOOST_FUSION_CREATE_PREPROCESSED_FILES))
#if defined(BOOST_FUSION_HAS_VARIADIC_LIST)
#undef BOOST_FUSION_HAS_VARIADIC_LIST
#endif
#else
#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
#define BOOST_FUSION_HAS_VARIADIC_LIST
#endif
#endif

///////////////////////////////////////////////////////////////////////////////
// With no variadics, we will use the C++03 version
///////////////////////////////////////////////////////////////////////////////
#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
#include <boost/fusion/container/list/detail/cpp03/list_fwd.hpp>
#else

///////////////////////////////////////////////////////////////////////////////
// C++11 interface
///////////////////////////////////////////////////////////////////////////////
namespace boost {
namespace fusion {
struct void_;

template <typename... T> struct list;
} // namespace fusion
} // namespace boost

#endif
#endif

max_stars_repo_name: henrywarhurst/matrix

max_issues_repo_name: henrywarhurst/matrix

max_forks_repo_name: henrywarhurst/matrix


text:
#include <ros/ros.h>
#include "ros/package.h"

#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <pcl/point_cloud.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>

#include <ssd_ros_msgs/BoundingBox.h>
#include <ssd_ros_msgs/BoundingBoxArray.h>
#include <ssd_ros_msgs/SSD.h>
#include <ssd_ros_msgs/Cluster.h>
#include <ssd_ros_msgs/ClusterArray.h>

#include <Eigen/Core>

using namespace std;
using namespace Eigen;

typedef pcl::PointXYZRGB PointA;
//typedef pcl::PointXYZINormal PointA;  // 法線情報・反射強度を利用する場合はNormal型に変更
typedef pcl::PointCloud<PointA> CloudA;
typedef pcl::PointCloud<PointA>::Ptr CloudAPtr;

ros::Publisher pub_cluster_centroid;
ros::Publisher pub_cluster_points;
ros::Publisher pub_cluster_centroid_max;
ros::Publisher pub_cluster_points_max;
ros::Publisher pub_cluster_array;

string target_frame = "/zed_left_camera";
string source_frame = "/velodyne";

bool pc_flag = false;

void getClusterInfo(CloudA pt, PointA& cluster)
{
    Vector3f centroid;
	centroid[0]=pt.points[0].x;
	centroid[1]=pt.points[0].y;
	centroid[2]=pt.points[0].z;

	Vector3f min_p;
	min_p[0]=pt.points[0].x;
	min_p[1]=pt.points[0].y;
    min_p[2]=pt.points[0].z;

	Vector3f max_p;
	max_p[0]=pt.points[0].x;
	max_p[1]=pt.points[0].y;
	max_p[2]=pt.points[0].z;

	for(size_t i=1;i<pt.points.size();i++){
        centroid[0]+=pt.points[i].x;
        centroid[1]+=pt.points[i].y;
        centroid[2]+=pt.points[i].z;
		if (pt.points[i].x<min_p[0]) min_p[0]=pt.points[i].x;
		if (pt.points[i].y<min_p[1]) min_p[1]=pt.points[i].y;
        if (pt.points[i].z<min_p[2]) min_p[2]=pt.points[i].z;

		if (pt.points[i].x>max_p[0]) max_p[0]=pt.points[i].x;
		if (pt.points[i].y>max_p[1]) max_p[1]=pt.points[i].y;
		if (pt.points[i].z>max_p[2]) max_p[2]=pt.points[i].z;
    }

	cluster.x=centroid[0]/(float)pt.points.size();
    cluster.y=centroid[1]/(float)pt.points.size();
    cluster.z=centroid[2]/(float)pt.points.size();
	// cluster.normal_x=max_p[0]-min_p[0];
	// cluster.normal_y=max_p[1]-min_p[1];
	// cluster.normal_z=max_p[2];
}

void cpu_clustering(CloudAPtr pcl_in, CloudA& cluster_centroid, CloudA& cluster_pt, CloudA& max_centroid, CloudA& max_pt)
{
    //クラスタリング
    pcl::search::KdTree<PointA>::Ptr tree (new pcl::search::KdTree<PointA>);
    tree->setInputCloud (pcl_in);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointA> ec;
    ec.setClusterTolerance(0.10);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(4000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (pcl_in);
    ec.extract(cluster_indices);

    // size_t max_cluster = 0;
    // size_t max_cluster_size = 0;
    size_t num = 0;

    cluster_centroid.resize(cluster_indices.size());

    // cout<<"size"<<cluster_indices.size()<<endl;
    for(size_t iii=0;iii<cluster_indices.size();iii++){
        CloudAPtr cloud_cluster (new CloudA);
        cloud_cluster->points.resize(cluster_indices[iii].indices.size());
        cluster_pt.points.resize(cluster_indices[iii].indices.size()+num);
        for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
            int p_num=cluster_indices[iii].indices[jjj];
            cloud_cluster->points[jjj]=pcl_in->points[p_num];
            cluster_pt.points[num+jjj]=pcl_in->points[p_num];
        }
        getClusterInfo(*cloud_cluster, cluster_centroid[iii]);

        if(iii==0){
             max_pt.points.resize(cluster_indices[iii].indices.size()+num);
             for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
                 int p_num=cluster_indices[iii].indices[jjj];
                 max_pt.points[num+jjj]=pcl_in->points[p_num];
             }
             max_centroid.resize(1);
             getClusterInfo(*cloud_cluster, max_centroid[iii]);
        }
        num=cluster_pt.points.size();//save previous size
    }
}

void sampling_cpu_clustering(CloudAPtr pcl_in, CloudA& cluster_centroid, CloudA& cluster_pt, CloudA& max_centroid, CloudA& max_pt)
{
    //Downsample//
    pcl::VoxelGrid<PointA> vg;
	CloudAPtr ds_cloud (new CloudA);
	vg.setInputCloud (pcl_in);
	vg.setLeafSize (0.01f, 0.01f, 0.01f);
	vg.filter (*ds_cloud);
    //downsampled point's z =>0
    vector<float> tmp_z;
    tmp_z.resize(ds_cloud->points.size());
	for(int i=0;i<(int)ds_cloud->points.size();i++){
        tmp_z[i]=ds_cloud->points[i].z;
		ds_cloud->points[i].z  = 0.0;
    }
    //Clustering//
    pcl::search::KdTree<PointA>::Ptr tree (new pcl::search::KdTree<PointA>);
    tree->setInputCloud (ds_cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointA> ec;
    ec.setClusterTolerance (0.05); // 15cm
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (4000);
    ec.setSearchMethod (tree);
    ec.setInputCloud(ds_cloud);
    ec.extract (cluster_indices);
    //reset z value
	for(int i=0;i<(int)ds_cloud->points.size();i++)
        ds_cloud->points[i].z=tmp_z[i];
    //get cluster information//
    size_t num=0;
    cluster_centroid.resize(cluster_indices.size());
    for(size_t iii=0;iii<cluster_indices.size();iii++){
        //cluster cenrtroid
        CloudAPtr cloud_cluster (new CloudA);
        cloud_cluster->points.resize(cluster_indices[iii].indices.size());
        //cluster points
        cluster_pt.points.resize(cluster_indices[iii].indices.size()+num);
        for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
            int p_num=cluster_indices[iii].indices[jjj];
            cloud_cluster->points[jjj]=ds_cloud->points[p_num];
            cluster_pt.points[num+jjj]=ds_cloud->points[p_num];
        }
        getClusterInfo(*cloud_cluster, cluster_centroid[iii]);
        if(iii==0){
            max_pt.points.resize(cluster_indices[iii].indices.size()+num);
            for(size_t jjj=0;jjj<cluster_indices[iii].indices.size();jjj++){
                int p_num=cluster_indices[iii].indices[jjj];
                max_pt.points[num+jjj]=ds_cloud->points[p_num];
            }
            max_centroid.resize(1);
            getClusterInfo(*cloud_cluster, max_centroid[iii]);
        }
        num=cluster_pt.points.size();//save previous size
    }
}

void cluster(const ssd_ros_msgs::SSDConstPtr& pc_msg)
{
    CloudAPtr pcl_cluster_points;
    CloudA pcl_cluster_centroid;
    CloudA pcl_max_centroid;
    CloudA pcl_max_points;

    ssd_ros_msgs::ClusterArray ssd_clusters;

    int grobal_count = 0;

    for(size_t i=0;i<pc_msg->ssd_array.size();i++)
    {
        int local_count = 0;
        const sensor_msgs::PointCloud2 &pc2_people = pc_msg->ssd_array[i];
        // printf("Num:%d size:%d\n", i+1, pc2_people.width);

        CloudAPtr pcl_people (new CloudA);
        CloudA cluster_points;
        CloudA cluster_centroid;
        CloudA max_centroid;
        CloudA max_points;

        // ssd_ros_msgs::Cluster ssd_cluster;

        pcl::fromROSMsg(pc2_people, *pcl_people);

        //cpu_clustering(pcl_people, cluster_centroid, cluster_points, max_centroid, max_points);
        cout<<"cpu_cluster"<<endl;
		sampling_cpu_clustering(pcl_people, cluster_centroid, cluster_points, max_centroid, max_points);
        cout<<"finish"<<endl;

		// for(size_t j=0;j<cluster_centroid.points.size();j++){
        //     PointA p = cluster_centroid.points[j];
        //     pcl_cluster_centroid.push_back(p);
        //     local_count++;
        //     grobal_count++;
        // }
        // for(size_t k=0;k<cluster_points.points.size();k++){
        //     PointA q = cluster_points.points[k];
        //     pcl_cluster_points->push_back(q);
        // }
		// if(0<pcl_cluster_points->points.size()){
		// 	boundingbox_array.push_back<CloudAPtr&>(pcl_cluster_points);
		// }
		cout<<"max"<<endl;
        for(size_t l=0;l<max_centroid.points.size();l++){
            PointA p = max_centroid.points[l];
            pcl_max_centroid.push_back(p);
        }
        for(size_t m=0;m<max_points.points.size();m++){
            PointA q = max_points.points[m];
            pcl_max_points.push_back(q);
        }

		cout<<"nest"<<endl;

        if(0<max_points.points.size()){
            ssd_ros_msgs::Cluster ssd_cluster;
            CloudA ssd_centroid;
            CloudA ssd_points;

            for(size_t l=0;l<max_centroid.points.size();l++){
                PointA p = max_centroid.points[l];
                ssd_centroid.push_back(p);
            }
            for(size_t m=0;m<max_points.points.size();m++){
                PointA q = max_points.points[m];
                ssd_points.push_back(q);
            }
            toROSMsg(ssd_centroid, ssd_cluster.centroid);
            toROSMsg(ssd_points, ssd_cluster.points);
            ssd_cluster.centroid.header.frame_id = target_frame;
            ssd_cluster.centroid.header.stamp = ros::Time::now();
            ssd_cluster.points.header.frame_id = target_frame;
            ssd_cluster.points.header.stamp = ros::Time::now();
            ssd_clusters.clusters.push_back(ssd_cluster);
        }

        // if(0<max_points.points.size()){
        //     toROSMsg(pcl_max_centroid, ssd_cluster.centroid);
        //     toROSMsg(pcl_max_points, ssd_cluster.points);
        //     ssd_cluster.centroid.header.frame_id = target_frame;
        //     ssd_cluster.centroid.header.stamp = ros::Time::now();
        //     ssd_cluster.points.header.frame_id = target_frame;
        //     ssd_cluster.points.header.stamp = ros::Time::now();
        //     ssd_clusters.clusters.push_back(ssd_cluster);
        // }

        printf("Array Num:%d Size:%d Class:%d\n",(int)i+1, (int)pc2_people.width, local_count);
    }

    printf("Total Class:%d\n", grobal_count);
    printf("Array size:%d\n", (int)ssd_clusters.clusters.size());

    // sensor_msgs::PointCloud2 cluster_centroid_ros;
    // toROSMsg(pcl_cluster_centroid, cluster_centroid_ros);
    // cluster_centroid_ros.header.frame_id=target_frame;
    // cluster_centroid_ros.header.stamp=ros::Time::now();
    // pub_cluster_centroid.publish(cluster_centroid_ros);

    // sensor_msgs::PointCloud2 cluster_points_ros;
    // toROSMsg(*pcl_cluster_points, cluster_points_ros);
    // cluster_points_ros.header.frame_id=target_frame;
    // cluster_points_ros.header.stamp=ros::Time::now();
    // pub_cluster_points.publish(cluster_points_ros);

    // boundingbox_array.publish();

    sensor_msgs::PointCloud2 max_centroid_ros;
    toROSMsg(pcl_max_centroid, max_centroid_ros);
    max_centroid_ros.header.frame_id = target_frame;
    pub_cluster_centroid_max.publish(max_centroid_ros);

    sensor_msgs::PointCloud2 max_points_ros;
    toROSMsg(pcl_max_points,max_points_ros);
    max_points_ros.header.frame_id = target_frame;
    pub_cluster_points_max.publish(max_points_ros);

    pub_cluster_array.publish(ssd_clusters);

}

ssd_ros_msgs::SSDConstPtr pc_;
void pcCallback(const ssd_ros_msgs::SSDConstPtr& msg)
{
    // printf("Human Count : %d\n", msg->ssd_array.size());
    pc_ = msg;
    pc_flag = true;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "ssd_cluster");
    ros::NodeHandle n;

    ros::Subscriber pc_sub = n.subscribe("/SSD/people",10,pcCallback);

    pub_cluster_centroid=n.advertise<sensor_msgs::PointCloud2>("/cluster/centroid",1);
    pub_cluster_points=n.advertise<sensor_msgs::PointCloud2>("/cluster/points",1);
    pub_cluster_centroid_max=n.advertise<sensor_msgs::PointCloud2>("/cluster/centroid/max",1);
    pub_cluster_points_max=n.advertise<sensor_msgs::PointCloud2>("/cluster/points/max",1);
    pub_cluster_array = n.advertise<ssd_ros_msgs::ClusterArray>("/cluster/array/ssd", 1);

    ros::Rate rate(20);

    while(ros::ok())
    {
        if(pc_flag) cluster(pc_);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}


text:
#include <algorithm>
#include <vector>

#include <Eigen/Core>

#include <aslam/common/memory.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <posegraph/pose-graph.h>
#include <posegraph/vertex.h>

#include "vi-map/check-map-consistency.h"
#include "vi-map/vi-map.h"

namespace vi_map {

class MapConsistencyCheckTest : public ::testing::Test {
 protected:
  MapConsistencyCheckTest() {}
  virtual void SetUp() {
    cameras_ = aslam::NCamera::createTestNCamera(kNumCameras);

    common::generateId(&mission_id1_);
    populatePosegraph(mission_id1_, &vertex_ids_mission_1_);
  }

  void populatePosegraph(
      const MissionId& mission_id,
      std::vector<pose_graph::VertexId>* vertex_ids);
  void addVisualFrameToVertex(
      const aslam::NCamera::Ptr& n_camera_ptr, vi_map::Vertex* vertex_ptr);
  void addCommonLandmarkIds(
      unsigned int count, const pose_graph::VertexId& vertex0_id,
      const pose_graph::VertexId& vertex1_id);
  void addInvalidEdge();
  void addOrphanedTrajectory();
  void addOrphanedVertex();
  void addMissionWithVisualNFrame();
  void addLandmarkAndVertexReference(
      const vi_map::LandmarkId& global_landmark_id,
      const pose_graph::VertexId& storing_vertex_id);
  void addMissionUsingAnExistingNCamera(const aslam::NCamera::Ptr& n_cameras);
  void addSecondMission();
  void addLoopClosureEdges();

  std::vector<pose_graph::VertexId> vertex_ids_mission_1_;
  std::vector<pose_graph::VertexId> vertex_ids_mission_2_;
  VIMap map_;
  vi_map::MissionId mission_id1_;
  vi_map::MissionId mission_id2_;
  aslam::NCamera::Ptr cameras_;

  static constexpr unsigned int kNumOfVertices = 4;
  static constexpr unsigned int kNumCameras = 1;
  static constexpr unsigned int kVisualFrameIndex = 0;
  static constexpr unsigned int kNumOfKeypointsPerVertex = 500;
  static constexpr int kDescriptorBytes = 48;

 private:
  void createMission(
      const pose_graph::VertexId& root_vertex_id,
      const vi_map::MissionId& mission_id,
      const aslam::NCamera::Ptr& n_camera_ptr);
};

void MapConsistencyCheckTest::addLoopClosureEdges() {
  common::generateId(&mission_id2_);
  populatePosegraph(mission_id2_, &vertex_ids_mission_2_);

  constexpr double kSwitchVariable = 1.0;
  constexpr double kSwitchVariableVariance = 1.0;
  const aslam::TransformationCovariance kT_A_B_covariance =
      aslam::TransformationCovariance::Identity();

  for (const pose_graph::VertexId& source_vertex_id_mission_1 :
       vertex_ids_mission_1_) {
    for (const pose_graph::VertexId& destination_vertex_id_mission_1 :
         vertex_ids_mission_1_) {
      if (source_vertex_id_mission_1 == destination_vertex_id_mission_1) {
        continue;
      }
      pose_graph::EdgeId edge_id;
      common::generateId(&edge_id);
      const aslam::Transformation T_G_I_source =
          map_.getVertex_T_G_I(source_vertex_id_mission_1);
      const aslam::Transformation T_G_I_dest =
          map_.getVertex_T_G_I(destination_vertex_id_mission_1);
      const aslam::Transformation T_A_B = T_G_I_source.inverse() * T_G_I_dest;

      LoopClosureEdge::UniquePtr loop_closure_edge =
          aligned_unique<LoopClosureEdge>(
              edge_id, source_vertex_id_mission_1,
              destination_vertex_id_mission_1, kSwitchVariable,
              kSwitchVariableVariance, T_A_B, kT_A_B_covariance);
      map_.addEdge(std::move(loop_closure_edge));
    }

    for (const pose_graph::VertexId& destination_vertex_id_mission_2 :
         vertex_ids_mission_2_) {
      pose_graph::EdgeId edge_id;
      common::generateId(&edge_id);
      const aslam::Transformation T_G_I_source =
          map_.getVertex_T_G_I(source_vertex_id_mission_1);
      const aslam::Transformation T_G_I_dest =
          map_.getVertex_T_G_I(destination_vertex_id_mission_2);
      const aslam::Transformation T_A_B = T_G_I_source.inverse() * T_G_I_dest;

      LoopClosureEdge::UniquePtr loop_closure_edge =
          aligned_unique<LoopClosureEdge>(
              edge_id, source_vertex_id_mission_1,
              destination_vertex_id_mission_2, kSwitchVariable,
              kSwitchVariableVariance, T_A_B, kT_A_B_covariance);
      map_.addEdge(std::move(loop_closure_edge));
    }
  }

  for (const pose_graph::VertexId& source_vertex_id_mission_2 :
       vertex_ids_mission_2_) {
    for (const pose_graph::VertexId& destination_vertex_id_mission_2 :
         vertex_ids_mission_2_) {
      if (source_vertex_id_mission_2 == destination_vertex_id_mission_2) {
        continue;
      }
      pose_graph::EdgeId edge_id;
      common::generateId(&edge_id);
      const aslam::Transformation T_G_I_source =
          map_.getVertex_T_G_I(source_vertex_id_mission_2);
      const aslam::Transformation T_G_I_dest =
          map_.getVertex_T_G_I(destination_vertex_id_mission_2);
      const aslam::Transformation T_A_B = T_G_I_source.inverse() * T_G_I_dest;

      LoopClosureEdge::UniquePtr loop_closure_edge =
          aligned_unique<LoopClosureEdge>(
              edge_id, source_vertex_id_mission_2,
              destination_vertex_id_mission_2, kSwitchVariable,
              kSwitchVariableVariance, T_A_B, kT_A_B_covariance);
      map_.addEdge(std::move(loop_closure_edge));
    }

    for (const pose_graph::VertexId& destination_vertex_id_mission_1 :
         vertex_ids_mission_1_) {
      pose_graph::EdgeId edge_id;
      common::generateId(&edge_id);
      const aslam::Transformation T_G_I_source =
          map_.getVertex_T_G_I(source_vertex_id_mission_2);
      const aslam::Transformation T_G_I_dest =
          map_.getVertex_T_G_I(destination_vertex_id_mission_1);
      const aslam::Transformation T_A_B = T_G_I_source.inverse() * T_G_I_dest;

      LoopClosureEdge::UniquePtr loop_closure_edge =
          aligned_unique<LoopClosureEdge>(
              edge_id, source_vertex_id_mission_2,
              destination_vertex_id_mission_1, kSwitchVariable,
              kSwitchVariableVariance, T_A_B, kT_A_B_covariance);
      map_.addEdge(std::move(loop_closure_edge));
    }
  }
}

// Orphaned since not connected to the graph already attached to mission_id.
void MapConsistencyCheckTest::addOrphanedTrajectory() {
  pose_graph::VertexId vertex_id_0;
  pose_graph::VertexId vertex_id_1;
  generateId(&vertex_id_0);
  generateId(&vertex_id_1);
  vi_map::Vertex* vertex_0(new vi_map::Vertex(cameras_));
  vi_map::Vertex* vertex_1(new vi_map::Vertex(cameras_));
  vertex_0->setId(vertex_id_0);
  vertex_1->setId(vertex_id_1);
  vertex_0->setMissionId(mission_id1_);
  vertex_1->setMissionId(mission_id1_);
  map_.addVertex(vi_map::Vertex::UniquePtr(vertex_0));
  map_.addVertex(vi_map::Vertex::UniquePtr(vertex_1));

  pose_graph::EdgeId edge_id;
  generateId(&edge_id);
  Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps;
  Eigen::Matrix<double, 6, Eigen::Dynamic> imu_data;

  vi_map::Edge* edge(
      new vi_map::ViwlsEdge(
          edge_id, vertex_id_0, vertex_id_1, imu_timestamps, imu_data));
  map_.addEdge(vi_map::Edge::UniquePtr(edge));
}

// Orphaned since not connected to the graph already attached to mission_id.
void MapConsistencyCheckTest::addOrphanedVertex() {
  pose_graph::VertexId vertex_id;
  generateId(&vertex_id);
  vi_map::Vertex* vertex(new vi_map::Vertex(cameras_));
  vertex->setId(vertex_id);
  vertex->setMissionId(mission_id1_);
  map_.addVertex(vi_map::Vertex::UniquePtr(vertex));
}

void MapConsistencyCheckTest::addInvalidEdge() {
  pose_graph::EdgeId edge_id;
  generateId(&edge_id);
  Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps;
  Eigen::Matrix<double, 6, Eigen::Dynamic> imu_data;
  vi_map::Edge* edge(
      new vi_map::ViwlsEdge(
          edge_id, vertex_ids_mission_1_[2], vertex_ids_mission_1_[0],
          imu_timestamps, imu_data));
  map_.addEdge(vi_map::Edge::UniquePtr(edge));
}

void MapConsistencyCheckTest::addSecondMission() {}

void MapConsistencyCheckTest::populatePosegraph(
    const MissionId& mission_id,
    std::vector<pose_graph::VertexId>* vertex_ids) {
  CHECK_NOTNULL(vertex_ids)->clear();
  CHECK(mission_id.isValid());
  pose_graph::VertexId vertex_id;
  vi_map::Vertex* vertex(new vi_map::Vertex(cameras_));
  addVisualFrameToVertex(cameras_, vertex);
  vertex_ids->resize(kNumOfVertices);

  generateId(&vertex_id);
  // Creates mission and sets root vertex id.
  createMission(vertex_id, mission_id, cameras_);

  vertex->setId(vertex_id);
  vertex->setMissionId(mission_id);
  VLOG(3) << "Add vertex " << vertex_id.hexString();
  (*vertex_ids)[0] = vertex_id;
  map_.addVertex(vi_map::Vertex::UniquePtr(vertex));

  for (unsigned int i = 1; i < kNumOfVertices; ++i) {
    vi_map::Vertex* vertex(new vi_map::Vertex(cameras_));
    addVisualFrameToVertex(cameras_, vertex);
    generateId(&vertex_id);
    vertex->setId(vertex_id);
    vertex->setMissionId(mission_id);
    VLOG(3) << "Add vertex " << vertex_id.hexString();
    map_.addVertex(vi_map::Vertex::UniquePtr(vertex));
    (*vertex_ids)[i] = vertex_id;

    pose_graph::EdgeId edge_id;
    generateId(&edge_id);
    Eigen::Matrix<int64_t, 1, Eigen::Dynamic> imu_timestamps;
    Eigen::Matrix<double, 6, Eigen::Dynamic> imu_data;
    vi_map::Edge* edge(
        new vi_map::ViwlsEdge(
            edge_id, (*vertex_ids)[i - 1], (*vertex_ids)[i], imu_timestamps,
            imu_data));
    map_.addEdge(vi_map::Edge::UniquePtr(edge));

    addCommonLandmarkIds(5, (*vertex_ids)[i], (*vertex_ids)[i - 1]);
  }
}

void MapConsistencyCheckTest::addVisualFrameToVertex(
    const aslam::NCamera::Ptr& n_camera_ptr, vi_map::Vertex* vertex_ptr) {
  CHECK(vertex_ptr != nullptr);

  const unsigned int num_cameras = n_camera_ptr->numCameras();

  for (unsigned int j = 0; j < num_cameras; ++j) {
    aslam::VisualFrame::Ptr frame(new aslam::VisualFrame);
    Eigen::Matrix2Xd img_points_distorted;
    Eigen::VectorXd uncertainties;
    aslam::VisualFrame::DescriptorsT descriptors;
    img_points_distorted.resize(Eigen::NoChange, kNumOfKeypointsPerVertex);
    uncertainties.resize(kNumOfKeypointsPerVertex);
    descriptors.resize(kDescriptorBytes, kNumOfKeypointsPerVertex);
    aslam::FrameId frame_id;
    common::generateId(&frame_id);
    frame->setId(frame_id);
    frame->setKeypointMeasurements(img_points_distorted);
    frame->setKeypointMeasurementUncertainties(uncertainties);
    frame->setDescriptors(descriptors);
    frame->setCameraGeometry(n_camera_ptr->getCameraShared(j));
    vertex_ptr->getVisualNFrame().setFrame(j, frame);

    LOG(INFO) << "Add global landmark ids to frame: " << j;
    for (unsigned int i = 0; i < kNumOfKeypointsPerVertex; ++i) {
      vertex_ptr->addObservedLandmarkId(j, vi_map::LandmarkId());
    }
  }
}

void MapConsistencyCheckTest::createMission(
    const pose_graph::VertexId& root_vertex_id,
    const vi_map::MissionId& mission_id,
    const aslam::NCamera::Ptr& n_camera_ptr) {
  vi_map::MissionBaseFrame baseframe;
  vi_map::MissionBaseFrameId baseframe_id;
  common::generateId(&baseframe_id);
  baseframe.setId(baseframe_id);

  vi_map::VIMission* mission_ptr(new vi_map::VIMission);

  mission_ptr->setId(mission_id);
  mission_ptr->setBaseFrameId(baseframe_id);

  mission_ptr->setRootVertexId(root_vertex_id);
  map_.missions.emplace(
      mission_ptr->id(), vi_map::VIMission::UniquePtr(mission_ptr));
  map_.mission_base_frames.emplace(baseframe.id(), baseframe);
  map_.sensor_manager_.addNCamera(n_camera_ptr, mission_id);
}

void MapConsistencyCheckTest::addLandmarkAndVertexReference(
    const vi_map::LandmarkId& landmark_id,
    const pose_graph::VertexId& storing_vertex_id) {
  map_.addLandmarkIndexReference(landmark_id, storing_vertex_id);
}

void MapConsistencyCheckTest::addCommonLandmarkIds(
    unsigned int count, const pose_graph::VertexId& vertex0_id,
    const pose_graph::VertexId& vertex1_id) {
  vi_map::Vertex& vertex0 = map_.getVertex(vertex0_id);
  vi_map::Vertex& vertex1 = map_.getVertex(vertex1_id);

  vi_map::LandmarkStore& landmark_store = vertex0.getLandmarks();

  for (unsigned int i = 0; i < count; ++i) {
    vi_map::LandmarkId landmark_id;
    generateId(&landmark_id);

    // Set every 10th GlobalLandmarkId in a vertex to a valid ID.
    const unsigned int keypoint_index0 =
        10 * vertex0.numValidObservedLandmarkIds(kVisualFrameIndex);
    const unsigned int keypoint_index1 =
        10 * vertex1.numValidObservedLandmarkIds(kVisualFrameIndex);

    vi_map::Landmark landmark;
    landmark.setId(landmark_id);
    landmark.addObservation(vertex0_id, kVisualFrameIndex, keypoint_index0);
    landmark.addObservation(vertex1_id, kVisualFrameIndex, keypoint_index1);
    landmark_store.addLandmark(landmark);

    CHECK_LT(
        keypoint_index0, vertex1.observedLandmarkIdsSize(kVisualFrameIndex));
    CHECK_LT(
        keypoint_index1, vertex0.observedLandmarkIdsSize(kVisualFrameIndex));
    vertex0.setObservedLandmarkId(
        kVisualFrameIndex, keypoint_index0, landmark_id);
    vertex1.setObservedLandmarkId(
        kVisualFrameIndex, keypoint_index1, landmark_id);

    map_.addLandmarkIndexReference(landmark_id, vertex0_id);

    VLOG(3) << "Add landmark " << landmark_id.hexString() << " to "
            << vertex0_id.hexString() << " and " << vertex1_id.hexString()
            << " pos " << keypoint_index0 << " and " << keypoint_index1;
  }
}

void MapConsistencyCheckTest::addMissionWithVisualNFrame() {
  static constexpr unsigned int kNumCamerasInMultiframeMission = 5;
  aslam::NCamera::Ptr n_cameras =
      aslam::NCamera::createTestNCamera(kNumCamerasInMultiframeMission);

  pose_graph::VertexId vertex_id;
  vi_map::Vertex* vertex(new vi_map::Vertex(n_cameras));

  common::generateId(&mission_id1_);
  common::generateId(&vertex_id);
  // Creates mission and sets root vertex id.
  createMission(vertex_id, mission_id1_, n_cameras);

  vertex->setId(vertex_id);
  vertex->setMissionId(mission_id1_);
  addVisualFrameToVertex(n_cameras, vertex);
  map_.addVertex(vi_map::Vertex::UniquePtr(vertex));
}

void MapConsistencyCheckTest::addMissionUsingAnExistingNCamera(
    const aslam::NCamera::Ptr& n_cameras) {
  CHECK(n_cameras);
  CHECK(map_.getSensorManager().hasSensor(n_cameras->getId()));

  pose_graph::VertexId vertex_id;
  vi_map::Vertex* vertex(new vi_map::Vertex(n_cameras));

  common::generateId(&mission_id1_);
  common::generateId(&vertex_id);
  // Creates mission and sets root vertex id.
  createMission(vertex_id, mission_id1_, n_cameras);

  vertex->setId(vertex_id);
  vertex->setMissionId(mission_id1_);
  addVisualFrameToVertex(n_cameras, vertex);
  map_.addVertex(vi_map::Vertex::UniquePtr(vertex));
}

TEST_F(MapConsistencyCheckTest, mapConsistencySingleVisualFrame) {
  EXPECT_TRUE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, mapConsistencyMultiVisualFrame) {
  addMissionWithVisualNFrame();
  EXPECT_TRUE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, mapInconsistentMissingBackLink) {
  vi_map::Vertex& vertex0 = map_.getVertex(vertex_ids_mission_1_[2]);
  vi_map::LandmarkStore& landmark_store = vertex0.getLandmarks();
  ASSERT_GT(landmark_store.size(), 0);

  vi_map::LandmarkId landmark_id;
  generateId(&landmark_id);

  vi_map::Landmark landmark;
  landmark.setId(landmark_id);
  landmark_store.addLandmark(landmark);
  addLandmarkAndVertexReference(landmark_id, vertex_ids_mission_1_[2]);

  const unsigned int keypoint_index =
      vertex0.numValidObservedLandmarkIds(kVisualFrameIndex);
  vertex0.setObservedLandmarkId(kVisualFrameIndex, keypoint_index, landmark_id);

  EXPECT_FALSE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, mapInconsistentPosegraphInvalidEdge) {
  addInvalidEdge();
  EXPECT_FALSE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, mapInconsistentPosegraphOrphanedTrajectory) {
  addOrphanedTrajectory();
  EXPECT_FALSE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, mapInconsistentPosegraphOrphanedVertex) {
  addOrphanedVertex();
  EXPECT_FALSE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, mapDuplicatedMissionNCameraReference) {
  addMissionUsingAnExistingNCamera(cameras_);
  EXPECT_TRUE(vi_map::checkMapConsistency(map_));
}

TEST_F(MapConsistencyCheckTest, MapConsistencyLoopClosureEdges) {
  addMissionWithVisualNFrame();
  EXPECT_TRUE(vi_map::checkMapConsistency(map_));
  addLoopClosureEdges();
  EXPECT_TRUE(vi_map::checkMapConsistency(map_));
}

}  // namespace vi_map

MAPLAB_UNITTEST_ENTRYPOINT

ext: cc

max_stars_repo_name: AdronTech/maplab

max_issues_repo_name: AdronTech/maplab

max_forks_repo_name: AdronTech/maplab


text:
#ifndef BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP

// Boost.Geometry - extensions-gis-projections (based on PROJ4)
// This file is automatically generated. DO NOT EDIT.

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 4.9.1

// Original copyright notice:

// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#include <boost/geometry/extensions/gis/projections/impl/base_static.hpp>
#include <boost/geometry/extensions/gis/projections/impl/base_dynamic.hpp>
#include <boost/geometry/extensions/gis/projections/impl/projects.hpp>
#include <boost/geometry/extensions/gis/projections/impl/factory_entry.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_phi2.hpp>
#include <boost/geometry/extensions/gis/projections/impl/pj_tsfn.hpp>

namespace boost { namespace geometry { namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace gstmerc
    {

            struct par_gstmerc
            {
                double lamc;
                double phic;
                double c;
                double n1;
                double n2;
                double XS;
                double YS;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename Geographic, typename Cartesian, typename Parameters>
            struct base_gstmerc_spheroid : public base_t_fi<base_gstmerc_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>
            {

                 typedef double geographic_type;
                 typedef double cartesian_type;

                par_gstmerc m_proj_parm;

                inline base_gstmerc_spheroid(const Parameters& par)
                    : base_t_fi<base_gstmerc_spheroid<Geographic, Cartesian, Parameters>,
                     Geographic, Cartesian, Parameters>(*this, par) {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(geographic_type& lp_lon, geographic_type& lp_lat, cartesian_type& xy_x, cartesian_type& xy_y) const
                {
                    double L, Ls, sinLs1, Ls1;
                    L= this->m_proj_parm.n1*lp_lon;
                    Ls= this->m_proj_parm.c+this->m_proj_parm.n1*log(pj_tsfn(-1.0*lp_lat,-1.0*sin(lp_lat),this->m_par.e));
                    sinLs1= sin(L)/cosh(Ls);
                    Ls1= log(pj_tsfn(-1.0*asin(sinLs1),0.0,0.0));
                    xy_x= (this->m_proj_parm.XS + this->m_proj_parm.n2*Ls1)*this->m_par.ra;
                    xy_y= (this->m_proj_parm.YS + this->m_proj_parm.n2*atan(sinh(Ls)/cos(L)))*this->m_par.ra;
                    /*fprintf(stderr,"fwd:\nL      =%16.13f\nLs     =%16.13f\nLs1    =%16.13f\nLP(%16.13f,%16.13f)=XY(%16.4f,%16.4f)\n",L,Ls,Ls1,lp_lon+this->m_par.lam0,lp_lat,(xy_x*this->m_par.a + this->m_par.x0)*this->m_par.to_meter,(xy_y*this->m_par.a + this->m_par.y0)*this->m_par.to_meter);*/
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(cartesian_type& xy_x, cartesian_type& xy_y, geographic_type& lp_lon, geographic_type& lp_lat) const
                {
                    double L, LC, sinC;
                    L= atan(sinh((xy_x*this->m_par.a - this->m_proj_parm.XS)/this->m_proj_parm.n2)/cos((xy_y*this->m_par.a - this->m_proj_parm.YS)/this->m_proj_parm.n2));
                    sinC= sin((xy_y*this->m_par.a - this->m_proj_parm.YS)/this->m_proj_parm.n2)/cosh((xy_x*this->m_par.a - this->m_proj_parm.XS)/this->m_proj_parm.n2);
                    LC= log(pj_tsfn(-1.0*asin(sinC),0.0,0.0));
                    lp_lon= L/this->m_proj_parm.n1;
                    lp_lat= -1.0*pj_phi2(exp((LC-this->m_proj_parm.c)/this->m_proj_parm.n1),this->m_par.e);
                    /*fprintf(stderr,"inv:\nL      =%16.13f\nsinC   =%16.13f\nLC     =%16.13f\nXY(%16.4f,%16.4f)=LP(%16.13f,%16.13f)\n",L,sinC,LC,((xy_x/this->m_par.ra)+this->m_par.x0)/this->m_par.to_meter,((xy_y/this->m_par.ra)+this->m_par.y0)/this->m_par.to_meter,lp_lon+this->m_par.lam0,lp_lat);*/
                }

                static inline std::string get_name()
                {
                    return "gstmerc_spheroid";
                }

            };

            // Gauss-Schreiber Transverse Mercator (aka Gauss-Laborde Reunion)
            template <typename Parameters>
            void setup_gstmerc(Parameters& par, par_gstmerc& proj_parm)
            {
                proj_parm.lamc= par.lam0;
                proj_parm.n1= sqrt(1.0+par.es*pow(cos(par.phi0),4.0)/(1.0-par.es));
                proj_parm.phic= asin(sin(par.phi0)/proj_parm.n1);
                proj_parm.c=       log(pj_tsfn(-1.0*proj_parm.phic,0.0,0.0))
                     -proj_parm.n1*log(pj_tsfn(-1.0*par.phi0,-1.0*sin(par.phi0),par.e));
                proj_parm.n2= par.k0*par.a*sqrt(1.0-par.es)/(1.0-par.es*sin(par.phi0)*sin(par.phi0));
                proj_parm.XS= 0;/* -par.x0 */
                proj_parm.YS= -1.0*proj_parm.n2*proj_parm.phic;/* -par.y0 */
                /*fprintf(stderr,"a  (m) =%16.4f\ne      =%16.13f\nl0(rad)=%16.13f\np0(rad)=%16.13f\nk0     =%16.4f\nX0  (m)=%16.4f\nY0  (m)=%16.4f\n\nlC(rad)=%16.13f\npC(rad)=%16.13f\nc      =%16.13f\nn1     =%16.13f\nn2 (m) =%16.4f\nXS (m) =%16.4f\nYS (m) =%16.4f\n", par.a, par.e, par.lam0, par.phi0, par.k0, par.x0, par.y0, proj_parm.lamc, proj_parm.phic, proj_parm.c, proj_parm.n1, proj_parm.n2, proj_parm.XS +par.x0, proj_parm.YS + par.y0);*/
            }

        }} // namespace detail::gstmerc
    #endif // doxygen

    /*!
        \brief Gauss-Schreiber Transverse Mercator (aka Gauss-Laborde Reunion) projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Cylindrical
         - Spheroid
         - Ellipsoid
        \par Projection parameters
         - lat_0: Latitude of origin
         - lon_0: Central meridian
         - k_0: Scale factor
        \par Example
        \image html ex_gstmerc.gif
    */
    template <typename Geographic, typename Cartesian, typename Parameters = parameters>
    struct gstmerc_spheroid : public detail::gstmerc::base_gstmerc_spheroid<Geographic, Cartesian, Parameters>
    {
        inline gstmerc_spheroid(const Parameters& par) : detail::gstmerc::base_gstmerc_spheroid<Geographic, Cartesian, Parameters>(par)
        {
            detail::gstmerc::setup_gstmerc(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Factory entry(s)
        template <typename Geographic, typename Cartesian, typename Parameters>
        class gstmerc_entry : public detail::factory_entry<Geographic, Cartesian, Parameters>
        {
            public :
                virtual projection<Geographic, Cartesian>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<gstmerc_spheroid<Geographic, Cartesian, Parameters>, Geographic, Cartesian, Parameters>(par);
                }
        };

        template <typename Geographic, typename Cartesian, typename Parameters>
        inline void gstmerc_init(detail::base_factory<Geographic, Cartesian, Parameters>& factory)
        {
            factory.add_to_factory("gstmerc", new gstmerc_entry<Geographic, Cartesian, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

}}} // namespace boost::geometry::projections

#endif // BOOST_GEOMETRY_PROJECTIONS_GSTMERC_HPP

max_stars_repo_name: bowlofstew/omim

max_issues_repo_name: bowlofstew/omim

max_forks_repo_name: bowlofstew/omim


text:
#include <signal.h>
#include <thrift/protocol/TBinaryProtocol.h>
#include <thrift/server/TThreadedServer.h>
#include <thrift/transport/TBufferTransports.h>
#include <thrift/transport/TServerSocket.h>

#include <boost/program_options.hpp>

#include "../utils.h"
#include "../utils_mongodb.h"
#include "../utils_redis.h"
#include "../utils_thrift.h"
#include "SocialGraphHandler.h"

using json = nlohmann::json;
using apache::thrift::protocol::TBinaryProtocolFactory;
using apache::thrift::server::TThreadedServer;
using apache::thrift::transport::TFramedTransportFactory;
using apache::thrift::transport::TServerSocket;
using namespace social_network;

void sigintHandler(int sig) { exit(EXIT_SUCCESS); }

int main(int argc, char *argv[]) {
  signal(SIGINT, sigintHandler);
  init_logger();

  // Command line options
  namespace po = boost::program_options;
  po::options_description desc("Options");
  desc.add_options()("help", "produce help message")(
      "redis-cluster",
      po::value<bool>()->default_value(false)->implicit_value(true),
      "Enable redis cluster mode");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 0;
  }

  bool redis_cluster_flag = false;
  if (vm.count("redis-cluster")) {
    if (vm["redis-cluster"].as<bool>()) {
      redis_cluster_flag = true;
    }
  }

  SetUpTracer("config/jaeger-config.yml", "social-graph-service");

  json config_json;
  if (load_config_file("config/service-config.json", &config_json) != 0) {
    exit(EXIT_FAILURE);
  }

  int port = config_json["social-graph-service"]["port"];

  int mongodb_conns = config_json["social-graph-mongodb"]["connections"];
  int mongodb_timeout = config_json["social-graph-mongodb"]["timeout_ms"];

  std::string user_addr = config_json["user-service"]["addr"];
  int user_port = config_json["user-service"]["port"];
  int user_conns = config_json["user-service"]["connections"];
  int user_timeout = config_json["user-service"]["timeout_ms"];
  int user_keepalive = config_json["user-service"]["keepalive_ms"];

  mongoc_client_pool_t *mongodb_client_pool =
      init_mongodb_client_pool(config_json, "social-graph", mongodb_conns);

  if (mongodb_client_pool == nullptr) {
    return EXIT_FAILURE;
  }

  ClientPool<ThriftClient<UserServiceClient>> user_client_pool(
      "social-graph", user_addr, user_port, 0, user_conns, user_timeout,
      user_keepalive, config_json);

  mongoc_client_t *mongodb_client = mongoc_client_pool_pop(mongodb_client_pool);
  if (!mongodb_client) {
    LOG(fatal) << "Failed to pop mongoc client";
    return EXIT_FAILURE;
  }
  bool r = false;
  while (!r) {
    r = CreateIndex(mongodb_client, "social-graph", "user_id", true);
    if (!r) {
      LOG(error) << "Failed to create mongodb index, try again";
      sleep(1);
    }
  }
  mongoc_client_pool_push(mongodb_client_pool, mongodb_client);

  std::shared_ptr<TServerSocket> server_socket =
      get_server_socket(config_json, "0.0.0.0", port);

  if (redis_cluster_flag) {
    RedisCluster redis_cluster_client_pool =
        init_redis_cluster_client_pool(config_json, "social-graph");
    TThreadedServer server(
        std::make_shared<SocialGraphServiceProcessor>(
            std::make_shared<SocialGraphHandler>(mongodb_client_pool,
                                                 &redis_cluster_client_pool,
                                                 &user_client_pool)),
        server_socket, std::make_shared<TFramedTransportFactory>(),
        std::make_shared<TBinaryProtocolFactory>());
    LOG(info) << "Starting the social-graph-service server with Resis cluster...";
    server.serve();
  } else {
    Redis redis_client_pool =
        init_redis_client_pool(config_json, "social-graph");
    TThreadedServer server(
        std::make_shared<SocialGraphServiceProcessor>(
            std::make_shared<SocialGraphHandler>(
                mongodb_client_pool, &redis_client_pool, &user_client_pool)),
        server_socket, std::make_shared<TFramedTransportFactory>(),
        std::make_shared<TBinaryProtocolFactory>());
    LOG(info) << "Starting the social-graph-service server ...";
    server.serve();
  }
}

max_stars_repo_name: rodrigo-bruno/DeathStarBench

max_issues_repo_name: rodrigo-bruno/DeathStarBench

max_forks_repo_name: rodrigo-bruno/DeathStarBench