35 #include <rb_tracker/RBTrackerDynReconfConfig.h> 37 #include <pcl/point_types.h> 38 #include <unsupported/Eigen/MatrixFunctions> 39 #include <Eigen/Geometry> 41 #include <boost/shared_ptr.hpp> 42 #include <boost/unordered_map.hpp> 45 #include <geometry_msgs/Pose.h> 46 #include <geometry_msgs/Twist.h> 47 #include <geometry_msgs/TwistWithCovariance.h> 48 #include <geometry_msgs/PoseWithCovariance.h> 53 #include <tf_conversions/tf_eigen.h> 60 #define STATE_DIM_RBF 6 63 #define MEAS_DIM_RBF 3 65 #include <Eigen/Dense> 66 #include <unsupported/Eigen/NonLinearOptimization> 67 #include <unsupported/Eigen/NumericalDiff> 102 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
131 const std::vector<Eigen::Matrix4d> &trajectory,
132 const Eigen::Twistd& initial_velocity,
134 double estimation_error_threshold);
161 virtual void predictState(
double time_interval_ns = -1.);
174 virtual void integrateShapeBasedPose(
const geometry_msgs::TwistWithCovariance twist_refinement,
double pose_time_elapsed_ns);
214 const Eigen::Matrix4d &predicted_pose,
215 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
228 bool contemporary_last_feat_loc_and_last_pose_in_trajectory,
247 virtual Eigen::Matrix4d
getPose()
const;
FeatureCloudPCLwc::Ptr _predicted_measurement_pc_bh
virtual void setPredictedState(omip_msgs::RigidBodyPoseAndVelMsg hypothesis)
virtual void estimateBestPredictionAndSupportingFeatures()
std::map< Feature::Id, Feature::Location > _predicted_measurement_map_vh
virtual void setMinCovarianceMeasurementX(double v)
Eigen::Matrix< double, 6, 6 > _predicted_velocity_cov_vh
virtual void setPriorCovarianceVelocity(double v)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< RBFilter > Ptr
boost::tuple< double, double, double > Location
FeatureCloudPCLwc::Ptr _predicted_measurement
double _last_time_interval_ns
virtual std::vector< Feature::Id > getSupportingFeatures() const
virtual void setCovarianceSystemAccelerationTy(double v)
virtual std::vector< Eigen::Matrix4d > getTrajectory() const
virtual void setCovarianceSystemAccelerationRz(double v)
virtual void addSupportingFeature(Feature::Id supporting_feat_id)
virtual Eigen::Matrix< double, 6, 6 > getPoseCovariance() const
virtual geometry_msgs::PoseWithCovariancePtr getPoseWithCovariance() const
virtual void setFeaturesDatabase(FeaturesDataBase::Ptr feats_database)
virtual FeatureCloudPCLwc getPredictedMeasurement(PredictionHypothesis hypothesis=BASED_ON_VELOCITY)
FeatureCloudPCLwc::Ptr _predicted_measurement_pc_vh
virtual void setMeasurementDepthFactor(double v)
Eigen::Matrix< double, 6, 6 > _predicted_velocity_covariance
virtual void correctState()
Eigen::Twistd _predicted_velocity
bool _use_predicted_state_from_kh
virtual void setMinCovarianceMeasurementZ(double v)
virtual geometry_msgs::TwistWithCovariancePtr getPoseECWithCovariance() const
std::map< Feature::Id, Feature::Location > _predicted_measurement_map
std::vector< Eigen::Matrix4d > Trajectory
Eigen::Matrix4d _predicted_pose
Eigen::VectorXd _R_inv_times_innovation_memory
Eigen::Matrix< double, 6, 6 > _predicted_velocity_cov_kh
virtual void setCovarianceSystemAccelerationRy(double v)
FeatureCloudPCLwc::Ptr _predicted_measurement_pc_kh
virtual Eigen::Matrix4d getPose() const
FeatureCloudPCLwc::Ptr _features_at_birth
Feature::Location _initial_location_of_centroid
void resetFeaturesAtBirth()
virtual void setMotionConstraint(int motion_constraint)
Set a constraint for the motion of the RB Right now this is only used by the StaticEnvironmentFilter...
Eigen::Twistd _predicted_velocity_bh
virtual void integrateShapeBasedPose(const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns)
static RB_id_t _rb_id_generator
Eigen::Matrix4d _predicted_delta_pose_kh
virtual void setNumberOfTrackedFeatures(int v)
std::map< Feature::Id, Feature::Location > _predicted_measurement_map_bh
virtual void setMinNumberOfSupportingFeaturesToCorrectPredictedState(int v)
virtual void _initializeAuxiliarMatrices()
virtual void setEstimationThreshold(double v)
FeatureCloudPCLwc::Ptr _features_predicted
std::map< Feature::Id, double > _supporting_feats_errors
std::vector< Feature::Id > _supporting_features_ids
Eigen::Matrix< double, 3, 6 > _D_T_p0_circle
virtual void setMinCovarianceMeasurementY(double v)
virtual void GetLocationOfFeatureAtBirthOfRB(Feature::Ptr feature, bool contemporary_last_feat_loc_and_last_pose_in_trajectory, Feature::Location &location_at_birth) const
Get the location of the feature at the frame when the RB was detected first. If the feature is "young...
Eigen::Matrix4d _predicted_pose_kh
bool _use_predicted_measurement_from_kh
Eigen::Matrix< double, 6, 6 > _predicted_velocity_cov_bh
RBFilter::Ptr clone() const
FeatureCloudPCLwc::Ptr getFeaturesPredicted()
virtual geometry_msgs::TwistWithCovariancePtr getVelocityWithCovariance() const
virtual void setCovarianceSystemAccelerationRx(double v)
Eigen::Matrix< double, 6, 6 > _predicted_pose_covariance
Eigen::Matrix< double, 6, 6 > _pose_covariance
FeaturesDataBase::Ptr _features_database
virtual FeaturesDataBase::Ptr getFeaturesDatabase() const
virtual Eigen::Matrix< double, 6, 6 > getVelocityCovariance() const
Eigen::Twistd _predicted_velocity_kh
double _estimation_error_threshold
Eigen::Matrix< double, 6, 6 > _predicted_pose_cov_bh
virtual RB_id_t getId() const
virtual void predictState(double time_interval_ns=-1.)
int _min_num_supporting_feats_to_correct
virtual Feature::Location getIntialLocationOfCentroid() const
double _meas_depth_factor
virtual void setCovarianceSystemAccelerationTz(double v)
virtual RBFilter * doClone() const
void resetFeaturesPredicted()
std::vector< Eigen::Matrix4d > _trajectory
virtual Eigen::Twistd getVelocity() const
pcl::PointCloud< FeaturePCLwc > FeatureCloudPCLwc
Eigen::Matrix4d _predicted_pose_bh
std::vector< double > _supporting_features_probs
virtual int getNumberSupportingFeatures() const
virtual void setCovarianceSystemAccelerationTx(double v)
std::map< Feature::Id, Feature::Location > _predicted_measurement_map_kh
virtual void addPredictedFeatureLocation(const Feature::Location &predicted_location_velocity, const Feature::Location &predicted_location_brake, const Feature::Id feature_id)
Eigen::Twistd _predicted_velocity_vh
FeatureCloudPCLwc::Ptr getFeaturesAtBirth()
Eigen::Matrix4d _predicted_pose_vh
Eigen::MatrixXd _G_t_memory
Eigen::Matrix< double, 6, 6 > _predicted_pose_cov_kh
Eigen::Matrix< double, 6, 6 > _velocity_covariance
virtual void setPriorCovariancePose(double v)
virtual void predictMeasurement()
Second step when updating the filter. The next measurement is predicted from the predicted next state...
virtual void PredictFeatureLocation(Feature::Ptr feature, const Eigen::Matrix4d &predicted_pose, bool contemporary_last_feat_loc_and_last_pose_in_trajectory, Feature::Location &predicted_location, bool publish_locations=false) const
Eigen::Matrix< double, 6, 6 > _predicted_pose_cov_vh
virtual void doNotUsePredictionFromHigherLevel()