00001
00002
00003
00004
00005
00006
00007 #include <boost/format.hpp>
00008 #include "articulation_models/models/generic_model.h"
00009
00010 #include "articulation_models/utils.h"
00011 #include "articulation_models/models/factory.h"
00012 #include <gsl/gsl_multimin.h>
00013 #include <gsl/gsl_blas.h>
00014
00015 using namespace std;
00016 using namespace boost;
00017
00018 using namespace Eigen;
00019
00020 using namespace articulation_msgs;
00021
00022 USING_PART_OF_NAMESPACE_EIGEN
00023
00024 #include <iomanip>
00025 #define VEC(a) setprecision(5)<<fixed<<a.x()<<" "<<a.y()<<" "<<a.z()<<" "<<a.w()<<" l="<<a.length()
00026 #define VEC2(a) "t=["<<VEC(a.getOrigin())<<"] r=[]"<<VEC(a.getRotation())<<"]"
00027 #define PRINT(a) cout << #a <<"=" << VEC(a)<<endl;
00028 #define PRINT2(a) cout << #a <<"=" << VEC2(a)<<endl;
00029
00030 namespace articulation_models {
00031
00032 GenericModel::GenericModel() {
00033 setId(-1);
00034 sigma_position = 0.005;
00035 sigma_orientation = 360 * M_PI/180.0;
00036
00037 avg_error_position = 0;
00038 avg_error_orientation = 0;
00039 bic = 0;
00040
00041 last_error_jacobian = 0;
00042 evaluated = false;
00043 supress_similar = true;
00044 outlier_ratio = 0.5;
00045 sac_iterations = 100;
00046 optimizer_iterations = 10;
00047
00048
00049 prior_outlier_ratio = log(0.01) / (- 0.05);
00050
00051 complexity = 0;
00052 jacobian = Eigen::MatrixXd();
00053 hessian = Eigen::MatrixXd();
00054 }
00055
00056 void GenericModel::setModel(const ModelMsg& model) {
00057 this->model = model;
00058 readParamsFromModel();
00059 prepareChannels();
00060 }
00061
00062 ModelMsg GenericModel::getModel() {
00063 writeParamsToModel();
00064 model.name = getModelName();
00065 model.header = model.track.header;
00066 model.track.pose_flags.resize(model.track.pose.size(),TrackMsg::POSE_VISIBLE );
00067 if(model.track.pose.size())
00068 model.track.pose_flags[model.track.pose.size()-1] |= TrackMsg::POSE_END_OF_SEGMENT;
00069 return model;
00070 }
00071
00072 void GenericModel::setTrack(const TrackMsg& track) {
00073 this->model.track = track;
00074 prepareChannels();
00075 }
00076
00077 TrackMsg GenericModel::getTrack() {
00078 return model.track;
00079 }
00080
00081 void GenericModel::setId(int id) {
00082 model.id = id;
00083 }
00084
00085 int GenericModel::getId() {
00086 return(model.id);
00087 }
00088
00089
00090 std::string GenericModel::getModelName() {
00091 return MultiModelFactory::instance.getLongName(this);
00092 }
00093
00094 size_t GenericModel::getDOFs() {
00095 return 0;
00096 }
00097
00098 size_t GenericModel::getSamples() {
00099 return model.track.pose.size();
00100 }
00101
00102
00103 void GenericModel::readParamsFromModel() {
00104 getParam("sigma_position",sigma_position);
00105 getParam("sigma_orientation",sigma_orientation);
00106 getParam("supress_similar",supress_similar);
00107 getParam("avg_error_position",avg_error_position);
00108 getParam("avg_error_orientation",avg_error_orientation);
00109 getParam("bic",bic);
00110 getParam("last_error_jacobian",last_error_jacobian);
00111 getParam("evaluated",evaluated);
00112
00113 getParam("jacobian",jacobian);
00114 getParam("hessian",hessian);
00115 getParam("loglikelihood",loglikelihood);
00116 getParam("outlier_ratio",outlier_ratio);
00117 getParam("sac_iterations",sac_iterations);
00118 getParam("prior_outlier_ratio",prior_outlier_ratio);
00119 }
00120
00121 void GenericModel::writeParamsToModel() {
00122 setParam("sigma_position",sigma_position,ParamMsg::PRIOR);
00123 setParam("sigma_orientation",sigma_orientation,ParamMsg::PRIOR);
00124 setParam("supress_similar",supress_similar,ParamMsg::PRIOR);
00125 setParam("avg_error_position",avg_error_position,ParamMsg::EVAL);
00126 setParam("avg_error_orientation",avg_error_orientation,ParamMsg::EVAL);
00127 setParam("loglikelihood",loglikelihood,ParamMsg::EVAL);
00128 setParam("bic",bic,ParamMsg::EVAL);
00129 setParam("last_error_jacobian",last_error_jacobian,ParamMsg::EVAL);
00130 setParam("evaluated",evaluated,ParamMsg::EVAL);
00131 setParam("complexity",complexity,ParamMsg::PRIOR);
00132 setParam("jacobian",jacobian,ParamMsg::EVAL);
00133 setParam("hessian",hessian,ParamMsg::EVAL);
00134 setParam("dofs",getDOFs(),ParamMsg::EVAL);
00135 setParam("samples",getSamples(),ParamMsg::EVAL);
00136 setParam("outlier_ratio",outlier_ratio,ParamMsg::EVAL);
00137 setParam("sac_iterations",sac_iterations,ParamMsg::PRIOR);
00138 setParam("prior_outlier_ratio",prior_outlier_ratio,ParamMsg::PRIOR);
00139 }
00140
00141 void GenericModel::prepareChannels() {
00142 channelOutlier = openChannel("outlier");
00143 channelLogLikelihood = openChannel("loglikelihood");
00144 channelInlierLogLikelihood = openChannel("loglikelihood_if_inlier");
00145 channelConfiguration.resize(getDOFs(),0);
00146 for(size_t i=0;i<getDOFs();i++) {
00147 channelConfiguration[i] = openChannel(str(format("q%d")%i));
00148 }
00149
00150 }
00151
00152 double GenericModel::getParam(std::string name) {
00153 for(size_t i=0;i<model.params.size();i++) {
00154 if(model.params[i].name == name) {
00155 return(model.params[i].value);
00156 }
00157 }
00158
00159 return 0.00;
00160 }
00161
00162 void GenericModel::getParam(std::string name,double& value) {
00163 if(hasParam(name))
00164 value = getParam(name);
00165 }
00166
00167 void GenericModel::getParam(std::string name,Eigen::VectorXd &vec) {
00168 for(int i=0;i<vec.size();i++)
00169 vec(i) = getParam( str(boost::format( name+"[%1%]") %i ) );
00170 }
00171
00172 void GenericModel::getParam(std::string name,btVector3 &vec) {
00173 vec.setX( getParam( name+".x" ) );
00174 vec.setY( getParam( name+".y" ) );
00175 vec.setZ( getParam( name+".z" ) );
00176 }
00177
00178 void GenericModel::getParam(std::string name,btQuaternion &quat) {
00179 quat.setX( getParam( name+".x" ) );
00180 quat.setY( getParam( name+".y" ) );
00181 quat.setZ( getParam( name+".z" ) );
00182 quat.setW( getParam( name+".w" ) );
00183 }
00184
00185 void GenericModel::getParam(std::string name,btTransform &t) {
00186 btQuaternion q;
00187 btVector3 v;
00188 getParam(name+".orientation",q);
00189 getParam(name+".position",v);
00190 t = btTransform(q,v);
00191 }
00192
00193 void GenericModel::getParam(std::string name,Eigen::MatrixXd &mat) {
00194 for(int r=0;r<mat.rows();r++)
00195 for(int c=0;c<mat.cols();c++)
00196 mat(r,c) = getParam( str(boost::format( name+"[%1%][%2%]") %r % c) );
00197 }
00198
00199 void GenericModel::setParam(std::string name,double value,int type) {
00200 for(size_t i=0;i<model.params.size();i++) {
00201 if(model.params[i].name == name) {
00202 model.params[i].value = value;
00203 return;
00204 }
00205 }
00206 ParamMsg data;
00207 data.name = name;
00208 data.value = value;
00209 data.type = type;
00210 model.params.push_back(data);
00211 }
00212
00213 void GenericModel::setParam(std::string name,const Eigen::VectorXd &vec,int type) {
00214 for(int i=0;i<vec.size();i++)
00215 setParam( str(boost::format( name+"[%1%]") %i ),vec(i),type );
00216 }
00217
00218 void GenericModel::setParam(std::string name,const btVector3 &vec,int type) {
00219 setParam( name+".x",vec.x(),type );
00220 setParam( name+".y",vec.y(),type );
00221 setParam( name+".z",vec.z(),type );
00222 }
00223
00224 void GenericModel::setParam(std::string name,const btQuaternion &quat,int type) {
00225 setParam( name+".x",quat.x(),type );
00226 setParam( name+".y",quat.y(),type );
00227 setParam( name+".z",quat.z(),type );
00228 setParam( name+".w",quat.w(),type );
00229 }
00230
00231 void GenericModel::setParam(std::string name,const btTransform &t,int type) {
00232 setParam(name+".position",t.getOrigin(),type);
00233 setParam(name+".orientation",t.getRotation(),type);
00234 }
00235
00236 void GenericModel::setParam(std::string name,const Eigen::MatrixXd &mat,int type) {
00237 for(int r=0;r<mat.rows();r++)
00238 for(int c=0;c<mat.cols();c++)
00239 setParam( str(boost::format( name+"[%1%][%2%]") %r%c ),mat(r,c),type );
00240 }
00241
00242 bool GenericModel::hasParam(std::string name) {
00243 for(size_t i=0;i<model.params.size();i++) {
00244 if(model.params[i].name == name)
00245 return(true);
00246 }
00247 return false;
00248 }
00249
00250
00251 int GenericModel::openChannel(std::string name,bool autocreate) {
00252 return(articulation_models::openChannel(model.track,name,autocreate));
00253 }
00254
00255
00256 bool GenericModel::fitModel() {
00257 if(!sampleConsensus()) {
00258
00259 return false;
00260 }
00261 if(!optimizeParameters()) {
00262
00263 return false;
00264 }
00265 if(!normalizeParameters()) {
00266 return false;
00267 }
00268 return true;
00269 }
00270
00271 bool GenericModel::fitMinMaxConfigurations() {
00272 setParam("q_min",getMinConfigurationObserved(),ParamMsg::PARAM);
00273 setParam("q_max",getMaxConfigurationObserved(),ParamMsg::PARAM);
00274 return true;
00275 }
00276
00277
00278
00279 bool GenericModel::evaluateModel() {
00280
00281 if(model.track.pose.size() == 0)
00282 return false;
00283
00284
00285 loglikelihood = getLogLikelihood(false);
00286
00287
00288
00289 size_t n = model.track.pose.size();
00290
00291
00292 double sum_position2 = 0;
00293 double sum_orientation2 = 0;
00294
00295 double sum_position = 0;
00296 double sum_orientation = 0;
00297
00298
00299 for(size_t i=0;i<n;i++) {
00300 btTransform p1 = poseToTransform(model.track.pose[i]);
00301 btTransform p2 = poseToTransform(model.track.pose_projected[i]);
00302 btTransform diff = p1.inverseTimes(p2);
00303 double err_position2 = diff.getOrigin().length2();
00304 double err_orientation2 = SQR(diff.getRotation().getAngle());
00305
00306
00307 sum_position2 += err_position2;
00308 sum_orientation2 += err_orientation2;
00309 sum_position += sqrt(err_position2);
00310 sum_orientation += sqrt(err_orientation2);
00311 }
00312
00313
00314 avg_error_position = sum_position/n;
00315 avg_error_orientation = sum_orientation/n;
00316 loglikelihood += - ((double)n)*getDOFs()*log(n);
00317
00318 bic =
00319 -2*(loglikelihood )
00320 +complexity
00321 * log( n );
00322
00323 last_error_jacobian = evalLatestJacobian();
00324
00325 evaluated = true;
00326
00327 if(model.track.pose.size()>=2) {
00328 VectorXd q1 = predictConfiguration( model.track.pose.front() );
00329 VectorXd q2 = predictConfiguration( model.track.pose.back() );
00330
00331 VectorXd p = pointToEigen(model.track.pose.back().position) - pointToEigen(model.track.pose.front().position);
00332 jacobian = predictJacobian( q2 );
00333 hessian = predictHessian( q2 );
00334 if(getDOFs()>=1 ) {
00335 if(p.dot(jacobian.col(0))<0)
00336 jacobian *= -1;
00337 hessian *= -1;
00338 }
00339 }
00340 writeParamsToModel();
00341 return true;
00342 }
00343
00344 double GenericModel::evalLatestJacobian() {
00345 if(model.track.pose.size()<2)
00346 return 0.00;
00347
00348
00349 VectorXd p1 = pointToEigen( model.track.pose[model.track.pose.size()-1].position );
00350 VectorXd p2 = pointToEigen( model.track.pose[model.track.pose.size()-2].position );
00351 VectorXd pD_obs = p1-p2;
00352 if(pD_obs.norm() == 0)
00353 return 0.00;
00354 pD_obs.normalize();
00355
00356
00357
00358
00359 if(getDOFs()==0)
00360 return 2*M_PI;
00361
00362
00363 VectorXd q1 = predictConfiguration( model.track.pose[model.track.pose.size()-1] );
00364 VectorXd q2 = predictConfiguration( model.track.pose[model.track.pose.size()-2] );
00365
00366
00367 MatrixXd J2 = predictJacobian( q2 );
00368
00369 VectorXd qD = (q1 - q2);
00370
00371 VectorXd pD_pred = J2 * qD;
00372
00373 if(pD_pred.norm() == 0)
00374 return 0.00;
00375 pD_pred.normalize();
00376
00377
00378
00379
00380
00381 return acos( pD_obs.dot( pD_pred ) );
00382 }
00383
00384 double GenericModel::getPositionError() {
00385 return avg_error_position;
00386 }
00387
00388 double GenericModel::getOrientationError() {
00389 return avg_error_orientation;
00390 }
00391
00392 double GenericModel::getBIC() {
00393 return bic;
00394 }
00395
00396
00397 geometry_msgs::Pose GenericModel::predictPose(V_Configuration q) {
00398 geometry_msgs::Pose p;
00399 p.orientation.w = 1;
00400 return p;
00401 }
00402
00403 M_CartesianJacobian GenericModel::predictJacobian(V_Configuration vq,double delta) {
00404 M_CartesianJacobian J;
00405 J.resize(3,getDOFs());
00406 VectorXd p = pointToEigen(predictPose(vq).position);
00407 for(size_t i=0;i<getDOFs();i++) {
00408 V_Configuration q = vq;
00409 q(i) += delta;
00410 J.col(i) = (pointToEigen( predictPose(q).position ) - p)/delta;
00411 }
00412 return J;
00413 }
00414
00415 M_CartesianJacobian GenericModel::predictHessian(V_Configuration q,double delta) {
00416 M_CartesianJacobian H;
00417 H.resize(3*getDOFs(),getDOFs());
00418
00419 for(size_t i=0;i<getDOFs();i++) {
00420 V_Configuration qd = q;
00421 q(i) += delta;
00422 M_CartesianJacobian H_part;
00423
00424 M_CartesianJacobian J = predictJacobian(q);
00425 M_CartesianJacobian Jd = predictJacobian(qd);
00426
00427
00428
00429
00430 H_part = (Jd - J)/delta;
00431
00432 for(size_t r=0;r<3;r++) {
00433 for(size_t c=0;c<getDOFs();c++) {
00434 H(r+3*i,c) = H_part(r,c);
00435 }
00436 }
00437 }
00438 return H;
00439 }
00440
00441
00442 void GenericModel::setConfiguration(size_t index,V_Configuration q) {
00443 std::map<int,int> channel;
00444
00445 for(size_t ch=0;ch<getDOFs();ch++) {
00446 std::stringstream s;
00447 s << "q" << ch;
00448 channel[ch] = openChannel( s.str() );
00449 }
00450
00451 for(int j=0;j<q.size();j++) {
00452 model.track.channels[ channel[j] ].values[index] = q[j];
00453 }
00454 }
00455
00456
00457 void GenericModel::setJacobian(size_t index,M_CartesianJacobian J) {
00458 std::map<int,std::map<int,int> > channel;
00459
00460 for(int r=0;r<J.rows();r++)
00461 for(int c=0;c<J.cols();c++)
00462 channel[r][c] = openChannel( boost::str(boost::format( "J[%1%][%2%]") %r%c ) );
00463
00464 for(int r=0;r<J.rows();r++)
00465 for(int c=0;c<J.cols();c++)
00466 model.track.channels[ channel[r][c] ].values[index] = J(r,c);
00467 }
00468
00469 V_Configuration GenericModel::getConfiguration(size_t index) {
00470 std::map<int,int> channel;
00471
00472 for(size_t ch=0;ch<getDOFs();ch++) {
00473 std::stringstream s;
00474 s << "q" << ch;
00475 channel[ch] = openChannel( s.str() );
00476 }
00477
00478 V_Configuration q;
00479 if (getDOFs()) q.resize(getDOFs());
00480 for(int j=0;j<q.size();j++) {
00481 q[j] = model.track.channels[ channel[j] ].values[index];
00482 }
00483 return(q);
00484 }
00485
00486 V_Configuration GenericModel::predictConfiguration(geometry_msgs::Pose pose) {
00487 V_Configuration q;
00488 if(getDOFs()>0) q.resize(getDOFs());
00489 return q;
00490 }
00491
00492
00493
00494 V_Configuration GenericModel::getMinConfigurationObserved() {
00495 if(getDOFs()==0) return V_Configuration();
00496 V_Configuration q_min(getDOFs());
00497 for(size_t j=0;j<getDOFs();j++) {
00498 q_min[j] = FLT_MAX;
00499 }
00500
00501 for(size_t i=0;i<model.track.pose.size();i++) {
00502 V_Configuration q = getConfiguration(i);
00503 for(size_t j=0;j<getDOFs();j++) {
00504 q_min[j] = MIN( q_min[j], q[j] );
00505 }
00506 }
00507
00508 return q_min;
00509 }
00510
00511 V_Configuration GenericModel::getMaxConfigurationObserved() {
00512 if(getDOFs()==0) return V_Configuration();
00513 V_Configuration q_max(getDOFs());
00514 for(size_t j=0;j<getDOFs();j++) {
00515 q_max[j] = -FLT_MAX;
00516 }
00517
00518 for(size_t i=0;i<model.track.pose.size();i++) {
00519 V_Configuration q = getConfiguration(i);
00520 for(size_t j=0;j<getDOFs();j++) {
00521 q_max[j] = MAX( q_max[j], q[j] );
00522 }
00523 }
00524
00525 return q_max;
00526 }
00527
00528
00529 void GenericModel::projectPoseToConfiguration() {
00530
00531 for(size_t i=0;i<model.track.pose.size();i++) {
00532 V_Configuration q;
00533 q = predictConfiguration( model.track.pose[i] );
00534 setConfiguration(i,q);
00535 }
00536 }
00537
00538 void GenericModel::projectConfigurationToPose() {
00539
00540 model.track.pose_projected.resize(model.track.pose.size());
00541 for(size_t i=0;i<model.track.pose.size();i++) {
00542 V_Configuration q = getConfiguration(i);
00543 model.track.pose_projected[i] = predictPose( q );
00544 }
00545 }
00546
00547 void GenericModel::projectConfigurationToJacobian() {
00548
00549 model.track.pose_projected.resize(model.track.pose.size());
00550 for(size_t i=0;i<model.track.pose.size();i++) {
00551 V_Configuration q = getConfiguration(i);
00552 setJacobian(i, predictJacobian( q ));
00553 }
00554 }
00555
00556 void GenericModel::sampleConfigurationSpace(double resolution) {
00557
00558
00559 model.track.pose_resampled.clear();
00560
00561 if(getDOFs() == 0) {
00562 model.track.pose_resampled.push_back( predictPose(V_Configuration() ) );
00563 return;
00564 }
00565
00566 V_Configuration q_min(getDOFs());
00567 V_Configuration q_max(getDOFs());
00568 getParam("q_min",q_min);
00569 getParam("q_max",q_max);
00570
00571 geometry_msgs::Pose p_min = predictPose( q_min );
00572 geometry_msgs::Pose p_max = predictPose( q_max );
00573 double dist =
00574 (positionToVector(p_min.position) - positionToVector(p_max.position)).length();
00575 if(isnan(dist) || isinf(dist)) return;
00576
00577
00578 for(size_t j=0;j<getDOFs();j++) {
00579 size_t num = MAX(2,1 + (int)( dist /resolution ));
00580 num = MIN(1000,MAX(num,fabs(q_min[0]-q_max[0])/resolution));
00581 double new_res = (q_max[j] - q_min[j]) / (num-1);
00582
00583 for(size_t i=0;i<num;i++) {
00584 V_Configuration q(getDOFs());
00585 q[j] = q_min[j] + i*new_res;
00586
00587 geometry_msgs::Pose pose = predictPose( q );
00588 model.track.pose_resampled.push_back(pose);
00589 }
00590 }
00591 }
00592
00593 void GenericModel::keepLatestPoseOnly() {
00594 if(model.track.pose.size()<=1) return;
00595 model.track.pose.erase( model.track.pose.begin(), model.track.pose.begin() + (model.track.pose.size() -1) );
00596 for(size_t i = 0; i < model.track.channels.size(); i++) {
00597 model.track.channels[i].values.erase(
00598 model.track.channels[i].values.begin(),
00599 model.track.channels[i].values.begin() + (model.track.channels[i].values.size() -1)
00600 );
00601 }
00602 }
00603
00604 double GenericModel::getInlierLogLikelihood( size_t index ) {
00605 geometry_msgs::Pose &pose_obs = model.track.pose[index];
00606 V_Configuration q_estimated = predictConfiguration(pose_obs);
00607 for(size_t i=0;i<(size_t)q_estimated.size();i++) {
00608 model.track.channels[channelConfiguration[i]].values[index] = q_estimated[i];
00609 }
00610
00611 geometry_msgs::Pose pose_estimated = predictPose(q_estimated);
00612 model.track.pose_projected[index] = pose_estimated;
00613
00614 btTransform diff = poseToTransform(pose_obs).inverseTimes( poseToTransform(pose_estimated) );
00615 double err_position = diff.getOrigin().length();
00616 double err_orientation = fabs(diff.getRotation().getAngle());
00617
00618 double loglikelihood =
00619 - log(2*M_PI * sigma_position*sigma_orientation)
00620 - 0.5*(
00621 ( SQR(err_position) / (SQR(sigma_position)) ) +
00622 ( SQR(err_orientation) / SQR(sigma_orientation)) );
00623 return loglikelihood;
00624 }
00625
00626 double GenericModel::getOutlierLogLikelihood() {
00627
00628 double chi2inv = 1.96;
00629
00630
00631 double err_position = chi2inv * sigma_position;
00632 double err_orientation = chi2inv * sigma_orientation;
00633
00634 double loglikelihood =
00635 - log(2*M_PI * sigma_position*sigma_orientation)
00636 - 0.5*(
00637 ( SQR(err_position) / (SQR(sigma_position)) ) +
00638 ( SQR(err_orientation) / SQR(sigma_orientation)) );
00639
00640 return( loglikelihood );
00641 }
00642
00643 double GenericModel::getLogLikelihoodForPoseIndex(size_t index) {
00644
00645
00646 double inlierLikelihood = getInlierLogLikelihood(index);
00647 double outlierLikelihood = getOutlierLogLikelihood();
00648
00649
00650 model.track.channels[channelInlierLogLikelihood].values[index] = inlierLikelihood;
00651 double pi = (1-outlier_ratio) * exp( inlierLikelihood );
00652 double po = outlier_ratio * exp( outlierLikelihood );
00653 model.track.channels[channelOutlier].values[index] = po / (pi+po);
00654 double p = (log(exp(inlierLikelihood) + exp(outlierLikelihood)));
00655 model.track.channels[channelLogLikelihood].values[index] = p;
00656 return p;
00657
00658
00659
00660
00661
00662
00663
00664
00665 }
00666
00667 double GenericModel::getLogLikelihood(bool estimate_outlier_ratio) {
00668 model.track.pose_projected.resize(model.track.pose.size());
00669
00670 model.track.channels[channelInlierLogLikelihood].values.resize(model.track.pose.size());
00671 model.track.channels[channelOutlier].values.resize(model.track.pose.size());
00672 model.track.channels[channelLogLikelihood].values.resize(model.track.pose.size());
00673 for(size_t i=0;i<(size_t)getDOFs();i++) {
00674 model.track.channels[channelConfiguration[i]].values.resize(model.track.pose.size());
00675 }
00676
00677 double sum_likelihood = 0;
00678 size_t n = getSamples();
00679 if(estimate_outlier_ratio) {
00680 outlier_ratio =0.5;
00681 for(size_t i=0;i<n;i++) {
00682 model.track.channels[channelOutlier].values[i] = 0.5;
00683 }
00684
00685 int iter = 0;
00686 double diff = 0;
00687 do{
00688 sum_likelihood = 0;
00689 for(size_t i=0;i<n;i++) {
00690 sum_likelihood += getLogLikelihoodForPoseIndex(i);
00691 }
00692
00693 double outlier_ratio_new = 0;
00694 for(size_t i=0;i<n;i++) {
00695 outlier_ratio_new += model.track.channels[channelOutlier].values[i]/n;
00696 }
00697 diff = fabs(outlier_ratio - outlier_ratio_new);
00698 iter++;
00699 outlier_ratio = outlier_ratio_new;
00700
00701 } while( diff>0.01 && iter<10 );
00702 } else {
00703 for(size_t i=0;i<n;i++) {
00704 sum_likelihood += getLogLikelihoodForPoseIndex(i);
00705 }
00706 }
00707 sum_likelihood += - prior_outlier_ratio * outlier_ratio * n;
00708 return sum_likelihood;
00709 }
00710
00711 bool GenericModel::guessParameters() {
00712
00713 return false;
00714 }
00715
00716 bool GenericModel::sampleConsensus() {
00717
00718 writeParamsToModel();
00719 vector<articulation_msgs::ParamMsg> bestParams = model.params;
00720 double bestLikelihood = -DBL_MAX;
00721 bool goodGuess = false;
00722 for(size_t it=0;it<sac_iterations;it++) {
00723 if(!guessParameters()) continue;
00724 goodGuess = true;
00725 double likelihood = getLogLikelihood(true);
00726
00727 if(bestLikelihood < likelihood) {
00728 writeParamsToModel();
00729 bestParams = model.params;
00730 bestLikelihood = likelihood;
00731 }
00732 }
00733
00734 model.params = bestParams;
00735 readParamsFromModel();
00736
00737 return goodGuess;
00738 }
00739
00740 double
00741 my_f (const gsl_vector *v, void *params)
00742 {
00743 GenericModel *p = (GenericModel *)params;
00744
00745 vector<double> delta( v->size );
00746 for(size_t i=0;i<v->size;i++) {
00747 delta[i] = gsl_vector_get (v, i);
00748
00749 }
00750
00751 p->model.params = p->params_initial;
00752 p->readParamsFromModel();
00753 p->updateParameters(delta);
00754 p->writeParamsToModel();
00755 double likelihood = p->getLogLikelihood(true);
00756
00757 return -likelihood;
00758 }
00759
00760
00761 void
00762 my_df (const gsl_vector *v, void *params,
00763 gsl_vector *df)
00764 {
00765 double likelihood = my_f(v,params);
00766
00767 gsl_vector * v_delta =gsl_vector_alloc (v->size);
00768
00769 double DELTA = 1e-3;
00770 for(size_t i=0;i<v->size;i++) {
00771 v_delta = gsl_vector_alloc (v->size);
00772 gsl_vector_memcpy(v_delta,v);
00773 gsl_vector_set(v_delta, i,gsl_vector_get(v, i)+DELTA);
00774
00775 double likelihood_delta = my_f(v_delta,params);
00776 gsl_vector_set(df, i,(likelihood_delta-likelihood)/DELTA);
00777 }
00778 gsl_vector_free (v_delta);
00779 }
00780
00781
00782 void
00783 my_fdf (const gsl_vector *x, void *params,
00784 double *f, gsl_vector *df)
00785 {
00786 *f = my_f(x, params);
00787 my_df(x, params, df);
00788 }
00789
00790 bool GenericModel::optimizeParameters() {
00791
00792
00793
00794 writeParamsToModel();
00795 params_initial = model.params;
00796
00797
00798 size_t iter = 0;
00799 int status;
00800
00801 const gsl_multimin_fdfminimizer_type *T;
00802 gsl_multimin_fdfminimizer *s;
00803
00804 gsl_vector *x;
00805 gsl_multimin_function_fdf my_func;
00806
00807 my_func.n = (int)complexity;
00808 my_func.f = my_f;
00809 my_func.df = my_df;
00810 my_func.fdf = my_fdf;
00811 my_func.params = this;
00812
00813 x = gsl_vector_alloc ((int)complexity);
00814 gsl_vector_set_zero (x);
00815
00816
00817 T = gsl_multimin_fdfminimizer_vector_bfgs2;
00818 s = gsl_multimin_fdfminimizer_alloc (T, (int)complexity);
00819
00820 gsl_multimin_fdfminimizer_set (s, &my_func, x, 0.01, 0.1);
00821
00822
00823
00824
00825
00826
00827
00828
00829 if(optimizer_iterations>0) do
00830 {
00831 iter++;
00832
00833 status = gsl_multimin_fdfminimizer_iterate (s);
00834
00835 if (status)
00836 break;
00837
00838 status = gsl_multimin_test_gradient (s->gradient, 1e-1);
00839
00840
00841
00842
00843
00844
00845
00846
00847
00848 }
00849 while (status == GSL_CONTINUE && iter < optimizer_iterations);
00850
00851
00852
00853
00854
00855
00856
00857 gsl_multimin_fdfminimizer_free (s);
00858 gsl_vector_free (x);
00859
00860 if(!fitMinMaxConfigurations())
00861 return false;
00862
00863
00864
00865
00866
00867
00868
00869 return true;
00870 }
00871
00872
00873 void GenericModel::updateParameters(std::vector<double> delta) {
00874 }
00875
00876 bool GenericModel::normalizeParameters() {
00877 return true;
00878 }
00879
00880 }