00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef RB_FILTER_H_
00033 #define RB_FILTER_H_
00034
00035 #include <rb_tracker/RBTrackerDynReconfConfig.h>
00036
00037 #include <pcl/point_types.h>
00038 #include <unsupported/Eigen/MatrixFunctions>
00039 #include <Eigen/Geometry>
00040 #include <lgsm/Lgsm>
00041 #include <boost/shared_ptr.hpp>
00042 #include <boost/unordered_map.hpp>
00043
00044 #include <ros/ros.h>
00045 #include <geometry_msgs/Pose.h>
00046 #include <geometry_msgs/Twist.h>
00047 #include <geometry_msgs/TwistWithCovariance.h>
00048 #include <geometry_msgs/PoseWithCovariance.h>
00049 #include <tf/tf.h>
00050 #include <tf/transform_datatypes.h>
00051 #include <tf/transform_broadcaster.h>
00052
00053 #include <tf_conversions/tf_eigen.h>
00054
00055 #include "omip_common/OMIPTypeDefs.h"
00056 #include "omip_common/Feature.h"
00057 #include "omip_common/FeaturesDataBase.h"
00058
00059
00060 #define STATE_DIM_RBF 6
00061
00062
00063 #define MEAS_DIM_RBF 3
00064
00065 #include <Eigen/Dense>
00066 #include <unsupported/Eigen/NonLinearOptimization>
00067 #include <unsupported/Eigen/NumericalDiff>
00068
00069
00088 namespace omip
00089 {
00090
00098 class RBFilter
00099 {
00100 public:
00101
00102 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00103
00104
00105 typedef boost::shared_ptr<RBFilter> Ptr;
00106
00107
00108 typedef std::vector<Eigen::Matrix4d> Trajectory;
00109
00110 enum PredictionHypothesis
00111 {
00112 BASED_ON_VELOCITY = 0,
00113 BASED_ON_BRAKING_EVENT = 1,
00114 BASED_ON_KINEMATICS = 2
00115 };
00116
00120 RBFilter();
00121
00130 RBFilter(double loop_period_ns,
00131 const std::vector<Eigen::Matrix4d> &trajectory,
00132 const Eigen::Twistd& initial_velocity,
00133 FeaturesDataBase::Ptr feats_database,
00134 double estimation_error_threshold);
00135
00136 virtual void Init();
00137
00141 virtual ~RBFilter();
00142
00147 RBFilter(const RBFilter &rbm);
00148
00153 RBFilter::Ptr clone() const
00154 {
00155 return (RBFilter::Ptr(doClone()));
00156 }
00157
00161 virtual void predictState(double time_interval_ns = -1.);
00162
00167 virtual void predictMeasurement();
00168
00172 virtual void correctState();
00173
00174 virtual void integrateShapeBasedPose(const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns);
00175
00180 virtual void estimateBestPredictionAndSupportingFeatures();
00181
00186 virtual void addSupportingFeature(Feature::Id supporting_feat_id);
00187
00193 virtual void setPredictedState(omip_msgs::RigidBodyPoseAndVelMsg hypothesis);
00194
00202 virtual void addPredictedFeatureLocation(const Feature::Location& predicted_location_velocity,
00203 const Feature::Location& predicted_location_brake, const Feature::Id feature_id);
00204
00213 virtual void PredictFeatureLocation(Feature::Ptr feature,
00214 const Eigen::Matrix4d &predicted_pose,
00215 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
00216 Feature::Location& predicted_location, bool publish_locations=false) const;
00217
00227 virtual void GetLocationOfFeatureAtBirthOfRB(Feature::Ptr feature,
00228 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
00229 Feature::Location& location_at_birth) const;
00230
00235 virtual void setFeaturesDatabase(FeaturesDataBase::Ptr feats_database);
00236
00241 virtual RB_id_t getId() const;
00242
00247 virtual Eigen::Matrix4d getPose() const;
00248
00254 virtual Eigen::Matrix<double, 6, 6> getPoseCovariance() const;
00255
00261 virtual Eigen::Twistd getVelocity() const;
00262
00268 virtual Eigen::Matrix<double, 6, 6> getVelocityCovariance() const;
00269
00274 virtual geometry_msgs::PoseWithCovariancePtr getPoseWithCovariance() const;
00275
00280 virtual geometry_msgs::TwistWithCovariancePtr getPoseECWithCovariance() const;
00281
00287 virtual geometry_msgs::TwistWithCovariancePtr getVelocityWithCovariance() const;
00288
00293 virtual std::vector<Eigen::Matrix4d> getTrajectory() const;
00294
00301 virtual FeatureCloudPCLwc getPredictedMeasurement(PredictionHypothesis hypothesis = BASED_ON_VELOCITY);
00302
00307 virtual std::vector<Feature::Id> getSupportingFeatures() const;
00308
00313 virtual int getNumberSupportingFeatures() const;
00314
00319 virtual FeaturesDataBase::Ptr getFeaturesDatabase() const;
00320
00327 virtual void setMotionConstraint(int motion_constraint){}
00328
00329 virtual void setPriorCovariancePose(double v)
00330 {
00331 this->_prior_cov_pose = v;
00332 }
00333
00334 virtual void setPriorCovarianceVelocity(double v)
00335 {
00336 this->_prior_cov_vel = v;
00337 }
00338
00339 virtual void setMinCovarianceMeasurementX(double v)
00340 {
00341 this->_min_cov_meas_x = v;
00342 }
00343
00344 virtual void setMinCovarianceMeasurementY(double v)
00345 {
00346 this->_min_cov_meas_y = v;
00347 }
00348
00349 virtual void setMinCovarianceMeasurementZ(double v)
00350 {
00351 this->_min_cov_meas_z = v;
00352 }
00353
00354 virtual void setMeasurementDepthFactor(double v)
00355 {
00356 this->_meas_depth_factor = v;
00357 }
00358
00359 virtual void setCovarianceSystemAccelerationTx(double v)
00360 {
00361 this->_cov_sys_acc_tx = v;
00362 }
00363
00364 virtual void setCovarianceSystemAccelerationTy(double v)
00365 {
00366 this->_cov_sys_acc_ty = v;
00367 }
00368
00369 virtual void setCovarianceSystemAccelerationTz(double v)
00370 {
00371 this->_cov_sys_acc_tz = v;
00372 }
00373
00374 virtual void setCovarianceSystemAccelerationRx(double v)
00375 {
00376 this->_cov_sys_acc_rx = v;
00377 }
00378
00379 virtual void setCovarianceSystemAccelerationRy(double v)
00380 {
00381 this->_cov_sys_acc_ry = v;
00382 }
00383
00384 virtual void setCovarianceSystemAccelerationRz(double v)
00385 {
00386 this->_cov_sys_acc_rz = v;
00387 }
00388
00389 virtual void setNumberOfTrackedFeatures(int v)
00390 {
00391 this->_num_tracked_feats = v;
00392 }
00393
00394 virtual void setMinNumberOfSupportingFeaturesToCorrectPredictedState(int v)
00395 {
00396 this->_min_num_supporting_feats_to_correct = v;
00397 }
00398
00399 virtual void setEstimationThreshold(double v)
00400 {
00401 this->_estimation_error_threshold = v;
00402 }
00403
00404 virtual Feature::Location getIntialLocationOfCentroid() const
00405 {
00406 return _initial_location_of_centroid;
00407 }
00408
00409 virtual void doNotUsePredictionFromHigherLevel()
00410 {
00411 _use_predicted_state_from_kh = false;
00412 }
00413
00414 FeatureCloudPCLwc::Ptr getFeaturesAtBirth()
00415 {
00416 return _features_at_birth;
00417 }
00418
00419 FeatureCloudPCLwc::Ptr getFeaturesPredicted()
00420 {
00421 return _features_predicted;
00422 }
00423
00424 void resetFeaturesAtBirth()
00425 {
00426 _features_at_birth->points.clear();
00427 }
00428
00429 void resetFeaturesPredicted()
00430 {
00431 _features_predicted->points.clear();
00432 }
00433
00434 protected:
00435
00447 virtual void _initializeAuxiliarMatrices();
00448
00449
00450
00451 double _estimation_error_threshold;
00452
00453 std::map<Feature::Id, double> _supporting_feats_errors;
00454
00455 double _prior_cov_pose;
00456 double _prior_cov_vel;
00457 double _min_cov_meas_x;
00458 double _min_cov_meas_y;
00459 double _min_cov_meas_z;
00460 double _meas_depth_factor;
00461 double _cov_sys_acc_tx;
00462 double _cov_sys_acc_ty;
00463 double _cov_sys_acc_tz;
00464 double _cov_sys_acc_ry;
00465 double _cov_sys_acc_rx;
00466 double _cov_sys_acc_rz;
00467
00468
00469 Eigen::MatrixXd _G_t_memory;
00470 Eigen::VectorXd _R_inv_times_innovation_memory;
00471
00472 Eigen::Matrix<double, 3, 6> _D_T_p0_circle;
00473
00474 static RB_id_t _rb_id_generator;
00475
00476 RB_id_t _id;
00477
00478
00479 Eigen::Matrix4d _pose;
00480 Eigen::Matrix<double, 6, 6> _pose_covariance;
00481
00482 Eigen::Twistd _velocity;
00483 Eigen::Matrix<double, 6, 6> _velocity_covariance;
00484
00485 Eigen::Matrix4d _predicted_pose;
00486 Eigen::Matrix<double, 6, 6> _predicted_pose_covariance;
00487
00488 Eigen::Twistd _predicted_velocity;
00489 Eigen::Matrix<double, 6, 6> _predicted_velocity_covariance;
00490
00491 Feature::Location _initial_location_of_centroid;
00492
00493 std::vector<Eigen::Matrix4d> _trajectory;
00494
00495 std::vector<Feature::Id> _supporting_features_ids;
00496 std::vector<double> _supporting_features_probs;
00497
00498 FeaturesDataBase::Ptr _features_database;
00499
00500
00501 Eigen::Matrix4d _predicted_pose_vh;
00502 Eigen::Matrix4d _predicted_pose_bh;
00503 Eigen::Matrix4d _predicted_delta_pose_kh;
00504 Eigen::Matrix4d _predicted_pose_kh;
00505
00506 Eigen::Matrix<double, 6, 6> _predicted_pose_cov_vh;
00507 Eigen::Matrix<double, 6, 6> _predicted_pose_cov_bh;
00508 Eigen::Matrix<double, 6, 6> _predicted_pose_cov_kh;
00509
00510 Eigen::Twistd _predicted_velocity_vh;
00511 Eigen::Twistd _predicted_velocity_bh;
00512 Eigen::Twistd _predicted_velocity_kh;
00513
00514 Eigen::Matrix<double, 6, 6> _predicted_velocity_cov_vh;
00515 Eigen::Matrix<double, 6, 6> _predicted_velocity_cov_bh;
00516 Eigen::Matrix<double, 6, 6> _predicted_velocity_cov_kh;
00517
00518 bool _use_predicted_state_from_kh;
00519 bool _use_predicted_measurement_from_kh;
00520
00521 FeatureCloudPCLwc::Ptr _features_predicted;
00522 FeatureCloudPCLwc::Ptr _features_at_birth;
00523
00524 FeatureCloudPCLwc::Ptr _predicted_measurement_pc_vh;
00525 FeatureCloudPCLwc::Ptr _predicted_measurement_pc_bh;
00526 FeatureCloudPCLwc::Ptr _predicted_measurement_pc_kh;
00527
00528 FeatureCloudPCLwc::Ptr _predicted_measurement;
00529
00530 std::map<Feature::Id, Feature::Location> _predicted_measurement_map_vh;
00531 std::map<Feature::Id, Feature::Location> _predicted_measurement_map_bh;
00532 std::map<Feature::Id, Feature::Location> _predicted_measurement_map_kh;
00533
00534 std::map<Feature::Id, Feature::Location> _predicted_measurement_map;
00535
00536 double _loop_period_ns;
00537 double _last_time_interval_ns;
00538
00539 virtual RBFilter* doClone() const
00540 {
00541 return (new RBFilter(*this));
00542 }
00543
00544 int _num_tracked_feats;
00545 int _min_num_supporting_feats_to_correct;
00546
00547 };
00548
00549 }
00550
00551 #endif