00001 #include <boost/foreach.hpp>
00002
00003 #include "rb_tracker/RBFilter.h"
00004 #include "omip_common/OMIPUtils.h"
00005
00006 #include <boost/numeric/ublas/matrix.hpp>
00007
00008 #include <iostream>
00009 #include <fstream>
00010
00011 #include <Eigen/Cholesky>
00012
00013 #include <pcl/filters/filter.h>
00014 #include <pcl/point_types.h>
00015 #include <pcl_ros/transforms.h>
00016
00017 using namespace omip;
00018
00019 RB_id_t RBFilter::_rb_id_generator = 1;
00020
00021 using namespace MatrixWrapper;
00022 using namespace BFL;
00023
00024
00025 #define MEAS_DIM_RBF 3
00026
00027
00028 #define COV_MEAS_RBF 1.0
00029
00030 RBFilter::RBFilter() :
00031 _estimation_error_threshold(-1.),
00032 _id(RBFilter::_rb_id_generator++),
00033 _velocity( 0., 0., 0., 0., 0., 0.),
00034 _features_database(),
00035 _loop_period_ns(0.),
00036 _min_num_supporting_feats_to_correct(5),
00037 _use_predicted_state_from_kh(false),
00038 _use_predicted_measurement_from_kh(false),
00039 _last_time_interval_ns(0.0)
00040 {
00041 _pose = Eigen::Matrix4d::Identity();
00042 this->_initial_location_of_centroid = Feature::Location(0,0,0);
00043 }
00044
00045 RBFilter::RBFilter(double loop_period_ns,
00046 const std::vector<Eigen::Matrix4d>& trajectory,
00047 const Eigen::Twistd &initial_velocity,
00048 FeaturesDataBase::Ptr feats_database,
00049 double estimation_error_threshold) :
00050 _loop_period_ns(loop_period_ns),
00051 _last_time_interval_ns(loop_period_ns),
00052 _estimation_error_threshold(estimation_error_threshold),
00053 _id(RBFilter::_rb_id_generator++),
00054 _pose(trajectory[trajectory.size()-1]),
00055 _velocity(initial_velocity),
00056 _features_database(feats_database),
00057 _min_num_supporting_feats_to_correct(5),
00058 _use_predicted_state_from_kh(false),
00059 _use_predicted_measurement_from_kh(false)
00060 {
00061 this->_initial_location_of_centroid = Feature::Location(trajectory[0](0,3),trajectory[0](1,3),trajectory[0](2,3));
00062 this->_trajectory = std::vector<Eigen::Matrix4d>(trajectory.begin()+1, trajectory.end());
00063 }
00064
00065 void RBFilter::Init()
00066 {
00067 this->_initializeAuxiliarMatrices();
00068 }
00069
00070 RBFilter::RBFilter(const RBFilter &rbm)
00071 {
00072 this->_id = rbm._id;
00073 this->_pose = rbm._pose;
00074 this->_velocity = rbm._velocity;
00075 this->_trajectory = rbm._trajectory;
00076 this->_supporting_features_ids = rbm._supporting_features_ids;
00077 this->_features_database = rbm.getFeaturesDatabase();
00078 this->_predicted_measurement_pc_bh = rbm._predicted_measurement_pc_bh;
00079 this->_predicted_measurement_pc_vh = rbm._predicted_measurement_pc_vh;
00080 this->_predicted_measurement_map_bh = rbm._predicted_measurement_map_bh;
00081 this->_predicted_measurement_map_vh = rbm._predicted_measurement_map_vh;
00082 this->_predicted_pose_bh = rbm._predicted_pose_bh;
00083 this->_predicted_pose_vh = rbm._predicted_pose_vh;
00084 this->_min_num_supporting_feats_to_correct = rbm._min_num_supporting_feats_to_correct;
00085 }
00086
00087 RBFilter::~RBFilter()
00088 {
00089 }
00090
00091 void RBFilter::predictState(double time_interval_ns)
00092 {
00093
00094 if(time_interval_ns < 0)
00095 {
00096 std::cout << "Time interval negative!" << std::endl;
00097 this->_last_time_interval_ns = time_interval_ns;
00098 getchar();
00099 }
00100
00101
00102 Eigen::Matrix<double, 6, 6> sys_noise_COV = Eigen::Matrix<double, 6, 6>::Zero();
00103 sys_noise_COV(0, 0) = this->_cov_sys_acc_tx*(time_interval_ns/1e9);
00104 sys_noise_COV(1, 1) = this->_cov_sys_acc_ty*(time_interval_ns/1e9);
00105 sys_noise_COV(2, 2) = this->_cov_sys_acc_tz*(time_interval_ns/1e9);
00106 sys_noise_COV(3, 3) = this->_cov_sys_acc_rx*(time_interval_ns/1e9);
00107 sys_noise_COV(4, 4) = this->_cov_sys_acc_ry*(time_interval_ns/1e9);
00108 sys_noise_COV(5, 5) = this->_cov_sys_acc_rz*(time_interval_ns/1e9);
00109
00110
00111
00112
00113
00114
00115
00116 Eigen::Twistd belief_delta_pose_ec = (time_interval_ns/1e9)*_velocity;
00117 _predicted_pose_vh = belief_delta_pose_ec.exp(1e-12).toHomogeneousMatrix()*_pose;
00118
00119
00120 Eigen::Matrix<double, 6, 6> delta_pose_covariance = (time_interval_ns/1e9)*_velocity_covariance;
00121
00122
00123 Eigen::Twistd pose_ec;
00124 TransformMatrix2Twist(_pose, pose_ec);
00125 Eigen::Matrix<double, 6, 6> transformed_cov;
00126 adjointXcovXadjointT(pose_ec, delta_pose_covariance, transformed_cov);
00127
00128
00129 _predicted_pose_cov_vh = _pose_covariance + delta_pose_covariance;
00130
00131
00132 _predicted_pose_cov_vh += sys_noise_COV;
00133
00134
00135
00136 this->_predicted_pose_bh = _pose;
00137 this->_predicted_velocity_bh = Eigen::Twistd(0,0,0,0,0,0);
00138
00139
00140 this->_predicted_pose_cov_bh = _pose_covariance + delta_pose_covariance + 1.001*sys_noise_COV;
00141 this->_predicted_velocity_cov_bh = sys_noise_COV;
00142 }
00143
00144
00145
00146
00147 void RBFilter::predictMeasurement()
00148 {
00149 this->_predicted_measurement_pc_vh->points.clear();
00150 this->_predicted_measurement_map_vh.clear();
00151 this->_predicted_measurement_pc_bh->points.clear();
00152 this->_predicted_measurement_map_bh.clear();
00153 this->_predicted_measurement_pc_kh->points.clear();
00154 this->_predicted_measurement_map_kh.clear();
00155
00156
00157 if(_use_predicted_state_from_kh)
00158 {
00159 this->_use_predicted_measurement_from_kh = true;
00160 }
00161
00162 Feature::Location predicted_location;
00163 FeaturePCLwc predicted_location_pcl;
00164 Feature::Ptr supporting_feat_ptr;
00165 BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
00166 {
00167 if(this->_features_database->isFeatureStored(supporting_feat_id))
00168 {
00169 supporting_feat_ptr = this->_features_database->getFeature(supporting_feat_id);
00170
00171
00172 this->PredictFeatureLocation(supporting_feat_ptr,this->_predicted_pose_vh, true, predicted_location);
00173 LocationAndId2FeaturePCLwc(predicted_location, supporting_feat_id, predicted_location_pcl);
00174 this->_predicted_measurement_pc_vh->points.push_back(predicted_location_pcl);
00175 this->_predicted_measurement_map_vh[supporting_feat_id] = predicted_location;
00176
00177
00178 this->PredictFeatureLocation(supporting_feat_ptr,this->_predicted_pose_bh, true, predicted_location);
00179 LocationAndId2FeaturePCLwc(predicted_location, supporting_feat_id, predicted_location_pcl);
00180 this->_predicted_measurement_pc_bh->points.push_back(predicted_location_pcl);
00181 this->_predicted_measurement_map_bh[supporting_feat_id] = predicted_location;
00182
00183 if(_use_predicted_state_from_kh)
00184 {
00185
00186 this->PredictFeatureLocation(supporting_feat_ptr,this->_predicted_pose_kh, true, predicted_location);
00187 LocationAndId2FeaturePCLwc(predicted_location, supporting_feat_id, predicted_location_pcl);
00188 this->_predicted_measurement_pc_kh->points.push_back(predicted_location_pcl);
00189 this->_predicted_measurement_map_kh[supporting_feat_id] = predicted_location;
00190 this->_use_predicted_state_from_kh = false;
00191 }
00192 }
00193 }
00194 }
00195
00196 void RBFilter::correctState()
00197 {
00198
00199
00200 int num_supporting_feats = (int)this->_supporting_features_ids.size();
00201
00202 if(num_supporting_feats > this->_min_num_supporting_feats_to_correct)
00203 {
00204 int meas_dimension = 3 * num_supporting_feats;
00205
00206 double feature_depth = 0.;
00207
00208 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > G_t((this->_G_t_memory.data()), 6, meas_dimension);
00209 Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> > R_inv_times_innovation((this->_R_inv_times_innovation_memory.data()), meas_dimension, 1);
00210
00211
00212 G_t.setZero();
00213 R_inv_times_innovation.setZero();
00214
00215 Feature::Location location_at_birth;
00216
00217 int feat_idx = 0;
00218
00219 Eigen::Matrix3d R_seq = Eigen::Matrix3d::Zero();
00220 Eigen::Matrix<double, 6, 6> P_minus_seq = _predicted_pose_covariance;
00221
00222
00223 Eigen::Matrix<double, 6, 3> P_HT_seq = Eigen::Matrix<double, 6, 3>::Zero();
00224 Eigen::Matrix3d S_seq = Eigen::Matrix3d::Zero();
00225 Eigen::Matrix3d S_inv_seq = Eigen::Matrix3d::Zero();
00226 Eigen::Matrix<double, 6, 3> K_seq = Eigen::Matrix<double, 6, 3>::Zero();
00227
00228 Eigen::Matrix<double, 6, 6> temp_seq1 = Eigen::Matrix<double, 6, 6>::Zero();
00229 Eigen::Matrix<double, 6, 6> temp_seq2 = Eigen::Matrix<double, 6, 6>::Zero();
00230
00231 Eigen::Vector4d feature_innovation;
00232 Eigen::Vector4d location_at_birth_eig;
00233
00234 Eigen::Vector4d predicted_location;
00235 Eigen::Vector4d current_location;
00236
00237 Feature::Ptr supporting_feat_ptr;
00238
00239 BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
00240 {
00241 supporting_feat_ptr = this->_features_database->getFeature(supporting_feat_id);
00242
00243 if(supporting_feat_ptr->getFeatureAge() > 1)
00244 this->GetLocationOfFeatureAtBirthOfRB( supporting_feat_ptr, false, location_at_birth);
00245 LocationOfFeature2EigenVectorHomogeneous(location_at_birth, location_at_birth_eig);
00246
00247 Feature::Location current_loc = supporting_feat_ptr->getLastLocation();
00248 LocationOfFeature2EigenVectorHomogeneous(current_loc, current_location);
00249
00250 predicted_location = _predicted_pose*location_at_birth_eig;
00251
00252 feature_innovation = current_location - predicted_location;
00253
00254 feature_depth = supporting_feat_ptr->getLastZ();
00255
00256 R_seq( 0, 0) = std::max(this->_min_cov_meas_x, feature_depth / this->_meas_depth_factor);
00257 R_seq( 1, 1) = std::max(this->_min_cov_meas_y, feature_depth / this->_meas_depth_factor);
00258 R_seq( 2, 2) = std::max(this->_min_cov_meas_z, feature_depth / this->_meas_depth_factor);
00259
00260 R_inv_times_innovation(3 * feat_idx ) = feature_innovation.x()/(std::max(this->_min_cov_meas_x, feature_depth / this->_meas_depth_factor));
00261 R_inv_times_innovation(3 * feat_idx + 1) = feature_innovation.y()/(std::max(this->_min_cov_meas_y, feature_depth / this->_meas_depth_factor));
00262 R_inv_times_innovation(3 * feat_idx + 2) = feature_innovation.z()/(std::max(this->_min_cov_meas_z, feature_depth / this->_meas_depth_factor));
00263
00264 _D_T_p0_circle(0,4) = predicted_location[2];
00265 _D_T_p0_circle(0,5) = -predicted_location[1];
00266 _D_T_p0_circle(1,3) = -predicted_location[2];
00267 _D_T_p0_circle(1,5) = predicted_location[0];
00268 _D_T_p0_circle(2,3) = predicted_location[1];
00269 _D_T_p0_circle(2,4) = -predicted_location[0];
00270
00271 G_t.block<6,3>(0, 3*feat_idx) = _D_T_p0_circle.transpose();
00272
00273 P_HT_seq = P_minus_seq * _D_T_p0_circle.transpose();
00274
00275 S_seq = _D_T_p0_circle * P_HT_seq;
00276
00277 S_seq += R_seq;
00278
00279 invert3x3MatrixEigen2(S_seq, S_inv_seq );
00280
00281 K_seq = P_HT_seq * S_inv_seq;
00282
00283 temp_seq1 = K_seq*_D_T_p0_circle;
00284
00285 temp_seq2 = temp_seq1 * P_minus_seq;
00286
00287 P_minus_seq -= temp_seq2;
00288
00289
00290 for ( unsigned int i=0; i<P_minus_seq.rows(); i++ )
00291 {
00292 for ( unsigned int j=0; j<=i; j++ )
00293 {
00294 P_minus_seq(j,i) = P_minus_seq(i,j);
00295 }
00296 }
00297
00298 feat_idx++;
00299 }
00300
00301 _pose_covariance = P_minus_seq;
00302
00303 Eigen::Matrix<double, 6, 1> pose_update = P_minus_seq * G_t * R_inv_times_innovation;
00304
00305 Eigen::Twistd pose_update_ec(pose_update[3], pose_update[4], pose_update[5], pose_update[0], pose_update[1], pose_update[2]);
00306 _pose = pose_update_ec.exp(1e-12).toHomogeneousMatrix()*_predicted_pose;
00307 }else{
00308 ROS_ERROR_STREAM_NAMED("RBFilter::correctState","RB" << this->_id << ": using predicted state as corrected state!");
00309 _pose = _predicted_pose;
00310 _pose_covariance = _predicted_pose_covariance;
00311 }
00312
00313 Eigen::Matrix4d delta_pose = _pose*_trajectory.at(_trajectory.size()-1).inverse();
00314
00315 Eigen::Twistd delta_pose_ec;
00316 TransformMatrix2Twist(delta_pose, delta_pose_ec);
00317 this->_velocity = delta_pose_ec/(_last_time_interval_ns/1e9);
00318 this->_trajectory.push_back(this->_pose);
00319 }
00320
00321 bool isPredictionYBetterThanX(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > X, boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > Y)
00322 {
00323
00324
00325 if(boost::tuples::get<0>(X) < boost::tuples::get<0>(Y))
00326 {
00327 return true;
00328 }
00329
00330
00331 else if(boost::tuples::get<0>(X) > boost::tuples::get<0>(Y))
00332 {
00333 return false;
00334 }
00335
00336
00337 else{
00338
00339
00340 if(boost::tuples::get<1>(X) > boost::tuples::get<1>(Y))
00341 {
00342 return true;
00343 }
00344
00345
00346 else if(boost::tuples::get<1>(X) < boost::tuples::get<1>(Y))
00347 {
00348 return false;
00349 }
00350
00351
00352 else
00353 {
00354 int num_larger_x_covariance = 0;
00355 for(int i =0; i<6; i++)
00356 {
00357 if(boost::tuples::get<2>(X)(i,i) > boost::tuples::get<2>(Y)(i,i))
00358 {
00359 num_larger_x_covariance +=1;
00360 }
00361 }
00362 if(num_larger_x_covariance > 3)
00363 {
00364 return true;
00365 }
00366 else{
00367 return false;
00368 }
00369 }
00370 }
00371 }
00372
00373
00374
00375
00376 void RBFilter::estimateBestPredictionAndSupportingFeatures()
00377 {
00378
00379 std::vector<Feature::Id> alive_supporting_features_ids;
00380 BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
00381 {
00382 if(this->_features_database->isFeatureStored(supporting_feat_id))
00383 {
00384 alive_supporting_features_ids.push_back(supporting_feat_id);
00385 }
00386 }
00387 this->_supporting_features_ids.swap(alive_supporting_features_ids);
00388
00389 std::vector<boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > > num_supporting_feats_and_error_and_pose_cov;
00390
00391
00392
00393 std::vector<Feature::Id> supporting_feats_based_on_vh;
00394 std::vector<Feature::Id> supporting_feats_based_on_bh;
00395 std::vector<Feature::Id> supporting_feats_based_on_kh;
00396 double error_vh = 0.0;
00397 double error_bh = 0.0;
00398 double error_kh = 0.0;
00399 double acc_error_vh = 0.0;
00400 double acc_error_bh = 0.0;
00401 double acc_error_kh = 0.0;
00402 BOOST_FOREACH(Feature::Id supporting_feat_id, this->_supporting_features_ids)
00403 {
00404 Feature::Location last_location = this->_features_database->getFeatureLastLocation(supporting_feat_id);
00405
00406 Feature::Location predicted_location_vh = this->_predicted_measurement_map_vh[supporting_feat_id];
00407 error_vh = L2Distance(last_location, predicted_location_vh);
00408 if (error_vh < this->_estimation_error_threshold)
00409 {
00410 supporting_feats_based_on_vh.push_back(supporting_feat_id);
00411 acc_error_vh += error_vh;
00412 }
00413
00414 Feature::Location predicted_location_bh = this->_predicted_measurement_map_bh[supporting_feat_id];
00415 error_bh = L2Distance(last_location, predicted_location_bh);
00416 if (error_bh < this->_estimation_error_threshold)
00417 {
00418 supporting_feats_based_on_bh.push_back(supporting_feat_id);
00419 acc_error_bh += error_bh;
00420 }
00421
00422
00423 if(this->_use_predicted_measurement_from_kh)
00424 {
00425 Feature::Location predicted_location_kh = this->_predicted_measurement_map_kh[supporting_feat_id];
00426 error_kh = L2Distance(last_location, predicted_location_kh);
00427 if (error_kh < this->_estimation_error_threshold)
00428 {
00429 supporting_feats_based_on_kh.push_back(supporting_feat_id);
00430 acc_error_kh += error_kh;
00431 }
00432 }
00433 }
00434
00435 num_supporting_feats_and_error_and_pose_cov.push_back(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> >(supporting_feats_based_on_vh.size(), acc_error_vh, _predicted_pose_cov_vh));
00436 num_supporting_feats_and_error_and_pose_cov.push_back(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> >(supporting_feats_based_on_bh.size(), acc_error_bh, _predicted_pose_cov_bh));
00437
00438
00439 if(this->_use_predicted_measurement_from_kh)
00440 {
00441 num_supporting_feats_and_error_and_pose_cov.push_back(boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> >(supporting_feats_based_on_kh.size(), acc_error_kh, _predicted_pose_cov_kh));
00442 this->_use_predicted_measurement_from_kh = false;
00443 }
00444
00445
00446 std::vector<boost::tuple<size_t, double, Eigen::Matrix<double, 6, 6> > >::iterator hyp_with_max_support_it =
00447 std::max_element(num_supporting_feats_and_error_and_pose_cov.begin(),
00448 num_supporting_feats_and_error_and_pose_cov.end(),
00449 isPredictionYBetterThanX);
00450 int hyp_with_max_support_idx = std::distance(num_supporting_feats_and_error_and_pose_cov.begin(), hyp_with_max_support_it);
00451
00452 switch(hyp_with_max_support_idx)
00453 {
00454
00455 case 0:
00456 {
00457 std::cout << "RB" << _id<<" Using velocity prediction" << std::endl;
00458 this->_predicted_pose = _predicted_pose_vh;
00459 this->_predicted_pose_covariance = _predicted_pose_cov_vh;
00460 this->_supporting_features_ids = supporting_feats_based_on_vh;
00461 this->_predicted_measurement = this->_predicted_measurement_pc_vh;
00462 this->_predicted_measurement_map = this->_predicted_measurement_map_vh;
00463 break;
00464 }
00465
00466 case 1:
00467 {
00468 std::cout << "RB" << _id<<" Using brake prediction" << std::endl;
00469 this->_predicted_pose = _predicted_pose_bh;
00470 this->_predicted_pose_covariance = _predicted_pose_cov_bh;
00471 this->_supporting_features_ids = supporting_feats_based_on_bh;
00472 this->_predicted_measurement = this->_predicted_measurement_pc_bh;
00473 this->_predicted_measurement_map = this->_predicted_measurement_map_bh;
00474 break;
00475 }
00476
00477 case 2:
00478 {
00479 ROS_ERROR_STREAM_NAMED("RBFilter.estimateBestPredictionAndSupportingFeatures",
00480 "RB" << _id << " Using prediction from higher level!");
00481 this->_predicted_pose = _predicted_pose_kh;
00482 this->_predicted_pose_covariance = _predicted_pose_cov_kh;
00483 this->_supporting_features_ids = supporting_feats_based_on_kh;
00484 this->_predicted_measurement = this->_predicted_measurement_pc_kh;
00485 this->_predicted_measurement_map = this->_predicted_measurement_map_kh;
00486 break;
00487 }
00488 default:
00489 break;
00490 }
00491 }
00492
00493 void RBFilter::addSupportingFeature(Feature::Id supporting_feat_id)
00494 {
00495 this->_supporting_features_ids.push_back(supporting_feat_id);
00496 }
00497
00498 void RBFilter::setPredictedState(const omip_msgs::RigidBodyPoseAndVelMsg hypothesis)
00499 {
00500 this->_use_predicted_state_from_kh = true;
00501
00502 Eigen::Twistd predicted_pose_kh_ec(hypothesis.pose_wc.twist.angular.x,
00503 hypothesis.pose_wc.twist.angular.y,
00504 hypothesis.pose_wc.twist.angular.z,
00505 hypothesis.pose_wc.twist.linear.x,
00506 hypothesis.pose_wc.twist.linear.y,
00507 hypothesis.pose_wc.twist.linear.z);
00508 this->_predicted_pose_kh = predicted_pose_kh_ec.exp(1e-12).toHomogeneousMatrix();
00509
00510 this->_predicted_velocity_kh = Eigen::Twistd(hypothesis.velocity_wc.twist.angular.x,
00511 hypothesis.velocity_wc.twist.angular.y,
00512 hypothesis.velocity_wc.twist.angular.z,
00513 hypothesis.velocity_wc.twist.linear.x,
00514 hypothesis.velocity_wc.twist.linear.y,
00515 hypothesis.velocity_wc.twist.linear.z);
00516 for(int i =0; i<6; i++)
00517 {
00518 for(int j=0; j<6; j++)
00519 {
00520 this->_predicted_pose_cov_kh(i,j) = hypothesis.pose_wc.covariance[6*i + j];
00521 this->_predicted_velocity_cov_kh(i,j) = hypothesis.velocity_wc.covariance[6*i + j];
00522 }
00523 }
00524 }
00525
00526 void RBFilter::integrateShapeBasedPose(const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns)
00527 {
00528 Eigen::Twistd shape_based_pose_ec = Eigen::Twistd(twist_refinement.twist.angular.x, twist_refinement.twist.angular.y, twist_refinement.twist.angular.z,
00529 twist_refinement.twist.linear.x, twist_refinement.twist.linear.y, twist_refinement.twist.linear.z);
00530
00531 if(pose_time_elapsed_ns < this->_loop_period_ns || _supporting_features_ids.size() < _min_num_supporting_feats_to_correct)
00532 {
00533 ROS_INFO_STREAM_NAMED("RBFilter.integrateShapeBasedPose", "Using shape-based pose (time elapsed=" << pose_time_elapsed_ns/1e9 <<
00534 " s, loop_period="<< _loop_period_ns/1e9 << " s)");
00535
00536
00537 this->_pose = shape_based_pose_ec.exp(1e-12).toHomogeneousMatrix();
00538
00539
00540 this->_trajectory.pop_back();
00541 this->_trajectory.push_back(this->_pose);
00542
00543 Eigen::Matrix4d delta_pose_ht = this->_trajectory[this->_trajectory.size()-1]*(this->_trajectory[this->_trajectory.size()-2].inverse());
00544 Eigen::Twistd delta_pose_ec;
00545 TransformMatrix2Twist(delta_pose_ht, delta_pose_ec);
00546 this->_velocity = delta_pose_ec/(this->_loop_period_ns/1e9);
00547 }else{
00548 ROS_INFO_STREAM_NAMED("RBFilter.integrateShapeBasedPose", "Shape-based pose is too old (time elapsed=" << pose_time_elapsed_ns/1e9 <<
00549 " s, loop_period="<< _loop_period_ns/1e9 << " s). Try reducing the framerate");
00550 }
00551 }
00552
00553 void RBFilter::addPredictedFeatureLocation(const Feature::Location& predicted_location_velocity,const Feature::Location& predicted_location_brake , const Feature::Id feature_id)
00554 {
00555 FeaturePCLwc predicted_feature_pcl;
00556
00557 LocationAndId2FeaturePCLwc(predicted_location_velocity, feature_id, predicted_feature_pcl);
00558 this->_predicted_measurement->points.push_back(predicted_feature_pcl);
00559 this->_predicted_measurement_map[feature_id] = predicted_location_velocity;
00560
00561 this->_predicted_measurement_pc_vh->points.push_back(predicted_feature_pcl);
00562 this->_predicted_measurement_map_vh[feature_id] = predicted_location_velocity;
00563
00564 this->_predicted_measurement_pc_kh->points.push_back(predicted_feature_pcl);
00565 this->_predicted_measurement_map_kh[feature_id] = predicted_location_brake;
00566
00567 LocationAndId2FeaturePCLwc(predicted_location_brake, feature_id, predicted_feature_pcl);
00568 this->_predicted_measurement_pc_bh->points.push_back(predicted_feature_pcl);
00569 this->_predicted_measurement_map_bh[feature_id] = predicted_location_brake;
00570 }
00571
00572 void RBFilter::setFeaturesDatabase(FeaturesDataBase::Ptr feats_database)
00573 {
00574 this->_features_database = feats_database;
00575 }
00576
00577 void RBFilter::PredictFeatureLocation(Feature::Ptr feature,
00578 const Eigen::Matrix4d& predicted_pose,
00579 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
00580 Feature::Location& predicted_location,
00581 bool publish_locations) const
00582 {
00583 Feature::Location location_at_birth_of_rb;
00584 this->GetLocationOfFeatureAtBirthOfRB(feature, contemporary_last_feat_loc_and_last_pose_in_trajectory, location_at_birth_of_rb);
00585
00586
00587 TransformLocation(location_at_birth_of_rb, predicted_pose, predicted_location);
00588
00589 if(publish_locations)
00590 {
00591 if(this->_id != 0)
00592 {
00593 Feature::Location at_birth_location = location_at_birth_of_rb + _initial_location_of_centroid;
00594 boost::shared_ptr<FeaturePCLwc> at_birth_feat(new FeaturePCLwc());
00595 at_birth_feat->x = boost::tuples::get<0>(at_birth_location);
00596 at_birth_feat->y = boost::tuples::get<1>(at_birth_location);
00597 at_birth_feat->z = boost::tuples::get<2>(at_birth_location);
00598 at_birth_feat->label = feature->getId();
00599
00600 _features_at_birth->points.push_back(*at_birth_feat);
00601 }else{
00602 boost::shared_ptr<FeaturePCLwc> at_birth_feat(new FeaturePCLwc());
00603 at_birth_feat->x = boost::tuples::get<0>(location_at_birth_of_rb);
00604 at_birth_feat->y = boost::tuples::get<1>(location_at_birth_of_rb);
00605 at_birth_feat->z = boost::tuples::get<2>(location_at_birth_of_rb);
00606 at_birth_feat->label = feature->getId();
00607
00608 _features_at_birth->points.push_back(*at_birth_feat);
00609 }
00610 boost::shared_ptr<FeaturePCLwc> predicted_feat(new FeaturePCLwc());
00611 predicted_feat->x = boost::tuples::get<0>(predicted_location);
00612 predicted_feat->y = boost::tuples::get<1>(predicted_location);
00613 predicted_feat->z = boost::tuples::get<2>(predicted_location);
00614 predicted_feat->label = feature->getId();
00615 _features_predicted->points.push_back(*predicted_feat);
00616 }
00617 }
00618
00619 void RBFilter::GetLocationOfFeatureAtBirthOfRB(Feature::Ptr feature,
00620 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
00621 Feature::Location& location_at_birth) const
00622 {
00623 const Feature::Trajectory& feat_traj = feature->getTrajectory();
00624
00625
00626 size_t feature_age = feature->getFeatureAge();
00627 size_t rigid_body_age = this->_trajectory.size();
00628
00629
00630
00631
00632 if(!contemporary_last_feat_loc_and_last_pose_in_trajectory)
00633 {
00634 rigid_body_age += 1;
00635 }
00636
00637
00638
00639
00640
00641
00642
00643 if (feature_age < rigid_body_age + 1)
00644 {
00645 Eigen::Matrix4d transform_from_rb_birth_to_feat_birth = this->_trajectory.at(rigid_body_age - feature_age);
00646
00647
00648 Eigen::Matrix4d inverse_transform_from_rb_birth_to_feat_birth = transform_from_rb_birth_to_feat_birth.inverse();
00649 TransformLocation(feat_traj.at(0), inverse_transform_from_rb_birth_to_feat_birth, location_at_birth);
00650 }
00651 else
00652 {
00653 location_at_birth = feat_traj.at(feature_age - (rigid_body_age + 1));
00654 if(this->_id != 0)
00655 {
00656 location_at_birth = location_at_birth - _initial_location_of_centroid;
00657 }
00658 }
00659
00660
00661 }
00662
00663 RB_id_t RBFilter::getId() const
00664 {
00665 return this->_id;
00666 }
00667
00668 Eigen::Matrix4d RBFilter::getPose() const
00669 {
00670 return _pose;
00671 }
00672
00673 Eigen::Matrix<double, 6, 6> RBFilter::getPoseCovariance() const
00674 {
00675 return _pose_covariance;
00676 }
00677
00678 Eigen::Twistd RBFilter::getVelocity() const
00679 {
00680 return _velocity;
00681 }
00682
00683 Eigen::Matrix<double, 6, 6> RBFilter::getVelocityCovariance() const
00684 {
00685 return _velocity_covariance;
00686 }
00687
00688 geometry_msgs::PoseWithCovariancePtr RBFilter::getPoseWithCovariance() const
00689 {
00690 geometry_msgs::PoseWithCovariancePtr pose_wc_ptr = geometry_msgs::PoseWithCovariancePtr(new geometry_msgs::PoseWithCovariance());
00691 pose_wc_ptr->pose.position.x = _pose(0,3);
00692 pose_wc_ptr->pose.position.y = _pose(1,3);
00693 pose_wc_ptr->pose.position.z = _pose(2,3);
00694 Eigen::Quaterniond quaternion(_pose.block<3,3>(0,0));
00695 pose_wc_ptr->pose.orientation.x = quaternion.x();
00696 pose_wc_ptr->pose.orientation.y = quaternion.y();
00697 pose_wc_ptr->pose.orientation.z = quaternion.z();
00698 pose_wc_ptr->pose.orientation.w = quaternion.w();
00699
00700 for (unsigned int i = 0; i < 6; i++)
00701 for (unsigned int j = 0; j < 6; j++)
00702 pose_wc_ptr->covariance[6 * i + j] = _pose_covariance(i, j);
00703
00704 return pose_wc_ptr;
00705 }
00706
00707 geometry_msgs::TwistWithCovariancePtr RBFilter::getPoseECWithCovariance() const
00708 {
00709 geometry_msgs::TwistWithCovariancePtr pose_wc_ptr = geometry_msgs::TwistWithCovariancePtr(new geometry_msgs::TwistWithCovariance());
00710 Eigen::Twistd pose_ec;
00711 TransformMatrix2Twist(_pose, pose_ec);
00712
00713 pose_wc_ptr->twist.linear.x = pose_ec.vx();
00714 pose_wc_ptr->twist.linear.y = pose_ec.vy();
00715 pose_wc_ptr->twist.linear.z = pose_ec.vz();
00716 pose_wc_ptr->twist.angular.x = pose_ec.rx();
00717 pose_wc_ptr->twist.angular.y = pose_ec.ry();
00718 pose_wc_ptr->twist.angular.z = pose_ec.rz();
00719
00720 for (unsigned int i = 0; i < 6; i++)
00721 for (unsigned int j = 0; j < 6; j++)
00722 pose_wc_ptr->covariance[6 * i + j] = _pose_covariance(i, j);
00723
00724 return pose_wc_ptr;
00725 }
00726
00727 geometry_msgs::TwistWithCovariancePtr RBFilter::getVelocityWithCovariance() const
00728 {
00729
00730 Eigen::Twistd velocity_copy = _velocity;
00731 geometry_msgs::TwistWithCovariancePtr vel_t_wc_ptr = geometry_msgs::TwistWithCovariancePtr(new geometry_msgs::TwistWithCovariance());
00732 vel_t_wc_ptr->twist.linear.x = _velocity.vx();
00733 vel_t_wc_ptr->twist.linear.y = _velocity.vy();
00734 vel_t_wc_ptr->twist.linear.z = _velocity.vz();
00735 vel_t_wc_ptr->twist.angular.x = velocity_copy.rx();
00736 vel_t_wc_ptr->twist.angular.y = velocity_copy.ry();
00737 vel_t_wc_ptr->twist.angular.z = velocity_copy.rz();
00738
00739 for (unsigned int i = 0; i < 6; i++)
00740 for (unsigned int j = 0; j < 6; j++)
00741 vel_t_wc_ptr->covariance[6 * i + j] = _velocity_covariance(i, j);
00742
00743 return vel_t_wc_ptr;
00744 }
00745
00746 std::vector<Eigen::Matrix4d> RBFilter::getTrajectory() const
00747 {
00748 return this->_trajectory;
00749 }
00750
00751 FeatureCloudPCLwc RBFilter::getPredictedMeasurement(PredictionHypothesis hypothesis)
00752 {
00753 FeatureCloudPCLwc predicted_measurement;
00754 switch(hypothesis)
00755 {
00756 case BASED_ON_VELOCITY:
00757 predicted_measurement = *(this->_predicted_measurement_pc_vh);
00758 break;
00759 case BASED_ON_BRAKING_EVENT:
00760 predicted_measurement = *(this->_predicted_measurement_pc_bh);
00761 break;
00762 default:
00763 ROS_ERROR_STREAM_NAMED("RBFilter::getPredictedMeasurement", "Wrong hypothesis. Value " << hypothesis);
00764 break;
00765 }
00766 return predicted_measurement;
00767 }
00768
00769 std::vector<Feature::Id> RBFilter::getSupportingFeatures() const
00770 {
00771 return this->_supporting_features_ids;
00772 }
00773
00774 int RBFilter::getNumberSupportingFeatures() const
00775 {
00776 return (int)this->_supporting_features_ids.size();
00777 }
00778
00779 FeaturesDataBase::Ptr RBFilter::getFeaturesDatabase() const
00780 {
00781 return this->_features_database;
00782 }
00783
00784 void RBFilter::_initializeAuxiliarMatrices()
00785 {
00786 _pose_covariance = Eigen::Matrix<double, 6, 6>::Zero();
00787 _velocity_covariance = Eigen::Matrix<double, 6, 6>::Zero();
00788 for (int i = 0; i < 6; i++)
00789 {
00790 _pose_covariance(i, i) = this->_prior_cov_pose;
00791 _velocity_covariance(i, i) = this->_prior_cov_vel;
00792 }
00793
00794 this->_predicted_measurement_pc_vh = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
00795 this->_predicted_measurement_pc_bh = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
00796 this->_predicted_measurement_pc_kh = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
00797 this->_features_at_birth= FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
00798 this->_features_predicted = FeatureCloudPCLwc::Ptr(new FeatureCloudPCLwc());
00799 this->_predicted_pose_vh = Eigen::Matrix4d::Identity();
00800 this->_predicted_pose_bh = Eigen::Matrix4d::Identity();
00801 this->_predicted_pose_kh = Eigen::Matrix4d::Identity();
00802
00803 _G_t_memory = Eigen::MatrixXd(6, 3*_num_tracked_feats);
00804 _R_inv_times_innovation_memory = Eigen::VectorXd(3*_num_tracked_feats);
00805
00806 _D_T_p0_circle = Eigen::Matrix<double, 3, 6>::Zero();
00807 _D_T_p0_circle.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
00808 _D_T_p0_circle.block<3,3>(0,3) = Eigen::Matrix3d::Zero();
00809 }