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