articulation_models: model_learner

00001 /*
00002  * model_learner.cpp
00003  *
00004  *  Created on: Oct 21, 2009
00005  *      Author: sturm
00006  */
00007 
00008 #include <ros/ros.h>
00009 
00010 #include "articulation_msgs/TrackModelSrv.h"
00011 
00012 #include "articulation_models/models/factory.h"
00013 #include "articulation_models/utils.h"
00014 
00015 using namespace std;
00016 using namespace articulation_models;
00017 using namespace articulation_msgs;
00018 
00019 MultiModelFactory factory;
00020 
00021 ros::NodeHandle *nh;
00022 
00023 bool selectModel(articulation_msgs::TrackModelSrv::Request &request,
00024                 articulation_msgs::TrackModelSrv::Response &response) {
00025         ROS_INFO("Service call, id=%d, poses=%d",(int) request.model.track.id,(int)request.model.track.pose.size());
00026 
00027         response.model = request.model;
00028 
00029         GenericModelVector models_new = factory.createModels(request.model.track);
00030         GenericModelVector models_filtered;
00031 
00032         double sigma_position = 0.00;
00033         double sigma_orientation = 0.00;
00034         nh->param("sigma_position", sigma_position, 0.01);
00035         nh->param("sigma_orientation", sigma_orientation, 5.00);
00036 
00037         // filter models, and setting parameters
00038         for (size_t i = 0; i < models_new.size(); i++) {
00039                 models_new[i]->setParam("sigma_position", sigma_position,
00040                                 ParamMsg::PRIOR);
00041                 models_new[i]->setParam("sigma_orientation", sigma_orientation,
00042                                 ParamMsg::PRIOR);
00043 
00044                 for (size_t j = 0; j < request.model.params.size(); j++) {
00045                         models_new[i]->setParam(request.model.params[j].name,
00046                                         request.model.params[j].value, request.model.params[j].type);
00047                 }
00048 
00049                 if (models_new[i]->getParam("disable_all") && !models_new[i]->getParam(
00050                                 "enable_" + models_new[i]->getModelName()))
00051                         continue;
00052 
00053                 models_new[i]->readParamsFromModel();
00054                 //        cout << models_new[i]->getParam("sigma_position") << endl;
00055                 models_filtered.push_back(models_new[i]);
00056 
00057         }
00058 
00059         GenericModelVector models_valid;
00060 
00061         ROS_INFO_STREAM( "number of created models: "<<models_new.size() );
00062 
00063         // fit new models, then add valid
00064         for (size_t i = 0; i < models_filtered.size(); i++) {
00065                 if (!models_filtered[i]->fitModel()) {
00066                         ROS_INFO_STREAM( models_filtered[i]->getModelName() << ": fitting failed" );
00067                         continue;
00068                 }
00069                 models_filtered[i]->projectPoseToConfiguration();
00070                 if (!models_filtered[i]->fitMinMaxConfigurations()) {
00071                         ROS_INFO_STREAM( models_filtered[i]->getModelName() << ": fit min max failed" );
00072                         continue;
00073                 }
00074                 if (!models_filtered[i]->evaluateModel()) {
00075                         ROS_INFO_STREAM( models_filtered[i]->getModelName() << ": eval failed" );
00076                         continue;
00077                 }
00078 
00079                 //        ROS_INFO_STREAM( models_filtered[i]->getModelName() << ": valid, adding" );
00080 
00081                 models_valid.push_back(models_filtered[i]);
00082         }
00083 
00084         map<double, GenericModelPtr> models_sorted;
00085         for (size_t i = 0; i < models_valid.size(); i++) {
00086                 if (isnan(models_valid[i]->getBIC()))
00087                         continue;
00088                 models_sorted[models_valid[i]->getBIC()] = models_valid[i];
00089         }
00090 
00091         map<double, GenericModelPtr>::iterator it = models_sorted.begin();
00092         if (it == models_sorted.end())
00093                 return (true);
00094 
00095         response.model = it->second->getModel();
00096 
00097         return (true);
00098 }
00099 
00100 bool fitModel(articulation_msgs::TrackModelSrv::Request &request,
00101                 articulation_msgs::TrackModelSrv::Response &response) {
00102         ROS_INFO("fit Service call, id=%d, poses=%d, name=%s", (int)request.model.track.id,(int)request.model.track.pose.size(),request.model.name.c_str());
00103 
00104         GenericModelPtr model = factory.restoreModel(request.model);
00105         if (!model) {
00106                 ROS_WARN("could not restore model '%s'",request.model.name.c_str());
00107                 return (true);
00108         }
00109         if (!model->fitModel()) {
00110                 ROS_INFO_STREAM( model->getModelName() << ": fitModel() failed" );
00111                 return (true);
00112         }
00113         model->projectPoseToConfiguration();
00114         if (!model->fitMinMaxConfigurations()) {
00115                 ROS_INFO_STREAM( model->getModelName() << ": fit min max failed" );
00116                 return (true);
00117         }
00118         if (!model->evaluateModel()) {
00119                 ROS_INFO_STREAM( model->getModelName() << ": eval failed" );
00120                 return (true);
00121         }
00122 
00123         response.model = model->getModel();
00124         return (true);
00125 }
00126 
00127 bool evalModel(articulation_msgs::TrackModelSrv::Request &request,
00128                 articulation_msgs::TrackModelSrv::Response &response) {
00129         ROS_INFO("eval Service call, id=%d, poses=%d, name=%s",(int) request.model.track.id,(int)request.model.track.pose.size(),request.model.name.c_str());
00130 
00131         GenericModelPtr model = factory.restoreModel(request.model);
00132 
00133         if (!model->evaluateModel()) {
00134                 ROS_INFO_STREAM( model->getModelName() << ": eval failed" );
00135                 return (true);
00136         }
00137 
00138         response.model = model->getModel();
00139 
00140         return (true);
00141 }
00142 
00143 bool resampleModel(articulation_msgs::TrackModelSrv::Request &request,
00144                 articulation_msgs::TrackModelSrv::Response &response) {
00145         ROS_INFO("resample Service call, id=%d, poses=%d, name=%s", (int)request.model.track.id,(int)request.model.track.pose.size(),request.model.name.c_str());
00146 
00147         GenericModelPtr model = factory.restoreModel(request.model);
00148 
00149         double density = 0.01;
00150         model->getParam("resample_density", density);
00151         model->sampleConfigurationSpace(density);
00152         response.model = model->getModel();
00153 
00154         return (true);
00155 }
00156 
00157 bool projPoseToConf(articulation_msgs::TrackModelSrv::Request &request,
00158                 articulation_msgs::TrackModelSrv::Response &response) {
00159         ROS_INFO("projPoseToConf Service call, id=%d, poses=%d, name=%s", (int)request.model.track.id,(int)request.model.track.pose.size(),request.model.name.c_str());
00160 
00161         GenericModelPtr model = factory.restoreModel(request.model);
00162 
00163         model->projectPoseToConfiguration();
00164         response.model = model->getModel();
00165 
00166         return (true);
00167 }
00168 
00169 bool projConfToPose(articulation_msgs::TrackModelSrv::Request &request,
00170                 articulation_msgs::TrackModelSrv::Response &response) {
00171         ROS_INFO("projConfToPose Service call, id=%d, poses=%d, name=%s", (int)request.model.track.id,(int)request.model.track.pose.size(),request.model.name.c_str());
00172 
00173         GenericModelPtr model = factory.restoreModel(request.model);
00174 
00175         model->projectConfigurationToPose();
00176         response.model = model->getModel();
00177 
00178         return (true);
00179 }
00180 
00181 int main(int argc, char** argv) {
00182         ros::init(argc, argv, "track_model_server");
00183 
00184         nh = new ros::NodeHandle();
00185 
00186         string filter_models("rigid rotational prismatic");
00187         ros::NodeHandle("~").getParam("filter_models", filter_models);
00188         factory.setFilter(filter_models);
00189         factory.listModelFactories();
00190 
00191         ros::ServiceServer selectService = nh->advertiseService("model_select",
00192                         selectModel);
00193 
00194         ros::ServiceServer fitService = nh->advertiseService("model_fit", fitModel);
00195 
00196         ros::ServiceServer evalService = nh->advertiseService("model_eval",
00197                         evalModel);
00198 
00199         ros::ServiceServer resampleService = nh->advertiseService("model_resample",
00200                         resampleModel);
00201 
00202         ros::ServiceServer projPoseToConfService = nh->advertiseService(
00203                         "model_project_pose_to_configuration", projPoseToConf);
00204 
00205         ros::ServiceServer projConfToPoseService = nh->advertiseService(
00206                         "model_project_configuration_to_pose", projConfToPose);
00207 
00208         ROS_INFO("Ready to select/fit/eval/resample models.");
00209         ros::spin();
00210 }
model_learner_srv.cpp
