00001
00059
00060
00061
00062
00063
00064
00065
00066 #ifndef SCP_OBJCTXT_H_
00067 #define SCP_OBJCTXT_H_
00068
00069 #include "../dof/tflink.h"
00070 #include "object.h"
00071 #include "bb.h"
00072
00073 namespace Slam_CurvedPolygon
00074 {
00075 template<typename _DOF6>
00076 class OBJCTXT
00077 {
00078 public:
00079 typedef boost::shared_ptr<OBJCTXT> Ptr;
00080 typedef _DOF6 DOF6;
00081 typedef Slam_CurvedPolygon::Object<DOF6> OBJECT;
00082
00083
00084 typedef class CtxtBB : public BoundingBox::FoVBB<typename DOF6::TYPE>
00085 {
00086 public:
00087 void update(const OBJCTXT &ctxt) {
00088 this->min_dist_ = this->min_x_ = this->min_y_ = std::numeric_limits<typename DOF6::TYPE>::max();
00089 this->max_dist_ = this->max_x_ = this->max_y_ = -std::numeric_limits<typename DOF6::TYPE>::max();
00090
00091 for(size_t i=0; i<ctxt.getObjs().size(); i++) {
00092 for(size_t j=0; j<ctxt.getObjs()[i]->getData().getPoints3D().size(); j++) {
00093 const Eigen::Vector3f &v = ctxt.getObjs()[i]->getData().getPoints3D()[j];
00094 this->min_dist_ = std::min(this->min_dist_, v(2));
00095 this->min_x_ = std::min(this->min_x_, v(0)/v(2));
00096 this->min_y_ = std::min(this->min_y_, v(1)/v(2));
00097 this->max_dist_ = std::max(this->max_dist_, v(2));
00098 this->max_x_ = std::max(this->max_x_, v(0)/v(2));
00099 this->max_y_ = std::max(this->max_y_, v(1)/v(2));
00100 }
00101 }
00102
00103 ROS_INFO("BB (%f %f) (%f %f) (%f %f)",this->min_dist_,this->max_dist_, this->min_x_,this->max_x_, this->min_y_,this->max_y_);
00104 }
00105 } BB;
00106
00107 private:
00108
00109 struct SCOR
00110 {
00111 typename OBJECT::Ptr a,b;
00112 bool used_;
00113 float prob;
00114 };
00115
00116 struct SCOR_DISTANCES;
00117
00118 struct SCOR_MEMBER
00119 {
00120 typedef boost::shared_ptr<SCOR_MEMBER> Ptr;
00121 typedef std::map<SCOR_MEMBER::Ptr, std::vector<typename DOF6::TYPE> > MAP;
00122
00123 typename OBJECT::Ptr obj_;
00124 std::vector<SCOR_MEMBER::Ptr> candidates_;
00125 MAP distances_;
00126 };
00127
00128
00129 std::vector<typename OBJECT::Ptr> objs_;
00130 std::vector<SCOR> used_cors_;
00131 std::map<typename OBJECT::Ptr,std::vector<size_t> > map_cors_;
00132 size_t frames_;
00133 BB bb_;
00134
00135
00136 bool enabled_all_merge_;
00137
00138 typename DOF6::TYPE check_assumption(typename SCOR_MEMBER::Ptr m1, typename SCOR_MEMBER::Ptr m2) const;
00139
00145 void findCorrespondences1(const OBJCTXT &ctxt, std::list<SCOR> &cors,
00146 const DOF6 &tf) const;
00147 void findCorrespondences3(const OBJCTXT &ctxt, std::vector<SCOR> &cors,
00148 const DOF6 &tf);
00149
00150
00156 DOF6 optimizeLink1(const DOF6 &tf, std::list<SCOR> &cors, const typename DOF6::TYPE &thr_rot, const typename DOF6::TYPE &thr_tr , const Eigen::Matrix3f &rot, const Eigen::Vector3f &tr,
00157 const int depth=0) const;
00158
00164 DOF6 optimizeLink2(const DOF6 &tf, std::list<SCOR> &cors, const typename DOF6::TYPE &thr_rot, const typename DOF6::TYPE &thr_tr , const Eigen::Matrix3f &rot, const Eigen::Vector3f &tr,
00165 const int depth=0) const;
00166
00167 DOF6 optimizeLink3(const OBJCTXT &ctxt, std::vector<SCOR> &cors, const DOF6 &tf, const typename DOF6::TYPE &thr_rot, const typename DOF6::TYPE &thr_tr , const Eigen::Matrix3f &rot, const Eigen::Vector3f &tr,
00168 const int depth=0) const;
00169
00170
00172 void filter();
00173
00175 void updateBB();
00176
00177 public:
00178
00179 OBJCTXT():frames_(0), enabled_all_merge_(true)
00180 {}
00181
00182 void operator+=(typename OBJECT::Ptr obj) {
00183
00184
00185 objs_.push_back(obj);
00186 }
00187
00188 void operator+=(const OBJCTXT &o) {
00189 for(size_t i=0; i<o.objs_.size(); i++)
00190 objs_.push_back(o.objs_[i]);
00191 updateBB();
00192 }
00193
00194 void clear() {objs_.clear();}
00195
00196 bool registration(const OBJCTXT &ctxt, DOF6 &tf, typename DOF6::TYPE &probability_success_rate, typename DOF6::TYPE &probability_error_rate);
00197
00198 bool merge(const OBJCTXT &ctxt, const DOF6 &tf, std::map<typename OBJECT::Ptr,bool> &used, const BoundingBox::TransformedFoVBB &fov, const bool only_merge);
00199
00201 void update();
00202
00203 size_t getNumObjs() const {return objs_.size();}
00204
00205 const std::vector<typename OBJECT::Ptr> &getObjs() const {return objs_;}
00206
00207 size_t getFramesProcessed() const {return frames_;}
00208
00209 const BB &getBoundingBox() const {return bb_;}
00210
00211 bool empty() const {return objs_.size()==0;}
00212
00213 typename OBJCTXT::Ptr clone() const;
00214 OBJCTXT &transform(const DOF6 &tf);
00215
00216 };
00217
00218 #include "impl/objctxt.hpp"
00219 #include "impl/registration.hpp"
00220
00221 }
00222
00223
00224
00225 #endif