00001
00002
00003
00004
00005
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;
00029 rot_mode = 0;
00030 }
00031
00032
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
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
00107
00108 if(rand() % 2 == 0)
00109 {
00110 rot_mode = 1;
00111
00112
00113
00114 double angle_12 = pose1.inverseTimes(pose2).getRotation().getAngle();
00115
00116
00117 double dist_12 = pose1.getOrigin().distance( pose2.getOrigin() );
00118
00119
00120 tf::Vector3 rot_12 = pose1.getBasis() * pose1.inverseTimes(pose2).getRotation().getAxis() * -1;
00121
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
00131
00132 tf::Vector3 c1 = (pose1.getOrigin() + pose2.getOrigin())/2;
00133
00134 tf::Vector3 v1 = (pose2.getOrigin() - pose1.getOrigin());
00135
00136 v1.normalize();
00137
00138 v1 = v1 - rot_12.dot(v1)*rot_12;
00139
00140 v1.normalize();
00141
00142
00143 rot_center = c1 + v1.cross(rot_12) * rot_radius * cos(angle_12/2);
00144
00145
00146 tf::Vector3 d(1,0,0);
00147 d = pose1.getOrigin() - rot_center;
00148 d.normalize();
00149
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
00156
00157
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
00163
00164
00165
00166
00167
00168 rot_orientation = rot_axis.inverse() * pose1.getRotation();
00169
00170
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
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
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
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
00201
00202
00203
00204
00205
00206
00207
00208
00209 }
00210 } else {
00211 rot_mode = 0;
00212
00213
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
00218
00219
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
00229
00230
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
00245
00246
00247
00248
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
00269 tf::Transform pred1 = poseToTransform( predictPose(q) );
00270
00271
00272 rot_orientation = pred1.inverseTimes(pose1).getRotation();
00273 }
00274
00275
00276
00277
00278
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
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312 return true;
00313 }
00314 }