00001 #include "joint_tracker/RevoluteJointFilter.h"
00002
00003 #include "omip_common/OMIPUtils.h"
00004
00005 #include <Eigen/Geometry>
00006
00007 #include <boost/math/distributions/chi_squared.hpp>
00008
00009 #include "geometry_msgs/PoseWithCovarianceStamped.h"
00010
00011 #include "std_msgs/Float64MultiArray.h"
00012
00013 using namespace omip;
00014 using namespace MatrixWrapper;
00015 using namespace BFL;
00016
00017
00018 #define REV_STATE_DIM 7
00019 #define MEAS_DIM 6
00020
00043 using namespace omip;
00044
00045 RevoluteJointFilter::RevoluteJointFilter() :
00046 JointFilter(),
00047 _sys_PDF(NULL),
00048 _sys_MODEL(NULL),
00049 _meas_PDF(NULL),
00050 _meas_MODEL(NULL),
00051 _ekf(NULL),
00052 _sigma_delta_meas_uncertainty_angular(-1),
00053 _accumulated_rotation(0.0)
00054 {
00055 }
00056
00057 void RevoluteJointFilter::setCovarianceDeltaMeasurementAngular(double sigma_delta_meas_uncertainty_angular)
00058 {
00059 this->_sigma_delta_meas_uncertainty_angular = sigma_delta_meas_uncertainty_angular;
00060 }
00061
00062 void RevoluteJointFilter::initialize()
00063 {
00064 JointFilter::initialize();
00065 this->_joint_orientation = Eigen::Vector3d( this->_current_delta_pose_in_rrbf.rx(),
00066 this->_current_delta_pose_in_rrbf.ry(),
00067 this->_current_delta_pose_in_rrbf.rz());
00068 this->_joint_state = this->_joint_orientation.norm();
00069
00070
00071
00072
00073 this->_joint_velocity = 0.0;
00074
00075 this->_joint_states_all.push_back(this->_joint_state);
00076
00077 Eigen::Vector3d linear_part = Eigen::Vector3d( this->_current_delta_pose_in_rrbf.vx(),
00078 this->_current_delta_pose_in_rrbf.vy(),
00079 this->_current_delta_pose_in_rrbf.vz());
00080
00081
00082
00083
00084
00085
00086
00087 this->_joint_position = (1.0 / (pow(this->_joint_state, 2))) * this->_joint_orientation.cross(linear_part);
00088 this->_joint_orientation.normalize();
00089
00090 this->_initializeSystemModel();
00091 this->_initializeMeasurementModel();
00092 this->_initializeEKF();
00093 }
00094
00095 void RevoluteJointFilter::setMinRotationRevolute(const double& value)
00096 {
00097 _rev_min_rot_for_ee = value;
00098 }
00099
00100 void RevoluteJointFilter::setMaxRadiusDistanceRevolute(const double& value)
00101 {
00102 _rev_max_joint_distance_for_ee = value;
00103 }
00104
00105 void RevoluteJointFilter::_initializeSystemModel()
00106 {
00107
00108 Matrix A(REV_STATE_DIM, REV_STATE_DIM);
00109 A = 0.;
00110 for (unsigned int i = 1; i <= REV_STATE_DIM; i++)
00111 {
00112 A(i, i) = 1.0;
00113 }
00114 A(6, 7) = this->_loop_period_ns/1e9;
00115
00116 ColumnVector sys_noise_MU(REV_STATE_DIM);
00117 sys_noise_MU = 0;
00118
00119 SymmetricMatrix sys_noise_COV(REV_STATE_DIM);
00120 sys_noise_COV = 0.0;
00121 sys_noise_COV(1, 1) = this->_sigma_sys_noise_phi* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
00122 sys_noise_COV(2, 2) = this->_sigma_sys_noise_theta* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
00123 sys_noise_COV(3, 3) = this->_sigma_sys_noise_px* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
00124 sys_noise_COV(4, 4) = this->_sigma_sys_noise_py* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
00125 sys_noise_COV(5, 5) = this->_sigma_sys_noise_pz* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
00126 sys_noise_COV(6, 6) = this->_sigma_sys_noise_jv* (std::pow((this->_loop_period_ns/1e9),3) / 3.0);
00127 sys_noise_COV(7, 7) = this->_sigma_sys_noise_jvd*(this->_loop_period_ns/1e9);
00128
00129
00130 Gaussian system_uncertainty_PDF(sys_noise_MU, sys_noise_COV);
00131 this->_sys_PDF = new LinearAnalyticConditionalGaussian( A, system_uncertainty_PDF);
00132 this->_sys_MODEL = new LinearAnalyticSystemModelGaussianUncertainty( this->_sys_PDF);
00133 }
00134
00135 void RevoluteJointFilter::_initializeMeasurementModel()
00136 {
00137
00138 ColumnVector meas_noise_MU(MEAS_DIM);
00139 meas_noise_MU = 0.0;
00140 SymmetricMatrix meas_noise_COV(MEAS_DIM);
00141 meas_noise_COV = 0.0;
00142 for (unsigned int i = 1; i <= MEAS_DIM; i++)
00143 meas_noise_COV(i, i) = this->_sigma_meas_noise;
00144
00145 Gaussian meas_uncertainty_PDF(meas_noise_MU, meas_noise_COV);
00146
00147 this->_meas_PDF = new NonLinearRevoluteMeasurementPdf(meas_uncertainty_PDF);
00148 this->_meas_MODEL = new AnalyticMeasurementModelGaussianUncertainty(this->_meas_PDF);
00149 }
00150
00151 void RevoluteJointFilter::_initializeEKF()
00152 {
00153 ColumnVector prior_MU(REV_STATE_DIM);
00154 prior_MU = 0.0;
00155
00156 SymmetricMatrix prior_COV(REV_STATE_DIM);
00157 prior_COV = 0.0;
00158
00159
00160
00161 if(this->_joint_orientation.x() == 0.0)
00162 {
00163 this->_joint_orientation.x() = 1e-6;
00164 }
00165
00166 prior_MU(1) = atan2( this->_joint_orientation.y() , this->_joint_orientation.x());
00167 prior_MU(2) = acos(this->_joint_orientation.z());
00168 prior_MU(3) = this->_joint_position.x();
00169 prior_MU(4) = this->_joint_position.y();
00170 prior_MU(5) = this->_joint_position.z();
00171 prior_MU(6) = this->_joint_state;
00172 prior_MU(7) = this->_joint_velocity;
00173
00174
00175 for (int i = 1; i <= REV_STATE_DIM; i++)
00176 {
00177 prior_COV(i, i) = _prior_cov_vel;
00178 }
00179
00180 Gaussian prior_PDF(prior_MU, prior_COV);
00181
00182 this->_ekf = new ExtendedKalmanFilter(&prior_PDF);
00183 }
00184
00185 RevoluteJointFilter::~RevoluteJointFilter()
00186 {
00187 if (this->_sys_PDF)
00188 {
00189 delete this->_sys_PDF;
00190 this->_sys_PDF = NULL;
00191 }
00192 if (this->_sys_MODEL)
00193 {
00194 delete this->_sys_MODEL;
00195 this->_sys_MODEL = NULL;
00196 }
00197 if (this->_meas_PDF)
00198 {
00199 delete this->_meas_PDF;
00200 this->_meas_PDF = NULL;
00201 }
00202 if (this->_meas_MODEL)
00203 {
00204 delete this->_meas_MODEL;
00205 this->_meas_MODEL = NULL;
00206 }
00207 if (this->_ekf)
00208 {
00209 delete this->_ekf;
00210 this->_ekf = NULL;
00211 }
00212 }
00213
00214 RevoluteJointFilter::RevoluteJointFilter(const RevoluteJointFilter &rev_joint) :
00215 JointFilter(rev_joint)
00216 {
00217 this->_sigma_delta_meas_uncertainty_angular = rev_joint._sigma_delta_meas_uncertainty_angular;
00218 this->_sys_PDF = new LinearAnalyticConditionalGaussian(*(rev_joint._sys_PDF));
00219 this->_sys_MODEL = new LinearAnalyticSystemModelGaussianUncertainty( *(rev_joint._sys_MODEL));
00220 this->_meas_PDF = new NonLinearRevoluteMeasurementPdf(*(rev_joint._meas_PDF));
00221 this->_meas_MODEL = new AnalyticMeasurementModelGaussianUncertainty( *(rev_joint._meas_MODEL));
00222 this->_ekf = new ExtendedKalmanFilter(*(rev_joint._ekf));
00223 }
00224
00225 void RevoluteJointFilter::predictState(double time_interval_ns)
00226 {
00227
00228 SymmetricMatrix sys_noise_COV(REV_STATE_DIM);
00229 sys_noise_COV = 0.0;
00230 sys_noise_COV(1, 1) = this->_sigma_sys_noise_phi* (std::pow((time_interval_ns/1e9),3) / 3.0);
00231 sys_noise_COV(2, 2) = this->_sigma_sys_noise_theta* (std::pow((time_interval_ns/1e9),3) / 3.0);
00232 sys_noise_COV(3, 3) = this->_sigma_sys_noise_px* (std::pow((time_interval_ns/1e9),3) / 3.0);
00233 sys_noise_COV(4, 4) = this->_sigma_sys_noise_py* (std::pow((time_interval_ns/1e9),3) / 3.0);
00234 sys_noise_COV(5, 5) = this->_sigma_sys_noise_pz* (std::pow((time_interval_ns/1e9),3) / 3.0);
00235 sys_noise_COV(6, 6) = this->_sigma_sys_noise_jv* (std::pow((time_interval_ns/1e9),3) / 3.0);
00236 sys_noise_COV(7, 7) = this->_sigma_sys_noise_jvd*(time_interval_ns/1e9);
00237
00238
00239
00240 Matrix A(REV_STATE_DIM, REV_STATE_DIM);
00241 A = 0.;
00242 for (unsigned int i = 1; i <= REV_STATE_DIM; i++)
00243 {
00244 A(i, i) = 1.0;
00245 }
00246 A(6, 7) = time_interval_ns/1e9;;
00247
00248 this->_sys_PDF->MatrixSet(0, A);
00249 this->_sys_PDF->AdditiveNoiseSigmaSet(sys_noise_COV);
00250
00251 this->_ekf->Update(this->_sys_MODEL);
00252 }
00253
00254 void RevoluteJointFilter::predictMeasurement()
00255 {
00256 ColumnVector empty;
00257 ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00258
00259 ColumnVector predicted_delta_pose_in_rrbf = this->_meas_MODEL->PredictionGet(empty, state_updated_state);
00260
00261 this->_predicted_delta_pose_in_rrbf = Eigen::Twistd( predicted_delta_pose_in_rrbf(4), predicted_delta_pose_in_rrbf(5),
00262 predicted_delta_pose_in_rrbf(6), predicted_delta_pose_in_rrbf(1),
00263 predicted_delta_pose_in_rrbf(2), predicted_delta_pose_in_rrbf(3));
00264
00265 Eigen::Displacementd predicted_delta = this->_predicted_delta_pose_in_rrbf.exp(1e-20);
00266 Eigen::Displacementd T_rrbf_srbf_t0 = this->_srb_initial_pose_in_rrbf.exp(1.0e-20);
00267 Eigen::Displacementd T_rrbf_srbf_t_next = predicted_delta * T_rrbf_srbf_t0;
00268
00269 this->_srb_predicted_pose_in_rrbf = T_rrbf_srbf_t_next.log(1.0e-20);
00270
00271 bool change = false;
00272 this->_srb_predicted_pose_in_rrbf = unwrapTwist(this->_srb_predicted_pose_in_rrbf, T_rrbf_srbf_t_next, this->_srb_previous_predicted_pose_in_rrbf, change);
00273
00274 this->_srb_previous_predicted_pose_in_rrbf = this->_srb_predicted_pose_in_rrbf;
00275 }
00276
00277 void RevoluteJointFilter::correctState()
00278 {
00279 ColumnVector updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00280 ColumnVector rb2_measured_delta_relative_pose_cv(MEAS_DIM);
00281 rb2_measured_delta_relative_pose_cv = 0.;
00282 rb2_measured_delta_relative_pose_cv(1) = this->_current_delta_pose_in_rrbf.vx();
00283 rb2_measured_delta_relative_pose_cv(2) = this->_current_delta_pose_in_rrbf.vy();
00284 rb2_measured_delta_relative_pose_cv(3) = this->_current_delta_pose_in_rrbf.vz();
00285 rb2_measured_delta_relative_pose_cv(4) = this->_current_delta_pose_in_rrbf.rx();
00286 rb2_measured_delta_relative_pose_cv(5) = this->_current_delta_pose_in_rrbf.ry();
00287 rb2_measured_delta_relative_pose_cv(6) = this->_current_delta_pose_in_rrbf.rz();
00288
00289
00290
00291
00292
00293 Eigen::Vector3d angular_component(this->_current_delta_pose_in_rrbf.rx(), this->_current_delta_pose_in_rrbf.ry(), this->_current_delta_pose_in_rrbf.rz());
00294 double meas_uncertainty_factor = 1 / (1.0 - exp(-sin(angular_component.norm()/2.0)/this->_sigma_delta_meas_uncertainty_angular));
00295
00296
00297 meas_uncertainty_factor = std::min(meas_uncertainty_factor, 1e6);
00298
00299 SymmetricMatrix current_delta_pose_cov_in_rrbf(6);
00300 for (unsigned int i = 0; i < 6; i++)
00301 {
00302 for (unsigned int j = 0; j < 6; j++)
00303 {
00304 current_delta_pose_cov_in_rrbf(i + 1, j + 1) = _current_delta_pose_cov_in_rrbf(i, j);
00305 }
00306 }
00307
00308 this->_meas_PDF->AdditiveNoiseSigmaSet(current_delta_pose_cov_in_rrbf * meas_uncertainty_factor );
00309
00310 this->_ekf->Update(this->_meas_MODEL, rb2_measured_delta_relative_pose_cv);
00311
00312 updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00313
00314 this->_joint_orientation(0) = sin(updated_state(2)) * cos(updated_state(1));
00315 this->_joint_orientation(1) = sin(updated_state(2)) * sin(updated_state(1));
00316 this->_joint_orientation(2) = cos(updated_state(2));
00317
00318 SymmetricMatrix updated_uncertainty = this->_ekf->PostGet()->CovarianceGet();
00319
00320 for(int i=0; i<3; i++)
00321 {
00322 this->_joint_position(i) = updated_state(i+3);
00323 for(int j=0; j<3; j++)
00324 {
00325 this->_uncertainty_joint_position(i,j) = updated_uncertainty(i+3, j+3);
00326 }
00327 }
00328
00329 double joint_state_before = _joint_state;
00330
00331
00332 if(_from_inverted_to_non_inverted)
00333 {
00334 _accumulated_rotation = std::round(joint_state_before/(2*M_PI))*2*M_PI;
00335 }
00336
00337
00338 if(_from_non_inverted_to_inverted)
00339 {
00340 _accumulated_rotation = std::round(joint_state_before/(M_PI))*2*M_PI;
00341 }
00342
00343 this->_joint_state = updated_state(6);
00344
00345
00346 if(_inverted_delta_srb_pose_in_rrbf)
00347 {
00348 _joint_state = _accumulated_rotation - _joint_state;
00349 }else{
00350 _joint_state = _accumulated_rotation + _joint_state;
00351 }
00352
00353
00354 this->_uncertainty_joint_state = updated_uncertainty(6,6);
00355 this->_joint_velocity = updated_state(7);
00356 this->_uncertainty_joint_velocity = updated_uncertainty(7,7);
00357
00358 this->_joint_orientation_phi = updated_state(1);
00359 this->_joint_orientation_theta = updated_state(2);
00360 this->_uncertainty_joint_orientation_phitheta(0,0) = updated_uncertainty(1, 1);
00361 this->_uncertainty_joint_orientation_phitheta(0,1) = updated_uncertainty(1, 2);
00362 this->_uncertainty_joint_orientation_phitheta(1,0) = updated_uncertainty(1, 2);
00363 this->_uncertainty_joint_orientation_phitheta(1,1) = updated_uncertainty(2, 2);
00364 }
00365
00366 void RevoluteJointFilter::estimateMeasurementHistoryLikelihood()
00367 {
00368 double accumulated_error = 0.;
00369
00370 double p_one_meas_given_model_params = 0;
00371 double p_all_meas_given_model_params = 0;
00372
00373 double sigma_translation = 0.05;
00374 double sigma_rotation = 0.2;
00375
00376 ColumnVector updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00377 double phi = updated_state(1);
00378 double theta = updated_state(2);
00379 double sp = sin(phi);
00380 double cp = cos(phi);
00381 double st = sin(theta);
00382 double ct = cos(theta);
00383
00384 double px = updated_state(3);
00385 double py = updated_state(4);
00386 double pz = updated_state(5);
00387
00388 this->_joint_states_all.push_back(updated_state(6));
00389
00390 Eigen::Vector3d rev_joint_rotation_unitary = Eigen::Vector3d(cp * st, sp * st, ct);
00391 rev_joint_rotation_unitary.normalize();
00392
00393
00394 Eigen::Vector3d rev_joint_position = Eigen::Vector3d(px, py, pz);
00395 Eigen::Vector3d rev_joint_translation_unitary = (-rev_joint_rotation_unitary).cross(rev_joint_position);
00396
00397 double counter = 0.;
00398 size_t trajectory_length = this->_delta_poses_in_rrbf.size();
00399 size_t amount_samples = std::min(trajectory_length, (size_t)this->_likelihood_sample_num);
00400 double delta_idx_samples = (double)std::max(1., (double)trajectory_length/(double)this->_likelihood_sample_num);
00401 size_t current_idx = 0;
00402
00403 double max_norm_of_deltas = 0;
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414 for (size_t sample_idx = 0; sample_idx < amount_samples; sample_idx++)
00415 {
00416 current_idx = boost::math::round(sample_idx*delta_idx_samples);
00417 Eigen::Displacementd rb2_last_delta_relative_displ = this->_delta_poses_in_rrbf.at(current_idx).exp(1e-12);
00418
00419 max_norm_of_deltas = std::max(this->_delta_poses_in_rrbf.at(current_idx).norm(), max_norm_of_deltas);
00420
00421 Eigen::Vector3d rb2_last_delta_relative_translation = rb2_last_delta_relative_displ.getTranslation();
00422 Eigen::Quaterniond rb2_last_delta_relative_rotation = Eigen::Quaterniond(rb2_last_delta_relative_displ.qw(),
00423 rb2_last_delta_relative_displ.qx(),
00424 rb2_last_delta_relative_displ.qy(),
00425 rb2_last_delta_relative_displ.qz());
00426
00427 Eigen::Vector3d rev_joint_translation = this->_joint_states_all.at(current_idx) * rev_joint_translation_unitary;
00428 Eigen::Vector3d rev_joint_rotation = this->_joint_states_all.at(current_idx) * rev_joint_rotation_unitary;
00429 Eigen::Displacementd rb2_last_delta_relative_displ_rev_hyp = Eigen::Twistd( rev_joint_rotation.x(),
00430 rev_joint_rotation.y(),
00431 rev_joint_rotation.z(),
00432 rev_joint_translation.x(),
00433 rev_joint_translation.y(),
00434 rev_joint_translation.z()).exp(1e-12);
00435 Eigen::Vector3d rb2_last_delta_relative_translation_rev_hyp = rb2_last_delta_relative_displ_rev_hyp.getTranslation();
00436 Eigen::Quaterniond rb2_last_delta_relative_rotation_rev_hyp = Eigen::Quaterniond(rb2_last_delta_relative_displ_rev_hyp.qw(),
00437 rb2_last_delta_relative_displ_rev_hyp.qx(),
00438 rb2_last_delta_relative_displ_rev_hyp.qy(),
00439 rb2_last_delta_relative_displ_rev_hyp.qz());
00440
00441
00442 double translation_error = (rb2_last_delta_relative_translation - rb2_last_delta_relative_translation_rev_hyp).norm();
00443 Eigen::Quaterniond rotation_error = rb2_last_delta_relative_rotation.inverse() * rb2_last_delta_relative_rotation_rev_hyp;
00444 double rotation_error_angle = Eigen::Displacementd(0., 0., 0., rotation_error.w(), rotation_error.x(), rotation_error.y(), rotation_error.z()).log(1e-12).norm();
00445
00446 accumulated_error += translation_error + fabs(rotation_error_angle);
00447
00448 p_one_meas_given_model_params = (1.0/(sigma_translation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(translation_error/sigma_translation, 2)) *
00449 (1.0/(sigma_rotation*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(rotation_error_angle/sigma_rotation, 2));
00450
00451 p_all_meas_given_model_params += (p_one_meas_given_model_params/(double)amount_samples);
00452
00453 counter++;
00454 }
00455
00456 if(counter != 0)
00457 {
00458 this->_measurements_likelihood = p_all_meas_given_model_params;
00459 }else{
00460 this->_measurements_likelihood = 1e-5;
00461 }
00462 }
00463
00464 void RevoluteJointFilter::estimateUnnormalizedModelProbability()
00465 {
00466 Eigen::Vector3d point1_in_rrbf = this->_joint_position + 100*this->_joint_orientation;
00467 Eigen::Vector3d point2_in_rrbf = this->_joint_position - 100*this->_joint_orientation;
00468
00469
00470
00471 double p_params_given_model = 1;
00472
00473
00474 double distance_to_rrb = 0;
00475 if(_rrb_id != 0)
00476 {
00477 distance_to_rrb = (point2_in_rrbf- point1_in_rrbf).cross(point1_in_rrbf).norm()/((point2_in_rrbf - point1_in_rrbf).norm());
00478 p_params_given_model *= (1.0/(_rev_max_joint_distance_for_ee*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(distance_to_rrb/_rev_max_joint_distance_for_ee, 2));
00479 }
00480
00481 Eigen::Vector4d point1_in_rrbf_homo(point1_in_rrbf.x(), point1_in_rrbf.y(), point1_in_rrbf.z(), 1.0);
00482 Eigen::Vector4d point1_in_sf_homo = _rrb_current_pose_in_sf.exp(1e-12).toHomogeneousMatrix()*point1_in_rrbf_homo;
00483 Eigen::Vector3d point1_in_sf(point1_in_sf_homo[0],point1_in_sf_homo[1],point1_in_sf_homo[2]);
00484
00485 Eigen::Vector4d point2_in_rrbf_homo(point2_in_rrbf.x(), point2_in_rrbf.y(), point2_in_rrbf.z(), 1.0);
00486 Eigen::Vector4d point2_in_sf_homo = _rrb_current_pose_in_sf.exp(1e-12).toHomogeneousMatrix()*point2_in_rrbf_homo;
00487 Eigen::Vector3d point2_in_sf(point2_in_sf_homo[0],point2_in_sf_homo[1],point2_in_sf_homo[2]);
00488
00489 double distance_to_srb = ((point2_in_sf - point1_in_sf).cross(point1_in_sf - _srb_centroid_in_sf)).norm()/((point2_in_sf - point1_in_sf).norm());
00490
00491 p_params_given_model *= (1.0/(_rev_max_joint_distance_for_ee*sqrt(2.0*M_PI)))*exp((-1.0/2.0)*pow(distance_to_srb/_rev_max_joint_distance_for_ee, 2));
00492
00493 this->_unnormalized_model_probability = _model_prior_probability*_measurements_likelihood*p_params_given_model;
00494 }
00495
00496 geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBDeltaPoseWithCovInSensorFrame()
00497 {
00498 Eigen::Matrix<double, 6, 6> adjoint;
00499 computeAdjoint(this->_rrb_current_pose_in_sf, adjoint);
00500 Eigen::Twistd predicted_delta_pose_in_sf = adjoint*this->_predicted_delta_pose_in_rrbf;
00501
00502 geometry_msgs::TwistWithCovariance hypothesis;
00503
00504 hypothesis.twist.linear.x = predicted_delta_pose_in_sf.vx();
00505 hypothesis.twist.linear.y = predicted_delta_pose_in_sf.vy();
00506 hypothesis.twist.linear.z = predicted_delta_pose_in_sf.vz();
00507 hypothesis.twist.angular.x = predicted_delta_pose_in_sf.rx();
00508 hypothesis.twist.angular.y = predicted_delta_pose_in_sf.ry();
00509 hypothesis.twist.angular.z = predicted_delta_pose_in_sf.rz();
00510
00511
00512 ColumnVector empty;
00513 ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00514 SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
00515 for(int i=0; i<6; i++)
00516 {
00517 for(int j=0; j<6; j++)
00518 {
00519 hypothesis.covariance[6 * i + j] = measurement_cov(i+1,j+1);
00520 }
00521 }
00522
00523 return hypothesis;
00524 }
00525
00526 geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBVelocityWithCovInSensorFrame()
00527 {
00528 Eigen::Matrix<double, 6, 6> adjoint;
00529 computeAdjoint(this->_rrb_current_pose_in_sf, adjoint);
00530 Eigen::Twistd predicted_delta_pose_in_sf = adjoint*(this->_predicted_delta_pose_in_rrbf/(_loop_period_ns/1e9));
00531
00532 geometry_msgs::TwistWithCovariance hypothesis;
00533
00534 hypothesis.twist.linear.x = predicted_delta_pose_in_sf.vx();
00535 hypothesis.twist.linear.y = predicted_delta_pose_in_sf.vy();
00536 hypothesis.twist.linear.z = predicted_delta_pose_in_sf.vz();
00537 hypothesis.twist.angular.x = predicted_delta_pose_in_sf.rx();
00538 hypothesis.twist.angular.y = predicted_delta_pose_in_sf.ry();
00539 hypothesis.twist.angular.z = predicted_delta_pose_in_sf.rz();
00540
00541
00542 ColumnVector empty;
00543 ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00544 SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
00545 for(int i=0; i<6; i++)
00546 {
00547 for(int j=0; j<6; j++)
00548 {
00549 hypothesis.covariance[6 * i + j] = measurement_cov(i+1,j+1);
00550 }
00551 }
00552
00553 return hypothesis;
00554 }
00555
00556 geometry_msgs::TwistWithCovariance RevoluteJointFilter::getPredictedSRBPoseWithCovInSensorFrame()
00557 {
00558 Eigen::Twistd delta_rrb_in_sf = this->_rrb_current_vel_in_sf*(this->_loop_period_ns/1e9);
00559 Eigen::Twistd rrb_next_pose_in_sf = (delta_rrb_in_sf.exp(1e-12)*this->_rrb_current_pose_in_sf.exp(1e-12)).log(1e-12);
00560
00561 Eigen::Displacementd T_sf_rrbf_next = rrb_next_pose_in_sf.exp(1e-12);
00562 Eigen::Displacementd T_rrbf_srbf_next = this->_srb_predicted_pose_in_rrbf.exp(1e-12);
00563
00564 Eigen::Displacementd T_sf_srbf_next = T_rrbf_srbf_next*T_sf_rrbf_next;
00565
00566 Eigen::Twistd srb_next_pose_in_sf = T_sf_srbf_next.log(1e-12);
00567
00568 geometry_msgs::TwistWithCovariance hypothesis;
00569
00570 hypothesis.twist.linear.x = srb_next_pose_in_sf.vx();
00571 hypothesis.twist.linear.y = srb_next_pose_in_sf.vy();
00572 hypothesis.twist.linear.z = srb_next_pose_in_sf.vz();
00573 hypothesis.twist.angular.x = srb_next_pose_in_sf.rx();
00574 hypothesis.twist.angular.y = srb_next_pose_in_sf.ry();
00575 hypothesis.twist.angular.z = srb_next_pose_in_sf.rz();
00576
00577
00578 ColumnVector empty;
00579 ColumnVector state_updated_state = this->_ekf->PostGet()->ExpectedValueGet();
00580 SymmetricMatrix measurement_cov = this->_meas_MODEL->CovarianceGet(empty, state_updated_state);
00581 Eigen::Matrix<double,6,6> measurement_cov_eigen;
00582 for(int i=0; i<6; i++)
00583 {
00584 for(int j=0; j<6; j++)
00585 {
00586 measurement_cov_eigen(i,j) = measurement_cov(i+1,j+1);
00587 }
00588 }
00589
00590
00591 Eigen::Matrix<double,6,6> tranformed_cov;
00592 adjointXcovXadjointT(_rrb_current_pose_in_sf, measurement_cov_eigen, tranformed_cov);
00593 Eigen::Matrix<double,6,6> new_pose_covariance = this->_rrb_pose_cov_in_sf + tranformed_cov;
00594 for (unsigned int i = 0; i < 6; i++)
00595 {
00596 for (unsigned int j = 0; j < 6; j++)
00597 {
00598 hypothesis.covariance[6 * i + j] = new_pose_covariance(i, j);
00599 }
00600 }
00601
00602
00603 #ifdef PUBLISH_PREDICTED_POSE_AS_PWC
00604
00605 geometry_msgs::PoseWithCovarianceStamped pose_with_cov_stamped;
00606 pose_with_cov_stamped.header.stamp = ros::Time::now();
00607 pose_with_cov_stamped.header.frame_id = "camera_rgb_optical_frame";
00608
00609 Eigen::Displacementd displ_from_twist = srb_next_pose_in_sf.exp(1e-12);
00610 pose_with_cov_stamped.pose.pose.position.x = displ_from_twist.x();
00611 pose_with_cov_stamped.pose.pose.position.y = displ_from_twist.y();
00612 pose_with_cov_stamped.pose.pose.position.z = displ_from_twist.z();
00613 pose_with_cov_stamped.pose.pose.orientation.x = displ_from_twist.qx();
00614 pose_with_cov_stamped.pose.pose.orientation.y = displ_from_twist.qy();
00615 pose_with_cov_stamped.pose.pose.orientation.z = displ_from_twist.qz();
00616 pose_with_cov_stamped.pose.pose.orientation.w = displ_from_twist.qw();
00617
00618 for (unsigned int i = 0; i < 6; i++)
00619 for (unsigned int j = 0; j < 6; j++)
00620 pose_with_cov_stamped.pose.covariance[6 * i + j] = new_pose_covariance(i, j);
00621
00622 _predicted_next_pose_publisher.publish(pose_with_cov_stamped);
00623 #endif
00624
00625 return hypothesis;
00626 }
00627
00628 std::vector<visualization_msgs::Marker> RevoluteJointFilter::getJointMarkersInRRBFrame() const
00629 {
00630
00631
00632
00633 Eigen::Vector3d rev_joint_ori_in_ref_rb = this->_joint_orientation;
00634 Eigen::Vector3d rev_joint_posi_in_ref_rb = this->_joint_position;
00635
00636
00637 std::vector<visualization_msgs::Marker> revolute_markers;
00638
00639 visualization_msgs::Marker axis_orientation_marker;
00640 axis_orientation_marker.ns = "kinematic_structure";
00641 axis_orientation_marker.action = visualization_msgs::Marker::ADD;
00642 axis_orientation_marker.type = visualization_msgs::Marker::ARROW;
00643
00644
00645
00646
00647
00648 axis_orientation_marker.id = 3 * this->_joint_id;
00649 axis_orientation_marker.scale.x = JOINT_AXIS_AND_VARIABLE_MARKER_RADIUS;
00650 axis_orientation_marker.scale.y = 0.f;
00651 axis_orientation_marker.scale.z = 0.f;
00652 axis_orientation_marker.color.r = 1.f;
00653 axis_orientation_marker.color.g = 0.f;
00654 axis_orientation_marker.color.b = 0.f;
00655 axis_orientation_marker.color.a = 1.f;
00656 Eigen::Vector3d point_on_rot_axis_1 = rev_joint_posi_in_ref_rb - this->_joint_state * rev_joint_ori_in_ref_rb;
00657 geometry_msgs::Point pt1;
00658 pt1.x = point_on_rot_axis_1.x();
00659 pt1.y = point_on_rot_axis_1.y();
00660 pt1.z = point_on_rot_axis_1.z();
00661 axis_orientation_marker.points.push_back(pt1);
00662 Eigen::Vector3d point_on_rot_axis_2 = rev_joint_posi_in_ref_rb + this->_joint_state * rev_joint_ori_in_ref_rb;
00663 geometry_msgs::Point pt2;
00664 pt2.x = point_on_rot_axis_2.x();
00665 pt2.y = point_on_rot_axis_2.y();
00666 pt2.z = point_on_rot_axis_2.z();
00667 axis_orientation_marker.points.push_back(pt2);
00668 revolute_markers.push_back(axis_orientation_marker);
00669
00670
00671 visualization_msgs::Marker axis_orientation_markerb;
00672 axis_orientation_markerb.ns = "kinematic_structure";
00673 axis_orientation_markerb.action = visualization_msgs::Marker::ADD;
00674 axis_orientation_markerb.type = visualization_msgs::Marker::ARROW;
00675
00676
00677
00678
00679 axis_orientation_markerb.id = 3 * this->_joint_id + 1;
00680 axis_orientation_markerb.scale.x = JOINT_AXIS_MARKER_RADIUS;
00681 axis_orientation_markerb.scale.y = 0.f;
00682 axis_orientation_markerb.scale.z = 0.f;
00683 axis_orientation_markerb.color.r = 1.f;
00684 axis_orientation_markerb.color.g = 0.f;
00685 axis_orientation_markerb.color.b = 0.f;
00686 axis_orientation_markerb.color.a = 1.f;
00687 Eigen::Vector3d point_on_rot_axis_1b = rev_joint_posi_in_ref_rb - 100 * rev_joint_ori_in_ref_rb;
00688 geometry_msgs::Point pt1b;
00689 pt1b.x = point_on_rot_axis_1b.x();
00690 pt1b.y = point_on_rot_axis_1b.y();
00691 pt1b.z = point_on_rot_axis_1b.z();
00692 axis_orientation_markerb.points.push_back(pt1b);
00693 Eigen::Vector3d point_on_rot_axis_2b = rev_joint_posi_in_ref_rb + 100 * rev_joint_ori_in_ref_rb;
00694 geometry_msgs::Point pt2b;
00695 pt2b.x = point_on_rot_axis_2b.x();
00696 pt2b.y = point_on_rot_axis_2b.y();
00697 pt2b.z = point_on_rot_axis_2b.z();
00698 axis_orientation_markerb.points.push_back(pt2b);
00699
00700 revolute_markers.push_back(axis_orientation_markerb);
00701
00702
00703 axis_orientation_markerb.points.clear();
00704 axis_orientation_markerb.id = 3 * this->_joint_id + 2;
00705 axis_orientation_markerb.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00706 axis_orientation_markerb.scale.z = JOINT_VALUE_TEXT_SIZE;
00707 std::ostringstream oss_joint_value;
00708 oss_joint_value << std::fixed<< std::setprecision(0) << (180/M_PI)*this->_joint_state;
00709 axis_orientation_markerb.text = oss_joint_value.str() + std::string(" deg");
00710 axis_orientation_markerb.pose.position.x = rev_joint_posi_in_ref_rb.x();
00711 axis_orientation_markerb.pose.position.y = rev_joint_posi_in_ref_rb.y();
00712 axis_orientation_markerb.pose.position.z = rev_joint_posi_in_ref_rb.z();
00713 axis_orientation_markerb.pose.orientation.x = 0;
00714 axis_orientation_markerb.pose.orientation.y = 0;
00715 axis_orientation_markerb.pose.orientation.z = 0;
00716 axis_orientation_markerb.pose.orientation.w = 1;
00717
00718 revolute_markers.push_back(axis_orientation_markerb);
00719
00720
00721
00722 visualization_msgs::Marker axis_position_uncertainty_marker;
00723 axis_position_uncertainty_marker.pose.position.x = rev_joint_posi_in_ref_rb.x();
00724 axis_position_uncertainty_marker.pose.position.y = rev_joint_posi_in_ref_rb.y();
00725 axis_position_uncertainty_marker.pose.position.z = rev_joint_posi_in_ref_rb.z();
00726
00727
00728
00729
00730 axis_position_uncertainty_marker.ns = "kinematic_structure_uncertainty";
00731 axis_position_uncertainty_marker.type = visualization_msgs::Marker::SPHERE;
00732 axis_position_uncertainty_marker.action = visualization_msgs::Marker::ADD;
00733 axis_position_uncertainty_marker.id = 3 * this->_joint_id ;
00734
00735
00736 axis_position_uncertainty_marker.scale.x = this->_uncertainty_joint_position(0,0);
00737 axis_position_uncertainty_marker.scale.y = this->_uncertainty_joint_position(1,1);
00738 axis_position_uncertainty_marker.scale.z = this->_uncertainty_joint_position(2,2);
00739 axis_position_uncertainty_marker.color.a = 0.3;
00740 axis_position_uncertainty_marker.color.r = 1.0;
00741 axis_position_uncertainty_marker.color.g = 0.0;
00742 axis_position_uncertainty_marker.color.b = 0.0;
00743 revolute_markers.push_back(axis_position_uncertainty_marker);
00744
00745 visualization_msgs::Marker rev_axis_unc_cone1;
00746 rev_axis_unc_cone1.type = visualization_msgs::Marker::MESH_RESOURCE;
00747 rev_axis_unc_cone1.action = visualization_msgs::Marker::ADD;
00748 rev_axis_unc_cone1.mesh_resource = "package://joint_tracker/meshes/cone.stl";
00749 rev_axis_unc_cone1.pose.position.x = rev_joint_posi_in_ref_rb.x();
00750 rev_axis_unc_cone1.pose.position.y = rev_joint_posi_in_ref_rb.y();
00751 rev_axis_unc_cone1.pose.position.z = rev_joint_posi_in_ref_rb.z();
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(this->_uncertainty_joint_orientation_phitheta);
00765
00766
00767
00768 double confidence_value = 0.5;
00769 boost::math::chi_squared chi_sq_dist(2);
00770 double critical_value = boost::math::quantile(chi_sq_dist, confidence_value);
00771 double major_axis_length = 2*eigensolver.eigenvalues()[1]*std::sqrt(critical_value);
00772 double minor_axis_length = 2*eigensolver.eigenvalues()[0]*std::sqrt(critical_value);
00773
00774
00775
00776 double alpha = atan2(eigensolver.eigenvectors().col(1)[1],eigensolver.eigenvectors().col(1)[0]);
00777
00778 Eigen::AngleAxisd init_rot(-alpha, Eigen::Vector3d::UnitZ());
00779
00780
00781
00782
00783
00784
00785 Eigen::Quaterniond ori_quat;
00786 ori_quat.setFromTwoVectors(Eigen::Vector3d::UnitZ(), rev_joint_ori_in_ref_rb);
00787
00788
00789
00790 Eigen::Vector3d coplanar_xy_orthogonal_to_joint_ori = rev_joint_ori_in_ref_rb.cross(Eigen::Vector3d::UnitZ());
00791
00792 coplanar_xy_orthogonal_to_joint_ori.normalize();
00793
00794
00795
00796 Eigen::Vector3d y_pos = rev_joint_ori_in_ref_rb.cross(coplanar_xy_orthogonal_to_joint_ori);
00797
00798
00799 Eigen::Matrix3d rotation_pos;
00800 rotation_pos << coplanar_xy_orthogonal_to_joint_ori.x(),y_pos.x(),rev_joint_ori_in_ref_rb.x(),
00801 coplanar_xy_orthogonal_to_joint_ori.y(),y_pos.y(),rev_joint_ori_in_ref_rb.y(),
00802 coplanar_xy_orthogonal_to_joint_ori.z(),y_pos.z(),rev_joint_ori_in_ref_rb.z();
00803
00804
00805 Eigen::Quaterniond ori_quat_final(rotation_pos);
00806
00807 Eigen::Quaterniond ori_quat_final_ellipse(ori_quat_final.toRotationMatrix()*init_rot.toRotationMatrix());
00808
00809 rev_axis_unc_cone1.pose.orientation.x = ori_quat_final_ellipse.x();
00810 rev_axis_unc_cone1.pose.orientation.y = ori_quat_final_ellipse.y();
00811 rev_axis_unc_cone1.pose.orientation.z = ori_quat_final_ellipse.z();
00812 rev_axis_unc_cone1.pose.orientation.w = ori_quat_final_ellipse.w();
00813
00814
00815
00816
00817 rev_axis_unc_cone1.ns = "kinematic_structure_uncertainty";
00818 rev_axis_unc_cone1.id = 3 * this->_joint_id + 1;
00819 rev_axis_unc_cone1.color.a = 0.4;
00820 rev_axis_unc_cone1.color.r = 1.0;
00821 rev_axis_unc_cone1.color.g = 0.0;
00822 rev_axis_unc_cone1.color.b = 0.0;
00823
00824
00825
00826
00827
00828
00829 rev_axis_unc_cone1.scale.x = major_axis_length / (M_PI / 6.0);
00830 rev_axis_unc_cone1.scale.y = minor_axis_length / (M_PI / 6.0);
00831 rev_axis_unc_cone1.scale.z = 1.;
00832 revolute_markers.push_back(rev_axis_unc_cone1);
00833
00834
00835
00836 Eigen::Vector3d rev_joint_ori_in_ref_rb_neg = -rev_joint_ori_in_ref_rb;
00837 Eigen::Quaterniond ori_quat_neg;
00838 ori_quat_neg.setFromTwoVectors(Eigen::Vector3d::UnitZ(), rev_joint_ori_in_ref_rb_neg);
00839
00840
00841
00842 Eigen::Vector3d coplanar_xy_orthogonal_to_joint_ori_neg = rev_joint_ori_in_ref_rb_neg.cross(Eigen::Vector3d::UnitZ());
00843
00844 coplanar_xy_orthogonal_to_joint_ori_neg.normalize();
00845
00846
00847
00848 Eigen::Vector3d y_neg = rev_joint_ori_in_ref_rb_neg.cross(coplanar_xy_orthogonal_to_joint_ori_neg);
00849
00850
00851 Eigen::Matrix3d rotation_neg;
00852 rotation_neg << coplanar_xy_orthogonal_to_joint_ori_neg.x(),y_neg.x(),rev_joint_ori_in_ref_rb_neg.x(),
00853 coplanar_xy_orthogonal_to_joint_ori_neg.y(),y_neg.y(),rev_joint_ori_in_ref_rb_neg.y(),
00854 coplanar_xy_orthogonal_to_joint_ori_neg.z(),y_neg.z(),rev_joint_ori_in_ref_rb_neg.z();
00855
00856
00857 Eigen::Quaterniond ori_quat_neg_final(rotation_neg);
00858
00859
00860 Eigen::AngleAxisd init_rot_neg(alpha, Eigen::Vector3d::UnitZ());
00861 Eigen::Quaterniond ori_quat_neg_final_ellipse(ori_quat_neg_final.toRotationMatrix()*init_rot_neg.toRotationMatrix());
00862
00863 rev_axis_unc_cone1.pose.orientation.x = ori_quat_neg_final_ellipse.x();
00864 rev_axis_unc_cone1.pose.orientation.y = ori_quat_neg_final_ellipse.y();
00865 rev_axis_unc_cone1.pose.orientation.z = ori_quat_neg_final_ellipse.z();
00866 rev_axis_unc_cone1.pose.orientation.w = ori_quat_neg_final_ellipse.w();
00867 rev_axis_unc_cone1.scale.x = major_axis_length / (M_PI / 6.0);
00868 rev_axis_unc_cone1.scale.y = minor_axis_length / (M_PI / 6.0);
00869 rev_axis_unc_cone1.scale.z = 1.;
00870 rev_axis_unc_cone1.id = 3 * this->_joint_id + 2;
00871 revolute_markers.push_back(rev_axis_unc_cone1);
00872
00873 return revolute_markers;
00874 }
00875
00876 JointFilterType RevoluteJointFilter::getJointFilterType() const
00877 {
00878 return REVOLUTE_JOINT;
00879 }
00880
00881 std::string RevoluteJointFilter::getJointFilterTypeStr() const
00882 {
00883 return std::string("RevoluteJointFilter");
00884 }