articulation_structure: structure_learner

00001 /*
00002  * model_learner.cpp
00003  */
00004 
00005 #include <ros/ros.h>
00006 #include <boost/foreach.hpp>
00007 
00008 #include "ArticulatedObject.h"
00009 #include "visualization_msgs/Marker.h"
00010 #include "visualization_msgs/MarkerArray.h"
00011 
00012 using namespace std;
00013 using namespace geometry_msgs;
00014 using namespace sensor_msgs;
00015 using namespace articulation_models;
00016 using namespace articulation_msgs;
00017 using namespace articulation_structure;
00018 using namespace visualization_msgs;
00019 
00020 ros::NodeHandle* nh;
00021 ros::NodeHandle* nh_local;
00022 
00023 ros::Publisher model_pub;
00024 ros::Publisher track_pub;
00025 ros::Publisher marker_pub;
00026 
00027 KinematicParams params;
00028 
00029 bool structureFitModels(articulation_msgs::ArticulatedObjectSrv::Request &request,
00030                 articulation_msgs::ArticulatedObjectSrv::Response &response) {
00031         ROS_INFO("Service call, fitting models, object parts=%d, obs=%d",
00032                 (int)request.object.parts.size(),
00033                 (int)request.object.parts.size()>0? (int)request.object.parts[0].pose.size():-1);
00034 
00035         ArticulatedObject object(params);
00036         object.SetObjectModel(request.object,nh_local);
00037         object.FitModels();
00038         response.object = object.GetObjectModel();
00039 
00040         return (true);
00041 }
00042 
00043 bool structureSelectSpanningTree(articulation_msgs::ArticulatedObjectSrv::Request &request,
00044                 articulation_msgs::ArticulatedObjectSrv::Response &response) {
00045         ROS_INFO("Service call, select spanning tree, object parts=%d, obs=%d",
00046                 (int)request.object.parts.size(),
00047                 (int)request.object.parts.size()>0? (int)request.object.parts[0].pose.size():-1);
00048 
00049         ArticulatedObject object(params);
00050         object.SetObjectModel(request.object,nh_local);
00051         object.FitModels();
00052         object.ComputeSpanningTree();
00053 
00054         response.object = object.GetObjectModel();
00055 
00056         return (true);
00057 }
00058 
00059 bool structureSelectFastGraph(articulation_msgs::ArticulatedObjectSrv::Request &request,
00060                 articulation_msgs::ArticulatedObjectSrv::Response &response) {
00061         ROS_INFO("Service call, select graph, object parts=%d, obs=%d",
00062                 (int)request.object.parts.size(),
00063                 (int)request.object.parts.size()>0? (int)request.object.parts[0].pose.size():-1);
00064 
00065         ArticulatedObject object(params);
00066         ROS_INFO("Setting object model");
00067         object.SetObjectModel(request.object,nh_local);
00068         ROS_INFO("Fitting link models");
00069         object.FitModels();
00070         ROS_INFO("Selecting kinematic graph (fast)");
00071         object.getFastGraph();
00072         response.object = object.GetObjectModel();
00073         return (true);
00074 }
00075 
00076 bool structureSelectGraph(articulation_msgs::ArticulatedObjectSrv::Request &request,
00077                 articulation_msgs::ArticulatedObjectSrv::Response &response) {
00078         ROS_INFO("Service call, select graph, object parts=%d, obs=%d",
00079                 (int)request.object.parts.size(),
00080                 (int)request.object.parts.size()>0? (int)request.object.parts[0].pose.size():-1);
00081 
00082         ArticulatedObject object(params);
00083         ROS_INFO("Setting object model");
00084         object.SetObjectModel(request.object,nh_local);
00085         ROS_INFO("Fitting link models");
00086         object.FitModels();
00087         ROS_INFO("Selecting kinematic graph (full eval)");
00088         object.getGraph();
00089         response.object = object.GetObjectModel();
00090         return (true);
00091 }
00092 
00093 bool structureSelectGraphAll(articulation_msgs::ArticulatedObjectSrv::Request &request,
00094                 articulation_msgs::ArticulatedObjectSrv::Response &response) {
00095         ROS_INFO("Service call, select graph ALL (eval), object parts=%d, obs=%d",
00096                 (int)request.object.parts.size(),
00097                 (int)request.object.parts.size()>0? (int)request.object.parts[0].pose.size():-1);
00098 
00099         ArticulatedObject object(params);
00100         ROS_INFO("Setting object model");
00101         object.SetObjectModel(request.object,nh_local);
00102         ROS_INFO("Fitting link models");
00103         object.FitModels();
00104         ROS_INFO("Selecting kinematic graph (full eval)");
00105         object.ComputeSpanningTree();
00106         object.getFastGraph();
00107         object.getGraph();
00108         response.object = object.GetObjectModel();
00109         return (true);
00110 }
00111 
00112 map<string,MarkerArray> markers,old_markers;
00113 vector<int> old_model_ids;
00114 
00115 void AddMarkerLine(string ns,Point point_from,Point point_to,double size=0.01,double H=0,double S=1,double V=1,Point point_relative=Point()) {
00116         Marker marker;
00117         marker.id = markers[ns].markers.size();
00118         marker.type = Marker::LINE_LIST;
00119         marker.action= Marker::ADD;
00120         marker.pose.orientation.w = 1.00;
00121         marker.color =  HSV_to_RGB(H,S,V);
00122         marker.scale.x = size;
00123         marker.scale.y = size;
00124         marker.scale.z = size;
00125         marker.pose.position = point_relative;
00126         Point point_diff1;
00127         point_diff1.x = point_from.x - point_relative.x;
00128         point_diff1.y = point_from.y - point_relative.y;
00129         point_diff1.z = point_from.z - point_relative.z;
00130         Point point_diff2;
00131         point_diff2.x = point_to.x - point_relative.x;
00132         point_diff2.y = point_to.y - point_relative.y;
00133         point_diff2.z = point_to.z - point_relative.z;
00134         marker.points.push_back(point_diff1);
00135         marker.points.push_back(point_diff2);
00136         marker.ns = ns;
00137         markers[ns].markers.push_back(marker);
00138 }
00139 
00140 void AddMarkerPoint(string ns,Point point,double size=0.01,double H=0,double S=1,double V=1) {
00141         Marker marker;
00142         marker.id = markers[ns].markers.size();
00143         marker.type = Marker::SPHERE;
00144         marker.action= Marker::ADD;
00145         marker.pose.orientation.w = 1.00;
00146         marker.color =  HSV_to_RGB(H,S,V);
00147         marker.scale.x = size;
00148         marker.scale.y = size;
00149         marker.scale.z = size;
00150         marker.pose.position = point;
00151         marker.ns = ns;
00152         markers[ns].markers.push_back(marker);
00153 }
00154 void AddMarkerLine(string ns,Pose pose_from,Pose pose_to,double size=0.01,double H=0,double S=1,double V=1,Pose relative=Pose()) {
00155         AddMarkerLine(ns,pose_from.position,pose_to.position,size,H,S,V,relative.position);
00156 }
00157 
00158 void AddMarkerLine(string ns,btTransform pose_from,btTransform pose_to,double size=0.01,double H=0,double S=1,double V=1,btTransform relative=btTransform::getIdentity()) {
00159         AddMarkerLine(ns,transformToPose(pose_from),transformToPose(pose_to),size,H,S,V,transformToPose(relative));
00160 }
00161 
00162 void AddMarkerLine(string ns,btVector3 pose_from,btVector3 pose_to,double size=0.01,double H=0,double S=1,double V=1,btVector3 relative=btVector3(0,0,0)) {
00163         AddMarkerLine(ns,vectorToPosition(pose_from),vectorToPosition(pose_to),size,H,S,V,vectorToPosition(relative));
00164 }
00165 
00166 void AddText(string ns,Point point,string s="hello",double size=0.05,double H=0,double S=1,double V=1) {
00167         Marker marker;
00168         marker.id = markers[ns].markers.size();
00169         marker.type = Marker::TEXT_VIEW_FACING;
00170         marker.action= Marker::ADD;
00171         marker.pose.position = point;
00172         marker.pose.orientation.w = 1.00;
00173         marker.color =  HSV_to_RGB(H,S,V);
00174         marker.scale.x = size;
00175         marker.scale.y = size;
00176         marker.scale.z = size;
00177         marker.ns = ns;
00178         marker.text = s;
00179         markers[ns].markers.push_back(marker);
00180 }
00181 
00182 void DeleteOldMarkers() {
00183         for(map<string,MarkerArray>::iterator i=markers.begin();i!=markers.end();i++) {
00184                 string ns = i->first;
00185                 while(markers[ns].markers.size() < old_markers[ns].markers.size()) {
00186                         Marker marker;
00187                         marker.id = markers[ns].markers.size();
00188                         marker.action= Marker::DELETE;
00189                         marker.ns = ns;
00190                         markers[ns].markers.push_back(marker);
00191                 }
00192         }
00193 }
00194 
00195 btVector3 getOrtho(btVector3 a,btVector3 b) {
00196         btVector3 bb = b - a.dot(b)*a;
00197         bb.normalize();
00198         a.normalize();
00199         return a.cross(bb);
00200 }
00201 
00202 bool visualizeGraph(articulation_msgs::ArticulatedObjectSrv::Request &request,
00203                 articulation_msgs::ArticulatedObjectSrv::Response &response) {
00204         ROS_INFO("Service call, visualize graph, object parts=%d, obs=%d",
00205                 (int)request.object.parts.size(),
00206                 (int)request.object.parts.size()>0? (int)request.object.parts[0].pose.size():-1);
00207 
00208         ArticulatedObject object(params);
00209         object.SetObjectModel(request.object,nh_local);
00210 
00211         // send tracks of object parts, and projected tracks of object parts
00212         for(size_t i=0;i<response.object.parts.size();i++) {
00213                 response.object.parts[i].id = i;
00214                 track_pub.publish(response.object.parts[i]);
00215         }
00216 
00217         // send link models, including observations, projected and resampled observations
00218         // first, delete old models
00219         for(size_t i=0;i<old_model_ids.size();i++) {
00220                 ModelMsg m;
00221                 m.header = object.object_msg.header;
00222                 m.id = old_model_ids[i];
00223                 m.track.id = old_model_ids[i];
00224                 model_pub.publish(m);
00225         }
00226         old_model_ids.clear();
00227 
00228         // send link visualization (links + text)
00229         for(KinematicGraph::iterator i= object.currentGraph.begin();i!=object.currentGraph.end();i++) {
00230                 int from = i->first.first;
00231                 int to = i->first.second;
00232                 GenericModelPtr &model = i->second;
00233                 Pose pose_from = object.object_msg.parts[from].pose.back();
00234                 Pose pose_to = object.object_msg.parts[to].pose.back();
00235 //              AddText("modelname.end",
00236 //                              pose_to.position,
00237 //                              model->getModelName(),
00238 //                              0.03,
00239 //                              0,0,0);
00240 
00241                 if(model->getModelName()=="rigid") {
00242                         // straight line between two parts
00243                         AddMarkerLine("link",pose_from,pose_to,0.01,0.1666,1.0,1.0);
00244                         AddMarkerPoint("decoration.poses",
00245                                         pose_to.position,
00246                                         0.015,0.166,1.0,1.0);
00247                         AddText("modelname",
00248                                         vectorToPosition((
00249                                                         positionToVector(pose_from.position)+positionToVector(pose_to.position))*0.5
00250                                                         + btVector3(0.,0.,-0.001)
00251                                         ),
00252                                         model->getModelName(),
00253                                         0.03,
00254                                         0,0,1);
00255                 } else if(model->getModelName()=="prismatic") {
00256                         AddMarkerPoint("decoration.poses",
00257                                         pose_to.position,
00258                                         0.015,0.0,1.0,1.0);
00259                         Pose origin = model->predictPose(model->predictConfiguration( transformToPose(btTransform::getIdentity()) ));
00260                         btTransform pose_orthogonal = poseToTransform(pose_from)*poseToTransform(origin);
00261 
00262                         btTransform pose_max = poseToTransform(pose_from)*poseToTransform(
00263                                         model->predictPose(model->getMinConfigurationObserved()));
00264                         btTransform pose_min = poseToTransform(pose_from)*poseToTransform(
00265                                         model->predictPose(model->getMaxConfigurationObserved()));
00266 
00267                         // draw tics
00268                         for(double q = model->getMinConfigurationObserved()[0];
00269                                         q<model->getMaxConfigurationObserved()[0];
00270                                         q += 0.05) {
00271                                 V_Configuration qq(1);
00272                                 qq(0) = q;
00273                                 btTransform pose_q = poseToTransform(pose_from)*poseToTransform(
00274                                                 model->predictPose(qq));
00275 
00276                                 btVector3 dir = poseToTransform(pose_from) * boost::static_pointer_cast<PrismaticModel>(model)->prismatic_dir
00277                                                 - poseToTransform(pose_from) * btVector3(0,0,0);
00278                                 dir.normalize();
00279 
00280                                 AddMarkerLine("decoration.poses",
00281                                                 pose_q.getOrigin(),
00282                                                 pose_q.getOrigin() + getOrtho(dir,btVector3(0,0,1))*0.005,
00283                                                 0.0025,0.0,0.0,0.3,pose_q.getOrigin());
00284                                 AddMarkerLine("decoration.poses",
00285                                                 pose_q.getOrigin() ,
00286                                                 pose_q.getOrigin() + getOrtho(dir,btVector3(0,0,1))*(-0.005),
00287                                                 0.0025,0.0,0.0,0.3,pose_q.getOrigin());
00288                         }
00289 
00290                         //AddMarkerLine("link",pose_from,pose_to,0.005,0.0,0.0,0.3);
00291                         AddMarkerLine("link",poseToTransform(pose_from),poseToTransform(pose_to),0.01,0.0,1.0,1.0);
00292                         //AddMarkerLine("link",pose_orthogonal,poseToTransform(pose_to),0.01,0.0,1.0,1.0);
00293                         AddMarkerLine("decoration",pose_min,pose_max,0.005,0.00,1.0,0.5);
00294                         //AddMarkerLine("decoration",pose_min,pose_max,0.0025,0.0,0.2,0.0);
00295                         AddText("modelname",
00296                                         vectorToPosition((
00297                                                         positionToVector(pose_from.position)+positionToVector(pose_to.position))*0.5
00298                                                         + btVector3(0.,0.,-0.001)
00299                                         ),
00300                                         model->getModelName(),
00301                                         0.03,
00302                                         0,0,1);
00303                 } else if(model->getModelName()=="rotational") {
00304                         // draw line to rotational center
00305                         geometry_msgs::Pose rot_center = transformToPose(
00306                                         poseToTransform(pose_from)*
00307                                         btTransform(
00308                                         boost::static_pointer_cast<RotationalModel>(model)->rot_axis,
00309                                         boost::static_pointer_cast<RotationalModel>(model)->rot_center)
00310                                         );
00311 
00312                         AddMarkerLine("link",pose_from,rot_center,0.01,0.666,1.0,1.0);
00313                         AddMarkerLine("link",rot_center,pose_to,0.01,0.666,1.0,1.0);
00314                         AddMarkerLine("decoration",rot_center,pose_to,0.01,0.666,1.0,1.0);
00315                         AddMarkerPoint("decoration.poses",
00316                                         pose_to.position,
00317                                         0.015,0.667,1.0,1.0);
00318 
00319                         double q0 = model->predictConfiguration( transformToPose(btTransform::getIdentity()) )[0];
00320                         double q1 = model->predictConfiguration( model->model.track.pose.back() )[0];
00321                         if(q0>q1) {
00322                                 double q= q1;
00323                                 q1 = q0;
00324                                 q0 = q;
00325                         }
00326                         if(q1-q0 > M_PI) {
00327                                 // go the other way around
00328                                 double q= q1;
00329                                 q1 = q0+2*M_PI;
00330                                 q0 = q;
00331                         }
00332 
00333                         V_Configuration Qmin = model->getMinConfigurationObserved();
00334                         V_Configuration Qmax = model->getMaxConfigurationObserved();
00335 
00336                         V_Configuration Q(1);
00337                         btTransform t1,t2,t3,t4;
00338                         double STEPSIZE = M_PI/16;
00339                         double radius = 0.05;
00340                         for(double q=0;q<2*M_PI+STEPSIZE;q+=STEPSIZE) {
00341                                 bool in_range = q>=Qmin[0] && q<=Qmax[0];
00342                                 if(Qmin[0]<0) in_range = q>=(Qmin[0]+2*M_PI) || q<=Qmax[0];
00343                                 Q[0] = q;
00344                                 t1 =    poseToTransform(rot_center) *
00345                                                 btTransform(btQuaternion(btVector3(0,0,1),-q),btVector3(0,0,0)) *
00346                                                 btTransform(btQuaternion(0,0,0,1),btVector3(radius,0,0.0));
00347                                 t2 =    poseToTransform(rot_center) *
00348                                                 btTransform(btQuaternion(btVector3(0,0,1),-(q+STEPSIZE)),btVector3(0,0,0)) *
00349                                                 btTransform(btQuaternion(0,0,0,1),btVector3(radius,0,0.0));
00350                                 t3 =    poseToTransform(rot_center) *
00351                                                 btTransform(btQuaternion(btVector3(0,0,1),-(q+STEPSIZE)),btVector3(0,0,0)) *
00352                                                 btTransform(btQuaternion(0,0,0,1),btVector3(radius*1.1,0,0.0));
00353                                 t4 =    poseToTransform(rot_center) *
00354                                                 btTransform(btQuaternion(btVector3(0,0,1),-(q+STEPSIZE)),btVector3(0,0,0)) *
00355                                                 btTransform(btQuaternion(0,0,0,1),btVector3(radius*0.9,0,0.0));
00356                                 AddMarkerLine("decoration",
00357                                                 t1*btTransform(btQuaternion(0,0,0,1),btVector3(-0.015,0,0)),
00358                                                 t2*btTransform(btQuaternion(0,0,0,1),btVector3(-0.015,0,0)),
00359                                                 0.005,
00360                                                 0.666,0.0,in_range?0.00:0.50,poseToTransform(rot_center));
00361                         }
00362 
00363                         // rotation axis:
00364                         AddMarkerLine("decoration",
00365                                         poseToTransform(rot_center),
00366                                         poseToTransform(rot_center) *
00367                                         btTransform(btQuaternion(0,0,0,1),btVector3(0,0,-0.10)),
00368                                         0.01,0.666,0.2,0.2,poseToTransform(rot_center));
00369                         AddMarkerLine("decoration",
00370                                         poseToTransform(rot_center),
00371                                         poseToTransform(rot_center) *
00372                                         btTransform(btQuaternion(0,0,0,1),btVector3(0,0,+0.10)),
00373                                         0.01,0.666,0.2,0.2,poseToTransform(rot_center));
00374                         AddText("modelname",
00375                                         vectorToPosition(
00376                                                         positionToVector(rot_center.position)
00377                                                         + btVector3(0.,0.,-0.001)
00378                                         ),
00379                                         model->getModelName(),
00380                                         0.03,
00381                                         0,0,1);
00382 
00383                         double STEPSIZE2 = M_PI/64;
00384 //                      bool first = true;
00385 
00386                         // draw arc
00387                         btTransform pose_q2;
00388                         btTransform pose_q1;
00389                         for(double q = Qmin[0]; q<=Qmax[0]; q += (Qmax[0]-Qmin[0])/32) {
00390 //                              cout <<"q="<<q<<endl;
00391                                 pose_q2 = pose_q1;
00392                                 V_Configuration qq(1);
00393                                 qq(0) = q;
00394                                 pose_q1 = poseToTransform(pose_from)*poseToTransform(model->predictPose(qq));
00395 
00396                                 if(q != Qmin[0]) {
00397                                         AddMarkerLine("decoration.poses",
00398                                                                 pose_q1.getOrigin(),
00399                                                                 pose_q2.getOrigin(),
00400                                                                 0.005,0.667,1.0,0.5);
00401                                 }
00402                         }
00403 
00404 
00405                         // draw tics
00406                         for(double q = model->getMinConfigurationObserved()[0];
00407                                         q<model->getMaxConfigurationObserved()[0];
00408                                         q += STEPSIZE2) {
00409                                 V_Configuration qq(1);
00410                                 qq(0) = q;
00411                                 btTransform pose_q1 = poseToTransform(pose_from)*poseToTransform(
00412                                                 model->predictPose(qq));
00413                                 qq(0) = q+STEPSIZE2;
00414                                 btTransform pose_q2 = poseToTransform(pose_from)*poseToTransform(
00415                                                 model->predictPose(qq));
00416 
00417                                 btVector3 dir = pose_q2.getOrigin()-pose_q1.getOrigin();
00418                                 dir.normalize();
00419 
00420                                 AddMarkerLine("decoration.poses",
00421                                                 pose_q1.getOrigin(),
00422                                                 pose_q1.getOrigin() + getOrtho(dir,btVector3(0,0,1))*0.005,
00423                                                 0.0025,0.0,0.0,0.3,pose_q1.getOrigin());
00424                                 AddMarkerLine("decoration.poses",
00425                                                 pose_q1.getOrigin() ,
00426                                                 pose_q1.getOrigin() + getOrtho(dir,btVector3(0,0,1))*(-0.005),
00427                                                 0.0025,0.0,0.0,0.3,pose_q1.getOrigin());
00428                         }
00429                 } else if(model->getModelName()=="pca_gp") {
00430                         AddMarkerPoint("decoration.poses",
00431                                         pose_to.position,
00432                                         0.015,0.333,1.0,1.0);
00433                         for(double q = model->getMinConfigurationObserved()[0];
00434                                         q<model->getMaxConfigurationObserved()[0];
00435                                         q += 0.05) {
00436                                 V_Configuration qq(1);
00437                                 qq(0) = q;
00438                                 btTransform pose_q2 = poseToTransform(pose_from)*poseToTransform(
00439                                                 model->predictPose(qq));
00440                                 qq(0) = q+0.05;
00441                                 btTransform pose_q1 = poseToTransform(pose_from)*poseToTransform(
00442                                                 model->predictPose(qq));
00443 
00444                                 AddMarkerLine("decoration.poses",
00445                                                 pose_q1.getOrigin(),
00446                                                 pose_q2.getOrigin(),
00447                                                 0.0025,0.0,0.0,0.3);
00448 
00449                                 btVector3 dir = pose_q2.getOrigin()-pose_q1.getOrigin();
00450                                 dir.normalize();
00451 
00452                                 AddMarkerLine("decoration.poses",
00453                                                 pose_q1.getOrigin(),
00454                                                 pose_q1.getOrigin() + getOrtho(dir,btVector3(0,0,1))*0.005,
00455                                                 0.0025,0.0,0.0,0.3,pose_q1.getOrigin());
00456                                 AddMarkerLine("decoration.poses",
00457                                                 pose_q1.getOrigin() ,
00458                                                 pose_q1.getOrigin() + getOrtho(dir,btVector3(0,0,1))*(-0.005),
00459                                                 0.0025,0.0,0.0,0.3,pose_q1.getOrigin());
00460                         }
00461 
00462 
00463                         AddMarkerLine("link",pose_from,pose_to,0.01,0.333,1.0,1.0);
00464                         AddText("modelname",
00465                                         vectorToPosition((
00466                                                         positionToVector(pose_from.position)+positionToVector(pose_to.position))*0.5
00467                                                         + btVector3(0.,0.,-0.001)
00468                                         ),
00469                                         model->getModelName(),
00470                                         0.03,
00471                                         0,0,1);
00472                 } else {
00473                         AddMarkerLine("link",pose_from,pose_to,0.01,5.0/6,1.0,1.0);
00474                         AddText("modelname",
00475                                         vectorToPosition((
00476                                                         positionToVector(pose_from.position)+positionToVector(pose_to.position))*0.5
00477                                                         + btVector3(0.,0.,-0.001)
00478                                         ),
00479                                         model->getModelName(),
00480                                         0.03,
00481                                         0,0,1);
00482                 }
00483         }
00484         map<string,MarkerArray> new_markers = markers;
00485         DeleteOldMarkers();
00486         old_markers = new_markers;
00487         MarkerArray all_markers;
00488         for(map<string,MarkerArray>::iterator i =markers.begin();i!=markers.end();i++) {
00489                 BOOST_FOREACH(Marker& m,i->second.markers) {
00490                         m.header = object.object_msg.header;
00491                         all_markers.markers.push_back(m);
00492 //                      cout <<"ns="<<m.ns<<" id="<<m.id<<" action="<<m.action<<" points="<<m.points.size()<<endl;
00493                 }
00494         }
00495 //      cout <<endl;
00496         markers.clear();
00497         marker_pub.publish(all_markers);
00498 
00499         // transform observed tracks into global frame, using estimated pose of latest observed pose
00500         for(KinematicGraph::iterator i= object.currentGraph.begin();i!=object.currentGraph.end();i++) {
00501                 // note: observations are relative to the first marker
00502                 int from = i->first.first;
00503 //              int to = i.first.second;
00504                 GenericModelPtr &model = i->second;
00505                 model->sampleConfigurationSpace(0.02);
00506 
00507                 Pose origin = object.object_msg.parts[from].pose.back();
00508                 BOOST_FOREACH(Pose& p, model->model.track.pose) {
00509                         p = transformToPose(poseToTransform(origin) * poseToTransform( p ));
00510                 }
00511                 BOOST_FOREACH(Pose& p, model->model.track.pose_projected) {
00512                         p = transformToPose(poseToTransform(origin) * poseToTransform( p ));
00513                 }
00514                 BOOST_FOREACH(Pose& p, model->model.track.pose_resampled) {
00515                         p = transformToPose(poseToTransform(origin) * poseToTransform( p ));
00516                 }
00517 
00518                 ModelMsg m = model->getModel();
00519                 m.header = object.object_msg.header;
00520                 model_pub.publish(m);
00521                 old_model_ids.push_back(model->model.id);
00522         }
00523 
00524 
00525         response.object = object.GetObjectModel();
00526         response.object.markers = all_markers;
00527         return (true);
00528 }
00529 
00530 
00531 int main(int argc, char** argv) {
00532         ros::init(argc, argv, "structure_learner_server");
00533         nh = new ros::NodeHandle();
00534         nh_local = new ros::NodeHandle("~");
00535 
00536         params.LoadParams(*nh_local,false);
00537 
00538         model_pub = nh->advertise<articulation_msgs::ModelMsg> ("model", 0);
00539         track_pub = nh->advertise<articulation_msgs::TrackMsg> ("track", 0);
00540         marker_pub = nh->advertise<visualization_msgs::MarkerArray> ("structure_array", 0);
00541         ros::Publisher marker2_pub = nh->advertise<visualization_msgs::Marker> ("structure", 0);
00542 
00543         ros::ServiceServer fitService = nh->advertiseService("fit_models",
00544                         structureFitModels);
00545         ros::ServiceServer selectServiceSpanningTree = nh->advertiseService("get_spanning_tree",
00546                         structureSelectSpanningTree);
00547         ros::ServiceServer selectServiceFastGraph = nh->advertiseService("get_fast_graph",
00548                         structureSelectFastGraph);
00549         ros::ServiceServer selectServiceGraph = nh->advertiseService("get_graph",
00550                         structureSelectGraph);
00551         ros::ServiceServer selectServiceGraphAll = nh->advertiseService("get_graph_all",
00552                         structureSelectGraphAll);
00553         ros::ServiceServer selectVisualizeGraph = nh->advertiseService("visualize_graph",
00554                         visualizeGraph);
00555 
00556         ROS_INFO("Ready to fit articulation models and structure to articulated objects.");
00557         ros::spin();
00558 }
structure_learner_srv.cpp
