Rotation Class Reference

This class represents a rotation using some kind of (non-standard) quaternion parametrization of rotation. More...

#include <Rotation.h>

Inheritance diagram for Rotation:
RigidTransfo

Public Member Functions

int GetRotationAxis (CVector3_t axis) const
const float * GetRotationAxis () const
virtual int SetTransfo (CVector3_t beta)
void GetRotationParam (CVector3_t param) const
const float * GetRotationParam () const
void GetQuaternion (CQuat_t q) const
void GetInverseQuaternion (CQuat_t q) const
void SetQuaternion (CQuat_t q)
 sets the rotation according to the quaternion
float GetRotationAngle () const
void Derivative (CMatrix3_t out) const
int SetAxis () const
int AxisOk () const
void CheckAxis () const
void InvertRotationAngle ()
 inverts the rotation angle
float AimingAngle (const CVector3_t tar, const CVector3_t with)
 provides the rotation angle in order to align as much as possible two vectors.
void AimAt (const CVector3_t tar, const CVector3_t with)
 updates the rotation angle in order to align as much as possible two vectors.
void AngleDerivative (CVector3_t v, CVector3_t out)
 computes dR(v)/dtheta
void NormDerivative (CVector3_t v, CVector3_t out)
 computes dR(v)/dnorm
void InverseNormDerivative (CVector3_t v, CVector3_t out)
 computes dR^{-1}(v)/dnorm
void BetaDerivative (CVector3_t x, CMatrix3_t out) const
 computes dR(v)/dbeta
void AxisDerivative (CVector3_t x, CMatrix3_t out)
 computes dR(v)/daxis
void InverseAxisDerivative (CVector3_t v, CMatrix3_t out)
 computes dR^{-1}(v)/daxis
void AngleAxisDerivative (CVector3_t v, CMatrix3_t out)
void Jacobian (CMatrix3_t out) const
 dR/dtheta
void Add (CVector3_t dbeta)
 update beta in a given direction
void AddToAxis (CVector3_t daxis)
 updates the rotation axis in a given direction
void AddToNorm (float dnorm)
 updates the rotation angle (actually the norm of beta) of a given amount
void RotFromMatrix (const CMatrix3_t m)
 Set the rotation from a matrix.
void RotToMatrix (CMatrix3_t m) const
 Sets the matrix from the rotation.
void Copy (const Rotation &r)
 sets the parameter of the transformation
void Update (CVector3_t v, CVector3_t v_tr)
 performs a gradient descent on the beta parameter of the rotation and on the translation in order to have a rigid transformation that brings v closer to v_tr.
void UpdateAxis (CVector3_t v, CVector3_t v_tr, float angle)
 updates the rotation axis of a rotation
virtual void SetRate (float new_eps)
 sets the learnig rate sizes
virtual void ScaleRate (float factor)
 the learning rate sizes
void Transform (const CVector3_t in, CVector3_t out) const
 Performs the rotation.
void InverseTransform (const CVector3_t in, CVector3_t out)
 Performs the inverse rigid tranformation.
void TransformRotation (const CVector3_t beta_in, CVector3_t beta_out)
 Performs compose this rotation with another one: beta_out = this*beta_in.
void QuaternionNormDerivative (CQuat_t out) const
 computes dq/dsin(theta/2) where q is the quaternion of the current rotation
void QuaternionAngleDerivative (CQuat_t out) const
 computes dq/d(theta) where q is the quaternion of the current rotation
void InverseQuaternionAngleDerivative (CQuat_t out) const
 computes d(-q)/d(theta) where q is the quaternion of the current rotation
void QuaternionAxisDerivative (CMatrix4_t out)
 computes dq/daxis where q is the quaternion of the current rotation
void QuaternionAxisAngleDerivative (CMatrix4_t out)
void InverseQuaternionNormDerivative (CQuat_t out) const
int SetAngle (float angle)
void SetNorm (float no)
int SetRotationAxis (const CVector3_t a)
int SetRotationParam (CVector3_t param)
float GetAngle () const
float GetAlpha () const
void Invert ()
void RandomAxis ()
void RandomAngle ()
void Identity ()
void MinimizeRotation (Rotation &rot)
 This function is the equivalent of AimAt for rotations.
void MinimizeRotation (const CQuat_t q)
float MinimizePositionAndRotationAngle (const CVector3_t tar, const CVector3_t with, const CQuat_t rot, float k=0.5)
void MinimizePositionAndRotation (const CVector3_t tar, const CVector3_t with, const CQuat_t rot, float k=0.5)
Rotationoperator* (const Rotation &r)
 not the intuitive operator implementation (not a const function)

Protected Member Functions

void ComputeMatrixC (const CVector3_t v, CMatrix3_t out) const
 computes the matrix .
void ComputeMatrixB (const CVector3_t v, CMatrix3_t out) const
 constructs the matrix d/db (b x v)

Protected Attributes

CVector3_t beta
 a vector whose norm is sin(theta/2) and direction is the rotation axis
float alpha
 cos(theta/2)
CVector3_t axis
 the rotation axis
float norm
int axis_flag
 specfies if axis and norm values are reliable.
float eps_b
 the learning rate for beta
float eps_a
 the learning rate for the axis
float base_eps_b
float base_eps_a

Detailed Description

This class represents a rotation using some kind of (non-standard) quaternion parametrization of rotation.

See Hestenes, "New Foundations for Classical Mechanics", pp 280 for the mathematical formalism

Definition at line 33 of file Rotation.h.


Member Function Documentation

void Rotation::AimAt ( const CVector3_t  tar,
const CVector3_t  with 
) [inline]

updates the rotation angle in order to align as much as possible two vectors.

This the updating step of the CCD inverse kinematics algorithm. The resulting rotation (around a fixed axis) will rotate <with> in order to align it with <tar>

Parameters:
tar target direction of alignment (in an external frame of ref)
with direction to align to the target (in a frame of ref attached to the zero position of the rotation)

Definition at line 117 of file Rotation.h.

References AimingAngle(), and SetAngle().

Referenced by KinematicChain::InverseKinPosCCD().

00117 {SetAngle(AimingAngle(tar,with));};

float Rotation::AimingAngle ( const CVector3_t  tar,
const CVector3_t  with 
)

provides the rotation angle in order to align as much as possible two vectors.

updates the rotation angle in order to align as much as possible two vectors.

This the updating step of the CCD inverse kinematics algorithm. The resulting rotation (around a fixed axis) will rotate <with> in order to align it with <tar>

Parameters:
tar target direction of alignment (in an external frame of ref)
with direction to align to the target (in a frame of ref attached to the zero position of the rotation)

Definition at line 122 of file Rotation.cpp.

References axis.

Referenced by AimAt().

00122                                                                      {
00123   CVector3_t vec;
00124   float f1,f2,f3,f4,angle,c;
00125   if(!AxisOk()){
00126     SetAxis();
00127   }
00128   v_cross(axis,with,vec);
00129   f1= v_dot(vec,tar);
00130   f2 = v_dot(with,axis);
00131   f3 = v_dot(tar,axis);
00132   f4 = v_dot(tar,with);
00133   f2 = f4-f2*f3;
00134   angle = atan2(f1,f2);
00135   c= cos(angle);
00136   if(c*f2+(f1*f1)/f2<0){ //checking it is a minimization
00137     angle = pi+angle;
00138   }
00139   return angle>pi?angle-2*pi:angle; //putting back to [-pi pi]
00140 }

void Rotation::AngleDerivative ( CVector3_t  v,
CVector3_t  out 
)

computes dR(v)/dtheta

Parameters:
v the transformed vector (see above)
out the vector where to put the result

Definition at line 265 of file Rotation.cpp.

References alpha, and axis.

Referenced by KinematicChain::UpdateWithJacobian().

00265                                                           {
00266   CVector3_t v1,v2,v3;
00267   float f;               // b = axis
00268   CheckAxis();
00269   v_scale(v,-2*norm*alpha,v1);//-4k*v
00270   f=(1-2*norm*norm);//(1-2*k^2)
00271   v_cross(axis,v,v2); //bxv
00272   v_scale(v2,f,v2); //2*(1-2*k^2)/sqrt(1-k^2)*(b x v)
00273   v_add(v1,v2,v3);;//-4k*v + (1-2*k^2)/sqrt(1-k^2)*(b x v)
00274   f=2*alpha*norm*v_dot(axis,v);//2*alpha*k*b'*v
00275   v_scale(axis,f,v1);
00276   v_add(v3,v1,out); //-4k*v+2*(1-2*k^2)/sqrt(1-k^2)*(b x v)+ 4*k*(b'*v)*b
00277 
00278 
00279 }

void Rotation::AxisDerivative ( CVector3_t  x,
CMatrix3_t  out 
)

computes dR(v)/daxis

Parameters:
v the transformed vector (see above)
out the matrix where to put the result

Definition at line 372 of file Rotation.cpp.

References alpha, axis, and ComputeMatrixB().

Referenced by KinematicChain::Update(), KinematicChain::UpdateTouch(), and KinematicChain::UpdateWithJacobian().

00372                                                          {
00373   CMatrix3_t B,m1,m2;
00374   float f;
00375   if(!AxisOk()){
00376     SetAxis();
00377   }
00378   ComputeMatrixB(v,B);
00379   m_rescale(2*norm*alpha,B,B); //2*k*sqrt(1-k²)*B
00380   v_mult(axis,v,m1); //b*v'
00381   f=v_dot(axis,v); //b'*v
00382   m_identity(m2);   
00383   m_rescale(f,m2,m2);//b'*v*I
00384   m_add(m1,m2,m1);// b*v+b'v*I
00385   m_rescale(2*norm*norm,m1,m1);//2k²*( b*v'+b'v*I )
00386   m_add(B,m1,out); //2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00387 }

void Rotation::BetaDerivative ( CVector3_t  x,
CMatrix3_t  out 
) const

computes dR(v)/dbeta

Parameters:
v the transformed vector (see above)
out the matrix where to put the result

Definition at line 349 of file Rotation.cpp.

References alpha, beta, ComputeMatrixB(), and ComputeMatrixC().

00349                                                               {
00350   CMatrix3_t B,C,m1; 
00351   CVector3_t v1;
00352   ComputeMatrixC(v,C);
00353   ComputeMatrixB(v,B);
00354 //   cout<<"C: "<<C<<endl;
00355 //   cout<<"B: "<<B<<endl;
00356   v_cross(beta,v,v1);//beta x v
00357   v_mult(v1,beta,m1);//(beta x v)*beta'
00358   if(alpha>epsilon){
00359     m_rescale(-2.0/alpha,m1,m1);//2/alpha * (beta x v)*beta'
00360     m_rescale(2*alpha,B,B);// 2*alpha*B
00361     m_add(m1,B,m1);// 
00362     m_sub(m1,C,out); 
00363   }
00364   else{//only that component
00365     m_rescale(-1,m1,out);
00366   }
00367 }

void Rotation::ComputeMatrixB ( const CVector3_t  v,
CMatrix3_t  out 
) const [protected]

constructs the matrix d/db (b x v)

Parameters:
v the second factor of the product
out the resulting matrix
  • v the second factor of the product
  • out the resulting matrix

Definition at line 811 of file Rotation.cpp.

Referenced by AxisDerivative(), BetaDerivative(), InverseAxisDerivative(), Jacobian(), Update(), RigidTransfo::Update(), and UpdateAxis().

00811                                                                     {
00812   CVector3_t v1;
00813   //  cout<<"v: ";coutvec(v);
00814   v_set(0,-v[2],v[1],v1);
00815   m_set_v3_column(v1,0,out);
00816 
00817   v_set(v[2],0,-v[0],v1);
00818   m_set_v3_column(v1,1,out);
00819 
00820   v_set(-v[1],v[0],0,v1);
00821   m_set_v3_column(v1,2,out);
00822 }

void Rotation::ComputeMatrixC ( const CVector3_t  v,
CMatrix3_t  out 
) const [protected]

computes the matrix .

..

Definition at line 791 of file Rotation.cpp.

References beta.

Referenced by BetaDerivative(), Update(), and RigidTransfo::Update().

00791                                                                     {
00792   CMatrix3_t m1,m2;
00793   float f;
00794   f=v_dot(beta,v);
00795   v_mult(v,beta,m1);
00796   m_rescale(2.0,m1,m2);
00797   v_mult(beta,v,m1);
00798   m_sub(m2,m1,m1);
00799   m_identity(m2);
00800   m_rescale(f,m2,m2);
00801   m_sub(m1,m2,out);
00802   m_rescale(2,out,out); // new correct version
00803 }

void Rotation::Copy ( const Rotation r  ) 

sets the parameter of the transformation

Parameters:
beta the new rotation parameter

Reimplemented in RigidTransfo.

Definition at line 36 of file Rotation.cpp.

References alpha, and beta.

00036                                     {
00037   r.GetRotationParam(beta);
00038   alpha = GetAlpha();
00039   SetAxis();
00040 }

void Rotation::InverseAxisDerivative ( CVector3_t  v,
CMatrix3_t  out 
)

computes dR^{-1}(v)/daxis

derivative of the inverse transfo

Parameters:
v the transformed vector (see above)
out the matrix where to put the result

Definition at line 392 of file Rotation.cpp.

References alpha, axis, and ComputeMatrixB().

Referenced by KinematicChain::Update(), and KinematicChain::UpdateTouch().

00392                                                                 {
00393   CMatrix3_t B,m1,m2;
00394   float f;
00395   if(!AxisOk()){
00396     SetAxis();
00397   }
00398   ComputeMatrixB(v,B);
00399   m_rescale(-2*norm*alpha,B,B); //-2*k*sqrt(1-k²)*B
00400   v_mult(axis,v,m1); //b*v'
00401   f=v_dot(axis,v); //b'*v
00402   m_identity(m2);   
00403   m_rescale(f,m2,m2);//b'*v*I
00404   m_add(m1,m2,m1);// b*v+b'v*I
00405   m_rescale(2*norm*norm,m1,m1);//2k²*( b*v'+b'v*I )
00406   m_add(B,m1,out); //-2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00407 }

void Rotation::InverseNormDerivative ( CVector3_t  v,
CVector3_t  out 
)

computes dR^{-1}(v)/dnorm

Parameters:
v the transformed vector (see above)
out the vector where to put the result

Definition at line 326 of file Rotation.cpp.

References alpha, and axis.

Referenced by KinematicChain::GetFullJacobian(), and KinematicChain::GetJacobian().

00326                                                                 {
00327   CVector3_t v1,v2,v3;
00328   float f;               // b = axis
00329   if(!AxisOk()){
00330     SetAxis();
00331   }
00332  if(alpha>epsilon){
00333     v_scale(v,-4*norm,v1);//-4k*v
00334     f= -2*(1-2*norm*norm)/alpha;//-2*(1-2*k^2)/sqrt(1-k^2) only the sine part is inverted
00335     v_cross(axis,v,v2); //bxv
00336     v_scale(v2,f,v2); //2*(1-2*k^2)/sqrt(1-k^2)*(b x v)
00337     v_add(v1,v2,v3);;//-4k*v + (1-2*k^2)/sqrt(1-k^2)*(b x v)
00338     f=4*norm*v_dot(axis,v);//4*k*b'*v
00339     v_scale(axis,f,v1);
00340     v_add(v3,v1,out); //-4k*v-(1-2*k^2)/sqrt(1-k^2)*(b x v)+ 4*k*(b'*v)*b
00341   }
00342   else{ //alpha is zero
00343      v_cross(axis,v,v2);
00344      v_scale(v2,-1,out);
00345   }
00346 }

void Rotation::InverseQuaternionAngleDerivative ( CQuat_t  out  )  const [inline]

computes d(-q)/d(theta) where q is the quaternion of the current rotation

Parameters:
out where to put the resulting quaternion

Definition at line 266 of file Rotation.h.

References alpha.

00267     {float f=0.5*alpha;InverseQuaternionNormDerivative(out);v4_scale(out,f,out);}; 

void Rotation::InverseTransform ( const CVector3_t  in,
CVector3_t  out 
)

Performs the inverse rigid tranformation.

Parameters:
in the vector to be transformed
out the resulting (transformed) vector

Reimplemented in RigidTransfo.

Definition at line 864 of file Rotation.cpp.

References beta, and Transform().

00864                                                                   {
00865   v_scale(beta,-1,beta);     // inverting rotation sign
00866   Rotation::Transform(in,out);           // rotating back
00867   v_scale(beta,-1,beta);     // resetting rotation sign
00868 }

void Rotation::MinimizeRotation ( const CQuat_t  q  ) 

Definition at line 155 of file Rotation.cpp.

References axis, beta, and SetAngle().

00155                                               {
00156   float num,den,a1,angle;
00157   CVector3_t v,b1;//rotation param of rot
00158  if(!AxisOk()){
00159     SetAxis();
00160   }
00161   v_copy(q,b1);
00162   a1 = q[3];
00163   //  a1 =sqrt(1-v_dot(b1,b1));;
00164   num=-2*a1*v_dot(axis,b1);
00165   v_cross(b1,beta,v);//it should be axis instead of beta
00166   den = 2*a1*a1-1+v_squ_length(v);
00167   angle = atan2(num,den);
00168  
00169 //  s= sin(angle/2);
00170 //  c= sin(angle/2);
00171 //   if(-2*(s/c)*num+(2*s/c-s/(c*c*c))*den < 0 && 0){ //checking it is a minimaization
00172 //     angle = pi+angle;
00173 //     cout<<"+";
00174 //   }
00175 //   cout<<" angle "<<angle*rad2deg<<endl;
00176 
00177   SetAngle(angle);
00178 }

void Rotation::MinimizeRotation ( Rotation rot  ) 

This function is the equivalent of AimAt for rotations.

Definition at line 146 of file Rotation.cpp.

Referenced by KinematicChain::InverseKinRotCCD().

00146                                             {
00147   CQuat_t b1;
00148   rot.GetQuaternion(b1);
00149   MinimizeRotation(b1);
00150 }

void Rotation::NormDerivative ( CVector3_t  v,
CVector3_t  out 
)

computes dR(v)/dnorm

Parameters:
v the transformed vector (see above)
out the vector where to put the result

Definition at line 282 of file Rotation.cpp.

References alpha, and axis.

Referenced by KinematicChain::GetFullJacobian(), KinematicChain::GetJacobian(), and KinematicChain::UpdateWithJacobian().

00282                                                          {
00283   CVector3_t v1,v2,v3;
00284   float f;               // b = axis
00285   CheckAxis();
00286   if(alpha>epsilon){
00287     v_scale(v,-4*norm,v1);//-4k*v
00288     f= 2*(1-2*norm*norm)/alpha;//2*(1-2*k^2)/sqrt(1-k^2)
00289     v_cross(axis,v,v2); //bxv
00290     v_scale(v2,f,v2); //2*(1-2*k^2)/sqrt(1-k^2)*(b x v)
00291     v_add(v1,v2,v3);;//-4k*v + (1-2*k^2)/sqrt(1-k^2)*(b x v)
00292     f=4*norm*v_dot(axis,v);//4*k*b'*v
00293     v_scale(axis,f,v1);
00294     v_add(v3,v1,out); //-4k*v+2*(1-2*k^2)/sqrt(1-k^2)*(b x v)+ 4*k*(b'*v)*b
00295   }
00296   else{ //alpha is zero
00297      v_cross(axis,v,v2);
00298      v_scale(v2,-1,out);
00299   }
00300   //  coutvec(out);
00301 }

void Rotation::QuaternionAngleDerivative ( CQuat_t  out  )  const [inline]

computes dq/d(theta) where q is the quaternion of the current rotation

Parameters:
out where to put the resulting quaternion

Definition at line 260 of file Rotation.h.

References alpha, and QuaternionNormDerivative().

00261     {float f=0.5*alpha;QuaternionNormDerivative(out);v4_scale(out,f,out);};

void Rotation::QuaternionAxisDerivative ( CMatrix4_t  out  )  [inline]

computes dq/daxis where q is the quaternion of the current rotation

Parameters:
out where to put the resulting matrix

Definition at line 272 of file Rotation.h.

00272                                                  {
00273         CheckAxis();m_identity(out);out[15]=0;m_rescale(norm,out,out);//can be optimized
00274     }

void Rotation::QuaternionNormDerivative ( CQuat_t  out  )  const [inline]

computes dq/dsin(theta/2) where q is the quaternion of the current rotation

Parameters:
out where to put the resulting quaternion

Definition at line 253 of file Rotation.h.

References alpha, and axis.

Referenced by KinematicChain::GetFullJacobian(), and QuaternionAngleDerivative().

00254     {CheckAxis();v_copy(axis,out);out[3]=alpha>epsilon?-norm/alpha:0;};

void Rotation::RotFromMatrix ( const CMatrix3_t  m  ) 

Set the rotation from a matrix.

Parameters:
m rotation matrix (orthogonal matrix is assumed)

Definition at line 826 of file Rotation.cpp.

References alpha, axis, axis_flag, and beta.

00826                                               {
00827   CVector3_t v;
00828   float angle = m_rotation_axis(m,v); //should be normalized already
00829   v_normalize(v,axis);
00830   norm = sin(angle/2);
00831   v_scale(axis,norm,beta);
00832   alpha = cos(angle/2);
00833   axis_flag=1;
00834 }

void Rotation::RotToMatrix ( CMatrix3_t  m  )  const

Sets the matrix from the rotation.

It can be a rotation or a homogeneous matrix

Definition at line 856 of file Rotation.cpp.

References Transform().

Referenced by KinematicChain::UpdateTouch(), and KinematicChain::UpdateWithJacobian().

00856                                              {
00857   m_identity(mat);
00858   Rotation::Transform(mat,mat);
00859   Rotation::Transform(mat+4, mat+4);
00860   Rotation::Transform(mat+8,mat+8);
00861 }

void Rotation::ScaleRate ( float  factor  )  [virtual]

the learning rate sizes

Parameters:
factor the scaling factor (for both rotation and translation learning ratea)

Reimplemented in RigidTransfo.

Definition at line 244 of file Rotation.cpp.

References eps_b.

00244                                     {
00245   eps_b = factor*base_eps_b;
00246 }

int Rotation::SetAngle ( float  angle  ) 
Returns:
1 if angle is within ]-pi pi],) 0 otherwise and sets the correct angle

Definition at line 730 of file Rotation.cpp.

References alpha, axis, and beta.

Referenced by AimAt(), MinimizeRotation(), KinematicChain::SetAngle(), KinematicChain::SetAngles(), KinematicChain::Transform(), and UpdateAxis().

00730                                  {
00731   if(angle>pi){
00732     return SetAngle(angle-2*pi)&& 0;
00733   }
00734   if(angle<=-pi){
00735     return SetAngle(angle+2*pi)&& 0;
00736   }
00737  //setting the rotation amplitude
00738   if(!AxisOk()){ //do this before modifying norm
00739    SetAxis();
00740  }
00741 
00742  norm = sin(angle/2.0f);
00743  alpha = cos(angle/2.0f);
00744  float n=norm;
00745 // n = norm/v_length(axis);//axis should already be scaled 
00746 //  assert(!isnan(n));
00747  v_scale(axis,n,beta);
00748 
00749  return 1;
00750 }

void Rotation::SetRate ( float  new_eps  )  [virtual]

sets the learnig rate sizes

Parameters:
new_eps rotation step size
new_eps_tr translation step size

Definition at line 240 of file Rotation.cpp.

References eps_b.

00240                                 {
00241   base_eps_b = eps_b = rate;
00242 }

int Rotation::SetTransfo ( CVector3_t  new_beta  )  [virtual]
Returns:
1 if new_beta is acceptable (norm<=1), 0 otherwise

Definition at line 80 of file Rotation.cpp.

References alpha, axis_flag, and beta.

Referenced by Add().

00080                                            {
00081   float sq_len = v_squ_length(new_beta);
00082   if(sq_len>1){
00083     return 0;
00084   }
00085   //  norm = sq_len;
00086   alpha = sqrt(1-sq_len);
00087   v_copy(new_beta,beta);
00088   axis_flag =0;
00089   return 1;
00090 }

void Rotation::Transform ( const CVector3_t  in,
CVector3_t  out 
) const

Performs the rotation.

Parameters:
in the vector to be transformed
out the resulting (transformed) vector

Definition at line 839 of file Rotation.cpp.

References alpha, and beta.

Referenced by KinematicChain::CollisionDetection(), KinematicChain::GetFullJacobian(), KinematicChain::GetJacobian(), InverseTransform(), RotToMatrix(), Update(), KinematicChain::Update(), UpdateAxis(), KinematicChain::UpdateTouch(), and KinematicChain::UpdateWithJacobian().

00839                                                                 {
00840 CVector3_t v1,v2,v3;
00841   float f1;
00842   f1 = alpha*alpha;
00843   v_scale(in,f1,v1); //alpha^2*v
00844   v_cross(beta,in,v2); //beta x v
00845   v_scale(v2,2*alpha,v3); // 2*alpha*(beta x v)
00846   v_add(v1,v3,v3); // alpha^2*v+2*alpha*(beta x v)
00847   f1 = v_dot(beta,in); //beta'*v
00848   v_scale(beta,f1,v1); //(beta'*v)*beta 
00849   v_add(v3,v1,v3);// alpha^2*v+2*alpha*(beta x v)+(beta'*v)*beta 
00850   v_cross(v2,beta,v1); //(beta x v) x beta
00851   v_sub(v3,v1,out);//alpha^2*v+2*alpha*(beta x v)+(beta'*v)*beta -(beta x v) x beta
00852 
00853 }

void Rotation::Update ( CVector3_t  v,
CVector3_t  v_tr 
)

performs a gradient descent on the beta parameter of the rotation and on the translation in order to have a rigid transformation that brings v closer to v_tr.

Only the direction of the gradient of beta is taken into account, not its size.

Parameters:
v vector a point that can be transformed
v_tr the observed image of v

Reimplemented in RigidTransfo.

Definition at line 483 of file Rotation.cpp.

References alpha, beta, ComputeMatrixB(), ComputeMatrixC(), eps_b, and Transform().

00483                                                   {
00484   // PD = -2/sqrt(1-p_beta'*p_beta) * cross(p_beta,x)*p_beta' +...
00485   //         2*sqrt(1-p_beta'*p_beta)*B-C;
00486   CVector3_t y,v1,dbeta;
00487   CMatrix3_t B,C,m1; 
00488   float beta_len,dbeta_len;
00489   //BETA_DERIVATIVE  
00490   ComputeMatrixC(v,C);
00491   ComputeMatrixB(v,B);
00492 //   cout<<"C: "<<C<<endl;
00493 //   cout<<"B: "<<B<<endl;
00494   v_cross(beta,v,v1);//beta x v
00495   v_mult(v1,beta,m1);//(beta x v)*beta'
00496   if(alpha>1e-7){
00497     m_rescale(-2.0/alpha,m1,m1);//2/alpha * (beta x v)*beta'
00498     m_rescale(2*alpha,B,B);// 2*alpha*B
00499     m_add(m1,B,m1);// 
00500     m_sub(m1,C,m1); 
00501   }
00502   //
00503 
00504   Transform(v,y);
00505   v_sub(v_tr,y,v1); // (y-RvR)
00506   v_transform_normal2(v1,m1,dbeta);
00507 
00508   // UPDATE
00509   // fixed increment size
00510   dbeta_len = v_length(dbeta);
00511   v_scale(dbeta,eps_b/dbeta_len,dbeta);
00512   //  v_scale(dbeta,eps_b,dbeta);
00513   v_add(beta,dbeta,beta);//v_add
00514   beta_len = v_length(beta);
00515   if(beta_len>1 && beta_len<(1+1e-2)){  //closing the manifold
00516     v_scale(beta,beta_len-2,beta);
00517     cout<<"cross"<<endl;
00518   }
00519   while(v_length(beta)>(1+1e-2)){
00520     v_scale(dbeta,0.5,dbeta);
00521     v_sub(beta,dbeta,beta);   
00522   }
00523 //   while((v_length(beta)>(1+1e-7))){ //avoids beta>1
00524 //     // cout<<(v_length(beta)>1+1e-10)<<endl;
00525 //     v_scale(dbeta,0.5,dbeta);
00526 //     v_sub(beta,dbeta,beta);//v_sub
00527 //   }
00528   alpha = sqrt(1-v_squ_length(beta));
00529 } 

void Rotation::UpdateAxis ( CVector3_t  v,
CVector3_t  v_tr,
float  angle 
)

updates the rotation axis of a rotation

Definition at line 541 of file Rotation.cpp.

References alpha, axis, axis_flag, ComputeMatrixB(), eps_a, SetAngle(), and Transform().

00541                                                                    {
00542   CMatrix3_t B,m1,m2;
00543   CVector3_t v1,y,db;
00544   float f;//,d_phi,d_psi;
00545 
00546 
00547 
00548  SetAngle(angle);
00549   if(!AxisOk()){
00550     SetAxis();
00551   }
00552 
00553 
00554  // computing d/daxis of euclidean distance (y-RvR)²
00555  ComputeMatrixB(v,B);
00556  m_rescale(2*norm*alpha,B,B); //2*k*sqrt(1-k²)*B
00557  v_mult(axis,v,m1); //b*v'
00558  f=v_dot(axis,v); //b'*v
00559  m_identity(m2);   
00560  m_rescale(f,m2,m2);//b'*v*I
00561  m_add(m1,m2,m1);// b*v+b'v*I
00562  m_rescale(2*norm*norm,m1,m1);//2k²*( b*v'+b'v*I )
00563  m_add(B,m1,m1); //2*k*sqrt(1-k²)*B + 2k²*( b*v'+b'v*I )
00564 
00565  Transform(v,y);
00566 //  cout<<"from " ;coutvec(v);
00567 //  cout<<"to " ;coutvec(y);
00568 //  cout<<"instead of " ;coutvec(v_tr);
00569   v_sub(v_tr,y,v1); // (y-RvR)
00570 //  cout<<"diff "<<v1<<endl;
00571 //  // v_normalize(v1,v1);//should not have an influence
00572   v_transform_normal2(v1,m1,db);
00573   f = v_length(db);
00574   f=f>pi/180?eps_a/f:eps_a;
00575   cout<<"norm "<<f<<endl;
00576   v_scale(db,f,db);
00577   v_add(axis,db,axis);
00578 
00579   f = v_normalize(axis,axis);
00580   axis_flag = 1;
00581   SetAngle(angle);
00582     
00583 //  cout<<"db ";coutvec(db);
00584 //  v_set(-sin(phi)*cos(psi), cos(phi)*cos(psi),0,v1);//db/dphi
00585 //  v_set(-cos(phi)*sin(psi),-sin(phi)*sin(psi),cos(psi),v2);//db/dpsi
00586  
00587 //  d_phi = v_dot(v1,db);
00588 //  d_psi = v_dot(v2,db);
00589  
00590 //  cout<<"d_phi "<<d_phi<<" d_psi "<<d_psi<<endl;
00591 //  while((f=d_phi*d_phi+d_psi*d_psi)>eps_a){
00592 //    d_phi /=2.0f;
00593 //    d_psi /=2.0f;
00594 //  }
00595  
00596 //  phi += d_phi;
00597 //  psi += d_psi;
00598 //  v_set(cos(phi)*cos(psi),sin(phi)*cos(psi),sin(psi),axis);
00599 }


The documentation for this class was generated from the following files:
 All Data Structures Functions Variables

Generated on Wed Sep 22 16:51:27 2010 for Body_Schema_Learning by  doxygen 1.6.1