RigidTransfo.cpp

00001 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
00002 /* 
00003  * Copyright (C) 2008 Micha Hersch, EPFL
00004  * RobotCub Consortium, European Commission FP6 Project IST-004370
00005  * email:   micha.hersch@robotcub.org
00006  * website: www.robotcub.org
00007  * Permission is granted to copy, distribute, and/or modify this program
00008  * under the terms of the GNU General Public License, version 2 or any
00009  * later version published by the Free Software Foundation.
00010  *
00011  * A copy of the license can be found at
00012  * http://www.robotcub.org/icub/license/gpl.txt
00013  *
00014  * This program is distributed in the hope that it will be useful, but
00015  * WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
00017  * Public License for more details
00018  */
00019 #include "RigidTransfo.h"
00020 
00021 //  RigidTransfo::RigidTransfo(): Rotation(), Translation(){
00022 //    adaptive = 1;
00023 //  }
00024 
00025 
00026 #ifdef WRONG_UPDATE
00027 //wrong
00028 void RigidTransfo::Update(CVector3_t v, CVector3_t v_tr){
00029   CVector3_t v1,v2;
00030   Translation::InverseTransform(v_tr,v1);
00031   Rotation::Transform(v,v2);
00032   Translation::Update(v2,v_tr);
00033   Rotation::Update(v,v1);
00034 }
00035 
00036 void RigidTransfo::UpdateAxisAndTranslation(CVector3_t v, CVector3_t v_tr,
00037                                             float angle){
00038   // CVector3_t v1,v2;
00039   //SetAngle(angle);
00040   // Translation::InverseTransform(v_tr,v1);
00041   // Rotation::Transform(v,v2);
00042   Translation::Update(v,v_tr);
00043   Rotation::UpdateAxis(v,v_tr,angle);
00044  
00045 }
00046 
00047 
00048 //probably wrong
00049 void RigidTransfo::UpdateAxis(CVector3_t v, CVector3_t v_tr,float angle){
00050   CVector3_t v1;
00051   Translation::InverseTransform(v_tr,v1);
00052   Rotation::UpdateAxis(v,v1,angle);
00053 }
00054 
00055 void RigidTransfo::UpdateTranslation(CVector3_t v, CVector3_t v_tr){
00056   CVector3_t v1;  
00057   Rotation::Transform(v,v1);
00058   Translation::Update(v1,v_tr);
00059 }
00060 
00061 void RigidTransfo::UpdateTranslation(CVector3_t v, CVector3_t v_tr,float angle){
00062   SetAngle(angle);
00063   UpdateTranslation(v,v_tr);
00064 }
00065 
00066 
00067 void RigidTransfo::UpdateRotation(CVector3_t v, CVector3_t v_tr){
00068   CVector3_t v1;  
00069   Translation::InverseTransform(v_tr,v1);
00070   Rotation::Update(v,v1);
00071 }
00072 
00073 #else
00074 
00075 float RigidTransfo::Update(CVector3_t v, CVector3_t v_tr){
00076   // PD = -2/sqrt(1-p_beta'*p_beta) * cross(p_beta,x)*p_beta' +...
00077   //         2*sqrt(1-p_beta'*p_beta)*B-C;
00078   CVector3_t y,v1,dbeta,dtrans;
00079   CMatrix3_t B,C,m1; 
00080   float beta_len,dbeta_len;
00081   
00082   ComputeMatrixC(v,C);
00083   ComputeMatrixB(v,B);
00084   //  cout<<"C: "<<C<<endl;
00085   //  cout<<"B: "<<B<<endl;
00086   v_cross(beta,v,v1);//beta x v
00087   v_mult(v1,beta,m1);//(beta x v)*beta'
00088   if(alpha>epsilon){
00089     m_rescale(-2.0/alpha,m1,m1);//2/alpha * (beta x v)*beta'
00090     m_rescale(2*alpha,B,B);// 2*alpha*B
00091     m_add(m1,B,m1);// 
00092     m_sub(m1,C,m1); 
00093   }
00094   // cout<<"m1: "<<m1<<endl;
00095   Transform(v,y);
00096   v_sub(v_tr,y,v1); // (y-RvR)
00097   //  cout<<"v1 ";coutvec(v1);
00098   v_transform_normal2(v1,m1,dbeta);
00099   // cout<<"db ";coutvec(dbeta);
00100   // fixed increment size
00101   dbeta_len = v_length(dbeta);
00102   if(abs(dbeta_len)>epsilon){
00103     v_scale(dbeta,eps_b/dbeta_len,dbeta);
00104   }
00105   //  v_scale(dbeta,eps_b,dbeta);
00106   v_add(beta,dbeta,beta);//v_add
00107 
00108  
00109   while(v_length(beta)>(1+1e-2)){
00110     v_scale(dbeta,0.5,dbeta);
00111     v_sub(beta,dbeta,beta);   
00112   }
00113 
00114   beta_len = v_length(beta);
00115  if(beta_len>1 && beta_len<(1+1e-2)){  //closing the manifold
00116     v_scale(beta,beta_len-2,beta);
00117        cout<<"cross"<<endl;
00118   }
00119 
00120 //   while((v_length(beta)>(1+1e-7))){ //avoids beta>1
00121 //     // cout<<(v_length(beta)>1+1e-10)<<endl;
00122 //     v_scale(dbeta,0.5,dbeta);
00123 //     v_sub(beta,dbeta,beta);//v_sub
00124 //   }
00125 
00126  //for numerical reason 
00127   if(v_squ_length(beta)>1){
00128           alpha=0;
00129   }else{
00130     alpha = sqrt(1-v_squ_length(beta));
00131   }  
00132   v_scale(v1,eps_tr,dtrans);
00133   v_add(trans,dtrans,trans); //
00134   return v_length(v1);
00135 } 
00136 
00137 
00138 #endif
00139 
00140 
00141 int RigidTransfo::SetTransfo(CVector3_t new_beta, CVector3_t transl){
00142   float sq_len = v_squ_length(new_beta);
00143   if(sq_len>1){
00144     return 0;
00145   } 
00146   alpha = sqrt(1-sq_len);
00147   v_copy(new_beta,beta);
00148   v_copy(transl,trans);
00149   return 1;
00150 }
00151 
00152 
00153 void RigidTransfo::SetRate(float n_eps_rot,float n_eps_tr){
00154   Rotation::SetRate(n_eps_rot);
00155   Translation::SetRate(n_eps_tr);
00156 }
00157 
00158 void RigidTransfo::ScaleRate(float factor){
00159   Rotation::ScaleRate(factor);
00160   Translation::ScaleRate(factor);
00161 }
00162 
00163 
00164 void RigidTransfo::Transform(const CVector3_t in,CVector3_t out){
00165   CVector3_t v;
00166   Rotation::Transform(in,v);
00167   Translation::Transform(v,out);
00168  
00169 }
00170 
00171 
00172 
00173 void RigidTransfo::InverseTransform(const CVector3_t in,CVector3_t out){
00174   CVector3_t v;
00175   Translation::InverseTransform(in,v);
00176   Rotation::InverseTransform(v,out);
00177 }
00178 
00179 
00180 
00181 
00185 void RigidTransfo::ReverseTransform(const CVector3_t in,CVector3_t out){
00186   CVector3_t v;
00187   Rotation::InverseTransform(in,v);
00188   Translation::InverseTransform(v,out);
00189 
00190 }
00191 
00192 
00193 void RigidTransfo::Invert(){
00194   CVector3_t tr;
00195   Rotation::Invert();
00196   Rotation::Transform(trans,tr);
00197   v_scale(tr,-1,trans);
00198 }
00199 
00200 RigidTransfo& RigidTransfo::operator*(const RigidTransfo& rt1){
00201   CVector3_t v;
00202   //new translation
00203   Rotation::Transform(rt1.GetTranslation(),v);
00204   v_add(v,trans,trans);
00205   (Rotation&)*this = ((Rotation&)(*this))*(Rotation&)rt1;
00206   return *this;
00207 }
00208 
00209 // void RigidTransfo::InverseTransform(CVector3_t in,float angle;CVector3_t out){
00210 //   SetAngle(angle);
00211 //   InverseTransform(in,out);
00212 // }
00213 ostream& operator<<(ostream& out, const RigidTransfo& rt){
00214   out<<(const Rotation&)(rt)<<" ";
00215   out<<(const Translation&)(rt);
00216   return out;
00217 }
00218 
00219 istream& operator>>(istream& in, RigidTransfo& rt){
00220   char c;
00221   in>>(Rotation&)(rt)>>c;
00222   in>>(Translation&)(rt);
00223   return in;
00224 } 
00225 
00226 #ifdef  RIGID_TRANSFO_MAIN
00227 
00228 //#define MATLAB_CHECK
00229 
00230 void swap(CVector3_t a,CVector3_t b){
00231   CVector3_t c;
00232   v_copy(a,c);
00233   v_copy(b,a);
00234   v_copy(c,b);
00235 }
00236 
00237 int main (int argc, char *argv[]){
00238   CVector3_t axis,x1,x2,x3,y1,y2,y3,n1;
00239   int x;
00240   RigidTransfo rt1;
00241   RigidTransfo rt2;
00242   srand(time(NULL));
00243 #ifdef MATLAB_CHECK
00244   float angle = pi/6;
00245   v_set(0.3,0.3,0.3,axis);
00246 #else
00247   float angle = RND(2*pi)-pi;
00248   v_set(RND(2)-1,RND(2)-1,RND(2)-1,axis);
00249 #endif
00250   v_normalize(axis,axis);
00251   v_scale(axis,sin(angle/2),axis);
00252   rt1.SetRotationParam(axis);
00253 #ifdef MATLAB_CHECK
00254   v_set(3,4,5,axis);
00255 #else
00256   v_set(RND(10),RND(10),RND(10),axis);
00257 #endif
00258  rt1.SetTranslation(axis);
00259   //  v_set(10,20,-15,x1);
00260 //   v_set(5,-10,10,x2);
00261 //   v_set(-15,-10,5,x3);
00262   
00263   rt1.Transform(x1,y1);
00264   rt1.Transform(x2,y3);
00265   rt1.Transform(x3,y3);
00266 
00267   rt2.SetRate(0.01,0.1);
00268   
00269   for(int i=0;i<500;i++){
00270   v_set(RND(20)-10,RND(20)-10,RND(20)-10,x1);
00271 #ifdef MATLAB_CHECK
00272   v_set(12, 5, 7,x1);
00273 #endif
00274   rt1.Transform(x1,y1);
00275   v_set(normalSample(0,1),normalSample(0,1),normalSample(0,1),n1);
00276     v_add(y1,n1,y1);
00277 
00278   cout<<rt2<<" "; 
00279   cout<<rt1<<endl;
00280    rt2.Update(x1,y1);
00281 #ifdef MATLAB_CHECK
00282    cout<<"y1 "<<y1<<endl;
00283 #endif  
00284   
00285 
00286 
00287   }
00288   return 0;
00289 }
00290 
00291 #endif
 All Data Structures Functions Variables

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