├── python ├── pykollagen │ ├── __init__.py │ ├── pykollagen.py │ └── pykollagen.cpp ├── plot.py ├── plotAPE.py ├── ericsson.mplstyle ├── animate.py ├── style.py └── plot_timing.py ├── setup.cfg ├── tests ├── test_data │ ├── multig2o_test │ │ ├── inter_agent_lc.dat │ │ ├── agent2 │ │ │ ├── agent2_GT.tum │ │ │ └── posegraph.g2o │ │ └── agent1 │ │ │ ├── agent1_GT.tum │ │ │ └── posegraph.g2o │ ├── walker2.tum │ ├── walker2.g2o │ ├── walker1.tum │ ├── walker2_3D.g2o │ ├── multig2o_test_3D │ │ ├── agent1 │ │ │ ├── agent1_GT.tum │ │ │ └── posegraph.g2o │ │ ├── agent2 │ │ │ ├── agent2_GT.tum │ │ │ └── posegraph.g2o │ │ └── inter_agent_lc.dat │ ├── walker1.g2o │ ├── test2.json │ ├── test.json │ └── walker1_3D.g2o ├── CMakeLists.txt ├── DataGeneratorAttorney.h ├── testJSONParse.cpp └── testDataGenerator.cpp ├── output └── .gitignore ├── doc └── information-matrix-derivation.pdf ├── include ├── kollagen.h └── kollagen │ ├── Point.h │ ├── MultiLoopclosure.h │ ├── InterLCParams.h │ ├── RNGenerator.h │ ├── Loopclosure.h │ ├── DiscreteCircle.h │ ├── WalkerParams.h │ ├── AgentParams.h │ ├── utils.hpp │ ├── LCParams.h │ ├── DataGeneratorParams.h │ ├── generate.h │ ├── JsonParse.h │ ├── Walker.h │ ├── Walker-impl.h │ ├── MultiG2oParse.h │ ├── DataGenerator.h │ └── Functions.h ├── .gitlab-ci.yml ├── src └── generate.cpp ├── .build_script.sh ├── LICENSE ├── CMakeLists.txt ├── .build_helper.sh ├── setup.py ├── examples ├── example.cpp ├── data │ ├── iSAM2example.json │ └── example.json ├── timing.cpp └── iSAM2example.cpp ├── .gitignore ├── data ├── Multi10000x5.json └── Multi3500x8.json └── README.md /python/pykollagen/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | description-file=README.md 3 | license_files=LICENSE 4 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test/inter_agent_lc.dat: -------------------------------------------------------------------------------- 1 | 2 0 1 0 1 -2 0 1 0.000000 0.000000 1 0.000000 1 2 | -------------------------------------------------------------------------------- /output/.gitignore: -------------------------------------------------------------------------------- 1 | # Output files should be ignored, but this directory is good to have 2 | * 3 | !.gitignore 4 | -------------------------------------------------------------------------------- /doc/information-matrix-derivation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/EricssonResearch/kollagen/HEAD/doc/information-matrix-derivation.pdf -------------------------------------------------------------------------------- /tests/test_data/multig2o_test/agent2/agent2_GT.tum: -------------------------------------------------------------------------------- 1 | 0 -1 2 0.0 0.0 0.0 0 1 2 | 1 0 2 0.0 0.0 0.0 0 1 3 | 2 1 2 0.0 0.0 0.0 0 1 4 | 3 2 2 0.0 0.0 0.0 0 1 5 | -------------------------------------------------------------------------------- /tests/test_data/walker2.tum: -------------------------------------------------------------------------------- 1 | 0 -1 2 0.0 0.0 0.0 0 1 2 | 1 0 2 0.0 0.0 0.0 0 1 3 | 2 1 2 0.0 0.0 0.0 0 1 4 | 3 2 2 0.0 0.0 0.0 0 1 5 | 4 3 2 0.0 0.0 0.0 0 1 6 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test/agent1/agent1_GT.tum: -------------------------------------------------------------------------------- 1 | 0 0 0 0.0 0.0 0.0 0 1 2 | 1 1 0 0.0 0.0 0.0 0 1 3 | 2 1 -1 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 4 | 3 2 -1 0.0 0.0 0.0 0 1 5 | -------------------------------------------------------------------------------- /include/kollagen.h: -------------------------------------------------------------------------------- 1 | #include "kollagen/Functions.h" 2 | #include "kollagen/DataGenerator.h" 3 | #include "kollagen/MultiG2oParse.h" 4 | #include "kollagen/JsonParse.h" 5 | 6 | #ifndef KOLLAGEN_H 7 | #define KOLLAGEN_H 8 | 9 | 10 | #endif /* !KOLLAGEN_H */ 11 | -------------------------------------------------------------------------------- /tests/test_data/walker2.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE2 0 -1 2 0 2 | VERTEX_SE2 1 0 2 0 3 | VERTEX_SE2 2 1 2 0 4 | VERTEX_SE2 3 2 2 0 5 | VERTEX_SE2 4 3 2 0 6 | EDGE_SE2 0 1 1 0 0 1 0 0 1 0 1 7 | EDGE_SE2 1 2 1 0 0 1 0 0 1 0 1 8 | EDGE_SE2 2 3 1 0 0 1 0 0 1 0 1 9 | EDGE_SE2 3 4 1 0 0 1 0 0 1 0 1 10 | -------------------------------------------------------------------------------- /include/kollagen/Point.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_H 2 | #define POINT_H 3 | 4 | namespace kollagen 5 | { 6 | 7 | struct Point { 8 | Point(int x, int y) : x(x), y(y){}; 9 | const int x; 10 | const int y; 11 | bool operator==(const Point &rhs) const { return x == rhs.x && y == rhs.y; }; 12 | }; 13 | 14 | } // namespace kollagen 15 | #endif /* !POINT_H */ 16 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test/agent2/posegraph.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE2 0 -1 2 0 2 | VERTEX_SE2 1 0 2 0 3 | VERTEX_SE2 2 1 2 0 4 | VERTEX_SE2 3 2 2 0 5 | VERTEX_SE2 4 3 2 0 6 | EDGE_SE2 0 1 1 0 0 1 0.000000 0.000000 1 0.000000 1 7 | EDGE_SE2 1 2 1 0 0 1 0.000000 0.000000 1 0.000000 1 8 | EDGE_SE2 2 3 1 0 0 1 0.000000 0.000000 1 0.000000 1 9 | EDGE_SE2 3 4 1 0 0 1 0.000000 0.000000 1 0.000000 1 10 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: ubuntu:jammy 2 | 3 | variables: 4 | DEBIAN_FRONTEND: noninteractive 5 | GIT_SUBMODULE_STRATEGY: recursive 6 | 7 | stages: 8 | - test-and-build 9 | 10 | test: 11 | stage: test-and-build 12 | script: 13 | - cat .build_script.sh | /bin/bash 14 | only: 15 | - master 16 | - develop 17 | - merge_requests 18 | artifacts: 19 | paths: 20 | - build/ 21 | - doc/ 22 | expire_in: 1 day 23 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test/agent1/posegraph.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE2 0 0 0 0 2 | VERTEX_SE2 1 1 0 0 3 | VERTEX_SE2 2 1 -1 -1.5707963267948966 4 | VERTEX_SE2 3 2 -1 0 5 | EDGE_SE2 0 1 1 0 0 1 0.000000 0.000000 1 0.000000 1 6 | EDGE_SE2 1 2 6.123233995736766e-17 -1 -1.5707963267948966 1 0.000000 0.000000 1 0.000000 1 7 | EDGE_SE2 2 3 6.123233995736766e-17 1 1.5707963267948966 1 0.000000 0.000000 1 0.000000 1 8 | EDGE_SE2 1 3 1 -1 0 10000 0.000000 0.000000 10000 0.000000 1000000 9 | -------------------------------------------------------------------------------- /tests/test_data/walker1.tum: -------------------------------------------------------------------------------- 1 | 0 0 0 0.0 0.0 0.0 0 1 2 | 1 1 0 0.0 0.0 0.0 0 1 3 | 2 1 -1 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 4 | 3 2 -1 0.0 0.0 0.0 0 1 5 | 4 2 0 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 6 | 5 1 0 0.0 0.0 0.0 -1 6.123233995736766e-17 7 | 6 1 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 8 | 7 1 2 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 9 | 8 1 3 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 10 | -------------------------------------------------------------------------------- /src/generate.cpp: -------------------------------------------------------------------------------- 1 | #include "kollagen/generate.h" 2 | 3 | int main(int argc, char *argv[]) { 4 | bool multig2o = true; 5 | bool singleg2o = true; 6 | auto [json_file, save_dir] = kollagen::get_CLI_arguments(argc, argv); 7 | 8 | if (!kollagen::CLI_arguments_OK(json_file, save_dir, multig2o, singleg2o)) { 9 | return EXIT_FAILURE; 10 | } 11 | 12 | kollagen::generate_and_save(json_file, save_dir, multig2o, singleg2o); 13 | 14 | return EXIT_SUCCESS; 15 | } 16 | -------------------------------------------------------------------------------- /.build_script.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # .build_script.sh 3 | 4 | set -e 5 | 6 | ROOT_DIR=`pwd` 7 | 8 | source ./.build_helper.sh 9 | 10 | kollagen_download_debian_packages 11 | kollagen_python_environment 12 | kollagen_python_dependencies 13 | cd "$ROOT_DIR" 14 | kollagen_prepare_build 15 | cd build 16 | kollagen_cmake 17 | kollagen_build 18 | kollagen_run_tests 19 | kollagen_run_example 20 | #kollagen_plot_and_animate 21 | cd "$ROOT_DIR" 22 | kollagen_build_pip_package 23 | kollagen_build_docs 24 | -------------------------------------------------------------------------------- /tests/test_data/walker2_3D.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT 0 -1 2 0.0 0.0 0.0 0 1 2 | VERTEX_SE3:QUAT 1 0 2 0.0 0.0 0.0 0 1 3 | VERTEX_SE3:QUAT 2 1 2 0.0 0.0 0.0 0 1 4 | VERTEX_SE3:QUAT 3 2 2 0.0 0.0 0.0 0 1 5 | VERTEX_SE3:QUAT 4 3 2 0.0 0.0 0.0 0 1 6 | EDGE_SE3:QUAT 0 1 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 7 | EDGE_SE3:QUAT 1 2 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 8 | EDGE_SE3:QUAT 2 3 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 9 | EDGE_SE3:QUAT 3 4 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 10 | -------------------------------------------------------------------------------- /python/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | plot.py: Plot results 6 | """ 7 | 8 | import sys 9 | import style 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | 13 | X = [] 14 | Y = [] 15 | 16 | try: 17 | filename = sys.argv[1] 18 | if filename[-4:] != ".svg": 19 | filename += ".svg" 20 | except: 21 | filename = "" 22 | 23 | 24 | OK = True 25 | i = 0 26 | while OK: 27 | i += 1 28 | try: 29 | X.append(np.fromfile("./x"+str(i)+".bin", dtype=np.double)) 30 | Y.append(np.fromfile("./y"+str(i)+".bin", dtype=np.double)) 31 | except: 32 | OK = False 33 | 34 | for i in range(0,len(X)): 35 | plt.plot(X[i],Y[i],".-", linewidth=0.5, markersize=1.0) 36 | 37 | plt.axis("equal") 38 | 39 | if filename: 40 | plt.savefig(filename) 41 | else: 42 | plt.show() 43 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test_3D/agent1/agent1_GT.tum: -------------------------------------------------------------------------------- 1 | 0 0 0 0.0 0.0 0.0 0 1 2 | 1 0.25 0 0.0 0.0 0.0 0 1 3 | 2 0.5 0 0.0 0.0 0.0 0 1 4 | 3 0.75 0 0.0 0.0 0.0 0 1 5 | 4 1 0 0.0 0.0 0.0 0 1 6 | 5 0.75 0 0.0 0.0 0.0 -1 6.123233995736766e-17 7 | 6 0.5 0 0.0 0.0 0.0 -1 6.123233995736766e-17 8 | 7 0.25 0 0.0 0.0 0.0 -1 6.123233995736766e-17 9 | 8 0 0 0.0 0.0 0.0 -1 6.123233995736766e-17 10 | 9 0 0.25 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 11 | 10 0 0.5 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 12 | 11 0 0.75 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 13 | 12 0 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 14 | 13 0 0.75 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 15 | 14 0 0.5 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 16 | 15 0 0.25 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 17 | 16 0 0 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 18 | -------------------------------------------------------------------------------- /include/kollagen/MultiLoopclosure.h: -------------------------------------------------------------------------------- 1 | #include "Loopclosure.h" 2 | 3 | #ifndef MULTILOOPCLOSURE_H 4 | #define MULTILOOPCLOSURE_H 5 | 6 | namespace kollagen 7 | { 8 | 9 | /** 10 | * \brief Inter-agent loop closure struct. 11 | * 12 | * Includes fields ID1 and ID2 to associate a loop-closure with the 13 | * corresponding agents. 14 | */ 15 | struct MultiLoopclosure : public Loopclosure { 16 | MultiLoopclosure(WalkerID ID1, WalkerID ID2, NodeID from, NodeID to, 17 | double dx, double dy, double dtheta, double I11, double I22, 18 | double I33) 19 | : Loopclosure(from, to, dx, dy, dtheta, I11, I22, I33), ID1(ID1), 20 | ID2(ID2){}; 21 | /** 22 | * \brief ID of first agent associated with loop closure. 23 | */ 24 | WalkerID ID1{}; 25 | /** 26 | * \brief ID of second agent associated with loop closure. 27 | */ 28 | WalkerID ID2{}; 29 | }; 30 | 31 | } // namespace kollagen 32 | #endif /* !MULTILOOPCLOSURE_H */ 33 | -------------------------------------------------------------------------------- /include/kollagen/InterLCParams.h: -------------------------------------------------------------------------------- 1 | #include "LCParams.h" 2 | #ifndef INTERLCPARAMS_H 3 | #define INTERLCPARAMS_H 4 | 5 | namespace kollagen 6 | { 7 | 8 | /** 9 | * \brief Loop Closure Parameters. 10 | * For inter-agent loop closures. 11 | */ 12 | struct InterLCParams : public LCParams { 13 | InterLCParams() = default; 14 | InterLCParams(double sigma_pos, double sigma_ang, 15 | double estimated_information_pos, 16 | double estimated_information_ang, double radius, 17 | double idemLCprob, double prob_scale, int seed) 18 | : LCParams(sigma_pos, sigma_ang, estimated_information_pos, 19 | estimated_information_ang, radius, 20 | idemLCprob, prob_scale), 21 | seed(seed){}; 22 | /** 23 | * \brief Random Number Generator seed (unitless) 24 | * 25 | * Seed used for inter-agent loop closures. 26 | */ 27 | int seed{1234}; 28 | }; 29 | 30 | } // namespace kollagen 31 | #endif /* !INTERLCPARAMS_H */ 32 | -------------------------------------------------------------------------------- /python/plotAPE.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | plot.py: Plot results 6 | """ 7 | 8 | import sys 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | X = np.fromfile("APE_time.bin", dtype=np.intc) 13 | Y = [] 14 | 15 | N = 100 16 | rolling_average = True 17 | 18 | try: 19 | filename = sys.argv[1] 20 | if filename[-4:] != ".svg": 21 | filename += ".svg" 22 | except: 23 | filename = "" 24 | 25 | 26 | OK = True 27 | i = 0 28 | while OK: 29 | i += 1 30 | try: 31 | Y.append(np.fromfile("./APE_agent"+str(i)+".bin", dtype=np.double)) 32 | except: 33 | OK = False 34 | 35 | for i in range(0,len(Y)): 36 | if rolling_average: 37 | plt.plot(X[N-1:], np.convolve(Y[i], np.ones(N)/N, mode='valid'), label="Agent %i" % (i+1)) 38 | else: 39 | plt.plot(X, Y[i], label="Agent %i" % (i+1)) 40 | 41 | plt.legend() 42 | 43 | if filename: 44 | plt.savefig(filename) 45 | else: 46 | plt.show() 47 | -------------------------------------------------------------------------------- /python/pykollagen/pykollagen.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import argparse 3 | 4 | def gen(): 5 | import kollagen 6 | parser = argparse.ArgumentParser(prog="kollagenr8",description="Generate data from json file using kollagen") 7 | parser.add_argument("json_file", action="store", type=str, help="Path to json file used for data generation") 8 | parser.add_argument("output_dir", action="store", nargs="?", default="./output", type=str, help="Output directory, defaults to './output'") 9 | parser.add_argument("-s", "--singleg2o", action="store_true", default=False, help="Save singleg2o") 10 | parser.add_argument("-m", "--multig2o", action="store_true", default=True, help="Save multig2o (default)") 11 | parser.add_argument("--no-singleg2o", action="store_false", dest="singleg2o", help="Don't save singleg2o (default)") 12 | parser.add_argument("--no-multig2o", action="store_false", dest="multig2o", help="Don't save multig2o") 13 | 14 | args = parser.parse_args() 15 | 16 | kollagen.generate(args.json_file, args.output_dir, args.multig2o, args.singleg2o) 17 | -------------------------------------------------------------------------------- /include/kollagen/RNGenerator.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef RNGENERATOR_H 4 | #define RNGENERATOR_H 5 | 6 | namespace kollagen 7 | { 8 | 9 | class RNGenerator { 10 | public: 11 | RNGenerator() = default; 12 | explicit RNGenerator(int seed, bool allow_retrocession = true) 13 | : gen(seed), generator(0.0, 1.0), 14 | int_generator(-1, 1 + static_cast(allow_retrocession)), 15 | uniform_generator(0.0, 1.0), seed_(seed){}; 16 | [[nodiscard]] double draw_normal() { return generator(gen); }; 17 | [[nodiscard]] double draw_uniform() { return uniform_generator(gen); }; 18 | [[nodiscard]] int draw_int() { return int_generator(gen); }; 19 | void seed(int new_seed) { gen.seed(new_seed); }; 20 | [[nodiscard]] int seed() const { return seed_; }; 21 | 22 | private: 23 | std::mt19937 gen; 24 | std::normal_distribution generator; 25 | std::uniform_int_distribution int_generator; 26 | std::uniform_real_distribution uniform_generator; 27 | int seed_; 28 | }; 29 | 30 | } // namespace kollagen 31 | #endif /* !RNGENERATOR_H */ 32 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test_3D/agent2/agent2_GT.tum: -------------------------------------------------------------------------------- 1 | 0 0 -2 0.0 0.0 0.0 0 1 2 | 1 0 -1.75 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 3 | 2 0 -1.5 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 4 | 3 0 -1.25 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 5 | 4 0 -1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 6 | 5 0 -0.75 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 7 | 6 0 -0.5 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 8 | 7 0 -0.25 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 9 | 8 0 0 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 10 | 9 0 0.25 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 11 | 10 0 0.5 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 12 | 11 0 0.75 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 13 | 12 0 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 14 | 13 0 1.25 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 15 | 14 0 1.5 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 16 | 15 0 1.75 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 17 | 16 0 2 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2022 Ericsson AB 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining 4 | a copy of this software and associated documentation files (the 5 | "Software"), to deal in the Software without restriction, including 6 | without limitation the rights to use, copy, modify, merge, publish, 7 | distribute, sublicense, and/or sell copies of the Software, and to 8 | permit persons to whom the Software is furnished to do so, subject to 9 | the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be 12 | included in all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 15 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 16 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 17 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 18 | LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 19 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 20 | WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test_3D/inter_agent_lc.dat: -------------------------------------------------------------------------------- 1 | 2 8 1 16 -0.014624 -0.018364 0.0 0.0 0.0 -1.000000 0.000441 1600.000000 0.0 0.0 0.0 0.0 0.0 1600.000000 0.0 0.0 0.0 0.0 1000000.000000 0.0 0.0 0.0 1600.000000 0.0 0.0 1600.000000 0.0 1600.000000 2 | 2 9 1 15 0.035083 0.008447 0.0 0.0 0.0 -1.000000 0.000722 1600.000000 0.0 0.0 0.0 0.0 0.0 1600.000000 0.0 0.0 0.0 0.0 1000000.000000 0.0 0.0 0.0 1600.000000 0.0 0.0 1600.000000 0.0 1600.000000 3 | 2 10 1 14 -0.022942 0.024511 0.0 0.0 0.0 -1.000000 0.000515 1600.000000 0.0 0.0 0.0 0.0 0.0 1600.000000 0.0 0.0 0.0 0.0 1000000.000000 0.0 0.0 0.0 1600.000000 0.0 0.0 1600.000000 0.0 1600.000000 4 | 2 11 1 13 -0.031795 -0.038002 0.0 0.0 0.0 -0.999999 0.001562 1600.000000 0.0 0.0 0.0 0.0 0.0 1600.000000 0.0 0.0 0.0 0.0 1000000.000000 0.0 0.0 0.0 1600.000000 0.0 0.0 1600.000000 0.0 1600.000000 5 | 2 12 1 12 0.047890 -0.032968 0.0 0.0 0.0 -0.001639 0.999999 1600.000000 0.0 0.0 0.0 0.0 0.0 1600.000000 0.0 0.0 0.0 0.0 1000000.000000 0.0 0.0 0.0 1600.000000 0.0 0.0 1600.000000 0.0 1600.000000 6 | 2 14 1 12 -0.514714 0.012303 0.0 0.0 0.0 0.001019 0.999999 1600.000000 0.0 0.0 0.0 0.0 0.0 1600.000000 0.0 0.0 0.0 0.0 1000000.000000 0.0 0.0 0.0 1600.000000 0.0 0.0 1600.000000 0.0 1600.000000 7 | -------------------------------------------------------------------------------- /python/ericsson.mplstyle: -------------------------------------------------------------------------------- 1 | patch.linewidth: 0.5 2 | patch.facecolor: 348ABD # blue 3 | patch.edgecolor: EEEEEE 4 | patch.antialiased: True 5 | 6 | font.size: 10.0 7 | 8 | axes.facecolor: e0e0e0 9 | axes.edgecolor: white 10 | axes.linewidth: 1 11 | axes.grid: True 12 | axes.titlesize: x-large 13 | axes.labelsize: large 14 | axes.labelcolor: 555555 15 | axes.axisbelow: True # grid/ticks are below elements (e.g., lines, text) 16 | 17 | axes.prop_cycle: cycler('color', ["ff3232","0082f0","af78d2","767676","fad22d","0fc373","ff8c0a"]) 18 | # Primary blues 19 | #000082: Blue 1 20 | #0050ca: Blue 2 21 | #0082f0: Ericsson Blue 22 | #48a8fa: Blue 3 23 | #85ccff: Blue 4 24 | # Secondary colors 25 | #0fc373: Green 26 | #fad22d: Yellow 27 | #ff8c0a: Orange 28 | #ff3232: Red 29 | #af78d2: Purple 30 | # Neutral tones 31 | #000000: Black 32 | #242424: Gray 1 33 | #767676: Gray 2 34 | #a0a0a0: Gray 3 35 | #e0e0e0: Gray 4 36 | #f2f2f2: Gray 5 37 | #fafafa: Ericsson White 38 | #ffffff: White 39 | 40 | xtick.color: 555555 41 | xtick.direction: out 42 | 43 | ytick.color: 555555 44 | ytick.direction: out 45 | 46 | grid.color: white 47 | grid.linestyle: - # solid line 48 | 49 | figure.facecolor: white 50 | figure.edgecolor: 0.50 51 | -------------------------------------------------------------------------------- /tests/test_data/walker1.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE2 0 0 0 0 2 | VERTEX_SE2 1 1 0 0 3 | VERTEX_SE2 2 1 -1 -1.5707963267948966 4 | VERTEX_SE2 3 2 -1 0 5 | VERTEX_SE2 4 2 0 1.5707963267948966 6 | VERTEX_SE2 5 1 -1.2246467991473532e-16 -3.1415926535897931 7 | VERTEX_SE2 6 1 0.99999999999999989 1.5707963267948966 8 | VERTEX_SE2 7 1 2 1.5707963267948966 9 | VERTEX_SE2 8 1 3 1.5707963267948966 10 | EDGE_SE2 0 1 1 0 0 1 0 0 1 0 1 11 | EDGE_SE2 1 2 6.123233995736766e-17 -1 -1.5707963267948966 1 0 0 1 0 1 12 | EDGE_SE2 2 3 6.123233995736766e-17 1 1.5707963267948966 1 0 0 1 0 1 13 | EDGE_SE2 3 4 6.123233995736766e-17 1 1.5707963267948966 1 0 0 1 0 1 14 | EDGE_SE2 4 5 6.123233995736766e-17 1 1.5707963267948966 1 0 0 1 0 1 15 | EDGE_SE2 5 6 6.123233995736766e-17 -1 -1.5707963267948966 1 0 0 1 0 1 16 | EDGE_SE2 6 7 1 0 0 1 0 0 1 0 1 17 | EDGE_SE2 7 8 1 0 0 1 0 0 1 0 1 18 | EDGE_SE2 0 2 1 -1 -1.5707963267948966 10000 0 0 10000 0 1000000 19 | EDGE_SE2 1 3 1 -1 0 10000 0 0 10000 0 1000000 20 | EDGE_SE2 2 4 -0.99999999999999989 1 -3.1415926535897931 10000 0 0 10000 0 1000000 21 | EDGE_SE2 1 5 0 0 -3.1415926535897931 10000 0 0 10000 0 1000000 22 | EDGE_SE2 3 5 -1 1 -3.1415926535897931 10000 0 0 10000 0 1000000 23 | EDGE_SE2 0 6 1 1 1.5707963267948966 10000 0 0 10000 0 1000000 24 | EDGE_SE2 4 6 0.99999999999999989 1 0 10000 0 0 10000 0 1000000 25 | -------------------------------------------------------------------------------- /include/kollagen/Loopclosure.h: -------------------------------------------------------------------------------- 1 | #ifndef LOOPCLOSURE_H 2 | #define LOOPCLOSURE_H 3 | 4 | using NodeID = int; 5 | using WalkerID = int; 6 | 7 | namespace kollagen 8 | { 9 | 10 | /** 11 | * \brief Loop closure struct. 12 | */ 13 | struct Loopclosure { 14 | Loopclosure(NodeID from, NodeID to, double dx, double dy, double dtheta, 15 | double I11, double I22, double I33) 16 | : from(from), to(to), dx(dx), dy(dy), dtheta(dtheta), I11(I11), I22(I22), 17 | I33(I33){}; 18 | 19 | /** 20 | * \brief ID of node from which the loop closure originates. 21 | */ 22 | NodeID from; 23 | /** 24 | * \brief ID of node to where the loop closure ends. 25 | */ 26 | NodeID to; 27 | /** 28 | * \brief "Measured" difference in x between the nodes. 29 | */ 30 | double dx; 31 | /** 32 | * \brief "Measured" difference in y between the nodes. 33 | */ 34 | double dy; 35 | /** 36 | * \brief "Measured" difference in angle between the nodes. 37 | */ 38 | double dtheta; 39 | /** 40 | * \brief Information matrix entry I11 [1/(length unit)^2]. 41 | */ 42 | double I11; 43 | /** 44 | * \brief Information matrix entry I22 [1/(length unit)^2]. 45 | */ 46 | double I22; 47 | /** 48 | * \brief Information matrix entry I33 [1/radians^2]. 49 | */ 50 | double I33; 51 | }; 52 | 53 | } // namespace kollagen 54 | #endif /* !LOOPCLOSURE_H */ 55 | -------------------------------------------------------------------------------- /python/pykollagen/pykollagen.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "kollagen/generate.h" 3 | 4 | static PyObject* generate_python(PyObject * /*unused*/, PyObject* args){ 5 | const char * _json_file = nullptr; 6 | const char * _save_dir = nullptr; 7 | int _multig2o = 0; 8 | int _singleg2o = 0; 9 | if (!PyArg_ParseTuple(args, "ssii", &_json_file, &_save_dir, &_multig2o, &_singleg2o)) { return NULL; } 10 | 11 | std::filesystem::path json_file{_json_file}; 12 | std::filesystem::path save_dir{_save_dir}; 13 | bool multig2o{static_cast(_multig2o)}; 14 | bool singleg2o{static_cast(_singleg2o)}; 15 | 16 | kollagen::add_extension(json_file, "json"); 17 | 18 | if (!kollagen::CLI_arguments_OK(json_file, save_dir, multig2o, singleg2o)) { 19 | PyErr_SetString(PyExc_Exception, "Improper arguments provided"); 20 | return NULL; 21 | } 22 | 23 | kollagen::generate_and_save(json_file, save_dir, multig2o, singleg2o); 24 | 25 | Py_RETURN_NONE; 26 | } 27 | 28 | static PyMethodDef kollagen_methods[] = { 29 | {"generate", (PyCFunction)generate_python, METH_VARARGS, nullptr}, 30 | {nullptr, nullptr, 0, nullptr} 31 | }; 32 | 33 | static struct PyModuleDef kollagen_module = { 34 | PyModuleDef_HEAD_INIT, "kollagen", NULL, -1, kollagen_methods 35 | }; 36 | 37 | PyMODINIT_FUNC PyInit_kollagen(void) { 38 | return PyModule_Create(&kollagen_module); 39 | } 40 | -------------------------------------------------------------------------------- /tests/test_data/test2.json: -------------------------------------------------------------------------------- 1 | { 2 | "number_of_steps": 1000, 3 | "align": true, 4 | "number_of_agents": 2, 5 | "exact_information_matrix": false, 6 | "agent_parameters": { 7 | "stepsize": 1.5, 8 | "step_fragment": 2, 9 | "allow_retrocession": true, 10 | "agent_specific": [{ 11 | "std_dev_position": 1, 12 | "std_dev_angle": 2, 13 | "estimated_information_position": 3, 14 | "estimated_information_angle": 4, 15 | "random_number_generator_seed": 5, 16 | "std_dev_intra_loop_closure_position": 6, 17 | "std_dev_intra_loop_closure_angle": 7, 18 | "estimated_information_intra_position": 8, 19 | "estimated_information_intra_angle": 9, 20 | "intra_loop_closure_radius": 10, 21 | "same_position_intra_loop_closure_probability": 11, 22 | "intra_loop_closure_probability_scaling": 12 23 | } 24 | ]}, 25 | "inter_agent_loop_closure_parameters": { 26 | "std_dev_loop_closure_position": 25, 27 | "std_dev_loop_closure_angle": 26, 28 | "estimated_information_position": 27, 29 | "estimated_information_angle": 28, 30 | "loop_closure_radius": 29, 31 | "loop_closure_probability_scaling": 30, 32 | "same_position_loop_closure_probability": 31, 33 | "random_number_generator_seed": 32 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | 3 | project(kollagen) 4 | 5 | include (CTest) 6 | 7 | set(CMAKE_CXX_STANDARD 20) 8 | if(NOT CMAKE_BUILD_TYPE) 9 | set(CMAKE_BUILD_TYPE Release) 10 | endif() 11 | if(NOT MSVC) 12 | set(CMAKE_CXX_FLAGS "-Wall -Wextra") 13 | set(CMAKE_CXX_FLAGS_DEBUG "-g -O0") 14 | set(CMAKE_CXX_FLAGS_Release "-DNDEBUG -O3") 15 | else() 16 | set(CMAKE_CXX_FLAGS "/utf-8 /D_USE_MATH_DEFINES /Ox") 17 | endif() 18 | 19 | option(KOLLAGEN_ENABLE_TESTS "Run the tests" OFF) 20 | option(KOLLAGEN_ENABLE_GTSAM "Enable GTSAM" OFF) 21 | option(KOLLAGEN_BUILD_EXAMPLES "Build examples" ON) 22 | 23 | list(APPEND KOLLAGEN_INCLUDE_DIRS "include") 24 | 25 | if(KOLLAGEN_ENABLE_GTSAM) 26 | find_package(GTSAM REQUIRED) 27 | list(APPEND KOLLAGEN_INCLUDE_DIRS ${GTSAM_INCLUDE_DIR} ) 28 | add_executable(iSAM2example examples/iSAM2example.cpp) 29 | configure_file(examples/data/iSAM2example.json ${CMAKE_CURRENT_BINARY_DIR}/data/iSAM2example.json COPYONLY) 30 | target_link_libraries(iSAM2example gtsam) 31 | endif() 32 | 33 | include_directories( 34 | ${KOLLAGEN_INCLUDE_DIRS} 35 | ) 36 | 37 | if(KOLLAGEN_BUILD_EXAMPLES) 38 | configure_file(examples/data/example.json ${CMAKE_CURRENT_BINARY_DIR}/data/example.json COPYONLY) 39 | add_executable(example examples/example.cpp) 40 | add_executable(timing examples/timing.cpp) 41 | endif() 42 | 43 | add_executable(generate src/generate.cpp) 44 | 45 | if(NOT MSVC) 46 | add_custom_target(${PROJECT_NAME}-symlink ALL ln --force -s ${CMAKE_CURRENT_BINARY_DIR}/generate ${CMAKE_SOURCE_DIR}/generate DEPENDS generate) 47 | endif() 48 | 49 | if(BUILD_TESTING AND KOLLAGEN_ENABLE_TESTS) 50 | add_subdirectory(tests) 51 | endif() 52 | -------------------------------------------------------------------------------- /.build_helper.sh: -------------------------------------------------------------------------------- 1 | kollagen_debian_dependencies() { 2 | echo "cmake libgtest-dev libgmock-dev libgflags-dev python3-pip clang-14 ffmpeg doxygen" 3 | } 4 | 5 | kollagen_download_debian_packages() { 6 | apt update && apt -y install $(kollagen_debian_dependencies) 7 | } 8 | 9 | kollagen_download_debian_packages_sudo() { 10 | sudo apt update && sudo apt -y install $(kollagen_debian_dependencies) 11 | } 12 | 13 | kollagen_python_environment() { 14 | echo "Creating python virtual environment and installing requirements" 15 | pip3 install virtualenv 16 | virtualenv -p python3 env $1 17 | source env/bin/activate 18 | } 19 | 20 | kollagen_python_dependencies() { 21 | pip3 install matplotlib numpy 22 | } 23 | 24 | kollagen_prepare_build() { 25 | mkdir build 26 | } 27 | 28 | kollagen_cmake() { 29 | CC=clang-14 CXX=clang++-14 cmake .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_INSTALL_PREFIX="./install" -DKOLLAGEN_ENABLE_TESTS=ON 30 | } 31 | 32 | kollagen_run_tests() { 33 | echo "Run tests..." 34 | ctest -VV --output-on-failure 35 | echo "Run tests completed." 36 | } 37 | 38 | kollagen_build() { 39 | echo "Move to build stage" 40 | echo "Compiling using all $(nproc --all) available cores" 41 | make -j$(nproc --all) 42 | echo "Compiling done" 43 | } 44 | 45 | kollagen_run_example() { 46 | echo "Running example" 47 | ./example 48 | } 49 | 50 | kollagen_build_docs() { 51 | echo "Building docs" 52 | doxygen Doxyfile 53 | } 54 | 55 | kollagen_plot_and_animate() { 56 | # Requires running kollagen_run_example 57 | echo "Animating" 58 | ../python/animate.py example 59 | echo "Plotting" 60 | ../python/plot.py example 61 | } 62 | 63 | kollagen_build_pip_package() { 64 | pip3 install . 65 | } 66 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | from distutils.core import Extension 3 | from setuptools.command.build_ext import build_ext 4 | 5 | compiler_options = { 6 | 'unix': ["-std=c++20","-O3", "-Wall", "-shared","-fpic"], 7 | 'msvc': ["/std:c++20","/Ox", "/Wall", "/utf-8", "/D_USE_MATH_DEFINES"], 8 | } 9 | 10 | class build_ext_subclass(build_ext): 11 | def build_extensions(self): 12 | c = self.compiler.compiler_type 13 | if c in compiler_options: 14 | for e in self.extensions: 15 | e.extra_compile_args += compiler_options[c] 16 | build_ext.build_extensions(self) 17 | 18 | kollagen_module = Extension( 19 | 'kollagen', 20 | sources=['python/pykollagen/pykollagen.cpp'], 21 | include_dirs=['include'], 22 | ) 23 | 24 | with open("README.md", "r") as fh: 25 | long_description = fh.read() 26 | 27 | setuptools.setup( 28 | name="kollagen", 29 | version="1.0.0", 30 | author="Roberto C. Sundin", 31 | author_email="roberto.castro.sundin@ericsson.com", 32 | description=(""), 33 | long_description=long_description, 34 | long_description_content_type="text/markdown", 35 | url="https://github.com/EricssonResearch/kollagen", 36 | packages=setuptools.find_packages(where="python"), 37 | package_dir={"": "python"}, 38 | entry_points={"console_scripts": ["kollagenr8=pykollagen.pykollagen:gen"]}, 39 | ext_modules=[kollagen_module], 40 | classifiers=[ 41 | "Programming Language :: Python :: 3", 42 | "License :: OSI Approved :: MIT License", 43 | "Operating System :: OS Independent", 44 | ], 45 | python_requires='>=3.7', 46 | cmdclass = {'build_ext': build_ext_subclass}, 47 | ) 48 | -------------------------------------------------------------------------------- /include/kollagen/DiscreteCircle.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Point.h" 4 | 5 | #ifndef DISCRETECIRCLE_H 6 | #define DISCRETECIRCLE_H 7 | 8 | namespace kollagen 9 | { 10 | 11 | class DiscreteCircle { 12 | public: 13 | explicit DiscreteCircle(int radius) : radius_(radius) { 14 | for (int x = -radius; x <= radius; ++x) { 15 | 16 | int y_limit = radius * radius - x * x; 17 | for (int y = 0; y * y <= y_limit; ++y) { 18 | coordinate_list_.emplace_back(x, y); 19 | if (y > 0) { 20 | coordinate_list_.emplace_back(x, -y); 21 | } 22 | } 23 | } 24 | }; 25 | 26 | [[nodiscard]] int radius() const { return radius_; }; 27 | 28 | [[nodiscard]] std::vector coordinates() const { 29 | return coordinate_list_; 30 | }; 31 | 32 | [[nodiscard]] std::vector x_list() const { 33 | std::vector x_list; 34 | for (auto coordinate : coordinate_list_) { 35 | x_list.emplace_back(coordinate.x); 36 | } 37 | return x_list; 38 | }; 39 | [[nodiscard]] std::vector y_list() const { 40 | std::vector y_list; 41 | for (auto coordinate : coordinate_list_) { 42 | y_list.emplace_back(coordinate.y); 43 | } 44 | return y_list; 45 | }; 46 | 47 | auto begin() { return coordinate_list_.begin(); } 48 | auto end() { return coordinate_list_.end(); } 49 | [[nodiscard]] auto cbegin() const { return coordinate_list_.begin(); } 50 | [[nodiscard]] auto cend() const { return coordinate_list_.end(); } 51 | [[nodiscard]] auto begin() const { return coordinate_list_.begin(); } 52 | [[nodiscard]] auto end() const { return coordinate_list_.end(); } 53 | 54 | private: 55 | const int radius_; 56 | std::vector coordinate_list_; 57 | }; 58 | 59 | 60 | } // namespace kollagen 61 | #endif /* !DISCRETECIRCLE_H */ 62 | -------------------------------------------------------------------------------- /python/animate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | animate.py: Animate results 6 | """ 7 | 8 | import sys 9 | import style 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from matplotlib import animation 13 | 14 | X = [] 15 | Y = [] 16 | try: 17 | filename = sys.argv[1] 18 | if filename[-4:]!=".mp4": 19 | filename += ".mp4" 20 | except: 21 | filename = "animation.mp4" 22 | 23 | OK = True 24 | i = 0 25 | while OK: 26 | i += 1 27 | try: 28 | X.append(np.fromfile("./x"+str(i)+".bin", dtype=np.double)) 29 | Y.append(np.fromfile("./y"+str(i)+".bin", dtype=np.double)) 30 | except: 31 | OK = False 32 | 33 | padding = 1 34 | 35 | x_max = np.max(X) + padding 36 | x_min = np.min(X) - padding 37 | 38 | y_max = np.max(Y) + padding 39 | y_min = np.min(Y) - padding 40 | 41 | Writer = animation.writers['ffmpeg'] 42 | writer = Writer(fps=60, metadata=dict(artist='Me'), bitrate=1800) 43 | 44 | def update(num, paths, positions, T): 45 | print("Frame", num, "of", T) 46 | for i, path in enumerate(paths): 47 | path[0].set_data(X[i][0:num], Y[i][0:num]) 48 | for i, position in enumerate(positions): 49 | position[0].set_data(X[i][num], Y[i][num]) 50 | 51 | 52 | T = len(X[0]) 53 | 54 | paths = [] 55 | positions = [] 56 | 57 | fig = plt.figure() 58 | ax = fig.gca() 59 | ax.set_xlim([x_min,x_max]) 60 | ax.set_ylim([y_min,y_max]) 61 | for i in range(len(X)): 62 | paths.append(ax.plot([], [], ":C%i" % i,linewidth=0.5),) 63 | positions.append(ax.plot([], [], "vC%i" % i, markersize=2.0),) 64 | 65 | ani = animation.FuncAnimation(fig, update, T, fargs=(paths, positions, T), interval=1, blit=False) 66 | 67 | print("Saving animation as %s" % filename) 68 | ani.save(filename, writer=writer,dpi=100) 69 | 70 | plt.show() 71 | -------------------------------------------------------------------------------- /python/style.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | style.py: Styling options for matplotlib 6 | """ 7 | 8 | import matplotlib.pyplot as plt 9 | import os 10 | 11 | script_path = os.path.realpath(__file__) 12 | script_dir = os.path.dirname(script_path) 13 | 14 | # Primary blues 15 | blue1 = "#000082" # Blue 1 16 | blue2 = "#0050ca" # Blue 2 17 | eriblue = "#0082f0" # Ericsson Blue 18 | blue3 = "#48a8fa" # Blue 3 19 | blue4 = "#85ccff" # Blue 4 20 | # Secondary colors 21 | green = "#0fc373" # Green 22 | yellow = "#fad22d" # Yellow 23 | orange = "#ff8c0a" # Orange 24 | red = "#ff3232" # Red 25 | purple = "#af78d2" # Purple 26 | # Neutral tones 27 | black = "#000000" # Black 28 | gray1 = "#242424" # Gray 1 29 | gray2 = "#767676" # Gray 2 30 | gray3 = "#a0a0a0" # Gray 3 31 | gray4 = "#e0e0e0" # Gray 4 32 | gray5 = "#f2f2f2" # Gray 5 33 | eriwhite = "#fafafa" # Ericsson White 34 | white = "#ffffff" # White 35 | 36 | # plt.style.use("ggplot") 37 | # plt.style.use("seaborn") 38 | # plt.style.use(script_dir+"/"+"ericsson.mplstyle") 39 | plt.rcParams["font.size"] = 12 40 | plt.rcParams["font.monospace"] = "Iosevka" 41 | plt.rcParams["mathtext.fontset"] = "stix" 42 | plt.rcParams["font.family"] = "Nimbus Roman" 43 | plt.rcParams["legend.facecolor"] = "white" 44 | plt.rcParams["figure.facecolor"] = "white" 45 | plt.rcParams["savefig.facecolor"] = "None" 46 | plt.rcParams["patch.edgecolor"] = "k" 47 | plt.rcParams["svg.fonttype"] = "none" 48 | plt.rcParams["pdf.fonttype"] = 42 49 | 50 | if __name__ == "__main__": 51 | a1 = [i*1 for i in range(10)] 52 | a2 = [i*2 for i in range(10)] 53 | a3 = [i*3 for i in range(10)] 54 | a4 = [i*4 for i in range(10)] 55 | a5 = [i*5 for i in range(10)] 56 | a6 = [i*6 for i in range(10)] 57 | plt.plot(a1) 58 | plt.plot(a2) 59 | plt.plot(a3) 60 | plt.plot(a4) 61 | plt.plot(a5) 62 | plt.plot(a6) 63 | plt.show() 64 | -------------------------------------------------------------------------------- /python/plot_timing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | """ 5 | plot_timing.py: Plot timing from timing.cpp. 6 | """ 7 | 8 | import style 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | plt.figure() 13 | X = [] 14 | Y = [] 15 | 16 | OK = True 17 | i = 0 18 | while OK: 19 | i += 1 20 | try: 21 | X.append(np.fromfile("./timing_x"+str(i)+".bin", dtype=np.intc)) 22 | Y.append(np.fromfile("./timing_y"+str(i)+".bin", dtype=np.double)) 23 | except: 24 | OK = False 25 | 26 | for i in range(0,len(X)): 27 | plt.loglog(X[i],Y[i],"v--", label="$N_\\mathrm{agents}=%i$" % (i+1)) 28 | 29 | plt.xlabel("$N_\\mathrm{steps}$") 30 | plt.ylabel("Time [ms]") 31 | plt.legend() 32 | 33 | plt.savefig("Nsteps.pdf") 34 | 35 | plt.figure() 36 | X2 = [] 37 | Y2 = [] 38 | 39 | OK = True 40 | i = 1 41 | while OK: 42 | i *= 10 43 | try: 44 | X2.append(np.fromfile("./timing_agents_x"+str(i)+".bin", dtype=np.intc)) 45 | Y2.append(np.fromfile("./timing_agents_y"+str(i)+".bin", dtype=np.double)) 46 | except: 47 | OK = False 48 | 49 | for i in range(0,len(X2)): 50 | plt.loglog(X2[i],Y2[i],"v--", label="$N_\\mathrm{steps}=%.0e$" % 10**(i+1)) 51 | 52 | plt.xlabel("$N_\\mathrm{agents}$") 53 | plt.ylabel("Time [ms]") 54 | plt.legend() 55 | 56 | plt.savefig("Nagents.pdf") 57 | 58 | plt.figure() 59 | X3 = [] 60 | Y3 = [] 61 | 62 | OK = True 63 | i = 1 64 | while OK: 65 | i *= 10 66 | try: 67 | X3.append(np.fromfile("./timing_radius_x"+str(i)+".bin", dtype=np.intc)) 68 | Y3.append(np.fromfile("./timing_radius_y"+str(i)+".bin", dtype=np.double)) 69 | except: 70 | OK = False 71 | 72 | for i in range(0,len(X3)): 73 | plt.loglog(X3[i],Y3[i],"v--", label="$N_\\mathrm{steps}=%.0e$" % 10**(i+1)) 74 | 75 | plt.xlabel("Radii (inter/intra-agent)") 76 | plt.ylabel("Time [ms]") 77 | plt.legend() 78 | 79 | plt.savefig("Radius.pdf") 80 | 81 | plt.show() 82 | -------------------------------------------------------------------------------- /include/kollagen/WalkerParams.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef WALKERPARAMS_H 4 | #define WALKERPARAMS_H 5 | 6 | namespace kollagen 7 | { 8 | 9 | struct WalkerParams { 10 | WalkerParams() = default; 11 | WalkerParams(double x, double y, double theta, double stepsize, 12 | double sigma_pos, double sigma_ang, int step_fragment, int seed, 13 | int ID, double I11est, double I22est, double I33est, 14 | bool allow_retrocession = true) 15 | : x(x), y(y), theta(theta), stepsize(stepsize), sigma_pos(sigma_pos), 16 | sigma_ang(sigma_ang), step_fragment(step_fragment), seed(seed), ID(ID), 17 | I11est(I11est), I22est(I22est), I33est(I33est), 18 | allow_retrocession(allow_retrocession){}; 19 | /// Initial x-position [length unit] 20 | double x{0}; 21 | /// Initial y-position [length unit] 22 | double y{0}; 23 | /// Initial heading angle [radians] 24 | double theta{0}; 25 | double stepsize{1.0}; 26 | double sigma_pos{0.01}; 27 | double sigma_ang{0.001}; 28 | int step_fragment{4}; 29 | int seed{1234}; 30 | /// ID (unitless) 31 | int ID{1}; 32 | double I11est{std::pow(sigma_pos, -2)}; 33 | double I22est{std::pow(sigma_pos, -2)}; 34 | double I33est{std::pow(sigma_ang, -2)}; 35 | bool allow_retrocession{true}; 36 | 37 | inline bool operator==(const WalkerParams& other) const{ 38 | return x == other.x && 39 | y == other.y && 40 | theta == other.theta && 41 | stepsize == other.stepsize && 42 | sigma_pos == other.sigma_pos && 43 | sigma_ang == other.sigma_ang && 44 | step_fragment == other.step_fragment && 45 | seed == other.seed && 46 | ID == other.ID && 47 | I11est == other.I11est && 48 | I22est == other.I22est && 49 | I33est == other.I33est && 50 | allow_retrocession == other.allow_retrocession; 51 | } 52 | }; 53 | 54 | } // namespace kollagen 55 | #endif /* !WALKERPARAMS_H */ 56 | -------------------------------------------------------------------------------- /tests/test_data/test.json: -------------------------------------------------------------------------------- 1 | { 2 | "number_of_steps": 1000, 3 | "align": true, 4 | "number_of_agents": 2, 5 | "exact_information_matrix": false, 6 | "agent_parameters": { 7 | "stepsize": 1.5, 8 | "step_fragment": 2, 9 | "allow_retrocession": true, 10 | "agent_specific": [{ 11 | "std_dev_position": 1, 12 | "std_dev_angle": 2, 13 | "estimated_information_position": 3, 14 | "estimated_information_angle": 4, 15 | "random_number_generator_seed": 5, 16 | "std_dev_intra_loop_closure_position": 6, 17 | "std_dev_intra_loop_closure_angle": 7, 18 | "estimated_information_intra_position": 8, 19 | "estimated_information_intra_angle": 9, 20 | "intra_loop_closure_radius": 10, 21 | "same_position_intra_loop_closure_probability": 11, 22 | "intra_loop_closure_probability_scaling": 12 23 | }, 24 | { 25 | "std_dev_position": 13, 26 | "std_dev_angle": 14, 27 | "estimated_information_position": 15, 28 | "estimated_information_angle": 16, 29 | "random_number_generator_seed": 17, 30 | "std_dev_intra_loop_closure_position": 18, 31 | "std_dev_intra_loop_closure_angle": 19, 32 | "estimated_information_intra_position": 20, 33 | "estimated_information_intra_angle": 21, 34 | "intra_loop_closure_radius": 22, 35 | "same_position_intra_loop_closure_probability": 23, 36 | "intra_loop_closure_probability_scaling": 24 37 | } 38 | ]}, 39 | "inter_agent_loop_closure_parameters": { 40 | "std_dev_loop_closure_position": 25, 41 | "std_dev_loop_closure_angle": 26, 42 | "estimated_information_position": 27, 43 | "estimated_information_angle": 28, 44 | "loop_closure_radius": 29, 45 | "loop_closure_probability_scaling": 30, 46 | "same_position_loop_closure_probability": 31, 47 | "random_number_generator_seed": 32 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /include/kollagen/AgentParams.h: -------------------------------------------------------------------------------- 1 | #include "WalkerParams.h" 2 | 3 | #ifndef AGENTPARAMS_H 4 | #define AGENTPARAMS_H 5 | 6 | namespace kollagen 7 | { 8 | 9 | /** 10 | * \brief Agent Parameters. 11 | * The agent parameters controls the behavior of the agents. 12 | */ 13 | struct AgentParams : public WalkerParams { 14 | AgentParams() = default; 15 | /** 16 | * \brief Constructor. 17 | * 18 | * \param stepsize Step size [length unit]. 19 | * 20 | * \param step_fragment Divides the steps in "sub"-steps (unitless). The 21 | * number of steps taken before (possibly) turning. By setting this to a 22 | * factor greater than 1, the actual number of steps are increased by this 23 | * factor, and the actual step size is decreased by the same factor. 24 | * 25 | * \param sigma_pos Standard deviation [length unit]. Standard deviation of 26 | * the noise added to the \f$x\f$ and \f$y\f$ components of the odometry. 27 | * 28 | * \param sigma_ang Standard deviation [radians] Standard deviation of the 29 | * noise added to the angular component of the odometry. 30 | * 31 | * \param estimated_information_pos Information matrix entries for position 32 | * [1/(length unit)^2]. Estimated Fisher information matrix entry for 33 | * \f$x\f$ and \f$y\f$ (first and second diagonal entries). 34 | * 35 | * \param estimated_information_ang Information matrix entry for heading 36 | * angle [1/radians^2]. Estimated Fisher information matrix entry for \f$\theta\f$ 37 | * (third diagonal entry). 38 | * 39 | * \param seed Random Number Generator seed (unitless). Seed used for the 40 | * random walk, and the intra-agent loop closure. 41 | * 42 | * \param allow_retrocession If set to true, the agent is able to turn 180 43 | * degrees, and thus retrocede. 44 | */ 45 | AgentParams(double stepsize, int step_fragment, double sigma_pos, 46 | double sigma_ang, double estimated_information_pos, 47 | double estimated_information_ang, int seed, 48 | bool allow_retrocession = true) 49 | : WalkerParams(0, 0, 0, stepsize, sigma_pos, sigma_ang, step_fragment, 50 | seed, 0, estimated_information_pos, 51 | estimated_information_pos, estimated_information_ang, 52 | allow_retrocession){}; 53 | }; 54 | 55 | } // namespace kollagen 56 | #endif /* !AGENTPARAMS_H */ 57 | -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(/usr/src/googletest 2 | ${PROJECT_BINARY_DIR}/gtest 3 | ) 4 | 5 | set(TEST_BINARY ${PROJECT_NAME}_test) 6 | add_executable(${TEST_BINARY} testWalker.cpp testDataGenerator.cpp testJSONParse.cpp testMultiG2oParse.cpp) 7 | 8 | target_link_libraries(${TEST_BINARY} gtest gtest_main gmock gmock_main) 9 | 10 | set(TEST_DATA_DIR test_data) 11 | configure_file(test_data/walker1.g2o ${TEST_DATA_DIR}/walker1.g2o COPYONLY) 12 | configure_file(test_data/walker2.g2o ${TEST_DATA_DIR}/walker2.g2o COPYONLY) 13 | configure_file(test_data/walker1_3D.g2o ${TEST_DATA_DIR}/walker1_3D.g2o COPYONLY) 14 | configure_file(test_data/walker2_3D.g2o ${TEST_DATA_DIR}/walker2_3D.g2o COPYONLY) 15 | configure_file(test_data/walker1.tum ${TEST_DATA_DIR}/walker1.tum COPYONLY) 16 | configure_file(test_data/walker2.tum ${TEST_DATA_DIR}/walker2.tum COPYONLY) 17 | configure_file(test_data/test.json ${TEST_DATA_DIR}/test.json COPYONLY) 18 | configure_file(test_data/test2.json ${TEST_DATA_DIR}/test2.json COPYONLY) 19 | 20 | # 2D MultiG2O 21 | configure_file(test_data/multig2o_test/agent1/agent1_GT.tum ${TEST_DATA_DIR}/multig2o_test/agent1/agent1_GT.tum COPYONLY) 22 | configure_file(test_data/multig2o_test/agent1/posegraph.g2o ${TEST_DATA_DIR}/multig2o_test/agent1/posegraph.g2o COPYONLY) 23 | configure_file(test_data/multig2o_test/agent2/agent2_GT.tum ${TEST_DATA_DIR}/multig2o_test/agent2/agent2_GT.tum COPYONLY) 24 | configure_file(test_data/multig2o_test/agent2/posegraph.g2o ${TEST_DATA_DIR}/multig2o_test/agent2/posegraph.g2o COPYONLY) 25 | configure_file(test_data/multig2o_test/inter_agent_lc.dat ${TEST_DATA_DIR}/multig2o_test/inter_agent_lc.dat COPYONLY) 26 | # 3D MultiG2O 27 | configure_file(test_data/multig2o_test_3D/agent1/agent1_GT.tum ${TEST_DATA_DIR}/multig2o_test_3D/agent1/agent1_GT.tum COPYONLY) 28 | configure_file(test_data/multig2o_test_3D/agent1/posegraph.g2o ${TEST_DATA_DIR}/multig2o_test_3D/agent1/posegraph.g2o COPYONLY) 29 | configure_file(test_data/multig2o_test_3D/agent2/agent2_GT.tum ${TEST_DATA_DIR}/multig2o_test_3D/agent2/agent2_GT.tum COPYONLY) 30 | configure_file(test_data/multig2o_test_3D/agent2/posegraph.g2o ${TEST_DATA_DIR}/multig2o_test_3D/agent2/posegraph.g2o COPYONLY) 31 | configure_file(test_data/multig2o_test_3D/inter_agent_lc.dat ${TEST_DATA_DIR}/multig2o_test_3D/inter_agent_lc.dat COPYONLY) 32 | 33 | add_test( 34 | NAME ${TEST_BINARY} 35 | COMMAND ${EXECUTABLE_OUTPUT_PATH}/${TEST_BINARY} 36 | ) 37 | -------------------------------------------------------------------------------- /include/kollagen/utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_HPP 2 | #define UTILS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace kollagen 13 | { 14 | 15 | inline double normalize_angle(double angle) { 16 | return angle - (std::floor((angle + M_PI) / (2 * M_PI))) * 2 * M_PI; 17 | } 18 | 19 | inline int round_to_int(double expression) { 20 | return static_cast(round(expression)); 21 | } 22 | 23 | // Save a vector of type T to binary file 24 | template 25 | inline int save_binary(std::string filename, std::vector data) 26 | { 27 | std::ofstream file(filename, std::ios_base::out | std::ios_base::binary); 28 | if (!file) { return EXIT_FAILURE; } 29 | file.write(reinterpret_cast (&data.front()), data.size() * sizeof(data.front())); 30 | return EXIT_SUCCESS; 31 | } 32 | 33 | // Load a binary file made from vector of type T 34 | template 35 | inline int load_binary(std::string filename, std::vector& data) 36 | { 37 | std::ifstream load_file(filename, std::ios_base::in | std::ios_base::binary | std::ios_base::ate); 38 | if (!load_file) { return EXIT_FAILURE; } 39 | auto filesize = load_file.tellg(); 40 | auto typesize = sizeof(data.front()); 41 | auto n_elements = filesize / typesize; 42 | data.resize(n_elements); 43 | std::fill_n(data.begin(), filesize / typesize, 0.0); 44 | std::ifstream file(filename, std::ios_base::in | std::ios_base::binary); 45 | file.read(reinterpret_cast (&data.front()), filesize); 46 | return EXIT_SUCCESS; 47 | } 48 | 49 | template 50 | inline std::vector load_binary(std::string filename) 51 | { 52 | std::vector data; 53 | load_binary(filename, data); 54 | return data; 55 | } 56 | 57 | template 58 | std::string string_format(const std::string& format, Args ... args ) 59 | { 60 | int size_s = std::snprintf( nullptr, 0, format.c_str(), args ... ) + 1; // Extra space for '\0' 61 | if( size_s <= 0 ){ throw std::runtime_error( "Error during formatting." ); } 62 | auto size = static_cast( size_s ); 63 | auto buf = std::make_unique( size ); 64 | std::snprintf( buf.get(), size, format.c_str(), args ... ); 65 | return std::string( buf.get(), buf.get() + size - 1 ); // We don't want the '\0' inside 66 | } 67 | 68 | } // namespace kollagen 69 | #endif /* !UTILS_HPP */ 70 | -------------------------------------------------------------------------------- /examples/example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "kollagen.h" 6 | 7 | int main() { 8 | 9 | int N_steps = 1200; 10 | double radius = 3.0; 11 | double idemLCprob = 0.5; 12 | double prob_scale = 0.05; 13 | 14 | int seed1 = 1406; 15 | int seed2 = 1302; 16 | 17 | double sigma_pos = 0.023; 18 | double sigma_ang = sigma_pos / 10; 19 | double Idiag = std::pow(sigma_pos, -2); 20 | 21 | double sigma_pos_single = 0.023; 22 | double sigma_ang_single = sigma_pos / 10; 23 | double Idiag_single = std::pow(sigma_pos_single, -2); 24 | 25 | double sigma_pos_inter = 0.023; 26 | double sigma_ang_inter = sigma_pos_inter / 10; 27 | double Idiag_inter = std::pow(sigma_pos_inter, -2); 28 | 29 | auto lc_param = 30 | kollagen::LCParams(sigma_pos_single, sigma_ang_single, Idiag_single, 31 | Idiag_single * 100, radius, idemLCprob, prob_scale); 32 | 33 | auto agent_param = 34 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 0); 35 | auto agent2_param = 36 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 1); 37 | auto agent3_param = 38 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 2); 39 | auto agent4_param = 40 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 3); 41 | auto agent5_param = 42 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 4); 43 | 44 | auto inter_lc_param = 45 | kollagen::InterLCParams(sigma_pos_inter, sigma_ang_inter, Idiag_inter, 46 | Idiag_inter * 100, radius, 1.0, prob_scale, seed1 + seed2); 47 | 48 | kollagen::DataGenerator data_gen = 49 | kollagen::DataGenerator({{agent_param, agent2_param, agent3_param, agent4_param, 50 | agent5_param}, 51 | lc_param, 52 | inter_lc_param, 53 | N_steps}); 54 | 55 | data_gen.generate(); 56 | 57 | data_gen.save_multig2o("Example4800"); 58 | data_gen.save_singleg2o("Example4800.g2o", false); 59 | data_gen.save_singleTUM("Example4800_GT.tum"); 60 | data_gen.save_singleTUM("Example4800.tum", false); 61 | 62 | std::cout << data_gen.LCs().size() << " inter agent LCs!" << std::endl; 63 | 64 | std::cout << "Saving files x.bin and y.bin... "; 65 | for (size_t i = 0; i < data_gen.Walkers().size(); ++i) { 66 | std::string label = std::to_string(i + 1); 67 | kollagen::save_binary("x" + label + ".bin", data_gen.Walkers().at(i).X()); 68 | kollagen::save_binary("y" + label + ".bin", data_gen.Walkers().at(i).Y()); 69 | } 70 | std::cout << "Done!" << std::endl; 71 | 72 | return EXIT_SUCCESS; 73 | } 74 | -------------------------------------------------------------------------------- /include/kollagen/LCParams.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef LCPARAMS_H 4 | #define LCPARAMS_H 5 | 6 | namespace kollagen 7 | { 8 | 9 | /** 10 | * \brief Loop Closure Parameters. 11 | * For intra-agent loop closures. 12 | */ 13 | struct LCParams { 14 | LCParams() = default; 15 | LCParams(double sigma_pos, double sigma_ang, double estimated_information_pos, double estimated_information_ang, 16 | double radius, double idemLCprob, 17 | double prob_scale) 18 | : sigma_pos(sigma_pos), sigma_ang(sigma_ang), 19 | I11est(estimated_information_pos > 0 ? estimated_information_pos : std::pow(sigma_pos, -2)), 20 | I22est(estimated_information_pos > 0 ? estimated_information_pos : std::pow(sigma_pos, -2)), 21 | I33est(estimated_information_ang > 0 ? estimated_information_ang : std::pow(sigma_pos, -2)), radius(radius), 22 | idemLCprob(idemLCprob), prob_scale(prob_scale){}; 23 | /** 24 | * \brief Standard deviation [length unit] 25 | * 26 | * Standard deviation of the noise added to the x and y components of the loop closure. 27 | */ 28 | double sigma_pos{0.01}; 29 | /** 30 | * \brief Standard deviation [radians] 31 | * 32 | * Standard deviation of the noise added to the angular component of the loop closure. 33 | */ 34 | double sigma_ang{0.001}; 35 | /** 36 | * \brief Information matrix entry I11 [1/(length unit)^2] 37 | * 38 | * Estimated Fisher information matrix entry for x (first diagonal entry). 39 | */ 40 | double I11est{std::pow(sigma_pos, -2)}; 41 | /** 42 | * \brief Information matrix entry I22 [1/(length unit)^2] 43 | * 44 | * Estimated Fisher information matrix entry for y (second diagonal entry). 45 | */ 46 | double I22est{std::pow(sigma_pos, -2)}; 47 | /** 48 | * \brief Information matrix entry I33 [1/radians^2] 49 | * 50 | * Estimated Fisher information matrix entry for theta (third diagonal entry). 51 | */ 52 | double I33est{std::pow(sigma_ang, -2)}; 53 | /** 54 | * \brief Maximum loop closure distance [length unit] 55 | * 56 | * This sets the size of the circle used to check for loop closure 57 | * candidates, which means it can be considered as an upper bound on the 58 | * distance for the loop closures. 59 | */ 60 | double radius{3.0}; 61 | /** 62 | * \brief Idem loop closure probability (unitless) 63 | * 64 | * This sets the probability of loop closure when revisiting a previously 65 | * visited position. 66 | */ 67 | double idemLCprob{0.5}; 68 | /** 69 | * \brief Loop closure probability scaling (unitless) 70 | * 71 | * This scales the probability of loop closure. Scaling is done through 72 | * multiplication, and thus setting to 1.0 means not scaling. 73 | */ 74 | double prob_scale{0.05}; 75 | }; 76 | 77 | } // namespace kollagen 78 | #endif /* !LCPARAMS_H */ 79 | -------------------------------------------------------------------------------- /tests/test_data/walker1_3D.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT 0 0 0 0.0 0.0 0.0 0 1 2 | VERTEX_SE3:QUAT 1 1 0 0.0 0.0 0.0 0 1 3 | VERTEX_SE3:QUAT 2 1 -1 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 4 | VERTEX_SE3:QUAT 3 2 -1 0.0 0.0 0.0 0 1 5 | VERTEX_SE3:QUAT 4 2 0 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 6 | VERTEX_SE3:QUAT 5 1 -1.2246467991473532e-16 0.0 0.0 0.0 -1 6.123233995736766e-17 7 | VERTEX_SE3:QUAT 6 1 0.99999999999999989 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 8 | VERTEX_SE3:QUAT 7 1 2 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 9 | VERTEX_SE3:QUAT 8 1 3 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 10 | EDGE_SE3:QUAT 0 1 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 11 | EDGE_SE3:QUAT 1 2 6.123233995736766e-17 -1 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 12 | EDGE_SE3:QUAT 2 3 6.123233995736766e-17 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 13 | EDGE_SE3:QUAT 3 4 6.123233995736766e-17 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 14 | EDGE_SE3:QUAT 4 5 6.123233995736766e-17 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 15 | EDGE_SE3:QUAT 5 6 6.123233995736766e-17 -1 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 16 | EDGE_SE3:QUAT 6 7 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 17 | EDGE_SE3:QUAT 7 8 1 0 0.0 0.0 0.0 0 1 1 0.0 0.0 0.0 0.0 0.0 1 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1 0.0 0.0 1 0.0 1 18 | EDGE_SE3:QUAT 0 2 1 -1 0.0 0.0 0.0 -0.70710678118654746 0.70710678118654757 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 19 | EDGE_SE3:QUAT 1 3 1 -1 0.0 0.0 0.0 0 1 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 20 | EDGE_SE3:QUAT 2 4 -0.99999999999999989 1 0.0 0.0 0.0 -1 6.123233995736766e-17 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 21 | EDGE_SE3:QUAT 1 5 0 0 0.0 0.0 0.0 -1 6.123233995736766e-17 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 22 | EDGE_SE3:QUAT 3 5 -1 1 0.0 0.0 0.0 -1 6.123233995736766e-17 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 23 | EDGE_SE3:QUAT 0 6 1 1 0.0 0.0 0.0 0.70710678118654746 0.70710678118654757 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 24 | EDGE_SE3:QUAT 4 6 0.99999999999999989 1 0.0 0.0 0.0 0 1 10000 0.0 0.0 0.0 0.0 0.0 10000 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 1000000 0.0 0.0 1000000 0.0 1000000 25 | -------------------------------------------------------------------------------- /include/kollagen/DataGeneratorParams.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "AgentParams.h" 6 | #include "InterLCParams.h" 7 | #include "LCParams.h" 8 | 9 | #ifndef DATAGENERATORPARAMS_H 10 | #define DATAGENERATORPARAMS_H 11 | 12 | namespace kollagen { 13 | 14 | /** 15 | * \brief Data Generator Parameters. 16 | * 17 | */ 18 | struct DataGeneratorParams { 19 | DataGeneratorParams() 20 | : agent_params({AgentParams()}), lc_params({LCParams()}){}; 21 | DataGeneratorParams(AgentParams Wp, LCParams LCp, InterLCParams ILCp, 22 | bool exact_information_matrix = false) 23 | : agent_params({Wp}), lc_params({LCp}), inter_lc_params(ILCp), 24 | exact_information_matrix(exact_information_matrix){}; 25 | 26 | DataGeneratorParams(std::vector Wp, std::vector LCp, 27 | InterLCParams ILCp, int N_steps, 28 | bool exact_information_matrix = false) 29 | : agent_params(std::move(Wp)), lc_params(std::move(LCp)), 30 | inter_lc_params(ILCp), N_agents(static_cast(agent_params.size())), 31 | N_steps(N_steps), exact_information_matrix(exact_information_matrix) { 32 | 33 | assert(agent_params.size() == N_agents); 34 | assert(lc_params.size() == N_agents); 35 | }; 36 | 37 | DataGeneratorParams(std::vector Wp, LCParams LCp, 38 | InterLCParams ILCp, int N_steps, bool 39 | exact_information_matrix = false) 40 | : agent_params(std::move(Wp)), inter_lc_params(ILCp), 41 | N_agents(static_cast(agent_params.size())), N_steps(N_steps), 42 | exact_information_matrix(exact_information_matrix) { 43 | lc_params.resize(N_agents); 44 | std::fill_n(lc_params.begin(), N_agents, LCp); 45 | assert(agent_params.size() == N_agents); 46 | assert(lc_params.size() == N_agents); 47 | }; 48 | 49 | DataGeneratorParams(AgentParams Wp, std::vector LCp, 50 | InterLCParams ILCp, int N_steps, 51 | bool exact_information_matrix = false) 52 | : lc_params(std::move(LCp)), inter_lc_params(ILCp), 53 | N_agents(static_cast(agent_params.size())), N_steps(N_steps), 54 | exact_information_matrix(exact_information_matrix) { 55 | agent_params.resize(N_agents); 56 | std::fill_n(agent_params.begin(), N_agents, Wp); 57 | assert(agent_params.size() == N_agents); 58 | assert(lc_params.size() == N_agents); 59 | }; 60 | 61 | /** 62 | * \brief Agent parameters 63 | */ 64 | std::vector agent_params{}; 65 | /** 66 | * \brief Intra-agent loop-closure parameters. 67 | */ 68 | std::vector lc_params{}; 69 | /** 70 | * \brief Inter-agent loop-closure parameters. 71 | */ 72 | InterLCParams inter_lc_params{}; 73 | /** 74 | * \brief Number of agents 75 | */ 76 | int N_agents{1}; 77 | /** 78 | * \brief Number of steps for each agent 79 | */ 80 | int N_steps{100}; 81 | /** 82 | * \brief Align the agents' centers of mass 83 | */ 84 | bool align_center_of_masses{true}; 85 | /** 86 | * \brief Use exact information matrix 87 | */ 88 | bool exact_information_matrix{false}; 89 | }; 90 | 91 | } // namespace kollagen 92 | #endif /* !DATAGENERATORPARAMS_H */ 93 | -------------------------------------------------------------------------------- /tests/DataGeneratorAttorney.h: -------------------------------------------------------------------------------- 1 | #include "kollagen/DataGenerator.h" 2 | 3 | #ifndef DATAGENERATORATTORNEY_H 4 | #define DATAGENERATORATTORNEY_H 5 | 6 | namespace kollagen 7 | { 8 | 9 | class DataGeneratorAttorney 10 | { 11 | public: 12 | static void step_forward(DataGenerator& a){ 13 | a.step_forward(); 14 | } 15 | static void step_right(DataGenerator& a){ 16 | a.step_right(); 17 | } 18 | static void step_left(DataGenerator& a){ 19 | a.step_left(); 20 | } 21 | static void align_center_of_masses(DataGenerator& a){ 22 | a.align_center_of_masses(); 23 | } 24 | static std::vector get_walker_seeds(DataGenerator& a){ 25 | return a.get_walker_seeds(); 26 | } 27 | static int get_max_seed(DataGenerator& a){ 28 | return a.get_max_seed(); 29 | } 30 | static bool seed_already_used(DataGenerator& a, int seed){ 31 | return a.seed_already_used(seed); 32 | } 33 | static bool different_stepsize(DataGenerator& a, double stepsize){ 34 | return a.different_stepsize(stepsize); 35 | } 36 | static bool different_step_fragment(DataGenerator& a, int step_fragment){ 37 | return a.different_step_fragment(step_fragment); 38 | } 39 | static void step(DataGenerator& a, int number_of_steps){ 40 | a.step(number_of_steps); 41 | } 42 | static void step(DataGenerator& a){ 43 | a.step(); 44 | } 45 | static void intra_agent_loopclose(DataGenerator& a, const LCParams& params){ 46 | a.intra_agent_loopclose(params); 47 | } 48 | static void intra_agent_loopclose(DataGenerator& a, const std::vector& params){ 49 | a.intra_agent_loopclose(params); 50 | } 51 | static void inter_agent_loopclose(DataGenerator& a, const InterLCParams& params){ 52 | a.inter_agent_loopclose(params); 53 | } 54 | static std::vector> get_all_pairs(DataGenerator& a) { 55 | return a.get_all_pairs(); 56 | } 57 | 58 | static void add_walker(DataGenerator& a){ 59 | a.add_walker(); 60 | } 61 | static void add_walker(DataGenerator& a, WalkerParams params){ 62 | a.add_walker(params); 63 | } 64 | static void add_walker(DataGenerator& a, const std::vector& params){ 65 | a.add_walker(params); 66 | } 67 | static void set_data_has_been_generated(DataGenerator& a, bool value){ 68 | a.data_has_been_generated_ = value; 69 | }; 70 | static void set_paramaters_have_been_set(DataGenerator& a, bool value){ 71 | a.parameters_have_been_set_ = value; 72 | }; 73 | 74 | static DataGenerator ConstructDataGenerator(int number_of_walkers); 75 | static DataGenerator ConstructDataGenerator(const std::vector& params); 76 | static DataGenerator ConstructDataGenerator(int number_of_walkers, int number_of_steps); 77 | 78 | }; 79 | 80 | inline DataGenerator DataGeneratorAttorney::ConstructDataGenerator(int number_of_walkers){ 81 | DataGenerator data_gen(number_of_walkers); 82 | return data_gen; 83 | } 84 | 85 | inline DataGenerator DataGeneratorAttorney::ConstructDataGenerator(const std::vector& params){ 86 | DataGenerator data_gen(params); 87 | return data_gen; 88 | } 89 | 90 | inline DataGenerator DataGeneratorAttorney::ConstructDataGenerator(int number_of_walkers, int number_of_steps){ 91 | DataGenerator data_gen(number_of_walkers, number_of_steps); 92 | return data_gen; 93 | } 94 | 95 | } // namespace kollagen 96 | #endif /* !DATAGENERATORATTORNEY_H */ 97 | -------------------------------------------------------------------------------- /include/kollagen/generate.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "kollagen.h" 6 | 7 | namespace kollagen 8 | { 9 | 10 | void print_help(); 11 | 12 | std::tuple 13 | get_CLI_arguments(int argc, char *argv[]); 14 | 15 | void add_extension(std::filesystem::path json_file); 16 | 17 | bool CLI_arguments_OK(const std::filesystem::path &json_file, 18 | const std::filesystem::path &save_dir, 19 | const bool &multi_g2o, 20 | const bool &single_g2o 21 | ); 22 | 23 | void generate_and_save(const std::filesystem::path &json_file, 24 | const std::filesystem::path &save_dir, const bool &multig2o, 25 | const bool &singleg2o); 26 | 27 | inline void print_help() { 28 | std::cout << "NAME\n" 29 | " generate - generate data from json file\n\n" 30 | "SYNOPSIS\n" 31 | " generate SOURCE [DEST]\n\n" 32 | "DESCRIPTION\n" 33 | " Generate data from json file SOURCE. Save in folder " 34 | "DEST, defaulting to './output'.\n"; 35 | } 36 | 37 | inline void add_extension(std::filesystem::path& file, const std::string& extension) { 38 | if (file.extension() != extension) { 39 | file.replace_extension(extension); 40 | } 41 | } 42 | 43 | inline std::tuple 44 | get_CLI_arguments(int argc, char *argv[]) { 45 | std::filesystem::path json_file{}; 46 | std::filesystem::path save_dir{}; 47 | 48 | if (argc == 1) { 49 | print_help(); 50 | return {"", ""}; 51 | } 52 | 53 | if (argc > 1) { 54 | json_file = argv[1]; 55 | save_dir = "./output"; 56 | if (json_file == "-h" || json_file == "--help") { 57 | print_help(); 58 | return {"", ""}; 59 | } 60 | add_extension(json_file, "json"); 61 | } 62 | 63 | if (argc > 2) { 64 | save_dir = argv[2]; 65 | } 66 | 67 | return {json_file, save_dir}; 68 | } 69 | 70 | inline bool CLI_arguments_OK(const std::filesystem::path &json_file, 71 | const std::filesystem::path &save_dir, 72 | const bool& multig2o, 73 | const bool& singleg2o 74 | ) { 75 | bool no_files_to_save = ! (multig2o || singleg2o); 76 | bool both_paths_empty = json_file.empty() && save_dir.empty(); 77 | bool no_such_json_file = !std::filesystem::exists(json_file); 78 | 79 | if (no_files_to_save) { 80 | std::cout << "[Error]: No files to save. Exiting." << '\n'; 81 | return false; 82 | } 83 | if (both_paths_empty) { 84 | std::cout << "[Error]: No paths given." << '\n'; 85 | return false; 86 | } 87 | if (no_such_json_file) { 88 | std::cout << "[Error] " + json_file.string() + ": No such file." << '\n'; 89 | return false; 90 | } 91 | return true; 92 | }; 93 | 94 | inline void generate_and_save(const std::filesystem::path &json_file, 95 | const std::filesystem::path &save_dir, const bool &multig2o, 96 | const bool &singleg2o) { 97 | auto params = get_params_from_json(json_file.string()); 98 | 99 | auto data_gen = DataGenerator(params); 100 | data_gen.generate(); 101 | 102 | if (multig2o) { 103 | data_gen.save_multig2o((save_dir / json_file.stem()).string()); 104 | } 105 | 106 | if (singleg2o) { 107 | auto singleg2o_save_dir = save_dir / (json_file.stem().string() + "_singleg2o"); 108 | data_gen.save_singleg2o((singleg2o_save_dir / json_file.stem()).string() + ".g2o", 109 | false); 110 | data_gen.save_singleTUM((singleg2o_save_dir / json_file.stem()).string() + "_GT.tum"); 111 | data_gen.save_singleTUM((singleg2o_save_dir / json_file.stem()).string() + ".tum", 112 | false); 113 | } 114 | } 115 | } // namespace kollagen 116 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Byte-compiled / optimized / DLL files 35 | __pycache__/ 36 | *.py[cod] 37 | *$py.class 38 | 39 | # C extensions 40 | *.so 41 | 42 | # Distribution / packaging 43 | .Python 44 | build/ 45 | develop-eggs/ 46 | dist/ 47 | downloads/ 48 | eggs/ 49 | .eggs/ 50 | lib/ 51 | lib64/ 52 | parts/ 53 | sdist/ 54 | var/ 55 | wheels/ 56 | share/python-wheels/ 57 | *.egg-info/ 58 | .installed.cfg 59 | *.egg 60 | MANIFEST 61 | 62 | # PyInstaller 63 | # Usually these files are written by a python script from a template 64 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 65 | *.manifest 66 | *.spec 67 | 68 | # Installer logs 69 | pip-log.txt 70 | pip-delete-this-directory.txt 71 | 72 | # Unit test / coverage reports 73 | htmlcov/ 74 | .tox/ 75 | .nox/ 76 | .coverage 77 | .coverage.* 78 | .cache 79 | nosetests.xml 80 | coverage.xml 81 | *.cover 82 | *.py,cover 83 | .hypothesis/ 84 | .pytest_cache/ 85 | cover/ 86 | 87 | # Translations 88 | *.mo 89 | *.pot 90 | 91 | # Django stuff: 92 | *.log 93 | local_settings.py 94 | db.sqlite3 95 | db.sqlite3-journal 96 | 97 | # Flask stuff: 98 | instance/ 99 | .webassets-cache 100 | 101 | # Scrapy stuff: 102 | .scrapy 103 | 104 | # Sphinx documentation 105 | docs/_build/ 106 | 107 | # PyBuilder 108 | .pybuilder/ 109 | target/ 110 | 111 | # Jupyter Notebook 112 | .ipynb_checkpoints 113 | 114 | # IPython 115 | profile_default/ 116 | ipython_config.py 117 | 118 | # pyenv 119 | # For a library or package, you might want to ignore these files since the code is 120 | # intended to run in multiple environments; otherwise, check them in: 121 | # .python-version 122 | 123 | # pipenv 124 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 125 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 126 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 127 | # install all needed dependencies. 128 | #Pipfile.lock 129 | 130 | # poetry 131 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 132 | # This is especially recommended for binary packages to ensure reproducibility, and is more 133 | # commonly ignored for libraries. 134 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 135 | #poetry.lock 136 | 137 | # pdm 138 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 139 | #pdm.lock 140 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 141 | # in version control. 142 | # https://pdm.fming.dev/#use-with-ide 143 | .pdm.toml 144 | 145 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 146 | __pypackages__/ 147 | 148 | # Celery stuff 149 | celerybeat-schedule 150 | celerybeat.pid 151 | 152 | # SageMath parsed files 153 | *.sage.py 154 | 155 | # Environments 156 | .env 157 | .venv 158 | env/ 159 | venv/ 160 | ENV/ 161 | env.bak/ 162 | venv.bak/ 163 | 164 | # Spyder project settings 165 | .spyderproject 166 | .spyproject 167 | 168 | # Rope project settings 169 | .ropeproject 170 | 171 | # mkdocs documentation 172 | /site 173 | 174 | # mypy 175 | .mypy_cache/ 176 | .dmypy.json 177 | dmypy.json 178 | 179 | # Pyre type checker 180 | .pyre/ 181 | 182 | # pytype static type analyzer 183 | .pytype/ 184 | 185 | # Cython debug symbols 186 | cython_debug/ 187 | 188 | # PyCharm 189 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 190 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 191 | # and can be added to the global gitignore or merged into this file. For a more nuclear 192 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 193 | #.idea/ 194 | 195 | compile_commands.json 196 | /generate 197 | 198 | *.bin 199 | 200 | .vscode 201 | -------------------------------------------------------------------------------- /data/Multi10000x5.json: -------------------------------------------------------------------------------- 1 | { 2 | "number_of_steps": 2500, 3 | "align": true, 4 | "number_of_agents": 5, 5 | "exact_information_matrix": false, 6 | "agent_parameters": { 7 | "stepsize": 1.0, 8 | "step_fragment": 4, 9 | "allow_retrocession": false, 10 | "agent_specific": [{ 11 | "std_dev_position": 0.023, 12 | "std_dev_angle": 0.0115, 13 | "estimated_information_position": 1890.359168241966, 14 | "estimated_information_angle": 7561.436672967864, 15 | "random_number_generator_seed": 1406, 16 | "std_dev_intra_loop_closure_position": 0.023, 17 | "std_dev_intra_loop_closure_angle": 0.0115, 18 | "estimated_information_intra_position": 1890.359168241966, 19 | "estimated_information_intra_angle": 7561.436672967864, 20 | "intra_loop_closure_radius": 3.0, 21 | "same_position_intra_loop_closure_probability": 0.5, 22 | "intra_loop_closure_probability_scaling": 0.05 23 | }, 24 | { 25 | "std_dev_position": 0.023, 26 | "std_dev_angle": 0.0115, 27 | "estimated_information_position": 1890.359168241966, 28 | "estimated_information_angle": 7561.436672967864, 29 | "random_number_generator_seed": 1302, 30 | "std_dev_intra_loop_closure_position": 0.023, 31 | "std_dev_intra_loop_closure_angle": 0.0115, 32 | "estimated_information_intra_position": 1890.359168241966, 33 | "estimated_information_intra_angle": 7561.436672967864, 34 | "intra_loop_closure_radius": 3.0, 35 | "same_position_intra_loop_closure_probability": 0.5, 36 | "intra_loop_closure_probability_scaling": 0.05 37 | }, 38 | { 39 | "std_dev_position": 0.023, 40 | "std_dev_angle": 0.0115, 41 | "estimated_information_position": 1890.359168241966, 42 | "estimated_information_angle": 7561.436672967864, 43 | "random_number_generator_seed": 3096, 44 | "std_dev_intra_loop_closure_position": 0.023, 45 | "std_dev_intra_loop_closure_angle": 0.0115, 46 | "estimated_information_intra_position": 1890.359168241966, 47 | "estimated_information_intra_angle": 7561.436672967864, 48 | "intra_loop_closure_radius": 3.0, 49 | "same_position_intra_loop_closure_probability": 0.5, 50 | "intra_loop_closure_probability_scaling": 0.05 51 | }, 52 | { 53 | "std_dev_position": 0.023, 54 | "std_dev_angle": 0.0115, 55 | "estimated_information_position": 1890.359168241966, 56 | "estimated_information_angle": 7561.436672967864, 57 | "random_number_generator_seed": 7324, 58 | "std_dev_intra_loop_closure_position": 0.023, 59 | "std_dev_intra_loop_closure_angle": 0.0115, 60 | "estimated_information_intra_position": 1890.359168241966, 61 | "estimated_information_intra_angle": 7561.436672967864, 62 | "intra_loop_closure_radius": 3.0, 63 | "same_position_intra_loop_closure_probability": 0.5, 64 | "intra_loop_closure_probability_scaling": 0.05 65 | }, 66 | { 67 | "std_dev_position": 0.023, 68 | "std_dev_angle": 0.0115, 69 | "estimated_information_position": 1890.359168241966, 70 | "estimated_information_angle": 7561.436672967864, 71 | "random_number_generator_seed": 2443, 72 | "std_dev_intra_loop_closure_position": 0.023, 73 | "std_dev_intra_loop_closure_angle": 0.0115, 74 | "estimated_information_intra_position": 1890.359168241966, 75 | "estimated_information_intra_angle": 7561.436672967864, 76 | "intra_loop_closure_radius": 3.0, 77 | "same_position_intra_loop_closure_probability": 0.5, 78 | "intra_loop_closure_probability_scaling": 0.05 79 | } 80 | ]}, 81 | "inter_agent_loop_closure_parameters": { 82 | "std_dev_loop_closure_position": 0.023, 83 | "std_dev_loop_closure_angle": 0.0115, 84 | "estimated_information_position": 1890.359168241966, 85 | "estimated_information_angle": 7561.436672967864, 86 | "loop_closure_radius": 3.0, 87 | "loop_closure_probability_scaling": 0.05, 88 | "same_position_loop_closure_probability": 1.0, 89 | "random_number_generator_seed": 2708 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /examples/data/iSAM2example.json: -------------------------------------------------------------------------------- 1 | { 2 | "number_of_steps": 2500, 3 | "align": true, 4 | "number_of_agents": 5, 5 | "exact_information_matrix": false, 6 | "agent_parameters": { 7 | "stepsize": 1.0, 8 | "step_fragment": 4, 9 | "allow_retrocession": true, 10 | "agent_specific": [{ 11 | "std_dev_position": 0.023, 12 | "std_dev_angle": 0.0115, 13 | "estimated_information_position": 1890.359168241966, 14 | "estimated_information_angle": 7561.436672967864, 15 | "std_dev_intra_loop_closure_position": 0.023, 16 | "std_dev_intra_loop_closure_angle": 0.0115, 17 | "estimated_information_intra_position": 1890.359168241966, 18 | "estimated_information_intra_angle": 7561.436672967864, 19 | "intra_loop_closure_radius": 3.0, 20 | "random_number_generator_seed": 1406, 21 | "same_position_intra_loop_closure_probability": 0.5, 22 | "intra_loop_closure_probability_scaling": 0.05 23 | }, 24 | { 25 | "std_dev_position": 0.023, 26 | "std_dev_angle": 0.0115, 27 | "estimated_information_position": 1890.359168241966, 28 | "estimated_information_angle": 7561.436672967864, 29 | "std_dev_intra_loop_closure_position": 0.023, 30 | "std_dev_intra_loop_closure_angle": 0.0115, 31 | "estimated_information_intra_position": 1890.359168241966, 32 | "estimated_information_intra_angle": 7561.436672967864, 33 | "intra_loop_closure_radius": 3.0, 34 | "random_number_generator_seed": 1302, 35 | "same_position_intra_loop_closure_probability": 0.5, 36 | "intra_loop_closure_probability_scaling": 0.05 37 | }, 38 | { 39 | "std_dev_position": 0.023, 40 | "std_dev_angle": 0.0115, 41 | "estimated_information_position": 1890.359168241966, 42 | "estimated_information_angle": 7561.436672967864, 43 | "std_dev_intra_loop_closure_position": 0.023, 44 | "std_dev_intra_loop_closure_angle": 0.0115, 45 | "estimated_information_intra_position": 1890.359168241966, 46 | "estimated_information_intra_angle": 7561.436672967864, 47 | "intra_loop_closure_radius": 3.0, 48 | "random_number_generator_seed": 3096, 49 | "same_position_intra_loop_closure_probability": 0.5, 50 | "intra_loop_closure_probability_scaling": 0.05 51 | }, 52 | { 53 | "std_dev_position": 0.023, 54 | "std_dev_angle": 0.0115, 55 | "estimated_information_position": 1890.359168241966, 56 | "estimated_information_angle": 7561.436672967864, 57 | "std_dev_intra_loop_closure_position": 0.023, 58 | "std_dev_intra_loop_closure_angle": 0.0115, 59 | "estimated_information_intra_position": 1890.359168241966, 60 | "estimated_information_intra_angle": 7561.436672967864, 61 | "intra_loop_closure_radius": 3.0, 62 | "random_number_generator_seed": 7324, 63 | "same_position_intra_loop_closure_probability": 0.5, 64 | "intra_loop_closure_probability_scaling": 0.05 65 | }, 66 | { 67 | "std_dev_position": 0.023, 68 | "std_dev_angle": 0.0115, 69 | "estimated_information_position": 1890.359168241966, 70 | "estimated_information_angle": 7561.436672967864, 71 | "std_dev_intra_loop_closure_position": 0.023, 72 | "std_dev_intra_loop_closure_angle": 0.0115, 73 | "estimated_information_intra_position": 1890.359168241966, 74 | "estimated_information_intra_angle": 7561.436672967864, 75 | "intra_loop_closure_radius": 3.0, 76 | "random_number_generator_seed": 2443, 77 | "same_position_intra_loop_closure_probability": 0.5, 78 | "intra_loop_closure_probability_scaling": 0.05 79 | } 80 | ]}, 81 | "inter_agent_loop_closure_parameters": { 82 | "std_dev_loop_closure_position": 0.023, 83 | "std_dev_loop_closure_angle": 0.0115, 84 | "estimated_information_position": 1599.9999999999998, 85 | "estimated_information_angle": 1599.9999999999998, 86 | "loop_closure_radius": 3.0, 87 | "loop_closure_probability_scaling": 0.05, 88 | "same_position_loop_closure_probability": 1.0, 89 | "random_number_generator_seed": 2708 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /examples/data/example.json: -------------------------------------------------------------------------------- 1 | { 2 | "number_of_steps": 1200, 3 | "align": true, 4 | "number_of_agents": 5, 5 | "exact_information_matrix": false, 6 | "agent_parameters": { 7 | "stepsize": 1.0, 8 | "step_fragment": 4, 9 | "allow_retrocession": true, 10 | "agent_specific": [{ 11 | "std_dev_position": 0.023, 12 | "std_dev_angle": 0.0023, 13 | "estimated_information_position": 1890.359168241966, 14 | "estimated_information_angle": 189035.91682419661, 15 | "std_dev_intra_loop_closure_position": 0.023, 16 | "std_dev_intra_loop_closure_angle": 0.0023, 17 | "estimated_information_intra_position": 1890.359168241966, 18 | "estimated_information_intra_angle": 189035.91682419661, 19 | "intra_loop_closure_radius": 3.0, 20 | "random_number_generator_seed": 1406, 21 | "same_position_intra_loop_closure_probability": 0.5, 22 | "intra_loop_closure_probability_scaling": 0.05 23 | }, 24 | { 25 | "std_dev_position": 0.023, 26 | "std_dev_angle": 0.0023, 27 | "estimated_information_position": 1890.359168241966, 28 | "estimated_information_angle": 189035.91682419661, 29 | "std_dev_intra_loop_closure_position": 0.023, 30 | "std_dev_intra_loop_closure_angle": 0.0023, 31 | "estimated_information_intra_position": 1890.359168241966, 32 | "estimated_information_intra_angle": 189035.91682419661, 33 | "intra_loop_closure_radius": 3.0, 34 | "random_number_generator_seed": 1407, 35 | "same_position_intra_loop_closure_probability": 0.5, 36 | "intra_loop_closure_probability_scaling": 0.05 37 | }, 38 | { 39 | "std_dev_position": 0.023, 40 | "std_dev_angle": 0.0023, 41 | "estimated_information_position": 1890.359168241966, 42 | "estimated_information_angle": 189035.91682419661, 43 | "std_dev_intra_loop_closure_position": 0.023, 44 | "std_dev_intra_loop_closure_angle": 0.0023, 45 | "estimated_information_intra_position": 1890.359168241966, 46 | "estimated_information_intra_angle": 189035.91682419661, 47 | "intra_loop_closure_radius": 3.0, 48 | "random_number_generator_seed": 1408, 49 | "same_position_intra_loop_closure_probability": 0.5, 50 | "intra_loop_closure_probability_scaling": 0.05 51 | }, 52 | { 53 | "std_dev_position": 0.023, 54 | "std_dev_angle": 0.0023, 55 | "estimated_information_position": 1890.359168241966, 56 | "estimated_information_angle": 189035.91682419661, 57 | "std_dev_intra_loop_closure_position": 0.023, 58 | "std_dev_intra_loop_closure_angle": 0.0023, 59 | "estimated_information_intra_position": 1890.359168241966, 60 | "estimated_information_intra_angle": 189035.91682419661, 61 | "intra_loop_closure_radius": 3.0, 62 | "random_number_generator_seed": 1409, 63 | "same_position_intra_loop_closure_probability": 0.5, 64 | "intra_loop_closure_probability_scaling": 0.05 65 | }, 66 | { 67 | "std_dev_position": 0.023, 68 | "std_dev_angle": 0.0023, 69 | "estimated_information_position": 1890.359168241966, 70 | "estimated_information_angle": 189035.91682419661, 71 | "std_dev_intra_loop_closure_position": 0.023, 72 | "std_dev_intra_loop_closure_angle": 0.0023, 73 | "estimated_information_intra_position": 1890.359168241966, 74 | "estimated_information_intra_angle": 189035.91682419661, 75 | "intra_loop_closure_radius": 3.0, 76 | "random_number_generator_seed": 1410, 77 | "same_position_intra_loop_closure_probability": 0.5, 78 | "intra_loop_closure_probability_scaling": 0.05 79 | } 80 | ]}, 81 | "inter_agent_loop_closure_parameters": { 82 | "std_dev_loop_closure_position": 0.023, 83 | "std_dev_loop_closure_angle": 0.0023, 84 | "estimated_information_position": 1599.9999999999998, 85 | "estimated_information_angle": 1599.9999999999998, 86 | "loop_closure_radius": 3.0, 87 | "loop_closure_probability_scaling": 0.05, 88 | "same_position_loop_closure_probability": 1.0, 89 | "random_number_generator_seed": 2708 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /include/kollagen/JsonParse.h: -------------------------------------------------------------------------------- 1 | #include "json/json.hpp" 2 | #include "DataGeneratorParams.h" 3 | #include 4 | #include 5 | 6 | namespace kollagen 7 | { 8 | 9 | inline nlohmann::json open_json(const std::string& filename){ 10 | std::ifstream f(filename); 11 | nlohmann::json data = nlohmann::json::parse(f); 12 | return data; 13 | } 14 | 15 | inline std::vector parse_agent_params(const nlohmann::json& data){ 16 | std::vector agents{}; 17 | int N_agents = data.at("number_of_agents"); 18 | double stepsize = data.at("agent_parameters").at("stepsize"); 19 | int step_fragment = data.at("agent_parameters").at("step_fragment"); 20 | bool allow_retrocession = data.at("agent_parameters").at("allow_retrocession"); 21 | for (const auto& agent_data : data.at("agent_parameters").at("agent_specific")) { 22 | AgentParams params; 23 | params.stepsize = stepsize; 24 | params.step_fragment = step_fragment; 25 | params.allow_retrocession = allow_retrocession; 26 | params.sigma_pos = agent_data.at("std_dev_position"); 27 | params.sigma_ang = agent_data.at("std_dev_angle"); 28 | params.I11est = agent_data.at("estimated_information_position"); 29 | params.I22est = agent_data.at("estimated_information_position"); 30 | params.I33est = agent_data.at("estimated_information_angle"); 31 | params.seed = agent_data.at("random_number_generator_seed"); 32 | agents.emplace_back(params); 33 | } 34 | if (N_agents != static_cast(agents.size())) { 35 | if(agents.size() != 1){ 36 | std::cerr << "[ERROR] Number of entries in 'agent_specific' field of " 37 | "json file must either be equal to 'number_of_agents', or equal to 1." 38 | << std::endl; 39 | std::terminate(); 40 | }; 41 | for (int i = 0; i < N_agents - 1; ++i) { 42 | agents.emplace_back(agents.at(0)); 43 | } 44 | } 45 | return agents; 46 | } 47 | 48 | inline std::vector parse_lc_params(const nlohmann::json& data){ 49 | std::vector lc_params{}; 50 | int N_agents = data.at("number_of_agents"); 51 | for (const auto& agent_data : data.at("agent_parameters").at("agent_specific")) { 52 | LCParams params; 53 | params.sigma_pos = agent_data.at("std_dev_intra_loop_closure_position"); 54 | params.sigma_ang = agent_data.at("std_dev_intra_loop_closure_angle"); 55 | params.I11est = agent_data.at("estimated_information_intra_position"); 56 | params.I22est = agent_data.at("estimated_information_intra_position"); 57 | params.I33est = agent_data.at("estimated_information_intra_angle"); 58 | params.radius = agent_data.at("intra_loop_closure_radius"); 59 | params.idemLCprob = agent_data.at("same_position_intra_loop_closure_probability"); 60 | params.prob_scale = agent_data.at("intra_loop_closure_probability_scaling"); 61 | lc_params.emplace_back(params); 62 | } 63 | if (N_agents != static_cast(lc_params.size())) { 64 | if(lc_params.size() != 1){ 65 | std::cerr << "[ERROR] Number of entries in 'agent_specific' field of " 66 | "json file must either be equal to 'number_of_agents', or equal to 1." 67 | << std::endl; 68 | std::terminate(); 69 | }; 70 | for (int i = 0; i < N_agents - 1; ++i) { 71 | lc_params.emplace_back(lc_params.at(0)); 72 | } 73 | } 74 | return lc_params; 75 | } 76 | 77 | inline InterLCParams parse_inter_lc_params(const nlohmann::json& data){ 78 | InterLCParams inter_lc_params{}; 79 | auto inter_lc_data = data.at("inter_agent_loop_closure_parameters"); 80 | inter_lc_params.sigma_pos = inter_lc_data.at("std_dev_loop_closure_position"); 81 | inter_lc_params.sigma_ang = inter_lc_data.at("std_dev_loop_closure_angle"); 82 | inter_lc_params.I11est = inter_lc_data.at("estimated_information_position"); 83 | inter_lc_params.I22est = inter_lc_data.at("estimated_information_position"); 84 | inter_lc_params.I33est = inter_lc_data.at("estimated_information_angle"); 85 | inter_lc_params.radius = inter_lc_data.at("loop_closure_radius"); 86 | inter_lc_params.idemLCprob = inter_lc_data.at("same_position_loop_closure_probability"); 87 | inter_lc_params.prob_scale = inter_lc_data.at("loop_closure_probability_scaling"); 88 | inter_lc_params.seed = inter_lc_data.at("random_number_generator_seed"); 89 | return inter_lc_params; 90 | } 91 | 92 | inline DataGeneratorParams get_params_from_json(const nlohmann::json& data){ 93 | auto agent_params = parse_agent_params(data); 94 | auto lc_params = parse_lc_params(data); 95 | auto inter_lc_params = parse_inter_lc_params(data); 96 | int N_steps = data.at("number_of_steps"); 97 | auto params = DataGeneratorParams(agent_params,lc_params,inter_lc_params,N_steps); 98 | params.align_center_of_masses = data.at("align"); 99 | params.exact_information_matrix = data.at("exact_information_matrix"); 100 | return params; 101 | } 102 | 103 | inline DataGeneratorParams get_params_from_json(const std::string& filename){ 104 | return get_params_from_json(open_json(filename)); 105 | } 106 | } // namespace kollagen 107 | -------------------------------------------------------------------------------- /examples/timing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "kollagen.h" 7 | 8 | using std::chrono::high_resolution_clock; 9 | using std::chrono::duration; 10 | using std::chrono::milliseconds; 11 | 12 | int main() { 13 | 14 | double radius = 3.0; 15 | double idemLCprob = 0.5; 16 | double prob_scale = 0.05; 17 | 18 | int seed1 = 9875; 19 | 20 | double sigma_pos = 0.023; 21 | double sigma_ang = sigma_pos / 10; 22 | double Idiag = std::pow(sigma_pos, -2); 23 | 24 | double sigma_pos_single = 0.023; 25 | double sigma_ang_single = sigma_pos / 10; 26 | double Idiag_single = std::pow(sigma_pos_single, -2); 27 | 28 | double sigma_pos_inter = 0.023; 29 | double sigma_ang_inter = sigma_pos_inter / 10; 30 | double Idiag_inter = std::pow(sigma_pos_inter, -2); 31 | 32 | auto lc_param = 33 | kollagen::LCParams(sigma_pos_single, sigma_ang_single, Idiag_single, 34 | Idiag_single, radius, idemLCprob, prob_scale); 35 | 36 | auto agent_param = 37 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 0); 38 | auto agent2_param = 39 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 1); 40 | auto agent3_param = 41 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 2); 42 | auto agent4_param = 43 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 3); 44 | auto agent5_param = 45 | kollagen::AgentParams(1.0, 4, sigma_pos, sigma_ang, Idiag, Idiag * 100, seed1 + 4); 46 | 47 | auto inter_lc_param = 48 | kollagen::InterLCParams(sigma_pos_inter, sigma_ang_inter, Idiag_inter, 49 | Idiag_inter, radius, 1.0, prob_scale, seed1 + 5); 50 | 51 | std::vector agent_params{ 52 | agent_param, 53 | agent2_param, 54 | agent3_param, 55 | agent4_param, 56 | agent5_param 57 | }; 58 | 59 | 60 | std::vector agents; 61 | for (int i = 0; i < 5; ++i) { 62 | std::cout << "i = " << i << '\n'; 63 | std::vector number_of_nodes{}; 64 | std::vector times{}; 65 | 66 | agents.emplace_back(agent_params.at(i)); 67 | for (int N = 1; N <= 2000000; N = N*2) { 68 | std::cout << "\tN = " << N << '\n'; 69 | auto t1 = high_resolution_clock::now(); 70 | kollagen::DataGenerator data_gen = kollagen::DataGenerator( 71 | {{agents}, lc_param, inter_lc_param, N} 72 | ); 73 | data_gen.generate(); 74 | auto t2 = high_resolution_clock::now(); 75 | 76 | duration ms_double = t2 - t1; 77 | double time = ms_double.count(); 78 | times.emplace_back(time); 79 | number_of_nodes.emplace_back(N); 80 | } 81 | kollagen::save_binary("timing_x"+std::to_string(i+1)+".bin", number_of_nodes); 82 | kollagen::save_binary("timing_y"+std::to_string(i+1)+".bin", times); 83 | } 84 | 85 | for (int N = 10; N <= 100000; N = N*10) { 86 | std::cout << "N = " << N << '\n'; 87 | std::vector agents; 88 | std::vector number_of_agents{}; 89 | std::vector times{}; 90 | 91 | for (int i = 1; i <= 1024; i = i*2) { 92 | std::cout << "\ti = " << i << '\n'; 93 | agents.emplace_back(kollagen::AgentParams(1.0, 1, sigma_pos, sigma_ang, Idiag, Idiag, seed1 + 1)); 94 | auto t1 = high_resolution_clock::now(); 95 | kollagen::DataGenerator data_gen = kollagen::DataGenerator( 96 | {{agents}, lc_param, inter_lc_param, N} 97 | ); 98 | data_gen.generate(); 99 | auto t2 = high_resolution_clock::now(); 100 | 101 | duration ms_double = t2 - t1; 102 | double time = ms_double.count(); 103 | times.emplace_back(time); 104 | number_of_agents.emplace_back(i+1); 105 | } 106 | kollagen::save_binary("timing_agents_x"+std::to_string(N)+".bin", number_of_agents); 107 | kollagen::save_binary("timing_agents_y"+std::to_string(N)+".bin", times); 108 | } 109 | 110 | for (int N = 10; N <= 1000; N = N*10) { 111 | std::cout << "N = " << N << '\n'; 112 | std::vector radius{}; 113 | std::vector times{}; 114 | 115 | for (int i = 1; i <= 128; i = i*2) { 116 | std::cout << "\ti = " << i << '\n'; 117 | lc_param.radius = i; 118 | inter_lc_param.radius = i; 119 | auto t1 = high_resolution_clock::now(); 120 | kollagen::DataGenerator data_gen = kollagen::DataGenerator( 121 | {{agent_params}, lc_param, inter_lc_param, N} 122 | ); 123 | data_gen.generate(); 124 | auto t2 = high_resolution_clock::now(); 125 | 126 | duration ms_double = t2 - t1; 127 | double time = ms_double.count(); 128 | times.emplace_back(time); 129 | radius.emplace_back(i); 130 | } 131 | kollagen::save_binary("timing_radius_x"+std::to_string(N)+".bin", radius); 132 | kollagen::save_binary("timing_radius_y"+std::to_string(N)+".bin", times); 133 | } 134 | 135 | 136 | return EXIT_SUCCESS; 137 | } 138 | 139 | 140 | -------------------------------------------------------------------------------- /tests/testJSONParse.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "kollagen.h" 5 | 6 | class TestJSONParse : public ::testing::Test { 7 | protected: 8 | void SetUp() override { 9 | test_data = kollagen::open_json("test_data/test.json"); 10 | test_data2 = kollagen::open_json("test_data/test2.json"); 11 | } 12 | nlohmann::json test_data; 13 | nlohmann::json test_data2; 14 | }; 15 | 16 | TEST_F(TestJSONParse, testParseAgentParams){ 17 | auto test = kollagen::parse_agent_params(test_data); 18 | EXPECT_EQ(test_data.at("number_of_agents"), test.size()); 19 | for (int i = 0; const auto& agent : test) { 20 | auto agent_data = test_data.at("agent_parameters").at("agent_specific").at(i); 21 | EXPECT_EQ(test_data.at("agent_parameters").at("stepsize"), agent.stepsize); 22 | EXPECT_EQ(test_data.at("agent_parameters").at("step_fragment"), agent.step_fragment); 23 | EXPECT_EQ(test_data.at("agent_parameters").at("allow_retrocession"), agent.allow_retrocession); 24 | EXPECT_EQ(agent_data.at("std_dev_position"), agent.sigma_pos); 25 | EXPECT_EQ(agent_data.at("std_dev_angle"), agent.sigma_ang); 26 | EXPECT_EQ(agent_data.at("estimated_information_position"), agent.I11est); 27 | EXPECT_EQ(agent_data.at("estimated_information_position"), agent.I22est); 28 | EXPECT_EQ(agent_data.at("estimated_information_angle"), agent.I33est); 29 | EXPECT_EQ(agent_data.at("random_number_generator_seed"), agent.seed); 30 | ++i; 31 | } 32 | } 33 | 34 | TEST_F(TestJSONParse, testParseAgentParams2){ 35 | auto test = kollagen::parse_agent_params(test_data2); 36 | EXPECT_EQ(test_data2.at("number_of_agents"), test.size()); 37 | for (const auto& agent : test) { 38 | auto agent_data = test_data2.at("agent_parameters").at("agent_specific").at(0); 39 | EXPECT_EQ(test_data2.at("agent_parameters").at("stepsize"), agent.stepsize); 40 | EXPECT_EQ(test_data2.at("agent_parameters").at("step_fragment"), agent.step_fragment); 41 | EXPECT_EQ(test_data2.at("agent_parameters").at("allow_retrocession"), agent.allow_retrocession); 42 | EXPECT_EQ(agent_data.at("std_dev_position"), agent.sigma_pos); 43 | EXPECT_EQ(agent_data.at("std_dev_angle"), agent.sigma_ang); 44 | EXPECT_EQ(agent_data.at("estimated_information_position"), agent.I11est); 45 | EXPECT_EQ(agent_data.at("estimated_information_position"), agent.I22est); 46 | EXPECT_EQ(agent_data.at("estimated_information_angle"), agent.I33est); 47 | EXPECT_EQ(agent_data.at("random_number_generator_seed"), agent.seed); 48 | } 49 | } 50 | 51 | TEST_F(TestJSONParse, testParseLCParams){ 52 | auto test = kollagen::parse_lc_params(test_data); 53 | for (int i = 0; const auto& lc : test) { 54 | auto lc_data = test_data.at("agent_parameters").at("agent_specific").at(i); 55 | EXPECT_EQ(lc_data.at("std_dev_intra_loop_closure_position"), lc.sigma_pos); 56 | EXPECT_EQ(lc_data.at("std_dev_intra_loop_closure_angle"), lc.sigma_ang); 57 | EXPECT_EQ(lc_data.at("estimated_information_intra_position"), lc.I11est); 58 | EXPECT_EQ(lc_data.at("estimated_information_intra_position"), lc.I22est); 59 | EXPECT_EQ(lc_data.at("estimated_information_intra_angle"), lc.I33est); 60 | EXPECT_EQ(lc_data.at("intra_loop_closure_radius"), lc.radius); 61 | EXPECT_EQ(lc_data.at("same_position_intra_loop_closure_probability"), lc.idemLCprob); 62 | EXPECT_EQ(lc_data.at("intra_loop_closure_probability_scaling"), lc.prob_scale); 63 | ++i; 64 | } 65 | } 66 | 67 | TEST_F(TestJSONParse, testParseLCParams2){ 68 | auto test = kollagen::parse_lc_params(test_data2); 69 | for (const auto& lc : test) { 70 | auto lc_data = test_data.at("agent_parameters").at("agent_specific").at(0); 71 | EXPECT_EQ(lc_data.at("std_dev_intra_loop_closure_position"), lc.sigma_pos); 72 | EXPECT_EQ(lc_data.at("std_dev_intra_loop_closure_angle"), lc.sigma_ang); 73 | EXPECT_EQ(lc_data.at("estimated_information_intra_position"), lc.I11est); 74 | EXPECT_EQ(lc_data.at("estimated_information_intra_position"), lc.I22est); 75 | EXPECT_EQ(lc_data.at("estimated_information_intra_angle"), lc.I33est); 76 | EXPECT_EQ(lc_data.at("intra_loop_closure_radius"), lc.radius); 77 | EXPECT_EQ(lc_data.at("same_position_intra_loop_closure_probability"), lc.idemLCprob); 78 | EXPECT_EQ(lc_data.at("intra_loop_closure_probability_scaling"), lc.prob_scale); 79 | } 80 | } 81 | 82 | TEST_F(TestJSONParse, testParseInterLCParams){ 83 | auto test = kollagen::parse_inter_lc_params(test_data); 84 | auto lc_data = test_data.at("inter_agent_loop_closure_parameters"); 85 | EXPECT_EQ(lc_data.at("std_dev_loop_closure_position"), test.sigma_pos); 86 | EXPECT_EQ(lc_data.at("std_dev_loop_closure_angle"), test.sigma_ang); 87 | EXPECT_EQ(lc_data.at("estimated_information_position"), test.I11est); 88 | EXPECT_EQ(lc_data.at("estimated_information_position"), test.I22est); 89 | EXPECT_EQ(lc_data.at("estimated_information_angle"), test.I33est); 90 | EXPECT_EQ(lc_data.at("loop_closure_radius"), test.radius); 91 | EXPECT_EQ(lc_data.at("same_position_loop_closure_probability"), test.idemLCprob); 92 | EXPECT_EQ(lc_data.at("loop_closure_probability_scaling"), test.prob_scale); 93 | EXPECT_EQ(lc_data.at("random_number_generator_seed"), test.seed); 94 | } 95 | 96 | TEST_F(TestJSONParse, testGetParamsFromJSON){ 97 | auto test = kollagen::get_params_from_json(test_data); 98 | EXPECT_EQ(test_data.at("align"), test.align_center_of_masses); 99 | EXPECT_EQ(test_data.at("number_of_steps"), test.N_steps); 100 | EXPECT_EQ(test_data.at("number_of_agents"), test.N_agents); 101 | EXPECT_EQ(test_data.at("exact_information_matrix"), test.exact_information_matrix); 102 | } 103 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # kollagen 2 | 3 | A Collaborative SLAM Pose Graph Generator 4 | 5 | This is a header-only data generator library which generates multi-agent, 6 | M3500esque pose-graphs with both intra-agent and inter-agent loop-closures. 7 | It's primary use is within the Collaborative SLAM activity. 8 | 9 | The package can be used either by interacting directly with the C++ API (see 10 | [example.cpp](src/example.cpp) and [iSAM2example.cpp](src/iSAM2example.cpp)) or 11 | by JSON files – using either the python pip package, or the 12 | [generate](src/generate.cpp) binary made available after building. 13 | 14 | ## Overview of the generation procedure 15 | 16 | In the generated multi-agent pose graphs, inspired by the popular M3500 17 | dataset, introduced by [Olson et al. in 2006](http://rvsn.csail.mit.edu/content/eolson/graphoptim/eolson-graphoptim2006.pdf), 18 | the agents move on a planar grid world. The pose graph consists of nodes 19 | and edges, where the nodes represent the poses of the agents and the edges 20 | represent a relative transformation from one pose to the other. 21 | 22 | The pose graph generation has three major components: 23 | 1. **Ground truth generation:** 24 | To generate the ground truth of each agent, we first let each agent turn 25 | randomly by -180, -90, 0, or 90 degrees. Then the agent moves a 26 | pre-defined amount of steps in that direction. This is repeated for a 27 | user-specified amount of times. 28 | 29 | 2. **Noisy odometry generation:** 30 | The ground truth of two consecutive poses is used to obtain a true distance 31 | and relative angle between the poses. The distance and relative angle are 32 | each modified by adding a zero-mean normally distributed noise terms with a 33 | user-specified standard deviations. In this way, ***kollagen*** generates 34 | noisy odometry for all consecutive poses. The noisy odometry is encoded in 35 | the edge between the two consecutive nodes in the form of a noisy relative 36 | transformation. 37 | 38 | 3. **Loop closure generation:** 39 | Loop closures are edges between two (possibly non-consecutive) nodes. The 40 | idea behind loop closures is that an agent recognizes a pose and creates an 41 | edge, encoding a noisy relative transformation between the poses, in the 42 | pose graph between its current pose and the recognized pose. The noisy 43 | relative transformation encoded by the edge is obtained similarly to the 44 | noisy odometry, i.e., adding a zero-mean normally distributed noise to the 45 | true distance and relative angle between the poses, which is then used to 46 | obtain the noisy relative transformation. 47 | There are two types of loop closures in ***kollagen***: 48 | - **Intra-agent loop closures** are edges between (possibly 49 | non-consecutive) nodes of the *same* agent. To produce intra-agent 50 | loop closures, we iterate through all poses, i.e., nodes, of an 51 | agent's pose graph and create a loop closure between the current and a 52 | previous poses within a certain distance based on a probability 53 | proportional to the distance between the poses. 54 | 55 | - **Inter-agent loop closures** are edges between nodes of *different* 56 | agents. To produce inter-agent loop closures, we iterate through all 57 | poses, i.e., nodes, of one agents and create a loop closure between 58 | the current pose of that agent and the pose of another agent within a 59 | certain distance based on a probability proportional to the distance 60 | between the poses. 61 | 62 | ## pip package 63 | 64 | If only interested in generating datasets (be it multig2o or singleg2o) from 65 | JSON, the provided pip package could be what you are after. 66 | 67 | To install, enter the project root, and issue 68 | 69 | ```bash 70 | pip3 install . 71 | ``` 72 | 73 | After this, the CLI command `kollagenr8` should become available. Issue 74 | 75 | ```bash 76 | kollagenr8 --help 77 | ``` 78 | 79 | for instructions on how to use. 80 | 81 | For Windows builds, make sure to have up-to-date versions of MSVC and the 82 | Windows SDK. See further below for tested versions. 83 | 84 | ### Troubleshooting 85 | 86 | Make sure you have a modern, C++20 compliant, compiler (tested with `clang` 14) 87 | 88 | ## Building 89 | 90 | ### Linux 91 | 92 | The building has been tested on Ubuntu 22.04 (Jammy) with Clang 14. 93 | 94 | ```bash 95 | # Clone the repository 96 | git clone https://github.com/EricssonResearch/kollagen.git 97 | 98 | # Update aptitude repositories and install debian-package dependencies 99 | sudo apt update 100 | sudo apt install cmake clang 101 | 102 | # OPTIONAL: Create python virtual environment and install python dependencies for 103 | # plotting 104 | sudo apt install python3-pip 105 | pip3 install virtualenv 106 | virtualenv -p python3 env 107 | source env/bin/activate 108 | pip3 install matplotlib numpy 109 | 110 | # Create build folder 111 | mkdir build && cd build 112 | 113 | # Configure make-files, setting Clang 14 as compiler 114 | export CC=clang 115 | export CXX=clang++ 116 | cmake .. -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DKOLLAGEN_ENABLE_TESTS=OFF -DKOLLAGEN_ENABLE_GTSAM=OFF -DKOLLAGEN_BUILD_EXAMPLES=ON 117 | 118 | # Compile 119 | make -j$(nproc --all) 120 | ``` 121 | 122 | ### Windows 123 | 124 | Building has been tested with MSVC v143 with Windows 10 SDK 10.0.20348. 125 | 126 | ### macOS 127 | 128 | Python installation with pip has been testen on macOS Monterey 12.6.2 with clang 14.0.0 129 | 130 | ## Citation 131 | 132 | If using kollagen in your research, we would appreciate if you could cite our work: 133 | 134 | ```bibtex 135 | @misc{kollagen, 136 | doi = {10.48550/ARXIV.2303.04753}, 137 | url = {https://arxiv.org/abs/2303.04753}, 138 | author = {Sundin, Roberto C. and Umsonst, David}, 139 | title = {kollagen: A Collaborative SLAM Pose Graph Generator}, 140 | publisher = {arXiv}, 141 | year = {2023}, 142 | } 143 | ``` 144 | -------------------------------------------------------------------------------- /include/kollagen/Walker.h: -------------------------------------------------------------------------------- 1 | #ifndef WALKER_H 2 | #define WALKER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "DiscreteCircle.h" 18 | #include "InterLCParams.h" 19 | #include "LCParams.h" 20 | #include "Loopclosure.h" 21 | #include "MultiLoopclosure.h" 22 | #include "RNGenerator.h" 23 | #include "WalkerParams.h" 24 | 25 | #include "utils.hpp" 26 | 27 | #define _USE_MATH_DEFINES 28 | 29 | namespace kollagen 30 | { 31 | 32 | class Walker { 33 | public: 34 | virtual ~Walker() = default; 35 | Walker(const Walker &) = default; 36 | Walker(Walker &&) = default; 37 | Walker &operator=(const Walker &) = default; 38 | Walker &operator=(Walker &&) = default; 39 | 40 | explicit Walker(WalkerParams params = WalkerParams()); 41 | 42 | Walker(double x, double y, double theta, double stepsize, double sigma_pos, 43 | double sigma_ang, int step_fragment, int seed, int ID, double I11est, 44 | double I22est, double I33est, bool allow_retrocession = true); 45 | 46 | void step(); 47 | void step(int N); 48 | 49 | void step_left(); 50 | void step_right(); 51 | void step_forward(); 52 | 53 | int writeg2o(const std::string &filename, const bool &_3D = false, const bool &exact_information_matrix = false) const; 54 | int write_ground_truth_as_TUM(const std::string &filename) const; 55 | 56 | [[nodiscard]] int step_fragment() const { return step_fragment_; }; 57 | [[nodiscard]] double stepsize() const { return stepsize_; }; 58 | 59 | [[nodiscard]] WalkerID ID() const { return ID_; }; 60 | 61 | [[nodiscard]] size_t size() const { return X_.size(); }; 62 | 63 | [[nodiscard]] std::vector Drx() const { return Drx_; }; 64 | [[nodiscard]] std::vector Dry() const { return Dry_; }; 65 | [[nodiscard]] std::vector Drtheta() const { return Drtheta_; }; 66 | 67 | [[nodiscard]] double Drx(int i) const { return Drx_.at(i); }; 68 | [[nodiscard]] double Dry(int i) const { return Dry_.at(i); }; 69 | [[nodiscard]] double Drtheta(int i) const { return Drtheta_.at(i); }; 70 | 71 | [[nodiscard]] double X(int i) const { return X_.at(i); }; 72 | [[nodiscard]] double Y(int i) const { return Y_.at(i); }; 73 | [[nodiscard]] double Theta(int i) const { return Theta_.at(i); }; 74 | [[nodiscard]] Loopclosure LC(int i) const { return LC_.at(i); }; 75 | 76 | [[nodiscard]] std::vector X() const { return X_; }; 77 | [[nodiscard]] std::vector Y() const { return Y_; }; 78 | [[nodiscard]] std::vector Theta() const { return Theta_; }; 79 | [[nodiscard]] std::vector LC() const { return LC_; }; 80 | 81 | [[nodiscard]] int seed() const { return rng.seed(); }; 82 | 83 | [[nodiscard]] WalkerParams Params() const { 84 | return {x0_, y0_, theta0_, stepsize_, 85 | std_dev_pos_, std_dev_ang_, step_fragment_, rng.seed(), 86 | ID_, I11est_, I22est_, I33est_}; 87 | }; 88 | 89 | [[nodiscard]] double odom_pos(int i) const { return odom_pos_.at(i+1); }; 90 | [[nodiscard]] double odom_ang(int i) const { return odom_ang_.at(i+1); }; 91 | 92 | [[nodiscard]] double std_dev_pos() const { return std_dev_pos_; }; 93 | [[nodiscard]] double std_dev_ang() const { return std_dev_ang_; }; 94 | 95 | [[nodiscard]] std::vector odom_pos() const { 96 | return {odom_pos_.begin() + 1, odom_pos_.end()}; 97 | }; 98 | [[nodiscard]] std::vector odom_ang() const { 99 | return {odom_ang_.begin() + 1, odom_ang_.end()}; 100 | }; 101 | 102 | [[nodiscard]] virtual double draw_normal() { return rng.draw_normal(); }; 103 | [[nodiscard]] virtual double draw_uniform() { return rng.draw_uniform(); }; 104 | [[nodiscard]] virtual int draw_int() { return rng.draw_int(); }; 105 | 106 | void addLC(const Loopclosure &LC) { LC_.emplace_back(LC); }; 107 | 108 | void translate(const int &x, const int &y); 109 | void translate(const Point &p); 110 | 111 | [[nodiscard]] double scale_int_length(int length) const; 112 | [[nodiscard]] static double scale_int_angle(int angle); 113 | [[nodiscard]] int scale_double_length(double length) const; 114 | [[nodiscard]] static int scale_double_angle(double angle); 115 | 116 | [[nodiscard]] double theta_as_double() const; 117 | [[nodiscard]] double x_as_double() const; 118 | [[nodiscard]] double y_as_double() const; 119 | 120 | [[nodiscard]] int dx(int i) const { return delta_x_.at(i); }; 121 | [[nodiscard]] int dy(int i) const { return delta_y_.at(i); }; 122 | [[nodiscard]] int dtheta(int i) const { return delta_theta_.at(i); }; 123 | 124 | private: 125 | WalkerID ID_; 126 | 127 | double x0_; 128 | double y0_; 129 | double theta0_; 130 | 131 | int step_fragment_; 132 | double stepsize_; 133 | 134 | int x_; 135 | int y_; 136 | int theta_; 137 | double std_dev_pos_; 138 | double std_dev_ang_; 139 | double I11est_; 140 | double I22est_; 141 | double I33est_; 142 | 143 | std::vector X_; 144 | std::vector Y_; 145 | std::vector Theta_; 146 | 147 | std::vector odom_pos_{0.0}; 148 | std::vector odom_ang_{0.0}; 149 | 150 | double drx_; 151 | double dry_; 152 | double drtheta_; 153 | 154 | std::vector Drx_; 155 | std::vector Dry_; 156 | std::vector Drtheta_; 157 | 158 | std::vector LC_{}; 159 | 160 | RNGenerator rng; 161 | 162 | std::vector delta_x_; 163 | std::vector delta_y_; 164 | std::vector delta_theta_; 165 | 166 | int random_turn(); 167 | void simulate_steps(int dtheta); 168 | void dead_reckon(double odom_ang, double odom_pos); 169 | [[nodiscard]] std::tuple position_change_in_global_frame() const; 170 | }; 171 | 172 | } // namespace kollagen 173 | #endif /* !WALKER_H */ 174 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test_3D/agent2/posegraph.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT 0 0 -2 0.0 0.0 0.0 0 1 2 | VERTEX_SE3:QUAT 1 -0.00096799166710853342 -1.7283394789157271 0.0 0.0 0.0 0.70836544858469874 0.70584586933083238 3 | VERTEX_SE3:QUAT 2 -0.0020712592916866304 -1.4324175676141393 0.0 0.0 0.0 0.70842367730633005 0.70578742793547744 4 | VERTEX_SE3:QUAT 3 -0.0029296423998657517 -1.1785411086676416 0.0 0.0 0.0 0.70830116696184664 0.70591037453807559 5 | VERTEX_SE3:QUAT 4 -0.0042613245138987546 -0.917036024265516 0.0 0.0 0.0 0.70890489827059211 0.70530407995981526 6 | VERTEX_SE3:QUAT 5 -0.0047007322632284382 -0.64223124834387191 0.0 0.0 0.0 0.70767187998508407 0.70654123041643979 7 | VERTEX_SE3:QUAT 6 -0.0049888236710465298 -0.39117897137962365 0.0 0.0 0.0 0.70751237967080716 0.70670094991626531 8 | VERTEX_SE3:QUAT 7 -0.0050408069769223469 -0.13603274909925567 0.0 0.0 0.0 0.70717881022727447 0.70703474480787465 9 | VERTEX_SE3:QUAT 8 -0.0058185183354620808 0.067442896424187104 0.0 0.0 0.0 0.70845681129661364 0.70575416862207363 10 | VERTEX_SE3:QUAT 9 -0.0074044751053005797 0.28857105577971476 0.0 0.0 0.0 0.7096379116713144 0.70456655776354848 11 | VERTEX_SE3:QUAT 10 -0.0088559413620259882 0.50666999633910415 0.0 0.0 0.0 0.70945575417906037 0.70474997897284153 12 | VERTEX_SE3:QUAT 11 -0.010215862022119975 0.74121211369413542 0.0 0.0 0.0 0.70915375497055522 0.70505386447502127 13 | VERTEX_SE3:QUAT 12 -0.011503870733858184 0.99224681296241357 0.0 0.0 0.0 0.7089184480516364 0.70529046074086332 14 | VERTEX_SE3:QUAT 13 -0.01299235360564276 1.2217374398366116 0.0 0.0 0.0 0.70939618430807783 0.70480994153682297 15 | VERTEX_SE3:QUAT 14 -0.015098312815756017 1.4878038870336638 0.0 0.0 0.0 0.70989961076836938 0.70430287705710648 16 | VERTEX_SE3:QUAT 15 -0.016530633129041232 1.7948834215320462 0.0 0.0 0.0 0.70875393450545843 0.70545578197576098 17 | VERTEX_SE3:QUAT 16 -0.016774619768256782 2.0948502920048906 0.0 0.0 0.0 0.7073942954078567 0.70681915001252049 18 | EDGE_SE3:QUAT 0 1 -0.00096799166710853342 0.27166052108427297 0.0 0.0 0.0 0.70836544858469874 0.70584586933083238 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 19 | EDGE_SE3:QUAT 1 2 0.29592396388905406 4.8826488571178526e-05 0.0 0.0 0.0 8.2498367876377915e-05 0.99999999659700967 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 20 | EDGE_SE3:QUAT 2 3 0.25387789478833539 -8.812840669987432e-05 0.0 0.0 0.0 -0.00017356454525332529 0.99999998493767417 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 21 | EDGE_SE3:QUAT 3 4 0.26150809220031412 0.00044750325997883666 0.0 0.0 0.0 0.00085561935158132282 0.99999963395769564 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 22 | EDGE_SE3:QUAT 4 5 0.27480345043275239 -0.00095998894215993183 0.0 0.0 0.0 -0.0017466748460327741 0.9999984745623276 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 23 | EDGE_SE3:QUAT 5 6 0.25105241667917771 -0.00011333639051492205 0.0 0.0 0.0 -0.00022572254701277831 0.9999999745246656 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 24 | EDGE_SE3:QUAT 6 7 0.2551461139400909 -0.00024080589110118001 0.0 0.0 0.0 -0.00047189786069206773 0.99999988865619838 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 25 | EDGE_SE3:QUAT 7 8 0.20347579975068092 0.00073625536313504038 0.0 0.0 0.0 0.0018091875034045119 0.99999836341894954 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 26 | EDGE_SE3:QUAT 8 9 0.22113260587048963 0.00074076971193340856 0.0 0.0 0.0 0.001674937514898101 0.99999859729117679 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 27 | EDGE_SE3:QUAT 9 10 0.21810374116970227 -0.00011276170491352887 0.0 0.0 0.0 -0.00025850472120717032 0.999999966587654 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 28 | EDGE_SE3:QUAT 10 11 0.23454597376187578 -0.00020097183027279419 0.0 0.0 0.0 -0.00042842725397683684 0.99999990822503981 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 29 | EDGE_SE3:QUAT 11 12 0.25103794759903952 -0.00016753631903024851 0.0 0.0 0.0 -0.00033368718285532668 0.99999994432643047 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 30 | EDGE_SE3:QUAT 12 13 0.22949524325823836 0.00031100838246996714 0.0 0.0 0.0 0.00067759175280547243 0.99999977043468191 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 31 | EDGE_SE3:QUAT 13 14 0.26607450988107628 0.00038023650739894783 0.0 0.0 0.0 0.00071452958115524284 0.99999974472370623 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 32 | EDGE_SE3:QUAT 14 15 0.30708125240777873 -0.00099823288528733318 0.0 0.0 0.0 -0.0016253498411500204 0.99999867911807461 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 33 | EDGE_SE3:QUAT 15 16 0.299964745512032 -0.0011551452956358257 0.0 0.0 0.0 -0.0019254577230356925 0.9999981463045603 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 34 | -------------------------------------------------------------------------------- /include/kollagen/Walker-impl.h: -------------------------------------------------------------------------------- 1 | #ifndef WALKER_IMPL_H 2 | #define WALKER_IMPL_H 3 | 4 | #include "Walker.h" 5 | #include "Functions.h" 6 | 7 | namespace kollagen 8 | { 9 | 10 | inline double Walker::theta_as_double() const { 11 | return scale_int_angle(theta_); 12 | } 13 | inline double Walker::x_as_double() const { return scale_int_length(x_); } 14 | inline double Walker::y_as_double() const { return scale_int_length(y_); } 15 | 16 | inline double Walker::scale_int_length(int length) const { 17 | return length * stepsize_ / step_fragment_; 18 | } 19 | 20 | inline double Walker::scale_int_angle(int angle) { 21 | return normalize_angle(angle * 0.5 * M_PI); 22 | } 23 | 24 | inline int Walker::scale_double_length(double length) const { 25 | return round_to_int(length * step_fragment_ / stepsize_); 26 | } 27 | 28 | inline int Walker::scale_double_angle(double angle) { 29 | return (round_to_int(normalize_angle(angle) / 0.5 / M_PI) + 4) % 4; 30 | } 31 | 32 | inline Walker::Walker(double x, double y, double theta, double stepsize, 33 | double sigma_pos, double sigma_ang, int step_fragment, 34 | int seed, int ID, double I11est, double I22est, 35 | double I33est, bool allow_retrocession) 36 | : ID_(ID), x0_(x), y0_(y), theta0_(theta), step_fragment_(step_fragment), 37 | stepsize_(stepsize), x_(scale_double_length(x)), 38 | y_(scale_double_length(y)), theta_(scale_double_angle(theta)), 39 | std_dev_pos_(sigma_pos), std_dev_ang_(sigma_ang), 40 | I11est_(I11est > 0 ? I11est : 1 / std::pow(sigma_pos, 2)), 41 | I22est_(I22est > 0 ? I22est : 1 / std::pow(sigma_pos, 2)), 42 | I33est_(I33est > 0 ? I33est : 1 / std::pow(sigma_pos, 2)), X_({x}), 43 | Y_({y}), Theta_({theta}), drx_(x), dry_(y), drtheta_(theta), Drx_({x}), 44 | Dry_({y}), Drtheta_({theta}), rng(seed, allow_retrocession){}; 45 | 46 | inline Walker::Walker(WalkerParams p) 47 | : ID_(p.ID), x0_(p.x), y0_(p.y), theta0_(p.theta), 48 | step_fragment_(p.step_fragment), stepsize_(p.stepsize), 49 | x_(scale_double_length(p.x)), y_(scale_double_length(p.y)), 50 | theta_(scale_double_angle(p.theta)), std_dev_pos_(p.sigma_pos), 51 | std_dev_ang_(p.sigma_ang), I11est_(p.I11est), I22est_(p.I22est), 52 | I33est_(p.I33est), X_({p.x}), Y_({p.y}), Theta_({p.theta}), drx_(p.x), 53 | dry_(p.y), drtheta_(p.theta), Drx_({p.x}), Dry_({p.y}), 54 | Drtheta_({p.theta}), rng(p.seed, p.allow_retrocession){}; 55 | 56 | inline void Walker::step(int N) { 57 | for (int i = 0; i < N; ++i) { 58 | this->step(); 59 | } 60 | }; 61 | 62 | inline int Walker::random_turn() { 63 | int dtheta = draw_int(); 64 | return dtheta; 65 | }; 66 | 67 | inline void Walker::dead_reckon(double odom_ang, double odom_pos) { 68 | drtheta_ += odom_ang; 69 | drtheta_ = normalize_angle(drtheta_); 70 | drx_ += odom_pos * std::cos(drtheta_); 71 | dry_ += odom_pos * std::sin(drtheta_); 72 | 73 | Drx_.emplace_back(drx_); 74 | Dry_.emplace_back(dry_); 75 | Drtheta_.emplace_back(drtheta_); 76 | } 77 | 78 | inline std::tuple Walker::position_change_in_global_frame() const { 79 | double theta = theta_as_double(); 80 | int dx = round_to_int(std::cos(theta)); 81 | int dy = round_to_int(std::sin(theta)); 82 | return {dx, dy}; 83 | } 84 | 85 | inline void Walker::simulate_steps(int dtheta) { 86 | for (int i = 0; i < step_fragment_; ++i) { 87 | if (i == 0) { 88 | theta_ = (theta_ + dtheta) % 4; 89 | } 90 | 91 | auto [int_dx, int_dy] = position_change_in_global_frame(); 92 | x_ += int_dx; 93 | y_ += int_dy; 94 | 95 | double dx = scale_int_length(int_dx); 96 | double dy = scale_int_length(int_dy); 97 | 98 | X_.emplace_back(x_as_double()); 99 | Y_.emplace_back(y_as_double()); 100 | Theta_.emplace_back(theta_as_double()); 101 | 102 | double odom_pos = std::hypot(dx, dy) + std_dev_pos_ * draw_normal(); 103 | double odom_ang{}; 104 | if (i == 0) { 105 | odom_ang = scale_int_angle(dtheta) + std_dev_ang_ * draw_normal(); 106 | delta_theta_.emplace_back(dtheta); 107 | } else if (i > 0) { 108 | odom_ang = std_dev_ang_ * draw_normal(); 109 | delta_theta_.emplace_back(0); 110 | } 111 | delta_x_.emplace_back(int_dx); 112 | delta_y_.emplace_back(int_dy); 113 | 114 | odom_pos_.emplace_back(odom_pos); 115 | odom_ang_.emplace_back(odom_ang); 116 | 117 | dead_reckon(odom_ang_.back(), odom_pos_.back()); 118 | } 119 | }; 120 | 121 | inline void Walker::step() { 122 | int dtheta = random_turn(); 123 | simulate_steps(dtheta); 124 | }; 125 | 126 | inline void Walker::step_left() { simulate_steps(1); }; 127 | 128 | inline void Walker::step_right() { simulate_steps(-1); }; 129 | 130 | inline void Walker::step_forward() { simulate_steps(0); }; 131 | 132 | inline int Walker::writeg2o(const std::string &filename, const bool &_3D, const bool &exact_information_matrix) const { 133 | auto file = open_file(filename); 134 | 135 | if (!file) { 136 | return EXIT_FAILURE; 137 | } 138 | 139 | write_vertices(file, {Drx(), Dry(), Drtheta()}, _3D); 140 | 141 | write_odometry(file, *this, _3D, exact_information_matrix); 142 | 143 | write_loop_closures(file, LC_, _3D); 144 | 145 | return EXIT_SUCCESS; 146 | }; 147 | 148 | inline int Walker::write_ground_truth_as_TUM(const std::string &filename) const { 149 | std::filesystem::path path(filename); 150 | if (path.extension() != ".tum") { 151 | path += ".tum"; 152 | } 153 | std::cout << "Saving ground truth to " << path.string() << '\n'; 154 | 155 | std::ofstream file(path.string(), std::ios_base::out); 156 | if (!file) { 157 | return EXIT_FAILURE; 158 | } 159 | 160 | write_TUM_vertices(file, {X(), Y(), Theta()}); 161 | 162 | return EXIT_SUCCESS; 163 | }; 164 | 165 | inline void Walker::translate(const int &x, const int &y) { 166 | std::for_each(X_.begin(), X_.end(), [&x](double &n) { n += x; }); 167 | std::for_each(Drx_.begin(), Drx_.end(), [&x](double &n) { n += x; }); 168 | 169 | std::for_each(Y_.begin(), Y_.end(), [&y](double &n) { n += y; }); 170 | std::for_each(Dry_.begin(), Dry_.end(), [&y](double &n) { n += y; }); 171 | x0_ += x; 172 | y0_ += y; 173 | } 174 | 175 | inline void Walker::translate(const Point &p) { translate(p.x, p.y); } 176 | 177 | } // namespace kollagen 178 | #endif /* !WALKER_IMPL_H */ 179 | -------------------------------------------------------------------------------- /data/Multi3500x8.json: -------------------------------------------------------------------------------- 1 | { 2 | "number_of_steps": 875, 3 | "align": true, 4 | "number_of_agents": 8, 5 | "exact_information_matrix": false, 6 | "agent_parameters": { 7 | "stepsize": 1.0, 8 | "step_fragment": 4, 9 | "allow_retrocession": true, 10 | "agent_specific": [{ 11 | "std_dev_position": 0.03, 12 | "std_dev_angle": 0.03, 13 | "estimated_information_position": 1111.1111111111111, 14 | "estimated_information_angle": 1111.1111111111111, 15 | "random_number_generator_seed": 1407, 16 | "std_dev_intra_loop_closure_position": 0.02, 17 | "std_dev_intra_loop_closure_angle": 0.02, 18 | "estimated_information_intra_position": 2500.0, 19 | "estimated_information_intra_angle": 2500.0, 20 | "intra_loop_closure_radius": 5.0, 21 | "same_position_intra_loop_closure_probability": 0.5, 22 | "intra_loop_closure_probability_scaling": 0.05 23 | }, 24 | { 25 | "std_dev_position": 0.03, 26 | "std_dev_angle": 0.03, 27 | "estimated_information_position": 1111.1111111111111, 28 | "estimated_information_angle": 1111.1111111111111, 29 | "random_number_generator_seed": 1303, 30 | "std_dev_intra_loop_closure_position": 0.02, 31 | "std_dev_intra_loop_closure_angle": 0.02, 32 | "estimated_information_intra_position": 2500.0, 33 | "estimated_information_intra_angle": 2500.0, 34 | "intra_loop_closure_radius": 5.0, 35 | "same_position_intra_loop_closure_probability": 0.5, 36 | "intra_loop_closure_probability_scaling": 0.05 37 | }, 38 | { 39 | "std_dev_position": 0.03, 40 | "std_dev_angle": 0.03, 41 | "estimated_information_position": 1111.1111111111111, 42 | "estimated_information_angle": 1111.1111111111111, 43 | "random_number_generator_seed": 3097, 44 | "std_dev_intra_loop_closure_position": 0.02, 45 | "std_dev_intra_loop_closure_angle": 0.02, 46 | "estimated_information_intra_position": 2500.0, 47 | "estimated_information_intra_angle": 2500.0, 48 | "intra_loop_closure_radius": 5.0, 49 | "same_position_intra_loop_closure_probability": 0.5, 50 | "intra_loop_closure_probability_scaling": 0.05 51 | }, 52 | { 53 | "std_dev_position": 0.03, 54 | "std_dev_angle": 0.03, 55 | "estimated_information_position": 1111.1111111111111, 56 | "estimated_information_angle": 1111.1111111111111, 57 | "random_number_generator_seed": 7325, 58 | "std_dev_intra_loop_closure_position": 0.02, 59 | "std_dev_intra_loop_closure_angle": 0.02, 60 | "estimated_information_intra_position": 2500.0, 61 | "estimated_information_intra_angle": 2500.0, 62 | "intra_loop_closure_radius": 5.0, 63 | "same_position_intra_loop_closure_probability": 0.5, 64 | "intra_loop_closure_probability_scaling": 0.05 65 | }, 66 | { 67 | "std_dev_position": 0.03, 68 | "std_dev_angle": 0.03, 69 | "estimated_information_position": 1111.1111111111111, 70 | "estimated_information_angle": 1111.1111111111111, 71 | "random_number_generator_seed": 2444, 72 | "std_dev_intra_loop_closure_position": 0.02, 73 | "std_dev_intra_loop_closure_angle": 0.02, 74 | "estimated_information_intra_position": 2500.0, 75 | "estimated_information_intra_angle": 2500.0, 76 | "intra_loop_closure_radius": 5.0, 77 | "same_position_intra_loop_closure_probability": 0.5, 78 | "intra_loop_closure_probability_scaling": 0.05 79 | }, 80 | { 81 | "std_dev_position": 0.03, 82 | "std_dev_angle": 0.03, 83 | "estimated_information_position": 1111.1111111111111, 84 | "estimated_information_angle": 1111.1111111111111, 85 | "random_number_generator_seed": 3832, 86 | "std_dev_intra_loop_closure_position": 0.02, 87 | "std_dev_intra_loop_closure_angle": 0.02, 88 | "estimated_information_intra_position": 2500.0, 89 | "estimated_information_intra_angle": 2500.0, 90 | "intra_loop_closure_radius": 5.0, 91 | "same_position_intra_loop_closure_probability": 0.5, 92 | "intra_loop_closure_probability_scaling": 0.05 93 | }, 94 | { 95 | "std_dev_position": 0.03, 96 | "std_dev_angle": 0.03, 97 | "estimated_information_position": 1111.1111111111111, 98 | "estimated_information_angle": 1111.1111111111111, 99 | "random_number_generator_seed": 1556, 100 | "std_dev_intra_loop_closure_position": 0.02, 101 | "std_dev_intra_loop_closure_angle": 0.02, 102 | "estimated_information_intra_position": 2500.0, 103 | "estimated_information_intra_angle": 2500.0, 104 | "intra_loop_closure_radius": 5.0, 105 | "same_position_intra_loop_closure_probability": 0.5, 106 | "intra_loop_closure_probability_scaling": 0.05 107 | }, 108 | { 109 | "std_dev_position": 0.03, 110 | "std_dev_angle": 0.03, 111 | "estimated_information_position": 1111.1111111111111, 112 | "estimated_information_angle": 1111.1111111111111, 113 | "random_number_generator_seed": 2674, 114 | "std_dev_intra_loop_closure_position": 0.02, 115 | "std_dev_intra_loop_closure_angle": 0.02, 116 | "estimated_information_intra_position": 2500.0, 117 | "estimated_information_intra_angle": 2500.0, 118 | "intra_loop_closure_radius": 5.0, 119 | "same_position_intra_loop_closure_probability": 0.5, 120 | "intra_loop_closure_probability_scaling": 0.05 121 | } 122 | ]}, 123 | "inter_agent_loop_closure_parameters": { 124 | "std_dev_loop_closure_position": 0.025, 125 | "std_dev_loop_closure_angle": 0.025, 126 | "estimated_information_position": 1599.9999999999998, 127 | "estimated_information_angle": 1599.9999999999998, 128 | "loop_closure_radius": 5.0, 129 | "loop_closure_probability_scaling": 0.05, 130 | "same_position_loop_closure_probability": 0.8, 131 | "random_number_generator_seed": 2710 132 | } 133 | } 134 | -------------------------------------------------------------------------------- /tests/test_data/multig2o_test_3D/agent1/posegraph.g2o: -------------------------------------------------------------------------------- 1 | VERTEX_SE3:QUAT 0 0 0 0.0 0.0 0.0 0 1 2 | VERTEX_SE3:QUAT 1 0.28660034220463471 -0.00026702747031739167 0.0 0.0 0.0 -0.0004658532180006681 0.99999989149038371 3 | VERTEX_SE3:QUAT 2 0.56886905531889354 0.00052095577697020674 0.0 0.0 0.0 0.001395799300253125 0.99999902587168221 4 | VERTEX_SE3:QUAT 3 0.86146653634403048 0.0016078049915356646 0.0 0.0 0.0 0.0018572333354007556 0.99999827534068175 5 | VERTEX_SE3:QUAT 4 1.133825842211849 0.0038571038579772775 0.0 0.0 0.0 0.0041291802617323507 0.99999147489884443 6 | VERTEX_SE3:QUAT 5 0.87950873076741543 0.0010432669754649642 0.0 0.0 0.0 -0.99998469898873121 0.0055318883228562387 7 | VERTEX_SE3:QUAT 6 0.62736012546378772 -0.0016739495225960048 0.0 0.0 0.0 -0.99998548521234321 0.0053878905551653037 8 | VERTEX_SE3:QUAT 7 0.35584192760149058 -0.0054995318698116136 0.0 0.0 0.0 -0.99997518878853808 0.0070442747907505342 9 | VERTEX_SE3:QUAT 8 0.11783054786483249 -0.0092544247805395365 0.0 0.0 0.0 -0.99996889462964444 0.0078873172350939838 10 | VERTEX_SE3:QUAT 9 0.11512900379775405 0.24559065690514534 0.0 0.0 0.0 0.71084461560306988 0.70334908293686138 11 | VERTEX_SE3:QUAT 10 0.11225380825594418 0.4941382464361751 0.0 0.0 0.0 0.71118465046290924 0.70300525812112502 12 | VERTEX_SE3:QUAT 11 0.10977581902651104 0.71495101252778182 0.0 0.0 0.0 0.71106308507165639 0.70312821664926684 13 | VERTEX_SE3:QUAT 12 0.10693545959773573 0.95730916134267663 0.0 0.0 0.0 0.71123796055907429 0.70295132367737145 14 | VERTEX_SE3:QUAT 13 0.1103842150679314 0.7212211261817999 0.0 0.0 0.0 -0.70192365571844539 0.71225218956690706 15 | VERTEX_SE3:QUAT 14 0.11445040260352612 0.46472470307348401 0.0 0.0 0.0 -0.70148028752335745 0.712688856525867 16 | VERTEX_SE3:QUAT 15 0.1180583292542563 0.22977094013900129 0.0 0.0 0.0 -0.70165729151677247 0.71251459301641462 17 | VERTEX_SE3:QUAT 16 0.12073051060145405 0.0011586093666100805 0.0 0.0 0.0 -0.70296233777942485 0.71122707461378731 18 | EDGE_SE3:QUAT 0 1 0.28660034220463471 -0.00026702747031739167 0.0 0.0 0.0 -0.0004658532180006681 0.99999989149038371 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 19 | EDGE_SE3:QUAT 1 2 0.28226785642980329 0.0010509744534258326 0.0 0.0 0.0 0.0018616519129953351 0.99999826712457596 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 20 | EDGE_SE3:QUAT 2 3 0.29259937495951516 0.00027003105678605378 0.0 0.0 0.0 0.0004614346332423154 0.99999989353903396 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 21 | EDGE_SE3:QUAT 3 4 0.27236578189264238 0.0012376155300146889 0.0 0.0 0.0 0.0022719556380044331 0.99999741910545892 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 22 | EDGE_SE3:QUAT 4 5 -0.25433167664632361 -0.00071351644112970078 0.0 0.0 0.0 -0.99999901618209119 0.0014027240818501579 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 23 | EDGE_SE3:QUAT 5 6 0.25216323512353289 -7.2622970441349821e-05 0.0 0.0 0.0 -0.00014399991368073579 0.99999998963201242 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 24 | EDGE_SE3:QUAT 6 7 0.27154365690039667 0.00089958203879594867 0.0 0.0 0.0 0.0016564156695242907 0.99999862814262386 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 25 | EDGE_SE3:QUAT 7 8 0.23804065830941221 0.0004013683350272312 0.0 0.0 0.0 0.00084306586522401152 0.99999964461991031 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 26 | EDGE_SE3:QUAT 8 9 -0.0013187550215991756 -0.25485598852081692 0.0 0.0 0.0 -0.70893386199126718 0.70527496717390092 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 27 | EDGE_SE3:QUAT 9 10 0.24856410284041186 0.0002403959858711792 0.0 0.0 0.0 0.00048356922581155787 0.99999988308039511 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 28 | EDGE_SE3:QUAT 10 11 0.22082665660653311 -7.6365097323481283e-05 0.0 0.0 0.0 -0.00017290732711432416 0.99999998505152798 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 29 | EDGE_SE3:QUAT 11 12 0.24237476229946212 0.00012057755413520462 0.0 0.0 0.0 0.00024874195192724739 0.99999996906372024 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 30 | EDGE_SE3:QUAT 12 13 -0.23611223891249722 -0.00068183292997908349 0.0 0.0 0.0 -0.99999895761904289 0.0014438700868065581 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 31 | EDGE_SE3:QUAT 13 14 0.25652845263757401 0.00031927387424937457 0.0 0.0 0.0 0.00062229683590030594 0.99999980637330532 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 32 | EDGE_SE3:QUAT 14 15 0.23498143378746408 -0.00011673464691620459 0.0 0.0 0.0 -0.00024839119038297071 0.99999996915090783 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 33 | EDGE_SE3:QUAT 15 16 0.22862641063747313 -0.00083826807348389462 0.0 0.0 0.0 -0.0018332611818667613 0.9999983195753076 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 34 | EDGE_SE3:QUAT 1 7 0.00070534665879937544 -0.008468276345526219 0.0 0.0 0.0 -0.99999999645008819 -8.4260450603654741e-05 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 35 | EDGE_SE3:QUAT 0 8 0.010073736378136359 -0.039903138891562245 0.0 0.0 0.0 -0.9999982167639998 0.0018885096823948941 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 36 | EDGE_SE3:QUAT 11 13 -0.013991981638450963 -0.02669565735519381 0.0 0.0 0.0 -0.99999706167073199 -0.0024241802536616912 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 37 | EDGE_SE3:QUAT 0 16 -0.034727147355296577 0.0058708814894198455 0.0 0.0 0.0 -0.70800674751665493 0.70620566796853701 1890.359168241966 0.0 0.0 0.0 0.0 0.0 1890.359168241966 0.0 0.0 0.0 0.0 1000000 0.0 0.0 0.0 189035.91682419661 0.0 0.0 189035.91682419661 0.0 189035.91682419661 38 | -------------------------------------------------------------------------------- /examples/iSAM2example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "kollagen.h" 12 | 13 | template using BetweenFactor = gtsam::BetweenFactor; 14 | 15 | using Key = gtsam::Key; 16 | using Symbol = gtsam::Symbol; 17 | using Pose2 = gtsam::Pose2; 18 | using Pose2Map = std::map>>; 19 | using FactorMap = std::map>>; 20 | 21 | const std::string alphabet = 22 | "ABCDEFGHIJKLMNOPQRSTUVXYZabcdefghijklmnopqrstuvxyz"; 23 | 24 | using ID_to_char_map = std::map; 25 | 26 | using Data = 27 | std::tuple; 28 | 29 | inline std::array, 3> Values_to_XYTheta(const gtsam::Values& values){ 30 | std::array, 3> XYTheta{}; 31 | for (const auto& value : values) { 32 | auto pose2 = value.value.cast(); 33 | XYTheta.at(0).emplace_back(pose2.x()); 34 | XYTheta.at(1).emplace_back(pose2.y()); 35 | XYTheta.at(2).emplace_back(pose2.theta()); 36 | } 37 | return XYTheta; 38 | } 39 | 40 | inline void writeTUM(const gtsam::Values &estimate, 41 | const std::string &filename) { 42 | std::fstream stream(filename.c_str(), std::fstream::out); 43 | double timestamp{0.0}; 44 | 45 | for (const auto &key_value : estimate) { 46 | const auto &pose = key_value.value.cast(); 47 | stream << std::setprecision(17) << timestamp << " " << pose.x() << " " 48 | << pose.y() << " " << 0.0 << " " << 0.0 << " " << 0.0 << " " 49 | << std::sin(pose.theta() / 2) << " " << std::cos(pose.theta() / 2) 50 | << '\n'; 51 | timestamp += 1; 52 | } 53 | } 54 | 55 | gtsam::Rot3 QuaternionFromTheta(double theta) { 56 | return gtsam::Rot3::Quaternion(std::cos(theta / 2), 0.0, 0.0, 57 | std::sin(theta / 2)); 58 | } 59 | 60 | Data get_data(const kollagen::DataGenerator &data_generator) { 61 | ID_to_char_map ID_to_char{}; 62 | for (int i = 0; const auto &agent : data_generator.Walkers()) { 63 | ID_to_char[agent.ID()] = alphabet[i]; 64 | i += 1; 65 | } 66 | 67 | Pose2Map gt_map{}; 68 | Pose2Map pose_map{}; 69 | FactorMap odometry_map{}; 70 | FactorMap loop_closure_map{}; 71 | 72 | odometry_map.insert({{0, {}}}); 73 | ; 74 | loop_closure_map.insert({{0, {}}}); 75 | ; 76 | 77 | for (const auto &agent : data_generator.Walkers()) { 78 | // Add agent ground truth 79 | for (int i = 0; i < static_cast(agent.size()); ++i) { 80 | double x = agent.X(i); 81 | double y = agent.Y(i); 82 | double theta = agent.Theta(i); 83 | gt_map[i].emplace_back(std::make_pair( 84 | Symbol(ID_to_char.at(agent.ID()), i), Pose2(x, y, theta))); 85 | } 86 | // Add agent dead-reckoning 87 | for (int i = 0; i < static_cast(agent.size()); ++i) { 88 | double x = agent.Drx(i); 89 | double y = agent.Dry(i); 90 | double theta = agent.Drtheta(i); 91 | pose_map[i].emplace_back(std::make_pair( 92 | Symbol(ID_to_char.at(agent.ID()), i), Pose2(x, y, theta))); 93 | } 94 | // Add agent odometry 95 | for (int i = 1; i < static_cast(agent.size()); ++i) { 96 | odometry_map.insert({{i, {}}}); 97 | loop_closure_map.insert({{i, {}}}); 98 | double theta = agent.odom_ang(i - 1); 99 | double x = agent.odom_pos(i - 1) * std::cos(theta); 100 | double y = agent.odom_pos(i - 1) * std::sin(theta); 101 | gtsam::SharedNoiseModel model = gtsam::noiseModel::Diagonal::Variances( 102 | gtsam::Vector3(1 / agent.Params().I11est, 1 / agent.Params().I22est, 103 | 1 / agent.Params().I33est)); 104 | odometry_map[i].emplace_back(BetweenFactor( 105 | Symbol(ID_to_char.at(agent.ID()), i - 1), 106 | Symbol(ID_to_char.at(agent.ID()), i), Pose2(x, y, theta), model)); 107 | } 108 | // Add agent loop closures 109 | for (const auto &lc : agent.LC()) { 110 | gtsam::SharedNoiseModel model = gtsam::noiseModel::Diagonal::Variances( 111 | gtsam::Vector3(1 / lc.I11, 1 / lc.I22, 1 / lc.I33)); 112 | int i = std::max(lc.from, lc.to); 113 | loop_closure_map[i].emplace_back( 114 | BetweenFactor(Symbol(ID_to_char.at(agent.ID()), lc.from), 115 | Symbol(ID_to_char.at(agent.ID()), lc.to), 116 | Pose2(lc.dx, lc.dy, lc.dtheta), model)); 117 | } 118 | } 119 | // Add inter-agent loop-closures 120 | for (const auto &lc : data_generator.LCs()) { 121 | gtsam::SharedNoiseModel model = gtsam::noiseModel::Diagonal::Variances( 122 | gtsam::Vector3(1 / lc.I11, 1 / lc.I22, 1 / lc.I33)); 123 | int i = std::max(lc.from, lc.to); 124 | loop_closure_map[i].emplace_back( 125 | BetweenFactor(Symbol(ID_to_char.at(lc.ID1), lc.from), 126 | Symbol(ID_to_char.at(lc.ID2), lc.to), 127 | Pose2(lc.dx, lc.dy, lc.dtheta), model)); 128 | } 129 | return {gt_map, pose_map, odometry_map, loop_closure_map, ID_to_char}; 130 | } 131 | 132 | int main() { 133 | 134 | std::string filename = "data/iSAM2example.json"; 135 | auto data_generator = 136 | kollagen::DataGenerator(kollagen::get_params_from_json(filename)); 137 | data_generator.generate(); 138 | 139 | auto [gt_map, pose_map, odometry_map, loop_closure_map, ID_to_char] = 140 | get_data(data_generator); 141 | 142 | gtsam::NonlinearFactorGraph g2o_graph; 143 | 144 | gtsam::NonlinearFactorGraph graph; 145 | gtsam::Values initial; 146 | 147 | graph.addPrior(pose_map.at(0).at(0).first, pose_map.at(0).at(0).second); 148 | 149 | gtsam::ISAM2 isam{}; 150 | 151 | int number_of_agents = static_cast(data_generator.Walkers().size()); 152 | int number_of_steps = static_cast(pose_map.size()); 153 | 154 | std::vector> APE(number_of_agents); 155 | for (int i = 0; i < number_of_agents; ++i) { 156 | APE.at(i).reserve(number_of_steps); 157 | } 158 | std::vector timesteps{}; 159 | 160 | for (const auto &indexed_pose : pose_map.at(0)) { 161 | auto index = indexed_pose.first; 162 | auto pose = indexed_pose.second; 163 | initial.insert(index, pose); 164 | } 165 | 166 | bool all_connected = false; 167 | gtsam::Values latest_result; 168 | latest_result = initial; 169 | gtsam::Values online_result; 170 | for (int i = 1; i < number_of_steps; ++i) { 171 | for (const auto &odometry : odometry_map.at(i)) { 172 | auto key1 = odometry.key1(); 173 | auto key2 = odometry.key2(); 174 | graph.add(odometry); 175 | g2o_graph.add(odometry); 176 | 177 | auto latest_pose = latest_result.at(key1); 178 | auto pose = latest_pose.compose(odometry.measured()); 179 | initial.insert(key2, pose); 180 | } 181 | 182 | for (const auto &loop_closure : loop_closure_map.at(i)) { 183 | graph.add(loop_closure); 184 | g2o_graph.add(loop_closure); 185 | } 186 | 187 | if (all_connected) { 188 | isam.update(graph, initial); 189 | latest_result = isam.calculateEstimate(); 190 | 191 | for (int j = 0; j < number_of_agents; ++j) { 192 | auto KeyPosePair = gt_map.at(i).at(j); 193 | auto estimate = isam.calculateEstimate(KeyPosePair.first); 194 | online_result.insert(KeyPosePair.first, estimate); 195 | auto ground_truth = KeyPosePair.second; 196 | APE.at(j).emplace_back(std::hypot(estimate.x() - ground_truth.x(), 197 | estimate.y() - ground_truth.y())); 198 | } 199 | timesteps.emplace_back(i); 200 | 201 | graph.resize(0); 202 | initial.clear(); 203 | } else { 204 | latest_result = initial; 205 | try { 206 | if (i == 0 || i < 3260) { 207 | continue; 208 | } 209 | gtsam::GaussNewtonOptimizer optimizer{graph, initial}; 210 | gtsam::Values result = optimizer.optimize(); 211 | if (!all_connected) { 212 | std::cout << "All connected at timestep " << i << std::endl; 213 | } 214 | all_connected = true; 215 | for (const auto& value : result) { 216 | online_result.insert(value.key, value.value.cast()); 217 | } 218 | } catch (gtsam::IndeterminantLinearSystemException const &) { 219 | } 220 | } 221 | } 222 | std::cout << "Done, now saving..." << std::endl; 223 | gtsam::Values offline_result = isam.calculateEstimate(); 224 | 225 | auto all_agents_TUM = kollagen::open_file("iSAM2_all.tum");; 226 | int offset = 0; 227 | 228 | for (int i = 0; auto const &agent : data_generator.Walkers()) { 229 | unsigned char symbol = ID_to_char.at(agent.ID()); 230 | gtsam::Values temp_result; 231 | uint k = 0; 232 | while (true) { 233 | try { 234 | temp_result.insert(k, online_result.at(Symbol(symbol, k))); 235 | k += 1; 236 | } catch (gtsam::ValuesKeyDoesNotExist const &) { 237 | std::string output_file = 238 | "agent" + std::to_string(agent.ID()) + "_" + "test" + ".tum"; 239 | std::cout << "Saving to " << output_file << '\n'; 240 | writeTUM(temp_result, output_file); 241 | std::string GT = "agent" + std::to_string(agent.ID()) + "_" + "test" + 242 | "_GT" + ".tum"; 243 | agent.write_ground_truth_as_TUM(GT); 244 | kollagen::write_TUM_vertices(all_agents_TUM, Values_to_XYTheta(temp_result), offset); 245 | offset += static_cast(temp_result.size()); 246 | 247 | break; 248 | } 249 | } 250 | kollagen::save_binary("APE_agent" + std::to_string(agent.ID()) + ".bin", APE.at(i)); 251 | i++; 252 | } 253 | 254 | kollagen::save_binary("APE_time.bin", timesteps); 255 | 256 | return EXIT_SUCCESS; 257 | } 258 | -------------------------------------------------------------------------------- /tests/testDataGenerator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "kollagen.h" 5 | #include "DataGeneratorAttorney.h" 6 | 7 | class TestDataGenerator : public ::testing::Test { 8 | protected: 9 | void SetUp() override { 10 | using ::testing::Return; 11 | walker_params1.sigma_ang = 0.0; 12 | walker_params1.sigma_pos = 0.0; 13 | walker_params1.seed = 1; 14 | 15 | walker_params2 = walker_params1; 16 | walker_params2.y = 1.0; 17 | walker_params2.seed = 2; 18 | 19 | walker_params3 = walker_params1; 20 | walker_params3.y = 2.0; 21 | walker_params3.seed = 3; 22 | 23 | walker_param_vec = {walker_params1, walker_params2, walker_params3}; 24 | 25 | lc_params.idemLCprob = 1.0; 26 | lc_params.sigma_ang = 0.0; 27 | lc_params.sigma_pos = 0.0; 28 | 29 | lc_param_vec = {lc_params, lc_params, lc_params}; 30 | 31 | inter_lc_params.idemLCprob = 1.0; 32 | inter_lc_params.radius = 1.0; 33 | } 34 | 35 | kollagen::WalkerParams walker_params1{}; 36 | kollagen::WalkerParams walker_params2; 37 | kollagen::WalkerParams walker_params3; 38 | 39 | std::vector walker_param_vec; 40 | 41 | kollagen::LCParams lc_params; 42 | std::vector lc_param_vec; 43 | 44 | kollagen::InterLCParams inter_lc_params; 45 | 46 | kollagen::DataGenerator data_gen; 47 | }; 48 | 49 | bool correct_walker_ids(const kollagen::DataGenerator& data_gen){ 50 | auto walkers = data_gen.Walkers(); 51 | std::vector walkers_IDs; 52 | std::vector correct_IDs; 53 | for (int i = 0; i < static_cast(walkers.size()); ++i) { 54 | correct_IDs.emplace_back(i); 55 | walkers_IDs.emplace_back(walkers.at(i).ID()); 56 | } 57 | return std::is_permutation(correct_IDs.begin(), correct_IDs.begin(), walkers_IDs.begin()); 58 | } 59 | 60 | TEST_F(TestDataGenerator, testConstructor) { 61 | correct_walker_ids(data_gen); 62 | ASSERT_EQ(data_gen.Walkers().size(), 0); 63 | } 64 | 65 | TEST_F(TestDataGenerator, testConstructor2) { 66 | kollagen::DataGenerator data_gen2 = kollagen::DataGeneratorAttorney::ConstructDataGenerator(walker_param_vec); 67 | correct_walker_ids(data_gen2); 68 | ASSERT_EQ(data_gen2.Walkers().size(), 3); 69 | } 70 | 71 | TEST_F(TestDataGenerator, testConstructor3) { 72 | auto a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(5); 73 | correct_walker_ids(a); 74 | ASSERT_EQ(a.Walkers().size(), 5); 75 | } 76 | 77 | TEST_F(TestDataGenerator, testConstructor4) { 78 | int number_of_steps = 100; 79 | kollagen::DataGenerator a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(7, number_of_steps); 80 | correct_walker_ids(a); 81 | int step_fragment = a.Walkers().at(0).step_fragment(); 82 | int expected_number_of_steps = number_of_steps * step_fragment + 1; 83 | ASSERT_EQ(a.Walkers().size(), 7); 84 | for (const auto& walker : a.Walkers()) { 85 | ASSERT_EQ(static_cast(walker.size()), expected_number_of_steps); 86 | } 87 | } 88 | 89 | TEST_F(TestDataGenerator, testAgentParams) { 90 | auto agent_params = kollagen::AgentParams(0.1, 2, 0.3, 0.4, 0.5, 0.6, 4729); 91 | auto dg_params = kollagen::DataGeneratorParams(agent_params, lc_params, inter_lc_params); 92 | dg_params.align_center_of_masses = false; 93 | kollagen::DataGenerator data_gen(dg_params); 94 | data_gen.generate(); 95 | auto walker_params = data_gen.Walkers().at(0).Params(); 96 | EXPECT_DOUBLE_EQ(walker_params.x, 0); 97 | EXPECT_DOUBLE_EQ(walker_params.y, 0); 98 | EXPECT_DOUBLE_EQ(walker_params.theta, 0); 99 | EXPECT_DOUBLE_EQ(walker_params.stepsize, agent_params.stepsize); 100 | EXPECT_DOUBLE_EQ(walker_params.sigma_pos, agent_params.sigma_pos); 101 | EXPECT_DOUBLE_EQ(walker_params.sigma_ang, agent_params.sigma_ang); 102 | EXPECT_DOUBLE_EQ(walker_params.I11est, agent_params.I11est); 103 | EXPECT_DOUBLE_EQ(walker_params.I22est, agent_params.I22est); 104 | EXPECT_DOUBLE_EQ(walker_params.I33est, agent_params.I33est); 105 | EXPECT_DOUBLE_EQ(walker_params.seed, agent_params.seed); 106 | EXPECT_EQ(walker_params.step_fragment, agent_params.step_fragment); 107 | } 108 | 109 | TEST_F(TestDataGenerator, testAdd) { 110 | correct_walker_ids(data_gen); 111 | kollagen::DataGeneratorAttorney::add_walker(data_gen); 112 | ASSERT_EQ(data_gen.Walkers().size(), 1); 113 | } 114 | 115 | TEST_F(TestDataGenerator, testAdd2) { 116 | correct_walker_ids(data_gen); 117 | kollagen::DataGeneratorAttorney::add_walker(data_gen, kollagen::WalkerParams()); 118 | ASSERT_EQ(data_gen.Walkers().size(), 1); 119 | } 120 | 121 | TEST_F(TestDataGenerator, testAdd3) { 122 | correct_walker_ids(data_gen); 123 | kollagen::DataGeneratorAttorney::add_walker(data_gen, walker_param_vec); 124 | ASSERT_EQ(data_gen.Walkers().size(), 3); 125 | } 126 | 127 | TEST_F(TestDataGenerator, testAdd4) { 128 | correct_walker_ids(data_gen); 129 | kollagen::DataGeneratorAttorney::add_walker(data_gen, walker_param_vec); 130 | ASSERT_EQ(data_gen.Walkers().size(), 3); 131 | } 132 | 133 | void sort_pairs(std::vector>& pairs){ 134 | using pair_array = std::array; 135 | std::sort(pairs.begin(), pairs.end(), [](const pair_array& lhs, const pair_array& rhs){ return lhs.front() < rhs.front();}); 136 | std::stable_sort(pairs.begin(), pairs.end(), [](const pair_array& lhs, const pair_array& rhs){ return lhs.front() < rhs.front();}); 137 | } 138 | 139 | void test_pairs(std::vector>& correct, int N){ 140 | auto data_gen = kollagen::DataGeneratorAttorney::ConstructDataGenerator(N); 141 | auto pairs = kollagen::DataGeneratorAttorney::get_all_pairs(data_gen); 142 | sort_pairs(pairs); 143 | sort_pairs(correct); 144 | EXPECT_EQ(pairs.size(), correct.size()); 145 | for (int i = 0; i < static_cast(pairs.size()); ++i) { 146 | EXPECT_EQ(pairs.at(i), correct.at(i)); 147 | } 148 | } 149 | 150 | TEST_F(TestDataGenerator, testGetAllPairs2Walkers){ 151 | std::vector> correct{ 152 | {0,1} 153 | }; 154 | test_pairs(correct, 2); 155 | } 156 | 157 | TEST_F(TestDataGenerator, testGetAllPairs3Walkers){ 158 | std::vector> correct{ 159 | {0,1}, 160 | {0,2}, 161 | {1,2}, 162 | }; 163 | test_pairs(correct,3); 164 | } 165 | 166 | TEST_F(TestDataGenerator, testSaveAndLoadBinary){ 167 | kollagen::DataGenerator a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(1, 100); 168 | auto walker = a.Walkers().front(); 169 | a.save_poses_to_binary("walker"); 170 | auto x1 = kollagen::load_binary("walker1_x.bin"); 171 | auto y1 = kollagen::load_binary("walker1_y.bin"); 172 | auto theta1 = kollagen::load_binary("walker1_theta.bin"); 173 | EXPECT_EQ(x1, walker.X()); 174 | EXPECT_EQ(y1, walker.Y()); 175 | EXPECT_EQ(theta1, walker.Theta()); 176 | } 177 | 178 | TEST_F(TestDataGenerator, testSaveMultiG2O){ 179 | kollagen::DataGeneratorAttorney::add_walker(data_gen, walker_param_vec); 180 | kollagen::DataGeneratorAttorney::step(data_gen, 400); 181 | kollagen::DataGeneratorAttorney::intra_agent_loopclose(data_gen, lc_param_vec); 182 | kollagen::DataGeneratorAttorney::inter_agent_loopclose(data_gen, inter_lc_params); 183 | kollagen::DataGeneratorAttorney::set_data_has_been_generated(data_gen, true); 184 | EXPECT_EQ(data_gen.save_multig2o("multig2o"), EXIT_SUCCESS); 185 | } 186 | 187 | TEST_F(TestDataGenerator, testSaveSingleG2O){ 188 | for (auto& param : walker_param_vec) { 189 | param.step_fragment = 1; 190 | } 191 | inter_lc_params.prob_scale=1.0; 192 | inter_lc_params.radius=1.0; 193 | kollagen::DataGeneratorAttorney::add_walker(data_gen, walker_param_vec); 194 | kollagen::DataGeneratorAttorney::step(data_gen, 4); 195 | kollagen::DataGeneratorAttorney::intra_agent_loopclose(data_gen, lc_param_vec); 196 | kollagen::DataGeneratorAttorney::inter_agent_loopclose(data_gen, inter_lc_params); 197 | kollagen::DataGeneratorAttorney::set_data_has_been_generated(data_gen, true); 198 | EXPECT_EQ(data_gen.save_singleg2o("single.g2o", false), EXIT_SUCCESS); 199 | } 200 | 201 | TEST_F(TestDataGenerator, testAlign){ 202 | walker_param_vec.pop_back(); 203 | walker_param_vec.at(1).x += 1.0; 204 | kollagen::DataGenerator a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(walker_param_vec); 205 | kollagen::DataGeneratorAttorney::step_forward(a); 206 | kollagen::DataGeneratorAttorney::step_forward(a); 207 | kollagen::DataGeneratorAttorney::step_right(a); 208 | kollagen::DataGeneratorAttorney::step_left(a); 209 | 210 | kollagen::DataGeneratorAttorney::align_center_of_masses(a); 211 | 212 | double x1 = a.Walkers().at(0).X().at(0); 213 | double x2 = a.Walkers().at(1).X().at(0); 214 | 215 | double y1 = a.Walkers().at(0).Y().at(0); 216 | double y2 = a.Walkers().at(1).Y().at(0); 217 | 218 | EXPECT_DOUBLE_EQ(x1, x2); 219 | EXPECT_DOUBLE_EQ(y1, y2); 220 | } 221 | 222 | int max_seed_from_param_vec(std::vector param_vec){ 223 | kollagen::DataGenerator a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(param_vec); 224 | return kollagen::DataGeneratorAttorney::get_max_seed(a); 225 | } 226 | 227 | TEST_F(TestDataGenerator, testGetMaxSeed){ 228 | EXPECT_EQ(max_seed_from_param_vec(walker_param_vec), 3); 229 | walker_param_vec.pop_back(); 230 | 231 | EXPECT_EQ(max_seed_from_param_vec(walker_param_vec), 2); 232 | walker_param_vec.pop_back(); 233 | 234 | EXPECT_EQ(max_seed_from_param_vec(walker_param_vec), 1); 235 | 236 | walker_param_vec.at(0).seed = 2468; 237 | EXPECT_EQ(max_seed_from_param_vec(walker_param_vec), 2468); 238 | } 239 | 240 | TEST_F(TestDataGenerator, testSeedAlreadyUsed){ 241 | walker_param_vec.at(0).seed = 2468; 242 | kollagen::DataGenerator a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(walker_param_vec); 243 | EXPECT_TRUE(kollagen::DataGeneratorAttorney::seed_already_used(a, 2468)); 244 | EXPECT_TRUE(kollagen::DataGeneratorAttorney::seed_already_used(a, 2)); 245 | EXPECT_TRUE(kollagen::DataGeneratorAttorney::seed_already_used(a, 3)); 246 | } 247 | 248 | bool all_walkers_have_same_stepsize(const kollagen::DataGenerator& a){ 249 | auto walkers = a.Walkers(); 250 | auto stepsize = walkers.at(0).stepsize(); 251 | return std::ranges::all_of(walkers.cbegin(), walkers.cend(), [&stepsize](const kollagen::Walker& walker){ return walker.stepsize() == stepsize; }); 252 | } 253 | 254 | bool all_walkers_have_same_step_fragment(const kollagen::DataGenerator& a){ 255 | auto walkers = a.Walkers(); 256 | auto step_fragment = walkers.at(0).step_fragment(); 257 | return std::ranges::all_of(walkers.cbegin(), walkers.cend(), [&step_fragment](const kollagen::Walker& walker){ return walker.step_fragment() == step_fragment; }); 258 | } 259 | 260 | TEST_F(TestDataGenerator, testAddWalker){ 261 | walker_param_vec.at(0).stepsize = 1.0; 262 | walker_param_vec.at(1).stepsize = 2.5; 263 | walker_param_vec.at(2).stepsize = 1.0; 264 | walker_param_vec.at(2).step_fragment = 9; 265 | kollagen::DataGenerator a = kollagen::DataGeneratorAttorney::ConstructDataGenerator(walker_param_vec); 266 | EXPECT_TRUE(all_walkers_have_same_stepsize(a)); 267 | EXPECT_TRUE(all_walkers_have_same_step_fragment(a)); 268 | } 269 | -------------------------------------------------------------------------------- /include/kollagen/MultiG2oParse.h: -------------------------------------------------------------------------------- 1 | #ifndef MULTIG2O_PARSE_H 2 | #define MULTIG2O_PARSE_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace kollagen::multig2o 16 | { 17 | 18 | struct Vertex { 19 | Vertex(int ID, std::vector pose) : ID(ID), pose(std::move(pose)) {}; 20 | int ID; 21 | std::vector pose{}; 22 | }; 23 | 24 | struct Edge { 25 | Edge(int from, int to, std::vector transform, std::vector information_matrix) : 26 | from(from), 27 | to(to), 28 | transform(std::move(transform)), 29 | information_matrix(std::move(information_matrix)){}; 30 | int from{}; 31 | int to{}; 32 | std::vector transform{}; 33 | std::vector information_matrix{}; 34 | }; 35 | 36 | struct InterAgentLC { 37 | InterAgentLC(int from_agent, int from_index, int to_agent, int to_index, std::vector transform, std::vector information_matrix) : 38 | from_agent(from_agent), 39 | from_index(from_index), 40 | to_agent(to_agent), 41 | to_index(to_index), 42 | edge{from_index, to_index, std::move(transform), std::move(information_matrix)}{}; 43 | int from_agent{}; 44 | int from_index{}; 45 | int to_agent{}; 46 | int to_index{}; 47 | Edge edge; 48 | }; 49 | 50 | struct AgentEntries { 51 | std::vector vertices{}; 52 | std::vector odometry{}; 53 | std::vector loop_closures{}; 54 | std::vector> ground_truth{}; 55 | }; 56 | 57 | struct MultiG2OPaths{ 58 | std::vector g2o_files{}; 59 | std::vector ground_truth_files{}; 60 | std::filesystem::path inter_agent_lc{}; 61 | }; 62 | 63 | struct MultiG2O 64 | { 65 | std::vector agents{}; 66 | std::vector inter_agent_lcs{}; 67 | }; 68 | 69 | namespace helper 70 | { 71 | 72 | inline bool is_agent_directory(const std::filesystem::directory_entry& dir_entry, const std::string& agent_dir_prefix = "agent"){ 73 | std::string dir_entry_string = dir_entry.path().filename().string(); 74 | bool result = false; 75 | bool has_agent_prefix = dir_entry_string.compare(0, agent_dir_prefix.size(), agent_dir_prefix) == 0; 76 | 77 | if (!has_agent_prefix) { 78 | return false; 79 | } 80 | 81 | bool contains_g2o = std::filesystem::exists(dir_entry.path()/"posegraph.g2o"); 82 | bool contains_tum = std::filesystem::exists(dir_entry.path()/(dir_entry_string+"_GT.tum")); 83 | result = contains_g2o && contains_tum; 84 | return result; 85 | } 86 | 87 | inline std::vector get_sorted_agent_dirs(const std::filesystem::path& dir){ 88 | using std::filesystem::directory_iterator; 89 | std::vector agent_dirs{}; 90 | 91 | for (const auto& dir_entry : directory_iterator(dir)) { 92 | if (is_agent_directory(dir_entry)) { 93 | agent_dirs.emplace_back(dir_entry.path()); 94 | } 95 | } 96 | std::sort(agent_dirs.begin(), agent_dirs.end()); 97 | 98 | return agent_dirs; 99 | } 100 | 101 | inline MultiG2OPaths extract_paths(const std::filesystem::path& dir){ 102 | std::filesystem::path inter_agent_lc = dir/"inter_agent_lc.dat"; 103 | if (!std::filesystem::exists(inter_agent_lc)) { 104 | throw std::invalid_argument("multig2o is missing an inter_agent_lc.dat"); 105 | } 106 | 107 | auto agent_dirs = get_sorted_agent_dirs(dir); 108 | if (agent_dirs.empty()) { 109 | throw std::invalid_argument("Unable to find agent folders in provided multig2o"); 110 | } 111 | 112 | MultiG2OPaths paths{}; 113 | for (const auto& agent_dir : agent_dirs) { 114 | paths.g2o_files.emplace_back(agent_dir/"posegraph.g2o"); 115 | paths.ground_truth_files.emplace_back(agent_dir/(agent_dir.filename().string()+"_GT.tum")); 116 | } 117 | paths.inter_agent_lc = inter_agent_lc; 118 | 119 | return paths; 120 | } 121 | 122 | inline Vertex parse_vertex(std::ifstream& g2o, const bool& _3D){ 123 | if (_3D) { 124 | int ID; 125 | double x, y, z, qx, qy, qz, qw; 126 | g2o >> ID >> x >> y >> z >> qx >> qy >> qz >> qw; 127 | return {ID, {x, y, z, qx, qy, qz, qw}}; 128 | } 129 | int ID; 130 | double x, y, theta; 131 | g2o >> ID >> x >> y >> theta; 132 | return {ID, {x, y, theta}}; 133 | } 134 | 135 | inline bool is_loopclosure(int from, int to){ 136 | return to - from != 1; 137 | } 138 | 139 | inline std::vector parse_inter_agent_lc(std::ifstream& inter_agent_lc, const bool& _3D){ 140 | std::vector result{}; 141 | if (_3D) { 142 | int from_agent; 143 | int from_index; 144 | int to_agent; 145 | int to_index; 146 | double x, y, z, qx, qy, qz, qw; 147 | double I11, I12, I13, I14, I15, I16, 148 | I22, I23, I24, I25, I26, 149 | I33, I34, I35, I36, 150 | I44, I45, I46, 151 | I55, I56, 152 | I66; 153 | while ( 154 | inter_agent_lc >> from_agent >> from_index >> to_agent >> to_index >> x >> y >> z >> qx >> qy >> qz >> qw >> 155 | I11 >> I12 >> I13 >> I14 >> I15 >> I16 >> 156 | I22 >> I23 >> I24 >> I25 >> I26 >> 157 | I33 >> I34 >> I35 >> I36 >> 158 | I44 >> I45 >> I46 >> 159 | I55 >> I56 >> 160 | I66) { 161 | result.emplace_back(InterAgentLC{from_agent, from_index, to_agent, to_index, {x, y, z, qx, qy, qz, qw}, {I11, I12, I13, I14, I15, I16, 162 | I22, I23, I24, I25, I26, I33, I34, I35, I36, I44, I45, I46, I55, I56, I66}}); 163 | } 164 | return result; 165 | } 166 | int from_agent; 167 | int from_index; 168 | int to_agent; 169 | int to_index; 170 | double x, y, theta; 171 | double I11, I12, I13, I22, I23, I33; 172 | while ( 173 | inter_agent_lc >> from_agent >> from_index >> to_agent >> to_index >> x >> y >> theta >> 174 | I11 >> I12 >> I13 >> 175 | I22 >> I23 >> 176 | I33) { 177 | 178 | result.emplace_back(InterAgentLC{from_agent, from_index, to_agent, to_index, {x, y, theta}, {I11, I12, I13, I22, I23, I33}}); 179 | } 180 | return result; 181 | } 182 | 183 | inline std::tuple parse_edge(std::ifstream& g2o, const bool& _3D){ 184 | if (_3D) { 185 | int from; 186 | int to; 187 | double x, y, z, qx, qy, qz, qw; 188 | double I11, I12, I13, I14, I15, I16, 189 | I22, I23, I24, I25, I26, 190 | I33, I34, I35, I36, 191 | I44, I45, I46, 192 | I55, I56, 193 | I66; 194 | g2o >> from >> to >> x >> y >> z >> qx >> qy >> qz >> qw >> 195 | I11 >> I12 >> I13 >> I14 >> I15 >> I16 >> 196 | I22 >> I23 >> I24 >> I25 >> I26 >> 197 | I33 >> I34 >> I35 >> I36 >> 198 | I44 >> I45 >> I46 >> 199 | I55 >> I56 >> 200 | I66; 201 | return {{from, to, {x, y, z, qx, qy, qz, qw}, {I11, I12, I13, I14, I15, I16, 202 | I22, I23, I24, I25, I26, I33, I34, I35, I36, I44, I45, I46, I55, I56, I66 203 | }}, is_loopclosure(from, to)}; 204 | } 205 | int from; 206 | int to; 207 | double x, y, theta; 208 | double I11, I12, I13, I22, I23, I33; 209 | g2o >> from >> to >> x >> y >> theta >> 210 | I11 >> I12 >> I13 >> 211 | I22 >> I23 >> 212 | I33; 213 | return {{from, to , {x, y, theta}, {I11, I12, I13, I22, I23, I33}}, is_loopclosure(from, to)}; 214 | } 215 | 216 | inline std::tuple, std::vector, std::vector> parse_g2o( 217 | const std::filesystem::path& g2o_path, const bool& _3D 218 | ){ 219 | std::ifstream g2o(g2o_path.c_str()); 220 | if (!g2o) { 221 | throw std::invalid_argument("file not found"); 222 | } 223 | std::string type; 224 | std::string vertex_type = _3D ? "VERTEX_SE3:QUAT" : "VERTEX_SE2"; 225 | std::string edge_type = _3D ? "EDGE_SE3:QUAT" : "EDGE_SE2"; 226 | 227 | std::vector vertices{}; 228 | std::vector odometry{}; 229 | std::vector loop_closures{}; 230 | 231 | g2o >> type; 232 | if (type != vertex_type) { 233 | throw std::invalid_argument("First line of g2o should read " + vertex_type); 234 | } 235 | 236 | do { 237 | if (type == edge_type) { 238 | break; 239 | } 240 | vertices.emplace_back(parse_vertex(g2o, _3D)); 241 | 242 | } while (g2o >> type); 243 | 244 | if (type != edge_type) { 245 | throw std::invalid_argument("First line after " + vertex_type + " in g2o should read " + edge_type); 246 | } 247 | 248 | do { 249 | auto [edge, is_loopclosure] = parse_edge(g2o, _3D); 250 | 251 | if (is_loopclosure) { 252 | loop_closures.emplace_back(edge); 253 | break; 254 | } 255 | odometry.emplace_back(edge); 256 | 257 | } while (g2o >> type); 258 | 259 | while (g2o >> type){ 260 | 261 | auto [edge, is_loopclosure] = parse_edge(g2o, _3D); 262 | loop_closures.emplace_back(edge); 263 | 264 | } 265 | 266 | return {vertices, odometry, loop_closures}; 267 | } 268 | 269 | inline std::vector> parse_TUM(const std::filesystem::path& tum_path){ 270 | std::vector> result; 271 | std::ifstream gt(tum_path.c_str()); 272 | if (!gt) { 273 | throw std::invalid_argument("file not found"); 274 | } 275 | double timestamp, tx, ty, tz, qx, qy, qz, qw; 276 | while(gt >> timestamp >> tx >> ty >> tz >> qx >> qy >> qz >> qw){ 277 | result.emplace_back(std::array({timestamp, tx, ty, tz, qx, qy, qz, qw})); 278 | } 279 | return result; 280 | } 281 | 282 | inline AgentEntries get_agent_entries(const std::filesystem::path& g2o_path, const std::filesystem::path& gt_path, const bool& _3D){ 283 | AgentEntries result; 284 | 285 | result.ground_truth = parse_TUM(gt_path); 286 | std::tie(result.vertices, result.odometry, result.loop_closures) = parse_g2o(g2o_path, _3D); 287 | 288 | return result; 289 | } 290 | 291 | inline std::vector get_inter_agent_lcs(const std::filesystem::path& inter_agent_lc_path, const bool& _3D){ 292 | std::vector result{}; 293 | 294 | std::ifstream inter_agent_lc(inter_agent_lc_path.c_str()); 295 | if (!inter_agent_lc) { 296 | throw std::invalid_argument("file not found"); 297 | } 298 | 299 | result = parse_inter_agent_lc(inter_agent_lc, _3D); 300 | 301 | return result; 302 | } 303 | 304 | 305 | inline MultiG2O parse(const MultiG2OPaths& paths, const bool& _3D){ 306 | MultiG2O multi_g2o{}; 307 | for (int i = 0; i < static_cast(paths.g2o_files.size()); ++i) { 308 | auto g2o_path = paths.g2o_files.at(i); 309 | auto gt_path = paths.ground_truth_files.at(i); 310 | multi_g2o.agents.emplace_back(get_agent_entries(g2o_path, gt_path, _3D)); 311 | } 312 | multi_g2o.inter_agent_lcs = get_inter_agent_lcs(paths.inter_agent_lc, _3D); 313 | return multi_g2o; 314 | } 315 | 316 | } // namespace helper 317 | 318 | inline MultiG2O open(const std::string& dirname, const bool& _3D = true){ 319 | std::filesystem::path path(dirname); 320 | MultiG2OPaths paths = helper::extract_paths(path); 321 | MultiG2O data = helper::parse(paths, _3D); 322 | return data; 323 | } 324 | 325 | } // namespace kollagen::multig2o 326 | #endif /* !MULTIG2O_PARSE_H */ 327 | -------------------------------------------------------------------------------- /include/kollagen/DataGenerator.h: -------------------------------------------------------------------------------- 1 | #include "DataGeneratorParams.h" 2 | #include "Functions.h" 3 | #include "Walker-impl.h" 4 | #include "Walker.h" 5 | 6 | #include 7 | 8 | #ifndef DATAGENERATOR_H 9 | #define DATAGENERATOR_H 10 | 11 | namespace kollagen 12 | { 13 | 14 | /** 15 | * \brief Data Generator. 16 | * Can generates and save data given a set of parameters. 17 | */ 18 | 19 | class DataGenerator { 20 | public: 21 | DataGenerator() = default; 22 | explicit DataGenerator(DataGeneratorParams params); 23 | 24 | /*! \brief Set parameters of Data Generator 25 | * 26 | * For use with the default constructor. Parameters can only be set once. 27 | * 28 | * \param params Data Generator Parameters 29 | * \return EXIT_FAILURE or EXIT_SUCCESS 30 | */ 31 | int set_params(DataGeneratorParams ¶ms); 32 | 33 | /*! \brief Generate the data 34 | * 35 | * Can only be run given that the parameters have ben set. Can only be run once. 36 | * 37 | * \return EXIT_FAILURE or EXIT_SUCCESS 38 | */ 39 | int generate(); 40 | 41 | /*! \brief Get const access to agents 42 | */ 43 | [[nodiscard]] std::vector Walkers() const { return walkers_; }; 44 | /*! \brief Get const access to inter-agent loop-closures 45 | */ 46 | [[nodiscard]] std::vector LCs() const { return LCs_; }; 47 | 48 | /*! \brief Save poses of agents in binary format 49 | * 50 | * Separate files for each agent and for each state (x, y, theta). Three 51 | * files are thus generated per agent, and named "