00001 /* 00002 * RigidJointFilter.h 00003 * 00004 * Author: roberto 00005 * 00006 * This is a modified implementation of the method for online estimation of kinematic structures described in our paper 00007 * "Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors" 00008 * (Martín-Martín and Brock, 2014). 00009 * This implementation can be used to reproduce the results of the paper and to be applied to new research. 00010 * The implementation allows also to be extended to perceive different information/models or to use additional sources of information. 00011 * A detail explanation of the method and the system can be found in our paper. 00012 * 00013 * If you are using this implementation in your research, please consider citing our work: 00014 * 00015 @inproceedings{martinmartin_ip_iros_2014, 00016 Title = {Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors}, 00017 Author = {Roberto {Mart\'in-Mart\'in} and Oliver Brock}, 00018 Booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems}, 00019 Pages = {2494-2501}, 00020 Year = {2014}, 00021 Location = {Chicago, Illinois, USA}, 00022 Note = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf}, 00023 Url = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf}, 00024 Projectname = {Interactive Perception} 00025 } 00026 * If you have questions or suggestions, contact us: 00027 * roberto.martinmartin@tu-berlin.de 00028 * 00029 * Enjoy! 00030 */ 00031 00032 #ifndef RIGIDJOINTFILTER_H_ 00033 #define RIGIDJOINTFILTER_H_ 00034 00035 #include "joint_tracker/JointFilter.h" 00036 00037 namespace omip 00038 { 00039 00040 class RigidJointFilter; 00041 typedef boost::shared_ptr<RigidJointFilter> RigidJointFilterPtr; 00042 00043 class RigidJointFilter : public JointFilter 00044 { 00045 public: 00046 00050 RigidJointFilter(); 00051 00055 virtual ~RigidJointFilter(); 00056 00063 RigidJointFilterPtr clone() const 00064 { 00065 return (RigidJointFilterPtr(doClone())); 00066 } 00067 00071 RigidJointFilter(const RigidJointFilter &rigid_joint); 00072 00077 virtual void predictMeasurement(); 00078 00083 virtual void estimateMeasurementHistoryLikelihood(); 00084 00089 virtual geometry_msgs::TwistWithCovariance getPredictedSRBPoseWithCovInSensorFrame(); 00090 00097 virtual geometry_msgs::TwistWithCovariance getPredictedSRBDeltaPoseWithCovInSensorFrame(); 00098 00105 virtual geometry_msgs::TwistWithCovariance getPredictedSRBVelocityWithCovInSensorFrame(); 00106 00107 00111 virtual std::vector<visualization_msgs::Marker> getJointMarkersInRRBFrame() const; 00112 00116 virtual JointFilterType getJointFilterType() const; 00117 00118 virtual void estimateUnnormalizedModelProbability() ; 00119 00123 virtual std::string getJointFilterTypeStr() const; 00124 00125 virtual void initialize(); 00126 00127 virtual void setMaxTranslationRigid(double max_trans); 00128 00129 virtual void setMaxRotationRigid(double max_rot); 00130 00131 protected: 00132 00139 virtual RigidJointFilter* doClone() const 00140 { 00141 return (new RigidJointFilter(*this)); 00142 } 00143 00144 double _rig_max_translation; 00145 double _rig_max_rotation; 00146 00147 double _motion_memory_prior; 00148 00149 }; 00150 00151 } 00152 00153 #endif /* RIGIDJOINTFILTER_H_ */