#1 minor fixes

matteodv99tn · Jan 2, 2023 · 765a177 · 765a177
1 parent fd2f44a
commit 765a177
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 2 deletions.
diff --git a/Classes/Laserscan.m b/Classes/Laserscan.m
@@ -53,6 +53,9 @@ function plot(obj)
 
         plot(obj.cartesian(:, 1), obj.cartesian(:, 2), '.k', ...
             'DisplayName', 'point cloud');
+        axis equal;
+        hold on;
+        grid on;
 
         for i = 1:size(obj.features, 2)
             feat = obj.features(i, 1:2);
@@ -212,6 +215,13 @@ function plot(obj)
                 is_line = false;
                 return
             end
+
+            P_curr = obj.cartesian(k, :);
+            P_pred = predict_point(line, obj.polar(k, 2));
+            if point_point_distance(P_curr, P_pred) > 0.12
+                is_line = false;
+                return
+            end
         end
     end
 

diff --git a/Functions/point_point_distance.m b/Functions/point_point_distance.m
@@ -1,5 +1,7 @@
 function d = point_point_distance(p1, p2)
 
-    d = sqrt(sumsqr(p1 - p2));
+    dx = p1(1) - p2(1);
+    dy = p1(2) - p2(2);
+    d = sqrt(dx.^2 + dy.^2);
 
 end
diff --git a/Functions/predict_point.m b/Functions/predict_point.m
@@ -5,7 +5,8 @@
     b   = line(2);
     c   = line(3);
     den = a*cos(theta) + b*sin(theta);
+    t   = - c / den;
 
-    pred_point = -c .* [cos(theta); sin(theta)] / den;
+    pred_point = t * [cos(theta); sin(theta)];
 
 end