diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 0000000..98a5142 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,74 @@ +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 + retention-days: 1 + 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: 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 + sudo make install + - 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: 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 + sudo make install + - name: Install libcmdapp2 + run: | + 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 diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index eb60e6b..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/" ], @@ -17,9 +17,9 @@ "name": "Linux", "includePath": [ "${default}", - "${workspaceFolder}/**", + "${workspaceFolder}/include", "/usr/include/SDL2", - "/usr/local/include/eigen3", + "/usr/include/eigen3", "/usr/local/include/sdlwrapper", "/usr/local/include/cmdapp", "/usr/local/include/config", 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 c1c3c2c..466b7b1 100644 --- a/Makefile +++ b/Makefile @@ -1,64 +1,79 @@ # Copyright (C) 2023 Ethan Uppal. All rights reserved. +INCLUDEDIR := include +LIBDIR := lib SRCDIR := src -INCLUDEDIR := src +TESTDIR := tests 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 +MAINNAME := main +TESTNAME := test -# 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 - -# CFLAGS += $(CRELEASE) +ifeq ($(OPT), RELEASE) +CFLAGS += $(CRELEASE) +else CFLAGS += $(CDEBUG) +endif -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/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/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) +$(MAINNAME): CFLAGS += $(MAININCLUDE) +$(MAINNAME): LDFLAGS += $(MAINLD) + +TESTSRC := $(shell find $(TESTDIR) -name "*.cpp" -type f) +TESTINCLUDE := -I/usr/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) --include $(DEPS) - -# Config parameters N := 1 METHOD := trimmed -$(TARGET): main.cpp $(OBJ) - $(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ - @make readme +ifeq ($(shell uname), Darwin) +AR := /usr/bin/libtool +AR_OPT := -static +else +AR := ar +AR_OPT := rcs $@ $^ +endif -.PHONY: test -test: test.cpp $(OBJ) - @$(CC) $(CFLAGS) -DTEST -o _temp $^ $(LDFLAGS) - @echo 'Running tests...' - @./_temp - @rm -f ./_temp +$(LIBNAME): $(LIBOBJ) + $(AR) $(AR_OPT) -o $@ $^ -.PHONY: view -view: $(TARGET) - ./$(TARGET) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --gui +$(MAINNAME): $(MAINOBJ) $(LIBNAME) + $(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ -.PHONY: bench -bench: $(TARGET) - ./$(TARGET) -S ex_data/scan$(N)/first.conf -D ex_data/scan$(N)/second.conf --method $(METHOD) --bench +$(TESTNAME): $(TESTOBJ) $(LIBNAME) + @$(CC) $(CFLAGS) $^ $(LDFLAGS) -o $@ + @./$(TESTNAME) + @rm $(TESTNAME) %.o: %.cpp @echo 'Compiling $@' @@ -66,7 +81,15 @@ bench: $(TARGET) .PHONY: clean clean: - rm -rf $(OBJ) $(TARGET) $(TARGET) $(DEPS) $(shell find . -name "*.dSYM") $(shell find . -name "*.d") docs + @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 + +.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; \ @@ -91,14 +114,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/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. 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 90% rename from src/icp/icp.h rename to include/icp/icp.h index 8e785f1..f82c4e2 100644 --- a/src/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/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/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 73% rename from src/icp/icp.cpp rename to lib/icp/icp.cpp index b5b2eef..bf9603a 100644 --- a/src/icp/icp.cpp +++ b/lib/icp/icp.cpp @@ -4,9 +4,20 @@ */ #include -#include "icp.h" + +#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 new file mode 100644 index 0000000..9ea6296 --- /dev/null +++ b/lib/icp/impl/trimmed.cpp @@ -0,0 +1,108 @@ +/* + * @author Ethan Uppal + * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved. + */ + +#include +#include +#include +#include +#include + +#include "icp/impl/trimmed.h" + +// TODO: should these docs be moved to the headers? + +/* #name Trimmed */ + +/* #desc Trimmed ICP is identical to \ref vanilla_icp with the addition of an +overlap rate parameter, which specifies the percentage of points between the two +point sets that have correspondences. When the overlap rate is 1, the algorithm +reduces to vanilla. */ + +namespace icp { + + 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() {} + + void Trimmed::setup() { + if (a_current.size() < a.size()) { + a_current.resize(a.size()); + } + + b_cm = get_centroid(b); + } + + void Trimmed::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]); + } + + /* #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 = 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); + 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); + + /* #step SVD: see \ref vanilla_icp for details. */ + 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(); + } + + 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(); + } + + transform.rotation = R * transform.rotation; + + /* #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 new file mode 100644 index 0000000..499fec9 --- /dev/null +++ b/lib/icp/impl/vanilla.cpp @@ -0,0 +1,123 @@ +/* + * @author Ethan Uppal + * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved. + */ + +#include +#include +#include +#include +#include + +#include "icp/impl/vanilla.h" + +/* #name Vanilla */ + +/* #desc The vanilla algorithm for ICP will match the point-cloud centers +exactly and then iterate until an optimal rotation has been found. */ + +namespace icp { + Vanilla::Vanilla([[maybe_unused]] const Config& config): ICP() {} + Vanilla::Vanilla(): ICP() {} + Vanilla::~Vanilla() {} + + void Vanilla::setup() { + if (a_current.size() < a.size()) { + a_current.resize(a.size()); + } + + b_cm = get_centroid(b); + } + + 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; + } + } + } + + /* + #step + SVD + + 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 = Matrix::Zero(); + 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; + + /* + #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. + + 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/icp/impl/trimmed.cpp b/src/icp/impl/trimmed.cpp deleted file mode 100644 index 507e522..0000000 --- a/src/icp/impl/trimmed.cpp +++ /dev/null @@ -1,119 +0,0 @@ -/* - * @author Ethan Uppal - * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved. - */ - -#include -#include -#include "../icp.h" -#include -#include -#include - -/* #name Trimmed */ - -/* #desc Trimmed ICP is identical to \ref vanilla_icp with the addition of an -overlap rate parameter, which specifies the percentage of points between the two -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()); - } - - b_cm = get_centroid(b); - } - - void iterate() override { - 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]); - } - - /* #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]; - } - - 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(); - } - - 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(); - } - - 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; - }(); -} diff --git a/src/icp/impl/vanilla.cpp b/src/icp/impl/vanilla.cpp deleted file mode 100644 index 45afbb4..0000000 --- a/src/icp/impl/vanilla.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/* - * @author Ethan Uppal - * @copyright Copyright (C) 2024 Ethan Uppal. All rights reserved. - */ - -#include -#include -#include "../icp.h" -#include -#include -#include - -/* #name Vanilla */ - -/* #desc The vanilla algorithm for ICP will match the point-cloud centers -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(): ICP() {} - ~Vanilla() override {} - - void setup() override { - if (a_current.size() < a.size()) { - a_current.resize(a.size()); - } - - b_cm = get_centroid(b); - } - - void iterate() override { - 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; - } - } - } - - /* - #step - SVD - - 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(); - } - - transform.rotation = R * transform.rotation; - - /* - #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. - - 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; - }(); -} diff --git a/main.cpp b/src/main.cpp similarity index 94% rename from main.cpp rename to src/main.cpp index 1a9df2f..b4a6e22 100644 --- a/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..b524092 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 { @@ -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 { 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; diff --git a/test.cpp b/tests/test.cpp similarity index 98% rename from test.cpp rename to tests/test.cpp index a8ee180..c57c974 100644 --- a/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);