Go to the documentation of this file.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 STATIC_ENVIRONMENT_FILTER_H_
00033 #define STATIC_ENVIRONMENT_FILTER_H_
00034
00035 #include "rb_tracker/RBFilter.h"
00036 #include "omip_common/OMIPTypeDefs.h"
00037 #include <pcl/registration/transformation_estimation_svd.h>
00038 #include <tf/transform_listener.h>
00039
00040 namespace omip
00041 {
00042
00043 typedef enum
00044 {
00045 NO_CONSTRAINED = 0,
00046 NO_ROLL_PITCH = 1,
00047 NO_ROLL_PITCH_TZ = 2,
00048 NO_TRANSLATION = 3,
00049 NO_TRANSLATION_ROLL_YAW = 4,
00050 NO_ROTATION = 5,
00051 NO_MOTION = 6,
00052 ROBOT_XY_BASELINK_PLANE = 7
00053 }
00054 MotionConstraint;
00055
00063 class StaticEnvironmentFilter : public RBFilter
00064 {
00065 public:
00066
00067
00068 typedef boost::shared_ptr<StaticEnvironmentFilter> Ptr;
00069
00074 StaticEnvironmentFilter(double loop_period_ns, FeaturesDataBase::Ptr feats_database, double environment_motion_threshold);
00075
00079 virtual ~StaticEnvironmentFilter(){}
00080
00086 virtual void predictState(double time_interval_ns);
00087
00093 virtual void correctState();
00094
00101 virtual void addSupportingFeature(Feature::Id supporting_feat_id);
00102
00109 virtual void setMotionConstraint(int motion_constraint);
00110
00119 virtual void estimateDeltaMotion(std::vector<Feature::Id>& supporting_features_ids, tf::Transform& previous_current_Tf);
00120
00130 virtual void iterativeICP( pcl::PointCloud<pcl::PointXYZ>& previous_locations,
00131 pcl::PointCloud<pcl::PointXYZ>& current_locations, tf::Transform& previous_current_Tf);
00132
00137 virtual void constrainMotion();
00138
00145 virtual void setComputationType(static_environment_tracker_t computation_type);
00146
00147 virtual void Init();
00148
00149 private:
00150
00151 double _environment_motion_threshold;
00152 int _motion_constraint;
00153
00154 int _max_iterations;
00155 double _tf_epsilon_linear;
00156 double _tf_epsilon_angular;
00157
00158 ros::NodeHandle _nh;
00159 tf::TransformListener _tf_listener;
00160
00161 static_environment_tracker_t _computation_type;
00162
00163 pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> _svdecomposer;
00164 };
00165 }
00166
00167 #endif