diff --git a/include/small_gicp/ann/knn_result.hpp b/include/small_gicp/ann/knn_result.hpp
index d405e00..757ab8d 100644
--- a/include/small_gicp/ann/knn_result.hpp
+++ b/include/small_gicp/ann/knn_result.hpp
@@ -31,7 +31,7 @@ struct KnnResult {
   /// @param indices        Buffer to store indices (must be larger than k=max(N, num_neighbors))
   /// @param distances      Buffer to store distances (must be larger than k=max(N, num_neighbors))
   /// @param num_neighbors  Number of neighbors to search (must be -1 for static case N > 0)
-  explicit KnnResult(size_t* indices, double* distances, int num_neighbors = -1) : num_neighbors(num_neighbors), indices(indices), distances(distances) {
+  explicit KnnResult(size_t* indices, double* distances, int num_neighbors = -1) : capacity(num_neighbors), num_found_neighbors(0), indices(indices), distances(distances) {
     if constexpr (N > 0) {
       if (num_neighbors >= 0) {
         std::cerr << "warning: Specifying dynamic num_neighbors=" << num_neighbors << " for a static KNN result container (N=" << N << ")" << std::endl;
@@ -53,12 +53,12 @@ struct KnnResult {
     if constexpr (N > 0) {
       return N;
     } else {
-      return num_neighbors;
+      return capacity;
     }
   }
 
   /// @brief Number of found neighbors.
-  size_t num_found() const { return std::distance(indices, std::find(indices, indices + buffer_size(), INVALID)); }
+  size_t num_found() const { return num_found_neighbors; }
 
   /// @brief Worst distance in the result.
   double worst_distance() const { return distances[buffer_size() - 1]; }
@@ -73,21 +73,22 @@ struct KnnResult {
       indices[0] = index;
       distances[0] = distance;
     } else {
-      for (int i = buffer_size() - 1; i >= 0; i--) {
-        if (i == 0 || distance >= distances[i - 1]) {
-          indices[i] = index;
-          distances[i] = distance;
-          break;
-        }
-
-        indices[i] = indices[i - 1];
-        distances[i] = distances[i - 1];
+      int insert_loc = std::min<int>(num_found_neighbors, buffer_size() - 1);
+      for (; insert_loc > 0 && distance < distances[insert_loc - 1]; insert_loc--) {
+        indices[insert_loc] = indices[insert_loc - 1];
+        distances[insert_loc] = distances[insert_loc - 1];
       }
+
+      indices[insert_loc] = index;
+      distances[insert_loc] = distance;
     }
+
+    num_found_neighbors = std::min<int>(num_found_neighbors + 1, buffer_size());
   }
 
 public:
-  const int num_neighbors;  ///< Maximum number of neighbors to search
+  const int capacity;       ///< Maximum number of neighbors to search
+  int num_found_neighbors;  ///< Number of found neighbors
   size_t* indices;          ///< Indices of neighbors
   double* distances;        ///< Distances to neighbors
 };
diff --git a/include/small_gicp/registration/optimizer.hpp b/include/small_gicp/registration/optimizer.hpp
index 1962c85..c786405 100644
--- a/include/small_gicp/registration/optimizer.hpp
+++ b/include/small_gicp/registration/optimizer.hpp
@@ -106,7 +106,7 @@ struct LevenbergMarquardtOptimizer {
       bool success = false;
       for (int j = 0; j < max_inner_iterations; j++) {
         // Solve with damping
-        const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen ::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
+        const Eigen::Matrix<double, 6, 1> delta = (H + lambda * Eigen::Matrix<double, 6, 6>::Identity()).ldlt().solve(-b);
 
         // Validte new solution
         const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta);
diff --git a/include/small_gicp/util/lie.hpp b/include/small_gicp/util/lie.hpp
index d53a55a..842acca 100644
--- a/include/small_gicp/util/lie.hpp
+++ b/include/small_gicp/util/lie.hpp
@@ -75,28 +75,23 @@ inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
 /// @param a  Twist vector [rx, ry, rz, tx, ty, tz]
 /// @return   SE3 matrix
 inline Eigen::Isometry3d se3_exp(const Eigen::Matrix<double, 6, 1>& a) {
-  using std::cos;
-  using std::sin;
   const Eigen::Vector3d omega = a.head<3>();
 
-  double theta = std::sqrt(omega.dot(omega));
-  const Eigen::Quaterniond so3 = so3_exp(omega);
-  const Eigen::Matrix3d Omega = skew(omega);
-  const Eigen::Matrix3d Omega_sq = Omega * Omega;
-  Eigen::Matrix3d V;
+  const double theta_sq = omega.dot(omega);
+  const double theta = std::sqrt(theta_sq);
+
+  Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();
+  se3.linear() = so3_exp(omega).toRotationMatrix();
 
   if (theta < 1e-10) {
-    V = so3.matrix();
+    se3.translation() = se3.linear() * a.tail<3>();
     /// Note: That is an accurate expansion!
   } else {
-    const double theta_sq = theta * theta;
-    V = (Eigen::Matrix3d::Identity() + (1.0 - cos(theta)) / (theta_sq)*Omega + (theta - sin(theta)) / (theta_sq * theta) * Omega_sq);
+    const Eigen::Matrix3d Omega = skew(omega);
+    const Eigen::Matrix3d V = (Eigen::Matrix3d::Identity() + (1.0 - std::cos(theta)) / theta_sq * Omega + (theta - std::sin(theta)) / (theta_sq * theta) * Omega * Omega);
+    se3.translation() = V * a.tail<3>();
   }
 
-  Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity();
-  se3.linear() = so3.toRotationMatrix();
-  se3.translation() = V * a.tail<3>();
-
   return se3;
 }