00001 /* 00002 * DisconnectedJointFilter.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 DISCONNECTEDJOINTFILTER_H_ 00033 #define DISCONNECTEDJOINTFILTER_H_ 00034 00035 #include "joint_tracker/JointFilter.h" 00036 00037 namespace omip 00038 { 00039 00040 class DisconnectedJointFilter; 00041 typedef boost::shared_ptr<DisconnectedJointFilter> DisconnectedJointFilterPtr; 00042 00043 class DisconnectedJointFilter : public JointFilter 00044 { 00045 public: 00046 00050 DisconnectedJointFilter(); 00051 00055 virtual ~DisconnectedJointFilter(); 00056 00063 DisconnectedJointFilterPtr clone() const 00064 { 00065 return (DisconnectedJointFilterPtr(doClone())); 00066 } 00067 00071 DisconnectedJointFilter(const DisconnectedJointFilter &rigid_joint); 00072 00077 virtual geometry_msgs::TwistWithCovariance getPredictedSRBPoseWithCovInSensorFrame(); 00078 00085 virtual geometry_msgs::TwistWithCovariance getPredictedSRBDeltaPoseWithCovInSensorFrame(); 00086 00093 virtual geometry_msgs::TwistWithCovariance getPredictedSRBVelocityWithCovInSensorFrame(); 00094 00098 virtual std::vector<visualization_msgs::Marker> getJointMarkersInRRBFrame() const; 00099 00103 virtual JointFilterType getJointFilterType() const; 00104 00108 virtual std::string getJointFilterTypeStr() const; 00109 00116 virtual double getProbabilityOfJointFilter() const; 00117 00118 virtual void initialize(); 00119 00120 protected: 00121 00128 // Disconnected Joint is the only model where we ALWAYS know the normalized probability of the model given the data, because 00129 // it is a value that we use as quality threshold for all other joints (the normalized probability of any other joint should be higher than this 00130 // constant value to be selected as most probable joint) 00131 //double _unnormalized_disc_joint_probability; 00132 00133 virtual DisconnectedJointFilter* doClone() const 00134 { 00135 return (new DisconnectedJointFilter(*this)); 00136 } 00137 00138 }; 00139 00140 } 00141 00142 #endif /* DISCONNECTEDJOINTFILTER_H_ */