Match real X, Y, Z, positions with calculated positions

Hello, I am a student and I am using a Matlab code on my prototype project. I am using IMU sensors to track a tool's position. I have a 3 columns matrix data of positions x, y, z comes from IMU sensor. I also have 3 columns excel table of the positions x, y, z of objects that I need to add another column that includes for each row a scanning of the other table that comes from the sensor and tells matched or not matched. But since it cannot match exactly it needs an approximation such as +-2cm.

Answer 1

[DGM]

This is a generalized example of finding and evaluating distances. I'm assuming here that simple euclidean distance is an appropriate metric for the task.

Dtolerance = 0.5;

% example location data
[x y z] = ndgrid(1:4);
partloc = [x(:) y(:) z(:)];
toolloc = rand(100,3)*3+1;

% find closest part, evaluate distance
D = sqrt((toolloc(:,1)-partloc(:,1).').^2 + ...
    (toolloc(:,2)-partloc(:,2).').^2 + ...
    (toolloc(:,3)-partloc(:,3).').^2);
[Dmin closestpart] = min(D,[],2);
iscloseenough = Dmin <= Dtolerance;

% show results
[iscloseenough closestpart]

ans = 100×2

0 27 1 30 1 26 1 13 0 28 0 58 0 30 0 33 1 11 1 18

% plot the points; draw lines between tool and closest part
plot3(partloc(:,1),partloc(:,2),partloc(:,3),'o'); hold on
plot3(toolloc(:,1),toolloc(:,2),toolloc(:,3),'*');
axis equal
grid on
for k = 1:numel(closestpart)
    a = toolloc(k,:);
    b = partloc(closestpart(k),:);
    if iscloseenough(k)
        plot3([a(1) b(1)],[a(2) b(2)],[a(3) b(3)],'g')
    else
        plot3([a(1) b(1)],[a(2) b(2)],[a(3) b(3)],'r:')
    end
end

[Mete Özcan]

Hello,

Thank you for trying to help me. I am sorry maybe I could not ask my question clearly.

I have a sensor data table inludes X, Y, Z positions of the sensor. Something like this;

IMU SENSOR POSITIONS TABLE

X Y Z

1 3 10

2 2 15

4 3 12

2 6 17

4 8 20

and there are thousads of rows like this.

and I have another table that includes X, Y, Z, positions of real objects which includes less rows of course. Consider I have 3 object. Then it is something like this;

REAL OBJECT POSITIONS TABLE

X Y Z

1 3 10

13 20 25

2 3 15

Now I need a check part for the second table. Because I need to know whether my position tracked sensor goes to real object positions or not. In order to do this, I need to add a check column to my real object positions table to check for each row of real object table seperetaly and scan all the rows on sensor data table. It's supposed to seem like this;

CHECK TABLE

X Y Z CHECK

1 3 10 OK (Because it matches with one of the rows on sensor positions table)

2 3 15 OK ( It should be ok because no data can match 100 %, so I need an appoximation in the code to consider match when the positions are close for example +- 2 units close.)

13 20 25 NOT OK (Because it does not match with one of the rows on sensor positions table)

If you can help me, I will be really appreciative.

Thank you.

[DGM]

That's what the example does. It checks all sensor positions (toolloc) against all object positions (partloc). It finds the closest object by euclidean distance and tests whether the closest object is within a specified euclidean distance. The rest of the code is just visualization for sake of demonstration.

If you want to test axial distance instead of euclidean distance (the valid region being a cube instead of a sphere), then you can do that too.

Dtolerance = 0.3;

% example location data
[x y z] = ndgrid(1:4);
partloc = [x(:) y(:) z(:)];
toolloc = rand(100,3)*3+1;

% find closest part, evaluate distance
D = sqrt((toolloc(:,1)-partloc(:,1).').^2 + ...
    (toolloc(:,2)-partloc(:,2).').^2 + ...
    (toolloc(:,3)-partloc(:,3).').^2);
[Dmin closestpart] = min(D,[],2);
% is part within a tolerance box?
iscloseenough = all([abs(toolloc(:,1)-partloc(closestpart,1))<Dtolerance, ...
    abs(toolloc(:,2)-partloc(closestpart,2))<Dtolerance, ...
    abs(toolloc(:,3)-partloc(closestpart,3))<Dtolerance],2);

% show results
[iscloseenough closestpart]

ans = 100×2

1 28 0 22 0 59 0 34 0 56 0 24 0 43 0 8 1 44 0 20

% plot the points; draw lines between tool and closest part
plot3(partloc(:,1),partloc(:,2),partloc(:,3),'o'); hold on; 
plot3(toolloc(:,1),toolloc(:,2),toolloc(:,3),'*');
axis equal
grid on
for k = 1:numel(closestpart)
    a = toolloc(k,:);
    b = partloc(closestpart(k),:);
    if iscloseenough(k)
        plot3([a(1) b(1)],[a(2) b(2)],[a(3) b(3)],'g')
    else
        plot3([a(1) b(1)],[a(2) b(2)],[a(3) b(3)],'r:')
    end
end

Note that Dtolerance does not need to be scalar. It can also be specified as a three-element vector for per-axis tolerance. The logical test would need to address the corresponding elements of said vector, of course.