00001 #include "joint_tracker/JointFilter.h"
00002
00003 #include "omip_common/OMIPUtils.h"
00004
00005 #include <tf/transform_listener.h>
00006
00007 using namespace omip;
00008
00009 JointFilter::JointFilter():
00010 _loop_period_ns(-1),
00011 _joint_id(-1),
00012 _likelihood_sample_num(-1),
00013 _externally_set_likelihood(false),
00014 _measurement_timestamp_ns(0),
00015 _unnormalized_model_probability(0),
00016 _rrb_id(-1),
00017 _inverted_delta_srb_pose_in_rrbf(false),
00018 _from_inverted_to_non_inverted(false),
00019 _from_non_inverted_to_inverted(false)
00020 {
00021 this->_initVariables(_joint_id);
00022 }
00023
00024 void JointFilter::setInitialMeasurement(const joint_measurement_t &initial_measurement,
00025 const Eigen::Twistd& rrb_pose_at_srb_birth_in_sf,
00026 const Eigen::Twistd& srb_pose_at_srb_birth_in_sf)
00027 {
00028
00029
00030 ROSTwist2EigenTwist(initial_measurement.first.pose_wc.twist, this->_rrb_current_pose_in_sf);
00031 this->_rrb_previous_pose_in_sf = this->_rrb_current_pose_in_sf;
00032
00033
00034
00035 ROSTwist2EigenTwist(initial_measurement.first.velocity_wc.twist, this->_rrb_current_vel_in_sf);
00036
00037
00038
00039 ROSTwist2EigenTwist(initial_measurement.second.pose_wc.twist, this->_srb_current_pose_in_sf);
00040
00041
00042
00043 ROSTwist2EigenTwist(initial_measurement.second.velocity_wc.twist, this->_srb_current_vel_in_sf);
00044
00045
00046 for (unsigned int i = 0; i < 6; i++)
00047 {
00048 for (unsigned int j = 0; j < 6; j++)
00049 {
00050 this->_rrb_pose_cov_in_sf(i, j) = initial_measurement.first.pose_wc.covariance[6 * i + j];
00051 this->_rrb_vel_cov_in_sf(i, j) = initial_measurement.first.velocity_wc.covariance[6 * i + j];
00052 this->_srb_pose_cov_in_sf(i, j) = initial_measurement.second.pose_wc.covariance[6 * i + j];
00053 }
00054 }
00055
00056
00057
00058 Eigen::Displacementd T_sf_srbf_t0 = srb_pose_at_srb_birth_in_sf.exp(1e-20);
00059 Eigen::Displacementd T_sf_rrbf_t0 = rrb_pose_at_srb_birth_in_sf.exp(1e-20);
00060 Eigen::Displacementd T_rrbf_sf_t0 = T_sf_rrbf_t0.inverse();
00061 Eigen::Displacementd T_rrbf_srbf_t0 = T_rrbf_sf_t0 * T_sf_srbf_t0;
00062 this->_srb_initial_pose_in_rrbf = T_rrbf_srbf_t0.log(1.0e-20);
00063
00064
00065
00066 Eigen::Matrix<double, 6, 6> rrb_pose_cov_in_rrbf;
00067 adjointXcovXadjointT(T_rrbf_sf_t0, _rrb_pose_cov_in_sf, rrb_pose_cov_in_rrbf);
00068 Eigen::Matrix<double, 6, 6> srb_pose_cov_in_rrbf;
00069 adjointXcovXadjointT(T_rrbf_sf_t0, _srb_pose_cov_in_sf, srb_pose_cov_in_rrbf);
00070 this->_srb_initial_pose_cov_in_rrbf = rrb_pose_cov_in_rrbf + srb_pose_cov_in_rrbf;
00071
00072
00073
00074 Eigen::Displacementd T_sf_rrbf_t = this->_rrb_current_pose_in_sf.exp(1e-20);
00075 Eigen::Displacementd T_sf_srbf_t = this->_srb_current_pose_in_sf.exp(1e-20);
00076 Eigen::Displacementd T_rrbf_sf_t = T_sf_rrbf_t.inverse();
00077 Eigen::Displacementd T_rrbf_srbf_t = T_rrbf_sf_t * T_sf_srbf_t;
00078 this->_srb_current_pose_in_rrbf = T_rrbf_srbf_t.log(1.0e-20);
00079 this->_srb_previous_pose_in_rrbf = this->_srb_current_pose_in_rrbf;
00080
00081
00082 this->_current_delta_pose_in_rrbf = (_srb_current_pose_in_rrbf.exp(1e-12)*(_srb_initial_pose_in_rrbf.exp(1e-12).inverse())).log(1e-12);
00083
00084 this->_previous_delta_pose_in_rrbf = this->_current_delta_pose_in_rrbf;
00085 this->_delta_poses_in_rrbf.push_back(this->_current_delta_pose_in_rrbf);
00086
00087 _rrb_id = initial_measurement.first.rb_id;
00088
00089 if(_rrb_id == 0)
00090 {
00091 this->_rrb_centroid_in_sf = Eigen::Vector3d(0.,0.,0.);
00092 }
00093 else
00094 {
00095 this->_rrb_centroid_in_sf = Eigen::Vector3d( initial_measurement.first.centroid.x,
00096 initial_measurement.first.centroid.y,
00097 initial_measurement.first.centroid.z);
00098 }
00099
00100
00101 this->_srb_centroid_in_sf = Eigen::Vector3d( initial_measurement.second.centroid.x,
00102 initial_measurement.second.centroid.y,
00103 initial_measurement.second.centroid.z);
00104 }
00105
00106 void JointFilter::_initVariables(JointCombinedFilterId joint_id)
00107 {
00108 this->_joint_id = joint_id;
00109
00110 this->_joint_state = 0;
00111 this->_joint_velocity = 0;
00112
00113 this->_measurements_likelihood = 0.;
00114 this->_model_prior_probability = 1.0/4.0;
00115 this->_joint_position = Eigen::Vector3d(0,0,0);
00116 this->_uncertainty_joint_position = Eigen::Matrix3d::Identity();
00117 this->_joint_orientation_phi = 0;
00118 this->_joint_orientation_theta = 0;
00119 this->_joint_orientation = Eigen::Vector3d(1,0,0);
00120 this->_uncertainty_joint_orientation_phitheta = Eigen::Matrix2d::Identity();
00121
00122 this->_rrb_current_pose_in_sf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00123 this->_rrb_previous_pose_in_sf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00124 this->_rrb_current_vel_in_sf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00125 this->_srb_current_pose_in_sf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00126 this->_srb_current_vel_in_sf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00127
00128 this->_srb_initial_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00129 this->_srb_current_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00130 this->_srb_previous_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00131 this->_srb_predicted_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00132 this->_srb_previous_predicted_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00133 this->_current_delta_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00134 this->_previous_delta_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00135 this->_predicted_delta_pose_in_rrbf = Eigen::Twistd(0.,0.,0.,0.,0.,0.);
00136
00137 this->_rrb_pose_cov_in_sf = Eigen::Matrix<double, 6, 6>::Zero();
00138 this->_rrb_vel_cov_in_sf = Eigen::Matrix<double, 6, 6>::Zero();
00139 this->_srb_pose_cov_in_sf = Eigen::Matrix<double, 6, 6>::Zero();
00140 this->_srb_pose_cov_in_sf = Eigen::Matrix<double, 6, 6>::Zero();
00141
00142 this->_normalizing_term = 1;
00143 }
00144
00145 JointFilter::~JointFilter()
00146 {
00147
00148 }
00149
00150 JointFilter::JointFilter(const JointFilter &joint)
00151 {
00152 this->_joint_id = joint._joint_id;
00153 this->_measurements_likelihood = joint._measurements_likelihood;
00154 this->_model_prior_probability = joint._model_prior_probability;
00155 this->_rrb_centroid_in_sf = joint._rrb_centroid_in_sf;
00156 this->_srb_centroid_in_sf = joint._srb_centroid_in_sf;
00157 this->_joint_position = joint._joint_position;
00158 this->_uncertainty_joint_position = joint._uncertainty_joint_position;
00159 this->_joint_orientation_phi = joint._joint_orientation_phi;
00160 this->_joint_orientation_theta = joint._joint_orientation_theta;
00161 this->_joint_orientation = joint._joint_orientation;
00162 this->_uncertainty_joint_orientation_phitheta = joint._uncertainty_joint_orientation_phitheta;
00163
00164 this->_rrb_current_pose_in_sf = joint._rrb_current_pose_in_sf;
00165 this->_rrb_previous_pose_in_sf = joint._rrb_previous_pose_in_sf;
00166 this->_srb_current_pose_in_sf = joint._srb_current_pose_in_sf;
00167 this->_rrb_current_vel_in_sf= joint._rrb_current_vel_in_sf;
00168 this->_srb_current_vel_in_sf = joint._srb_current_vel_in_sf;
00169
00170 this->_srb_initial_pose_in_rrbf = joint._srb_initial_pose_in_rrbf;
00171 this->_srb_current_pose_in_rrbf= joint._srb_current_pose_in_rrbf;
00172 this->_srb_previous_pose_in_rrbf = joint._srb_previous_pose_in_rrbf;
00173 this->_srb_predicted_pose_in_rrbf = joint._srb_predicted_pose_in_rrbf;
00174
00175 this->_srb_previous_predicted_pose_in_rrbf = joint._srb_previous_predicted_pose_in_rrbf;
00176
00177 this->_current_delta_pose_in_rrbf = joint._current_delta_pose_in_rrbf;
00178 this->_previous_delta_pose_in_rrbf = joint._previous_delta_pose_in_rrbf;
00179 this->_predicted_delta_pose_in_rrbf = joint._predicted_delta_pose_in_rrbf;
00180
00181 this->_delta_poses_in_rrbf = joint._delta_poses_in_rrbf;
00182
00183 this->_rrb_pose_cov_in_sf = joint._rrb_pose_cov_in_sf;
00184 this->_srb_pose_cov_in_sf = joint._srb_pose_cov_in_sf;
00185
00186 this->_loop_period_ns = joint._loop_period_ns;
00187 }
00188
00189 void JointFilter::setMeasurement(joint_measurement_t acquired_measurement, const double &measurement_timestamp_ns)
00190 {
00191 _measurement_timestamp_ns = measurement_timestamp_ns;
00192
00193
00194 _rrb_id = acquired_measurement.first.rb_id;
00195 int srb_id = acquired_measurement.second.rb_id;
00196
00197
00198 this->_rrb_previous_pose_in_sf = this->_rrb_current_pose_in_sf;
00199
00200 ROSTwist2EigenTwist(acquired_measurement.first.pose_wc.twist,this->_rrb_current_pose_in_sf);
00201
00202 ROSTwist2EigenTwist(acquired_measurement.first.velocity_wc.twist, this->_rrb_current_vel_in_sf);
00203
00204 ROSTwist2EigenTwist(acquired_measurement.second.pose_wc.twist, this->_srb_current_pose_in_sf);
00205
00206 ROSTwist2EigenTwist(acquired_measurement.second.velocity_wc.twist, this->_srb_current_vel_in_sf);
00207
00208 for (unsigned int i = 0; i < 6; i++)
00209 {
00210 for (unsigned int j = 0; j < 6; j++)
00211 {
00212 this->_rrb_pose_cov_in_sf(i, j) = acquired_measurement.first.pose_wc.covariance[6 * i + j];
00213 this->_rrb_vel_cov_in_sf(i, j) = acquired_measurement.first.velocity_wc.covariance[6 * i + j];
00214 this->_srb_pose_cov_in_sf(i, j) = acquired_measurement.second.pose_wc.covariance[6 * i + j];
00215 }
00216 }
00217
00218 Eigen::Displacementd T_sf_rrbf = this->_rrb_current_pose_in_sf.exp(1.0e-20);
00219 Eigen::Displacementd T_sf_srbf = this->_srb_current_pose_in_sf.exp(1.0e-20);
00220 Eigen::Displacementd T_rrbf_sf = T_sf_rrbf.inverse();
00221 Eigen::Displacementd T_rrbf_srbf = T_rrbf_sf * T_sf_srbf;
00222 this->_srb_current_pose_in_rrbf = T_rrbf_srbf.log(1e-20);
00223
00224
00225
00226 bool inverted = false;
00227 this->_srb_current_pose_in_rrbf = invertTwist(this->_srb_current_pose_in_rrbf, this->_srb_previous_pose_in_rrbf, inverted);
00228
00229
00230 this->_srb_previous_pose_in_rrbf = this->_srb_current_pose_in_rrbf;
00231
00232
00233
00234 Eigen::Matrix<double, 6, 6> rrb_pose_cov_in_rrbf;
00235 adjointXcovXadjointT(T_rrbf_sf, _rrb_pose_cov_in_sf, rrb_pose_cov_in_rrbf);
00236 Eigen::Matrix<double, 6, 6> srb_pose_cov_in_rrbf;
00237 adjointXcovXadjointT(T_rrbf_sf, _srb_pose_cov_in_sf, srb_pose_cov_in_rrbf);
00238 this->_srb_current_pose_cov_in_rrbf = rrb_pose_cov_in_rrbf + srb_pose_cov_in_rrbf;
00239
00240 Eigen::Displacementd T_rrbf_srbf_t0 = this->_srb_initial_pose_in_rrbf.exp(1.0e-20);
00241 Eigen::Displacementd delta_displ = T_rrbf_srbf * (T_rrbf_srbf_t0.inverse());
00242
00243 this->_current_delta_pose_in_rrbf = delta_displ.log(1.0e-20);
00244
00245
00246
00247 bool inverted_before = _inverted_delta_srb_pose_in_rrbf;
00248 this->_current_delta_pose_in_rrbf = invertTwist(this->_current_delta_pose_in_rrbf, this->_previous_delta_pose_in_rrbf, this->_inverted_delta_srb_pose_in_rrbf);
00249 this->_previous_delta_pose_in_rrbf = this->_current_delta_pose_in_rrbf;
00250
00251 _from_inverted_to_non_inverted = false;
00252 _from_non_inverted_to_inverted = false;
00253 if(inverted_before != _inverted_delta_srb_pose_in_rrbf)
00254 {
00255 if(inverted_before && !_inverted_delta_srb_pose_in_rrbf)
00256 {
00257 _from_inverted_to_non_inverted = true;
00258 }else{
00259 _from_non_inverted_to_inverted = true;
00260 }
00261 }
00262
00263 this->_delta_poses_in_rrbf.push_back(this->_current_delta_pose_in_rrbf);
00264
00265
00266
00267 Eigen::Matrix<double, 6, 6> transformed_cov;
00268 adjointXinvAdjointXcovXinvAdjointTXadjointT(T_rrbf_srbf, T_rrbf_srbf_t0, _srb_initial_pose_cov_in_rrbf, transformed_cov);
00269 this->_current_delta_pose_cov_in_rrbf = _srb_current_pose_cov_in_rrbf + transformed_cov;
00270
00271
00272 if(_rrb_id == 0)
00273 {
00274 this->_rrb_centroid_in_sf = Eigen::Vector3d(0.,0.,0.);
00275 }
00276 else
00277 {
00278 this->_rrb_centroid_in_sf = Eigen::Vector3d( acquired_measurement.first.centroid.x,
00279 acquired_measurement.first.centroid.y,
00280 acquired_measurement.first.centroid.z);
00281 }
00282
00283
00284 this->_srb_centroid_in_sf = Eigen::Vector3d( acquired_measurement.second.centroid.x,
00285 acquired_measurement.second.centroid.y,
00286 acquired_measurement.second.centroid.z);
00287 }
00288
00289 Eigen::Twistd JointFilter::getPredictedMeasurement() const
00290 {
00291 return this->_srb_predicted_pose_in_rrbf;
00292 }
00293
00294 void JointFilter::setLoopPeriodNS(double loop_period_ns)
00295 {
00296 this->_loop_period_ns = loop_period_ns;
00297 }
00298
00299 double JointFilter::getLoopPeriodNS() const
00300 {
00301 return this->_loop_period_ns;
00302 }
00303
00304 void JointFilter::setNumSamplesForLikelihoodEstimation(int likelihood_sample_num)
00305 {
00306 this->_likelihood_sample_num = likelihood_sample_num;
00307 }
00308
00309 int JointFilter::getNumSamplesForLikelihoodEstimation() const
00310 {
00311 return this->_likelihood_sample_num;
00312 }
00313
00314 void JointFilter::setNormalizingTerm(double normalizing_term)
00315 {
00316 this->_normalizing_term = normalizing_term;
00317 }
00318
00319 double JointFilter::getNormalizingTerm()
00320 {
00321 return this->_normalizing_term;
00322 }
00323
00324 void JointFilter::setModelPriorProbability(double model_prior_probability)
00325 {
00326 this->_model_prior_probability = model_prior_probability;
00327 }
00328
00329 double JointFilter::getModelPriorProbability() const
00330 {
00331 return this->_model_prior_probability;
00332 }
00333
00334 double JointFilter::getLikelihoodOfLastMeasurements() const
00335 {
00336 return this->_measurements_likelihood;
00337 }
00338
00339 void JointFilter::setLikelihoodOfLastMeasurements(double likelihood)
00340 {
00341 this->_measurements_likelihood = likelihood;
00342 this->_externally_set_likelihood = true;
00343 }
00344
00345 double JointFilter::getUnnormalizedProbabilityOfJointFilter() const
00346 {
00347 return _unnormalized_model_probability;
00348 }
00349
00350 double JointFilter::getProbabilityOfJointFilter() const
00351 {
00352 return (_unnormalized_model_probability / this->_normalizing_term);
00353 }
00354
00355 void JointFilter::setJointId(JointCombinedFilterId joint_id)
00356 {
00357 this->_joint_id = joint_id;
00358 }
00359
00360 JointCombinedFilterId JointFilter::getJointId() const
00361 {
00362 return this->_joint_id;
00363 }
00364
00365 double JointFilter::getJointState() const
00366 {
00367 return _joint_state;
00368 }
00369
00370 double JointFilter::getJointVelocity() const
00371 {
00372 return _joint_velocity;
00373 }
00374
00375 double JointFilter::getOrientationPhiInRRBFrame() const
00376 {
00377 return this->_joint_orientation_phi;
00378 }
00379
00380 double JointFilter::getOrientationThetaInRRBFrame() const
00381 {
00382 return this->_joint_orientation_theta;
00383 }
00384
00385 double JointFilter::getCovarianceOrientationPhiPhiInRRBFrame() const
00386 {
00387 return this->_uncertainty_joint_orientation_phitheta(0,0);
00388 }
00389
00390 double JointFilter::getCovarianceOrientationThetaThetaInRRBFrame() const
00391 {
00392 return this->_uncertainty_joint_orientation_phitheta(1,1);
00393 }
00394
00395 double JointFilter::getCovarianceOrientationPhiThetaInRRBFrame() const
00396 {
00397 return this->_uncertainty_joint_orientation_phitheta(1,0);
00398 }
00399
00400 Eigen::Vector3d JointFilter::getJointPositionInRRBFrame() const
00401 {
00402 return this->_joint_position;
00403 }
00404
00405 Eigen::Vector3d JointFilter::getJointOrientationRPYInRRBFrame() const
00406 {
00407 double phi = std::acos(this->_joint_orientation.z() / this->_joint_orientation.norm());
00408 double theta = std::atan(this->_joint_orientation.y() / this->_joint_orientation.x());
00409 Eigen::Vector3d joint_ori_rpy(0., theta, phi);
00410
00411 return joint_ori_rpy;
00412 }
00413
00414 Eigen::Vector3d JointFilter::getJointOrientationUnitaryVector() const
00415 {
00416 return this->_joint_orientation;
00417 }
00418
00419 void JointFilter::setCovariancePrior(double prior_cov_vel)
00420 {
00421 this->_prior_cov_vel = prior_cov_vel;
00422 }
00423
00424 void JointFilter::setCovarianceAdditiveSystemNoisePhi(double sigma_sys_noise_phi)
00425 {
00426 this->_sigma_sys_noise_phi = sigma_sys_noise_phi;
00427 }
00428
00429 void JointFilter::setCovarianceAdditiveSystemNoiseTheta(double sigma_sys_noise_theta)
00430 {
00431 this->_sigma_sys_noise_theta = sigma_sys_noise_theta;
00432 }
00433
00434 void JointFilter::setCovarianceAdditiveSystemNoisePx(double sigma_sys_noise_px)
00435 {
00436 this->_sigma_sys_noise_px = sigma_sys_noise_px;
00437 }
00438
00439 void JointFilter::setCovarianceAdditiveSystemNoisePy(double sigma_sys_noise_py)
00440 {
00441 this->_sigma_sys_noise_py = sigma_sys_noise_py;
00442 }
00443
00444 void JointFilter::setCovarianceAdditiveSystemNoisePz(double sigma_sys_noise_pz)
00445 {
00446 this->_sigma_sys_noise_pz = sigma_sys_noise_pz;
00447 }
00448
00449 void JointFilter::setCovarianceAdditiveSystemNoiseJointState(double sigma_sys_noise_pv)
00450 {
00451 this->_sigma_sys_noise_jv = sigma_sys_noise_pv;
00452 }
00453
00454 void JointFilter::setCovarianceAdditiveSystemNoiseJointVelocity(double sigma_sys_noise_pvd)
00455 {
00456 this->_sigma_sys_noise_jvd = sigma_sys_noise_pvd;
00457 }
00458
00459 void JointFilter::setCovarianceMeasurementNoise(double sigma_meas_noise)
00460 {
00461 this->_sigma_meas_noise = sigma_meas_noise;
00462 }
00463
00464 void JointFilter::initialize()
00465 {
00466 #ifdef PUBLISH_PREDICTED_POSE_AS_PWC
00467
00468 std::ostringstream oss_pwc_topic;
00469 oss_pwc_topic << "/joint_tracker/predpose_srb_j" << this->_joint_id << "_" << this->getJointFilterTypeStr();
00470 _predicted_next_pose_publisher = _predicted_next_pose_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(oss_pwc_topic.str(), 100);
00471 #endif
00472 }