remove movable objects in LIO

Author: JanuszBedkowski

apps/lidar_odometry_step_1/lidar_odometry_gui.cpp

@@ -154,6 +154,7 @@ bool step_1_done = false;
 bool step_2_done = false;
 bool step_3_done = false;
 bool calculations_failed = false;
+bool show_without_filtered_buckets  = false;
 
 int lastPar = 1;
 
@@ -1754,7 +1755,7 @@ void progress_window()
     ImGui::Checkbox("Show trajectory as axes", &show_trajectory_as_axes);
     ImGui::Checkbox("Show prediction vectors", &show_prediction_vectors);
     ImGui::Checkbox("Show intermediate trajectory prediction axes", &intermediate_trajectory_prediction_axes);
-
+    ImGui::Checkbox("Show without filtered buckets", &show_without_filtered_buckets);
     // ImGui::Checkbox("Show covs indoor", &show_covs_indoor);
     // ImGui::Checkbox("Show covs outdoor", &show_covs_outdoor);
 
@@ -2071,7 +2072,13 @@ void display()
             {
                 glColor3f(1, 0, 0);
             }
-            glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+            if (!show_without_filtered_buckets){
+                glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+            }else{
+                if (b.second.number_of_hits < 20){
+                    glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+                }
+            }
         }
         glEnd();
     }
@@ -2091,7 +2098,14 @@ void display()
             {
                 glColor3f(0, 0, 1);
             }
-            glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+            //glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+            if (!show_without_filtered_buckets){
+                glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+            }else{
+                if (b.second.number_of_hits < 20){
+                    glVertex3f(b.second.mean.x(), b.second.mean.y(), b.second.mean.z());
+                }
+            }
         }
         glEnd();
     }

apps/lidar_odometry_step_1/lidar_odometry_utils.cpp

@@ -237,6 +237,46 @@ void update_rgd(
             if (lookup_count)
                 ++(*lookup_count);*/
         }
+    
+    /////////
+        //number_of_hits
+        //Eigen::Vector3d direction = points_global[i].point - viewport;
+        //direction.normalize();
+        //double bucket_norm = b.norm();
+
+        Eigen::Vector3d direction = viewport - points_global[i].point;
+        double distance = direction.norm();
+        double b_norm = b.norm();
+        int steps = static_cast<int>(std::ceil(distance / b_norm));
+
+        direction.normalize();
+
+        steps -= 5;
+
+        if (steps > 5)
+        {
+            for(int j = 2; j < steps; ++j){
+                
+                Eigen::Vector3d b_front = points_global[i].point + direction * (b_norm * (j + 1));
+
+                auto index_of_bucket = get_rgd_index_3d(b_front, b);
+
+                auto bucket_it = buckets.find(index_of_bucket);
+                
+                if (bucket_it != buckets.end()){
+                    auto& this_bucket = bucket_it->second;
+                    char noh = this_bucket.number_of_hits;
+
+                    if(noh < 30){
+                        this_bucket.number_of_hits++;
+
+                        //std::cout << "bucket: " << index_of_bucket << " number_of_hits: " << static_cast<int>(this_bucket.number_of_hits) << std::endl;
+                    }
+                }
+            }
+        }
+
+        /////////
     }
 }
 

apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp

@@ -1716,7 +1716,7 @@ static void compute_hessian(
 
     if (indoor_it != buckets_indoor.end())
     {
-        if (indoor_it->second.number_of_points != -1)
+        if (indoor_it->second.number_of_points != -1 && indoor_it->second.number_of_hits < 20)
         {
             add_indoor_hessian_contribution(
                 indoor_it->second,
@@ -1756,7 +1756,7 @@ static void compute_hessian(
 
         if (outdoor_it != buckets_outdoor.end())
         {
-            if (outdoor_it->second.number_of_points != -1)
+            if (outdoor_it->second.number_of_points != -1 && outdoor_it->second.number_of_hits < 20)
             {
                 add_outdoor_hessian_contribution(
                     outdoor_it->second,
@@ -3323,6 +3323,8 @@ void Consistency(std::vector<WorkerData>& worker_data, const LidarOdometryParams
                 if (intermediate_points_i.point.norm() < 1.0)
                     return;
 
+                   
+
                 Eigen::Vector3d point_global = trajectory[intermediate_points_i.index_pose] * intermediate_points_i.point;
                 auto index_of_bucket = get_rgd_index_3d(point_global, b);
 

core/include/Core/ndt.h

@@ -44,6 +44,8 @@ class NDT
         uint64_t number_of_points = 0;
         uint64_t index_begin = 0;
         uint64_t index_end = 0;
+
+        char number_of_hits = 0; 
     };
 
     struct BucketCoef
@@ -53,6 +55,8 @@ class NDT
         Eigen::Vector3d normal_vector = Eigen::Vector3d::Zero();
         std::vector<uint64_t> point_indexes;
         bool valid = false;
+
+        char number_of_hits = 0;
     };
 
     struct Bucket2