text:
// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>

#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/solver.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/types/sim3/types_seven_dof_expmap.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam3d/types_slam3d.h"
#include "g2o/types/slam3d/vertex_se3.h"

using namespace std;
using namespace g2o;

extern "C" void G2O_FACTORY_EXPORT g2o_type_VertexSE3(void);

// Convert SE3 Vertex to Sim3 Vertex
void ToVertexSim3(const g2o::VertexSE3& v_se3,
                  g2o::VertexSim3Expmap* const v_sim3) {
  Eigen::Isometry3d se3 = v_se3.estimate().inverse();
  Eigen::Matrix3d r = se3.rotation();
  Eigen::Vector3d t = se3.translation();
  g2o::Sim3 sim3(r, t, 1.0);

  v_sim3->setEstimate(sim3);
}

// Convert Sim3 Vertex to SE3 Vertex
void ToVertexSE3(const g2o::VertexSim3Expmap& v_sim3,
                 g2o::VertexSE3* const v_se3) {
  g2o::Sim3 sim3 = v_sim3.estimate().inverse();
  Eigen::Matrix3d r = sim3.rotation().toRotationMatrix();
  Eigen::Vector3d t = sim3.translation();
  Eigen::Isometry3d se3;
  se3 = r;
  se3.translation() = t;

  v_se3->setEstimate(se3);
}

// Converte EdgeSE3 to EdgeSim3
void ToEdgeSim3(const g2o::EdgeSE3& e_se3, g2o::EdgeSim3* const e_sim3) {
  Eigen::Isometry3d se3 = e_se3.measurement().inverse();
  Eigen::Matrix3d r = se3.rotation();
  Eigen::Vector3d t = se3.translation();
  g2o::Sim3 sim3(r, t, 1.0);

  e_sim3->setMeasurement(sim3);
}

// Using VertexSim3 and EdgeSim3 is the core of this example.
// This example optimize the data created by create_sphere.
// Because the data is recore by VertexSE3 and EdgeSE3, SE3 is used for
// interface and Sim is used for optimization.
// g2o_viewer is avaliable to the result.

int main(int argc, char** argv) {
  g2o_type_VertexSE3();
  if (argc != 2) {
    cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
    return 1;
  }
  ifstream fin(argv[1]);
  if (!fin) {
    cout << "file " << argv[1] << " does not exist." << endl;
    return 1;
  }

  //  define the optimizer
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<7, 7>> BlockSolverType;
  typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType>
      LinearSolverType;
  auto solver = new g2o::OptimizationAlgorithmLevenberg(
      g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));

  g2o::SparseOptimizer optimizer;
  optimizer.setAlgorithm(solver);
  optimizer.setVerbose(true);

  // Load and Save in SE3
  g2o::SparseOptimizer interface;
  interface.load(argv[1]);

  // Convert all vertices
  for (auto& tmp : interface.vertices()) {
    const int& id = tmp.first;
    g2o::VertexSE3* v_se3 = static_cast<g2o::VertexSE3*>(tmp.second);
    g2o::VertexSim3Expmap* v_sim3 = new g2o::VertexSim3Expmap();
    v_sim3->setId(id);
    v_sim3->setMarginalized(false);

    ToVertexSim3(*v_se3, v_sim3);
    optimizer.addVertex(v_sim3);
    if (id == 0) {
      v_sim3->setFixed(true);
    }
  }

  // Convert all edges
  int edge_index = 0;
  for (auto& tmp : interface.edges()) {
    g2o::EdgeSE3* e_se3 = static_cast<g2o::EdgeSE3*>(tmp);
    int idx0 = e_se3->vertex(0)->id();
    int idx1 = e_se3->vertex(1)->id();
    g2o::EdgeSim3* e_sim3 = new g2o::EdgeSim3();

    ToEdgeSim3(*e_se3, e_sim3);
    e_sim3->setId(edge_index++);
    e_sim3->setVertex(0, optimizer.vertices()[idx0]);
    e_sim3->setVertex(1, optimizer.vertices()[idx1]);
    e_sim3->information() = Eigen::Matrix<double, 7, 7>::Identity();

    optimizer.addEdge(e_sim3);
  }

  cout << "optimizing ..." << endl;
  optimizer.initializeOptimization();
  optimizer.optimize(30);

  cout << "saving optimization results in VertexSE3..." << endl;
  auto vertices_sim3 = optimizer.vertices();
  auto vertices_se3 = interface.vertices();

  for (auto& tmp : vertices_sim3) {
    const int& id = tmp.first;
    g2o::VertexSim3Expmap* v_sim3 =
        static_cast<g2o::VertexSim3Expmap*>(tmp.second);
    g2o::VertexSE3* v_se3 = static_cast<g2o::VertexSE3*>(vertices_se3[id]);

    ToVertexSE3(*v_sim3, v_se3);
  }

  interface.save("result.g2o");
  return 0;
}


text:
#include <iostream>
#include <chrono>
#include <thread>
#include <boost/asio.hpp>

using boost::asio::ip::tcp;

int main() {
    try{
        boost::asio::io_service io_service;

        tcp::socket s(io_service);
        tcp::resolver resolver(io_service); // 存储服务器的地址信息（IP，端口）。

        boost::asio::connect(s, resolver.resolve({"127.0.0.1", "9000"})); // 连接服务器，执行 TCP 三次握手的过程。
        std::this_thread::sleep_for(std::chrono::seconds(2)); // 本线程睡眠两秒

    }
    catch (std::exception& e)
    {
        std::cerr << "Exception: " << e.what() << "\n";
    }

    return 0;
}

ext: cpp

max_stars_repo_name: eftales/RPC-code

max_issues_repo_name: eftales/RPC-code

max_forks_repo_name: eftales/RPC-code


text:
/*
 * InputProcessorTest.cpp
 *
 * Author: Anthony San Lucas
 * Email:  sanlucas@gmail.com
 */

#define BOOST_TEST_MODULE InputProcessorTest
#include <boost/assign/std/vector.hpp>
#include <boost/test/included/unit_test.hpp>
#include "InputProcessor.h"

using namespace boost::assign; //bring += into scope

BOOST_AUTO_TEST_SUITE(InputProcessorTest)

BOOST_AUTO_TEST_CASE(extractTest) {
	std::vector<char> alleles0;
	std::vector<char> alleles1;
	alleles0 += 'A', 'C', 'G', 'T';
	alleles1 += 'A', 'T', 'T', '?';

	std::vector< std::vector<char> > alleles;
	alleles += alleles0, alleles1;

	std::vector<unsigned long> hetIndices;
	hetIndices += 1, 2;

	hlsutil::InputProcessor inputProc;

	std::vector<double> freqs;
	freqs += 1.0, 0.0, 0.5, 0.4;
	std::vector<double> hetFreqs = inputProc.extract(hetIndices, freqs);
	BOOST_REQUIRE_EQUAL(hetIndices.size(), hetFreqs.size());
	BOOST_REQUIRE_EQUAL(hetFreqs[0], 0.0);
	BOOST_REQUIRE_EQUAL(hetFreqs[1], 0.5);

	std::vector< std::vector<char> > hetAlleles = inputProc.extract(hetIndices, alleles);
	BOOST_REQUIRE_EQUAL(hetIndices.size(), hetAlleles[0].size());
	BOOST_REQUIRE_EQUAL(hetIndices.size(), hetAlleles[1].size());
	BOOST_REQUIRE_EQUAL(hetAlleles[0][0], 'C');
	BOOST_REQUIRE_EQUAL(hetAlleles[0][1], 'G');
	BOOST_REQUIRE_EQUAL(hetAlleles[1][0], 'T');
	BOOST_REQUIRE_EQUAL(hetAlleles[1][1], 'T');

}

BOOST_AUTO_TEST_CASE(hetIndicesTest) {
	std::vector<char> alleles0;
	std::vector<char> alleles1;
	alleles0 += 'A', 'C', 'G', 'T';
	alleles1 += 'A', 'T', 'T', '?';

	std::vector< std::vector<char> > alleles;
	alleles += alleles0, alleles1;

	hlsutil::InputProcessor inputProc;
	std::vector<unsigned long> hetIndices = inputProc.hetIndices(alleles);
	BOOST_REQUIRE_EQUAL(hetIndices.size(), 2);
	BOOST_REQUIRE_EQUAL(hetIndices[0], 1);
	BOOST_REQUIRE_EQUAL(hetIndices[1], 2);
}

BOOST_AUTO_TEST_CASE(isNoCallTest) {
	hlsutil::InputProcessor inputProc;
	BOOST_REQUIRE_EQUAL(inputProc.isNoCall('A'), false);
	BOOST_REQUIRE_EQUAL(inputProc.isNoCall('?'), true);

	std::vector<char> noCallDef;
	noCallDef += '.', ' ';
	hlsutil::InputProcessor inputProc2(noCallDef);
	BOOST_REQUIRE_EQUAL(inputProc2.isNoCall('?'), false);
}

BOOST_AUTO_TEST_CASE(isHetTest) {
	std::vector<char> noCallDef;
	noCallDef += '.', ' ', '?';
	hlsutil::InputProcessor inputProc(noCallDef);

	BOOST_REQUIRE_EQUAL(inputProc.isHet('A','T'), true);
	BOOST_REQUIRE_EQUAL(inputProc.isHet('T','T'), false);
	BOOST_REQUIRE_EQUAL(inputProc.isHet('A','?'), false);
	BOOST_REQUIRE_EQUAL(inputProc.isHet('A','.'), false);
	BOOST_REQUIRE_EQUAL(inputProc.isHet('A',' '), false);
	BOOST_REQUIRE_EQUAL(inputProc.isHet('C','\0'), true);
}

BOOST_AUTO_TEST_CASE(readAllelesTest) {
	hlsutil::StringUtil str;
	hlsutil::InputProcessor inputProc;
	std::vector< std::vector<char> > alleles;
	inputProc.readAlleles("./resources/smalltest/inputs/short.hapguess", alleles, ' ', '\t', 0);
	std::string actualAlleles0 =
			"?BABBABABBAABBBBABABBBBAAABBAABABBBBABBBBABABBBAAB"
			"BBBAABAABAAABABBABBBAABBBBBBAAABAABABABABABAABAABA"
			"BABABAABABAAABBBABAABABBBBAAABAAABBBBBBA";
	std::string actualAlleles1 =
			"?AAAABAAABABBBBBBABAAABBABBAAABBAABABBAAABABABABAA"
			"AABBBBBBBABBBABBBBBBAABBBABBBBABBBABBBABBBBAAAAABA"
			"BABABAABABAAABABABAABABBBBAAABAAAAABBAAB";
	std::string alleles0(alleles[0].begin(), alleles[0].end());
	std::string alleles1(alleles[1].begin(), alleles[1].end());

	BOOST_REQUIRE_EQUAL(alleles.size(), 2);
	BOOST_REQUIRE_EQUAL(alleles[0].size(), 140);
	BOOST_REQUIRE_EQUAL(alleles[1].size(), 140);
	BOOST_REQUIRE_EQUAL(alleles0, actualAlleles0);
	BOOST_REQUIRE_EQUAL(alleles1, actualAlleles1);
}

BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: outpaddling/haplohseq

max_issues_repo_name: outpaddling/haplohseq

max_forks_repo_name: outpaddling/haplohseq


text:
/**
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#ifndef IS_RPC_CALL_HPP
#define IS_RPC_CALL_HPP
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/type_traits/function_traits.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/int.hpp>

#include <graphlab/rpc/dc_types.hpp>
#include <graphlab/rpc/function_arg_types_def.hpp>

/**
\ingroup rpc_internal
\file
An RPC-aware call is a function of the form:
... fn(distributed_control& dc, procid_t source....)
The code here checks for that.
*/

namespace graphlab {
class distributed_control;
namespace dc_impl {

namespace is_rpc_call_detail {
/**
\ingroup rpc_internal
Whether the function has less than or equal to 2 arguments
*/
template <typename F>
struct less_than_2_args {
  typedef typename boost::mpl::bool_<__GLRPC_FARITY < 2 >::type type;
};

/**
\ingroup rpc_internal
Now, arg1_type and arg_2 type may not exist in function_traits if the
number of arguments is < 2. I will need to wrap it to make it safe
*/
template <typename F, size_t nargs>
struct get_args{
 typedef __GLRPC_NIF0 arg1_type;
 typedef __GLRPC_NIF1 arg2_type;
};

// if 0 args. then make both void
template <typename F>
struct get_args<F, 0>{
 typedef void arg1_type;
 typedef void arg2_type;
};

// if 1 arg then make just make arg2 void
template <typename F>
struct get_args<F, 1>{
 typedef __GLRPC_NIF0 arg1_type;
 typedef void arg2_type;
};

template <typename F>
struct check_first_arg {
  typedef typename boost::is_same<typename get_args<F,__GLRPC_FARITY>::arg1_type, distributed_control>::type type;
};

template <typename F>
struct check_second_arg {
  typedef typename boost::is_integral<typename get_args<F,__GLRPC_FARITY>::arg2_type>::type type;
};

}

/**
 * \ingroup rpc_internal
 * ::type is true if F is an RPC call interface.
 * \tparam F the function to test
 */
template <typename F>
struct is_rpc_call {
  typedef typename boost::mpl::if_< typename is_rpc_call_detail::less_than_2_args<F>::type,
               boost::false_type,
               typename boost::mpl::and_<
                    typename is_rpc_call_detail::check_first_arg<F>::type,
                    typename is_rpc_call_detail::check_second_arg<F>::type>::type >::type type;

};

// Varargs are all none RPC calls
#define BLOCK_VAR_ARGS(Z,N,_)  \
template <typename RetType BOOST_PP_COMMA_IF(N) BOOST_PP_ENUM_PARAMS(N, typename T)> \
struct is_rpc_call<RetType (BOOST_PP_ENUM_PARAMS(N, T) BOOST_PP_COMMA_IF(N) ...)> { \
   typedef boost::false_type type; \
}; \
\
template <typename RetType BOOST_PP_COMMA_IF(N) BOOST_PP_ENUM_PARAMS(N, typename T)> \
struct is_rpc_call<RetType (*)(BOOST_PP_ENUM_PARAMS(N, T) BOOST_PP_COMMA_IF(N) ...)> { \
   typedef boost::false_type type; \
};
BOOST_PP_REPEAT(6, BLOCK_VAR_ARGS, _)
#undef BLOCK_VAR_ARGS

#define GEN_GET_USER_ARG(Z,N,_)  \
template <typename F, typename BoolType>  \
struct BOOST_PP_CAT(get_cleaned_rpc_or_basic_arg, N) { \
  typedef BOOST_PP_CAT(__GLRPC_NIF, N) arg_type;  \
};  \
template <typename F> \
struct BOOST_PP_CAT(get_cleaned_rpc_or_basic_arg, N) <F,  boost::mpl::bool_<true> > {  \
  typedef BOOST_PP_CAT(__GLRPC_F, N) arg_type;  \
};  \
template <typename F>   \
struct BOOST_PP_CAT(get_cleaned_user_arg, N) {  \
  typedef typename BOOST_PP_CAT(get_cleaned_rpc_or_basic_arg, N)<F,typename is_rpc_call<F>::type>::arg_type arg_type; \
};

BOOST_PP_REPEAT(6, GEN_GET_USER_ARG, _)
#undef GEN_GET_USER_ARG

} // namespace dc_impl
} // namespace graphlab

#include <graphlab/rpc/function_arg_types_undef.hpp>

#endif

ext: hpp

max_stars_repo_name: iivek/graphlab-cmu-mirror

max_issues_repo_name: iivek/graphlab-cmu-mirror

max_forks_repo_name: iivek/graphlab-cmu-mirror


text:
/*! \file
    \brief A trie.

    Copyright (C) 2019-2022 kaoru  https://www.tetengo.org/
*/

#include <algorithm>
#include <any>
#include <filesystem>
#include <fstream> // IWYU pragma: keep
#include <functional>
#include <iterator>
#include <memory>
#include <sstream>
#include <string>
#include <string_view>
#include <unordered_map>
#include <utility>
#include <vector>

#include <stddef.h>

#include <boost/preprocessor.hpp>
#include <boost/scope_exit.hpp>
#include <boost/stl_interfaces/iterator_interface.hpp> // IWYU pragma: keep
#include <boost/test/unit_test.hpp>

#include <tetengo/trie/default_serializer.hpp>
#include <tetengo/trie/double_array.hpp>
#include <tetengo/trie/memory_storage.hpp>
#include <tetengo/trie/storage.h>
#include <tetengo/trie/storage.hpp>
#include <tetengo/trie/trie.h>
#include <tetengo/trie/trie.hpp>
#include <tetengo/trie/trieIterator.h>
#include <tetengo/trie/trie_iterator.hpp>
#include <tetengo/trie/value_serializer.hpp>

namespace
{
    constexpr char operator""_c(const unsigned long long int uc)
    {
        return static_cast<char>(uc);
    }

    static const std::string kumamoto1{ 0xE7_c, 0x86_c, 0x8A_c, 0x6_c, 0x9C_c, 0xAC_c }; // Kumamoto in Kanji in UTF-8

    static const std::string tamana1{ 0xE7_c, 0x8E_c, 0x89_c, 0xE5_c, 0x90_c, 0x8D_c }; // Tamana in Kanji in UTF-8

    static const std::string tamarai1{ 0xE7_c, 0x8E_c, 0x89_c, 0xE6_c, 0x9D_c, 0xA5_c }; // Tamarai in Kanji in UTF-8

    static const std::wstring kumamoto2{ 0x718A, 0x672C }; // Kumamoto in Kanji in UTF-16/32

    static const std::wstring tamana2{ 0x7389, 0x540D }; // Tamana in Kanji in UTF-16/32

    static const std::wstring tamarai2{ 0x7389, 0x6765 }; // Tamarai in Kanji in UTF-16/32

    static const std::wstring tama2{ 0x7389 }; // Tama in Kanji in UTF-16/32

    static const std::wstring uto2{ 0x5B87, 0x571F }; // Uto in Kanji in UTF-16/32

    const std::vector<char> serialized{
        // clang-format off
        // base check array
        0x00_c, 0x00_c, 0x00_c, 0x0B_c,
        0xFF_c, 0xFF_c, 0x90_c, 0xFF_c,
        0xFF_c, 0xFF_c, 0x78_c, 0x71_c,
        0xFF_c, 0xFF_c, 0x9D_c, 0x8A_c,
        0xFF_c, 0xFF_c, 0x7E_c, 0x73_c,
        0xFF_c, 0xFF_c, 0xD9_c, 0x67_c,
        0x00_c, 0x00_c, 0x06_c, 0x2C_c,
        0x00_c, 0x00_c, 0x00_c, 0x00_c,
        0xFF_c, 0xFF_c, 0xB4_c, 0x89_c,
        0xFF_c, 0xFF_c, 0xFC_c, 0x54_c,
        0x00_c, 0x00_c, 0x0A_c, 0x0D_c,
        0x00_c, 0x00_c, 0x01_c, 0x00_c,

        // value array
        0x00_c, 0x00_c, 0x00_c, 0x02_c,
        0x00_c, 0x00_c, 0x00_c, 0x00_c,
        0x00_c, 0x00_c, 0x00_c, 0x06_c,
        0xE7_c, 0x86_c, 0x8A_c, 0x06_c, 0x9C_c, 0xAC_c,
        0x00_c, 0x00_c, 0x00_c, 0x06_c,
        0xE7_c, 0x8E_c, 0x89_c, 0xE5_c, 0x90_c, 0x8D_c,
        // clang-format on
    };

    std::unique_ptr<std::istream> create_input_stream()
    {
        return std::make_unique<std::stringstream>(std::string{ std::begin(serialized), std::end(serialized) });
    }

    void adding_observer(const char* const key, void* const p_context)
    {
        std::vector<std::string>* const p_added_serialized_keys = static_cast<std::vector<std::string>*>(p_context);
        p_added_serialized_keys->push_back(key);
    }

    void done_observer(void* const p_context)
    {
        bool* const p_done = static_cast<bool*>(p_context);
        *p_done = true;
    }

    const std::vector<char> serialized_c_if{
        // clang-format off
        0x00_c, 0x00_c, 0x00_c, 0x11_c,
        0xFF_c, 0xFF_c, 0xB6_c, 0xFF_c,
        0xFF_c, 0xFF_c, 0x8D_c, 0x4B_c,
        0xFF_c, 0xFF_c, 0x96_c, 0x75_c,
        0xFF_c, 0xFF_c, 0xA3_c, 0x6D_c,
        0xFF_c, 0xFF_c, 0x98_c, 0x61_c,
        0xFF_c, 0xFF_c, 0x97_c, 0x6D_c,
        0xFF_c, 0xFF_c, 0x93_c, 0x6F_c,
        0xFF_c, 0xFF_c, 0x99_c, 0x74_c,
        0x00_c, 0x00_c, 0x09_c, 0x6F_c,
        0x00_c, 0x00_c, 0x00_c, 0x00_c,
        0xFF_c, 0xFF_c, 0xAA_c, 0x54_c,
        0xFF_c, 0xFF_c, 0x9F_c, 0x61_c,
        0xFF_c, 0xFF_c, 0xAC_c, 0x6D_c,
        0xFF_c, 0xFF_c, 0xA0_c, 0x61_c,
        0xFF_c, 0xFF_c, 0xAE_c, 0x6E_c,
        0x00_c, 0x00_c, 0x10_c, 0x61_c,
        0x00_c, 0x00_c, 0x01_c, 0x00_c,
        0x00_c, 0x00_c, 0x00_c, 0x02_c,
        0x00_c, 0x00_c, 0x00_c, 0x04_c,
        0x2A_c, 0x00_c, 0x00_c, 0x00_c,
        0x18_c, 0x00_c, 0x00_c, 0x00_c,
        // clang-format on
    };

    std::filesystem::path temporary_file_path(const std::vector<char>& initial_content = std::vector<char>{})
    {
        const auto path = std::filesystem::temp_directory_path() / "test_tetengo.trie.trie";

        {
            std::ofstream stream{ path, std::ios_base::binary };
            stream.write(std::data(initial_content), std::size(initial_content));
        }

        return path;
    }

    bool& copy_detecting()
    {
        static bool singleton = false;
        return singleton;
    }

    void begin_copy_detection()
    {
        copy_detecting() = true;
    }

    void end_copy_detection()
    {
        copy_detecting() = false;
    }

    template <typename T>
    struct copy_detector
    {
        T value;

        explicit copy_detector(const T& value_) : value{ value_ } {}

        explicit copy_detector(T&& value_) : value{ std::move(value_) } {}

        copy_detector(const copy_detector& another) : value{ another.value }
        {
            if (copy_detecting())
            {
                BOOST_CHECK_MESSAGE(false, "Object copy detected.");
            }
        }

        copy_detector(copy_detector&& another) noexcept : value{ std::move(another.value) } {}
    };

    template <typename T>
    copy_detector<T> detect_copy(const T& value)
    {
        return copy_detector<T>{ value };
    }

    template <typename T>
    copy_detector<T> detect_copy(T&& value)
    {
        return copy_detector<T>{ std::move(value) };
    }

}

BOOST_AUTO_TEST_SUITE(test_tetengo)
BOOST_AUTO_TEST_SUITE(trie)
BOOST_AUTO_TEST_SUITE(trie)

BOOST_AUTO_TEST_CASE(null_building_observer_set)
{
    BOOST_TEST_PASSPOINT();

    [[maybe_unused]] const auto& observer_set = tetengo::trie::trie<std::string, int>::null_building_observer_set();

    tetengo_trie_trie_nullAddingObserver("hoge", nullptr);
    tetengo_trie_trie_nullDoneObserver(nullptr);
}

BOOST_AUTO_TEST_CASE(default_double_array_density_factor)
{
    BOOST_TEST_PASSPOINT();

    {
        using trie_type = tetengo::trie::trie<std::string, int>;
        BOOST_TEST(
            trie_type::default_double_array_density_factor() == tetengo::trie::double_array::default_density_factor());
    }
    {
        BOOST_TEST(
            tetengo_trie_trie_defaultDoubleArrayDensityFactor() ==
            tetengo::trie::double_array::default_density_factor());
    }
}

BOOST_AUTO_TEST_CASE(construction)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::string, int> trie_{};
    }
    {
        const tetengo::trie::trie<std::string, int> trie_{ tetengo::trie::default_serializer<std::string>{ true } };
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 }, { "Tamana", 24 } };
    }
    {
        const tetengo::trie::trie<std::string, int> trie_{ { "Kumamoto", 42 }, { "Tamana", 24 } };
    }
    {
        const tetengo::trie::trie<std::string, int> trie_{ { kumamoto1, 42 }, { tamana1, 24 } };
    }
    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 }, { tamana2, tamana1 } };
    }
    {
        std::vector<std::string>                                          added_serialized_keys{};
        auto                                                              done = false;
        tetengo::trie::trie<std::string, int>::building_observer_set_type observer{
            [&added_serialized_keys](const std::string_view& serialized_key) {
                added_serialized_keys.emplace_back(serialized_key);
            },
            [&done]() { done = true; }
        };
        const tetengo::trie::trie<std::string, int> trie_{
            { { "Kumamoto", 42 }, { "Tamana", 24 } },
            tetengo::trie::default_serializer<std::string>{ true },
            observer,
            tetengo::trie::trie<std::string, int>::default_double_array_density_factor()
        };

        static const tetengo::trie::default_deserializer<std::string> key_deserializer{ true };
        BOOST_TEST_REQUIRE(std::size(added_serialized_keys) == 2U);
        BOOST_TEST(key_deserializer(added_serialized_keys[0]) == "Kumamoto");
        BOOST_TEST(key_deserializer(added_serialized_keys[1]) == "Tamana");
        BOOST_TEST(done);
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::begin(content), std::end(content) };
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };
    }
    {
        std::unordered_map<std::string_view, std::string> content{ { "Kumamoto", kumamoto1 }, { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{
            std::begin(content), std::end(content), tetengo::trie::default_serializer<std::string_view>{ true }
        };
    }
    {
        auto                                    p_input_stream = create_input_stream();
        const tetengo::trie::value_deserializer value_deserializer_{ [](const std::vector<char>& serialized) {
            static const tetengo::trie::default_deserializer<std::string>string_deserializer{ false };
            return string_deserializer(std::string{ std::begin(serialized), std::end(serialized) });
        } };
        auto p_storage = std::make_unique<tetengo::trie::memory_storage>(*p_input_stream, value_deserializer_);
        const tetengo::trie::trie<std::string, std::string> trie_{ std::move(p_storage) };
    }
    {
        auto                                    p_input_stream = create_input_stream();
        const tetengo::trie::value_deserializer value_deserializer_{ [](const std::vector<char>& serialized) {
            static const tetengo::trie::default_deserializer<std::string>string_deserializer{ false };
            return string_deserializer(std::string{ std::begin(serialized), std::end(serialized) });
        } };
        auto p_storage = std::make_unique<tetengo::trie::memory_storage>(*p_input_stream, value_deserializer_);
        const tetengo::trie::trie<std::string, std::string> trie_{
            std::move(p_storage), tetengo::trie::default_serializer<std::string>{ true }
        };
    }

    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        std::vector<std::string> added_serialized_keys{};
        auto                     done = false;
        const auto* const        p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            adding_observer,
            &added_serialized_keys,
            done_observer,
            &done,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST_REQUIRE(std::size(added_serialized_keys) == 2U);
        BOOST_TEST(added_serialized_keys[0] == "Kumamoto");
        BOOST_TEST(added_serialized_keys[1] == "Tamana");
        BOOST_TEST(done);
    }
    {
        const auto file_path = temporary_file_path(serialized_c_if);
        BOOST_SCOPE_EXIT(&file_path)
        {
            std::filesystem::remove(file_path);
        }
        BOOST_SCOPE_EXIT_END;

        auto* const p_storage = tetengo_trie_storage_createMemoryStorage(file_path.c_str());
        BOOST_TEST(tetengo_trie_storage_valueCount(p_storage) == 2U);

        const auto* const p_trie = tetengo_trie_trie_createWithStorage(p_storage);
        BOOST_SCOPE_EXIT(&p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(p_trie);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        std::vector<std::string> added_serialized_keys{};
        auto                     done = false;
        const auto* const        p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            adding_observer,
            &added_serialized_keys,
            done_observer,
            &done,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        auto* const p_storage = tetengo_trie_storage_createStorage(p_trie);

        const auto* const p_trie2 = tetengo_trie_trie_createWithStorage(p_storage);
        BOOST_TEST(!p_trie2);
    }
    {
        std::vector<std::string> added_serialized_keys{};
        auto                     done = false;
        const auto* const        p_trie = tetengo_trie_trie_create(
            nullptr,
            0,
            sizeof(int),
            adding_observer,
            &added_serialized_keys,
            done_observer,
            &done,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());

        BOOST_TEST(p_trie);
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;
    }
    {
        std::vector<std::string> added_serialized_keys{};
        auto                     done = false;
        const auto* const        p_trie = tetengo_trie_trie_create(
            nullptr,
            1,
            sizeof(int),
            adding_observer,
            &added_serialized_keys,
            done_observer,
            &done,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());

        BOOST_TEST(!p_trie);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        std::vector<std::string> added_serialized_keys{};
        auto                     done = false;
        const auto* const        p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            nullptr,
            &added_serialized_keys,
            done_observer,
            &done,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());

        BOOST_TEST(!p_trie);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        std::vector<std::string> added_serialized_keys{};
        auto                     done = false;
        const auto* const        p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            adding_observer,
            &added_serialized_keys,
            nullptr,
            &done,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());

        BOOST_TEST(!p_trie);
    }
    {
        const auto* const p_trie2 = tetengo_trie_trie_createWithStorage(nullptr);
        BOOST_TEST(!p_trie2);
    }
}

BOOST_AUTO_TEST_CASE(empty)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{};

        BOOST_TEST(std::empty(trie_));
    }
    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 } };

        BOOST_TEST(!std::empty(trie_));
    }
    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 }, { tamana2, tamana1 } };

        BOOST_TEST(!std::empty(trie_));
    }

    {
        const tetengo::trie::trie<std::string_view, int> trie_{};

        BOOST_TEST(std::empty(trie_));
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        BOOST_TEST(!std::empty(trie_));
    }

    {
        std::vector<std::pair<std::string_view, std::string>>    content{};
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        BOOST_TEST(std::empty(trie_));
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        BOOST_TEST(!std::empty(trie_));
    }

    {
        const auto* const p_trie = tetengo_trie_trie_create(
            nullptr,
            0,
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(tetengo_trie_trie_empty(p_trie));
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(!tetengo_trie_trie_empty(p_trie));
    }
    {
        BOOST_TEST(!tetengo_trie_trie_empty(nullptr));
    }
}

BOOST_AUTO_TEST_CASE(size)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{};

        BOOST_TEST(std::size(trie_) == 0U);
    }
    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 } };

        BOOST_TEST(std::size(trie_) == 1U);
    }
    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 }, { tamana2, tamana1 } };

        BOOST_TEST(std::size(trie_) == 2U);
    }

    {
        const tetengo::trie::trie<std::string_view, int> trie_{};

        BOOST_TEST(std::size(trie_) == 0U);
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        BOOST_TEST(std::size(trie_) == 1U);
    }

    {
        std::vector<std::pair<std::string_view, std::string>>    content{};
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        BOOST_TEST(std::size(trie_) == 0U);
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        BOOST_TEST(std::size(trie_) == 2U);
    }

    {
        const auto* const p_trie = tetengo_trie_trie_create(
            nullptr,
            0,
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(tetengo_trie_trie_size(p_trie) == 0U);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(tetengo_trie_trie_size(p_trie) == 1U);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(tetengo_trie_trie_size(p_trie) == 2U);
    }
    {
        BOOST_TEST(tetengo_trie_trie_size(nullptr) == static_cast<size_t>(-1));
    }
}

BOOST_AUTO_TEST_CASE(contains)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{};

        BOOST_TEST(!trie_.contains(kumamoto2));
    }
    {
        const tetengo::trie::trie<std::wstring, copy_detector<std::string>> trie_{
            { kumamoto2, detect_copy(kumamoto1) }, { tamana2, detect_copy(tamana1) }
        };
        begin_copy_detection();

        BOOST_TEST(trie_.contains(kumamoto2));
        BOOST_TEST(trie_.contains(tamana2));
        BOOST_TEST(!trie_.contains(uto2));

        end_copy_detection();
    }

    {
        const tetengo::trie::trie<std::string_view, int> trie_{};

        BOOST_TEST(!trie_.contains("Kumamoto"));
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        BOOST_TEST(trie_.contains("Kumamoto"));
    }

    {
        std::vector<std::pair<std::string_view, std::string>>    content{};
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        BOOST_TEST(!trie_.contains("Kumamoto"));
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        BOOST_TEST(trie_.contains("Kumamoto"));
    }

    {
        const auto* const p_trie = tetengo_trie_trie_create(
            nullptr,
            0,
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(!tetengo_trie_trie_contains(p_trie, "Kumamoto"));
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(tetengo_trie_trie_contains(p_trie, "Kumamoto"));
        BOOST_TEST(tetengo_trie_trie_contains(p_trie, "Tamana"));
        BOOST_TEST(!tetengo_trie_trie_contains(p_trie, "Uto"));
    }
    {
        BOOST_TEST(!tetengo_trie_trie_contains(nullptr, "Kumamoto"));
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(!tetengo_trie_trie_contains(p_trie, nullptr));
    }
}

BOOST_AUTO_TEST_CASE(find)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{};

        const auto* const p_found = trie_.find(kumamoto2);
        BOOST_TEST(!p_found);
    }
    {
        const tetengo::trie::trie<std::wstring, copy_detector<std::string>> trie_{
            { kumamoto2, detect_copy(kumamoto1) }, { tamana2, detect_copy(tamana1) }
        };
        begin_copy_detection();

        {
            const auto* const p_found = trie_.find(kumamoto2);
            BOOST_REQUIRE(p_found);
            BOOST_TEST(p_found->value == kumamoto1);
        }
        {
            const auto* const p_found = trie_.find(tamana2);
            BOOST_REQUIRE(p_found);
            BOOST_TEST(p_found->value == tamana1);
        }
        {
            const auto* const p_found = trie_.find(uto2);
            BOOST_TEST(!p_found);
        }

        end_copy_detection();
    }

    {
        const tetengo::trie::trie<std::string_view, int> trie_{};

        const auto* const p_found = trie_.find("Kumamoto");
        BOOST_TEST(!p_found);
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        const auto* const p_found = trie_.find("Kumamoto");
        BOOST_REQUIRE(p_found);
        BOOST_TEST(*p_found == 42);
    }

    {
        std::vector<std::pair<std::string_view, std::string>>    content{};
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        const auto* const p_found = trie_.find("Kumamoto");
        BOOST_TEST(!p_found);
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        const auto* const p_found = trie_.find("Kumamoto");
        BOOST_REQUIRE(p_found);
        BOOST_TEST(*p_found == kumamoto1);
    }

    {
        const auto* const p_trie = tetengo_trie_trie_create(
            nullptr,
            0,
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_found = tetengo_trie_trie_find(p_trie, "Kumamoto");
        BOOST_TEST(!p_found);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_found = tetengo_trie_trie_find(p_trie, "Kumamoto");
        BOOST_REQUIRE(p_found);
        BOOST_TEST(*static_cast<const int*>(p_found) == kumamoto_value);
    }
    {
        const auto* const p_found = tetengo_trie_trie_find(nullptr, "Kumamoto");
        BOOST_TEST(!p_found);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_found = tetengo_trie_trie_find(p_trie, nullptr);
        BOOST_TEST(!p_found);
    }
}

BOOST_AUTO_TEST_CASE(begin_end)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{};

        const auto first = std::begin(trie_);
        const auto last = std::end(trie_);
    }
    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 }, { tamana2, tamana1 } };

        const auto first = std::begin(trie_);
        const auto last = std::end(trie_);
    }

    {
        const tetengo::trie::trie<std::string_view, int> trie_{};

        const auto first = std::begin(trie_);
        const auto last = std::end(trie_);
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        const auto first = std::begin(trie_);
        const auto last = std::end(trie_);
    }

    {
        std::vector<std::pair<std::string_view, std::string>>    content{};
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        const auto first = std::begin(trie_);
        const auto last = std::end(trie_);
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        const auto first = std::begin(trie_);
        const auto last = std::end(trie_);
    }

    {
        const auto* const p_trie = tetengo_trie_trie_create(
            nullptr,
            0,
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_iterator = tetengo_trie_trie_createIterator(p_trie);
        BOOST_SCOPE_EXIT(p_iterator)
        {
            tetengo_trie_trie_destroyIterator(p_iterator);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(p_iterator);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_iterator = tetengo_trie_trie_createIterator(p_trie);
        BOOST_SCOPE_EXIT(p_iterator)
        {
            tetengo_trie_trie_destroyIterator(p_iterator);
        }
        BOOST_SCOPE_EXIT_END;

        BOOST_TEST(p_iterator);
    }
    {
        const auto* const p_iterator = tetengo_trie_trie_createIterator(nullptr);
        BOOST_TEST(!p_iterator);
    }
}

BOOST_AUTO_TEST_CASE(subtrie)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, copy_detector<std::string>> trie_{};

        const auto p_subtrie = trie_.subtrie(tama2);
        BOOST_CHECK(!p_subtrie);
    }
    {
        const tetengo::trie::trie<std::wstring, copy_detector<std::string>> trie_{ { kumamoto2,
                                                                                detect_copy(kumamoto1) } };

        const auto p_subtrie = trie_.subtrie(tama2);
        BOOST_CHECK(!p_subtrie);
    }
    {
        const tetengo::trie::trie<std::wstring, copy_detector<std::string>> trie_{
            { kumamoto2, detect_copy(kumamoto1) }, { tamana2, detect_copy(tamana1) }
        };

        const auto p_subtrie = trie_.subtrie(tama2);
        BOOST_REQUIRE(p_subtrie);

        auto iterator_ = std::begin(*p_subtrie);
        BOOST_REQUIRE(iterator_ != std::end(*p_subtrie));
        BOOST_CHECK(iterator_->value == tamana1);

        ++iterator_;
        BOOST_CHECK(iterator_ == std::end(*p_subtrie));
    }
    {
        const tetengo::trie::trie<std::wstring, copy_detector<std::string>> trie_{
            { kumamoto2, detect_copy(kumamoto1) },
            { tamana2, detect_copy(tamana1) },
            { tamarai2, detect_copy(tamarai1) }
        };

        const auto p_subtrie = trie_.subtrie(tama2);
        BOOST_REQUIRE(p_subtrie);

        auto iterator_ = std::begin(*p_subtrie);
        BOOST_REQUIRE(iterator_ != std::end(*p_subtrie));
        BOOST_CHECK(iterator_->value == tamana1);

        ++iterator_;
        BOOST_REQUIRE(iterator_ != std::end(*p_subtrie));
        BOOST_CHECK(iterator_->value == tamarai1);

        ++iterator_;
        BOOST_CHECK(iterator_ == std::end(*p_subtrie));
    }

    {
        const tetengo::trie::trie<std::string_view, int> trie_{};

        const auto p_subtrie = trie_.subtrie("Kuma");
        BOOST_CHECK(!p_subtrie);
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        const auto p_subtrie = trie_.subtrie("Kuma");
        BOOST_REQUIRE(p_subtrie);

        auto iterator_ = std::begin(*p_subtrie);
        BOOST_REQUIRE(iterator_ != std::end(*p_subtrie));
        BOOST_CHECK(*iterator_ == 42);

        ++iterator_;
        BOOST_CHECK(iterator_ == std::end(*p_subtrie));
    }

    {
        std::vector<std::pair<std::string_view, std::string>>    content{};
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        const auto p_subtrie = trie_.subtrie("Kuma");
        BOOST_CHECK(!p_subtrie);
    }
    {
        std::vector<std::pair<std::string_view, std::string>>    content{ { "Kumamoto", kumamoto1 },
                                                                       { "Tamana", tamana1 } };
        const tetengo::trie::trie<std::string_view, std::string> trie_{ std::make_move_iterator(std::begin(content)),
                                                                        std::make_move_iterator(std::end(content)) };

        const auto p_subtrie = trie_.subtrie("Kuma");
        BOOST_REQUIRE(p_subtrie);

        auto iterator_ = std::begin(*p_subtrie);
        BOOST_REQUIRE(iterator_ != std::end(*p_subtrie));
        BOOST_CHECK(*iterator_ == kumamoto1);

        ++iterator_;
        BOOST_CHECK(iterator_ == std::end(*p_subtrie));
    }

    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        constexpr auto                          tamarai_value = static_cast<int>(35);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value },
                                                          { "Tamarai", &tamarai_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        {
            const auto* const p_subtrie = tetengo_trie_trie_subtrie(p_trie, "Tama");
            BOOST_SCOPE_EXIT(p_subtrie)
            {
                tetengo_trie_trie_destroy(p_subtrie);
            }
            BOOST_SCOPE_EXIT_END;

            BOOST_TEST_REQUIRE(p_subtrie);

            auto* const p_subtrie_iterator = tetengo_trie_trie_createIterator(p_subtrie);
            BOOST_SCOPE_EXIT(p_subtrie_iterator)
            {
                tetengo_trie_trie_destroyIterator(p_subtrie_iterator);
            }
            BOOST_SCOPE_EXIT_END;

            BOOST_TEST_REQUIRE(tetengo_trie_trieIterator_hasNext(p_subtrie_iterator));
            BOOST_TEST(*static_cast<const int*>(tetengo_trie_trieIterator_get(p_subtrie_iterator)) == tamana_value);
            tetengo_trie_trieIterator_next(p_subtrie_iterator);

            BOOST_TEST_REQUIRE(tetengo_trie_trieIterator_hasNext(p_subtrie_iterator));
            BOOST_TEST(*static_cast<const int*>(tetengo_trie_trieIterator_get(p_subtrie_iterator)) == tamarai_value);
            tetengo_trie_trieIterator_next(p_subtrie_iterator);

            BOOST_TEST(!tetengo_trie_trieIterator_hasNext(p_subtrie_iterator));
        }
        {
            const auto* const p_subtrie = tetengo_trie_trie_subtrie(p_trie, "Uto");
            BOOST_SCOPE_EXIT(p_subtrie)
            {
                tetengo_trie_trie_destroy(p_subtrie);
            }
            BOOST_SCOPE_EXIT_END;

            BOOST_TEST(!p_subtrie);
        }
    }
    {
        const auto* const p_subtrie = tetengo_trie_trie_subtrie(nullptr, "Tama");
        BOOST_TEST(!p_subtrie);
    }
    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        constexpr auto                          tamarai_value = static_cast<int>(35);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value },
                                                          { "Tamarai", &tamarai_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_subtrie = tetengo_trie_trie_subtrie(p_trie, nullptr);
        BOOST_TEST(!p_subtrie);
    }
}

BOOST_AUTO_TEST_CASE(get_storage)
{
    BOOST_TEST_PASSPOINT();

    {
        const tetengo::trie::trie<std::wstring, std::string> trie_{ { kumamoto2, kumamoto1 }, { tamana2, tamana1 } };

        [[maybe_unused]] const auto& storage = trie_.get_storage();
    }
    {
        const tetengo::trie::trie<std::string_view, int> trie_{ { "Kumamoto", 42 } };

        [[maybe_unused]] const auto& storage = trie_.get_storage();
    }
    {
        auto                                    p_input_stream = create_input_stream();
        const tetengo::trie::value_deserializer value_deserializer_{ [](const std::vector<char>& serialized) {
            static const tetengo::trie::default_deserializer<std::string>string_deserializer{ false };
            return string_deserializer(std::string{ std::begin(serialized), std::end(serialized) });
        } };
        auto p_storage = std::make_unique<tetengo::trie::memory_storage>(*p_input_stream, value_deserializer_);
        const tetengo::trie::trie<std::string, std::string> trie_{ std::move(p_storage) };

        const auto& storage = trie_.get_storage();

        std::stringstream                     stream{};
        const tetengo::trie::value_serializer serializer{
            [](const std::any& value) {
                static const tetengo::trie::default_serializer<std::string> serializer{ false };
                const auto serialized = serializer(std::any_cast<std::string>(value));
                return std::vector<char>{ std::begin(serialized), std::end(serialized) };
            },
            0
        };
        storage.serialize(stream, serializer);
        const auto storage_serialized = stream.str();

        BOOST_CHECK_EQUAL_COLLECTIONS(
            std::begin(storage_serialized), std::end(storage_serialized), std::begin(serialized), std::end(serialized));
    }

    {
        constexpr auto                          kumamoto_value = static_cast<int>(42);
        constexpr auto                          tamana_value = static_cast<int>(24);
        constexpr auto                          tamarai_value = static_cast<int>(35);
        std::vector<tetengo_trie_trieElement_t> elements{ { "Kumamoto", &kumamoto_value },
                                                          { "Tamana", &tamana_value },
                                                          { "Tamarai", &tamarai_value } };

        const auto* const p_trie = tetengo_trie_trie_create(
            std::data(elements),
            std::size(elements),
            sizeof(int),
            tetengo_trie_trie_nullAddingObserver,
            nullptr,
            tetengo_trie_trie_nullDoneObserver,
            nullptr,
            tetengo_trie_trie_defaultDoubleArrayDensityFactor());
        BOOST_SCOPE_EXIT(p_trie)
        {
            tetengo_trie_trie_destroy(p_trie);
        }
        BOOST_SCOPE_EXIT_END;

        const auto* const p_storage = tetengo_trie_trie_getStorage(p_trie);
        BOOST_TEST(p_storage);
    }
    {
        const auto* const p_storage = tetengo_trie_trie_getStorage(nullptr);
        BOOST_TEST(!p_storage);
    }
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()

max_stars_repo_name: tetengo/tetengo

max_issues_repo_name: tetengo/tetengo

max_forks_repo_name: tetengo/tetengo


text:
/**
 * @file constrained_ik.cpp
 * @brief Constrained Inverse Kinematic Solver
 *
 * @author dsolomon
 * @date Sep 15, 2013
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2013, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <constrained_ik/constrained_ik.h>
#include "constrained_ik/constraint_group.h"
#include <boost/make_shared.hpp>
#include <constrained_ik/constraint_results.h>
#include <ros/ros.h>

const std::vector<std::string> SUPPORTED_COLLISION_DETECTORS = {"IndustrialFCL", "CollisionDetectionOpenVDB"}; /**< Supported collision detector */

namespace constrained_ik
{
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Affine3d;

Constrained_IK::Constrained_IK():nh_("~")
{
  initialized_ = false;

  loadDefaultSolverConfiguration();
}

void Constrained_IK::addConstraintsFromParamServer(const std::string &parameter_name)
{
  XmlRpc::XmlRpcValue constraints_xml;
  boost::shared_ptr<pluginlib::ClassLoader<constrained_ik::Constraint> > constraint_loader;

  constraint_loader.reset(new pluginlib::ClassLoader<constrained_ik::Constraint>("constrained_ik", "constrained_ik::Constraint"));

  if (!nh_.getParam(parameter_name, constraints_xml))
  {
    ROS_ERROR("Unable to find ros parameter: %s", parameter_name.c_str());
    ROS_BREAK();
    return;
  }

  if(constraints_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("ROS parameter %s must be an array", parameter_name.c_str());
    ROS_BREAK();
    return;
  }

  for (int i=0; i<constraints_xml.size(); ++i)
  {
    XmlRpc::XmlRpcValue constraint_xml = constraints_xml[i];

    if (constraint_xml.hasMember("class") &&
              constraint_xml["class"].getType() == XmlRpc::XmlRpcValue::TypeString &&
              constraint_xml.hasMember("primary") &&
              constraint_xml["primary"].getType() == XmlRpc::XmlRpcValue::TypeBoolean)
    {
      std::string class_name = constraint_xml["class"];
      bool is_primary = constraint_xml["primary"];

      Constraint *constraint;
      try
      {
        constraint = constraint_loader->createUnmanagedInstance(class_name);

        constraint->loadParameters(constraint_xml);
        if (is_primary)
          addConstraint(constraint, constraint_types::Primary);
        else
          addConstraint(constraint, constraint_types::Auxiliary);

      }
      catch (pluginlib::PluginlibException& ex)
      {
        ROS_ERROR("Couldn't load constraint named %s.\n Error: %s", class_name.c_str(), ex.what());
        ROS_BREAK();
      }
    }
    else
    {
      ROS_ERROR("Constraint must have class(string) and primary(boolean) members");
    }
  }
}

void Constrained_IK::loadDefaultSolverConfiguration()
{
  ConstrainedIKConfiguration config;
  config.debug_mode = false;
  config.allow_joint_convergence = false;
  config.allow_primary_normalization = true;
  config.allow_auxiliary_nomalization = true;
  config.limit_primary_motion = false;
  config.limit_auxiliary_motion = false;
  config.limit_auxiliary_interations = false;
  config.solver_max_iterations = 500;
  config.solver_min_iterations = 0;
  config.auxiliary_max_iterations = 5;
  config.primary_max_motion = 2.0;
  config.auxiliary_max_motion = 0.2;
  config.primary_norm = 1.0;
  config.auxiliary_norm = 0.2;
  config.primary_gain = 1.0;
  config.auxiliary_gain = 1.0;
  config.joint_convergence_tol = 0.0001;

  setSolverConfiguration(config);
}

void Constrained_IK::setSolverConfiguration(const ConstrainedIKConfiguration &config)
{
  config_ = config;
  validateConstrainedIKConfiguration<ConstrainedIKConfiguration>(config_);
}

constrained_ik::ConstraintResults Constrained_IK::evalConstraint(constraint_types::ConstraintTypes constraint_type, const constrained_ik::SolverState &state) const
{
  switch(constraint_type)
  {
    case constraint_types::Primary:
      return primary_constraints_.evalConstraint(state);
    case constraint_types::Auxiliary:
      return auxiliary_constraints_.evalConstraint(state);
  }
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjection(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv = calcDampedPseudoinverse(J);
  MatrixXd JplusJ = J_pinv * J;
  int mn = JplusJ.rows();
  MatrixXd P = MatrixXd::Identity(mn,mn)-JplusJ;
  return (P);
}

Eigen::MatrixXd Constrained_IK::calcNullspaceProjectionTheRightWay(const Eigen::MatrixXd &A) const
{
  Eigen::JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullV);
  MatrixXd V(svd.matrixV());

  // determine rank using same algorithym as latest Eigen
  // TODO replace next 10 lines of code with rnk = svd.rank(); once eigen3 is updated
  int rnk = 0;
  if(svd.singularValues().size()==0)
  {
    rnk = 0;
  }
  else
  {
    double threshold = std::min(A.rows(),A.cols())*Eigen::NumTraits<double>::epsilon();
    double premultiplied_threshold = svd.singularValues().coeff(0) * threshold;
    rnk = svd.nonzeroSingularValues()-1;
    while(rnk>=0 && svd.singularValues().coeff(rnk) < premultiplied_threshold) --rnk;
    rnk++;
  }

  // zero singular vectors in the range of A
  for(int i=0; i<rnk; ++i)
  {
    for(int j=0; j<A.cols(); j++) V(j,i) = 0;
  }

  MatrixXd P = V *  V.transpose();
  return(P);
}

Eigen::MatrixXd Constrained_IK::calcDampedPseudoinverse(const Eigen::MatrixXd &J) const
{
  MatrixXd J_pinv;

  if (basic_kin::BasicKin::dampedPInv(J,J_pinv))
  {
    return J_pinv;
  }
  else
  {
    ROS_ERROR_STREAM("Not able to calculate damped pseudoinverse!");
    throw std::runtime_error("Not able to calculate damped pseudoinverse!  IK solution may be invalid.");
  }
}

bool Constrained_IK::calcInvKin(const Eigen::Affine3d &goal,
                                const Eigen::VectorXd &joint_seed,
                                Eigen::VectorXd &joint_angles) const
{
  return calcInvKin(goal,joint_seed, planning_scene::PlanningSceneConstPtr(), joint_angles);
}

bool Constrained_IK::calcInvKin(const Eigen::Affine3d &goal,
                                const Eigen::VectorXd &joint_seed,
                                const planning_scene::PlanningSceneConstPtr planning_scene,
                                Eigen::VectorXd &joint_angles) const
{
  double dJoint_norm;
  SolverStatus status;

    // initialize state
  joint_angles = joint_seed;  // initialize result to seed value
  constrained_ik::SolverState state = getState(goal, joint_seed); // create state vars for this IK solve
  state.condition = checkInitialized();
  state.planning_scene = planning_scene;
  state.group_name = kin_.getJointModelGroup()->getName();

  //TODO: Does this still belong here?
  if(planning_scene)
  {
    state.robot_state = robot_state::RobotStatePtr(new moveit::core::RobotState(planning_scene->getCurrentState()));

    //Check and make sure the correct collision detector is loaded.
    auto pos = std::find(SUPPORTED_COLLISION_DETECTORS.begin(),SUPPORTED_COLLISION_DETECTORS.end(),
                         planning_scene->getActiveCollisionDetectorName());
    if (pos == SUPPORTED_COLLISION_DETECTORS.end())
    {
      std::stringstream error_message;
      error_message<<" Constrained IK requires the use of collision detectors: ";
      for(auto& d : SUPPORTED_COLLISION_DETECTORS)
      {
        error_message<<"'"<< d<<"' ";
      }
      error_message<<".\nSet or add the 'collision_detector' parameter to an allowed collision detector in the move_group.launch file"<<std::endl;
      throw std::runtime_error(error_message.str());
    }

    state.collision_robot = boost::dynamic_pointer_cast<const collision_detection::CollisionRobotIndustrial>(planning_scene->getCollisionRobot());
    state.collision_world = boost::dynamic_pointer_cast<const collision_detection::CollisionWorldIndustrial>(planning_scene->getCollisionWorld());
  }

  if (state.condition == initialization_state::NothingInitialized || state.condition == initialization_state::AuxiliaryOnly)
    throw std::runtime_error("Must call init() before using Constrained_IK and have a primary constraint.");

  //Cache the joint angles to return if max iteration is reached.
  Eigen::VectorXd cached_joint_angles = joint_seed;

  // iterate until solution converges (or aborted)
  while (true)
  {
    // re-update internal state variables
    updateState(state, joint_angles);

    // calculate a Jacobian (relating joint-space updates/deltas to cartesian-space errors/deltas)
    // and the associated cartesian-space error/delta vector
    // Primary Constraints
    constrained_ik::ConstraintResults primary = evalConstraint(constraint_types::Primary, state);
    // TODO since we already have J_p = USV, use that to get null-projection too.
    // otherwise, we are repeating the expensive calculation of the SVD

    VectorXd dJoint_p;
    dJoint_p.setZero(joint_seed.size());
    if (!primary.isEmpty()) // This is required because not all constraints always return data.
    {
      MatrixXd Ji_p = calcDampedPseudoinverse(primary.jacobian);
      dJoint_p = config_.primary_gain*(Ji_p*primary.error);
      dJoint_norm = dJoint_p.norm();
      if(config_.allow_primary_normalization && dJoint_norm > config_.primary_norm)// limit maximum update radian/meter
      {
        dJoint_p = config_.primary_norm * (dJoint_p/dJoint_norm);
        dJoint_norm = dJoint_p.norm();
      }
      state.primary_sum += dJoint_norm;
    }

    // Auxiliary Constraints
    VectorXd dJoint_a;
    constrained_ik::ConstraintResults auxiliary;
    dJoint_a.setZero(dJoint_p.size());
    if (state.condition == initialization_state::PrimaryAndAuxiliary)
    {
      if (!((config_.limit_auxiliary_motion && state.auxiliary_sum >= config_.auxiliary_max_motion) ||
          (config_.limit_auxiliary_interations && state.iter > config_.auxiliary_max_iterations)))
      {
        auxiliary = evalConstraint(constraint_types::Auxiliary, state);
        if (!auxiliary.isEmpty()) // This is required because not all constraints always return data.
        {
          MatrixXd N_p = calcNullspaceProjectionTheRightWay(primary.jacobian);
          MatrixXd Jnull_a = calcDampedPseudoinverse(auxiliary.jacobian*N_p);
          dJoint_a = config_.auxiliary_gain*Jnull_a*(auxiliary.error-auxiliary.jacobian*dJoint_p);
          dJoint_norm = dJoint_a.norm();
          if(config_.allow_auxiliary_nomalization && dJoint_norm > config_.auxiliary_norm)// limit maximum update radian/meter
          {
            dJoint_a = config_.auxiliary_norm * (dJoint_a/dJoint_norm);
            dJoint_norm = dJoint_a.norm();
          }
          state.auxiliary_sum += dJoint_norm;
        }
      }
      else
      {
        state.auxiliary_at_limit = true;
      }
    }

    status = checkStatus(state, primary, auxiliary);

    if (status == Converged)
    {
      ROS_DEBUG_STREAM("Found IK solution in " << state.iter << " iterations: " << joint_angles.transpose());
      return true;
    }
    else if (status == NotConverged)
    {
      // update joint solution by the calculated update (or a partial fraction)
      joint_angles += (dJoint_p + dJoint_a);
      clipToJointLimits(joint_angles);
    }
    else if (status == Failed)
    {
      joint_angles = cached_joint_angles;
      return false;
    }
  }
}

SolverStatus Constrained_IK::checkStatus(const constrained_ik::SolverState &state, const constrained_ik::ConstraintResults &primary, const constrained_ik::ConstraintResults &auxiliary) const
{
  // Check the status of convergence
  if(state.condition == initialization_state::PrimaryAndAuxiliary)
  {
    bool status = (primary.status && auxiliary.status);

    if (state.iter > config_.solver_min_iterations)
    {
      if (!status && primary.status && state.auxiliary_at_limit)
      {
        ROS_DEBUG("Auxiliary motion or iteration limit reached!");
        return Converged;
      }
      else if(status)
      {
        return Converged;
      }
    }
  }

  if(state.condition == initialization_state::PrimaryOnly)
  {
    if (primary.status && state.iter > config_.solver_min_iterations)
    {
      return Converged;
    }
  }

  // check for joint convergence
  //   - this is an error: joints stabilize, but goal pose not reached
  if (config_.allow_joint_convergence)
  {
    if (state.joints_delta.cwiseAbs().maxCoeff() < config_.joint_convergence_tol)
    {
      if (state.iter > config_.solver_min_iterations)
      {
        ROS_DEBUG_STREAM("Joint convergence reached " << state.iter << " / " << config_.solver_max_iterations << " iterations before convergence.");
        return Converged;
      }
    }
  }

  if (state.iter > config_.solver_max_iterations || (config_.limit_primary_motion && state.primary_sum >= config_.primary_max_motion))
  {
    if (!primary.status)
    {
      if (config_.limit_primary_motion && state.primary_sum >= config_.primary_max_motion)
      {
        ROS_WARN_STREAM("Primary reached max allowed motion, no solution returned.");
      }
      else
      {
        ROS_WARN_STREAM("Solver reached max allowed iteration, no solution returned.");
      }
      return Failed;
    }
    else if (state.condition == initialization_state::PrimaryAndAuxiliary)
    {
      ROS_WARN_STREAM("Maximum iterations reached but primary converged so returning solution.");
      return Converged;
    }
  }

  return NotConverged;
}

void Constrained_IK::clearConstraintList()
{
  primary_constraints_.clear();
  auxiliary_constraints_.clear();
}

void Constrained_IK::clipToJointLimits(Eigen::VectorXd &joints) const
{
  const MatrixXd limits = kin_.getLimits();
  const VectorXd orig_joints(joints);

  if (joints.size() != limits.rows())
    throw std::invalid_argument("clipToJointLimits: Unexpected number of joints");

  for (size_t i=0; i<limits.rows(); ++i)
  {
    joints[i] = std::max(limits(i,0), std::min(limits(i,1), joints[i]));
  }
  if (config_.debug_mode && !joints.isApprox(orig_joints))
      ROS_WARN("Joints have been clipped");
}

void Constrained_IK::init(const basic_kin::BasicKin &kin)
{
  if (!kin.checkInitialized())
    throw std::invalid_argument("Input argument 'BasicKin' must be initialized");

  kin_ = kin;
  initialized_ = true;
  primary_constraints_.init(this);
  auxiliary_constraints_.init(this);

}

double Constrained_IK::rangedAngle(double angle)
{
    angle = copysign(fmod(fabs(angle),2.0*M_PI), angle);
    if (angle < -M_PI) return angle+2.*M_PI;
    if (angle > M_PI)  return angle-2.*M_PI;
    return angle;
}

constrained_ik::SolverState Constrained_IK::getState(const Eigen::Affine3d &goal, const Eigen::VectorXd &joint_seed) const
{
  if (!kin_.checkJoints(joint_seed))
    throw std::invalid_argument("Seed doesn't match kinematic model");

  if (!goal.matrix().block(0,0,3,3).isUnitary(1e-6))
        throw std::invalid_argument("Goal pose not proper affine");

  return constrained_ik::SolverState(goal, joint_seed);
}

void Constrained_IK::updateState(constrained_ik::SolverState &state, const Eigen::VectorXd &joints) const
{
  // update maximum iterations
  state.iter++;

  // update joint convergence
  state.joints_delta = joints - state.joints;
  state.joints = joints;
  kin_.calcFwdKin(joints, state.pose_estimate);

  if(state.planning_scene && state.robot_state)
  {
    state.robot_state->setJointGroupPositions(kin_.getJointModelGroup()->getName(), joints);
    state.robot_state->update();
  }

  if (config_.debug_mode)
    state.iteration_path.push_back(joints);

}

} // namespace constrained_ik


text:
// Boost.TypeErasure library
//
// Copyright 2011-2012 Steven Watanabe
//
// Distributed under the Boost Software License Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// $Id$

#if !defined(BOOST_PP_IS_ITERATING)

#ifndef BOOST_TYPE_ERASURE_REQUIRE_MATCH_HPP_INCLUDED
#define BOOST_TYPE_ERASURE_REQUIRE_MATCH_HPP_INCLUDED

#include <boost/throw_exception.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/and.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/preprocessor/cat.hpp>
#include <boost/preprocessor/facilities/intercept.hpp>
#include <boost/preprocessor/iteration/iterate.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>
#include <boost/preprocessor/repetition/enum_trailing_params.hpp>
#include <boost/preprocessor/repetition/enum_trailing_binary_params.hpp>
#include <boost/type_erasure/detail/extract_concept.hpp>
#include <boost/type_erasure/relaxed.hpp>
#include <boost/type_erasure/check_match.hpp>
#include <boost/type_erasure/exception.hpp>

namespace boost {
namespace type_erasure {

#ifndef BOOST_TYPE_ERASURE_DOXYGEN
template<class Concept>
class binding;
#endif

#ifdef BOOST_TYPE_ERASURE_DOXYGEN

/**
 * Checks that the actual types stored in all the @ref any
 * arguments match the types specified by @c binding.  If
 * they do not match then,
 * - If @ref relaxed is in @c Concept, throws @ref bad_function_call.
 * - Otherwise the behavior is undefined.
 *
 * If @c binding is not specified, it will be deduced from
 * the arguments.
 *
 * \post \call<code>(binding, f, args...)</code> is valid.
 */
template<class Concept, class Op, class... U>
void require_match(const binding<Concept>& binding_arg, const Op& f, U&&... args);

/**
 * \overload
 */
template<class Op, class... U>
void require_match(const Op& f, U&&... args);

#else

#if !defined(BOOST_NO_CXX11_VARIADIC_TEMPLATES) && !defined(BOOST_NO_CXX11_RVALUE_REFERENCES)

namespace detail {

template<class Concept, class Op, class... U>
void require_match_impl(
    ::boost::mpl::true_,
    const ::boost::type_erasure::binding<Concept>& table,
    const Op& op,
    U&&... arg)
{
    if(!::boost::type_erasure::check_match(table, op, std::forward<U>(arg)...)) {
        BOOST_THROW_EXCEPTION(::boost::type_erasure::bad_function_call());
    }
}

template<class Concept, class Op, class... U>
void require_match_impl(
    ::boost::mpl::false_,
    const ::boost::type_erasure::binding<Concept>&,
    const Op&,
    U&&...)
{}

template<class Op, class... U>
void require_match_impl(
    ::boost::mpl::true_,
    const Op& op,
    U&&... arg)
{
    if(!::boost::type_erasure::check_match(op, ::std::forward<U>(arg)...)) {
        BOOST_THROW_EXCEPTION(::boost::type_erasure::bad_function_call());
    }
}

template<class Op, class... U>
void require_match_impl(
    ::boost::mpl::false_,
    const Op&,
    U&&...)
{}

}

template<class Concept, class Op, class... U>
void require_match(
    const ::boost::type_erasure::binding<Concept>& table,
    const Op& op,
    U&&... arg)
{
    ::boost::type_erasure::is_relaxed<Concept> cond;
    ::boost::type_erasure::detail::require_match_impl(cond, table, op, ::std::forward<U>(arg)...);
}

#ifndef BOOST_TYPE_ERASURE_USE_MP11

template<class Op, class... U>
void require_match(
    const Op& op,
    U&&... arg)
{
    ::boost::type_erasure::is_relaxed<
        typename ::boost::type_erasure::detail::extract_concept<
            typename ::boost::type_erasure::detail::get_signature<Op>::type,
            U...>::type
    > cond;
    ::boost::type_erasure::detail::require_match_impl(cond, op, ::std::forward<U>(arg)...);
}

#else

template<class Op, class... U>
void require_match(
    const Op& op,
    U&&... arg)
{
    ::boost::type_erasure::is_relaxed<
        ::boost::type_erasure::detail::extract_concept_t<
            ::boost::type_erasure::detail::get_args_t<
                typename ::boost::type_erasure::detail::get_signature<Op>::type
            >,
            ::boost::mp11::mp_list< ::boost::remove_reference_t<U>...> >
    > cond;
    ::boost::type_erasure::detail::require_match_impl(cond, op, ::std::forward<U>(arg)...);
}

#endif

#else

#define BOOST_PP_FILENAME_1 <boost/type_erasure/require_match.hpp>
#define BOOST_PP_ITERATION_LIMITS (0, BOOST_TYPE_ERASURE_MAX_ARITY)
#include BOOST_PP_ITERATE()

#endif

#endif

}
}

#endif

#else

#define N BOOST_PP_ITERATION()

#ifdef BOOST_NO_CXX11_RVALUE_REFERENCES
#define RREF &
#define BOOST_TYPE_ERASURE_FORWARD_ARGS(N, X, x) BOOST_PP_ENUM_TRAILING_PARAMS(N, x)
#else
#define RREF &&
#define BOOST_TYPE_ERASURE_FORWARD_ARGS_I(z, n, data) std::forward<BOOST_PP_CAT(BOOST_PP_TUPLE_ELEM(2, 0, data), n)>(BOOST_PP_CAT(BOOST_PP_TUPLE_ELEM(2, 1, data), n))
#define BOOST_TYPE_ERASURE_FORWARD_ARGS(N, X, x) BOOST_PP_ENUM_TRAILING(N, BOOST_TYPE_ERASURE_FORWARD_ARGS_I, (X, x))
#endif

namespace detail {

template<
    class Concept,
    class Op
    BOOST_PP_ENUM_TRAILING_PARAMS(N, class U)
>
void require_match_impl(
    ::boost::mpl::true_,
    const ::boost::type_erasure::binding<Concept>& table,
    const Op& op
    BOOST_PP_ENUM_TRAILING_BINARY_PARAMS(N, U, RREF arg))
{
    if(!::boost::type_erasure::check_match
        (table, op BOOST_TYPE_ERASURE_FORWARD_ARGS(N, U, arg))) {
        BOOST_THROW_EXCEPTION(::boost::type_erasure::bad_function_call());
    }
}

template<
    class Concept,
    class Op
    BOOST_PP_ENUM_TRAILING_PARAMS(N, class U)
>
void require_match_impl(
    ::boost::mpl::false_,
    const ::boost::type_erasure::binding<Concept>&,
    const Op&
    BOOST_PP_ENUM_TRAILING_BINARY_PARAMS(N, U, RREF BOOST_PP_INTERCEPT))
{}

#if N != 0

template<
    class Op
    BOOST_PP_ENUM_TRAILING_PARAMS(N, class U)
>
void require_match_impl(
    ::boost::mpl::true_,
    const Op& op
    BOOST_PP_ENUM_TRAILING_BINARY_PARAMS(N, U, RREF arg))
{
    if(!::boost::type_erasure::check_match
        (op BOOST_TYPE_ERASURE_FORWARD_ARGS(N, U, arg))) {
        BOOST_THROW_EXCEPTION(::boost::type_erasure::bad_function_call());
    }
}

template<
    class Op
    BOOST_PP_ENUM_TRAILING_PARAMS(N, class U)
>
void require_match_impl(
    ::boost::mpl::false_,
    const Op&
    BOOST_PP_ENUM_TRAILING_BINARY_PARAMS(N, U, RREF BOOST_PP_INTERCEPT))
{}

#endif

}

template<
    class Concept,
    class Op
    BOOST_PP_ENUM_TRAILING_PARAMS(N, class U)
>
void require_match(
    const ::boost::type_erasure::binding<Concept>& table,
    const Op& op
    BOOST_PP_ENUM_TRAILING_BINARY_PARAMS(N, U, RREF arg))
{
    ::boost::type_erasure::is_relaxed<Concept> cond;
    ::boost::type_erasure::detail::require_match_impl
        (cond, table, op BOOST_TYPE_ERASURE_FORWARD_ARGS(N, U, arg));
}

#if N != 0

template<
    class Op
    BOOST_PP_ENUM_TRAILING_PARAMS(N, class U)
>
void require_match(
    const Op& op
    BOOST_PP_ENUM_TRAILING_BINARY_PARAMS(N, U, RREF arg))
{
    ::boost::type_erasure::is_relaxed<
        typename ::boost::type_erasure::detail::BOOST_PP_CAT(do_extract_concept, N)<
        typename ::boost::type_erasure::detail::get_signature<Op>::type,
        BOOST_PP_ENUM_PARAMS(N, U)>::type
    > cond;
    ::boost::type_erasure::detail::require_match_impl
        (cond, op BOOST_TYPE_ERASURE_FORWARD_ARGS(N, U, arg));
}

#endif

#undef RREF
#undef BOOST_TYPE_ERASURE_FORWARD_ARGS
#undef BOOST_TYPE_ERASURE_FORWARD_ARGS_I
#undef N

#endif


text:
//
// Copyright (c) 2018-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/vinniefalco/BeastLounge
//

// Test that header file is self-contained.
#include <boost/beast/_experimental/json/allocator.hpp>

#include <boost/beast/_experimental/unit_test/suite.hpp>
#include <vector>

namespace boost {
namespace beast {
namespace json {

class allocator_test : public unit_test::suite
{
public:
    void
    testAllocator()
    {
        auto a1 = make_storage_ptr(
            std::allocator<void>{});
        auto a2 = a1;
        auto a3 = std::move(a2);
        a2 = a1;
        a3 = std::move(a2);

        std::vector<char, allocator<char>> v(
            allocator<char>(make_storage_ptr(
                std::allocator<void>{})));
        v.resize(100);
        BEAST_EXPECT(v.capacity() >= 100);
    }

    void
    run()
    {
        testAllocator();
    }
};

BEAST_DEFINE_TESTSUITE(beast,json,allocator);

} // json
} // beast
} // boost

max_stars_repo_name: djarek/BeastLounge

max_issues_repo_name: djarek/BeastLounge

max_forks_repo_name: djarek/BeastLounge


text: #include <boost/range/atl.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
//          Copyright Oliver Kowalke 2014.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <cstdlib>
#include <exception>
#include <iostream>
#include <stdexcept>
#include <string>

#include <boost/context/execution_context.hpp>

namespace ctx = boost::context;

struct my_exception : public std::runtime_error {
    my_exception( std::string const& what) :
        std::runtime_error{ what } {
    }
};

int main() {
    ctx::execution_context< void > ctx([](ctx::execution_context<void> && ctx) {
        for (;;) {
            try {
                    std::cout << "entered" << std::endl;
                    ctx = ctx();
            } catch ( ctx::ontop_error const& e) {
                try {
                    std::rethrow_if_nested( e);
                } catch ( my_exception const& ex) {
                    std::cerr << "my_exception: " << ex.what() << std::endl;
                }
                return e.get_context< void >();
            }
        }
        return std::move( ctx);
    });
    ctx = ctx();
    ctx = ctx();
    ctx = ctx( ctx::exec_ontop_arg, []() { throw my_exception("abc"); });

    std::cout << "main: done" << std::endl;

    return EXIT_SUCCESS;
}


text:
// Copyright 2021, Autonomous Space Robotics Lab (ASRL)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * \file sensor_model_base.hpp
 * \brief Header file for the ASRL vision package
 * \details Several convenient typedefs
 *
 * \author Kirk MacTavish, Autonomous Space Robotics Lab (ASRL)
 */
#pragma once

#include <Eigen/Core>
#include <opencv2/features2d/features2d.hpp>

namespace vtr {
namespace vision {

// Feature keypoints
typedef cv::KeyPoint Keypoint;
typedef std::vector<Keypoint> Keypoints;
typedef Eigen::Map<Eigen::Matrix<float, 2, Eigen::Dynamic>, Eigen::Unaligned,
                   Eigen::Stride<sizeof(Keypoint), sizeof(float)> >
    KeypointsMap;
typedef Eigen::Map<const Eigen::Matrix<float, 2, Eigen::Dynamic>,
                   Eigen::Unaligned,
                   Eigen::Stride<sizeof(Keypoint) / sizeof(float), 0> >
    KeypointsConstMap;

// Camera points
typedef Eigen::Vector2f CameraPoint;
typedef Eigen::Matrix<float, 2, Eigen::Dynamic> CameraPoints;
typedef Eigen::Vector3f HomogeneousPoint;
typedef Eigen::Matrix<float, 3, Eigen::Dynamic> HomogeneousPoints;
// To convert CameraPoints to HomogeneousPoints,
// homo = cam.colwise().homogeneous();
// To convert HomogeneousPoints to CameraPoints,
// cam = homo.hnormalized()

// Whitened measurement errors
typedef Eigen::Matrix<double, 1, Eigen::Dynamic> ErrorList;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> MeasVarList;

}  // namespace vision
}  // namespace vtr