From e15000f33899d757ad1089a0d40c392c6caed2fb Mon Sep 17 00:00:00 2001
From: acoromin <acoromin@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Wed, 11 Feb 2015 14:16:12 +0000
Subject: [PATCH] Added scilab file to prototype corner detection
---
src/capture_laser_2D.cpp | 8 +-------
src/lineFit.sce | 36 ++++++++++++++++++++++++++++++++++++
2 files changed, 37 insertions(+), 7 deletions(-)
create mode 100644 src/lineFit.sce
diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 41eca5424..1e8691e40 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -18,11 +18,5 @@ void CaptureLaser2D::processCapture()
void CaptureLaser2D::extractCorners()
{
- std::cout << "Extracting corners ... " << std::endl;
- //TODO by Andreu: create class FeatureCorner2D + main to test this method.
- //TODO by Juan AC
- // Laser ranges are at data_
- // Scan size is data_.size()
- // Corners should be created as FeatureCorner2D. Corner 3 params to be stored at FeatureBase::measurement_
- // After creation, they have to be pushed back to down_node_list_ by means of the method Capture::addFeature(const FeatureShPtr& _f_ptr)
+
}
diff --git a/src/lineFit.sce b/src/lineFit.sce
new file mode 100644
index 000000000..7feb14c60
--- /dev/null
+++ b/src/lineFit.sce
@@ -0,0 +1,36 @@
+// clear all
+xdel(winsid());
+clear;
+
+
+//invent a set of points + noise
+points = [1 2 3 4 5 6;1 2 3 4 5 6];
+points = points + rand(points,"normal")*0.01;
+[xx N] = size(points);
+
+//build the system : Ax=0. Matrix A = a_ij
+a_00 = sum( points(1,:).^2 );
+a_01 = sum( points(1,:).*points(2,:) );
+a_02 = sum( points(1,:) );
+a_10 = a_01;
+a_11 = sum( points(2,:).^2 );
+a_12 = sum( points(2,:) );
+a_20 = a_02;
+a_21 = a_12;
+a_22 = N;
+A = [a_00 a_01 a_02; a_10 a_11 a_12; a_20 a_21 a_22; 0 0 1];
+
+//solve
+line = pinv(A)*[zeros(3,1);1];
+m = -line(1)/line(2);
+xc = -line(3)/line(2);
+disp("line: ");disp(line);
+disp("m: ");disp(m);
+disp("xc: ");disp(xc);
+
+//plot
+fig1 = figure(0);
+fig1.background = 8;
+plot(points(1,:),points(2,:),"g.");
+
+
--
GitLab