From b39801b63807b544bfe1e9eaa9afe5bf92c36be7 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Sat, 2 Nov 2024 10:53:10 -0400 Subject: [PATCH 01/13] Start build refactor --- .vscode/c_cpp_properties.json | 2 +- Makefile | 92 ++++++++++++----------------- {src => include}/algo/kdtree.h | 0 {src => include}/algo/quickselect.h | 0 {src => include}/icp/geo.h | 0 {src => include}/icp/icp.h | 0 {src => lib}/algo/kdtree.cpp | 0 {src => lib}/algo/quickselect.cpp | 0 {src => lib}/icp/geo.cpp | 2 +- {src => lib}/icp/icp.cpp | 2 +- {src => lib}/icp/impl/trimmed.cpp | 3 +- {src => lib}/icp/impl/vanilla.cpp | 3 +- main.cpp => src/main.cpp | 0 test.cpp => test/test.cpp | 0 14 files changed, 44 insertions(+), 60 deletions(-) rename {src => include}/algo/kdtree.h (100%) rename {src => include}/algo/quickselect.h (100%) rename {src => include}/icp/geo.h (100%) rename {src => include}/icp/icp.h (100%) rename {src => lib}/algo/kdtree.cpp (100%) rename {src => lib}/algo/quickselect.cpp (100%) rename {src => lib}/icp/geo.cpp (94%) rename {src => lib}/icp/icp.cpp (99%) rename {src => lib}/icp/impl/trimmed.cpp (99%) rename {src => lib}/icp/impl/vanilla.cpp (99%) rename main.cpp => src/main.cpp (100%) rename test.cpp => test/test.cpp (100%) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index eb60e6b..387ae49 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -17,7 +17,7 @@ "name": "Linux", "includePath": [ "${default}", - "${workspaceFolder}/**", + "${workspaceFolder}/include", "/usr/include/SDL2", "/usr/local/include/eigen3", "/usr/local/include/sdlwrapper", diff --git a/Makefile b/Makefile index c1c3c2c..2c72ed5 100644 --- a/Makefile +++ b/Makefile @@ -1,73 +1,66 @@ # Copyright (C) 2023 Ethan Uppal. All rights reserved. +INCLUDEDIR := include +LIBDIR := lib SRCDIR := src -INCLUDEDIR := src +TESTDIR := test CC := $(shell which g++ || which clang++) PY := $(shell which python3 || which python) CFLAGS := -std=c++17 -pedantic -Wall -Wextra -I $(INCLUDEDIR) CDEBUG := -g CRELEASE := -O3 -DRELEASE_BUILD -TARGET := main LIBNAME := libcevicp.a - -# follow instructions in README to install in /usr/local -LDFLAGS := $(shell sdl2-config --libs) \ - /usr/local/lib/libcmdapp.a \ - /usr/local/lib/libsdlwrapper.a \ - /usr/local/lib/libconfig.a -CFLAGS += $(shell sdl2-config --cflags) \ - -I/usr/local/include/cmdapp \ - -I/usr/local/include/config \ - -I/usr/local/include/sdlwrapper \ - -I/usr/local/include/simple_test \ - -I/usr/local/include/eigen3 +MAINNAME := main +TESTNAME := test # CFLAGS += $(CRELEASE) CFLAGS += $(CDEBUG) -SRC := $(shell find $(SRCDIR) -name "*.cpp") -OBJ := $(SRC:.cpp=.o) -DEPS := $(OBJS:.o=.d) - - -LIBSRC := $(shell find $(SRCDIR)/icp -name "*.cpp" -type f) \ - $(shell find $(SRCDIR)/algo -name "*.cpp" -type f) +LIBSRC := $(shell find $(LIBDIR) -name "*.cpp" -type f) +LIBINCLUDE := -I/usr/local/include/eigen3 LIBOBJ := $(LIBSRC:.cpp=.o) +LIBDEPS := $(LIBOBJ:.o=.d) --include $(DEPS) +MAINSRC := $(shell find $(SRCDIR) -name "*.cpp" -type f) +MAININCLUDE := $(shell sdl2-config --cflags) \ + -I/usr/local/include/eigen3 \ + -I/usr/local/include/cmdapp \ + -I/usr/local/include/config \ + -I/usr/local/include/sdlwrapper +MAINLD := $(shell sdl2-config --libs) \ + /usr/local/lib/libcmdapp.a \ + /usr/local/lib/libsdlwrapper.a \ + /usr/local/lib/libconfig.a +MAINOBJ := $(MAINSRC:.cpp=.o) +MAINDEPS := $(MAINOBJ:.o=.d) -# Config parameters -N := 1 -METHOD := trimmed +$(LIBNAME): CFLAGS += $(LIBINCLUDE) +$(MAINNAME): CFLAGS += $(MAININCLUDE) -$(TARGET): main.cpp $(OBJ) - $(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ - @make readme +$(MAINNAME): LDFLAGS += $(MAINLD) + +-include $(LIBDEPS) +-include $(MAINDEPS) -.PHONY: test -test: test.cpp $(OBJ) - @$(CC) $(CFLAGS) -DTEST -o _temp $^ $(LDFLAGS) - @echo 'Running tests...' - @./_temp - @rm -f ./_temp +ifeq ($(shell uname), Darwin) +AR := /usr/bin/libtool +AR_OPT := -static +else +AR := ar +AR_OPT := rcs $@ $^ +endif -.PHONY: view -view: $(TARGET) - ./$(TARGET) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --gui +$(LIBNAME): $(LIBOBJ) + $(AR) $(AR_OPT) -o $@ $^ -.PHONY: bench -bench: $(TARGET) - ./$(TARGET) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --bench +$(MAINNAME): $(MAINOBJ) $(LIBNAME) + $(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ %.o: %.cpp @echo 'Compiling $@' $(CC) $(CFLAGS) -MMD -MP $< -c -o $@ -.PHONY: clean -clean: - rm -rf $(OBJ) $(TARGET) $(TARGET) $(DEPS) $(shell find . -name "*.dSYM") $(shell find . -name "*.d") docs - # Not building book rn, add these commands to build # cd book; \ pdflatex icp.tex; \ @@ -91,14 +84,3 @@ readme: .PHONY: math math: @cd math; $(PY) ./icp_math.py - -ifeq ($(shell uname), Darwin) -AR := /usr/bin/libtool -AR_OPT := -static -else -AR := ar -AR_OPT := rcs $@ $^ -endif - -$(LIBNAME): $(LIBOBJ) - $(AR) $(AR_OPT) $^ -o $@ diff --git a/src/algo/kdtree.h b/include/algo/kdtree.h similarity index 100% rename from src/algo/kdtree.h rename to include/algo/kdtree.h diff --git a/src/algo/quickselect.h b/include/algo/quickselect.h similarity index 100% rename from src/algo/quickselect.h rename to include/algo/quickselect.h diff --git a/src/icp/geo.h b/include/icp/geo.h similarity index 100% rename from src/icp/geo.h rename to include/icp/geo.h diff --git a/src/icp/icp.h b/include/icp/icp.h similarity index 100% rename from src/icp/icp.h rename to include/icp/icp.h diff --git a/src/algo/kdtree.cpp b/lib/algo/kdtree.cpp similarity index 100% rename from src/algo/kdtree.cpp rename to lib/algo/kdtree.cpp diff --git a/src/algo/quickselect.cpp b/lib/algo/quickselect.cpp similarity index 100% rename from src/algo/quickselect.cpp rename to lib/algo/quickselect.cpp diff --git a/src/icp/geo.cpp b/lib/icp/geo.cpp similarity index 94% rename from src/icp/geo.cpp rename to lib/icp/geo.cpp index f8818f0..7ac4cb3 100644 --- a/src/icp/geo.cpp +++ b/lib/icp/geo.cpp @@ -4,7 +4,7 @@ */ #include -#include "geo.h" +#include "icp/geo.h" namespace icp { Vector get_centroid(const std::vector& points) { diff --git a/src/icp/icp.cpp b/lib/icp/icp.cpp similarity index 99% rename from src/icp/icp.cpp rename to lib/icp/icp.cpp index b5b2eef..e15fb39 100644 --- a/src/icp/icp.cpp +++ b/lib/icp/icp.cpp @@ -4,7 +4,7 @@ */ #include -#include "icp.h" +#include "icp/icp.h" namespace icp { static Methods* global; diff --git a/src/icp/impl/trimmed.cpp b/lib/icp/impl/trimmed.cpp similarity index 99% rename from src/icp/impl/trimmed.cpp rename to lib/icp/impl/trimmed.cpp index 507e522..195c2d2 100644 --- a/src/icp/impl/trimmed.cpp +++ b/lib/icp/impl/trimmed.cpp @@ -5,11 +5,12 @@ #include #include -#include "../icp.h" #include #include #include +#include "icp/icp.h" + /* #name Trimmed */ /* #desc Trimmed ICP is identical to \ref vanilla_icp with the addition of an diff --git a/src/icp/impl/vanilla.cpp b/lib/icp/impl/vanilla.cpp similarity index 99% rename from src/icp/impl/vanilla.cpp rename to lib/icp/impl/vanilla.cpp index 45afbb4..9005c91 100644 --- a/src/icp/impl/vanilla.cpp +++ b/lib/icp/impl/vanilla.cpp @@ -5,11 +5,12 @@ #include #include -#include "../icp.h" #include #include #include +#include "icp/icp.h" + /* #name Vanilla */ /* #desc The vanilla algorithm for ICP will match the point-cloud centers diff --git a/main.cpp b/src/main.cpp similarity index 100% rename from main.cpp rename to src/main.cpp diff --git a/test.cpp b/test/test.cpp similarity index 100% rename from test.cpp rename to test/test.cpp From 3bdc511101d686cdca56a5472f06d1068352c97d Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Sat, 2 Nov 2024 14:27:42 -0400 Subject: [PATCH 02/13] Main actually works without segfaulting now. Had to remove the nice static initialization though. --- .vscode/launch.json | 34 +++++++ .vscode/settings.json | 5 +- .vscode/tasks.json | 17 ++++ Makefile | 4 + include/icp/icp.h | 24 +++-- include/icp/impl/trimmed.h | 22 +++++ include/icp/impl/vanilla.h | 23 +++++ lib/icp/icp.cpp | 39 ++++++-- lib/icp/impl/trimmed.cpp | 155 ++++++++++++++---------------- lib/icp/impl/vanilla.cpp | 192 +++++++++++++++++-------------------- src/main.cpp | 18 +++- src/sim/lidar_view.cpp | 6 +- src/sim/lidar_view.h | 2 +- 13 files changed, 322 insertions(+), 219 deletions(-) create mode 100644 .vscode/launch.json create mode 100644 .vscode/tasks.json create mode 100644 include/icp/impl/trimmed.h create mode 100644 include/icp/impl/vanilla.h diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..b479799 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,34 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "Debug with gdb on linux", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/main", + "args": [ + "-S", + "ex_data/scan1/first.conf", + "-D", + "ex_data/scan1/second.conf", + "--method", + "vanilla", + "--gui" + ], + "stopAtEntry": false, + "cwd": "${workspaceFolder}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "miDebuggerPath": "/usr/bin/gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ], + "preLaunchTask": "Build main with make" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 0457654..069e832 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -106,6 +106,9 @@ "stop_token": "cpp", "svd": "cpp", "core": "cpp", - "text_encoding": "cpp" + "text_encoding": "cpp", + "strstream": "cpp", + "source_location": "cpp", + "stdfloat": "cpp" } } \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..842904d --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,17 @@ +{ + "version": "2.0.0", + "tasks": [ + { + "label": "Build main with make", + "type": "shell", + "command": "make main", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [ + "$gcc" + ] + } + ] +} \ No newline at end of file diff --git a/Makefile b/Makefile index 2c72ed5..cb39bc8 100644 --- a/Makefile +++ b/Makefile @@ -61,6 +61,10 @@ $(MAINNAME): $(MAINOBJ) $(LIBNAME) @echo 'Compiling $@' $(CC) $(CFLAGS) -MMD -MP $< -c -o $@ +.PHONY: clean +clean: + @rm -f $(LIBOBJ) $(LIBDEPS) $(LIBNAME) $(MAINOBJ) $(MAINDEPS) $(MAINNAME) + # Not building book rn, add these commands to build # cd book; \ pdflatex icp.tex; \ diff --git a/include/icp/icp.h b/include/icp/icp.h index 8e785f1..f82c4e2 100644 --- a/include/icp/icp.h +++ b/include/icp/icp.h @@ -12,6 +12,8 @@ #include #include #include +#include + #include "geo.h" namespace icp { @@ -57,10 +59,10 @@ namespace icp { */ RBTransform transform; - /** The source point cloud relative to its centroid. */ + /** The source point cloud. */ std::vector a; - /** The destination point cloud relative to its centroid. */ + /** The destination point cloud. */ std::vector b; /** Keeps track of the previous cost to ensure that progress is being @@ -150,6 +152,10 @@ namespace icp { /** The current transform. */ const RBTransform& current_transform() const; + /** Registers methods built into libcevicp. Must be called before constructing ICP instances + * for built-in methods. */ + static void register_builtin_methods(); + /** Registers a new ICP method that can be created with `constructor`, * returning `false` if `name` has already been registered. */ static bool register_method(std::string name, @@ -166,15 +172,7 @@ namespace icp { * @pre `name` is a valid registered method. See * ICP::is_registered_method. */ - static std::unique_ptr from_method(std::string name, const Config& params = Config()); - - /** Whether `name` is a registered ICP method. */ - static bool is_registered_method(std::string name); - }; - - struct Methods { - std::vector registered_method_names; - std::vector(const ICP::Config&)>> - registered_method_constructors; + static std::optional> from_method(std::string name, + const Config& params = Config()); }; -} +} \ No newline at end of file diff --git a/include/icp/impl/trimmed.h b/include/icp/impl/trimmed.h new file mode 100644 index 0000000..8291a54 --- /dev/null +++ b/include/icp/impl/trimmed.h @@ -0,0 +1,22 @@ +/* + * @author Utku Melemetci + */ + +#include "icp/icp.h" + +namespace icp { + class Trimmed final : public ICP { + public: + Trimmed(double overlap_rate); + Trimmed(const Config& config); + ~Trimmed(); + + void setup() override; + void iterate() override; + + private: + double overlap_rate; + std::vector a_current; + icp::Vector b_cm; + }; +} diff --git a/include/icp/impl/vanilla.h b/include/icp/impl/vanilla.h new file mode 100644 index 0000000..8bb8063 --- /dev/null +++ b/include/icp/impl/vanilla.h @@ -0,0 +1,23 @@ +// TODO: we should probably find a good solution for CEV copyright + +/* + * @author Utku Melemetci + */ + +#include "icp/icp.h" + +namespace icp { + class Vanilla final : public ICP { + public: + Vanilla(); + Vanilla(const Config& config); + ~Vanilla(); + + void setup() override; + void iterate() override; + + private: + std::vector a_current; + icp::Vector b_cm; + }; +} \ No newline at end of file diff --git a/lib/icp/icp.cpp b/lib/icp/icp.cpp index e15fb39..bf9603a 100644 --- a/lib/icp/icp.cpp +++ b/lib/icp/icp.cpp @@ -4,9 +4,20 @@ */ #include + #include "icp/icp.h" +// methods for builtin registration +#include "icp/impl/vanilla.h" +#include "icp/impl/trimmed.h" + namespace icp { + struct Methods { + std::vector registered_method_names; + std::vector(const ICP::Config&)>> + registered_method_constructors; + }; + static Methods* global; ICP::ICP() {} @@ -82,6 +93,13 @@ namespace icp { } } + void ICP::register_builtin_methods() { + register_method("vanilla", + [](const ICP::Config& config) { return std::make_unique(config); }); + register_method("trimmed", + [](const ICP::Config& config) { return std::make_unique(config); }); + } + bool ICP::register_method(std::string name, std::function(const ICP::Config&)> constructor) { ensure_methods_exists(); @@ -95,17 +113,18 @@ namespace icp { return global->registered_method_names; } - std::unique_ptr ICP::from_method(std::string name, const ICP::Config& config) { + std::optional> ICP::from_method(std::string name, + const ICP::Config& config) { ensure_methods_exists(); - size_t index = std::find(global->registered_method_names.begin(), - global->registered_method_names.end(), name) - - global->registered_method_names.begin(); - return global->registered_method_constructors[index](config); - } + auto name_it = std::find(global->registered_method_names.begin(), + global->registered_method_names.end(), name); + + if (name_it == global->registered_method_names.end()) { + return {}; + } - bool ICP::is_registered_method(std::string name) { - return std::find(global->registered_method_names.begin(), - global->registered_method_names.end(), name) - != global->registered_method_names.end(); + size_t index = name_it - global->registered_method_names.begin(); + + return global->registered_method_constructors[index](config); } } diff --git a/lib/icp/impl/trimmed.cpp b/lib/icp/impl/trimmed.cpp index 195c2d2..85f1557 100644 --- a/lib/icp/impl/trimmed.cpp +++ b/lib/icp/impl/trimmed.cpp @@ -9,7 +9,9 @@ #include #include -#include "icp/icp.h" +#include "icp/impl/trimmed.h" + +// TODO: should these docs be moved to the headers? /* #name Trimmed */ @@ -19,102 +21,87 @@ point sets that have correspondences. When the overlap rate is 1, the algorithm reduces to vanilla. */ namespace icp { - struct Trimmed final : public ICP { - double overlap_rate; - std::vector a_current; - icp::Vector b_cm; - - Trimmed(double overlap_rate): ICP(), overlap_rate(overlap_rate) {} - ~Trimmed() override {} - void setup() override { - if (a_current.size() < a.size()) { - a_current.resize(a.size()); - } + Trimmed::Trimmed(double overlap_rate): ICP(), overlap_rate(overlap_rate) {} + Trimmed::Trimmed(const Config& config) + : ICP(), overlap_rate(config.get("overlap_rate", 0.7)) {} + Trimmed::~Trimmed() {} - b_cm = get_centroid(b); + void Trimmed::setup() { + if (a_current.size() < a.size()) { + a_current.resize(a.size()); } - void iterate() override { - const size_t n = a.size(); - const size_t m = b.size(); + b_cm = get_centroid(b); + } - for (size_t i = 0; i < n; i++) { - a_current[i] = transform.apply_to(a[i]); - } + void Trimmed::iterate() { + const size_t n = a.size(); + const size_t m = b.size(); - /* #step Matching Step: see \ref vanilla_icp for details. */ - for (size_t i = 0; i < n; i++) { - matches[i].point = i; - matches[i].sq_dist = std::numeric_limits::infinity(); - for (size_t j = 0; j < m; j++) { - // Point-to-point matching - double dist_ij = (b[j] - a_current[i]).squaredNorm(); - - if (dist_ij < matches[i].sq_dist) { - matches[i].sq_dist = dist_ij; - matches[i].pair = j; - } + for (size_t i = 0; i < n; i++) { + a_current[i] = transform.apply_to(a[i]); + } + + /* #step Matching Step: see \ref vanilla_icp for details. */ + for (size_t i = 0; i < n; i++) { + matches[i].point = i; + matches[i].sq_dist = std::numeric_limits::infinity(); + for (size_t j = 0; j < m; j++) { + // Point-to-point matching + double dist_ij = (b[j] - a_current[i]).squaredNorm(); + + if (dist_ij < matches[i].sq_dist) { + matches[i].sq_dist = dist_ij; + matches[i].pair = j; } } + } - /* - #step - Trimming Step - - Matches are considered in increasing order of distance. - - Sources: - https://ieeexplore.ieee.org/abstract/document/1047997 - */ - std::sort(matches.begin(), matches.end(), - [](const auto& a, const auto& b) { return a.sq_dist < b.sq_dist; }); - size_t new_n = (size_t)(overlap_rate * n); - - // yeah, i know this is inefficient. we'll get back to it later. - std::vector trimmed_current(new_n); - std::vector trimmed_b(new_n); - for (size_t i = 0; i < new_n; i++) { - trimmed_current[i] = a_current[matches[i].point]; - trimmed_b[i] = b[matches[i].point]; - } + /* + #step + Trimming Step + + Matches are considered in increasing order of distance. + + Sources: + https://ieeexplore.ieee.org/abstract/document/1047997 + */ + std::sort(matches.begin(), matches.end(), + [](const auto& a, const auto& b) { return a.sq_dist < b.sq_dist; }); + size_t new_n = (size_t)(overlap_rate * n); + + // yeah, i know this is inefficient. we'll get back to it later. + std::vector trimmed_current(new_n); + std::vector trimmed_b(new_n); + for (size_t i = 0; i < new_n; i++) { + trimmed_current[i] = a_current[matches[i].point]; + trimmed_b[i] = b[matches[i].point]; + } - icp::Vector trimmed_cm = get_centroid(trimmed_current); - icp::Vector trimmed_b_cm = get_centroid(trimmed_b); + icp::Vector trimmed_cm = get_centroid(trimmed_current); + icp::Vector trimmed_b_cm = get_centroid(trimmed_b); - /* #step SVD: see \ref vanilla_icp for details. */ - Matrix N{}; - for (size_t i = 0; i < new_n; i++) { - N += (trimmed_current[i] - trimmed_cm) * (trimmed_b[i] - trimmed_b_cm).transpose(); - } + /* #step SVD: see \ref vanilla_icp for details. */ + Matrix N{}; + for (size_t i = 0; i < new_n; i++) { + N += (trimmed_current[i] - trimmed_cm) * (trimmed_b[i] - trimmed_b_cm).transpose(); + } - auto svd = N.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); - Matrix U = svd.matrixU(); - Matrix V = svd.matrixV(); - Matrix R = V * U.transpose(); + auto svd = N.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); + Matrix U = svd.matrixU(); + Matrix V = svd.matrixV(); + Matrix R = V * U.transpose(); - /* #step Reflection Handling: see \ref vanilla_icp for details. */ - if (R.determinant() < 0) { - V = V * Eigen::DiagonalMatrix(1, -1); - R = V * U.transpose(); - } + /* #step Reflection Handling: see \ref vanilla_icp for details. */ + if (R.determinant() < 0) { + V = V * Eigen::DiagonalMatrix(1, -1); + R = V * U.transpose(); + } - transform.rotation = R * transform.rotation; + transform.rotation = R * transform.rotation; - /* #step Transformation Step: see \ref vanilla_icp for details. */ - transform.translation += trimmed_b_cm - R * trimmed_cm; - } - }; - - static bool static_initialization = []() { - assert(ICP::register_method("trimmed", - [](const ICP::Config& config) -> std::unique_ptr { - /* #conf "overlap_rate" A `double` between `0.0` and `1.0` for - * the overlap rate. The default is `1.0`. */ - double overlap_rate = config.get("overlap_rate", 1.0); - assert(overlap_rate >= 0 && overlap_rate <= 1); - return std::make_unique(overlap_rate); - })); - return true; - }(); + /* #step Transformation Step: see \ref vanilla_icp for details. */ + transform.translation += trimmed_b_cm - R * trimmed_cm; + } } diff --git a/lib/icp/impl/vanilla.cpp b/lib/icp/impl/vanilla.cpp index 9005c91..f8dbcad 100644 --- a/lib/icp/impl/vanilla.cpp +++ b/lib/icp/impl/vanilla.cpp @@ -9,7 +9,7 @@ #include #include -#include "icp/icp.h" +#include "icp/impl/vanilla.h" /* #name Vanilla */ @@ -17,119 +17,107 @@ exactly and then iterate until an optimal rotation has been found. */ namespace icp { - struct Vanilla final : public ICP { - std::vector a_current; - icp::Vector b_cm; + Vanilla::Vanilla(const Config& config): ICP() {} + Vanilla::Vanilla(): ICP() {} + Vanilla::~Vanilla() {} - Vanilla(): ICP() {} - ~Vanilla() override {} - - void setup() override { - if (a_current.size() < a.size()) { - a_current.resize(a.size()); - } - - b_cm = get_centroid(b); + void Vanilla::setup() { + if (a_current.size() < a.size()) { + a_current.resize(a.size()); } - void iterate() override { - const size_t n = a.size(); - const size_t m = b.size(); + b_cm = get_centroid(b); + } - for (size_t i = 0; i < n; i++) { - a_current[i] = transform.apply_to(a[i]); - } + void Vanilla::iterate() { + const size_t n = a.size(); + const size_t m = b.size(); + + for (size_t i = 0; i < n; i++) { + a_current[i] = transform.apply_to(a[i]); + } - // can optimize by just transforming prev centroid if necessary - auto a_current_cm = get_centroid(a_current); - - /* - #step - Matching Step: match closest points. - - Sources: - https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 - https://arxiv.org/pdf/2206.06435.pdf - https://web.archive.org/web/20220615080318/https://www.cs.technion.ac.il/~cs236329/tutorials/ICP.pdf - https://en.wikipedia.org/wiki/Iterative_closest_point - https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf - -> use k-d tree - */ - for (size_t i = 0; i < n; i++) { - matches[i].sq_dist = std::numeric_limits::infinity(); - for (size_t j = 0; j < m; j++) { - // Point-to-point matching - double dist_ij = (b[j] - a_current[i]).squaredNorm(); - - if (dist_ij < matches[i].sq_dist) { - matches[i].sq_dist = dist_ij; - matches[i].pair = j; - } + // can optimize by just transforming prev centroid if necessary + auto a_current_cm = get_centroid(a_current); + + /* + #step + Matching Step: match closest points. + + Sources: + https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 + https://arxiv.org/pdf/2206.06435.pdf + https://web.archive.org/web/20220615080318/https://www.cs.technion.ac.il/~cs236329/tutorials/ICP.pdf + https://en.wikipedia.org/wiki/Iterative_closest_point + https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf + -> use k-d tree + */ + for (size_t i = 0; i < n; i++) { + matches[i].sq_dist = std::numeric_limits::infinity(); + for (size_t j = 0; j < m; j++) { + // Point-to-point matching + double dist_ij = (b[j] - a_current[i]).squaredNorm(); + + if (dist_ij < matches[i].sq_dist) { + matches[i].sq_dist = dist_ij; + matches[i].pair = j; } } + } - /* - #step - SVD + /* + #step + SVD - We compute the SVD of this magical matrix. A proof that this yields the optimal - transform R is in the source below. + We compute the SVD of this magical matrix. A proof that this yields the optimal + transform R is in the source below. - Sources: - https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 - */ - Matrix N{}; - for (size_t i = 0; i < n; i++) { - N += (a_current[i] - a_current_cm) * (b[matches[i].pair] - b_cm).transpose(); - } - auto svd = N.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); - const Matrix U = svd.matrixU(); - Matrix V = svd.matrixV(); - Matrix R = V * U.transpose(); - - /* - #step - Reflection Handling - - SVD may return a reflection instead of a rotation if it's equally good or better. - This is exceedingly rare with real data but may happen in very high noise - environment with sparse point cloud. - - In the 2D case, we can always recover a reasonable rotation by negating the last - column of V. I do not know if this is the optimal rotation among rotations, but - we can probably get an answer with more effort. - - Sources: - https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 - */ - if (R.determinant() < 0) { - V = V * Eigen::DiagonalMatrix(1, -1); - R = V * U.transpose(); - } + Sources: + https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 + */ + Matrix N{}; + for (size_t i = 0; i < n; i++) { + N += (a_current[i] - a_current_cm) * (b[matches[i].pair] - b_cm).transpose(); + } + auto svd = N.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); + const Matrix U = svd.matrixU(); + Matrix V = svd.matrixV(); + Matrix R = V * U.transpose(); + + /* + #step + Reflection Handling + + SVD may return a reflection instead of a rotation if it's equally good or better. + This is exceedingly rare with real data but may happen in very high noise + environment with sparse point cloud. + + In the 2D case, we can always recover a reasonable rotation by negating the last + column of V. I do not know if this is the optimal rotation among rotations, but + we can probably get an answer to that question with more effort. + + Sources: + https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 + */ + if (R.determinant() < 0) { + V = V * Eigen::DiagonalMatrix(1, -1); + R = V * U.transpose(); + } - transform.rotation = R * transform.rotation; + transform.rotation = R * transform.rotation; - /* - #step - Transformation Step: determine optimal transformation. + /* + #step + Transformation Step: determine optimal transformation. - The translation vector is determined by the displacement between - the centroids of both point clouds. The rotation matrix is - calculated via singular value decomposition. + The translation vector is determined by the displacement between + the centroids of both point clouds. The rotation matrix is + calculated via singular value decomposition. - Sources: - https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 - https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf - */ - transform.translation += b_cm - R * a_current_cm; - } - }; - - static bool static_initialization = []() { - assert(ICP::register_method("vanilla", - []([[maybe_unused]] const ICP::Config& config) -> std::unique_ptr { - return std::make_unique(); - })); - return true; - }(); + Sources: + https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 + https://courses.cs.duke.edu/spring07/cps296.2/scribe_notes/lecture24.pdf + */ + transform.translation += b_cm - R * a_current_cm; + } } diff --git a/src/main.cpp b/src/main.cpp index 1a9df2f..b4a6e22 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,8 +10,11 @@ extern "C" { #include #include #include +#include #include "sim/view_config.h" #include "sim/lidar_view.h" +#include "icp/impl/vanilla.h" +#include "icp/impl/trimmed.h" struct LidarScan { double range_max; @@ -94,10 +97,10 @@ void launch_gui(LidarView* view, std::string visualized = "LiDAR scans") { window.present(); } -void run_benchmark(const char* method, const LidarScan& source, const LidarScan& destination) { +void run_benchmark(const char* method, std::unique_ptr icp, const LidarScan& source, + const LidarScan& destination) { std::cout << "ICP ALGORITHM BENCHMARKING\n"; std::cout << "=======================================\n"; - std::unique_ptr icp = icp::ICP::from_method(method); constexpr size_t N = 50; constexpr size_t burn_in = 0; @@ -204,7 +207,10 @@ int main(int argc, const char** argv) { view_config::use_light_background = true; } - if (!icp::ICP::is_registered_method(method)) { + icp::ICP::register_builtin_methods(); + std::optional> icp_opt = icp::ICP::from_method(method); + + if (!icp_opt.has_value()) { std::cerr << "error: unknown ICP method '" << method << "'. expected one of:\n"; for (const std::string& registered_method: icp::ICP::registered_methods()) { std::cerr << "* " << registered_method << '\n'; @@ -212,6 +218,8 @@ int main(int argc, const char** argv) { std::exit(1); } + std::unique_ptr icp = std::move(icp_opt.value()); + // std::vector a = {icp::Vector(100, 200), icp::Vector(130, 420), // icp::Vector(-100, -200), icp::Vector(-50, -100)}; // std::vector b = {icp::Vector(100, -200), icp::Vector(130, -420), @@ -229,11 +237,11 @@ int main(int argc, const char** argv) { if (*use_gui) { icp::ICP::Config config; config.set("overlap_rate", 0.9); - LidarView* view = new LidarView(source.points, destination.points, method, config); + LidarView* view = new LidarView(source.points, destination.points, std::move(icp)); launch_gui(view, std::string(f_src) + std::string(" and ") + std::string(f_dst)); } else if (*do_bench) { - run_benchmark(method, source, destination); + run_benchmark(method, std::move(icp), source, destination); } } } diff --git a/src/sim/lidar_view.cpp b/src/sim/lidar_view.cpp index f541cc5..9ddd6f8 100644 --- a/src/sim/lidar_view.cpp +++ b/src/sim/lidar_view.cpp @@ -15,14 +15,14 @@ #define CIRCLE_RADIUS 3 LidarView::LidarView(std::vector source, std::vector destination, - const std::string method, const icp::ICP::Config& config) + std::unique_ptr icp) : source(source), destination(destination), + icp(std::move(icp)), keyboard(false), is_iterating(false), iterations(0) { - icp = icp::ICP::from_method(method, config); - icp->begin(source, destination, icp::RBTransform()); + this->icp->begin(source, destination, icp::RBTransform()); } LidarView::~LidarView() noexcept { diff --git a/src/sim/lidar_view.h b/src/sim/lidar_view.h index d26c608..da225e8 100644 --- a/src/sim/lidar_view.h +++ b/src/sim/lidar_view.h @@ -25,7 +25,7 @@ class LidarView final : public View { /** Constructs a new lidar view visualizing ICP (by method `method`) on * the given instance (`source` and `destination`). */ LidarView(std::vector source, std::vector destination, - const std::string method, const icp::ICP::Config& config = icp::ICP::Config()); + std::unique_ptr icp); ~LidarView() noexcept override; void on_event(const SDL_Event& event) override; From 421b959ad92100ac502ce7776e173d21d70cf9bf Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 19:50:30 -0500 Subject: [PATCH 03/13] Add test to Makefile --- Makefile | 20 ++++++++++++++++---- install/Makefile | 1 + lib/icp/impl/trimmed.cpp | 3 ++- {test => tests}/test.cpp | 4 +++- 4 files changed, 22 insertions(+), 6 deletions(-) rename {test => tests}/test.cpp (98%) diff --git a/Makefile b/Makefile index cb39bc8..5c4a8a7 100644 --- a/Makefile +++ b/Makefile @@ -3,7 +3,7 @@ INCLUDEDIR := include LIBDIR := lib SRCDIR := src -TESTDIR := test +TESTDIR := tests CC := $(shell which g++ || which clang++) PY := $(shell which python3 || which python) @@ -21,6 +21,7 @@ LIBSRC := $(shell find $(LIBDIR) -name "*.cpp" -type f) LIBINCLUDE := -I/usr/local/include/eigen3 LIBOBJ := $(LIBSRC:.cpp=.o) LIBDEPS := $(LIBOBJ:.o=.d) +$(LIBNAME): CFLAGS += $(LIBINCLUDE) MAINSRC := $(shell find $(SRCDIR) -name "*.cpp" -type f) MAININCLUDE := $(shell sdl2-config --cflags) \ @@ -34,14 +35,20 @@ MAINLD := $(shell sdl2-config --libs) \ /usr/local/lib/libconfig.a MAINOBJ := $(MAINSRC:.cpp=.o) MAINDEPS := $(MAINOBJ:.o=.d) - -$(LIBNAME): CFLAGS += $(LIBINCLUDE) $(MAINNAME): CFLAGS += $(MAININCLUDE) +$(MAINNAME): LDFLAGS += $(MAINLD) -$(MAINNAME): LDFLAGS += $(MAINLD) +TESTSRC := $(shell find $(TESTDIR) -name "*.cpp" -type f) +TESTINCLUDE := -I/usr/local/include/eigen3 \ + -I/usr/local/include/simple_test +TESTOBJ := $(TESTSRC:.cpp=.o) +TESTDEPS := $(TESTOBJ:.o=.d) +$(TESTNAME): CFLAGS += $(TESTINCLUDE) +$(TESTNAME): CFLAGS += -DTEST -include $(LIBDEPS) -include $(MAINDEPS) +-include $(TESTDEPS) ifeq ($(shell uname), Darwin) AR := /usr/bin/libtool @@ -57,6 +64,11 @@ $(LIBNAME): $(LIBOBJ) $(MAINNAME): $(MAINOBJ) $(LIBNAME) $(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ +$(TESTNAME): $(TESTOBJ) $(LIBNAME) + @$(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ + @./$@ + @rm $@ + %.o: %.cpp @echo 'Compiling $@' $(CC) $(CFLAGS) -MMD -MP $< -c -o $@ diff --git a/install/Makefile b/install/Makefile index 9dd6708..5bee482 100644 --- a/install/Makefile +++ b/install/Makefile @@ -1,4 +1,5 @@ # Copyright (C) 2024 Ethan Uppal. +# TODO: move to main makefile LIB := /usr/local/lib HEADER := /usr/local/include diff --git a/lib/icp/impl/trimmed.cpp b/lib/icp/impl/trimmed.cpp index 85f1557..3701305 100644 --- a/lib/icp/impl/trimmed.cpp +++ b/lib/icp/impl/trimmed.cpp @@ -69,7 +69,8 @@ namespace icp { */ std::sort(matches.begin(), matches.end(), [](const auto& a, const auto& b) { return a.sq_dist < b.sq_dist; }); - size_t new_n = (size_t)(overlap_rate * n); + size_t new_n = static_cast(overlap_rate * n); + new_n = std::max(new_n, 1); // yeah, i know this is inefficient. we'll get back to it later. std::vector trimmed_current(new_n); diff --git a/test/test.cpp b/tests/test.cpp similarity index 98% rename from test/test.cpp rename to tests/test.cpp index a8ee180..c57c974 100644 --- a/test/test.cpp +++ b/tests/test.cpp @@ -13,7 +13,7 @@ extern "C" { void test_kdtree(void) {} void test_icp(const std::string& method) { - std::unique_ptr icp = icp::ICP::from_method(method); + std::unique_ptr icp = icp::ICP::from_method(method).value(); { std::vector a = {icp::Vector(0, 0)}; @@ -75,6 +75,8 @@ void test_icp(const std::string& method) { void test_main() { test_kdtree(); + + icp::ICP::register_builtin_methods(); for (const auto& method: icp::ICP::registered_methods()) { std::cout << "testing icp method: " << method << '\n'; test_icp(method); From e131fa2de8e185d9058a2ffef5c995604fdda4ff Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 19:53:41 -0500 Subject: [PATCH 04/13] Add view to Makefile --- Makefile | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 5c4a8a7..dc51eb9 100644 --- a/Makefile +++ b/Makefile @@ -50,6 +50,9 @@ $(TESTNAME): CFLAGS += -DTEST -include $(MAINDEPS) -include $(TESTDEPS) +N := 1 +METHOD := trimmed + ifeq ($(shell uname), Darwin) AR := /usr/bin/libtool AR_OPT := -static @@ -66,8 +69,8 @@ $(MAINNAME): $(MAINOBJ) $(LIBNAME) $(TESTNAME): $(TESTOBJ) $(LIBNAME) @$(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ - @./$@ - @rm $@ + @./$(TESTNAME) + @rm $(TESTNAME) %.o: %.cpp @echo 'Compiling $@' @@ -77,6 +80,11 @@ $(TESTNAME): $(TESTOBJ) $(LIBNAME) clean: @rm -f $(LIBOBJ) $(LIBDEPS) $(LIBNAME) $(MAINOBJ) $(MAINDEPS) $(MAINNAME) +.PHONY: view +view: $(MAINNAME) + ./$(MAINNAME) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --gui + + # Not building book rn, add these commands to build # cd book; \ pdflatex icp.tex; \ From b41dc7274bbb9f8f210d1703843a1bf1448c63a4 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:00:27 -0500 Subject: [PATCH 05/13] Add bench to Makefile --- Makefile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Makefile b/Makefile index dc51eb9..991873d 100644 --- a/Makefile +++ b/Makefile @@ -84,6 +84,9 @@ clean: view: $(MAINNAME) ./$(MAINNAME) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --gui +.PHONY: bench +bench: $(MAINNAME) + ./$(MAINNAME) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --bench # Not building book rn, add these commands to build # cd book; \ From d8cc7cb8af2bc0e87381eb477ac6fbe8d5a7553f Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:18:11 -0500 Subject: [PATCH 06/13] Github Actions --- .github/workflows/ci.yaml | 47 +++++++++++++++++++++++++++++++++++++++ Makefile | 5 ++++- 2 files changed, 51 insertions(+), 1 deletion(-) create mode 100644 .github/workflows/ci.yaml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..0748266 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,47 @@ +name: CI + +on: + pull_request: + push: + branches: + - main + +jobs: + build_lib: + runs-on: ubuntu-22.04 + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Install Eigen + run: sudo apt-get install libeigen3-dev + - name: Build library + run: make libcevicp.a OPT=RELEASE + - name: Upload library + uses: actions/upload-artifact@v4 + with: + name: libcevicp.a + path: libcevicp.a + test: + runs-on: ubuntu-22.04 + needs: build_lib + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Download library + uses: actions/download-artifact@v4 + with: + name: libcevicp.a + - name: Run tests + run: make test OPT=RELEASE + bench: + runs-on: ubuntu-22.04 + needs: build_lib + steps: + - name: Checkout + uses: actions/checkout@v4 + - name: Download library + uses: actions/download-artifact@v4 + with: + name: libcevicp.a + - name: Run benchmarks + run: make bench OPT=RELEASE diff --git a/Makefile b/Makefile index 991873d..a296ea4 100644 --- a/Makefile +++ b/Makefile @@ -14,8 +14,11 @@ LIBNAME := libcevicp.a MAINNAME := main TESTNAME := test -# CFLAGS += $(CRELEASE) +ifeq ($(OPT), RELEASE) +CFLAGS += $(CRELEASE) +else CFLAGS += $(CDEBUG) +endif LIBSRC := $(shell find $(LIBDIR) -name "*.cpp" -type f) LIBINCLUDE := -I/usr/local/include/eigen3 From b8bafb873813e2472527fc5ad6cb63c0f8787799 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:26:36 -0500 Subject: [PATCH 07/13] Change eigen location --- Makefile | 6 +++--- book/main.md | 14 +++++++------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/Makefile b/Makefile index a296ea4..466b7b1 100644 --- a/Makefile +++ b/Makefile @@ -21,14 +21,14 @@ CFLAGS += $(CDEBUG) endif LIBSRC := $(shell find $(LIBDIR) -name "*.cpp" -type f) -LIBINCLUDE := -I/usr/local/include/eigen3 +LIBINCLUDE := -I/usr/include/eigen3 LIBOBJ := $(LIBSRC:.cpp=.o) LIBDEPS := $(LIBOBJ:.o=.d) $(LIBNAME): CFLAGS += $(LIBINCLUDE) MAINSRC := $(shell find $(SRCDIR) -name "*.cpp" -type f) MAININCLUDE := $(shell sdl2-config --cflags) \ - -I/usr/local/include/eigen3 \ + -I/usr/include/eigen3 \ -I/usr/local/include/cmdapp \ -I/usr/local/include/config \ -I/usr/local/include/sdlwrapper @@ -42,7 +42,7 @@ $(MAINNAME): CFLAGS += $(MAININCLUDE) $(MAINNAME): LDFLAGS += $(MAINLD) TESTSRC := $(shell find $(TESTDIR) -name "*.cpp" -type f) -TESTINCLUDE := -I/usr/local/include/eigen3 \ +TESTINCLUDE := -I/usr/include/eigen3 \ -I/usr/local/include/simple_test TESTOBJ := $(TESTSRC:.cpp=.o) TESTDEPS := $(TESTOBJ:.o=.d) diff --git a/book/main.md b/book/main.md index 0d1e0e0..1af609e 100644 --- a/book/main.md +++ b/book/main.md @@ -46,13 +46,13 @@ Please read [this document](icp.pdf) to learn more about the math. First, download and install the dependencies. **Only eigen3 is necessary for the library. If you only wish to build the library, install eigen3 only. The remaining dependencies are only for the visualization tool.** -| Dependency | Library Location (at which) | Header Location (under which) | -| --- | --- | --- | -| [eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) | N/A | `/usr/local/include/eigen3/` | -| [SDL2](https://www.libsdl.org) | `$(sdl2-config --cflags)` | `$(sdl2-config --libs)` | -| My [SDL2 wrapper](https://github.com/cornellev/sdl-wrapper) | `/usr/local/lib/libsdlwrapper.a` | `/usr/local/include/sdlwrapper/` | -| [libcmdapp2](https://ethanuppal.com/libcmdapp2/) | `/usr/local/lib/libcmdapp.a` | `/usr/local/include/` | -| [libconfig](https://github.com/ethanuppal/config) | `/usr/local/lib/libconfig.a` | `/usr/local/include/` | +| Dependency | Library Location (at which) | Header Location (under which) | +| -------------------------------------------------------------- | -------------------------------- | -------------------------------- | +| [eigen3](http://eigen.tuxfamily.org/index.php?title=Main_Page) | N/A | `/usr/include/eigen3/` | +| [SDL2](https://www.libsdl.org) | `$(sdl2-config --cflags)` | `$(sdl2-config --libs)` | +| My [SDL2 wrapper](https://github.com/cornellev/sdl-wrapper) | `/usr/local/lib/libsdlwrapper.a` | `/usr/local/include/sdlwrapper/` | +| [libcmdapp2](https://ethanuppal.com/libcmdapp2/) | `/usr/local/lib/libcmdapp.a` | `/usr/local/include/cmdapp` | +| [libconfig](https://github.com/ethanuppal/config) | `/usr/local/lib/libconfig.a` | `/usr/local/include/config` | There is also a dependency on [simple-test](https://github.com/ethanuppal/simple-test) if you want to run the tests (`make test`). Please follow the installation instructions there. From 8c2a314cebde0f9778ae49a1443f4c1bc70780df Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:34:43 -0500 Subject: [PATCH 08/13] Install dependencies in test and bench --- .github/workflows/ci.yaml | 16 ++++++++++++++++ .vscode/c_cpp_properties.json | 4 ++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 0748266..b20b392 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -31,6 +31,13 @@ jobs: uses: actions/download-artifact@v4 with: name: libcevicp.a + - name: Install Eigen + run: sudo apt-get install libeigen3-dev + - name: Install simple-test + run: | + git clone https://github.com/cornellev/simple-test + cd simple-test + make install - name: Run tests run: make test OPT=RELEASE bench: @@ -43,5 +50,14 @@ jobs: uses: actions/download-artifact@v4 with: name: libcevicp.a + - name: Install Eigen + run: sudo apt-get install libeigen3-dev + - name: Install SDL2 + run: sudo apt-get install libsdl2-dev + - name: Install sdl-wrapper + run: | + git clone https://github.com/cornellev/sdl-wrapper.git + cd sdl-wrapper + make install - name: Run benchmarks run: make bench OPT=RELEASE diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 387ae49..7cddd72 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -5,7 +5,7 @@ "includePath": [ "${default}", "/usr/local/include/sdlwrapper", - "/usr/local/include/eigen3", + "/usr/include/eigen3", "/usr/local/include", "/Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/lib/clang/15.0.0/include/" ], @@ -19,7 +19,7 @@ "${default}", "${workspaceFolder}/include", "/usr/include/SDL2", - "/usr/local/include/eigen3", + "/usr/include/eigen3", "/usr/local/include/sdlwrapper", "/usr/local/include/cmdapp", "/usr/local/include/config", From a169befe0a33825d72aa780821fdb17d9eaa3489 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:36:42 -0500 Subject: [PATCH 09/13] sudo make install --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index b20b392..586a1d0 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -37,7 +37,7 @@ jobs: run: | git clone https://github.com/cornellev/simple-test cd simple-test - make install + sudo make install - name: Run tests run: make test OPT=RELEASE bench: @@ -58,6 +58,6 @@ jobs: run: | git clone https://github.com/cornellev/sdl-wrapper.git cd sdl-wrapper - make install + sudo make install - name: Run benchmarks run: make bench OPT=RELEASE From 28a7c096047c0d9e2ae59b4e188fd4e836c7027d Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:54:14 -0500 Subject: [PATCH 10/13] Fixed UB --- lib/icp/impl/trimmed.cpp | 2 +- lib/icp/impl/vanilla.cpp | 4 ++-- src/sim/lidar_view.cpp | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/lib/icp/impl/trimmed.cpp b/lib/icp/impl/trimmed.cpp index 3701305..9ea6296 100644 --- a/lib/icp/impl/trimmed.cpp +++ b/lib/icp/impl/trimmed.cpp @@ -84,7 +84,7 @@ namespace icp { icp::Vector trimmed_b_cm = get_centroid(trimmed_b); /* #step SVD: see \ref vanilla_icp for details. */ - Matrix N{}; + Matrix N = Matrix::Zero(); for (size_t i = 0; i < new_n; i++) { N += (trimmed_current[i] - trimmed_cm) * (trimmed_b[i] - trimmed_b_cm).transpose(); } diff --git a/lib/icp/impl/vanilla.cpp b/lib/icp/impl/vanilla.cpp index f8dbcad..499fec9 100644 --- a/lib/icp/impl/vanilla.cpp +++ b/lib/icp/impl/vanilla.cpp @@ -17,7 +17,7 @@ exactly and then iterate until an optimal rotation has been found. */ namespace icp { - Vanilla::Vanilla(const Config& config): ICP() {} + Vanilla::Vanilla([[maybe_unused]] const Config& config): ICP() {} Vanilla::Vanilla(): ICP() {} Vanilla::~Vanilla() {} @@ -75,7 +75,7 @@ namespace icp { Sources: https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=4767965 */ - Matrix N{}; + Matrix N = Matrix::Zero(); for (size_t i = 0; i < n; i++) { N += (a_current[i] - a_current_cm) * (b[matches[i].pair] - b_cm).transpose(); } diff --git a/src/sim/lidar_view.cpp b/src/sim/lidar_view.cpp index 9ddd6f8..b524092 100644 --- a/src/sim/lidar_view.cpp +++ b/src/sim/lidar_view.cpp @@ -57,7 +57,8 @@ void LidarView::on_event(const SDL_Event& event) { } } -void LidarView::draw(SDL_Renderer* renderer, const SDL_Rect* frame, double dtime) { +void LidarView::draw(SDL_Renderer* renderer, [[maybe_unused]] const SDL_Rect* frame, + [[maybe_unused]] double dtime) { if (view_config::use_light_background) { SDL_SetRenderDrawColor(renderer, 100, 100, 100, 255); } else { From 5a8f3bdbfdbcd63f47b46fd8e6af080dc6b256d2 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 21:00:16 -0500 Subject: [PATCH 11/13] Install libcmdapp2 --- .github/workflows/ci.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 586a1d0..f164b95 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -59,5 +59,10 @@ jobs: git clone https://github.com/cornellev/sdl-wrapper.git cd sdl-wrapper sudo make install + - name: Install libcmdapp2 + run: | + git clone https://github.com/cornellev/libcmdapp2.git + cd libcmdapp2 + sudo make install - name: Run benchmarks run: make bench OPT=RELEASE From 08f5ea4e88415402fd914c74b56ee5c9ec4df013 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 21:02:48 -0500 Subject: [PATCH 12/13] Set retention days --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index f164b95..bda50f6 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -21,6 +21,7 @@ jobs: with: name: libcevicp.a path: libcevicp.a + retention-days: 1 test: runs-on: ubuntu-22.04 needs: build_lib From 6d1616d162f4fb97c4a2bf37d1026a14db9c2ac5 Mon Sep 17 00:00:00 2001 From: Yey007 <55263178+Yey007@users.noreply.github.com> Date: Tue, 5 Nov 2024 21:04:06 -0500 Subject: [PATCH 13/13] Install config --- .github/workflows/ci.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index bda50f6..98a5142 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -65,5 +65,10 @@ jobs: git clone https://github.com/cornellev/libcmdapp2.git cd libcmdapp2 sudo make install + - name: Install libconfig + run: | + git clone https://github.com/cornellev/config + cd config + sudo make install - name: Run benchmarks run: make bench OPT=RELEASE