Hello, i have a problem with fsolve. My equation is really easy, i must calculate the rotation angles (eulero angles) and the translation vector from 3 points (calculate in absolute and relative).orientation .

My input are: - S : 3 points (x,y,z) on a global reference system - coord_global: 3 points (x,y,z) on a relative reference system

when i solve the equation "no solution found." appear.

The equation to solve is: Coord_global= R*Coord_local + tran

function [F]=eulero(x)
    %S =[ 6.6404e+03    6.7604e+03    6.2463e+03;
    %   -9.4099e+03   -9.9223e+03   -9.7810e+03;
    %    1.2576e+03    1.3005e+03    1.2662e+03];

      %x1= psi
      %x2= theta
      %x3= phi
      %x4= x
      %x5= y
      %x6= z
      load -ASCII Bott_Rover.txt;

      global S;
      for i=1:size(Bott_Rover,2)            %copio le coordinate locali delle bottiglie in Coord_local
          Coord_local1(i)=Bott_Rover(i,1);
      end
      Coord_local1=Coord_local1';

      for i=1:size(Bott_Rover,2)
          Coord_local2(i)=Bott_Rover(i,2);
      end
      Coord_local2=Coord_local2';

      for i=1:size(Bott_Rover,2)
          Coord_local3(i)=Bott_Rover(i,3);
      end
      Coord_local3=Coord_local3';

      for i=1:size(S,2)      %copio le coordinate ottenute dal localizzatore in Coord_global
          Coord_global1(i)=S(i,1);
      end
      Coord_global1=Coord_global1';

      for i=1:size(S,2)
          Coord_global2(i)=S(i,2);
      end
      Coord_global2=Coord_global2';

      for i=1:size(S,2)
          Coord_global3(i)=S(i,3);
      end
      Coord_global3=Coord_global3';

      R=[ cos(x(2))*cos(x(1)) sin(x(3))*sin(x(2))*cos(x(1))-cos(x(3))*sin(x(1)) cos(x(3))*sin(x(2))*cos(x(1))-sin(x(3))*sin(x(1));
          cos(x(2))*sin(x(1)) sin(x(3))*sin(x(2))*sin(x(1))+cos(x(3))*cos(x(1)) cos(x(3))*sin(x(2))*sin(x(1))-sin(x(3))*cos(x(1));
          -sin(x(2)) sin(x(3))*cos(x(2)) cos(x(3))*cos(x(2))]; %matrice di rotazione

      tran= [x(4) x(5) x(6)]'; %vettore di traslazione

      %Coord_global= R*Coord_local + tran
      %Coord_global= coordinate globali trovate dal localizzatore
      %Coord_local= coordinate locali dei marker rispetto al Rover
      %tran= vettore di traslazione
      %R= matrice di rotazione

      a1=R*Coord_local1;
      a2=R*Coord_local2;
      a3=R*Coord_local3;

      b1= a1+tran;
      b2= a2+tran;
      b3= a3+tran;

      F(1)=Coord_global1(1)-b1(1);
      F(2)=Coord_global1(2)-b1(2);
      F(3)=Coord_global1(3)-b1(3);
      F(4)=Coord_global2(1)-b2(1);
      F(5)=Coord_global2(2)-b2(2);
      F(6)=Coord_global2(3)-b2(3);
      F(7)=Coord_global3(1)-b3(1);
      F(8)=Coord_global3(2)-b3(2);
      F(9)=Coord_global3(3)-b3(3);

      end

       ----------------------------------------------------------------------
    x=[0 0 0 0 0 0];

    options=optimset('Algorithm','Levenberg-Marquardt', 'Display', 'iter', 'MaxFunEvals',1000000);
    y=fsolve(@eulero, x, options);

Answer 1

[Matt J]

There is no way we can troubleshoot your code based only on what you posted. You haven't given us enough to run the code.

However, if the goal is to find the rotation and translation that matches Coord_local with Coord_global, then an iterative solver like FSOLVE is quite unnecessary. A closed-form solution to the problem exists, and is implemented here.

[ALESSANDRO NOVERO]

In this algorithm i must use solve to solve it. My question is this: I have the 3 Coord_global points. Can i calculate the coord_local point with the difference of Coord_global? ex. i assign 0 to x1,y1,z1 (global) x2(local)=x2(global)-x1(global)

[Matt J]

Your original equations are,

Coord_global= R*Coord_local + tran

Taking the difference between Coord_global and Coord_local transforms your equation to,

Difference = (R-I)*Coord_local + tran