title: How to rotate a 2D theoretical path to fit or be overlaid on a path from videoprocessing
body: <p>I track the motion of an object from a video file in MATLAB and save the locations from each frame in a numberOfFrames x 2 array. </p>
  
  <p>I know the theoretical path or intended path. When recording the movie the camera is at some unknown angle in space. Therefore, the path is skewed. The only information I have is the scaling between the pixels and millimeters by using the object diameter. </p>
  
  <p>Now I would like to rotate the intended path, and move it around until it is overlaid on the tracked motion path.</p>
  
  <p>I start with my theoretical path (Pth) then rotate it in 3-dimensions to "Pthr". After that I loop over each point in "M". And for each point in "M", I look for the closest point from "Pthr". Then, I repeat for the next point in "M". This probably has a problem of choosing the same point in "Pthr" for multiple points in "M". </p>
  
  <p>I noticed this is sensitive to my initial guess and it gives terrible results. </p>
  
  <p>Also, M is not a perfect path, since it is experimental measurements it is no where near perfect. <a href="https://i.stack.imgur.com/1x6EH.jpg" rel="nofollow noreferrer">measured vs. theoretical unrotated path</a></p>
  
  <pre><code>% M = [Mx,My], is location in x and y, from motion tracking.
  % scale = 20; % pixels/mm, using the size of object
  % I build the theoretical path (Pth) goes from to (0,0,0) to (0,3,0) to
  % (3,3,0) to (3,0,0) to be approximately the same length as M
  Pthup = linspace(0,3,num)';
  Pthdwn = linspace(3,0,num)';
  Pth0 = zeros(size(Pthup));
  Pth3 = 3*ones(size(Pthup));
  % Pth is approximately same length as M
  Pth = scale*[Pth0 Pthup Pth0;Pthup Pth3 Pth0;Pth3 Pthdwn Pth0];
  
  % using fmincon in matlab to minimize the sum of the square
  lb = [145 0 -45 min(min(M)) min(min(M))]; %upper bound
  ub = [180 90 45  max(max(M)) max(max(M))]; %lower bound
  
  coro = [180 0 0 mean(Mx) mean(My)]; %initial guess
  % initial guess (theta(x),theta(y),theta(z), shift in x, shift in y)
  cnt = 0; er = 1;
  while (abs(er)&gt;0.1)
      [const,fval] = fmincon(@(cor)findOrientation(cor,Pth,M),coro,[],[],[],[],lb,ub);
      er = sum(const-coro);
      coro = const;
      cnt = 1+cnt;
      if (cnt&gt;50)
          cnt = cnt;
          break
      end
  end
  
  %% function findOrientation keeps rotating Pth until it is closest to M
  
  function [Eo] = findOrientation(cor,Pth,M)
  % cor = [angle of rotations, center coordinate];(degrees, non-dimensiolaized in pixels)
  % M = measured coordinates from movie in pixel
  % coor: is output of the form [x-coordiante,y-coordinate, absolute distance from Center(i,:)]
  % F = sum of least square, sum(coor(:,3))
  
  %% Rotation of theoretical path about z,y,x and shifting in it in xy
  thx = cor(1); 
  thy = cor(2);
  thz = cor(3);
  xy = cor([4:5]);
  % T = [cosd(thn) -sind(thn);
  %         sind(thn) cosd(thn)]; %rotation matrix in 3D
  Tz = [cosd(thz) -sind(thz) 0;
          sind(thz) cosd(thz) 0;0 0 1]; %rotation matrix
  Ty = [cosd(thy) 0 -sind(thy);0 1 0;
      sind(thy) 0 cosd(thy)]; %rotation matrix
  Tx = [1 0 0;0 cosd(thx)  -sind(thx);
      0 sind(thx) cosd(thx)]; %rotation matrix
  
  Pthr = zeros(size(Pth));
  for i = 1:size(Pth,1)
      xp = Tz*Pth(i,:)';
      xp = Ty*xp;
      xp = Tx*xp;
      Pthr(i,:) = xp.';
  end
  
  Pthr = Pthr(:,[1,2]); % omit third value because it is 2D
  Pthr = Pthr + [cor([4:5])]; 
  
  rin = sqrt(Pthr(:,1).^2+Pthr(:,2).^2); %theoretical radius
  Centern = sqrt(M(:,1).^2 + M(:,2).^2);%measured radius
  
  
  for i = 1:size(M,1) %loop over each point in tracked motion
      sub = Pthr-M(i,:); %subtracting M(i,:) from all Pthr
      for j = 1:length(sub)
          dist(j,1) = norm(sub(j,:));% distance from M(i,:) to all ri
      end
      %index is based on the min absolute distance between Pthr and M(i,:). It chooses the closest Pthr to a specific M(i,:)
      [mn, index] = min(dist); 
      erri = abs(rin(index)-Centern(i))./rin(index);
      coor(i,:) = erri;
  end
  
  Eo = sum(coor);
  </code></pre>