Repository: pglira/simpleICP (language: python)

Issue:
Extended feature: direct observation of transformation parameters
It should be possible to directly observe the transformation parameters of the rigid body transformation.


Discussion / hints:

Fix patch:
diff --git a/.gitignore b/.gitignore
index 4a1d14b..ed376ab 100644
--- a/.gitignore
+++ b/.gitignore
@@ -13,4 +13,5 @@ python/simpleicp/__pycache__/
 README.md
 octave/check/
 octave/octave-workspace
-python/build/
\ No newline at end of file
+python/build/
+rust/
\ No newline at end of file
diff --git a/README.md b/README.md
index 30878e1..98dedd5 100644
--- a/README.md
+++ b/README.md
@@ -6,13 +6,13 @@ This repo contains implementations of a rather simple version of the [Iterative
 
 Currently, an implementation is available for:
 
-| Language | Code | Main dependencies |
-| --- | --- | --- |
-| C++ | [Link](c++) | [nanoflann](https://github.com/jlblancoc/nanoflann), [Eigen](http://eigen.tuxfamily.org), [cxxopts](https://github.com/jarro2783/cxxopts) |
-| Julia | [Link](julia) | [NearestNeighbors.jl](https://github.com/KristofferC/NearestNeighbors.jl) |
-| Matlab | [Link](matlab) | [Statistics and Machine Learning Toolbox](https://www.mathworks.com/products/statistics.html) |
-| Octave | [Link](octave) | |
-| Python | [Link](python) | [NumPy](https://numpy.org), [SciPy](https://scipy.org) |
+| Language | Code           | Main dependencies                                                                                                                         |
+| -------- | -------------- | ----------------------------------------------------------------------------------------------------------------------------------------- |
+| C++      | [Link](c++)    | [nanoflann](https://github.com/jlblancoc/nanoflann), [Eigen](http://eigen.tuxfamily.org), [cxxopts](https://github.com/jarro2783/cxxopts) |
+| Julia    | [Link](julia)  | [NearestNeighbors.jl](https://github.com/KristofferC/NearestNeighbors.jl)                                                                 |
+| Matlab   | [Link](matlab) | [Statistics and Machine Learning Toolbox](https://www.mathworks.com/products/statistics.html)                                             |
+| Octave   | [Link](octave) |                                                                                                                                           |
+| Python   | [Link](python) | [NumPy](https://numpy.org), [SciPy](https://scipy.org)                                                                                    |
 
 I've tried to optimize the readability of the code, i.e. the code structure is as simple as possible and tests are rather rare.
 
@@ -54,6 +54,23 @@ The following basic features are implemented in all languages:
 - The normal vector of the plane (needed to compute the point-to-plane distance) is estimated from the fixed point cloud using a fixed number of neighbors. Default is ``neighbors = 10``.
 - The point clouds must not fully overlap, i.e. a partial overlap of the point cloud is allowed. An example for such a case is the *Bunny* dataset, see [here](#test-data-sets). The initial overlapping area between two point  clouds can be defined by the parameter ``max_overlap_distance``. More specifically, the correspondences are only selected across points of the fixed point cloud for which the initial distance to the nearest neighbor of the movable point cloud is ``<= max_overlap_distance``.
 
+### Extended features
+
+The extended features are currently *not* implemented in all languages. The differences are documented in the following table:
+
+| Feature                               | C++ | Julia | Matlab | Octave | Python |
+| ------------------------------------- | --- | ----- | ------ | ------ | ------ |
+| **a priori observation of transform** | no  | no    | no     | no     | yes    |
+
+Description of extended features:
+
+- **a priori observation of transform**: this is useful in at least three cases:
+  <!-- 1. if the estimation of a subset of the 6 rigid-body transformation parameters is -->
+  <!-- 2. if a subset of the 6 rigid-body transformation parameters ... -->
+  <!-- 3. if a solution from a previous run of the ICP algorithm exists -->
+
+  New arguments are ...
+
 ## Output
 
 All implementations generate the same screen output. This is an example from the C++ version for the *Bunny* dataset:
@@ -61,53 +78,53 @@ All implementations generate the same screen output. This is an example from the
 ```
 $ run_simpleicp.sh
 Processing dataset "Bunny"
-[09:42:37.690] Create point cloud objects ...
-[09:42:37.690] Consider partial overlap of point clouds ...
-[09:42:37.770] Select points for correspondences within overlap area of fixed point cloud ...
-[09:42:37.770] Estimate normals of selected points ...
-[09:42:37.775] Start iterations ...
-[09:42:37.780] Iteration | correspondences | mean(residuals) |  std(residuals)
-[09:42:37.780]         0 |             960 |         -0.0003 |          0.0025
-[09:42:37.780]         1 |             960 |          0.0000 |          0.0013
-[09:42:37.785]         2 |             903 |         -0.0000 |          0.0005
-[09:42:37.790]         3 |             903 |         -0.0000 |          0.0004
-[09:42:37.794]         4 |             883 |         -0.0000 |          0.0003
-[09:42:37.799]         5 |             877 |         -0.0000 |          0.0003
-[09:42:37.803]         6 |             869 |         -0.0000 |          0.0002
-[09:42:37.808]         7 |             860 |         -0.0000 |          0.0002
-[09:42:37.813]         8 |             855 |         -0.0000 |          0.0002
-[09:42:37.818]         9 |             851 |         -0.0000 |          0.0002
-[09:42:37.822]        10 |             849 |         -0.0000 |          0.0002
-[09:42:37.827] Convergence criteria fulfilled -> stop iteration!
-[09:42:37.827] Estimated transformation matrix H:
-[09:42:37.827] [    0.990045    -0.172046     0.000965    -0.000304]
-[09:42:37.827] [    0.172037     0.990039    -0.001838    -0.000220]
-[09:42:37.827] [   -0.000824     0.001815     1.000023    -0.000015]
-[09:42:37.827] [    0.000000     0.000000     0.000000     1.000000]
-[09:42:37.827] Finished in 0.137 seconds!
+Create point cloud objects ...
+Consider partial overlap of point clouds ...
+Select points for correspondences within overlap area of fixed point cloud ...
+Estimate normals of selected points ...
+Start iterations ...
+Iteration | correspondences | mean(residuals) |  std(residuals)
+        0 |             961 |         -0.0321 |          0.2494
+        1 |             961 |          0.0027 |          0.1349
+        2 |             904 |         -0.0011 |          0.0540
+        3 |             904 |         -0.0034 |          0.0390
+        4 |             883 |         -0.0021 |          0.0325
+        5 |             877 |         -0.0019 |          0.0284
+        6 |             869 |         -0.0016 |          0.0243
+        7 |             862 |         -0.0013 |          0.0213
+        8 |             857 |         -0.0013 |          0.0191
+        9 |             852 |         -0.0014 |          0.0188
+       10 |             850 |         -0.0015 |          0.0186
+Convergence criteria fulfilled -> stop iteration!
+Estimated transformation matrix H:
+[    0.990048    -0.172044     0.000977    -0.030468]
+[    0.172035     0.990043    -0.001831    -0.022069]
+[   -0.000834     0.001813     1.000022    -0.001465]
+[    0.000000     0.000000     0.000000     1.000000]
+Finished in 0.131 seconds!
 ```
 
 ## Test data sets
 
 The test data sets are included in the [data](data) subfolder. An example call for each language can be found in the ``run_simpleicp.*`` files, e.g. [run_simpleicp.py](python/simpleicp/tests/run_simpleicp.py) for the python version.
 
-| Dataset | | pc1 (no_pts) | pc2 (no_pts) | Overlap | Source |
-| :--- | --- | --- | --- | --- | --- |
-| *Dragon* | ![Dragon](/data/dragon_small.png) | [pc1](data/dragon1.xyz) (100k) | [pc2](data/dragon2.xyz) (100k) | full overlap | [The Stanford 3D Scanning Repository](http://graphics.stanford.edu/data/3Dscanrep/) |
-| *Airborne Lidar* | ![AirborneLidar](/data/airborne_lidar_small.png) | [pc1](data/airborne_lidar1.xyz) (1340k) | [pc2](data/airborne_lidar2.xyz) (1340k) | full overlap | Airborne Lidar fligth campaign over Austrian Alps |
-| *Terrestrial Lidar* | ![TerrestrialLidar](/data/terrestrial_lidar_small.png) | [pc1](data/terrestrial_lidar1.xyz) (1250k) | [pc2](data/terrestrial_lidar2.xyz) (1250k) | full overlap | Terrestrial Lidar point clouds of a stone block|
-| *Bunny* | ![Bunny](/data/bunny_small.png) | [pc1](data/bunny_part1.xyz) (21k) | [pc2](data/bunny_part2.xyz) (22k) | partial overlap | [The Stanford 3D Scanning Repository](http://graphics.stanford.edu/data/3Dscanrep/) |
+| Dataset             |                                                        | pc1 (no_pts)                               | pc2 (no_pts)                               | Overlap         | Source                                                                              |
+| :------------------ | ------------------------------------------------------ | ------------------------------------------ | ------------------------------------------ | --------------- | ----------------------------------------------------------------------------------- |
+| *Dragon*            | ![Dragon](/data/dragon_small.png)                      | [pc1](data/dragon1.xyz) (100k)             | [pc2](data/dragon2.xyz) (100k)             | full overlap    | [The Stanford 3D Scanning Repository](http://graphics.stanford.edu/data/3Dscanrep/) |
+| *Airborne Lidar*    | ![AirborneLidar](/data/airborne_lidar_small.png)       | [pc1](data/airborne_lidar1.xyz) (1340k)    | [pc2](data/airborne_lidar2.xyz) (1340k)    | full overlap    | Airborne Lidar fligth campaign over Austrian Alps                                   |
+| *Terrestrial Lidar* | ![TerrestrialLidar](/data/terrestrial_lidar_small.png) | [pc1](data/terrestrial_lidar1.xyz) (1250k) | [pc2](data/terrestrial_lidar2.xyz) (1250k) | full overlap    | Terrestrial Lidar point clouds of a stone block                                     |
+| *Bunny*             | ![Bunny](/data/bunny_small.png)                        | [pc1](data/bunny_part1.xyz) (21k)          | [pc2](data/bunny_part2.xyz) (22k)          | partial overlap | [The Stanford 3D Scanning Repository](http://graphics.stanford.edu/data/3Dscanrep/) |
 
 ### Benchmark
 
 These are the runtimes on my PC for the data sets above:
 
-| Dataset             | C++   | Julia | Matlab | Octave* | Python |
-| :--- | ---: | ---: | ---: | ---: | ---: |
-| *Dragon*            | 0.16s | 3.99s |  1.34s | 95.7s   | 0.89s  |
-| *Airborne Lidar*    | 3.98s | 5.38s | 15.08s | -       | 5.45s  |
-| *Terrestrial Lidar* | 3.62s | 5.22s | 13.24s | -       | 5.68s  |
-| *Bunny*             | 0.13s | 0.38s |  0.37s | 72.8s   | 0.80s  |
+| Dataset             |   C++ | Julia | Matlab | Octave* | Python |
+| :------------------ | ----: | ----: | -----: | ------: | -----: |
+| *Dragon*            | 0.16s | 3.99s |  1.34s |   95.7s |  0.89s |
+| *Airborne Lidar*    | 3.98s | 5.38s | 15.08s |       - |  5.45s |
+| *Terrestrial Lidar* | 3.62s | 5.22s | 13.24s |       - |  5.68s |
+| *Bunny*             | 0.13s | 0.38s |  0.37s |   72.8s |  0.80s |
 
 For all versions the same input parameters (``correspondences``, ``neighbors``, ...) are used.
 
diff --git a/c++/.devcontainer/devcontainer.json b/c++/.devcontainer/devcontainer.json
index d83c570..5a581fb 100644
--- a/c++/.devcontainer/devcontainer.json
+++ b/c++/.devcontainer/devcontainer.json
@@ -16,7 +16,8 @@
 	// Add the IDs of extensions you want installed when the container is created.
 	"extensions": [
 		"ms-vscode.cpptools",
-		"ms-vscode.cmake-tools"
+		"ms-vscode.cmake-tools",
+		"visualstudioexptteam.vscodeintellicode"
 	],
 
 	// Use 'forwardPorts' to make a list of ports inside the container available locally.
diff --git a/c++/CMakeLists.txt b/c++/CMakeLists.txt
index e21b9da..520dbe6 100644
--- a/c++/CMakeLists.txt
+++ b/c++/CMakeLists.txt
@@ -2,13 +2,15 @@ cmake_minimum_required(VERSION 3.15)
 project(simpleicp)
 
 set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wconversion -Wshadow -Wpedantic -fsanitize=undefined,address")
+set(CMAKE_CXX_FLAGS_RELEASE "-O2")
 
 find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 
 add_executable(simpleicp
-        src/simpleicp-cli.cpp
-        src/simpleicp.cpp src/simpleicp.h
-        src/pointcloud.cpp src/pointcloud.h
-        src/corrpts.cpp src/corrpts.h)
+src/simpleicp-cli.cpp
+src/simpleicp.cpp src/simpleicp.h
+src/pointcloud.cpp src/pointcloud.h
+src/corrpts.cpp src/corrpts.h)
 
 target_link_libraries(simpleicp Eigen3::Eigen)
diff --git a/c++/run_simpleicp.sh b/c++/run_simpleicp.sh
index 9ec0b6c..a271a5b 100755
--- a/c++/run_simpleicp.sh
+++ b/c++/run_simpleicp.sh
@@ -21,7 +21,7 @@ if [[ "${dataset}" == "Airborne Lidar" || "${dataset}" == "all" ]]; then
 fi
 
 if [[ "${dataset}" == "Terrestrial Lidar" || "${dataset}" == "all" ]]; then
-    echo "Processing dataset \"Airborne Lidar\""
+    echo "Processing dataset \"Terrestrial Lidar\""
     ${simpleicp} \
     --fixed ../data/terrestrial_lidar1.xyz \
     --movable ../data/terrestrial_lidar2.xyz
diff --git a/c++/src/corrpts.cpp b/c++/src/corrpts.cpp
index 6e439d1..810d882 100644
--- a/c++/src/corrpts.cpp
+++ b/c++/src/corrpts.cpp
@@ -25,7 +25,7 @@ void CorrPts::Match()
 void CorrPts::GetPlanarityFromPc1()
 {
   planarity_ = Eigen::VectorXd(idx_pc1_.size());
-  for (int i = 0; i < idx_pc1_.size(); i++)
+  for (uint i = 0; i < idx_pc1_.size(); i++)
   {
     planarity_[i] = pc1_.planarity()[idx_pc1_[i]];
   }
@@ -36,7 +36,7 @@ void CorrPts::ComputeDists()
   dists_ = Eigen::VectorXd(idx_pc1_.size());
   dists_.fill(NAN);
 
-  for (int i = 0; i < idx_pc1_.size(); i++)
+  for (uint i = 0; i < idx_pc1_.size(); i++)
   {
     double x_pc1 = pc1_.X()(idx_pc1_[i], 0);
     double y_pc1 = pc1_.X()(idx_pc1_[i], 1);
@@ -96,7 +96,7 @@ void CorrPts::EstimateRigidBodyTransformation(Eigen::Matrix<double, 4, 4> &H,
   Eigen::MatrixXd A(no_corr_pts, 6);
   Eigen::VectorXd l(no_corr_pts);
 
-  for (int i = 0; i < no_corr_pts; i++)
+  for (uint i = 0; i < no_corr_pts; i++)
   {
     double x_pc1 = pc1_.X()(idx_pc1_[i], 0);
     double y_pc1 = pc1_.X()(idx_pc1_[i], 1);
diff --git a/c++/src/pointcloud.cpp b/c++/src/pointcloud.cpp
index 881380b..443cf22 100644
--- a/c++/src/pointcloud.cpp
+++ b/c++/src/pointcloud.cpp
@@ -7,7 +7,7 @@ Eigen::MatrixXd PointCloud::GetXOfSelectedPts()
 {
   auto sel_idx = GetIdxOfSelectedPts();
   Eigen::MatrixXd X_sel(sel_idx.size(), 3);
-  for (int i = 0; i < sel_idx.size(); i++)
+  for (uint i = 0; i < sel_idx.size(); i++)
   {
     X_sel(i, 0) = X_(sel_idx[i], 0);
     X_sel(i, 1) = X_(sel_idx[i], 1);
@@ -19,7 +19,7 @@ Eigen::MatrixXd PointCloud::GetXOfSelectedPts()
 std::vector<int> PointCloud::GetIdxOfSelectedPts()
 {
   std::vector<int> idx;
-  for (int i = 0; i < NoPts(); i++)
+  for (uint i = 0; i < NoPts(); i++)
   {
     if (sel_[i])
     {
@@ -42,7 +42,7 @@ void PointCloud::SelectInRange(const Eigen::MatrixX3d &X, const double &max_rang
   Eigen::VectorXd dists(no_selected_points);
   dists.fill(NAN);
 
-  for (int i = 0; i < no_selected_points; i++)
+  for (uint i = 0; i < no_selected_points; i++)
   {
     double x_query{X_query(i, 0)};
     double y_query{X_query(i, 1)};
@@ -62,7 +62,7 @@ void PointCloud::SelectInRange(const Eigen::MatrixX3d &X, const double &max_rang
   }
 
   // Deselect points which exceed maximum range
-  for (int i = 0; i < no_selected_points; i++)
+  for (uint i = 0; i < no_selected_points; i++)
   {
     if (dists(i) > max_range)
     {
@@ -72,23 +72,23 @@ void PointCloud::SelectInRange(const Eigen::MatrixX3d &X, const double &max_rang
 
 }
 
-void PointCloud::SelectNPts(const int &n)
+void PointCloud::SelectNPts(const uint &n)
 {
   auto sel_idx{GetIdxOfSelectedPts()};
 
   if (n < sel_idx.size())
   {
     // Deactivate all points first
-    for (int i = 1; i < NoPts(); i++)
+    for (long i = 1; i < NoPts(); i++)
     {
       sel_[i] = false;
     }
 
     // Re-activate n points
-    auto idx_not_rounded{Eigen::VectorXd::LinSpaced(n, 0, sel_idx.size() - 1)};
-    for (int i = 0; i < n; i++)
+    auto idx_not_rounded{Eigen::VectorXd::LinSpaced(n, 0, static_cast<uint>(sel_idx.size()) - 1)};
+    for (uint i = 0; i < n; i++)
     {
-      int idx_rounded{static_cast<int>(round(idx_not_rounded(i)))};
+      uint idx_rounded{static_cast<uint>(round(idx_not_rounded(i)))};
       sel_[sel_idx[idx_rounded]] = true;
     }
   }
@@ -110,7 +110,7 @@ void PointCloud::EstimateNormals(const int &neighbors)
   mat_idx_nn = KnnSearch(X_, GetXOfSelectedPts(), neighbors);
 
   auto sel_idx = GetIdxOfSelectedPts();
-  for (int i = 0; i < sel_idx.size(); i++)
+  for (uint i = 0; i < sel_idx.size(); i++)
   {
     // Build matrix with nn
     Eigen::MatrixXd X_nn(neighbors, 3);
@@ -147,7 +147,7 @@ void PointCloud::Transform(Eigen::Matrix<double, 4, 4> &H)
   X_ << X_out_H.row(0).transpose(), X_out_H.row(1).transpose(), X_out_H.row(2).transpose();
 }
 
-int PointCloud::NoPts() { return X_.rows(); }
+long PointCloud::NoPts() { return X_.rows(); }
 
 // Getters
 const Eigen::MatrixXd &PointCloud::X() { return X_; }
diff --git a/c++/src/pointcloud.h b/c++/src/pointcloud.h
index 2af086c..17323cf 100644
--- a/c++/src/pointcloud.h
+++ b/c++/src/pointcloud.h
@@ -15,13 +15,13 @@ class PointCloud
 
   void SelectInRange(const Eigen::MatrixX3d &X, const double &max_range);
 
-  void SelectNPts(const int &n);
+  void SelectNPts(const uint &n);
 
   void EstimateNormals(const int &neighbors);
 
   void Transform(Eigen::Matrix<double, 4, 4> &H);
 
-  int NoPts();
+  long NoPts();
 
   // Getters
   const Eigen::MatrixXd &X();
diff --git a/c++/src/simpleicp-cli.cpp b/c++/src/simpleicp-cli.cpp
index 76f893e..f4dbd68 100644
--- a/c++/src/simpleicp-cli.cpp
+++ b/c++/src/simpleicp-cli.cpp
@@ -90,7 +90,7 @@ Eigen::MatrixXd ImportXYZFileToMatrix(const std::string &path_to_pc)
     }
 
     // Check if each line contains exactly 3 values
-    for (int i = 0; i < parsedData.size(); i++)
+    for (uint i = 0; i < parsedData.size(); i++)
     {
       if (parsedData[i].size() != 3)
       {
@@ -101,9 +101,9 @@ Eigen::MatrixXd ImportXYZFileToMatrix(const std::string &path_to_pc)
 
     // Create eigen array
     Eigen::MatrixXd X(parsedData.size(), 3);
-    for (int i = 0; i < parsedData.size(); i++)
+    for (uint i = 0; i < parsedData.size(); i++)
     {
-      for (int j = 0; j < parsedData[i].size(); j++)
+      for (uint j = 0; j < parsedData[i].size(); j++)
       {
         try
         {
diff --git a/c++/src/simpleicp.cpp b/c++/src/simpleicp.cpp
index b1f5ce7..d25a436 100644
--- a/c++/src/simpleicp.cpp
+++ b/c++/src/simpleicp.cpp
@@ -136,7 +136,7 @@ Eigen::MatrixXi KnnSearch(const Eigen::MatrixXd &X, const Eigen::MatrixXd &X_que
 
   // Iterate over all query points
   Eigen::MatrixXi mat_idx_nn(X_query.rows(), k);
-  for (size_t i = 0; i < X_query.rows(); i++)
+  for (int i = 0; i < X_query.rows(); i++)
   {
     // Query point
     std::vector<double> qp{X_query(i, 0), X_query(i, 1), X_query(i, 2)};
diff --git a/python/.devcontainer/Dockerfile b/python/.devcontainer/Dockerfile
index b396e47..5e1b436 100644
--- a/python/.devcontainer/Dockerfile
+++ b/python/.devcontainer/Dockerfile
@@ -17,7 +17,7 @@ RUN if [ "${NODE_VERSION}" != "none" ]; then su vscode -c "umask 0002 && . /usr/
 # COPY requirements.txt /tmp/pip-tmp/
 # RUN pip3 --disable-pip-version-check --no-cache-dir install -r /tmp/pip-tmp/requirements.txt \
 #    && rm -rf /tmp/pip-tmp
-RUN pip3 install -U twine matplotlib
+RUN pip3 install -U twine matplotlib pandas
 
 # [Optional] Uncomment this line to install global node packages.
 # RUN su vscode -c "source /usr/local/share/nvm/nvm.sh && npm install -g <your-package-here>" 2>&1
\ No newline at end of file
diff --git a/python/.devcontainer/devcontainer.json b/python/.devcontainer/devcontainer.json
index 9a51a12..93876c1 100644
--- a/python/.devcontainer/devcontainer.json
+++ b/python/.devcontainer/devcontainer.json
@@ -34,9 +34,11 @@
 	// Add the IDs of extensions you want installed when the container is created.
 	"extensions": [
 		"ms-python.python",
-		"ms-python.vscode-pylance",
+		"visualstudioexptteam.vscodeintellicod",
+		"njpwerner.autodocstring",
 		"gruntfuggly.todo-tree",
-		"streetsidesoftware.code-spell-checker"
+		"streetsidesoftware.code-spell-checker",
+		"yzhang.markdown-all-in-one"
 	],
 
 	// Uncomment when using a ptrace-based debugger like C++, Go, and Rust
diff --git a/python/.vscode/settings.json b/python/.vscode/settings.json
index 07d0811..334a985 100644
--- a/python/.vscode/settings.json
+++ b/python/.vscode/settings.json
@@ -2,5 +2,6 @@
     "python.pythonPath": "/usr/local/bin/python",
     "python.linting.pylintArgs": [
         "--disable=invalid-name"
-    ]
+    ],
+    "cSpell.enabled": false
 }
\ No newline at end of file
diff --git a/python/.vscode/tasks.json b/python/.vscode/tasks.json
index 9a36192..5070bcd 100644
--- a/python/.vscode/tasks.json
+++ b/python/.vscode/tasks.json
@@ -7,6 +7,16 @@
             "label": "build and upload simpleicp package to pypi",
             "type": "shell",
             "command": "rm -rf dist && python setup.py sdist bdist_wheel && twine upload dist/*"
+        },
+        {
+            "label": "run current python file",
+            "type": "shell",
+            "command": "python ${file}"
+        },
+        {
+            "label": "run minimal working example",
+            "type": "shell",
+            "command": "python ${workspaceFolder}/simpleicp/tests/minimal_working_example.py"
         }
     ]
 }
\ No newline at end of file
diff --git a/python/README.md b/python/README.md
index c69549d..be4e94c 100644
--- a/python/README.md
+++ b/python/README.md
@@ -17,39 +17,61 @@ pip install simpleicp
 ## How to use
 
 ```python
-from simpleicp import simpleicp
+from simpleicp import PointCloud, SimpleICP
 import numpy as np
 
-# Read fixed and movable point cloud from xyz files into n-by-3 numpy arrays
-X_fix = np.genfromtxt("dragon1.xyz")
-X_mov = np.genfromtxt("dragon2.xyz")
+# Read point clouds from xyz files into n-by-3 numpy arrays
+X_fix = np.genfromtxt("bunny_part1.xyz")
+X_mov = np.genfromtxt("bunny_part2.xyz")
 
-# Run simpleICP!
-H, X_mov_transformed = simpleicp.simpleicp(X_fix, X_mov)
+# Create point cloud objects
+pc_fix = PointCloud(X_fix, columns=["x", "y", "z"])
+pc_mov = PointCloud(X_mov, columns=["x", "y", "z"])
+
+# Create simpleICP object, add point clouds, and run algorithm!
+icp = SimpleICP()
+icp.add_point_clouds(pc_fix, pc_mov)
+H, X_mov_transformed = icp.run(max_overlap_distance=1)
 ```
 
-This should give (except for timestamps and timings) this output:
-
-```log
-[11:18:30.890] Create point cloud objects ...
-[11:18:30.891] Select points for correspondences within overlap area of fixed point cloud ...
-[11:18:30.891] Estimate normals of selected points ...
-[11:18:31.048] Start iterations ...
-[11:18:31.084] Iteration | correspondences | mean(residuals) | std(residuals)
-[11:18:31.084]         0 |             768 |          0.0464 |          0.3179
-[11:18:31.084]         1 |             768 |          0.0124 |          0.2534
-[11:18:31.155]         2 |             773 |          0.0100 |          0.1655
-[11:18:31.212]         3 |             772 |          0.0040 |          0.0830
-[11:18:31.306]         4 |             749 |          0.0010 |          0.0191
-[11:18:31.415]         5 |             758 |         -0.0000 |          0.0024
-[11:18:31.487]         6 |             774 |          0.0000 |          0.0022
-[11:18:31.556] Convergence criteria fulfilled -> stop iteration!
-[11:18:31.556] Estimated transformation matrix H:
-[11:18:31.557] H = [    0.998693     0.052911    -0.034201    -0.197402]
-[11:18:31.557] H = [   -0.052402     0.999035     0.019119    -0.400079]
-[11:18:31.557] H = [    0.034816    -0.017661     0.999446    -0.602222]
-[11:18:31.558] H = [    0.000000     0.000000     0.000000     1.000000]
-[11:18:31.558] Finished in 0.669 seconds!
+This should give this output:
+
+```
+Consider partial overlap of point clouds ...
+Select points for correspondences in fixed point cloud ...
+Estimate normals of selected points ...
+Start iterations ...
+iteration | correspondences | mean(residuals) |  std(residuals)
+   orig:0 |             951 |          0.0401 |          0.2397
+        1 |             950 |          0.0027 |          0.1356
+        2 |             889 |          0.0026 |          0.0586
+        3 |             897 |          0.0020 |          0.0407
+        4 |             873 |          0.0004 |          0.0303
+        5 |             854 |          0.0004 |          0.0245
+        6 |             847 |          0.0003 |          0.0208
+        7 |             826 |         -0.0006 |          0.0154
+        8 |             799 |          0.0005 |          0.0099
+        9 |             787 |          0.0002 |          0.0068
+       10 |             783 |         -0.0001 |          0.0047
+       11 |             779 |         -0.0001 |          0.0037
+       12 |             776 |         -0.0000 |          0.0033
+       13 |             776 |         -0.0000 |          0.0033
+Convergence criteria fulfilled -> stop iteration!
+Estimated transformation matrix H:
+[    0.984804    -0.173671    -0.000041     0.000420]
+[    0.173671     0.984804     0.000051    -0.000750]
+[    0.000032    -0.000057     1.000000     0.000054]
+[    0.000000     0.000000     0.000000     1.000000]
+... which corresponds to the following rigid body transformation parameters:
+parameter |       est.value | est.uncertainty |       obs.value |      obs.weight
+   alpha1 |       -0.002906 |        0.004963 |        0.000000 |        0.000000
+   alpha2 |       -0.002353 |        0.002339 |        0.000000 |        0.000000
+   alpha3 |       10.001317 |        0.006276 |        0.000000 |        0.000000
+       tx |        0.000420 |        0.000459 |        0.000000 |        0.000000
+       ty |       -0.000750 |        0.000974 |        0.000000 |        0.000000
+       tz |        0.000054 |        0.000209 |        0.000000 |        0.000000
+(Unit of est.value, est.uncertainty, and obs.value for alpha1/2/3 is degree)
+Finished in 4.320 seconds!
 ```
 
-Note that ``dragon1.xyz`` and ``dragon2.xyz`` are not included in this package. They can be downloaded (among other example files) [here](https://github.com/pglira/simpleICP/tree/master/data).
\ No newline at end of file
+Note that ``bunny_part1.xyz`` and ``bunny_part2.xyz`` are not included in this package. They can be downloaded (among other example files) [here](https://github.com/pglira/simpleICP/tree/master/data).
\ No newline at end of file
diff --git a/python/setup.py b/python/setup.py
index ac61bff..08bb773 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -7,7 +7,7 @@
 
 setup(
     name="simpleicp",
-    version="1.0.2",  # should match version in __init__.py
+    version="2.0.0",  # should match version in __init__.py
     description="Simple version of the Iterative Closest Point (ICP) algorithm",
     long_description=readme_filepath,
     long_description_content_type="text/markdown",
@@ -21,5 +21,5 @@
         "Programming Language :: Python :: 3.9",
     ],
     packages=find_packages(exclude=("tests",)),
-    install_requires=["numpy", "scipy"],
+    install_requires=["numpy", "scipy", "lmfit"],
 )
diff --git a/python/simpleicp/__init__.py b/python/simpleicp/__init__.py
index 7863915..47a0953 100644
--- a/python/simpleicp/__init__.py
+++ b/python/simpleicp/__init__.py
@@ -1,1 +1,7 @@
-__version__ = "1.0.2"
+__version__ = "2.0.0"
+
+# Import statements in order to simplify API
+# Reference: https://stackoverflow.com/a/35733111
+# Import here only objects which are directly used by the user
+from .simpleicp import SimpleICP
+from .pointcloud import PointCloud
\ No newline at end of file
diff --git a/python/simpleicp/corrpts.py b/python/simpleicp/corrpts.py
new file mode 100644
index 0000000..a84c551
--- /dev/null
+++ b/python/simpleicp/corrpts.py
@@ -0,0 +1,209 @@
+"""
+Class for corresponding points, i.e. correspondences.
+"""
+
+import numpy as np
+import pandas as pd
+from scipy import spatial, stats
+
+from . import pointcloud
+
+
+class CorrPts:
+    """Class for managing corresponding points between two overlapping point clouds."""
+
+    def __init__(self, pc1: pointcloud.PointCloud, pc2: pointcloud.PointCloud) -> None:
+        """Constructor method.
+
+        Args:
+            pc1 (pointcloud.PointCloud): Point cloud 1.
+            pc2 (pointcloud.PointCloud): Point cloud 2.
+        """
+        self._pc1 = pc1
+        self._pc2 = pc2
+
+        # DataFrame containing the data about the correspondences
+        self._df = pd.DataFrame()
+
+    @property
+    def pc1_x(self) -> np.array:
+        """Returns x coordinates of point cloud 1 of the correspondences."""
+        if "pc1_idx" in self._df:
+            return self._pc1.iloc[self._df["pc1_idx"]]["x"].to_numpy()
+        return None
+
+    @property
+    def pc1_y(self) -> np.array:
+        """Returns y coordinates of point cloud 1 of the correspondences."""
+        if "pc1_idx" in self._df:
+            return self._pc1.iloc[self._df["pc1_idx"]]["y"].to_numpy()
+        return None
+
+    @property
+    def pc1_z(self) -> np.array:
+        """Returns z coordinates of point cloud 1 of the correspondences."""
+        if "pc1_idx" in self._df:
+            return self._pc1.iloc[self._df["pc1_idx"]]["z"].to_numpy()
+        return None
+
+    @property
+    def pc1_nx(self) -> np.array:
+        """Returns x component of the normal vectors of point cloud 1 of the correspondences."""
+        if "pc1_idx" in self._df:
+            return self._pc1.iloc[self._df["pc1_idx"]]["nx"].to_numpy()
+        return None
+
+    @property
+    def pc1_ny(self) -> np.array:
+        """Returns y component of the normal vectors of point cloud 1 of the correspondences."""
+        if "pc1_idx" in self._df:
+            return self._pc1.iloc[self._df["pc1_idx"]]["ny"].to_numpy()
+        return None
+
+    @property
+    def pc1_nz(self) -> np.array:
+        """Returns z component of the normal vectors of point cloud 1 of the correspondences."""
+        if "pc1_idx" in self._df:
+            return self._pc1.iloc[self._df["pc1_idx"]]["nz"].to_numpy()
+        return None
+
+    @property
+    def pc2_x(self) -> np.array:
+        """Returns x coordinates of point cloud 2 of the correspondences."""
+        if "pc2_idx" in self._df:
+            return self._pc2.iloc[self._df["pc2_idx"]]["x"].to_numpy()
+        return None
+
+    @property
+    def pc2_y(self) -> np.array:
+        """Returns y coordinates of point cloud 2 of the correspondences."""
+        if "pc2_idx" in self._df:
+            return self._pc2.iloc[self._df["pc2_idx"]]["y"].to_numpy()
+        return None
+
+    @property
+    def pc2_z(self) -> np.array:
+        """Returns z coordinates of point cloud 2 of the correspondences."""
+        if "pc2_idx" in self._df:
+            return self._pc2.iloc[self._df["pc2_idx"]]["z"].to_numpy()
+        return None
+
+    @property
+    def pc2_nx(self) -> np.array:
+        """Returns x component of the normal vectors of point cloud 2 of the correspondences."""
+        if "pc2_idx" in self._df:
+            return self._pc2.iloc[self._df["pc2_idx"]]["nx"].to_numpy()
+        return None
+
+    @property
+    def pc2_ny(self) -> np.array:
+        """Returns y component of the normal vectors of point cloud 2 of the correspondences."""
+        if "pc2_idx" in self._df:
+            return self._pc2.iloc[self._df["pc2_idx"]]["ny"].to_numpy()
+        return None
+
+    @property
+    def pc2_nz(self) -> np.array:
+        """Returns z component of the normal vectors of point cloud 2 of the correspondences."""
+        if "pc2_idx" in self._df:
+            return self._pc2.iloc[self._df["pc2_idx"]]["nz"].to_numpy()
+        return None
+
+    @property
+    def point_to_plane_distances(self) -> np.array:
+        """Returns point-to-plane distances of correspondences."""
+        return self._df["point_to_plane_distances"].to_numpy()
+
+    @property
+    def num_corr_pts(self) -> int:
+        """Returns number of corresponding points."""
+        return len(self._df)
+
+    def match(self) -> None:
+        """Matching of point clouds.
+
+        For each *selected* point in self._pc1 the nearest neighbor in all *selected* points of
+        self._pc2 is searched.
+        """
+
+        kdtree = spatial.cKDTree(self._pc2.X_selected)
+        _, idx_nn = kdtree.query(self._pc1.X_selected, k=1, p=2, n_jobs=-1)
+
+        self._df["pc1_idx"] = self._pc1.idx_selected
+        self._df["pc2_idx"] = self._pc2.idx_selected[idx_nn]
+
+        self.__compute_point_to_plane_distances()
+
+    def reject_wrt_planarity(self, min_planarity: float) -> None:
+        """Rejects corresponding points w.r.t. the planarity of the points.
+
+        Notes:
+            - Point attribute "planarity" is estimated in PointCloud.estimate_normals() and is
+              defined between 0 and 1.
+            - The "planarity" attribute of both point clouds is checked.
+
+        Args:
+            min_planarity (float): Minimum planarity value needed to keep correspondence alive.
+        """
+
+        if "planarity" in self._pc1:
+            keep = (
+                self._pc1.iloc[self._df["pc1_idx"]]["planarity"].to_numpy()
+                >= min_planarity
+            )
+            self._df = self._df.loc[keep][:]
+
+        if "planarity" in self._pc2:
+            keep = (
+                self._pc2.iloc[self._df["pc2_idx"]]["planarity"].to_numpy()
+                >= min_planarity
+            )
+            self._df = self._df.loc[keep][:]
+
+    def reject_wrt_point_to_plane_distances(self) -> None:
+        """Reject corresponding points w.r.t. to their point-to-plane distance.
+
+        This method is used to remove correspondence outliers. All correspondences with a
+        point-to-plane distance (dp) outside the range
+
+        [median-3*sigma_mad(dp) median+3*sigma_mad(dp)]
+
+        are rejected.
+
+        Note: sigma_mad is a robust estimator for the standard deviation of a data set under the
+        assumption that the set has a Gaussian distribution:
+
+        sigma_mad = 1.4812 * mad
+
+        where mad is the median of the absolute differences (with respect to the median) of the data
+        set.
+        """
+
+        distances = self._df["point_to_plane_distances"]
+        median = np.median(distances)
+        sigma_mad = stats.median_absolute_deviation(distances)
+        keep = [abs(d - median) <= 3 * sigma_mad for d in distances]
+        self._df = self._df.loc[keep][:]
+
+    def reject_wrt_to_angle_between_normals(self) -> None:
+        """Reject corresponding points w.r.t. to the angle between their normal vectors."""
+
+        raise NotImplementedError
+
+    def __compute_point_to_plane_distances(self) -> None:
+        """Compute point-to-plane distances between corresponding points."""
+
+        dx = self.pc2_x - self.pc1_x
+        dy = self.pc2_y - self.pc1_y
+        dz = self.pc2_z - self.pc1_z
+
+        nx = self.pc1_nx
+        ny = self.pc1_ny
+        nz = self.pc1_nz
+
+        point_to_plane_distances = np.empty(self.num_corr_pts)
+
+        for i in range(0, self.num_corr_pts):
+            point_to_plane_distances[i] = dx[i] * nx[i] + dy[i] * ny[i] + dz[i] * nz[i]
+
+        self._df["point_to_plane_distances"] = point_to_plane_distances
diff --git a/python/simpleicp/mathutils.py b/python/simpleicp/mathutils.py
new file mode 100644
index 0000000..97e8c92
--- /dev/null
+++ b/python/simpleicp/mathutils.py
@@ -0,0 +1,93 @@
+"""
+Collection of math utility functions used by > 1 modules.
+"""
+
+from typing import Tuple
+
+import numpy as np
+
+
+def euler_coord_to_homogeneous_coord(Xe: np.array) -> np.array:
+    """Convert Euler coordinates to homogeneous coordinates."""
+
+    no_points = np.shape(Xe)[0]
+    Xh = np.column_stack((Xe, np.ones(no_points)))
+
+    return Xh
+
+
+def homogeneous_coord_to_euler_coord(Xh: np.array) -> np.array:
+    """Convert homogeneous coordinates to Euler coordinates."""
+
+    Xe = np.column_stack(
+        (Xh[:, 0] / Xh[:, 3], Xh[:, 1] / Xh[:, 3], Xh[:, 2] / Xh[:, 3])
+    )
+
+    return Xe
+
+
+def euler_angles_to_linearized_rotation_matrix(
+    alpha1: float, alpha2: float, alpha3: float
+) -> np.array:
+    """Compute linearized rotation matrix from three Euler angles."""
+
+    dR = np.array([[1, -alpha3, alpha2], [alpha3, 1, -alpha1], [-alpha2, alpha1, 1]])
+
+    return dR
+
+
+def euler_angles_to_rotation_matrix(
+    alpha1: float, alpha2: float, alpha3: float
+) -> np.array:
+    """Get Euler angles from rotation matrix."""
+
+    R = np.array(
+        [
+            [
+                np.cos(alpha2) * np.cos(alpha3),
+                -np.cos(alpha2) * np.sin(alpha3),
+                np.sin(alpha2),
+            ],
+            [
+                np.cos(alpha1) * np.sin(alpha3)
+                + np.sin(alpha1) * np.sin(alpha2) * np.cos(alpha3),
+                np.cos(alpha1) * np.cos(alpha3)
+                - np.sin(alpha1) * np.sin(alpha2) * np.sin(alpha3),
+                -np.sin(alpha1) * np.cos(alpha2),
+            ],
+            [
+                np.sin(alpha1) * np.sin(alpha3)
+                - np.cos(alpha1) * np.sin(alpha2) * np.cos(alpha3),
+                np.sin(alpha1) * np.cos(alpha3)
+                + np.cos(alpha1) * np.sin(alpha2) * np.sin(alpha3),
+                np.cos(alpha1) * np.cos(alpha2),
+            ],
+        ]
+    )
+
+    return R
+
+
+def rotation_matrix_to_euler_angles(R: np.array) -> Tuple[float, float, float]:
+    """Extract Euler angles from rotation matrix R."""
+
+    alpha1 = np.arctan2(-R[1, 2], R[2, 2])
+    alpha2 = np.arcsin(R[0, 2])
+    alpha3 = np.arctan2(-R[0, 1], R[0, 0])
+
+    return alpha1, alpha2, alpha3
+
+
+def create_homogeneous_transformation_matrix(R: np.array, t: np.array) -> np.array:
+    """Create homogeneous transformation matrix from rotation matrix R and translation vector t."""
+
+    H = np.array(
+        [
+            [R[0, 0], R[0, 1], R[0, 2], t[0]],
+            [R[1, 0], R[1, 1], R[1, 2], t[1]],
+            [R[2, 0], R[2, 1], R[2, 2], t[2]],
+            [0, 0, 0, 1],
+        ]
+    )
+
+    return H
diff --git a/python/simpleicp/optimization.py b/python/simpleicp/optimization.py
new file mode 100644
index 0000000..7f3ed02
--- /dev/null
+++ b/python/simpleicp/optimization.py
@@ -0,0 +1,388 @@
+"""
+SimpleICPOptimization class.
+"""
+
+from __future__ import (
+    annotations,
+)  # needed for type hinting of RigidBodyParameters before it is defined
+
+from dataclasses import dataclass, fields
+from typing import List, Optional, Tuple
+
+import lmfit
+import numpy as np
+
+from . import corrpts, mathutils
+
+
+class SimpleICPOptimization:
+    """Class for setting up and solve least squares optimization for simpleICP.
+
+    Dev notes:
+        - The basic structure of the class does not depend on the solver specifically used to
+        solve the LS optimization, e.g. lmfit. Consequently, it should be fairly easy to substitute
+        the solver in the future.
+    """
+
+    def __init__(
+        self,
+        corr_pts: corrpts.CorrPts,
+        distance_weights: Optional[float],  # can also be None
+        rbp_initial_values: Tuple[float],
+        rbp_observed_values: Tuple[float],
+        rbp_observation_weights: Tuple[float],
+    ) -> None:
+        """[summary]
+
+        Args:
+            corr_pts (corrpts.CorrPts): Corresponding points between pc1 and pc2.
+            distance_weights (Optional[float]): See docstring of SimpleICP.run.
+            rbp_observed_values (Tuple[float]): See docstring of SimpleICP.run.
+            rbp_observation_weights (Tuple[float]): See docstring of SimpleICP.run.
+        """
+        self._cp = corr_pts
+
+        self._rbp = RigidBodyParameters()
+        self._rbp.set_parameter_attributes_from_list(
+            "initial_value", rbp_initial_values
+        )
+        self._rbp.set_parameter_attributes_from_list(
+            "observed_value", rbp_observed_values
+        )
+        self._rbp.set_parameter_attributes_from_list(
+            "observation_weight", rbp_observation_weights
+        )
+
+        self._distance_weights = distance_weights
+
+        self._optim_results = None
+
+    @property
+    def rbp(self) -> RigidBodyParameters:
+        """Returns rigid body parameters."""
+        return self._rbp
+
+    def estimate_parameters(self) -> np.array:
+        """Estimate values of unknown parameters by setting up and solving a LS optimization.
+
+        Returns:
+            np.array: Distance residuals. The distances are signed point-to-plane distances.
+        """
+        # Define rigid body parameters for lmfit
+        params = lmfit.Parameters()
+        for parameter in fields(self._rbp):
+
+            initial_value = getattr(self._rbp, parameter.name).initial_value
+            observed_value = getattr(self._rbp, parameter.name).observed_value
+            observation_weight = getattr(self._rbp, parameter.name).observation_weight
+            vary = np.isfinite(observation_weight)
+
+            params[parameter.name] = lmfit.Parameter(
+                name=parameter.name,
+                value=initial_value,
+                vary=vary,
+                user_data={
+                    "observed_value": observed_value,
+                    "observation_weight": observation_weight,
+                    "is_observed": observation_weight > 0
+                    and np.isfinite(observation_weight),
+                },
+            )
+
+        # Optimize with lmfit
+        self._optim_results = lmfit.minimize(
+            SimpleICPOptimization.__residuals,
+            params,
+            method="least_squares",
+            args=(
+                self._cp,
+                self._distance_weights,
+            ),
+        )
+
+        # Save estimated values
+        estimated_values = [
+            self._optim_results.params["alpha1"].value,
+            self._optim_results.params["alpha2"].value,
+            self._optim_results.params["alpha3"].value,
+            self._optim_results.params["tx"].value,
+            self._optim_results.params["ty"].value,
+            self._optim_results.params["tz"].value,
+        ]
+        self._rbp.set_parameter_attributes_from_list(
+            "estimated_value", estimated_values
+        )
+
+        # Compute unweighted distance residuals
+        weighted_distance_residuals = self._optim_results.residual[
+            0 : self._cp.num_corr_pts - 1
+        ]
+        unweighted_distance_residuals = (
+            weighted_distance_residuals / self._distance_weights
+        )
+
+        return unweighted_distance_residuals
+
+    def estimate_parameter_uncertainties(self):
+        """Estimate the uncertainties of the unknown parameters.
+
+        The uncertainty of a parameter is defined as the a posteriori standard deviation of the
+        estimated value.
+        """
+        # Attention: Unfortunately we can not simply use the stderr values which lmfit provides
+        # (e.g. out.params["alpha1"].stderr) for each parameter if the residuals are weighted.
+        # Instead we need to "unweight" the Jacobian A and the residuals and calculate the
+        # uncertainties as usual from them.
+
+        # Create array with weights of all observations
+        weights = np.full((self._cp.num_corr_pts,), self._distance_weights)
+        for param_name in self._optim_results.params:
+            if self._optim_results.params[param_name].user_data["is_observed"]:
+                observation_weight = self._optim_results.params[param_name].user_data[
+                    "observation_weight"
+                ]
+                weights = np.append(weights, observation_weight)
+
+        # Compute unweighted Jakobian A
+        A_unweighted = np.empty_like(self._optim_results.jac)
+        for col in range(0, A_unweighted.shape[1]):
+            A_unweighted[:, col] = self._optim_results.jac[:, col] / weights
+
+        # Compute unweighted residuals
+        residuals_unweighted = self._optim_results.residual / weights
+
+        # Compute covariance matrix
+        P = np.diag(weights)
+        N = A_unweighted.T @ P @ A_unweighted
+        Qxx = np.linalg.inv(N)
+        vPv = np.sum(weights * residuals_unweighted ** 2)
+        [num_obs, num_prm] = np.shape(self._optim_results.jac)
+        s0 = np.sqrt(vPv / (num_obs - num_prm))
+        Cxx = s0 ** 2 * Qxx
+
+        # Get uncertainties from covariance matrix
+        idx_prm = 0
+        for param_name in self._optim_results.params:
+            if self._optim_results.params[param_name].vary:
+                getattr(self._rbp, param_name).estimated_uncertainty = np.sqrt(
+                    Cxx[idx_prm, idx_prm]
+                )
+                idx_prm += 1
+
+    @staticmethod
+    def __residuals(
+        params: lmfit.Parameters,
+        cp: corrpts.CorrPts,
+        distance_weights: float,
+    ) -> np.array:
+        """Returns weighted residuals as numpy array for lmfit."""
+        (
+            pc2_x_transformed,
+            pc2_y_transformed,
+            pc2_z_transformed,
+        ) = SimpleICPOptimization.__transform_point_cloud(
+            params, cp.pc2_x, cp.pc2_y, cp.pc2_z
+        )
+
+        weighted_distance_residuals = (
+            SimpleICPOptimization.__compute_weighted_distance_residuals(
+                cp.pc1_x,
+                cp.pc1_y,
+                cp.pc1_z,
+                cp.pc1_nx,
+                cp.pc1_ny,
+                cp.pc1_nz,
+                pc2_x_transformed,
+                pc2_y_transformed,
+                pc2_z_transformed,
+                distance_weights,
+            )
+        )
+
+        weighted_rbp_observation_residuals = (
+            SimpleICPOptimization.__compute_weighted_rbp_observation_residuals(params)
+        )
+
+        # Combine all residuals
+        weighted_residuals = np.concatenate(
+            (weighted_distance_residuals, weighted_rbp_observation_residuals)
+        )
+
+        return weighted_residuals
+
+    @staticmethod
+    def __transform_point_cloud(
+        params: lmfit.Parameters, x: np.array, y: np.array, z: np.array
+    ) -> Tuple[np.array, np.array, np.array]:
+        """Transform point cloud by rigid body transformation."""
+        R = mathutils.euler_angles_to_rotation_matrix(
+            params["alpha1"].value,
+            params["alpha2"].value,
+            params["alpha3"].value,
+        )
+
+        t = np.array(
+            [
+                params["tx"].value,
+                params["ty"].value,
+                params["tz"].value,
+            ]
+        )
+
+        H = mathutils.create_homogeneous_transformation_matrix(R, t)
+
+        Xe = np.column_stack((x, y, z))
+        Xh = mathutils.euler_coord_to_homogeneous_coord(Xe)
+        Xh_transformed = np.transpose(H @ Xh.T)
+        Xe_transformed = mathutils.homogeneous_coord_to_euler_coord(Xh_transformed)
+
+        x_transformed = Xe_transformed[:, 0]
+        y_transformed = Xe_transformed[:, 1]
+        z_transformed = Xe_transformed[:, 2]
+
+        return x_transformed, y_transformed, z_transformed
+
+    @staticmethod
+    def __compute_weighted_distance_residuals(
+        pc1_x: np.array,
+        pc1_y: np.array,
+        pc1_z: np.array,
+        pc1_nx: np.array,
+        pc1_ny: np.array,
+        pc1_nz: np.array,
+        pc2_x: np.array,
+        pc2_y: np.array,
+        pc2_z: np.array,
+        distance_weights: float,
+    ) -> np.array:
+        """Compute weighted residuals for point-to-plane distances between correspondences."""
+        no_correspondences = len(pc1_x)
+
+        dx = pc2_x - pc1_x
+        dy = pc2_y - pc1_y
+        dz = pc2_z - pc1_z
+
+        distance_residuals = np.empty(no_correspondences)
+        for i in range(0, no_correspondences):
+            distance_residuals[i] = (
+                dx[i] * pc1_nx[i] + dy[i] * pc1_ny[i] + dz[i] * pc1_nz[i]
+            )
+
+        weighted_distance_residuals = distance_weights * distance_residuals
+
+        return weighted_distance_residuals
+
+    @staticmethod
+    def __compute_weighted_rbp_observation_residuals(
+        params: lmfit.Parameters,
+    ) -> np.array:
+        """Compute weighted residuals for direct observation of rbp."""
+
+        weighted_residual_list = []
+
+        for param_name in params:
+            if params[param_name].user_data["is_observed"]:
+                residual = (
+                    params[param_name].value
+                    - params[param_name].user_data["observed_value"]
+                )
+                weighted_residual = (
+                    params[param_name].user_data["observation_weight"] * residual
+                )
+                weighted_residual_list.append(weighted_residual)
+
+        return np.array(weighted_residual_list)
+
+
+@dataclass
+class Parameter:
+    """Data class for a single optimization parameter."""
+
+    initial_value: float = np.nan
+    observed_value: float = np.nan
+    observation_weight: float = np.nan
+    estimated_value: float = np.nan
+    estimated_uncertainty: float = np.nan
+    scale_for_logging: float = 1
+
+    @property
+    def initial_value_scaled(self):
+        """Returns scaled initial value for logging."""
+        return self.initial_value * self.scale_for_logging
+
+    @property
+    def observed_value_scaled(self):
+        """Returns observed value for logging."""
+        return self.observed_value * self.scale_for_logging
+
+    @property
+    def estimated_value_scaled(self):
+        """Returns estimated value for logging."""
+        return self.estimated_value * self.scale_for_logging
+
+    @property
+    def estimated_uncertainty_scaled(self):
+        """Returns uncertainty for logging."""
+        return self.estimated_uncertainty * self.scale_for_logging
+
+
+@dataclass
+class RigidBodyParameters:
+    """Data class for rigid body parameters."""
+
+    alpha1: Parameter = Parameter(scale_for_logging=180 / np.pi)
+    alpha2: Parameter = Parameter(scale_for_logging=180 / np.pi)
+    alpha3: Parameter = Parameter(scale_for_logging=180 / np.pi)
+    tx: Parameter = Parameter()
+    ty: Parameter = Parameter()
+    tz: Parameter = Parameter()
+
+    @property
+    def H(self) -> np.array:
+        """Returns homogeneous transformation matrix from estimated values."""
+
+        R = mathutils.euler_angles_to_rotation_matrix(
+            self.alpha1.estimated_value,
+            self.alpha2.estimated_value,
+            self.alpha3.estimated_value,
+        )
+
+        t = np.array(
+            [self.tx.estimated_value, self.ty.estimated_value, self.tz.estimated_value]
+        )
+
+        H = mathutils.create_homogeneous_transformation_matrix(R, t)
+
+        return H
+
+    def set_parameter_attributes_from_list(self, attribute_name: str, array: List):
+        """Set an attribute for all six rbp parameters from a given array.
+
+        Args:
+            attribute_name (str): Name of attribute.
+            array (List): Array with 6 elements with attribute values.
+        """
+        setattr(self.alpha1, attribute_name, array[0])
+        setattr(self.alpha2, attribute_name, array[1])
+        setattr(self.alpha3, attribute_name, array[2])
+        setattr(self.tx, attribute_name, array[3])
+        setattr(self.ty, attribute_name, array[4])
+        setattr(self.tz, attribute_name, array[5])
+
+    def get_parameter_attributes_as_list(self, attribute_name: str) -> List:
+        """Returns an attribute of all six rbp parameters as array.
+
+        Args:
+            attribute_name (str): Name of attribute.
+
+        Returns:
+            List: Attribute values as array.
+        """
+        return [
+            getattr(self.alpha1, attribute_name),
+            getattr(self.alpha2, attribute_name),
+            getattr(self.alpha3, attribute_name),
+            getattr(self.tx, attribute_name),
+            getattr(self.ty, attribute_name),
+            getattr(self.tz, attribute_name),
+        ]
diff --git a/python/simpleicp/pointcloud.py b/python/simpleicp/pointcloud.py
index e070d96..74aae34 100644
--- a/python/simpleicp/pointcloud.py
+++ b/python/simpleicp/pointcloud.py
@@ -2,129 +2,219 @@
 PointCloud class.
 """
 
+from typing import List
+
 import numpy as np
+import pandas as pd
 from scipy import spatial
 
+from . import mathutils
 
-class PointCloud:
-    """Class for working with point cloud data."""
-
-    def __init__(self, x: np.array, y: np.array, z: np.array):
-        self.x = x
-        self.y = y
-        self.z = z
-
-        self.nx = None
-        self.ny = None
-        self.nz = None
-        self.planarity = None
-
-        self.no_points = len(x)
-        self.sel = np.arange(0, len(x), 1)
 
-    def select_in_range(self, X: np.array, max_range: float):
-        """Select points within range of points X."""
+class PointCloud(pd.DataFrame):
+    """Class for working with point cloud data.
 
-        assert np.shape(X)[1] == 3, "X must have 3 columns!"
+    Dev notes:
+        - The PointCloud class is a child class of pandas.DataFrame.
+        - All attributes which should not be changed outside this class are declared as private,
+        e.g. self._num_points. To access these attributes outside this class, getter methods are
+        defined, e.g. self.num_points.
+    """
 
-        kdtree = spatial.cKDTree(X)
-
-        distances, _ = kdtree.query(
-            self.X_sel, k=1, p=2, distance_upper_bound=max_range, workers=-1
-        )
+    def __init__(self, *args, **kwargs) -> None:
+        """Constructor method.
 
-        keep = np.isfinite(distances)  # distances > max_range are infinite
+        The PointCloud class is a child class of pandas.DataFrame. Thus, when creating an object
+        all arguments are passes to the pandas.DataFrame constructor:
+        https://pandas.pydata.org/pandas-docs/stable/reference/api/pandas.DataFrame.html
 
-        self.sel = self.sel[keep]
+        Note that the created DataFrame must contain the columns "x", "y", and "z" for the
+        coordinates of the points.
 
-    def select_n_points(self, n: int):
-        """Select n points equidistantly."""
+        An additional column "selected" of dtype bool is automatically added if missing. It is used
+        to mark a selected subset of points.
+        """
+        super().__init__(*args, **kwargs)
 
-        if self.no_selected_points > n:
-            idx = np.round(np.linspace(0, self.no_selected_points - 1, n)).astype(int)
-            self.sel = self.sel[idx]
+        for coordinate in ("x", "y", "z"):
+            if coordinate not in self:
+                raise PointCloudException(
+                    f'Column "{coordinate}" is missing in DataFrame.'
+                )
 
-    def estimate_normals(self, neighbors: int):
-        """Estimate normal vectors for selected points from its neighborhood."""
+        self._num_points = len(self)
 
-        self.nx = np.full(self.no_points, np.nan)
-        self.ny = np.full(self.no_points, np.nan)
-        self.nz = np.full(self.no_points, np.nan)
-        self.planarity = np.full(self.no_points, np.nan)
+        if "selected" not in self:
+            self["selected"] = [True] * self._num_points
 
-        kdtree = spatial.cKDTree(self.X)
-        _, idxNN_all_qp = kdtree.query(self.X_sel, k=neighbors, p=2, workers=-1)
+    @property
+    def x(self) -> np.array:
+        """Returns x coordinates of all points as 1-dim numpy array."""
+        return self["x"].to_numpy()
 
-        for (i, idxNN) in enumerate(idxNN_all_qp):
-            selected_points = np.column_stack(
-                (self.x[idxNN], self.y[idxNN], self.z[idxNN])
-            )
-            C = np.cov(selected_points.T, bias=False)
-            eig_vals, eig_vecs = np.linalg.eig(C)
-            idx_sort = eig_vals.argsort()[::-1]  # sort from large to small
-            eig_vals = eig_vals[idx_sort]
-            eig_vecs = eig_vecs[:, idx_sort]
-            self.nx[self.sel[i]] = eig_vecs[0, 2]
-            self.ny[self.sel[i]] = eig_vecs[1, 2]
-            self.nz[self.sel[i]] = eig_vecs[2, 2]
-            self.planarity[self.sel[i]] = (eig_vals[1] - eig_vals[2]) / eig_vals[0]
+    @property
+    def x_selected(self) -> np.array:
+        """Returns x coordinates of selected points as 1-dim numpy array."""
+        return self.loc[self["selected"], "x"].to_numpy()
 
-    def transform(self, H: np.array):
-        """Transform point cloud by given homogeneous transformation matrix H."""
+    @property
+    def y(self) -> np.array:
+        """Returns y coordinates of all points as 1-dim numpy array."""
+        return self["y"].to_numpy()
 
-        XInH = PointCloud.euler_coord_to_homogeneous_coord(self.X)
-        XOutH = np.transpose(H @ XInH.T)
-        XOut = PointCloud.homogeneous_coord_to_euler_coord(XOutH)
+    @property
+    def y_selected(self) -> np.array:
+        """Returns y coordinates of selected points as 1-dim numpy array."""
+        return self.loc[self["selected"], "y"].to_numpy()
 
-        self.x = XOut[:, 0]
-        self.y = XOut[:, 1]
-        self.z = XOut[:, 2]
+    @property
+    def z(self) -> np.array:
+        """Returns z coordinates of all points as 1-dim numpy array."""
+        return self["z"].to_numpy()
 
     @property
-    def x_sel(self) -> np.array:
-        """Returns x coordinates of selected points."""
-        return self.x[self.sel]
+    def z_selected(self) -> np.array:
+        """Returns z coordinates of selected points as 1-dim numpy array."""
+        return self.loc[self["selected"], "z"].to_numpy()
 
     @property
-    def y_sel(self) -> np.array:
-        """Returns y coordinates of selected points."""
-        return self.y[self.sel]
+    def X(self) -> np.array:
+        """Returns x,y,z coordinates of all points as numpy array of shape (n,3)."""
+        return self[["x", "y", "z"]].to_numpy()
 
     @property
-    def z_sel(self) -> np.array:
-        """Returns z coordinates of selected points."""
-        return self.z[self.sel]
+    def X_selected(self) -> np.array:
+        """Returns x,y,z coordinates of selected points as numpy array of shape (n,3)."""
+        return self.loc[self["selected"], ["x", "y", "z"]].to_numpy()
 
     @property
-    def X(self) -> np.array:
-        """Returns n-by-3 matrix of all points."""
-        return np.column_stack((self.x, self.y, self.z))
+    def idx_selected(self) -> np.array:
+        """Returns indices of selected points as 1-dim numpy array."""
+        return np.where(self["selected"])[0]
+
+    @idx_selected.setter
+    def idx_selected(self, idx_selected: List[int]) -> None:
+        """Set indices of selected points."""
+        self.unselect_all_points()
+        self.loc[idx_selected, "selected"] = True
 
     @property
-    def X_sel(self) -> np.array:
-        """Returns n-by-3 matrix of selected points."""
-        return np.column_stack((self.x[self.sel], self.y[self.sel], self.z[self.sel]))
+    def num_points(self) -> int:
+        """Returns total number of points."""
+        return self._num_points
 
     @property
-    def no_selected_points(self) -> int:
+    def num_selected_points(self) -> int:
         """Returns number of selected points."""
-        return len(self.sel)
+        return sum(self["selected"])
+
+    def select_all_points(self) -> None:
+        """Select all points."""
+        self["selected"].values[:] = True
+
+    def unselect_all_points(self) -> None:
+        """Unselect all points."""
+        self["selected"].values[:] = False
+
+    def select_by_indices(self, indices: List[int]) -> None:
+        """Select points by a list of indices.
 
-    @staticmethod
-    def euler_coord_to_homogeneous_coord(XE: np.array):
-        """Convert Euler coordinates to homogeneous coordinates."""
+        Note that this method selects a subset of the currently selected points. I.e., if a point
+        is unselected but its index is contained in idx_selected, it remains unselected. To avoid
+        this, call select_all_points() beforehand.
 
-        no_points = np.shape(XE)[0]
-        XH = np.column_stack((XE, np.ones(no_points)))
+        Args:
+            indices (List[int]): List of indices to select.
+        """
+        self.idx_selected = np.intersect1d(self.idx_selected, indices)
 
-        return XH
+    def select_n_points(self, n: int) -> None:
+        """Select n points.
 
-    @staticmethod
-    def homogeneous_coord_to_euler_coord(XH: np.array):
-        """Convert homogeneous coordinates to Euler coordinates."""
+        Note that this method selects a subset of the currently selected points. Points are selected
+        equidistantly accross the indices of the currently selected points.
 
-        XE = np.column_stack(
-            (XH[:, 0] / XH[:, 3], XH[:, 1] / XH[:, 3], XH[:, 2] / XH[:, 3])
+        Args:
+            n (int): Number of points to be selected.
+        """
+        if self.num_selected_points > n:
+            idx_subset_n_points = np.round(
+                np.linspace(0, self.num_selected_points - 1, n)
+            ).astype(int)
+            idx_selected_new = self.idx_selected[idx_subset_n_points]
+            self.unselect_all_points()
+            self.loc[idx_selected_new, "selected"] = True
+
+    def select_in_range(self, X: np.array, max_range: float) -> None:
+        """Select points within range of points X.
+
+        Note that this method selects a subset of the currently selected points.
+
+        Args:
+            X (np.array): Points of shape (n,3).
+            max_range (float): Maximum range from points X.
+        """
+        if np.shape(X)[1] != 3:
+            raise PointCloudException("X must have 3 columns!")
+
+        kdtree = spatial.cKDTree(X)
+
+        distances, _ = kdtree.query(
+            self.X_selected, k=1, p=2, distance_upper_bound=max_range, workers=-1
         )
 
-        return XE
+        keep = np.isfinite(distances)  # distances > max_range are infinite
+
+        idx_selected_new = self.idx_selected[keep]
+        self.unselect_all_points()
+        self.loc[idx_selected_new, "selected"] = True
+
+    def estimate_normals(self, neighbors: int) -> None:
+        """Estimate normal vectors for selected points from its neighborhood.
+
+        Args:
+            neighbors (int): Number of nearest neighbors to use for normal vector estimation.
+        """
+        # Initialize point attributes
+        nx = np.full((self._num_points,), np.nan, dtype=np.float32)
+        ny = np.full((self._num_points,), np.nan, dtype=np.float32)
+        nz = np.full((self._num_points,), np.nan, dtype=np.float32)
+        planarity = np.full((self._num_points,), np.nan, dtype=np.float32)
+
+        kdtree = spatial.cKDTree(self.X)
+        _, idxNN_all_qp = kdtree.query(self.X_selected, k=neighbors, p=2, workers=-1)
+
+        for (i, idxNN) in enumerate(idxNN_all_qp):
+            selected_points = self.X[idxNN, :]
+            C = np.cov(selected_points.T, bias=False)
+            eig_vals, eig_vecs = np.linalg.eig(C)
+            idx_sort = eig_vals.argsort()[::-1]  # sort from large to small
+            eig_vals = eig_vals[idx_sort]
+            eig_vecs = eig_vecs[:, idx_sort]
+            nx[self.idx_selected[i]] = eig_vecs[0, 2]
+            ny[self.idx_selected[i]] = eig_vecs[1, 2]
+            nz[self.idx_selected[i]] = eig_vecs[2, 2]
+            planarity[self.idx_selected[i]] = (eig_vals[1] - eig_vals[2]) / eig_vals[0]
+
+        self["nx"] = pd.arrays.SparseArray(nx)
+        self["ny"] = pd.arrays.SparseArray(ny)
+        self["nz"] = pd.arrays.SparseArray(nz)
+        self["planarity"] = pd.arrays.SparseArray(planarity)
+
+    def transform_by_H(self, H: np.array) -> None:
+        """Transform points by applying a homogeneous transformation matrix H.
+
+        Args:
+            H (np.array): Homogenous transformation matrix H of shape (4,4).
+        """
+        Xh = mathutils.euler_coord_to_homogeneous_coord(self.X)
+        Xh = np.transpose(H @ Xh.T)  # transform in-place to save memory
+        Xe = mathutils.homogeneous_coord_to_euler_coord(Xh)
+
+        self["x"] = Xe[:, 0]
+        self["y"] = Xe[:, 1]
+        self["z"] = Xe[:, 2]
+
+
+class PointCloudException(Exception):
+    """The PointCloud class raises this when the class is misused."""
diff --git a/python/simpleicp/simpleicp.py b/python/simpleicp/simpleicp.py
index fc9dcf8..9dbc304 100644
--- a/python/simpleicp/simpleicp.py
+++ b/python/simpleicp/simpleicp.py
@@ -1,232 +1,286 @@
 """
 Implementation of a rather simple version of the Iterative Closest Point (ICP) algorithm.
+
+Dev notes:
+    - Define type hints for all parameters and return values.
+    - All non-private functions and class methods should have extended docstrings, i.e. including
+    parameter and return values description. For the rest single-line docstrings suffice.
 """
 
 import time
-from typing import Tuple
+from dataclasses import fields
+from typing import Optional, Tuple
 
 import numpy as np
-from scipy import spatial, stats
-
-from .pointcloud import PointCloud
-
-
-def matching(pcfix: PointCloud, pcmov: PointCloud) -> np.array:
-    """Matching of point clouds."""
-    kdtree = spatial.cKDTree(pcmov.X)
-    _, pcmov.sel = kdtree.query(pcfix.X_sel, k=1, p=2, n_jobs=-1)
-
-    dx = pcmov.x_sel - pcfix.x_sel
-    dy = pcmov.y_sel - pcfix.y_sel
-    dz = pcmov.z_sel - pcfix.z_sel
-
-    nx = pcfix.nx[pcfix.sel]
-    ny = pcfix.ny[pcfix.sel]
-    nz = pcfix.nz[pcfix.sel]
-
-    no_correspondences = pcfix.no_selected_points
-    distances = np.empty(no_correspondences)
-    for i in range(0, no_correspondences):
-        distances[i] = dx[i] * nx[i] + dy[i] * ny[i] + dz[i] * nz[i]
-
-    return distances
-
-
-def reject(
-    pcfix: PointCloud, pcmov: PointCloud, min_planarity: float, distances: np.array
-) -> np.array:
-    """Rejection of correspondences on the basis of multiple criterias."""
-
-    planarity = pcfix.planarity[pcfix.sel]
-
-    med = np.median(distances)
-    sigmad = stats.median_absolute_deviation(distances)
 
-    keep_distance = np.array([abs(d - med) <= 3 * sigmad for d in distances])
-    keep_planarity = np.array([p >= min_planarity for p in planarity])
-
-    keep = keep_distance & keep_planarity
-
-    pcfix.sel = pcfix.sel[keep]
-    pcmov.sel = pcmov.sel[keep]
-    distances = distances[keep]
-
-    return distances
-
-
-def estimate_rigid_body_transformation(
-    x_fix: np.array,
-    y_fix: np.array,
-    z_fix: np.array,
-    nx_fix: np.array,
-    ny_fix: np.array,
-    nz_fix: np.array,
-    x_mov: np.array,
-    y_mov: np.array,
-    z_mov: np.array,
-) -> Tuple[np.array, np.array]:
-    """Estimate rigid body transformation for a given set of correspondences."""
-
-    A = np.column_stack(
-        (
-            -z_mov * ny_fix + y_mov * nz_fix,
-            z_mov * nx_fix - x_mov * nz_fix,
-            -y_mov * nx_fix + x_mov * ny_fix,
-            nx_fix,
-            ny_fix,
-            nz_fix,
+from . import corrpts, optimization, pointcloud
+
+
+class SimpleICP:
+    """Class for setting up and run simpleICP."""
+
+    def __init__(self) -> None:
+        """Constructor method."""
+
+        self.pc1 = None
+        self.pc2 = None
+
+    def add_point_clouds(
+        self,
+        pc_fix: pointcloud.PointCloud,
+        pc_mov: pointcloud.PointCloud,
+    ) -> None:
+        """Add fixed and movable point cloud.
+
+        Args:
+            pc_fix (pointcloud.PointCloud): Fixed point cloud.
+            pc_mov (pointcloud.PointCloud): Movable point cloud. This point cloud
+                will be shifted and rotated (transformed) by applying a rigid body transformation.
+        """
+
+        # We pc_fix/pc_mov only for the user interface - internally they are pc1/pc2
+        self.pc1 = pc_fix
+        self.pc2 = pc_mov
+
+    def run(
+        self,
+        correspondences: int = 1000,
+        neighbors: int = 10,
+        min_planarity: float = 0.3,
+        max_overlap_distance: float = np.inf,
+        min_change: float = 1.0,
+        max_iterations: int = 100,
+        distance_weights: Optional[float] = 1,  # can also be None
+        rbp_observed_values: Tuple[float] = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+        rbp_observation_weights: Tuple[float] = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+    ) -> Tuple[np.array, np.array]:
+        """Run simpleICP algorithm.
+
+        Note: See https://github.com/pglira/simpleICP for an extended description of the algorithm
+            and arguments.
+
+        Args:
+            correspondences (int, optional): Number of correspondences selected initially in the
+                fixed point cloud for the subsequent point cloud matching. Defaults to 1000.
+            neighbors (int, optional): Number of neighboring points used to estimate the normal
+                vector and the planarity of the selected points. Defaults to 10.
+            min_planarity (float, optional): Minimum planarity value of a correspondence. Defaults
+                to 0.3.
+            max_overlap_distance (float, optional): Maximum overlap distance between the two point
+                clouds. Defaults to np.inf.
+            min_change (float, optional): Value to test a convergency criteria after each iteration:
+                if the mean and the standard deviation of the point-to-plane distances do not
+                change more than min_change (in %), the iteration is stopped. Defaults to 1.0.
+            max_iterations (int, optional): Maximum number of ICP iterations. Defaults to 100.
+            distance_weights (Optional[float], optional): Weight factor by which the point-to-plane
+                residuals are multiplied. Set to None for automatic estimation - however, this makes
+                only sense if the rigid body transformation parameters are observed, see the
+                following arguments. Defaults to 1.
+            rbp_observed_values (Tuple[float], optional): Values of direct observation of the rigid
+                body parameters. Defaults to (0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
+            rbp_observation_weights (Tuple[float], optional): Weight factors with which the
+                residuals of the direct observations are multiplied. The residuals are defined as
+                difference between the estimated rbp and the observed rbp. Defaults to
+                (0.0, 0.0, 0.0, 0.0, 0.0, 0.0).
+
+        Returns:
+            Tuple[np.array, np.array]:
+                H: Estimated homogeneous transformation matrix.
+                X_mov_transformed: Points of movable point cloud transformed by H.
+        """
+
+        self.__check_arguments(
+            distance_weights, rbp_observed_values, rbp_observation_weights
         )
-    )
 
-    l = nx_fix * (x_fix - x_mov) + ny_fix * (y_fix - y_mov) + nz_fix * (z_fix - z_mov)
+        start_time = time.time()
 
-    x, _, _, _ = np.linalg.lstsq(A, l, rcond=None)
+        if np.isfinite(max_overlap_distance):
 
-    residuals = A @ x - l
+            print("Consider partial overlap of point clouds ...")
+            self.pc1.select_in_range(self.pc2.X, max_range=max_overlap_distance)
 
-    R = euler_angles_to_linearized_rotation_matrix(x[0], x[1], x[2])
+            if not self.pc1.num_selected_points > 0:
+                raise SimpleICPException(
+                    "Point clouds do not overlap within max_overlap_distance = "
+                    f"{max_overlap_distance:.5f}! Consider increasing the value of "
+                    "max_overlap_distance."
+                )
 
-    t = x[3:6]
+        print("Select points for correspondences in fixed point cloud ...")
+        self.pc1.select_n_points(correspondences)
+        selected_orig = self.pc1["selected"]
 
-    H = create_homogeneous_transformation_matrix(R, t)
+        if not {"nx", "ny", "nz", "planarity"}.issubset(self.pc1.columns):
+            print("Estimate normals of selected points ...")
+            self.pc1.estimate_normals(neighbors)
 
-    return H, residuals
+        H = np.eye(4)
 
+        distance_residuals = []
 
-def euler_angles_to_linearized_rotation_matrix(
-    alpha1: float, alpha2: float, alpha3: float
-) -> np.array:
-    """Compute linearized rotation matrix from three Euler angles."""
+        print("Start iterations ...")
+        for it in range(0, max_iterations):
 
-    dR = np.array([[1, -alpha3, alpha2], [alpha3, 1, -alpha1], [-alpha2, alpha1, 1]])
+            cp = corrpts.CorrPts(self.pc1, self.pc2)
 
-    return dR
+            # We need to temporarily transform the moving point cloud for matching
+            self.pc2.transform_by_H(H)
+            cp.match()
+            self.pc2.transform_by_H(np.linalg.inv(H))
 
+            # Rejection of possibly false correspondences
+            cp.reject_wrt_planarity(min_planarity)
+            cp.reject_wrt_point_to_plane_distances()
+            # cp.reject_wrt_to_angle_between_normals()  # not implemented yet
 
-def create_homogeneous_transformation_matrix(R: np.array, t: np.array) -> np.array:
-    """Create homogeneous transformation matrix from rotation matrix R and translation vector t."""
+            if it == 0:
+                initial_distances = cp.point_to_plane_distances
+                rbp_initial_values = rbp_observed_values
+            else:
+                rbp_initial_values = rbp_estimated_values
 
-    H = np.array(
-        [
-            [R[0, 0], R[0, 1], R[0, 2], t[0]],
-            [R[1, 0], R[1, 1], R[1, 2], t[1]],
-            [R[2, 0], R[2, 1], R[2, 2], t[2]],
-            [0, 0, 0, 1],
-        ]
-    )
+            # Estimate weight of distances if value is None
+            if distance_weights is None:
+                distance_weights = 1 / (np.std(cp.point_to_plane_distances) ** 2)
 
-    return H
-
-
-def check_convergence_criteria(
-    distances_new: np.array, distances_old: np.array, min_change: float
-) -> bool:
-    """Check if the convergence criteria is met."""
-
-    def change(new, old):
-        return np.abs((new - old) / old * 100)
-
-    change_of_mean = change(np.mean(distances_new), np.mean(distances_old))
-    change_of_std = change(np.std(distances_new), np.std(distances_old))
+            optim = optimization.SimpleICPOptimization(
+                cp,
+                distance_weights,
+                rbp_initial_values,
+                rbp_observed_values,
+                rbp_observation_weights,
+            )
 
-    return True if change_of_mean < min_change and change_of_std < min_change else False
+            distance_residuals_new = optim.estimate_parameters()
+            rbp = optim.rbp
 
+            rbp_estimated_values = rbp.get_parameter_attributes_as_list(
+                "estimated_value"
+            )
+            H = rbp.H
+
+            distance_residuals.append(distance_residuals_new)
+
+            self.pc1["selected"] = selected_orig  # restore selected points
+
+            if it > 0:
+                if self.__check_convergence_criteria(
+                    distance_residuals[it], distance_residuals[it - 1], min_change
+                ):
+                    optim.estimate_parameter_uncertainties()
+                    print("Convergence criteria fulfilled -> stop iteration!")
+                    break
+
+            if it == 0:
+                print(
+                    f"{'iteration':>9s} | "
+                    f"{'correspondences':>15s} | "
+                    f"{'mean(residuals)':>15s} | "
+                    f"{'std(residuals)':>15s}"
+                )
+                print(
+                    f"{'orig:0':>9s} | "
+                    f"{len(initial_distances):15d} | "
+                    f"{np.mean(initial_distances):15.4f} | "
+                    f"{np.std(initial_distances):15.4f}"
+                )
+            print(
+                f"{it+1:9d} | "
+                f"{len(distance_residuals[it]):15d} | "
+                f"{np.mean(distance_residuals[it]):15.4f} | "
+                f"{np.std(distance_residuals[it]):15.4f}"
+            )
 
-def simpleicp(
-    X_fix: PointCloud,
-    X_mov: PointCloud,
-    correspondences: int = 1000,
-    neighbors: int = 10,
-    min_planarity: float = 0.3,
-    max_overlap_distance: float = np.inf,
-    min_change: float = 1.0,
-    max_iterations: int = 100,
-) -> Tuple[np.array, np.array]:
-    """Implementation of a rather simple version of the Iterative Closest Point (ICP) algorithm."""
+        print("Estimated transformation matrix H:")
+        print(f"[{H[0, 0]:12.6f} {H[0, 1]:12.6f} {H[0, 2]:12.6f} {H[0, 3]:12.6f}]")
+        print(f"[{H[1, 0]:12.6f} {H[1, 1]:12.6f} {H[1, 2]:12.6f} {H[1, 3]:12.6f}]")
+        print(f"[{H[2, 0]:12.6f} {H[2, 1]:12.6f} {H[2, 2]:12.6f} {H[2, 3]:12.6f}]")
+        print(f"[{H[3, 0]:12.6f} {H[3, 1]:12.6f} {H[3, 2]:12.6f} {H[3, 3]:12.6f}]")
 
-    start_time = time.time()
-    log("Create point cloud objects ...")
-    pcfix = PointCloud(X_fix[:, 0], X_fix[:, 1], X_fix[:, 2])
-    pcmov = PointCloud(X_mov[:, 0], X_mov[:, 1], X_mov[:, 2])
+        print(
+            "... which corresponds to the following rigid body transformation parameters:"
+        )
+        print(
+            f"{'parameter':>9s} | "
+            f"{'est.value':>15s} | "
+            f"{'est.uncertainty':>15s} | "
+            f"{'obs.value':>15s} | "
+            f"{'obs.weight':>15s}"
+        )
+        for parameter in fields(rbp):
+            print(
+                f"{parameter.name:>9s} | "
+                f"{getattr(rbp, parameter.name).estimated_value_scaled:15.6f} | "
+                f"{getattr(rbp, parameter.name).estimated_uncertainty_scaled:15.6f} | "
+                f"{getattr(rbp, parameter.name).observed_value_scaled:15.6f} | "
+                f"{getattr(rbp, parameter.name).observation_weight:15.6f}"
+            )
 
-    if np.isfinite(max_overlap_distance):
-        print("Consider partial overlap of point clouds ...")
-        pcfix.select_in_range(pcmov.X, max_range=max_overlap_distance)
-        assert pcfix.no_selected_points > 0, (
-            "Point clouds do not overlap within max_overlap_distance = ",
-            f"{max_overlap_distance:.5f}! Consider increasing the value of max_overlap_distance.",
+        print(
+            "(Unit of est.value, est.uncertainty, and obs.value for alpha1/2/3 is degree)"
         )
 
-    print(
-        "Select points for correspondences within overlap area of fixed point cloud ..."
-    )
-    pcfix.select_n_points(correspondences)
-    sel_orig = pcfix.sel
+        # Apply final transformation
+        self.pc2.transform_by_H(H)
 
-    print("Estimate normals of selected points ...")
-    pcfix.estimate_normals(neighbors)
+        print(f"Finished in {time.time() - start_time:.3f} seconds!")
 
-    H = np.eye(4)
-    residual_distances = []
+        return H, self.pc2.X
 
-    print("Start iterations ...")
-    for i in range(0, max_iterations):
+    @staticmethod
+    def __check_arguments(
+        distance_weights, rbp_observed_values, rbp_observation_weights
+    ):
+        """Some (i.e. not exhaustive) checks of the arguments passed to the constructor method."""
+        if distance_weights is not None:
+            if distance_weights <= 0:
+                raise SimpleICPException("distance_weights must be > 0.")
 
-        initial_distances = matching(pcfix, pcmov)
+        if not len(rbp_observed_values) == 6:
+            raise SimpleICPException(
+                "rbp_observed_values must have exactly 6 elements."
+            )
 
-        initial_distances = reject(pcfix, pcmov, min_planarity, initial_distances)
+        if not len(rbp_observation_weights) == 6:
+            raise SimpleICPException(
+                "rbp_observation_weights must have exactly 6 elements."
+            )
 
-        dH, residuals = estimate_rigid_body_transformation(
-            pcfix.x_sel,
-            pcfix.y_sel,
-            pcfix.z_sel,
-            pcfix.nx[pcfix.sel],
-            pcfix.ny[pcfix.sel],
-            pcfix.nz[pcfix.sel],
-            pcmov.x_sel,
-            pcmov.y_sel,
-            pcmov.z_sel,
-        )
+        if not all([w >= 0 for w in rbp_observation_weights]):
+            raise SimpleICPException(
+                "All elements of rbp_observation_weights must be >= 0."
+            )
 
-        residual_distances.append(residuals)
+        if not any(np.isfinite(rbp_observation_weights)):
+            raise SimpleICPException(
+                "At least one element in rbp_observation_weights must be finite."
+            )
 
-        pcmov.transform(dH)
+    @staticmethod
+    def __check_convergence_criteria(
+        distance_residuals_new: np.array,
+        distance_residuals_old: np.array,
+        min_change: float,
+    ) -> bool:
+        """Check if the convergence criteria is met."""
 
-        H = dH @ H
-        pcfix.sel = sel_orig
+        def change(new, old):
+            return np.abs((new - old) / old * 100)
 
-        if i > 0:
-            if check_convergence_criteria(
-                residual_distances[i], residual_distances[i - 1], min_change
-            ):
-                print("Convergence criteria fulfilled -> stop iteration!")
-                break
+        change_of_mean = change(
+            np.mean(distance_residuals_new), np.mean(distance_residuals_old)
+        )
+        change_of_std = change(
+            np.std(distance_residuals_new), np.std(distance_residuals_old)
+        )
 
-        if i == 0:
-            print(
-                f"{'Iteration':9s} | "
-                f"{'correspondences':15s} | "
-                f"{'mean(residuals)':15s} | "
-                f"{'std(residuals)':>15s}"
-            )
-            print(
-                f"{0:9d} | "
-                f"{len(initial_distances):15d} | "
-                f"{np.mean(initial_distances):15.4f} | "
-                f"{np.std(initial_distances):15.4f}"
-            )
-        print(
-            f"{i+1:9d} | "
-            f"{len(residual_distances[i]):15d} | "
-            f"{np.mean(residual_distances[i]):15.4f} | "
-            f"{np.std(residual_distances[i]):15.4f}"
+        return (
+            True
+            if change_of_mean < min_change and change_of_std < min_change
+            else False
         )
 
-    print("Estimated transformation matrix H:")
-    print(f"[{H[0, 0]:12.6f} {H[0, 1]:12.6f} {H[0, 2]:12.6f} {H[0, 3]:12.6f}]")
-    print(f"[{H[1, 0]:12.6f} {H[1, 1]:12.6f} {H[1, 2]:12.6f} {H[1, 3]:12.6f}]")
-    print(f"[{H[2, 0]:12.6f} {H[2, 1]:12.6f} {H[2, 2]:12.6f} {H[2, 3]:12.6f}]")
-    print(f"[{H[3, 0]:12.6f} {H[3, 1]:12.6f} {H[3, 2]:12.6f} {H[3, 3]:12.6f}]")
-    print(f"Finished in {time.time() - start_time:.3f} seconds!")
 
-    return H, pcmov.X
+class SimpleICPException(Exception):
+    """The SimpleICP class raises this when the class is misused."""


Test patch:
diff --git a/python/simpleicp/tests/minimal_working_example.py b/python/simpleicp/tests/minimal_working_example.py
new file mode 100644
index 0000000..694149e
--- /dev/null
+++ b/python/simpleicp/tests/minimal_working_example.py
@@ -0,0 +1,22 @@
+"""
+Read two point clouds from xyz files and run simpleICP.
+"""
+
+from pathlib import Path
+
+import numpy as np
+from simpleicp import PointCloud, SimpleICP
+
+# Read point clouds from xyz files into n-by-3 numpy arrays
+tests_dirpath = Path(__file__).parent
+X_fix = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/bunny_part1.xyz")))
+X_mov = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/bunny_part2.xyz")))
+
+# Create point cloud objects
+pc_fix = PointCloud(X_fix, columns=["x", "y", "z"])
+pc_mov = PointCloud(X_mov, columns=["x", "y", "z"])
+
+# Create simpleICP object, add point clouds, and run algorithm!
+icp = SimpleICP()
+icp.add_point_clouds(pc_fix, pc_mov)
+H, X_mov_transformed = icp.run(max_overlap_distance=1)
diff --git a/python/simpleicp/tests/run_simpleicp.py b/python/simpleicp/tests/run_simpleicp.py
index a87b500..fd16912 100644
--- a/python/simpleicp/tests/run_simpleicp.py
+++ b/python/simpleicp/tests/run_simpleicp.py
@@ -5,44 +5,85 @@
 from pathlib import Path
 
 import numpy as np
+from simpleicp import PointCloud, SimpleICP
 
-from simpleicp import simpleicp
-
+# User inputs
 dataset = "all"
 export_results = False
 plot_results = False
 
 tests_dirpath = Path(__file__).parent
 
+
+def run_simpleicp(X_fix, X_mov, kwargs):
+    """Small helper function to run simpleICP for each dataset."""
+
+    # Create point cloud for fixed point cloud
+    pc_fix = PointCloud(X_fix, columns=["x", "y", "z"])
+
+    # Create point cloud for movable point cloud (copy=True to keep original X_mov for plot)
+    pc_mov = PointCloud(X_mov, columns=["x", "y", "z"], copy=True)
+
+    # Create simpleICP object, add point clouds, and run algorithm!
+    icp = SimpleICP()
+    icp.add_point_clouds(pc_fix, pc_mov)
+    _, X_mov_transformed = icp.run(**kwargs)
+
+    return X_mov_transformed
+
+
 if dataset == "Dragon" or dataset == "all":
     print('Processing dataset "Dragon"')
     X_fix = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/dragon1.xyz")))
     X_mov = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/dragon2.xyz")))
-    H, X_mov_transformed = simpleicp.simpleicp(X_fix, X_mov)
+    kwargs = {}
+    X_mov_transformed = run_simpleicp(X_fix, X_mov, kwargs)
 
 if dataset == "Airborne Lidar" or dataset == "all":
     print('Processing dataset "Airborne Lidar"')
-    X_fix = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/airborne_lidar1.xyz")))
-    X_mov = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/airborne_lidar2.xyz")))
-    H, X_mov_transformed = simpleicp.simpleicp(X_fix, X_mov)
+    X_fix = np.genfromtxt(
+        tests_dirpath.joinpath(Path("../../../data/airborne_lidar1.xyz"))
+    )
+    X_mov = np.genfromtxt(
+        tests_dirpath.joinpath(Path("../../../data/airborne_lidar2.xyz"))
+    )
+    kwargs = {}
+    X_mov_transformed = run_simpleicp(X_fix, X_mov, kwargs)
 
 if dataset == "Terrestrial Lidar" or dataset == "all":
     print('Processing dataset "Terrestrial Lidar"')
-    X_fix = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/terrestrial_lidar1.xyz")))
-    X_mov = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/terrestrial_lidar2.xyz")))
-    H, X_mov_transformed = simpleicp.simpleicp(X_fix, X_mov)
+    X_fix = np.genfromtxt(
+        tests_dirpath.joinpath(Path("../../../data/terrestrial_lidar1.xyz"))
+    )
+    X_mov = np.genfromtxt(
+        tests_dirpath.joinpath(Path("../../../data/terrestrial_lidar2.xyz"))
+    )
+    kwargs = {}
+    X_mov_transformed = run_simpleicp(X_fix, X_mov, kwargs)
 
 if dataset == "Bunny" or dataset == "all":
     print('Processing dataset "Bunny"')
     X_fix = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/bunny_part1.xyz")))
     X_mov = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/bunny_part2.xyz")))
-    H, X_mov_transformed = simpleicp.simpleicp(X_fix, X_mov, max_overlap_distance=1)
+    kwargs = {
+        "max_overlap_distance": 1,
+    }
+    X_mov_transformed = run_simpleicp(X_fix, X_mov, kwargs)
 
 if dataset == "Multisensor" or dataset == "all":
     print('Processing dataset "Multisensor"')
-    X_fix = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/multisensor_lidar.xyz")))
-    X_mov = np.genfromtxt(tests_dirpath.joinpath(Path("../../../data/multisensor_radar.xyz")))
-    H, X_mov_transformed = simpleicp.simpleicp(X_fix, X_mov, max_overlap_distance=2.0)
+    X_fix = np.genfromtxt(
+        tests_dirpath.joinpath(Path("../../../data/multisensor_lidar.xyz"))
+    )
+    X_mov = np.genfromtxt(
+        tests_dirpath.joinpath(Path("../../../data/multisensor_radar.xyz"))
+    )
+    kwargs = {
+        "max_overlap_distance": 1,
+        "rbp_observed_values": (0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
+        "rbp_observation_weights": (0.0, np.inf, 0.0, 0.0, 0.0, 0.0),
+    }
+    X_mov_transformed = run_simpleicp(X_fix, X_mov, kwargs)
 
 # Export original and adjusted point clouds to xyz files to check the result
 if export_results:
@@ -70,21 +111,21 @@
         X_fix[idx_points_fix, 1],
         X_fix[idx_points_fix, 2],
         c="r",
-        marker=",",
+        marker=".",
     )
     ax.scatter(
         X_mov[idx_points_mov, 0],
         X_mov[idx_points_mov, 1],
         X_mov[idx_points_mov, 2],
         c="g",
-        marker=",",
+        marker=".",
     )
     ax.scatter(
         X_mov_transformed[idx_points_mov, 0],
         X_mov_transformed[idx_points_mov, 1],
         X_mov_transformed[idx_points_mov, 2],
         c="b",
-        marker=",",
+        marker=".",
     )
     ax.set_xlabel("x")
     ax.set_ylabel("y")