RANSAC reimplemented, time measured

.gitignore

@@ -1,2 +1,3 @@
 build/
 extra/
+.vscode/

build.sh

@@ -0,0 +1,6 @@
+mkdir build 
+cd build
+cmake .. || { echo 'cmake failed' ; exit 1; }
+make -j8 || { echo 'make failed' ; exit 1; }
+
+./process-sequence /kitti/tracking/training/ 0000

src/point_cloud_kernels.h

@@ -37,7 +37,6 @@
 
 #include <pcl/common/transforms.h>
 #include <pcl/filters/conditional_removal.h>
-#include <pcl/filters/voxel_grid.h>
 
 #include <Eigen/Geometry>
 
@@ -81,22 +80,7 @@ auto decimate_cloud(typename pcl::PointCloud<PointT>::Ptr input_cloud_ptr,
             decimaterd_ptr->push_back(point);
         }
     }
-
-    auto filtered_ptr = std::make_shared<typename pcl::PointCloud<PointT>>();
-    if (decimation_coef > 1)
-    {
-        pcl::VoxelGrid<PointT> voxel_grid;
-        voxel_grid.setInputCloud(decimaterd_ptr);
-        const float voxel_size = 0.1f;
-        voxel_grid.setLeafSize(voxel_size, voxel_size, voxel_size);
-        voxel_grid.filter(*filtered_ptr);
-    }
-    else
-    {
-        filtered_ptr = decimaterd_ptr;
-    }
-
-    return filtered_ptr;
+    return decimaterd_ptr;
 }
 
 

src/ransac_ground.h

@@ -21,7 +21,8 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  * SOFTWARE.
  */
-
+
+ // made by Danila Flex and Serega Bizon
 
 #pragma once
 
@@ -33,129 +34,72 @@
 #include <Eigen/Geometry>
 
 
-namespace lidar_course {
-
-
-// A plane is represented with a point on the plane (base_point)
-// and a normal vector to the plane.
-struct Plane
+namespace lidar_course
 {
-    Eigen::Vector3f base_point;
-    Eigen::Vector3f normal;
 
-    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-};
+    // A plane is represented with a point on the plane (base_point)
+    // and a normal vector to the plane.
+    struct Plane
+    {
+        Eigen::Vector3f base_point;
+        Eigen::Vector3f normal;
 
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    };
 
-// This helper function finds indices of points that are considered inliers,
-// given a plane description and a condition on distance from the plane.
-std::vector<size_t> find_inlier_indices(
-    const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud_ptr,
-    const Plane& plane,
-    std::function<bool(float)> condition_z_fn)
-{
-    typedef Eigen::Transform<float, 3, Eigen::Affine, Eigen::DontAlign> Transform3f;
-
-    auto base_point = plane.base_point;
-    auto normal = plane.normal;
-
-    // Before rotation of the coordinate frame we need to relocate the point cloud to
-    // the position of base_point of the plane.
-    Transform3f world_to_ransac_base = Transform3f::Identity();
-    world_to_ransac_base.translate(-base_point);
-    auto ransac_base_cloud_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
-    pcl::transformPointCloud(*input_cloud_ptr, *ransac_base_cloud_ptr, world_to_ransac_base);
-
-    // We are going to use a quaternion to determine the rotation transform
-    // which is required to rotate a coordinate system that plane's normal
-    // becomes aligned with Z coordinate axis.
-    auto rotate_to_plane_quat = Eigen::Quaternionf::FromTwoVectors(
-        normal,
-        Eigen::Vector3f::UnitZ()
-    ).normalized();
-
-    // Now we can create a rotation transform and align the cloud that
-    // the candidate plane matches XY plane.
-    Transform3f ransac_base_to_ransac = Transform3f::Identity();
-    ransac_base_to_ransac.rotate(rotate_to_plane_quat);
-    auto aligned_cloud_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
-    pcl::transformPointCloud(*ransac_base_cloud_ptr, *aligned_cloud_ptr, ransac_base_to_ransac);
-
-    // Once the point cloud is transformed into the plane coordinates,
-    // We can apply a simple criterion on Z coordinate to find inliers.
-    std::vector<size_t> indices;
-    for (size_t i_point = 0; i_point < aligned_cloud_ptr->size(); i_point++)
-    {
-        const auto& p = (*aligned_cloud_ptr)[i_point];
-        if (condition_z_fn(p.z))
-        {
-            indices.push_back(i_point);
-        }
+    float dot_product(const Eigen::Vector3f& lhs, const Eigen::Vector3f& rhs) {
+        return lhs(0) * rhs(0) + lhs(1) * rhs(1) + lhs(2) * rhs(2);
     }
-    return indices;
-}
 
+    float distance_from_point_to_plane(const Plane& plane, const Eigen::Vector3f& point) {
+        float num = dot_product(plane.base_point, plane.normal) + dot_product(point, plane.normal);
+        float den = sqrt(dot_product(plane.normal, plane.normal));
 
-// This function performs plane detection with RANSAC sampling of planes
-// that lie on triplets of points randomly sampled from the cloud.
-// Among all trials the plane that is picked is the one that has the highest
-// number of inliers. Inlier points are then removed as belonging to the ground.
-auto remove_ground_ransac(
-    pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_ptr)
-{
-    // Threshold for rough point dropping by Z coordinate (meters)
-    const float rough_filter_thr = 0.5f;
-    // How much to decimate the input cloud for RANSAC sampling and inlier counting
-    const size_t decimation_rate = 10;
-
-    // Tolerance threshold on the distance of an inlier to the plane (meters)
-    const float ransac_tolerance = 0.1f;
-    // After the final plane is found this is the threshold below which all
-    // points are discarded as belonging to the ground.
-    const float remove_ground_threshold = 0.2f;
-
-    // To reduce the number of outliers (non-ground points) we can roughly crop
-    // the point cloud by Z coordinate in the range (-rough_filter_thr, rough_filter_thr).
-    // Simultaneously we perform decimation of the remaining points since the full
-    // point cloud is excessive for RANSAC.
-    std::mt19937::result_type decimation_seed = 41;
-    std::mt19937 rng_decimation(decimation_seed);
-    auto decimation_gen = std::bind(
-        std::uniform_int_distribution<size_t>(0, decimation_rate), rng_decimation);
-
-    auto filtered_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
-    for (const auto& p : *input_cloud_ptr)
+        return num / den;
+    }
+
+    // This helper function finds indices of points that are considered inliers,
+    // given a plane description and a condition on distance from the plane.
+    std::vector<size_t> find_inlier_indices(
+        const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud_ptr,
+        const Plane &plane,
+        std::function<bool(float)> condition_z_fn)
     {
-        if ((p.z > -rough_filter_thr) && (p.z < rough_filter_thr))
+        typedef Eigen::Transform<float, 3, Eigen::Affine, Eigen::DontAlign> Transform3f;
+
+        auto base_point = plane.base_point;
+        auto normal = plane.normal;
+
+        std::vector<size_t> indices;
+        for (size_t i_point = 0; i_point < input_cloud_ptr->size(); i_point++)
         {
-            // Use random number generator to avoid introducing patterns
-            // (which are possible with structured subsampling
-            // like picking each Nth point).
-            if (decimation_gen() == 0)
+            const auto &p = (*input_cloud_ptr)[i_point];
+            Eigen::Vector3f p_vec { p.x, p.y, p.z };
+            if (condition_z_fn(distance_from_point_to_plane(plane, p_vec)))
             {
-                filtered_ptr->push_back(p);
+                indices.push_back(i_point);
             }
         }
+        return indices;
     }
 
-    // We need a random number generator for sampling triplets of points.
-    std::mt19937::result_type sampling_seed = 42;
-    std::mt19937 sampling_rng(sampling_seed);
-    auto random_index_gen = std::bind(
-        std::uniform_int_distribution<size_t>(0, filtered_ptr->size()), sampling_rng);
-
-    // Number of RANSAC trials
-    const size_t num_iterations = 25;
-    // The best plane is determined by a pair of (number of inliers, plane specification)
     typedef std::pair<size_t, Plane> BestPair;
-    auto best = std::unique_ptr<BestPair>();
-    for (size_t i_iter = 0; i_iter < num_iterations; i_iter++)
+    #define MAX_ITERATIONS_DISTANCE 5
+
+    bool find_best_recursion(std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> point_cloud, size_t first, size_t second, BestPair& best)
     {
-        // Sample 3 random points.
-        // pa is special in the sense that is becomes an anchor - a base_point of the plane
-        Eigen::Vector3f pa = (*filtered_ptr)[random_index_gen()].getVector3fMap();
-        Eigen::Vector3f pb = (*filtered_ptr)[random_index_gen()].getVector3fMap();
-        Eigen::Vector3f pc = (*filtered_ptr)[random_index_gen()].getVector3fMap();
+
+        size_t third = first + (second - first) / 2; // middle point between first and second
+        if (third - first < MAX_ITERATIONS_DISTANCE)
+        {
+            return false;
+        }
+
+        BestPair left = {}, right = {};
+
+        Eigen::Vector3f pa = (*point_cloud)[first].getVector3fMap();
+        Eigen::Vector3f pb = (*point_cloud)[second].getVector3fMap();
+        Eigen::Vector3f pc = (*point_cloud)[third].getVector3fMap();
 
         // Here we figure out the normal to the plane which can be easily calculated
         // as a normalized cross product.
@@ -169,64 +113,113 @@ auto remove_ground_ransac(
             normal = -normal;
         }
 
-        Plane plane{pa, normal};
+        Plane plane{ pa, normal };
 
         // Call find_inlier_indices to retrieve inlier indices.
         // We will need only the number of inliers.
-        auto inlier_indices = find_inlier_indices(filtered_ptr, plane,
-            [ransac_tolerance](float z) -> bool {
-                return (z >= -ransac_tolerance) && (z <= ransac_tolerance);
-            });
-
-        // If new best plane is found, update the best
-        bool found_new_best = false;
-        if (best)
+        const float ransac_tolerance = 0.1f;
+        auto inlier_indices = find_inlier_indices(point_cloud, plane,
+                                                  [ransac_tolerance](float z) -> bool
+                                                  {
+                                                      return (z >= -ransac_tolerance) && (z <= ransac_tolerance);
+                                                  });
+
+        best = std::make_pair(inlier_indices.size(), plane);
+
+        if (find_best_recursion(point_cloud, first, third, left))
         {
-            if (inlier_indices.size() > best->first)
-            {
-                found_new_best = true;
-            }
+            if (left.first > best.first)
+                best = left;
         }
-        else
+        if (find_best_recursion(point_cloud, third, second, right))
         {
-            // For the first trial update anyway
-            found_new_best = true;
+            if (right.first > best.first)
+                best = right;
         }
 
-        if (found_new_best)
-        {
-            best = std::unique_ptr<BestPair>(new BestPair{inlier_indices.size(), plane});
-        }
+        return true;
     }
 
-    // For the best plane filter out all the points that are
-    // below the plane + remove_ground_threshold.
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_ground_ptr;
-    if (best)
+    // This function performs plane detection with RANSAC sampling of planes
+    // that lie on triplets of points randomly sampled from the cloud.
+    // Among all trials the plane that is picked is the one that has the highest
+    // number of inliers. Inlier points are then removed as belonging to the ground.
+    auto remove_ground_ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_ptr)
     {
-        cloud_no_ground_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
-        auto inlier_indices = find_inlier_indices(input_cloud_ptr, best->second,
-            [remove_ground_threshold](float z) -> bool {
-                return z <= remove_ground_threshold;
-            });
-        std::unordered_set<size_t> inlier_set(inlier_indices.begin(), inlier_indices.end());
-        for (size_t i_point = 0; i_point < input_cloud_ptr->size(); i_point++)
+        // Threshold for rough point dropping by Z coordinate (meters)
+        const float rough_filter_thr = 0.5f;
+        // How much to decimate the input cloud for RANSAC sampling and inlier counting
+        const size_t decimation_rate = 10;
+
+        // Tolerance threshold on the distance of an inlier to the plane (meters)
+        const float ransac_tolerance = 0.1f;
+        // After the final plane is found this is the threshold below which all
+        // points are discarded as belonging to the ground.
+        const float remove_ground_threshold = 0.2f;
+
+        // To reduce the number of outliers (non-ground points) we can roughly crop
+        // the point cloud by Z coordinate in the range (-rough_filter_thr, rough_filter_thr).
+        // Simultaneously we perform decimation of the remaining points since the full
+        // point cloud is excessive for RANSAC.
+        std::mt19937::result_type decimation_seed = 41;
+        std::mt19937 rng_decimation(decimation_seed);
+        auto decimation_gen = std::bind(
+            std::uniform_int_distribution<size_t>(0, decimation_rate), rng_decimation);
+
+
+        auto filtered_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
+        for (const auto& p : *input_cloud_ptr)
         {
-            bool extract_non_ground = true;
-            if ((inlier_set.find(i_point) == inlier_set.end()) == extract_non_ground)
+            if ((p.z > -rough_filter_thr) && (p.z < rough_filter_thr))
             {
-                const auto& p = (*input_cloud_ptr)[i_point];
-                cloud_no_ground_ptr->push_back(p);
+                // Use random number generator to avoid introducing patterns
+                // (which are possible with structured subsampling
+                // like picking each Nth point).
+                if (decimation_gen() == 0)
+                {
+                    filtered_ptr->push_back(p);
+                }
             }
         }
-    }
-    else
-    {
-        cloud_no_ground_ptr = input_cloud_ptr;
-    }
 
-    return cloud_no_ground_ptr;
-}
+        // Start time measurement
+        auto start_time_point = std::chrono::steady_clock::now();
+
+        BestPair best;
+        // The best plane is determined by a pair of (number of inliers, plane specification)
+        if (!find_best_recursion(filtered_ptr, 0, filtered_ptr->size() -1, best))
+        {
+            return input_cloud_ptr;
+        }
+
+        // For the best plane filter out all the points that are
+        // below the plane + remove_ground_threshold.
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_no_ground_ptr;
+        {
+            cloud_no_ground_ptr = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+            auto inlier_indices = find_inlier_indices(input_cloud_ptr, best.second,
+                                                      [remove_ground_threshold](float z) -> bool
+                                                      {
+                                                          return z <= remove_ground_threshold;
+                                                      });
+            std::unordered_set<size_t> inlier_set(inlier_indices.begin(), inlier_indices.end());
+            for (size_t i_point = 0; i_point < input_cloud_ptr->size(); i_point++)
+            {
+                bool extract_non_ground = true;
+                if ((inlier_set.find(i_point) == inlier_set.end()) == extract_non_ground)
+                {
+                    const auto &p = (*input_cloud_ptr)[i_point];
+                    cloud_no_ground_ptr->push_back(p);
+                }
+            }
+        }
+
+        // Finish time measurement
+        auto finish_time_point = std::chrono::steady_clock::now();
 
+        std::cout << "Time measured = " << static_cast<float>((finish_time_point - start_time_point).count()) / 1e6 << " ms\n"; 
+
+        return cloud_no_ground_ptr;
+    }
 
 } // namespace lidar_course