text:
//
// Created by tao on 19-3-25.
//
#include "file_key_controller.h"
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/filesystem/fstream.hpp>
#include <boost/filesystem/operations.hpp>
#include "log_init.h"
#include "AES_func_adaptor.h"
#include "get_usr_passwd.h"
#include "file_key_controller.h"
#include "../crypto_adaptor/AES_func_adaptor.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#define FILE_KEY_PREFIX "./sysdata"
FileKeyTable *FileKeyTable::Instance = new FileKeyTable();
std::mutex FileKeyTable::lock{};

std::string get_file_key(int user, int para) {
    FileKeyTable *ptr = FileKeyTable::Instance;
    if (ptr->userID == -1
        || ptr->userID != user) {
        ptr->userID = user;
        ptr->loadKeyTab();
    }
    return ptr->getKey(para);
}

void FileKeyNode::operator<<(const YAML::Node &node) {
    this->id = node["id"].as<int>();
    this->key = node["key"].as<std::string>();
}

void FileKeyNode::operator>>(YAML::Node &node) const {
    node["id"] = this->id;
    node["key"] = this->key;
}

std::string FileKeyTable::getKey(int id) {
    return tab[id];
}

int FileKeyTable::insertKey(const FileKeyNode &node) {
    return tab.insert(std::make_pair(node.id, node.key)).second;
}

int FileKeyTable::insertKey(const std::string &key) {
    int _id{};
    do {
        _id = rand();
        auto dest = tab.find(_id);
        if (dest == tab.end()) {
            break;
        }
    } while (true);
    FileKeyNode node;
    node.id = _id;
    node.key = key;

    return insertKey(node);
}

void FileKeyTable::operator<<(const YAML::Node &node) {
    FileKeyNode iNode;
    for (auto n:node) {
        iNode.clear();
        iNode << n;
        tab.insert(std::make_pair(iNode.id, iNode.key));
    }
}

void FileKeyTable::operator>>(YAML::Node &node) const {
    for (const auto &n:tab) {
        YAML::Node temp;
        FileKeyNode(n.first, n.second) >> temp;
        node.push_back(temp);
    }
}

int FileKeyTable::loadKeyTab() {
    LockGround lockGround(&lock);

    char userIDStr[64]{};
    sprintf(userIDStr, "%s/%d.filePasswd.data", FILE_KEY_PREFIX, userID);

    namespace bf=boost::filesystem;
    bf::path userKeyFile(userIDStr);
    bf::ifstream infile(userKeyFile);
    if (!infile) {
        INFO("file not exists :" << userIDStr)
        tab.insert(std::make_pair(0, "default file key,don't use it!"));
        return 0;
    }
    size_t buffLen = bf::file_size(userKeyFile) * 3;
    char *buff = (char *) malloc(buffLen);
    char *decBuff = (char *) malloc(buffLen);

    size_t dataLen = 0;
    while (!infile.eof()) {
        infile.read(buff + dataLen, buffLen);
        dataLen += infile.gcount();
    }
    if (aesDataToData(get_usr_key(userID), buff, dataLen, decBuff, buffLen, 0)) {
        ERROR_LOG("dec index fail :" << userIDStr)
        free(buff);
        free(decBuff);
        return -1;
    }
    decBuff[buffLen] = '\0';
//    DEBUG("yaml dec is" << decBuff)

    YAML::Node root = YAML::Load(decBuff);
    tab.clear();
    *Instance << root;
    free(buff);
    free(decBuff);
    if(tab.size()==0){
        FileKeyNode keyNode;
        keyNode.id=0;
        keyNode.key="asdfajklshflashdflasdkfjlasdfj";
        insertKey(keyNode);
    }
    return 0;
}

int FileKeyTable::savrKeyTab() {
    LockGround lockGround(&lock);
    //输出字符串
    YAML::Node node;
    *Instance >> node;
    std::ostringstream outString;
    outString << node;
    std::string yamlNode = outString.str();
    size_t outLen = yamlNode.length() * 10;
    char *decBuff = (char *) malloc(outLen);

    //加密
    if (aesDataToData(get_usr_key(userID), yamlNode.c_str(), yamlNode.length(), decBuff, outLen, 1)) {
        ERROR_LOG("enc index fail ")
        free(decBuff);
        return -1;
    }
    decBuff[outLen] = '\0';
    //保存
    namespace bf=boost::filesystem;

    char userIDStr[64]{};
    sprintf(userIDStr, "%s/%d.filePasswd.data", FILE_KEY_PREFIX, userID);

    bf::ofstream outfile(userIDStr, std::ios::out | std::ios::trunc | std::ios::binary);
    if (!outfile) {
        ERROR_LOG("file don't exists :" << userIDStr)
        free(decBuff);
        return -1;
    }
    outfile.write(decBuff, outLen);
    free(decBuff);
    return 0;
}


text:
#include "Misc.h"
#include <cstdlib>
#include <boost/algorithm/string.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/insert_linebreaks.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/ostream_iterator.hpp>
#include <boost/filesystem.hpp>
#include <boost/system/error_code.hpp>

using voice::Misc;

Misc::Misc(){}

Misc::Misc(const Misc&){}

Misc::~Misc(){}

void Misc::trim(std::string& s){
	size_t p = s.find_first_not_of(" \t\r\n");
		s.erase(0, p);
	p = s.find_last_not_of(" \t\r\n");
	if (std::string::npos != p)
		s.erase(p+1);
}

unsigned int Misc::hashString(const std::string& s){
	unsigned int a = 54059; // A prime
	unsigned int b = 76963; // Another prime
	unsigned int h = 31; // also prime
	for(size_t i = 0; i < s.size(); ++i) {
		h = (h * a) ^ (s[i] * b);
	}
	return h;
}

unsigned int Misc::hashString(const char* s){
	unsigned int a = 54059; // A prime
	unsigned int b = 76963; // Another prime
	unsigned int h = 31; // also prime
	while (*s) {
		h = (h * a) ^ (*s * b);
		s++;
	}
	return h;
}

bool Misc::convert(const std::string& input, bool& value){
	std::string tsd = boost::trim_copy(input);

	if((value = boost::iequals("true", input)) || boost::iequals("false", input))
		return true;

	int iValue;
	if (convert(tsd, iValue))
	{
		value = iValue != 0;
		return true;
	}
	return false;
}

bool Misc::convert(const std::string& input, boost::int8_t& value){
	boost::int32_t iValue;
	try{
		std::stringstream(input) >> iValue;
		value = (boost::int8_t)iValue;
		return true;
	}catch(...){ return false; }
}

bool Misc::convert(const std::string& input, boost::int16_t& value){
	std::stringstream ss(input);
	try{
		ss >> value;
		return true;
	}catch(...){ return false; }
}

bool Misc::convert(const std::string& input, boost::int32_t& value){
	try{
		std::stringstream(input) >> value;
		return true;
	}catch(...){ return false; }
}

bool Misc::convert(const std::string& input, boost::int64_t& value){
	try{
		std::stringstream(input) >> value;
		return true;
	}catch(...){ return false; }
}

bool Misc::convert(const std::string& input, float& value){
	try{
		std::stringstream(input) >> value;
		return true;
	}catch(...){ return false; }
}

bool Misc::convert(const std::string& input, double& value){
	try{
		std::stringstream(input) >> value;
		return true;
	}catch(...){ return false; }
}

void Misc::replace(std::string& str, const std::string& oldString, const std::string& newString){
	if(str.empty() || oldString.empty())
		return;
	size_t start = 0;
	while((start = str.find(oldString, start)) != std::string::npos) {
		str.replace(start, oldString.length(), newString);
		start += newString.length(); // In case 'to' contains 'from', like replacing 'x' with 'yx'
	}
}

std::vector<char> Misc::decodeBase64(const std::string& encodedData){
	const std::string base64_padding[] = {"", "==","="};
	namespace bai = boost::archive::iterators;
	std::stringstream os;

	const char* begin = reinterpret_cast<const char*>(&encodedData[0]);
	typedef bai::transform_width<bai::binary_from_base64<const char *>, 8, 6> base64_dec;
	size_t size = encodedData.size();
	size_t decodedSize = 3 * size / 4;

	// Remove the padding characters, cf. https://svn.boost.org/trac/boost/ticket/5629
	if (size && encodedData[size - 1] == '=') {
		--size;
		--decodedSize;
		if (size && encodedData[size - 1] == '='){
			--size;
			--decodedSize;
		}
	}

	if (size == 0) return std::vector<char>();
	std::vector<char> decoded = std::vector<char>(decodedSize);

	std::copy(base64_dec(encodedData.data()), base64_dec(encodedData.data() + size), &decoded[0]);

	return decoded;
}

std::string Misc::encodeBase64(const std::vector<char>& data){
	const std::string base64_padding[] = {"", "==","="};
	namespace bai = boost::archive::iterators;
	std::stringstream os;

	const char* begin = reinterpret_cast<const char*>(&data[0]);
	const char* end = (const char*)(begin + data.size());

	// convert binary values to base64 characters
	typedef bai::base64_from_binary
		// retrieve 6 bit integers from a sequence of 8 bit bytes
		<bai::transform_width<const char *, 6, 8> > base64_enc; // compose all the above operations in to a new iterator

	std::copy(base64_enc(begin), base64_enc(end), std::ostream_iterator<char>(os));

	os << base64_padding[data.size() % 3];
	return os.str();
}

std::string Misc::getHomePath(){
	return boost::filesystem::canonical(boost::filesystem::path(std::getenv("HOME"))).string();
}

std::string Misc::getCanonicalPath(const std::string& path){
	if(path.empty())
		return boost::filesystem::current_path().string();

#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) && !defined(__CYGWIN__)
	return boost::filesystem::canonical(boost::filesystem::path(path)).string();
#else
	std::string p(path);
	if(p[0] == '~') p = getHomePath() + p.substr(1);
	boost::filesystem::path relative(p);
	boost::filesystem::path canonical(relative);
	return canonical.string();
#endif
}

void Misc::linuxRun(const std::string& command){
	#if !defined(WIN32) && !defined(_WIN32) && !defined(__WIN32) || defined(__CYGWIN__)
	system(command.c_str());
	#endif
}

ext: cpp

max_stars_repo_name: BioRoboticsUNAM/SP-GEN-linux

max_issues_repo_name: BioRoboticsUNAM/SP-GEN-linux

max_forks_repo_name: BioRoboticsUNAM/SP-GEN-linux


text: #include <boost/geometry/formulas/result_inverse.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#ifndef IRODS_POLICY_COMPOSITION_FRAMEWORK_POLICY_ENGINE_HPP
#define IRODS_POLICY_COMPOSITION_FRAMEWORK_POLICY_ENGINE_HPP

#include "irods_re_plugin.hpp"
#include "irods_re_ruleexistshelper.hpp"

#define IRODS_QUERY_ENABLE_SERVER_SIDE_API
#include "irods_query.hpp"

#include "rcMisc.h"
#include <boost/any.hpp>
#include "policy_composition_framework_utilities.hpp"
#include "policy_composition_framework_plugin_configuration_json.hpp"

#include "json.hpp"
#include "fmt/format.h"

namespace irods::policy_composition::policy_engine {

    // clang-format off
    namespace pc   = irods::policy_composition;
    using     json = nlohmann::json;

    struct context {
        ruleExecInfo_t* rei{};
        std::string     usage_text{};
        std::string     instance_name{};
        std::string     policy_name{};
        std::string     policy_usage{};
        json            parameters{};
        json            configuration{};
    }; // struct context

    using arg_type            = std::string*;
    using plugin_type         = pluggable_rule_engine<irods::default_re_ctx>;
    using plugin_pointer_type = plugin_type*;
    using implementation_type = std::function<error(const context&, arg_type)>;

    auto                log_errors{false};
    json                plugin_config;
    context             policy_context;
    implementation_type policy_implementation;
    // clang-format on

    namespace {
        auto start(
              default_re_ctx&
            , const std::string&) -> error
        {
            RuleExistsHelper::Instance()->registerRuleRegex(
                policy_context.policy_name + ".*");
            return SUCCESS();
        }

        auto stop(
            default_re_ctx&,
            const std::string& ) -> error
        {
            return SUCCESS();
        }

        auto rule_name_is_supported(const std::string& _rule_name) -> bool
        {
            auto supported = (policy_context.policy_name  == _rule_name ||
                              policy_context.policy_usage == _rule_name);

            return supported;

        } // rule_name_is_supported

        auto rule_exists(
              default_re_ctx&
            , const std::string& _rule_name
            , bool&              _return_value) -> error
        {
            _return_value = rule_name_is_supported(_rule_name);
            return SUCCESS();
        }

        auto list_rules(
             default_re_ctx&
            , std::vector<std::string>& _rules) -> error
        {
            _rules.push_back(policy_context.policy_name);
            _rules.push_back(policy_context.policy_usage);
            return SUCCESS();
        }

        auto get_log_errors_flag(
              const json& _params
            , const json& _config) -> bool
        {
            bool flag = false;
            if(!_params.empty()) {
                flag = (_params.find("log_errors") !=
                        _params.end());
            }

            if(!flag && !_config.empty()) {
                flag = (_config.find("log_errors") !=
                        _config.end());
            }

            return flag;

        } // get_log_errors_flag

        auto client_message(const json& _msg) -> void
        {
            if(!log_errors) {
                return;
            }

            addRErrorMsg(
                    &policy_context.rei->rsComm->rError,
                    0,
                    _msg.dump(4).c_str());

            rodsLog(LOG_NOTICE, "%s", _msg.dump(4).c_str());

        } // details

        error exec_rule(
            default_re_ctx&
            , const std::string&     _rule_name
            , std::list<boost::any>& _arguments
            , callback               _eff_hdlr)
        {

            ruleExecInfo_t* rei{};
            const auto err = _eff_hdlr("unsafe_ms_ctx", &rei);
            if(!err.ok()) {
                return ERROR(SYS_NOT_SUPPORTED, err.result());
            }

            try {
                if(policy_context.policy_usage == _rule_name) {

                    auto it = _arguments.begin();

                    std::advance(it, 2);

                    auto* out = boost::any_cast<arg_type>(*it);

                    *out = policy_context.usage_text;

                    return SUCCESS();
                }
                else if(policy_context.policy_name == _rule_name) {

                    policy_context.rei = rei;

                    auto it = _arguments.begin();
                    auto* parameters    = boost::any_cast<std::string*>(*it); ++it;
                    auto* configuration = boost::any_cast<std::string*>(*it); ++it;
                    auto* out_variable  = boost::any_cast<std::string*>(*it);

                    if(!parameters->empty()) {
                        policy_context.parameters = json::parse(*parameters);
                    }

                    policy_context.configuration = plugin_config;
                    if(!configuration->empty()) {
                        // combine policy config and plugin specific config
                        auto j = json::parse(*configuration);

                        for(auto&& [k, v] : j.items()) {
                            policy_context.configuration[k] = v;
                        }
                    }

                    log_errors = get_log_errors_flag(
                                       policy_context.parameters
                                     , policy_context.configuration);

                    auto err = policy_implementation(policy_context, out_variable);

                    if(!err.ok()) {
                        // support for stop_on_error behavior
                        *out_variable = pc::error_to_json(err).dump(4);

                        addRErrorMsg(
                                &rei->rsComm->rError,
                                err.code(),
                                err.result().c_str());

                        if(log_errors) { irods::log(err); }

                        THROW(err.code(), err.result());
                    }
                }

            }
            // TODO :: add more context to these errors for the user
            catch(const std::invalid_argument& _e) {
                if(log_errors) { irods::log(err); }
                pc::exception_to_rerror(
                    SYS_NOT_SUPPORTED,
                    _e.what(),
                    rei->rsComm->rError);
                return ERROR(
                           SYS_NOT_SUPPORTED,
                           _e.what());
            }
            catch(const boost::bad_any_cast& _e) {
                if(log_errors) { irods::log(err); }
                pc::exception_to_rerror(
                    SYS_NOT_SUPPORTED,
                    _e.what(),
                    rei->rsComm->rError);
                return ERROR(
                           SYS_NOT_SUPPORTED,
                           _e.what());
            }
            catch(const exception& _e) {
                if(log_errors) { irods::log(err); }
                pc::exception_to_rerror(
                    _e,
                    rei->rsComm->rError);
                return ERROR(
                           SYS_NOT_SUPPORTED,
                           _e.what());
            }
            catch(const json::exception& _e) {
                addRErrorMsg(
                        &rei->rsComm->rError,
                        SYS_NOT_SUPPORTED,
                        _e.what());
                if(log_errors) { rodsLog(LOG_ERROR, "%s", _e.what()); }
                return ERROR(
                           SYS_NOT_SUPPORTED,
                           _e.what());
            }
            catch(...) {
                auto msg = "policy_engine :: an unknown error has occurred.";
                addRErrorMsg(
                        &rei->rsComm->rError,
                        SYS_NOT_SUPPORTED,
                        msg);
                rodsLog(LOG_ERROR, msg);
                return ERROR(
                           SYS_NOT_SUPPORTED,
                           msg);
            }

            // given that this is a specific policy implementation which does not react
            // to policy enforcement points we can return SUCCESS() rather than
            // CODE(RULE_ENGINE_CONTINUE), as there should only be one policy engine configured
            return SUCCESS();

        } // exec_rule

        error exec_rule_text(
              default_re_ctx&
            , const std::string&
            , msParamArray_t*
            , const std::string&
            , callback )
        {
            return ERROR(
                    RULE_ENGINE_CONTINUE,
                    "exec_rule_text is not supported");
        } // exec_rule_text

        error exec_rule_expression(
              default_re_ctx&
            , const std::string&
            , msParamArray_t*
            , callback)
        {
            return ERROR(
                    RULE_ENGINE_CONTINUE,
                    "exec_rule_expression is not supported");
        } // exec_rule_expression

    } // namespace

    plugin_pointer_type make(
            const std::string&  _plugin_name
          , const std::string&  _policy_name
          , const std::string&  _usage_text
          , implementation_type _policy_implementation)
    {

        policy_implementation        = _policy_implementation;
        policy_context.usage_text    = _usage_text;
        policy_context.policy_name   = _policy_name;
        policy_context.policy_usage  = _policy_name + "_usage";
        policy_context.instance_name = _plugin_name;

        plugin_config = get_plugin_specific_configuration(_plugin_name);

        auto rule_engine_plugin = new plugin_type(policy_context.instance_name, "");

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            const std::string&>(
                "start",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        const std::string&)>(start));

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            const std::string&>(
                "stop",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        const std::string&)>(stop));

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            const std::string&,
            bool&>(
                "rule_exists",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        const std::string&,
                        bool&)>(rule_exists));

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            std::vector<std::string>&>(
                "list_rules",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        std::vector<std::string>&)>(list_rules));

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            const std::string&,
            std::list<boost::any>&,
            irods::callback>(
                "exec_rule",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        const std::string&,
                        std::list<boost::any>&,
                        irods::callback)>(exec_rule));

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            const std::string&,
            msParamArray_t*,
            const std::string&,
            irods::callback>(
                "exec_rule_text",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        const std::string&,
                        msParamArray_t*,
                        const std::string&,
                        irods::callback)>(exec_rule_text));

        rule_engine_plugin->add_operation<
            irods::default_re_ctx&,
            const std::string&,
            msParamArray_t*,
            irods::callback>(
                "exec_rule_expression",
                std::function<
                    irods::error(
                        irods::default_re_ctx&,
                        const std::string&,
                        msParamArray_t*,
                        irods::callback)>(exec_rule_expression));

        return rule_engine_plugin;

    } // make

} // namespace irods::policy_composition::policy_engine

#endif // IRODS_POLICY_COMPOSITION_FRAMEWORK_POLICY_ENGINE_HPP


text:
#include "stdafx.h"
#include "CompletionEngine.h"
#include <CodeAssistLib/impl/RelevanceConstants.h>
#include <JCDT_Lib/internal/lookup/TypeSymbol.h>
#include <JCDT_Lib/internal/env/LookupEnvironment.h>
#include <JCDT_Lib/internal/semantic/semantic.h>
#include <JCDT_Lib/internal/util/spell.h>
#include <JCDT_Lib/internal/util/CharOperation.h>
#include <boost/algorithm/string.hpp>
using namespace Jikes::JavaModel;

namespace Jikes { // Open namespace Jikes block

namespace CodeAssist{

	int CompletionEngine::computeRelevanceForExpectingType(wstring packageName, const wstring& typeName)
	{
		if (expectedTypes.size() != true)
		{
			boost::replace_all(packageName, L"/", L".");
			for (int i = 0; i < expectedTypes.size(); i++)
			{
				if (expectedTypes[i]->ContainingPackageName() == packageName &&
					expectedTypes[i]->qualifiedSourceName('.') == typeName)
				{
					return R_EXPECTED_TYPE;
				}
			}
		}
		return 0;
	}

	int CompletionEngine::computeRelevanceForExpectingType(TypeSymbol* proposalType) {
		if (expectedTypes.empty() != true && proposalType != nullptr) {
			for (int i = 0; i < expectedTypes.size(); i++) {

				if (lookupEnvironment->system_semantic->CanMethodInvocationConvert(proposalType, expectedTypes[i])) {
					return R_EXPECTED_TYPE;
				}

			}
		}
		return 0;
	}

	double  CompletionEngine::computeRelevanceForCaseMatching(const wstring& token, const wstring& proposalName)
	{

	 if (CharOperation::prefixEquals(token, proposalName, true /* do not ignore case */)) {
			if (CharOperation::equals(token, proposalName, true /* do not ignore case */))
			{
				return R_CASE + R_EXACT_NAME;
			}

			auto leftData = token.c_str();
			auto testToken = proposalName.c_str();

			double rate = Spell::IsLeftStringOrderInRightString(leftData, testToken);
			if (rate) {
				int value = (RelevanceConstants::R_CASE * rate);
				return  value > 0 ? value : 1;
			}

	}
	 else if (CharOperation::prefixEquals(token, proposalName, false /* ignore case */))
	 {
		 if (CharOperation::equals(token, proposalName, false /* ignore case */)) {
			 return R_CASE;
		 }

		 auto leftData = token.c_str();
		 auto testToken = proposalName.c_str();
		 double rate = Spell::IsLeftStringOrderInRightString(leftData, testToken);
		 if (rate) {

			 int value = (RelevanceConstants::R_CASE * rate);
			 return  value > 0 ? value : 1;

		 }
	 }
	 else
	 {
		 auto leftData = token.c_str();
		 auto testToken = proposalName.c_str();
		 int  index = Spell::Index(leftData, testToken);
		 if (index > 7) {
			 return (RelevanceConstants::R_EXACT_NAME * index / 10.0);

		 }

		 double rate = Spell::IsLeftStringOrderInRightString(leftData, testToken);
		 if (rate) {
			 int value = (RelevanceConstants::R_EXACT_NAME * rate);
			 return  value  >  0 ? value : 1 ;

		 }
		 rate = Spell::levenshtein(leftData, testToken) / (proposalName.size()*1.0);
		 if (rate < 0.3) {
			 return  (RelevanceConstants::R_EXACT_NAME * (1 - rate));

		 }
	 }

	return 0;
	}

	 int  CompletionEngine::computeRelevanceForClass() {
		if (assistNodeIsClass) {
			return R_CLASS;
		}
		return 0;
	}
	 double  CompletionEngine::computeBaseRelevance() {
		 return R_DEFAULT;
	 }
	 int CompletionEngine::computeBaseRelevancePackageRelevance()
	{
		if (hasImportWord){
			return R_PACKAGE;
		}
		else return  0;

	}
	 int CompletionEngine::computeRelevanceForResolution() {
		 return computeRelevanceForResolution(true);
	 }
	 int CompletionEngine::computeRelevanceForResolution(bool isResolved) {
		 if (isResolved) {
			 return R_RESOLVED;
		 }
		 return 0;
	 }
	 int CompletionEngine::computeRelevanceForRestrictions(int accessRuleKind) {
		 if (accessRuleKind == IAccessRule::K_ACCESSIBLE) {
			 return R_NON_RESTRICTED;
		 }
		 return 0;
	 }

	 int  CompletionEngine::computeRelevanceForEnum() {
		if (assistNodeIsEnum) {
			return R_ENUM;
		}
		return 0;
	}
	  int CompletionEngine::computeRelevanceForFinal(bool onlyFinal, bool isFinal) {
		 if (onlyFinal && isFinal) {
			 return R_FINAL;
		 }
		 return 0;
	 }
	 int  CompletionEngine::computeRelevanceForEnumConstant(TypeSymbol* proposalType) {
		if (assistNodeIsEnum &&
			proposalType != nullptr) {
			for (int i = 0; i <= expectedTypes.size(); i++) {
				if (proposalType->ACC_ENUM() &&
					proposalType == expectedTypes[i]) {
					return R_ENUM + R_ENUM_CONSTANT;
				}

			}
		}
		return 0;
	}

	 int  CompletionEngine::omputeRelevanceForException() {
		if (assistNodeIsException) {
			return R_EXCEPTION;
		}
		return 0;
	}
	 int  CompletionEngine::computeRelevanceForInterface() {
		if (assistNodeIsInterface) {
			return R_INTERFACE;
		}
		return 0;
	}

	int CompletionEngine::computeRelevanceForQualification(bool prefixRequired) {
		if (!prefixRequired && !insideQualifiedReference) {
			return R_UNQUALIFIED;
		}

		if (prefixRequired && insideQualifiedReference) {
			return R_QUALIFIED;
		}
		return 0;
	}

	 int CompletionEngine::computeRelevanceForStatic(bool onlyStatic, bool isStatic) {
		if (insideQualifiedReference && !onlyStatic && !isStatic) {
			return R_NON_STATIC;
		}
		return 0;
	}
	 int CompletionEngine::computeRelevanceForInheritance(TypeSymbol* receiverType, TypeSymbol* declaringClass) {
		 if (receiverType == declaringClass) return R_NON_INHERITED;
		 return 0;
	 }

} // Close namespace Jikes block

} // Close namespace CodeAssist block

max_stars_repo_name: kuafuwang/JCDT

max_issues_repo_name: kuafuwang/JCDT

max_forks_repo_name: kuafuwang/JCDT


text:
// Gao Wang and Kushal K. Dey (c) 2016
// code format configuration: clang-format -style=Google -dump-config >
// ~/.clang-format
#ifndef _PFA_HPP
#define _PFA_HPP

#include <omp.h>
#include <armadillo>
#include <map>
#include <string>

static const double INV_SQRT_2PI = 0.3989422804014327;
static const double INV_SQRT_2PI_LOG = -0.91893853320467267;

inline double normal_pdf(double x, double m, double sd) {
  double a = (x - m) / sd;
  return INV_SQRT_2PI / sd * std::exp(-0.5 * a * a);
};

inline double normal_pdf_log(double x, double m, double sd) {
  double a = (x - m) / sd;
  return INV_SQRT_2PI_LOG - std::log(sd) - 0.5 * a * a;
};

/* The digamma function is the derivative of gammaln.
   Reference:
    J Bernardo,
    Psi ( Digamma ) Function,
    Algorithm AS 103,
    Applied Statistics,
    Volume 25, Number 3, pages 315-317, 1976.
    From http://www.psc.edu/~burkardt/src/dirichlet/dirichlet.f
    (with modifications for negative numbers and extra precision)
*/
inline double digamma(double x) {
  double neginf = -INFINITY;
  static const double c = 12, digamma1 = -0.57721566490153286,
                      trigamma1 = 1.6449340668482264365, /* pi^2/6 */
      s = 1e-6, s3 = 1. / 12, s4 = 1. / 120, s5 = 1. / 252, s6 = 1. / 240,
                      s7 = 1. / 132, s8 = 691. / 32760, s9 = 1. / 12,
                      s10 = 3617. / 8160;
  double result;
  /* Illegal arguments */
  if ((x == neginf) || std::isnan(x)) {
    return NAN;
  }
  /* Singularities */
  if ((x <= 0) && (floor(x) == x)) {
    return neginf;
  }
  /* Negative values */
  /* Use the reflection formula (Jeffrey 11.1.6):
 * digamma(-x) = digamma(x+1) + pi*cot(pi*x)
 *
 * This is related to the identity
 * digamma(-x) = digamma(x+1) - digamma(z) + digamma(1-z)
 * where z is the fractional part of x
 * For example:
 * digamma(-3.1) = 1/3.1 + 1/2.1 + 1/1.1 + 1/0.1 + digamma(1-0.1)
 *               = digamma(4.1) - digamma(0.1) + digamma(1-0.1)
 * Then we use
 * digamma(1-z) - digamma(z) = pi*cot(pi*z)
 */
  if (x < 0) {
    return digamma(1 - x) + M_PI / tan(-M_PI * x);
  }
  /* Use Taylor series if argument <= S */
  if (x <= s) return digamma1 - 1 / x + trigamma1 * x;
  /* Reduce to digamma(X + N) where (X + N) >= C */
  result = 0;
  while (x < c) {
    result -= 1 / x;
    x++;
  }
  /* Use de Moivre's expansion if argument >= C */
  /* This expansion can be computed in Maple via asympt(Psi(x),x) */
  if (x >= c) {
    double r = 1 / x, t;
    result += log(x) - 0.5 * r;
    r *= r;
#if 1
    result -= r * (s3 - r * (s4 - r * (s5 - r * (s6 - r * s7))));
#else
    /* this version for lame compilers */
    t = (s5 - r * (s6 - r * s7));
    result -= r * (s3 - r * (s4 - r * t));
#endif
  }
  return result;
}

template <typename T>
void print(const T &e) {
  std::cout << e << std::endl;
}

class Exception {
 public:
  /// constructor
  /// \param msg error message
  Exception(const std::string &msg) : m_msg(msg) {}

  /// return error message
  const char *message() { return m_msg.c_str(); }

  virtual ~Exception(){};

 private:
  /// error message
  std::string m_msg;
};

/// exception, thrown if out of memory
class StopIteration : public Exception {
 public:
  StopIteration(const std::string msg) : Exception(msg){};
};

/// exception, thrown if index out of range
class IndexError : public Exception {
 public:
  IndexError(const std::string msg) : Exception(msg){};
};

/// exception, thrown if value of range etc
class ValueError : public Exception {
 public:
  ValueError(const std::string msg) : Exception(msg){};
};

/// exception, thrown if system error occurs
class SystemError : public Exception {
 public:
  SystemError(const std::string msg) : Exception(msg){};
};

/// exception, thrown if a runtime error occurs
class RuntimeError : public Exception {
 public:
  RuntimeError(const std::string msg) : Exception(msg){};
};

extern "C" int pfa_em(double *, double *, double *, double *, int *, int *,
                      int *, int *, double *, int *, double *, int *, int *,
                      double *, double *, double *, double *, int *, int *,
                      int *, int *, int *, int *);
extern "C" int pfa_model_loglik(double *, double *, double *, double *, int *,
                                int *, int *, double *);

class PFA {
 public:
  PFA(double *cX, double *cF, double *cP, double *cQ, double *cLout, int N,
      int J, int K, int C)
      :  // mat(aux_mem*, n_rows, n_cols, copy_aux_mem = true, strict = true)
        D(cX, N, J, false, true),
        F(cF, K, J, false, true),
        P(cP, K, K, false, true),
        q(cQ, C, false, true),
        L(cLout, N, K, false, true) {
    // initialize residual sd with sample sd
    s = arma::vectorise(arma::stddev(D));
    W.set_size(F.n_rows, F.n_rows);
    delta.set_size(int((F.n_rows + 1) * F.n_rows / 2), q.n_elem, D.n_rows);
    delta.fill(0);
    n_threads = omp_get_max_threads();
    n_updates = 0;
    for (size_t k1 = 0; k1 < F.n_rows; k1++) {
      for (size_t k2 = 0; k2 <= k1; k2++) {
        // set factor pair coordinates to avoid
        // having to compute it at each iteration
        // (b + 1) * b / 2 - (b - a)
        size_t k1k2 = size_t(k1 * (k1 + 1) / 2 + k2);
        F_pair_coord[std::make_pair(k1, k2)] = k1k2;
        F_pair_coord[std::make_pair(k2, k1)] = k1k2;
      }
    }
  }
  virtual ~PFA() {}

  virtual PFA *clone() const { return new PFA(*this); }

  virtual void write(std::ostream &out, int info) {
    throw RuntimeError("The base write() function should not be called");
  }
  virtual int E_step() {
    throw RuntimeError("The base E_step() function should not be called");
  }

  virtual int M_step() {
    throw RuntimeError("The base M_step() function should not be called");
  }

  void update_model_loglik(arma::vec &true_s, arma::mat &true_q);
  void set_threads(int n) { n_threads = n; }
  void update_ldelta(int core = 0);
  void update_loglik_and_delta();
  void update_factor_model();
  void update_residual_error();
  double get_loglik() { return loglik; }
  double get_bic() {
    // BIC = -2 * loglik + d * log(N)
    // number of parameters:
    // P: (K + 1) * K / 2
    // L: N * K
    // F: K * J
    return -2 * loglik +
           ((P.n_rows + 1) * P.n_rows / 2 + D.n_rows * P.n_rows +
            P.n_rows * D.n_cols) *
               std::log(D.n_rows);
  }

 protected:
  // N by J matrix of data
  arma::mat D;
  // K by K matrix of factor pair frequencies
  arma::mat P;
  // K by J matrix of factors
  arma::mat F;
  // Q by 1 vector of membership grids
  arma::vec q;
  // J by 1 vector of residual standard error
  arma::vec s;
  // K1K2 by Q by N tensor
  arma::cube delta;
  arma::cube ldelta;
  std::map<std::pair<size_t, size_t>, size_t> F_pair_coord;
  // N by K matrix of loadings
  arma::mat L;
  // W = L'L is K X K matrix
  arma::mat W;
  // loglik
  double loglik;
  // number of threads
  int n_threads;
  // updates on the model
  int n_updates;
};

class PFA_EM : public PFA {
 public:
  PFA_EM(double *cX, double *cF, double *cP, double *cQ, double *cLout, int N,
         int J, int K, int C)
      : PFA(cX, cF, cP, cQ, cLout, N, J, K, C) {}

  PFA *clone() const { return new PFA_EM(*this); }

  void update_paired_factor_weights();
  int E_step() {
    update_ldelta();
    update_loglik_and_delta();
    update_paired_factor_weights();
    return 0;
  }

  int M_step() {
    update_factor_model();
    update_residual_error();
    n_updates += 1;
    return 0;
  }

  void write(std::ostream &out, int info) {
    if (info == 0) {
      // D.print(out, "Data Matrix:");
      q.print(out, "Membership grids:");
    }
    if (info == 1) {
      F.print(out, "Factor matrix:");
      P.print(out, "Factor frequency matrix:");
      if (n_updates > 0)
        L.print(out, "Loading matrix:");
      else
        out << "Loading matrix:" << std::endl;
    }
    if (info == 2) {
      // W.print(out, "E[L'L] matrix:");
      s.print(out, "Residual standard deviation of data columns:");
      if (n_updates > 0) {
        ldelta.print(out, "log delta tensor");
        delta.print(out, "delta tensor");
      }
    }
  }
};

class PFA_VEM : public PFA {
 public:
  PFA_VEM(double *cX, double *cF, double *cP, double *cQ, double *cLout,
          double *calphaout, int N, int J, int K, int C, double alpha0)
      : PFA(cX, cF, cP, cQ, cLout, N, J, K, C),
        alpha(calphaout, K, K, false, true),
        alpha0(alpha0) {
    alpha.zeros();
    digamma_alpha.set_size(P.n_rows, P.n_rows);
    digamma_alpha.fill(digamma(alpha0));
    digamma_sum_alpha = 0;
    maxiter = 1000;
    tol = 1E-3;
  }

  PFA *clone() const { return new PFA_VEM(*this); }

  void update_variational_ldelta();
  double get_variational_lowerbound();
  void update_variational_parameters();
  void update_paired_factor_weights();

  int E_step() {
    int status = 0;
    update_ldelta(1);
    size_t niter = 0;
    lowerbound.resize(0);
    while (niter <= maxiter) {
      update_variational_ldelta();
      update_loglik_and_delta();
      update_variational_parameters();
      update_paired_factor_weights();
      lowerbound.push_back(get_variational_lowerbound());
      if (lowerbound.back() != lowerbound.back()) {
        std::cerr
            << "[ERROR] lower bound nan produced in variational approximation!"
            << std::endl;
        status = 1;
        break;
      }
      niter++;
      if (niter > 1) {
        double diff = lowerbound[niter - 1] - lowerbound[niter - 2];
        if (diff < 0.0) {
          std::cerr << "[ERROR] lower bound decreased in variational "
                       "approximation:  \n\tfrom "
                    << lowerbound[niter - 2] << " to " << lowerbound[niter - 1]
                    << "!" << std::endl;
          status = 1;
          break;
        }
        if (diff < tol) break;
      }
      if (niter == maxiter) {
        std::cerr << "[WARNIKNG] variational approximation procedure failed to "
                     "converge at tolerance level "
                  << tol << ", after " << maxiter
                  << " iterations: \n\tlog lower bound starts "
                  << lowerbound.front() << ", ends " << lowerbound.back() << "!"
                  << std::endl;
        status = 1;
        break;
      }
    }
    return status;
  }

  int M_step() {
    update_factor_model();
    update_residual_error();
    n_updates += 1;
    return 0;
  }

  void write(std::ostream &out, int info) {
    if (info == 0) {
      // D.print(out, "Data Matrix:");
      q.print(out, "Membership grids:");
    }
    if (info == 1) {
      F.print(out, "Factor matrix:");
      P.print(out, "Factor frequency matrix:");
      if (n_updates > 0)
        L.print(out, "Loading matrix:");
      else
        out << "Loading matrix:" << std::endl;
    }
    if (info == 2) {
      // W.print(out, "E[L'L] matrix:");
      s.print(out, "Residual standard deviation of data columns:");
      if (n_updates > 0) {
        alpha.print(out, "Dirichlet parameter for factor pairs:");
        out << "log lower bound:" << std::endl;
        for (size_t i = 0; i < lowerbound.size(); ++i) {
          out << lowerbound[i] << ", ";
        }
        out << std::endl;
      }
    }
  }

 private:
  // Dirichlet priors for factors and grids
  double alpha0;
  // K by K matrix of digamma of variational parameter for factor pair
  // frequencies
  arma::mat digamma_alpha;
  arma::mat alpha;
  double digamma_sum_alpha;
  size_t maxiter;
  double tol;
  std::vector<double> lowerbound;
};
#endif

max_stars_repo_name: gaow/pfar

max_issues_repo_name: gaow/pfar

max_forks_repo_name: gaow/pfar


text:
/*
 * Copyright (c) 2018 Ryan Berryhill, University of Toronto
 * 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 "pme/id.h"
#include "pme/engine/variable_manager.h"
#include "pme/engine/global_state.h"
#include "pme/ivc/correction_set_finder.h"

#define BOOST_TEST_MODULE CorrectionSetFinderTest
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

using namespace PME;

struct CorrectionSetFixture
{
    aiger * aig;
    ExternalID l0, l1, l2, l3, a0, a1, a2, a3, a4, a5, a6, o0;
    VariableManager vars;
    std::unique_ptr<DebugTransitionRelation> tr;

    CorrectionSetFixture()
    {
        aig = aiger_init();

        l0 = 2;
        l1 = 4;
        l2 = 6;
        l3 = 8;
        a0 = 10;
        a1 = 12;
        a2 = 14;
        a3 = 16;
        a4 = 18;
        a5 = 20;
        a6 = 22;

        // l0' = l3
        aiger_add_latch(aig, l0, l3, "l0");

        // l1' = l0
        aiger_add_latch(aig, l1, l0, "l1");

        // l2' = l1
        aiger_add_latch(aig, l2, l1, "l2");

        // l3' = l2
        aiger_add_latch(aig, l3, l2, "l3");

        // a0-a3 are ands of each pair of adjacent latches
        // a4 and a5 are ands of adjacent pairs of a0 and a3
        // a6 = a4 & a5 = <every adjacent pair is high>
        // a6 <=> ALL(l0-l3), but the structure is important
        aiger_add_and(aig, a0, l0, l1);
        aiger_add_and(aig, a1, l1, l2);
        aiger_add_and(aig, a2, l2, l3);
        aiger_add_and(aig, a3, l3, l0);
        aiger_add_and(aig, a4, a0, a1);
        aiger_add_and(aig, a5, a2, a3);
        aiger_add_and(aig, a6, a4, a5);
        aiger_add_output(aig, a6, "o0");
        o0 = a6;
        tr.reset(new DebugTransitionRelation(vars, aig));
    }

    void setInit(ID latch, ID val)
    {
        tr->setInit(latch, val);
    }

    // Shrink a correction set to an MCS in order to simplify testing
    // for correction set finders that don't guarantee minimality
    void shrink(CorrectionSet & corr)
    {
        std::vector<std::set<ID>> mcses;

        ID b0 = tr->toInternal(a0);
        ID b1 = tr->toInternal(a1);
        ID b2 = tr->toInternal(a2);
        ID b3 = tr->toInternal(a3);
        ID b4 = tr->toInternal(a4);
        ID b5 = tr->toInternal(a5);
        ID b6 = tr->toInternal(a6);

        mcses.push_back({b6});
        mcses.push_back({b4, b5});
        mcses.push_back({b0, b1, b5});
        mcses.push_back({b2, b3, b4});
        mcses.push_back({b0, b1, b2, b3});

        std::sort(corr.begin(), corr.end());

        for (const auto & mcs : mcses)
        {
            if (std::includes(corr.begin(), corr.end(), mcs.begin(), mcs.end()))
            {
                corr.clear();
                corr.insert(corr.end(), mcs.begin(), mcs.end());
                return;
            }
        }

        // Reaching this line indicates failure
        BOOST_REQUIRE(false);
    }

    ~CorrectionSetFixture()
    {
        aiger_reset(aig);
        aig = nullptr;
    }
};

BOOST_AUTO_TEST_CASE(mcs)
{
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    MCSFinder finder(f.vars, *f.tr);

    BOOST_REQUIRE(finder.moreCorrectionSets());

    bool found;
    CorrectionSet corr;

    // MCS {a6}
    finder.setCardinality(1);
    std::tie(found, corr) = finder.findAndBlock();

    BOOST_REQUIRE(found);
    BOOST_CHECK_EQUAL(corr.size(), 1);
    BOOST_CHECK_EQUAL(corr.at(0), a6);

    std::tie(found, corr) = finder.findAndBlock();
    BOOST_REQUIRE(!found);

    BOOST_REQUIRE(finder.moreCorrectionSets());

    // MCS {a4, a5}
    finder.setCardinality(2);
    std::tie(found, corr) = finder.findAndBlock();

    BOOST_REQUIRE(found);
    BOOST_CHECK_EQUAL(corr.size(), 2);
    CorrectionSet expected = {a4, a5};
    std::sort(expected.begin(), expected.end());
    std::sort(corr.begin(), corr.end());
    BOOST_CHECK(corr == expected);

    std::tie(found, corr) = finder.findAndBlock();
    BOOST_REQUIRE(!found);

    BOOST_REQUIRE(finder.moreCorrectionSets());

    // MCSes {a0, a1, a5} and {a2, a3, a4}
    finder.setCardinality(3);
    CorrectionSet expected1 = {a0, a1, a5};
    CorrectionSet expected2 = {a2, a3, a4};
    std::sort(expected1.begin(), expected1.end());
    std::sort(expected2.begin(), expected2.end());

    CorrectionSet actual1, actual2;
    std::tie(found, actual1) = finder.findAndBlock();
    BOOST_REQUIRE(found);
    std::tie(found, actual2) = finder.findAndBlock();
    BOOST_REQUIRE(found);

    BOOST_CHECK_EQUAL(actual1.size(), 3);
    BOOST_CHECK_EQUAL(actual2.size(), 3);

    std::tie(found, corr) = finder.findAndBlock();
    BOOST_REQUIRE(!found);

    std::sort(actual1.begin(), actual1.end());
    std::sort(actual2.begin(), actual2.end());

    BOOST_REQUIRE(actual1 != actual2);
    BOOST_CHECK(actual1 == expected1 || actual1 == expected2);
    BOOST_CHECK(actual2 == expected1 || actual2 == expected2);

    BOOST_REQUIRE(finder.moreCorrectionSets());

    // MCS {a0, a1, a2, a3}
    finder.setCardinality(4);
    std::tie(found, corr) = finder.findAndBlock();
    BOOST_REQUIRE(found);
    BOOST_CHECK_EQUAL(corr.size(), 4);
    expected = {a0, a1, a2, a3};
    std::sort(expected.begin(), expected.end());
    BOOST_CHECK(corr == expected);

    std::tie(found, corr) = finder.findAndBlock();
    BOOST_REQUIRE(!found);

    BOOST_REQUIRE(!finder.moreCorrectionSets());
}

BOOST_AUTO_TEST_CASE(mcs_over_gates)
{
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    std::vector<ID> all = {a0, a1, a2, a3, a4, a5, a6};

    MCSFinder finder(f.vars, *f.tr);

    BOOST_REQUIRE(finder.moreCorrectionSets());

    bool found;
    CorrectionSet corr;

    // MCS {a6}
    finder.setCardinality(1);

    std::tie(found, corr) = finder.findAndBlockOverGates({a4});
    BOOST_CHECK(!found);

    std::tie(found, corr) = finder.findAndBlockOverGates({a4, a1});
    BOOST_CHECK(!found);

    std::tie(found, corr) = finder.findAndBlockOverGates(all);
    BOOST_REQUIRE(found);
    BOOST_CHECK_EQUAL(corr.size(), 1);
    BOOST_CHECK_EQUAL(corr.at(0), a6);

    BOOST_REQUIRE(finder.moreCorrectionSets());

    // MCS {a4, a5}
    finder.setCardinality(2);
    std::tie(found, corr) = finder.findAndBlockOverGates({a6, a5});
    BOOST_CHECK(!found);

    std::tie(found, corr) = finder.findAndBlockOverGates({a6, a4});
    BOOST_CHECK(!found);

    std::tie(found, corr) = finder.findAndBlockOverGates({a0, a1, a2, a3, a4, a5});

    BOOST_REQUIRE(found);
    BOOST_CHECK_EQUAL(corr.size(), 2);
    CorrectionSet expected = {a4, a5};
    std::sort(expected.begin(), expected.end());
    std::sort(corr.begin(), corr.end());
    BOOST_CHECK(corr == expected);

    std::tie(found, corr) = finder.findAndBlock();
    BOOST_CHECK(!found);

    std::tie(found, corr) = finder.findAndBlockOverGates({a4, a5});
    BOOST_CHECK(!found);
}

// Returns true if one element of all is contained within soln
bool containsOneOf(CorrectionSet soln,
                   const std::vector<CorrectionSet> all)
{
    std::sort(soln.begin(), soln.end());

    for (const CorrectionSet & corr : all)
    {
        bool subset = std::includes(soln.begin(), soln.end(), corr.begin(), corr.end());
        if (subset) { return true; }
    }

    return false;
}

BOOST_AUTO_TEST_CASE(approximate_mcs_over_gates)
{
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    std::vector<ID> all = {a0, a1, a2, a3, a4, a5, a6};

    ApproximateMCSFinder finder(f.vars, *f.tr);

    // block MCS {a6} as the approximate finder assumes size-1 MCSes are
    // already blocked
    finder.blockSolution({a6});

    // Remaining MCSes are:
    // {a4, a5}
    // {a0, a1, a5}
    // {a2, a3, a4}
    // {a0, a1, a2, a3}
    std::vector<CorrectionSet> all_mcs;
    all_mcs.push_back({a4, a5});
    all_mcs.push_back({a0, a1, a5});
    all_mcs.push_back({a2, a3, a4});
    all_mcs.push_back({a0, a1, a2, a3});

    for (CorrectionSet & corr : all_mcs)
    {
        std::sort(corr.begin(), corr.end());
    }

    // Set an upper limit of 63 solutions (sum over i=1..6 of 6 choose i)
    unsigned count = 0;
    bool found = true;
    while (found && count <= 63)
    {
        CorrectionSet corr;
        std::tie(found, corr) = finder.findAndBlockOverGates(all);
        if (found)
        {
            BOOST_CHECK(containsOneOf(corr, all_mcs));
            count++;
        }
    }

    // All solutions should've been found
    std::tie(found, std::ignore) = finder.findAndBlockOverGates(all);
    BOOST_CHECK(!found);
}

void sortVector(std::vector<CorrectionSet> & mcses)
{
    for (auto & mcs : mcses)
    {
        std::sort(mcs.begin(), mcs.end());
    }
    std::sort(mcses.begin(), mcses.end());
}

template<class Finder>
void testFindMCS(bool guaranteed_minimal)
{
    // The MCSes are:
    // {a6}
    // {a4, a5}
    // {a0, a1, a5}
    // {a2, a3, a4}
    // {a0, a1, a2, a3}
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    Finder finder(f.vars, *f.tr);

    BOOST_REQUIRE(finder.moreCorrectionSets());
    BOOST_REQUIRE(finder.moreCorrectionSets(1));
    BOOST_REQUIRE(finder.moreCorrectionSets(4));
    BOOST_REQUIRE(finder.moreCorrectionSets(6));
    BOOST_REQUIRE(finder.moreCorrectionSets(10));

    bool found;
    CorrectionSet corr;

    // MCS {a6}
    std::tie(found, corr) = finder.findNext();

    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(corr); }
    BOOST_CHECK_EQUAL(corr.size(), 1);
    BOOST_CHECK_EQUAL(corr.at(0), a6);

    BOOST_REQUIRE(!finder.moreCorrectionSets(1));
    BOOST_REQUIRE(finder.moreCorrectionSets());
    BOOST_REQUIRE(finder.moreCorrectionSets(2));

    // MCS {a4, a5}
    std::tie(found, corr) = finder.findNext();

    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(corr); }
    BOOST_CHECK_EQUAL(corr.size(), 2);
    CorrectionSet expected = {a4, a5};
    std::sort(expected.begin(), expected.end());
    std::sort(corr.begin(), corr.end());
    BOOST_CHECK(corr == expected);

    BOOST_REQUIRE(finder.moreCorrectionSets());
    BOOST_REQUIRE(finder.moreCorrectionSets(3));
    BOOST_REQUIRE(!finder.moreCorrectionSets(2));
    BOOST_REQUIRE(!finder.moreCorrectionSets(1));

    // MCSes {a0, a1, a5} and {a2, a3, a4}
    CorrectionSet expected1 = {a0, a1, a5};
    CorrectionSet expected2 = {a2, a3, a4};
    std::sort(expected1.begin(), expected1.end());
    std::sort(expected2.begin(), expected2.end());

    CorrectionSet actual1, actual2;
    std::tie(found, actual1) = finder.findNext();
    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(actual1); }
    BOOST_REQUIRE(finder.moreCorrectionSets(3));

    std::tie(found, actual2) = finder.findNext();
    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(actual2); }

    BOOST_CHECK(!finder.moreCorrectionSets(3));
    BOOST_CHECK(finder.moreCorrectionSets());
    BOOST_CHECK(finder.moreCorrectionSets(4));

    BOOST_CHECK_EQUAL(actual1.size(), 3);
    BOOST_CHECK_EQUAL(actual2.size(), 3);

    std::sort(actual1.begin(), actual1.end());
    std::sort(actual2.begin(), actual2.end());

    BOOST_REQUIRE(actual1 != actual2);
    BOOST_CHECK(actual1 == expected1 || actual1 == expected2);
    BOOST_CHECK(actual2 == expected1 || actual2 == expected2);

    // MCS {a0, a1, a2, a3}
    std::tie(found, corr) = finder.findNext();
    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(corr); }
    BOOST_CHECK_EQUAL(corr.size(), 4);
    expected = {a0, a1, a2, a3};
    std::sort(expected.begin(), expected.end());
    BOOST_CHECK(corr == expected);

    std::tie(found, corr) = finder.findNext();
    BOOST_REQUIRE(!found);

    BOOST_REQUIRE(!finder.moreCorrectionSets(1));
    BOOST_REQUIRE(!finder.moreCorrectionSets(4));
    BOOST_REQUIRE(!finder.moreCorrectionSets(10));
    BOOST_REQUIRE(!finder.moreCorrectionSets());
}

template<class Finder>
void testFindMCSExternallyBlocked(bool guaranteed_minimal)
{
    // The MCSes are:
    // {a6}
    // {a4, a5}
    // {a0, a1, a5}
    // {a2, a3, a4}
    // {a0, a1, a2, a3}
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    Finder finder(f.vars, *f.tr);

    finder.block({a5, a4});
    finder.block({a0, a1, a5});

    BOOST_REQUIRE(finder.moreCorrectionSets());
    BOOST_REQUIRE(finder.moreCorrectionSets(1));
    BOOST_REQUIRE(finder.moreCorrectionSets(4));
    BOOST_REQUIRE(finder.moreCorrectionSets(6));
    BOOST_REQUIRE(finder.moreCorrectionSets(10));

    bool found;
    CorrectionSet corr;

    // MCS {a6}
    std::tie(found, corr) = finder.findNext();

    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(corr); }
    BOOST_CHECK_EQUAL(corr.size(), 1);
    BOOST_CHECK_EQUAL(corr.at(0), a6);

    BOOST_REQUIRE(!finder.moreCorrectionSets(1));
    BOOST_REQUIRE(finder.moreCorrectionSets());
    BOOST_REQUIRE(finder.moreCorrectionSets(3));

    // MCSes {a2, a3, a4}
    CorrectionSet expected = {a2, a3, a4};
    std::sort(expected.begin(), expected.end());

    std::tie(found, corr) = finder.findNext();
    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(corr); }

    BOOST_CHECK(!finder.moreCorrectionSets(3));
    BOOST_CHECK(finder.moreCorrectionSets(4));

    BOOST_CHECK_EQUAL(corr.size(), 3);
    std::sort(corr.begin(), corr.end());

    BOOST_CHECK(corr == expected);

    // MCS {a0, a1, a2, a3}
    std::tie(found, corr) = finder.findNext();
    BOOST_REQUIRE(found);
    if (!guaranteed_minimal) { f.shrink(corr); }
    BOOST_CHECK_EQUAL(corr.size(), 4);
    expected = {a0, a1, a2, a3};
    std::sort(expected.begin(), expected.end());
    BOOST_CHECK(corr == expected);

    std::tie(found, corr) = finder.findNext();
    BOOST_REQUIRE(!found);

    BOOST_REQUIRE(!finder.moreCorrectionSets());
}

template<class Finder>
void testFindAll()
{
    // The MCSes are:
    // {a6}
    // {a4, a5}
    // {a0, a1, a5}
    // {a2, a3, a4}
    // {a0, a1, a2, a3}
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    std::vector<CorrectionSet> expected;
    expected.push_back({a6});
    expected.push_back({a4, a5});
    expected.push_back({a0, a1, a5});
    expected.push_back({a2, a3, a4});
    expected.push_back({a0, a1, a2, a3});
    sortVector(expected);

    // Check for cardinality 2
    Finder finder(f.vars, *f.tr);
    std::vector<CorrectionSet> actual = finder.findAll(2);
    BOOST_REQUIRE_EQUAL(actual.size(), 2);
    sortVector(actual);

    for (const auto & mcs : actual)
    {
        BOOST_CHECK(mcs.size() <= 2);
        auto it = std::find(expected.begin(), expected.end(), mcs);
        BOOST_CHECK(it != expected.end());
    }

    // Check for all cardinalities
    Finder finder2(f.vars, *f.tr);
    actual = finder2.findAll(8);
    BOOST_REQUIRE_EQUAL(actual.size(), expected.size());
    sortVector(actual);

    // Check there are no duplicates
    std::set<CorrectionSet> actual_set(actual.begin(), actual.end());
    BOOST_REQUIRE_EQUAL(actual_set.size(), actual.size());

    // Check every MCS was found
    for (const auto & mcs : actual)
    {
        auto it = std::find(expected.begin(), expected.end(), mcs);
        BOOST_CHECK(it != expected.end());
    }
}

template<class Finder>
void testFindBatch()
{
    // The MCSes are:
    // {a6}
    // {a4, a5}
    // {a0, a1, a5}
    // {a2, a3, a4}
    // {a0, a1, a2, a3}
    CorrectionSetFixture f;

    ID a0 = f.tr->toInternal(f.a0);
    ID a1 = f.tr->toInternal(f.a1);
    ID a2 = f.tr->toInternal(f.a2);
    ID a3 = f.tr->toInternal(f.a3);
    ID a4 = f.tr->toInternal(f.a4);
    ID a5 = f.tr->toInternal(f.a5);
    ID a6 = f.tr->toInternal(f.a6);

    std::vector<CorrectionSet> expected;
    expected.push_back({a6});
    expected.push_back({a4, a5});
    expected.push_back({a0, a1, a5});
    expected.push_back({a2, a3, a4});
    expected.push_back({a0, a1, a2, a3});
    sortVector(expected);

    // Check for cardinality 2
    Finder finder(f.vars, *f.tr);
    std::vector<CorrectionSet> actual = finder.findBatch(2);
    sortVector(actual);

    for (const auto & mcs : actual)
    {
        BOOST_CHECK(mcs.size() <= 2);
        // Iterate over expected to see if one of them is contained in mcs
        bool found = false;
        for (const auto & expected_mcs : expected)
        {
            if (std::includes(mcs.begin(), mcs.end(), expected_mcs.begin(), expected_mcs.end()))
            {
                found = true;
                break;
            }
        }

        BOOST_REQUIRE(found);
    }

    // Check for all cardinalities
    Finder finder2(f.vars, *f.tr);
    actual = finder2.findBatch(5);
    BOOST_REQUIRE_EQUAL(actual.size(), expected.size());
    sortVector(actual);

    for (const auto & mcs : actual)
    {
        BOOST_CHECK(mcs.size() <= 5);
        // Iterate over expected to see if one of them is contained in mcs
        bool found = false;
        for (const auto & expected_mcs : expected)
        {
            if (std::includes(mcs.begin(), mcs.end(), expected_mcs.begin(), expected_mcs.end()))
            {
                found = true;
                break;
            }
        }

        BOOST_REQUIRE(found);
    }
}

BOOST_AUTO_TEST_CASE(basic_mcs_finder)
{
    testFindMCS<BasicMCSFinder>(true);
    testFindMCSExternallyBlocked<BasicMCSFinder>(true);
    testFindAll<BasicMCSFinder>();
    testFindBatch<BasicMCSFinder>();
}

BOOST_AUTO_TEST_CASE(bmc_finder)
{
    testFindMCS<BMCCorrectionSetFinder>(false);
    testFindMCSExternallyBlocked<BMCCorrectionSetFinder>(false);
    testFindAll<BMCCorrectionSetFinder>();
    testFindBatch<BMCCorrectionSetFinder>();
}

max_stars_repo_name: ryanberryhill/pme

max_issues_repo_name: ryanberryhill/pme

max_forks_repo_name: ryanberryhill/pme


text:
#pragma once

#include <cstdint>
#include <string>

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

#include "minimal_serializer/serializer.hpp"

namespace pgl {
	// 8 bytes
	struct datetime final : private boost::less_than_comparable<datetime>, boost::equality_comparable<datetime> {
		datetime() = default;
		datetime(int year, int month, int day);
		datetime(int year, int month, int day, int hour, int minute, int second);

		[[nodiscard]] int year() const;

		[[nodiscard]] int month() const;

		[[nodiscard]] int day() const;

		[[nodiscard]] int hour() const;

		[[nodiscard]] int minute() const;

		[[nodiscard]] int second() const;

		[[nodiscard]] size_t get_hash() const;

		bool operator<(const datetime& other) const;

		bool operator==(const datetime& other) const;

		static datetime now();

		void on_serialize(minimal_serializer::serializer& serializer) {
			serializer += unix_time_;
		}

	private:
		int64_t unix_time_;
	};

	std::string get_now_datetime_string();
}

namespace boost {
	inline size_t hash_value(const pgl::datetime& datetime) {
		return datetime.get_hash();
	}
}

namespace std {
	template <>
	struct hash<pgl::datetime> {
		size_t operator()(const pgl::datetime& datetime) const noexcept {
			return boost::hash_value(datetime);
		}
	};
}

max_stars_repo_name: InstytutXR/PlanetaMatchMaker

max_issues_repo_name: InstytutXR/PlanetaMatchMaker

max_forks_repo_name: InstytutXR/PlanetaMatchMaker


text:
#pragma once

#ifdef USEWINSDK
#include <windows.h>
#include <boost/optional.hpp>

#include <QLibrary>

namespace chatterino {
namespace util {
static bool tryHandleDpiChangedMessage(void *message, int &dpi)
{
    MSG *msg = reinterpret_cast<MSG *>(message);

    // WM_DPICHANGED
    if (msg->message == 0x02E0) {
        dpi = HIWORD(msg->wParam);

        return true;
    }
    return false;
}

static boost::optional<UINT> getWindowDpi(quintptr ptr)
{
    typedef UINT(WINAPI * GetDpiForWindow)(HWND);
    QLibrary user32("user32.dll", NULL);

    GetDpiForWindow getDpiForWindow = (GetDpiForWindow)user32.resolve("GetDpiForWindow");

    if (getDpiForWindow) {
        UINT value = getDpiForWindow((HWND)ptr);

        return value == 0 ? boost::none : boost::optional<UINT>(value);
    }

    return boost::none;
}
}  // namespace util
}  // namespace chatterino

#endif


text:
// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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 "Registration.h"

#include <cstdlib>
#include <ctime>
#include <iostream>

#include <Eigen/Dense>
#include <Core/Utility/Console.h>
#include <Core/Geometry/PointCloud.h>
#include <Core/Geometry/KDTreeFlann.h>
#include <Core/Registration/Feature.h>

namespace open3d {

namespace {

double compute_distance_squared(const Eigen::Vector3d p1, const Eigen::Vector3d p2){
    Eigen::Vector3d diff = p1 - p2;
    return diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2];
}

int compute_closest_vertex(const Eigen::Vector3d best_intpoint, int f, const TriangleMesh target){
    int vertex0 = target.triangles_[f][0];
    int vertex1 = target.triangles_[f][1];
    int vertex2 = target.triangles_[f][2];
    double d0 = compute_distance_squared(best_intpoint, target.vertices_[vertex0]);
    double d1 = compute_distance_squared(best_intpoint, target.vertices_[vertex1]);
    double d2 = compute_distance_squared(best_intpoint, target.vertices_[vertex2]);
    //PrintInfo("d0: %.2f\n", std::sqrt(d0));
    //PrintInfo("d1: %.2f\n", std::sqrt(d1));
    //PrintInfo("d2: %.2f\n", std::sqrt(d2));
    if(d0 <= d1 && d0 <= d2){
        //PrintInfo("picking d0\n");
        return vertex0;
    }
    else{
        if(d1 <= d0 && d1 <= d2){
            //PrintInfo("picking d1\n");
            return vertex1;
        }
        else{
            if(d2 <= d0 && d2 <= d1){
                //PrintInfo("picking d2\n");
                return vertex2;
            }
            else{
                //PrintInfo("Big mistake\n");
                return -1;
            }
        }
    }
}

// Returns true if a ray intersects a triangle.
bool RayIntersectsTriangle(const Eigen::Vector3d rayOrigin,
                        const Eigen::Vector3d rayVector,
                        const Eigen::Vector3d vertex0,
                        const Eigen::Vector3d vertex1,
                        const Eigen::Vector3d vertex2,
                        Eigen::Vector3d& outIntersectionPoint)
{
    const float EPSILON = 0.0000001;
    Eigen::Vector3d edge1, edge2, h, s, q;
    float a,f,u,v;
    edge1 = vertex1 - vertex0;
    edge2 = vertex2 - vertex0;
    h = rayVector.cross(edge2);
    a = edge1.dot(h);
    if (a > -EPSILON && a < EPSILON)
        return false;    // This ray is parallel to this triangle.
    f = 1.0/a;
    s = rayOrigin - vertex0;
    u = f * (s.dot(h));
    if (u < 0.0 || u > 1.0)
        return false;
    q = s.cross(edge1);
    v = f * rayVector.dot(q);
    if (v < 0.0 || u + v > 1.0)
        return false;
    // At this stage we can compute t to find out where the intersection point is on the line.
    float t = f * edge2.dot(q);
    if (t > EPSILON) // ray intersection
    {
        outIntersectionPoint = rayOrigin + rayVector * t;
        return true;
    }
    else // This means that there is a line intersection but not a ray intersection.
        return false;
}

// For each of the neighboring triangles to each point, find which one is intersected by the line.
void find_sc_corr(const TriangleMesh &target, const std::vector<int> &neigh_triangles,\
        const Eigen::Vector3d &source_point, const Eigen::Vector3d &source_normal,\
        double max_correspondence_distance, \
        std::vector<int> &target_corr, std::vector<double> & sc){

    // For each triangle in the set, find if it is intersecting.
    Eigen::Vector3d best_intpoint(0.0, 0.0, 0.0);
    double best_intpoint_dist = 100000000;
    int intpoint_face_ix = -1;
    for (int i = 0; i < (int) neigh_triangles.size(); i++){
        Eigen::Vector3i f = target.triangles_[neigh_triangles[i]];

        Eigen::Vector3d intPoint;
        bool intersect = RayIntersectsTriangle(source_point, source_normal, \
                                                target.vertices_[f[0]], target.vertices_[f[1]],\
                                                target.vertices_[f[2]], intPoint);
        if(!intersect){
            continue;
        }

        double dist2 = compute_distance_squared(intPoint, source_point);
        if(dist2 < best_intpoint_dist){
            best_intpoint_dist = dist2;
            best_intpoint = intPoint;
            intpoint_face_ix = neigh_triangles[i];
        }
    }
    if(intpoint_face_ix >= 0){
        int f = intpoint_face_ix;

        // Pick the vertex closest to the intersection point.
        target_corr[0] = compute_closest_vertex(best_intpoint, f, target);
        double dist2 = compute_distance_squared(target.vertices_[target_corr[0]], best_intpoint);
        Eigen::Vector3d v = best_intpoint;
        //PrintInfo("--------------\n");
        //PrintInfo("Intpoint: %.2f %.2f %.2f\n", v[0], v[1], v[2]);
        v = target.vertices_[target_corr[0]];
        //PrintInfo("vrtpoint: %.2f %.2f %.2f\n", v[0], v[1], v[2]);
        //PrintInfo("vrtpoint to intpoint: %.2f\n", std::sqrt(dist2));
        v = source_point;
        //PrintInfo("Srcpoint: %.2f %.2f %.2f\n", v[0], v[1], v[2]);
        dist2 = compute_distance_squared(target.vertices_[target_corr[0]], source_point);

        // Compute shape complementarity
        sc[0] = target.vertex_normals_[target_corr[0]].dot(-source_normal);
        //PrintInfo("normal mult: %f\n", sc[0]);
        sc[0] = sc[0]*std::exp(-0.5*(dist2));
        //PrintInfo("shape comp: %f \n", sc[0]);
    }
    else{
        target_corr[0] = -1;
        sc[0] = 0.0;
    }
}

    //
// PGC 2019: Get registration results based on shape complementarity
RegistrationResult GetRegistrationResultAndCorrespondencesShapeComplementarity(
        const PointCloud &source,
        const TriangleMesh &target,
        const KDTreeFlann &target_faces_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation,
        const Feature &source_feature,
        const Feature &target_feature,
        const int fitness_type
        ) {

    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return std::move(result);
    }

    double error2 = 0.0;

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        // Compute the correspondences based on shape complementarity.
        // First find all target points within X of each source point. X=max_correspondence_distance
        // Then get all the triangles on which each target point participates.
        // Find the intersection of the line between source point/normal  and each of the triangles.
        // If the intersection is less than 2, we are good.
        // Find the nearest neighbor to the intersection point.
        for (int i = 0; i < (int)source.points_.size(); i++) {
            std::vector<int> indices(20);
            std::vector<double> dists(20);
            std::vector<int> target_corr(1);
            std::vector<double> target_sc(1);
            const auto &point = source.points_[i];
            const auto &normal= source.normals_[i];
            // Identify all faces within X of the target, up to 20
            if (target_faces_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           20, indices, dists) > 0) {
                // Find the intersection of the line between source point/normal
                //      and each of the triangles.
                find_sc_corr(target, indices, point, normal, max_correspondence_distance, \
                        target_corr, target_sc);

                if(target_sc[0] > 0){
                    //PrintInfo("Adding correspondences: %d %d\n", i, target_corr[0]);
                    error2_private += target_sc[0];
                    correspondence_set_private.push_back(
                        Eigen::Vector2i(i, target_corr[0]));
                }
            }
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif

    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 1000.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        //result.fitness_ = (double)corres_number / (double)source.points_.size();
        if(fitness_type == 1){
            // PGC 2019: fitness: the number of correspondences
            result.fitness_ = (double)corres_number;
        }
        else{
            if(fitness_type == 2){
                // Compute the descriptor distances for all pairs of correspondences.
                double myfitness = 0;
                for (int i = 0; i < (int)result.correspondence_set_.size(); i++) {
                    int s_vix =  result.correspondence_set_[i][0];
                    int t_vix = result.correspondence_set_[i][1];
                    Eigen::VectorXd feat_s = Eigen::VectorXd(source_feature.data_.col(s_vix));
                    Eigen::VectorXd feat_t = Eigen::VectorXd(target_feature.data_.col(t_vix));
                    double desc_dist = 0.0;
//                    std::cout << "Dimension = " << source_feature.Dimension() << std::endl;
//                    std::cout << "Size = " << source_feature.Num() << std::endl;
                    for (int j = 0; j < source_feature.Dimension(); j++){
                        double dist = feat_t[j] - feat_s[j];
                        dist = dist*dist;
                        desc_dist += dist;
                    }

                    myfitness += 1.0/desc_dist;
                }

                result.fitness_ = myfitness;

            }
            else{
                if (fitness_type == 3.0){ // Use shape complementarity (which is in the error).
                    result.fitness_ = error2 / (double)source.points_.size();
                }
                else{
                    result.fitness_ = (double)corres_number / (double)source.points_.size();
                }
            }
        }
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return std::move(result);
}

// PGC 2019: include the fitness type and the features for a custom fitness function.
RegistrationResult GetRegistrationResultAndCorrespondencesCustom(
        const PointCloud &source,
        const PointCloud &target,
        const KDTreeFlann &target_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation,
        const Feature &source_feature,
        const Feature &target_feature,
        const int fitness_type
        ) {
    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return std::move(result);
    }

    double error2 = 0.0;

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int i = 0; i < (int)source.points_.size(); i++) {
            std::vector<int> indices(1);
            std::vector<double> dists(1);
            const auto &point = source.points_[i];
            if (target_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           1, indices, dists) > 0) {
                error2_private += dists[0];
                correspondence_set_private.push_back(
                        Eigen::Vector2i(i, indices[0]));
            }
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif

    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        //result.fitness_ = (double)corres_number / (double)source.points_.size();
        if(fitness_type == 1){
            // PGC 2019: fitness: the number of correspondences
            result.fitness_ = (double)corres_number;
        }
        else{
            if(fitness_type == 2){
                // Compute the descriptor distances for all pairs of correspondences.
                double myfitness = 0;
                for (int i = 0; i < (int)result.correspondence_set_.size(); i++) {
                    int s_vix =  result.correspondence_set_[i][0];
                    int t_vix = result.correspondence_set_[i][1];
                    Eigen::VectorXd feat_s = Eigen::VectorXd(source_feature.data_.col(s_vix));
                    Eigen::VectorXd feat_t = Eigen::VectorXd(target_feature.data_.col(t_vix));
                    double desc_dist = 0.0;
//                    std::cout << "Dimension = " << source_feature.Dimension() << std::endl;
//                    std::cout << "Size = " << source_feature.Num() << std::endl;
                    for (int j = 0; j < source_feature.Dimension(); j++){
                        double dist = feat_t[j] - feat_s[j];
                        dist = dist*dist;
                        desc_dist += dist;
                    }

                    myfitness += 1.0/desc_dist;
                }

                result.fitness_ = myfitness;

            }
            else{
                result.fitness_ = (double)corres_number / (double)source.points_.size();
            }
        }
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return std::move(result);
}

RegistrationResult GetRegistrationResultAndCorrespondences(
        const PointCloud &source,
        const PointCloud &target,
        const KDTreeFlann &target_kdtree,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    RegistrationResult result(transformation);
    if (max_correspondence_distance <= 0.0) {
        return std::move(result);
    }

    double error2 = 0.0;

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        double error2_private = 0.0;
        CorrespondenceSet correspondence_set_private;
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int i = 0; i < (int)source.points_.size(); i++) {
            std::vector<int> indices(1);
            std::vector<double> dists(1);
            const auto &point = source.points_[i];
            if (target_kdtree.SearchHybrid(point, max_correspondence_distance,
                                           1, indices, dists) > 0) {
                error2_private += dists[0];
                correspondence_set_private.push_back(
                        Eigen::Vector2i(i, indices[0]));
            }
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            for (int i = 0; i < (int)correspondence_set_private.size(); i++) {
                result.correspondence_set_.push_back(
                        correspondence_set_private[i]);
            }
            error2 += error2_private;
        }
#ifdef _OPENMP
    }
#endif

    if (result.correspondence_set_.empty()) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        size_t corres_number = result.correspondence_set_.size();
        //result.fitness_ = (double)corres_number / (double)source.points_.size();
        // PGC 2019: fitness is the number of correspondences, not the fraction.
        result.fitness_ = (double)corres_number;
        result.inlier_rmse_ = std::sqrt(error2 / (double)corres_number);
    }
    return std::move(result);
}

RegistrationResult EvaluateRANSACBasedOnCorrespondence(
        const PointCloud &source,
        const PointCloud &target,
        const CorrespondenceSet &corres,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    RegistrationResult result(transformation);
    double error2 = 0.0;
    int good = 0;
    double max_dis2 = max_correspondence_distance * max_correspondence_distance;
    for (const auto &c : corres) {
        double dis2 =
                (source.points_[c[0]] - target.points_[c[1]]).squaredNorm();
        if (dis2 < max_dis2) {
            good++;
            error2 += dis2;
        }
    }
    if (good == 0) {
        result.fitness_ = 0.0;
        result.inlier_rmse_ = 0.0;
    } else {
        result.fitness_ = (double)good / (double)corres.size();
        result.inlier_rmse_ = std::sqrt(error2 / (double)good);
    }
    return result;
}

}  // unnamed namespace

RegistrationResult EvaluateRegistration(
        const PointCloud &source,
        const PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d
                &transformation /* = Eigen::Matrix4d::Identity()*/) {
    KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
    PointCloud pcd = source;
    if (transformation.isIdentity() == false) {
        pcd.Transform(transformation);
    }
    return GetRegistrationResultAndCorrespondences(
            pcd, target, kdtree, max_correspondence_distance, transformation);
}

RegistrationResult RegistrationICP(
        const PointCloud &source,
        const PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        const ICPConvergenceCriteria
                &criteria /* = ICPConvergenceCriteria()*/) {
    if (max_correspondence_distance <= 0.0) {
        PrintError("Error: Invalid max_correspondence_distance.\n");
        return RegistrationResult(init);
    }
    if (estimation.GetTransformationEstimationType() ==
                TransformationEstimationType::PointToPlane &&
        (!source.HasNormals() || !target.HasNormals())) {
        PrintError(
                "Error: TransformationEstimationPointToPlane requires "
                "pre-computed normal vectors.\n");
        return RegistrationResult(init);
    }

    Eigen::Matrix4d transformation = init;
    KDTreeFlann kdtree;
    kdtree.SetGeometry(target);
    PointCloud pcd = source;
    if (init.isIdentity() == false) {
        pcd.Transform(init);
    }
    RegistrationResult result;
    result = GetRegistrationResultAndCorrespondences(
            pcd, target, kdtree, max_correspondence_distance, transformation);
    for (int i = 0; i < criteria.max_iteration_; i++) {
        PrintDebug("ICP Iteration #%d: Fitness %.4f, RMSE %.4f\n", i,
                   result.fitness_, result.inlier_rmse_);
        Eigen::Matrix4d update = estimation.ComputeTransformation(
                pcd, target, result.correspondence_set_);
        transformation = update * transformation;
        pcd.Transform(update);
        RegistrationResult backup = result;
        result = GetRegistrationResultAndCorrespondences(
                pcd, target, kdtree, max_correspondence_distance,
                transformation);
        if (std::abs(backup.fitness_ - result.fitness_) <
                    criteria.relative_fitness_ &&
            std::abs(backup.inlier_rmse_ - result.inlier_rmse_) <
                    criteria.relative_rmse_) {
            break;
        }
    }
    return result;
}

RegistrationResult RegistrationRANSACBasedOnCorrespondence(
        const PointCloud &source,
        const PointCloud &target,
        const CorrespondenceSet &corres,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 6*/,
        const RANSACConvergenceCriteria &criteria
        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || (int)corres.size() < ransac_n ||
        max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }
    std::srand((unsigned int)std::time(0));
    Eigen::Matrix4d transformation;
    CorrespondenceSet ransac_corres(ransac_n);
    RegistrationResult result;
    for (int itr = 0;
         itr < criteria.max_iteration_ && itr < criteria.max_validation_;
         itr++) {
        for (int j = 0; j < ransac_n; j++) {
            ransac_corres[j] = corres[std::rand() % (int)corres.size()];
        }
        transformation =
                estimation.ComputeTransformation(source, target, ransac_corres);
        PointCloud pcd = source;
        pcd.Transform(transformation);
        auto this_result = EvaluateRANSACBasedOnCorrespondence(
                pcd, target, corres, max_correspondence_distance,
                transformation);
        if (this_result.fitness_ > result.fitness_ ||
            (this_result.fitness_ == result.fitness_ &&
             this_result.inlier_rmse_ < result.inlier_rmse_)) {
            result = this_result;
        }
    }
    PrintDebug("RANSAC: Fitness %.4f, RMSE %.4f\n", result.fitness_,
               result.inlier_rmse_);
    return result;
}

RegistrationResult RegistrationRANSACBasedOnFeatureMatching(
        const PointCloud &source,
        const PointCloud &target,
        const Feature &source_feature,
        const Feature &target_feature,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 4*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria,
        const double ransac_random_seed, /* default: 0*/
        const int fitness_type /* 0: standard ransac fitness function. 1: use the number of inliers (instead of the ration); 2: use 1/d^2 for inliers, where d is descriptor distance.*/

        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }

    RegistrationResult result;
    int total_validation = 0;
    bool finished_validation = false;
    int num_similar_features = 1;
    std::vector<std::vector<int>> similar_features(source.points_.size());

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        CorrespondenceSet ransac_corres(ransac_n);
        KDTreeFlann kdtree(target);
        KDTreeFlann kdtree_feature(target_feature);
        RegistrationResult result_private;
        unsigned int seed_number;
#ifdef _OPENMP
        // each thread has different seed_number
        //seed_number = (unsigned int)std::time(0) * (omp_get_thread_num() + 1);
        seed_number = (unsigned int)ransac_random_seed * (omp_get_thread_num() + 1);
#else
        //seed_number = (unsigned int)std::time(0);
        seed_number = (unsigned int)ransac_random_seed;//std::time(0);
#endif
        std::srand(seed_number);

#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int itr = 0; itr < criteria.max_iteration_; itr++) {
            if (!finished_validation) {
                std::vector<double> dists(num_similar_features);
                Eigen::Matrix4d transformation;
                for (int j = 0; j < ransac_n; j++) {
                    int source_sample_id =
                            std::rand() % (int)source.points_.size();
                    if (similar_features[source_sample_id].empty()) {
                        std::vector<int> indices(num_similar_features);
                        kdtree_feature.SearchKNN(
                                Eigen::VectorXd(source_feature.data_.col(
                                        source_sample_id)),
                                num_similar_features, indices, dists);
#ifdef _OPENMP
#pragma omp critical
#endif
                        { similar_features[source_sample_id] = indices; }
                    }
                    ransac_corres[j](0) = source_sample_id;
                    if (num_similar_features == 1)
                        ransac_corres[j](1) =
                                similar_features[source_sample_id][0];
                    else
                        ransac_corres[j](1) =
                                similar_features[source_sample_id]
                                                [std::rand() %
                                                 num_similar_features];
                }
                bool check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == false &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                transformation = estimation.ComputeTransformation(
                        source, target, ransac_corres);
                check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == true &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                PointCloud pcd = source;
                pcd.Transform(transformation);
                auto this_result = GetRegistrationResultAndCorrespondencesCustom(
                        pcd, target, kdtree, max_correspondence_distance,
                        transformation, source_feature, target_feature, fitness_type);
                if (this_result.fitness_ > result_private.fitness_ ||
                    (this_result.fitness_ == result_private.fitness_ &&
                     this_result.inlier_rmse_ < result_private.inlier_rmse_)) {
                    result_private = this_result;
                }
#ifdef _OPENMP
#pragma omp critical
#endif
                {
                    total_validation = total_validation + 1;
                    if (total_validation >= criteria.max_validation_)
                        finished_validation = true;
                }
            }  // end of if statement
        }      // end of for-loop
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            if (result_private.fitness_ > result.fitness_ ||
                (result_private.fitness_ == result.fitness_ &&
                 result_private.inlier_rmse_ < result.inlier_rmse_)) {
                result = result_private;
            }
        }
#ifdef _OPENMP
    }
#endif
    PrintDebug("total_validation : %d\n", total_validation);
    PrintDebug("RANSAC: Fitness %.4f, RMSE %.4f\n", result.fitness_,
               result.inlier_rmse_);
    return result;
}

RegistrationResult RegistrationRANSACBasedOnShapeComplementarity(
        const PointCloud &source,
        const PointCloud &target,
        const TriangleMesh &target_mesh,
        const PointCloud &target_face_centroids_pcd,
        const Feature &source_feature,
        const Feature &target_feature,
        double max_correspondence_distance,
        const TransformationEstimation &estimation
        /* = TransformationEstimationPointToPoint(false)*/,
        int ransac_n /* = 4*/,
        const std::vector<std::reference_wrapper<const CorrespondenceChecker>>
                &checkers /* = {}*/,
        const RANSACConvergenceCriteria &criteria,
        const double ransac_random_seed, /* default: 0*/
        const int fitness_type /* 0: standard ransac fitness function. 1: use the number of inliers (instead of the ration); 2: use 1/d^2 for inliers, where d is descriptor distance.*/

        /* = RANSACConvergenceCriteria()*/) {
    if (ransac_n < 3 || max_correspondence_distance <= 0.0) {
        return RegistrationResult();
    }

    RegistrationResult result;
    int total_validation = 0;
    bool finished_validation = false;
    int num_similar_features = 1;
    std::vector<std::vector<int>> similar_features(source.points_.size());

#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        CorrespondenceSet ransac_corres(ransac_n);
        // Do a KDTree for the triangles.
        PointCloud target_triangle_centroids();
        KDTreeFlann kdtree(target_face_centroids_pcd);
        KDTreeFlann kdtree_feature(target_feature);
        RegistrationResult result_private;
        unsigned int seed_number;
#ifdef _OPENMP
        // each thread has different seed_number
        //seed_number = (unsigned int)std::time(0) * (omp_get_thread_num() + 1);
        seed_number = (unsigned int)ransac_random_seed * (omp_get_thread_num() + 1);
#else
        //seed_number = (unsigned int)std::time(0);
        seed_number = (unsigned int)ransac_random_seed;//std::time(0);
#endif
        std::srand(seed_number);

#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (int itr = 0; itr < criteria.max_iteration_; itr++) {
            if (!finished_validation) {
                std::vector<double> dists(num_similar_features);
                Eigen::Matrix4d transformation;
                for (int j = 0; j < ransac_n; j++) {
                    int source_sample_id =
                            std::rand() % (int)source.points_.size();
                    if (similar_features[source_sample_id].empty()) {
                        std::vector<int> indices(num_similar_features);
                        kdtree_feature.SearchKNN(
                                Eigen::VectorXd(source_feature.data_.col(
                                        source_sample_id)),
                                num_similar_features, indices, dists);
#ifdef _OPENMP
#pragma omp critical
#endif
                        { similar_features[source_sample_id] = indices; }
                    }
                    ransac_corres[j](0) = source_sample_id;
                    if (num_similar_features == 1)
                        ransac_corres[j](1) =
                                similar_features[source_sample_id][0];
                    else
                        ransac_corres[j](1) =
                                similar_features[source_sample_id]
                                                [std::rand() %
                                                 num_similar_features];
                }
                bool check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == false &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                transformation = estimation.ComputeTransformation(
                        source, target, ransac_corres);
                check = true;
                for (const auto &checker : checkers) {
                    if (checker.get().require_pointcloud_alignment_ == true &&
                        checker.get().Check(source, target, ransac_corres,
                                            transformation) == false) {
                        check = false;
                        break;
                    }
                }
                if (check == false) continue;
                PointCloud pcd = source;
                pcd.Transform(transformation);
                //PrintInfo("###################\n");
                //PrintInfo("###################\n");
                //PrintInfo("Evaluating transformation\n");
                auto this_result = GetRegistrationResultAndCorrespondencesShapeComplementarity(
                        pcd, target_mesh, kdtree, max_correspondence_distance,
                        transformation, source_feature, target_feature, fitness_type);
                if (this_result.fitness_ > result_private.fitness_ ||
                    (this_result.fitness_ == result_private.fitness_ &&
                     this_result.inlier_rmse_ < result_private.inlier_rmse_)) {
                    result_private = this_result;
                }
#ifdef _OPENMP
#pragma omp critical
#endif
                {
                    total_validation = total_validation + 1;
                    if (total_validation >= criteria.max_validation_)
                        finished_validation = true;
                }
            }  // end of if statement
        }      // end of for-loop
#ifdef _OPENMP
#pragma omp critical
#endif
        {
            if (result_private.fitness_ > result.fitness_ ||
                (result_private.fitness_ == result.fitness_ &&
                 result_private.inlier_rmse_ < result.inlier_rmse_)) {
                result = result_private;
            }
        }
#ifdef _OPENMP
    }
#endif
    PrintDebug("total_validation : %d\n", total_validation);
    PrintDebug("RANSAC: Fitness %.4f, RMSE %.4f\n", result.fitness_,
               result.inlier_rmse_);
    return result;
}

Eigen::Matrix6d GetInformationMatrixFromPointClouds(
        const PointCloud &source,
        const PointCloud &target,
        double max_correspondence_distance,
        const Eigen::Matrix4d &transformation) {
    PointCloud pcd = source;
    if (transformation.isIdentity() == false) {
        pcd.Transform(transformation);
    }
    RegistrationResult result;
    KDTreeFlann target_kdtree(target);
    result = GetRegistrationResultAndCorrespondences(
            pcd, target, target_kdtree, max_correspondence_distance,
            transformation);

    // write q^*
    // see http://redwood-data.org/indoor/registration.html
    // note: I comes first in this implementation
    Eigen::Matrix6d GTG = Eigen::Matrix6d::Identity();
#ifdef _OPENMP
#pragma omp parallel
    {
#endif
        Eigen::Matrix6d GTG_private = Eigen::Matrix6d::Identity();
        Eigen::Vector6d G_r_private = Eigen::Vector6d::Zero();
#ifdef _OPENMP
#pragma omp for nowait
#endif
        for (auto c = 0; c < result.correspondence_set_.size(); c++) {
            int t = result.correspondence_set_[c](1);
            double x = target.points_[t](0);
            double y = target.points_[t](1);
            double z = target.points_[t](2);
            G_r_private.setZero();
            G_r_private(1) = z;
            G_r_private(2) = -y;
            G_r_private(3) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = -z;
            G_r_private(2) = x;
            G_r_private(4) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
            G_r_private.setZero();
            G_r_private(0) = y;
            G_r_private(1) = -x;
            G_r_private(5) = 1.0;
            GTG_private.noalias() += G_r_private * G_r_private.transpose();
        }
#ifdef _OPENMP
#pragma omp critical
#endif
        { GTG += GTG_private; }
#ifdef _OPENMP
    }
#endif
    return std::move(GTG);
}

}  // namespace open3d