rotational_model.cpp
Go to the documentation of this file.
00001 /*
00002  * rotational_model.cpp
00003  *
00004  *  Created on: Oct 22, 2009
00005  *      Author: sturm
00006  */
00007 
00008 #include "articulation_models/models/rotational_model.h"
00009 #include "articulation_models/utils.h"
00010 
00011 using namespace std;
00012 using namespace articulation_msgs;
00013 
00014 #include <iomanip>
00015 #define VEC(a) setprecision(5)<<fixed<<a.x()<<" "<<a.y()<<" "<<a.z()<<" "<<a.w()<<" l="<<a.length()
00016 #define VEC2(a) "t=["<<VEC(a.getOrigin())<<"] r=[]"<<VEC(a.getRotation())<<"]"
00017 #define PRINT(a) cout << #a <<"=" << VEC(a)<<endl;
00018 #define PRINT2(a) cout << #a <<"=" << VEC2(a)<<endl;
00019 
00020 namespace articulation_models {
00021 
00022 
00023 RotationalModel::RotationalModel() {
00024         rot_center = tf::Vector3 (0,0,0);
00025         rot_axis = tf::Quaternion (0,0,0,1);
00026         rot_radius = 1;
00027         rot_orientation = tf::Quaternion (0,0,0,1);
00028         complexity = 3+2+1+3;   // rotation center + rotation axis + radius + orientation
00029         rot_mode = 0;// use position (=0) or orientation (=1) for configuration estimation
00030 }
00031 
00032 // -- params
00033 void RotationalModel::readParamsFromModel() {
00034         GenericModel::readParamsFromModel();
00035         getParam("rot_center",rot_center);
00036         getParam("rot_axis",rot_axis);
00037         getParam("rot_radius",rot_radius);
00038         getParam("rot_orientation",rot_orientation);
00039         getParam("rot_mode",rot_mode);
00040 }
00041 
00042 void RotationalModel::writeParamsToModel() {
00043         GenericModel::writeParamsToModel();
00044         setParam("rot_center",rot_center,ParamMsg::PARAM);
00045         setParam("rot_axis",rot_axis,ParamMsg::PARAM);
00046         setParam("rot_radius",rot_radius,ParamMsg::PARAM);
00047         setParam("rot_orientation",rot_orientation,ParamMsg::PARAM);
00048         setParam("rot_mode",rot_mode,ParamMsg::PARAM);
00049 }
00050 
00051 V_Configuration RotationalModel::predictConfiguration(geometry_msgs::Pose pose) {
00052         V_Configuration q(1);
00053 
00054         if(rot_mode==1) {
00055                 // estimate configuration from pose orientation
00056                 tf::Matrix3x3 m_pose( poseToTransform(pose).getBasis() );
00057                 tf::Matrix3x3 m_axis(rot_axis);
00058                 tf::Matrix3x3 m_orient(rot_orientation);
00059 
00060                 tf::Matrix3x3 rot_z = m_axis.inverse() * m_pose * m_orient.inverse();
00061                 tf::Quaternion rot_z_quat;
00062                 rot_z.getRotation(rot_z_quat);
00063 
00064                 q(0) = rot_z_quat.getAngle();
00065         } else {
00066 
00067                 tf::Transform center(rot_axis,rot_center);
00068 
00069                 tf::Transform rel = center.inverseTimes( poseToTransform(pose) );
00070 
00071                 q(0) = -atan2(rel.getOrigin().y(), rel.getOrigin().x());
00072         }
00073 
00074         return q;
00075 }
00076 
00077 geometry_msgs::Pose RotationalModel::predictPose(V_Configuration q) {
00078         geometry_msgs::Pose pose;
00079 
00080         tf::Transform center(rot_axis,rot_center);
00081         tf::Transform rotZ(tf::Quaternion(tf::Vector3(0, 0, 1), -q[0]), tf::Vector3(0, 0, 0));
00082         tf::Transform r(tf::Quaternion(0,0,0,1), tf::Vector3(rot_radius, 0, 0));
00083         tf::Transform offset(rot_orientation, tf::Vector3(0, 0, 0));
00084 
00085         pose = transformToPose( center * rotZ * r * offset );
00086 
00087         return pose;
00088 }
00089 
00090 bool RotationalModel::guessParameters() {
00091         if(model.track.pose.size() < 3)
00092                 return false;
00093 
00094         size_t i,j,k;
00095         do{
00096                 i = rand() % getSamples();
00097                 j = rand() % getSamples();
00098                 k = rand() % getSamples();
00099         } while (i==j || j==k || i==k);
00100 
00101         tf::Transform pose1 = poseToTransform(model.track.pose[i]);
00102         tf::Transform pose2 = poseToTransform(model.track.pose[j]);
00103         tf::Transform pose3 = poseToTransform(model.track.pose[k]);
00104 
00105 
00106 //      if( pose1.getOrigin().distance(pose2.getOrigin())/sigma_position <
00107 //              pose1.getRotation().angle(pose2.getRotation())/sigma_orientation
00108         if(rand() % 2 == 0)
00109         {
00110                 rot_mode = 1;
00111 //              cout <<"using rot computation"<<endl;
00112 
00113                 // angle between pose 1 and pose 2
00114                 double angle_12 = pose1.inverseTimes(pose2).getRotation().getAngle();
00115 
00116                 // distance between pose 1 and pose 2
00117                 double dist_12 = pose1.getOrigin().distance( pose2.getOrigin() );
00118 
00119                 // rot axis between pose 1 and pose 2
00120                 tf::Vector3 rot_12 = pose1.getBasis() * pose1.inverseTimes(pose2).getRotation().getAxis() * -1;
00121 //              PRINT(rot_12);
00122 
00123                 rot_center = tf::Vector3(0.5,0.5,0.5);
00124                 rot_axis = tf::Quaternion(0,0,0,1);
00125                 rot_orientation = tf::Quaternion(0,0,0,1);
00126                 rot_radius = 0.0;
00127 
00128                 rot_radius = (dist_12 * 0.5) / sin( angle_12 * 0.5 );
00129 
00130 //              PRINT(pose1.getOrigin());
00131 //              PRINT(pose2.getOrigin());
00132                 tf::Vector3 c1 = (pose1.getOrigin() + pose2.getOrigin())/2;
00133 //              PRINT(c1);
00134                 tf::Vector3 v1 = (pose2.getOrigin() - pose1.getOrigin());
00135 //              PRINT(v1);
00136                 v1.normalize();
00137 //              PRINT(v1);
00138                 v1 = v1 - rot_12.dot(v1)*rot_12;
00139 //              PRINT(v1);
00140                 v1.normalize();
00141 //              PRINT(v1);
00142 //              PRINT(rot_12);
00143                 rot_center = c1 + v1.cross(rot_12) * rot_radius * cos(angle_12/2);
00144 //              PRINT(rot_center);
00145 
00146                 tf::Vector3 d(1,0,0);
00147                 d = pose1.getOrigin() - rot_center;
00148                 d.normalize();
00149 //              PRINT(d);
00150 
00151                 tf::Vector3 rot_z = rot_12;
00152                 tf::Vector3 rot_x = d - rot_z.dot(d)*rot_z;
00153                 rot_x.normalize();
00154                 tf::Vector3 rot_y = rot_z.cross(rot_x);
00155 //              rot_x = tf::Vector3(-1,0,0);
00156 //              rot_y = tf::Vector3(0,-1,0);
00157 //              rot_z = tf::Vector3(0,0,-1);
00158                 tf::Matrix3x3(
00159                                 rot_x.x(),rot_y.x(),rot_z.x(),
00160                                 rot_x.y(),rot_y.y(),rot_z.y(),
00161                                 rot_x.z(),rot_y.z(),rot_z.z()).getRotation(rot_axis);
00162 //              PRINT(rot_x);
00163 //              PRINT(rot_y);
00164 //              PRINT(rot_z);
00165 //              PRINT(rot_axis);
00166 //              rot_axis=tf::Quaternion(0,0,0,1);
00167 
00168                 rot_orientation = rot_axis.inverse() * pose1.getRotation();
00169 
00170                 // eval ---------------------
00171                 tf::Transform t_rotaxis(rot_axis,tf::Vector3(0,0,0));
00172                 tf::Transform t_radius(tf::Quaternion(0,0,0,1),tf::Vector3(rot_radius,0,0));
00173                 tf::Transform t_orient(rot_orientation,tf::Vector3(0,0,0));
00174                 tf::Transform t_rotcenter(tf::Quaternion(0,0,0,1),rot_center);
00175 
00176                 // show diff to ppose1 and ppose2
00177                 tf::Transform diff;
00178                 tf::Transform pp1 =
00179                                 t_rotcenter *
00180                                 t_rotaxis *
00181                                 tf::Transform(tf::Quaternion(tf::Vector3(0,0,1),0.00),tf::Vector3(0,0,0)) *
00182                                 t_radius *
00183                                 t_orient;
00184                 diff = pose1.inverseTimes(pp1);
00185 //              cout <<"pp1: angle=" <<0.00<<" poserr="<<diff.getOrigin().length()<<" orienterr="<<diff.getRotation().getAngle()<<endl;
00186 
00187                 tf::Transform pp2 =
00188                                 t_rotcenter *
00189                                 t_rotaxis *
00190                                 tf::Transform(tf::Quaternion(tf::Vector3(0,0,1),angle_12),tf::Vector3(0,0,0)) *
00191                                 t_radius *
00192                                 t_orient;
00193                 diff = pose2.inverseTimes(pp2);
00194 //              cout <<"pp2: angle=" <<angle_12<<" poserr="<<diff.getOrigin().length()<<" orienterr="<<diff.getRotation().getAngle()<<endl;
00195 
00196                 for(size_t a=0;a<getSamples();a++) {
00197                         V_Configuration q =predictConfiguration(model.track.pose[a]);
00198                         tf::Transform p2 = poseToTransform(predictPose(q));
00199                         diff = poseToTransform(model.track.pose[a]).inverseTimes(p2);
00200 //                      cout <<"angle=" <<q[0]<<" poserr="<<diff.getOrigin().length()<<" orienterr="<<diff.getRotation().getAngle()<<endl;
00201 //                      tf::Transform pp =
00202 //                                      t_rotcenter *
00203 //                                      t_rotaxis *
00204 //                                      tf::Transform(tf::Quaternion(tf::Vector3(0,0,1),-q[0]),tf::Vector3(0,0,0)) *
00205 //                                      t_radius *
00206 //                                      t_orient;
00207 //                      diff = poseToTransform(model.track.pose[a]).inverseTimes(pp);
00208 //                      cout <<"vs angle=" <<q[0]<<" poserr="<<diff.getOrigin().length()<<" orienterr="<<diff.getRotation().getAngle()<<endl;
00209                 }
00210         } else {
00211                 rot_mode = 0;
00212 //              cout<<"using plane computation"<<endl;
00213                 // first, find the plane
00214                 tf::Vector3 plane_pos = pose1.getOrigin();
00215                 tf::Vector3 plane_v = pose2.getOrigin() - pose1.getOrigin();
00216                 tf::Vector3 plane_w = pose3.getOrigin() - pose1.getOrigin();
00217         //      PRINT(plane_pos);
00218         //      PRINT(plane_v);
00219         //      PRINT(plane_w);
00220                 plane_v.normalize();
00221                 plane_w.normalize();
00222 
00223                 tf::Vector3 plane_x = plane_v;
00224                 tf::Vector3 plane_y = plane_w - (plane_w.dot(plane_v))*plane_v;
00225                 plane_x.normalize();
00226                 plane_y.normalize();
00227                 tf::Vector3 plane_z = plane_x.cross(plane_y);
00228         //      PRINT(plane_x);
00229         //      PRINT(plane_y);
00230         //      PRINT(plane_z);
00231 
00232 
00233                 tf::Matrix3x3 plane_rot(
00234                                 plane_x.x(),plane_y.x(),plane_z.x(),
00235                                 plane_x.y(),plane_y.y(),plane_z.y(),
00236                                 plane_x.z(),plane_y.z(),plane_z.z()
00237                                 );
00238 
00239                 tf::Transform plane(plane_rot,plane_pos);
00240 
00241                 tf::Transform onplane_pose1 = plane.inverseTimes(pose1);
00242                 tf::Transform onplane_pose2 = plane.inverseTimes(pose2);
00243                 tf::Transform onplane_pose3 = plane.inverseTimes(pose3);
00244         //      cout <<"onplane_pose1"<<VEC2(onplane_pose1)<<endl;
00245         //      cout <<"onplane_pose2"<<VEC2(onplane_pose2)<<endl;
00246         //      cout <<"onplane_pose3"<<VEC2(onplane_pose3)<<endl;
00247 
00248                 //http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
00249                 tf::Vector3 p1 = (onplane_pose1.getOrigin() + onplane_pose2.getOrigin())/2;
00250                 tf::Vector3 p21 = (onplane_pose2.getOrigin() - onplane_pose1.getOrigin()).rotate(tf::Vector3(0,0,1),M_PI/2);;
00251 
00252                 tf::Vector3 p3 = (onplane_pose1.getOrigin() + onplane_pose3.getOrigin())/2;
00253                 tf::Vector3 p43 = (onplane_pose3.getOrigin() - onplane_pose1.getOrigin()).rotate(tf::Vector3(0,0,1),M_PI/2);;
00254 
00255                 tf::Vector3 p13 = p1 - p3;
00256 
00257                 double u =      ( p43.x()*p13.y() - p43.y()*p13.x() ) /
00258                                         ( p43.y()*p21.x() - p43.x()*p21.y() );
00259                 tf::Vector3 onplane_center = p1 + u*p21;
00260 
00261                 tf::Transform rotcenter(plane_rot,plane * onplane_center);
00262                 rot_center = rotcenter.getOrigin();
00263                 rot_axis = rotcenter.getRotation();
00264 
00265                 rot_radius = rotcenter.inverseTimes(pose1).getOrigin().length();
00266                 rot_orientation = tf::Quaternion(0,0,0,1);
00267                 V_Configuration q = predictConfiguration( model.track.pose[i]);
00268         //      cout <<"q="<<q[0]<<endl;
00269                 tf::Transform pred1 = poseToTransform( predictPose(q) );
00270         //      PRINT2(pose1);
00271         //      PRINT2(pred1);
00272                 rot_orientation = pred1.inverseTimes(pose1).getRotation();
00273         }
00274 
00275 //      cout<<"rot_radius="<<rot_radius<<endl;
00276 //      PRINT(rot_center);
00277 //      PRINT(rot_axis);
00278 //      PRINT(rot_orientation);
00279 
00280         if(!check_values(rot_center)) return false;
00281         if(!check_values(rot_axis)) return false;
00282         if(!check_values(rot_radius)) return false;
00283         if(!check_values(rot_orientation)) return false;
00284 
00285         return true;
00286 }
00287 
00288 void RotationalModel::updateParameters(std::vector<double> delta) {
00289         rot_center = rot_center + tf::Vector3(delta[0],delta[1],delta[2]);
00290         tf::Quaternion q;
00291         q.setRPY(delta[3],delta[4],0.00);
00292         rot_axis = rot_axis * q;
00293 
00294         rot_radius = rot_radius + delta[5];
00295 
00296         tf::Quaternion q2;
00297         q2.setRPY(delta[6],delta[7],delta[8]);
00298         rot_orientation = rot_orientation * q2;
00299 }
00300 
00301 bool RotationalModel::normalizeParameters() {
00302 //      if(model.track.pose.size()>2) {
00303 //              {
00304 //                      V_Configuration q = predictConfiguration(model.track.pose.front());
00305 //                      rot_axis = rot_axis * tf::Quaternion(tf::Vector3(0, 0, 1), -q[0]);
00306 //              }
00307 //              if(predictConfiguration(model.track.pose.back())[0]<0)
00308 //                      rot_axis = rot_axis * tf::Quaternion(tf::Vector3(1,0,0),M_PI);
00309 //
00310 //              rot_orientation = rot_axis.inverse() * poseToTransform(model.track.pose.front()).getRotation();
00311 //      }
00312         return true;
00313 }
00314 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


articulation_models
Author(s): Juergen Sturm
autogenerated on Wed Dec 26 2012 15:35:18