|
virtual void | addSupportingFeature (Feature::Id supporting_feat_id) |
|
virtual void | constrainMotion () |
| Function that constrains the last estimated pose of the static environment to the camera according to the motion constraint. More...
|
|
virtual void | correctState () |
|
virtual void | estimateDeltaMotion (std::vector< Feature::Id > &supporting_features_ids, tf::Transform &previous_current_Tf) |
| Estimate the motion between the last 2 frames of the set of supporting features, assuming they are all on the static environment. More...
|
|
virtual void | Init () |
|
virtual void | iterativeICP (pcl::PointCloud< pcl::PointXYZ > &previous_locations, pcl::PointCloud< pcl::PointXYZ > ¤t_locations, tf::Transform &previous_current_Tf) |
| Estimate the transformation between 2 sets of feature locations by iteratively estimating ICP until either the maximum number of iterations is reached or the iterations do not change the transformation (convergence) More...
|
|
virtual void | predictState (double time_interval_ns) |
|
virtual void | setComputationType (static_environment_tracker_t computation_type) |
| Set the computation type for the static environment Options: ICP based (t-1 to t) or EKF based (similar to any other RB) More...
|
|
virtual void | setMotionConstraint (int motion_constraint) |
| Set a constraint for the motion of the RB Right now this is only used by the StaticEnvironmentFilter. More...
|
|
| StaticEnvironmentFilter (double loop_period_ns, FeaturesDataBase::Ptr feats_database, double environment_motion_threshold) |
|
virtual | ~StaticEnvironmentFilter () |
|
virtual void | addPredictedFeatureLocation (const Feature::Location &predicted_location_velocity, const Feature::Location &predicted_location_brake, const Feature::Id feature_id) |
|
RBFilter::Ptr | clone () const |
|
virtual void | doNotUsePredictionFromHigherLevel () |
|
virtual void | estimateBestPredictionAndSupportingFeatures () |
|
FeatureCloudPCLwc::Ptr | getFeaturesAtBirth () |
|
virtual FeaturesDataBase::Ptr | getFeaturesDatabase () const |
|
FeatureCloudPCLwc::Ptr | getFeaturesPredicted () |
|
virtual RB_id_t | getId () const |
|
virtual Feature::Location | getIntialLocationOfCentroid () const |
|
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 "younger" we use the previous motion of the RB to estimate what would have been the location of the feature at the initial frame. More...
|
|
virtual int | getNumberSupportingFeatures () const |
|
virtual Eigen::Matrix4d | getPose () const |
|
virtual Eigen::Matrix< double, 6, 6 > | getPoseCovariance () const |
|
virtual geometry_msgs::TwistWithCovariancePtr | getPoseECWithCovariance () const |
|
virtual geometry_msgs::PoseWithCovariancePtr | getPoseWithCovariance () const |
|
virtual FeatureCloudPCLwc | getPredictedMeasurement (PredictionHypothesis hypothesis=BASED_ON_VELOCITY) |
|
virtual std::vector< Feature::Id > | getSupportingFeatures () const |
|
virtual std::vector< Eigen::Matrix4d > | getTrajectory () const |
|
virtual Eigen::Twistd | getVelocity () const |
|
virtual Eigen::Matrix< double, 6, 6 > | getVelocityCovariance () const |
|
virtual geometry_msgs::TwistWithCovariancePtr | getVelocityWithCovariance () const |
|
virtual void | integrateShapeBasedPose (const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns) |
|
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 |
|
virtual void | predictMeasurement () |
| Second step when updating the filter. The next measurement is predicted from the predicted next state. More...
|
|
| RBFilter () |
|
| RBFilter (double loop_period_ns, const std::vector< Eigen::Matrix4d > &trajectory, const Eigen::Twistd &initial_velocity, FeaturesDataBase::Ptr feats_database, double estimation_error_threshold) |
|
| RBFilter (const RBFilter &rbm) |
|
void | resetFeaturesAtBirth () |
|
void | resetFeaturesPredicted () |
|
virtual void | setCovarianceSystemAccelerationRx (double v) |
|
virtual void | setCovarianceSystemAccelerationRy (double v) |
|
virtual void | setCovarianceSystemAccelerationRz (double v) |
|
virtual void | setCovarianceSystemAccelerationTx (double v) |
|
virtual void | setCovarianceSystemAccelerationTy (double v) |
|
virtual void | setCovarianceSystemAccelerationTz (double v) |
|
virtual void | setEstimationThreshold (double v) |
|
virtual void | setFeaturesDatabase (FeaturesDataBase::Ptr feats_database) |
|
virtual void | setMeasurementDepthFactor (double v) |
|
virtual void | setMinCovarianceMeasurementX (double v) |
|
virtual void | setMinCovarianceMeasurementY (double v) |
|
virtual void | setMinCovarianceMeasurementZ (double v) |
|
virtual void | setMinNumberOfSupportingFeaturesToCorrectPredictedState (int v) |
|
virtual void | setNumberOfTrackedFeatures (int v) |
|
virtual void | setPredictedState (omip_msgs::RigidBodyPoseAndVelMsg hypothesis) |
|
virtual void | setPriorCovariancePose (double v) |
|
virtual void | setPriorCovarianceVelocity (double v) |
|
virtual | ~RBFilter () |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< RBFilter > | Ptr |
|
virtual void | _initializeAuxiliarMatrices () |
|
virtual RBFilter * | doClone () const |
|
double | _cov_sys_acc_rx |
|
double | _cov_sys_acc_ry |
|
double | _cov_sys_acc_rz |
|
double | _cov_sys_acc_tx |
|
double | _cov_sys_acc_ty |
|
double | _cov_sys_acc_tz |
|
Eigen::Matrix< double, 3, 6 > | _D_T_p0_circle |
|
double | _estimation_error_threshold |
|
FeatureCloudPCLwc::Ptr | _features_at_birth |
|
FeaturesDataBase::Ptr | _features_database |
|
FeatureCloudPCLwc::Ptr | _features_predicted |
|
Eigen::MatrixXd | _G_t_memory |
|
RB_id_t | _id |
|
Feature::Location | _initial_location_of_centroid |
|
double | _last_time_interval_ns |
|
double | _loop_period_ns |
|
double | _meas_depth_factor |
|
double | _min_cov_meas_x |
|
double | _min_cov_meas_y |
|
double | _min_cov_meas_z |
|
int | _min_num_supporting_feats_to_correct |
|
int | _num_tracked_feats |
|
Eigen::Matrix4d | _pose |
|
Eigen::Matrix< double, 6, 6 > | _pose_covariance |
|
Eigen::Matrix4d | _predicted_delta_pose_kh |
|
FeatureCloudPCLwc::Ptr | _predicted_measurement |
|
std::map< Feature::Id, Feature::Location > | _predicted_measurement_map |
|
std::map< Feature::Id, Feature::Location > | _predicted_measurement_map_bh |
|
std::map< Feature::Id, Feature::Location > | _predicted_measurement_map_kh |
|
std::map< Feature::Id, Feature::Location > | _predicted_measurement_map_vh |
|
FeatureCloudPCLwc::Ptr | _predicted_measurement_pc_bh |
|
FeatureCloudPCLwc::Ptr | _predicted_measurement_pc_kh |
|
FeatureCloudPCLwc::Ptr | _predicted_measurement_pc_vh |
|
Eigen::Matrix4d | _predicted_pose |
|
Eigen::Matrix4d | _predicted_pose_bh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_pose_cov_bh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_pose_cov_kh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_pose_cov_vh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_pose_covariance |
|
Eigen::Matrix4d | _predicted_pose_kh |
|
Eigen::Matrix4d | _predicted_pose_vh |
|
Eigen::Twistd | _predicted_velocity |
|
Eigen::Twistd | _predicted_velocity_bh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_velocity_cov_bh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_velocity_cov_kh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_velocity_cov_vh |
|
Eigen::Matrix< double, 6, 6 > | _predicted_velocity_covariance |
|
Eigen::Twistd | _predicted_velocity_kh |
|
Eigen::Twistd | _predicted_velocity_vh |
|
double | _prior_cov_pose |
|
double | _prior_cov_vel |
|
Eigen::VectorXd | _R_inv_times_innovation_memory |
|
std::map< Feature::Id, double > | _supporting_feats_errors |
|
std::vector< Feature::Id > | _supporting_features_ids |
|
std::vector< double > | _supporting_features_probs |
|
std::vector< Eigen::Matrix4d > | _trajectory |
|
bool | _use_predicted_measurement_from_kh |
|
bool | _use_predicted_state_from_kh |
|
Eigen::Twistd | _velocity |
|
Eigen::Matrix< double, 6, 6 > | _velocity_covariance |
|
static RB_id_t | _rb_id_generator = 1 |
|
Class StaticEnvironmentICPFilter Represents an special type of RBFilter that tracks the motion of the static environment wrt to the camera Features of the environment that are not moving in the environment support this filter and should be used to estimate the motion of the environment wrt to the camera
Definition at line 63 of file StaticEnvironmentFilter.h.