MATLAB Central Newsreader - How to synchronize two 3D Trajectories

How to synchronize two 3D Trajectories

smar — Thu, 11 Dec 2008 02:12:02 -0500

Hello,
There are two 3D Trajectories in Matlab.
A = [x y z] rows = n (about 20,000)
B = [x y z] rows = m (about 50,000)

Both trajectories A and B represent a same 3D path in space. Thus all 3D points on trajectory A should have their corresponding 3D points in trajectory B.

Both trajectories are generated from different sources.
There are two constraints:

1. There are small deviations (in ?m) in corresponding x y and z values of corresponding 3D points.

2. There is a small shift (variable along the trajectory) in relative position of corresponding 3D points in trajectories. In other words separation between points (point’s density) is different over the trajectory.

The final task is to calculate these deviations. But first problem is to find the corresponding points in both trajectories (synchronizing the both trajectories or searching for the matching points).

Finally the deviation would be calculated such as:
For example

Points in A - corresponding points in B = deviations
--
--
--

[34.00 56.05 67.06] - [34.01 56.04 67.07] = [-00.01 00.01 -00.07]
[34.50 56.55 67.50] - [34.52 56.57 67.48] = [-00.02 -00.02 00.02]
[34.70 56.75 67.76] - [34.71 56.74 67.77] = [-00.01 -00.01 -00.01]
--
--
--

Need guideline/Help needed

• How I can syschronize the trajectories or find the maching points for trajectory A in trajectory B (any Matlab functions or methods ?)
Thanks in advance for an expected help

Regards

Re: How to synchronize two 3D Trajectories

NZTideMan — Thu, 11 Dec 2008 03:10:54 -0500

On Dec 11, 3:12=A0pm, "smar " <smar...@yahoo.com> wrote:
> Hello,
> There are two 3D Trajectories in Matlab.
> A =3D [x y z] =A0 rows =3D n =A0 (about 20,000)
> B =3D [x y z] =A0 rows =3D m =A0 (about 50,000)
>
> Both trajectories A and B represent a same 3D path in space. Thus all 3D =
points on trajectory A should have their corresponding 3D points in traject=
ory B.
>
> Both trajectories are generated from different sources.
> There are two constraints:
>
> 1. =A0 =A0 =A0There are small deviations (in ?m) in corresponding x y and=
z values of corresponding 3D points.
>
> 2. =A0 =A0 =A0There is a small shift (variable along the trajectory) in r=
elative position of corresponding 3D points in trajectories. In other words=
separation between points (point=92s density) is different over the trajec=
tory.
>
> The final task is to calculate these deviations. But first problem is to =
find the corresponding points in both trajectories (synchronizing the both =
trajectories or searching for the matching points).
>
> Finally the deviation would be calculated such as:
> For example
>
> =A0 Points in A =A0 - corresponding points in B =3D =A0 deviations
> --
> --
> --
>
> [34.00 56.05 67.06] - [34.01 56.04 67.07] =3D [-00.01 00.01 -00.07]
> [34.50 56.55 67.50] - [34.52 56.57 67.48] =3D [-00.02 -00.02 00.02]
> [34.70 56.75 67.76] - [34.71 56.74 67.77] =3D [-00.01 -00.01 -00.01]
> --
> --
> --
>
> Need guideline/Help needed
>
> =95 =A0 =A0 How I can syschronize the trajectories or find the maching po=
ints for trajectory A in trajectory B (any Matlab functions or methods ?)
> Thanks in advance for an expected help
>
> Regards

If there were a 4th dimension (time) the problem would be trivial:
you'd interpolate for the time in A for each of the dimensions, using
B.
In the absence of time, one way would be to find the nearest point in
B corresponding to each point in A.
dev=3Dzeros(nz,3);
for ia=3D1:na
[dmn,ib]=3Dmin((xb-xa(ia)).^2 + (yb-ya(ia)).^2 + (zb-za(ia)).^2);
dev(ia,:)=3D[xb(ib)-xa(ia) yb(ib)-ya(ia) zb(ib)-za(ia)];
end

Re: How to synchronize two 3D Trajectories

smar — Sat, 13 Dec 2008 18:34:02 -0500

Hello
Thanks for a very useful replay

Yes there is a 4th dimension (time) too.
But I guess, it can not be taken to compare A and B. Because there is a small time shift in both trajectories. Also:
DeltaT of a certain pair of 2 connective points in A could be different to -------- DeltaT of their corresponding points.

I have implemented point matching (based on minimum 3D distance suggested by NZTideMan ). It worked fine as all indices of all matching points are in ascending order.

But there could still be one thing:
As there are small deviation between corresponding points. Minimum distance point matching may results some time in wrong matches (matching neighbourhood points)

How I can make this robust?
Or there should be some alternative method?

I would be very thankful for any further assistance.
Regards
Smar

Re: How to synchronize two 3D Trajectories

NZTideMan — Sat, 13 Dec 2008 22:32:27 -0500

On Dec 14, 7:34=A0am, "smar " <smar...@yahoo.com> wrote:
> Hello
> Thanks for a very useful replay
>
> Yes there is a 4th dimension (time) too.
> But I guess, it can not be taken to compare A and B. Because there is a s=
mall time shift in both trajectories. Also:
> DeltaT of a certain pair of =A02 connective points in A could be differen=
t to -------- DeltaT of their corresponding points.
>
> I have implemented point matching (based on minimum 3D distance suggested=
by NZTideMan ). It worked fine as all indices of all matching points are i=
n ascending order.
>
> But there could still be one thing:
> As there are small deviation between corresponding points. Minimum distan=
ce point matching may results some time in wrong matches (matching neighbou=
rhood points)
>
> How I can make this robust?
> Or there should be some alternative method?
>
> I would be very thankful for any further assistance.
> Regards
> Smar

Providing the times of each trajectory are accurate and have the same
start time, it doesn't matter that the time intervals are different:
you just use time as a parameter like this:
ti=3D[0:dt:tfin]';
xai=3Dinterp1(xa,ta,ti);
yai=3Dinterp1(ya,ta,ti);
zai=3Dinterp1(za,ta,ti);
xbi=3Dinterp1(xb,tb,ti);
ybi=3Dinterp1(yb,tb,ti);
zbi=3Dinterp1(zb,tb,ti);

devx=3Dxai-xbi;
devy=3Dyai-ybi;
devz=3Dzai-zbi;

Re: How to synchronize two 3D Trajectories

smar — Sun, 14 Dec 2008 15:01:42 -0500

Thank again for an usful reply,

I think if I do like this

devx=3Dxai-xbi;
devy=3Dyai-ybi;
devz=3Dzai-zbi;

It is possible that xai-xbi aare not the right corresponding points.

To explain more, Trajectory A is a reference trajectory which is fed to controller and Trajectory B is real trajectory which is measured through sensors.

Trajectory A -------fed to controller ------- Trajectory B (measured)

Controller produced uneven and variable delays over the whole processing.
Finally is may happen that:
Trajectory A …… completion time is 20 seconds
Trajectory B …… completion time is 20.05 seconds

Now time 0.05 second is time shift which is accumulated sum of many variable delays through out the trajectory.

I appreciate for any further assistance to my problem.
Best wishes
smar