cob_3d_mapping_slam: node.hpp Source File

Go to the documentation of this file.
00001 
00060 template<typename OBJECT_CONTEXT>
00061 bool Node<OBJECT_CONTEXT>::registration(const OBJCTXT &ctxt, DOF6 &tf, typename DOF6::TYPE &probability_success_rate, typename DOF6::TYPE &probability_error_rate)
00062 {
00063   //1. find correspondences
00064   //2. register
00065   return ctxt_.registration(ctxt, tf, probability_success_rate, probability_error_rate);
00066 }
00067 
00068 
00069 template<typename OBJECT_CONTEXT>
00070 bool Node<OBJECT_CONTEXT>::addCtxt(const OBJCTXT &ctxt, const DOF6 &tf)
00071 {
00072   std::map<typename OBJECT::Ptr,bool> used;
00073   return ctxt_.merge(ctxt, tf, used, false);
00074 }
00075 
00076 template<typename OBJECT_CONTEXT>
00077 bool Node<OBJECT_CONTEXT>::merge(const OBJCTXT &ctxt, const DOF6 &tf, std::map<typename OBJECT::Ptr,bool> &used, const BoundingBox::TransformedFoVBB &fov, const bool only_merge)
00078 {
00079   //1. find correspondences
00080   //2. register
00081   return ctxt_.merge(ctxt, tf, used, fov, only_merge);
00082 }
00083 
00084 template<typename OBJECT_CONTEXT>
00085 bool Node<OBJECT_CONTEXT>::compute(const OBJCTXT &ctxt, DOF6 &link, std::map<typename OBJECT::Ptr, bool> &used, std::map<const Node*,bool> &_visited_list_, const bool only_merge, const int depth)
00086 {
00087   _visited_list_.clear();
00088   if(_register(ctxt,link,used,_visited_list_, only_merge, depth) || (connections_.size()==0 && ctxt_.empty())) {
00089     _visited_list_.clear();
00090     _merge(ctxt,link,used,_visited_list_, only_merge, depth);
00091   }
00092 }
00093 
00094 
00095 template<typename OBJECT_CONTEXT>
00096 bool Node<OBJECT_CONTEXT>::_register(const OBJCTXT &ctxt, DOF6 &link, std::map<typename OBJECT::Ptr, bool> &used, std::map<const Node*,bool> &_visited_list_, const bool only_merge, const int depth)
00097 {
00098   //prevent endless loop
00099   if(_visited_list_.find(this)!=_visited_list_.end())
00100     return false;
00101   _visited_list_[this] = true;
00102 
00103   const float noise = 0.02f;
00104 
00105   //check bounding box
00106   if(depth>1 && !(connections_.size()==0 && ctxt_.empty()) && !(ctxt_.getBoundingBox().transform(link.getRotation(),link.getTranslation())&ctxt.getBoundingBox().transform(Eigen::Matrix3f::Identity(), Eigen::Vector3f::Zero())))
00107   {
00108     ROS_INFO("no intersection");
00109     return false;
00110   }
00111 
00112   std::cout<<"COMPUTING\n"<<link<<"\n";
00113 //  for(size_t i=0; i<connections_.size(); i++)
00114 //    std::cout<<"LINK BEF"<<connections_[i].link_<<"\n";
00115 
00116   DOF6 cp_link;
00117   cp_link.deepCopy(link);
00118 
00119   //register
00120   typename DOF6::TYPE success_rate, error_rate;
00121   bool r = registration(ctxt, link, success_rate, error_rate);
00122   if(!r)
00123   {
00124     // restore link
00125     link.deepCopy(cp_link);
00126     link.getSource1()->reset();
00127     ROS_INFO("restore link");
00128   }
00129   bool ret = r || (connections_.size()==0 && ctxt_.empty());
00130 
00131   ROS_INFO("r was %d",r);
00132   std::cout<<link<<"\n";
00133 
00134   for(size_t i=0; i<connections_.size(); i++)
00135   {
00136     //    std::cout<<"LINK AFTER\n"<<connections_[i].link_<<"\n";
00137     //    std::cout<<"COPY LINK\n"<<cp_link<<"\n";
00138 
00139     //#error cp_link variance different from bef.
00140     DOF6 tmp_link;
00141     tmp_link.deepCopy(link);
00142     tmp_link.setVariance(cp_link.getTranslationVariance()+connections_[i].link_.getTranslationVariance() + noise,
00143                          ((Eigen::Matrix3f)link.getRotation())*connections_[i].link_.getTranslation()+link.getTranslation(),
00144                          cp_link.getRotationVariance()+connections_[i].link_.getRotationVariance() + noise,
00145                          (typename DOF6::TROTATION)( ((Eigen::Matrix3f)link.getRotation())*((Eigen::Matrix3f)connections_[i].link_.getRotation())) );
00146     tmp_link.getSource1()->reset();
00147     bool r2 = connections_[i].node_->_register(ctxt, tmp_link, used, _visited_list_, true, depth+1);
00148 
00149     ROS_INFO("r2 was %d",r2);
00150 
00151     if(r2&&r) {
00152       std::cout<<"LINK BEF\n";
00153       std::cout<<*connections_[i].link_.getSource1()<<"\n";
00154 
00155       *connections_[i].link_.getSource1() += link.getSource1()->transpose() + *tmp_link.getSource1(); //TODO: is not correct
00156       connections_[i].link_.setVariance(connections_[i].link_.getTranslationVariance(), connections_[i].link_.getTranslation(),
00157                                         connections_[i].link_.getRotationVariance(), connections_[i].link_.getRotation());
00158 
00159       std::cout<<"UPDATE LINK\n";;
00160 
00161       std::cout<<"CONNECTION\n";;
00162       std::cout<<link<<"\n";
00163       std::cout<<"CONNECTION TRANSPOSED\n";;
00164       std::cout<<link.getSource1()->transpose()<<"\n";
00165       std::cout<<"TMPLINK\n";
00166       std::cout<<*tmp_link.getSource1()<<"\n";
00167       std::cout<<"ADDED\n";
00168       std::cout<<(link.getSource1()->transpose() + *tmp_link.getSource1())<<"\n";
00169 
00170       std::cout<<"LINK\n";
00171       std::cout<<*connections_[i].link_.getSource1()<<"\n";
00172       std::cout<<"SHOULD\n";
00173       std::cout<<(link.getSource1()->getRotation().inverse()*tmp_link.getSource1()->getRotation())<<"\n";
00174 
00175       //ret = (link.getSource1()->getTranslationVariance()+link.getSource1()->getRotationVariance())<0.3f;
00176     }
00177     else if(r2 /*&& (connections_[i].link_.getSource1()->getTranslationVariance()+connections_[i].link_.getSource1()->getRotationVariance())<0.3f*/ ) {
00178       if(connections_[i].link_.getSource1()->isSet())
00179       {
00180         ROS_INFO("rX was 1");
00181         *link.getSource1() += connections_[i].link_.getSource1()->transpose() + *tmp_link.getSource1(); //TODO: is not correct
00182         link.setVariance(link.getTranslationVariance(), link.getTranslation(),
00183                          link.getRotationVariance(), link.getRotation());
00184 
00185         std::cout<<"CONNECTION\n";;
00186         std::cout<<connections_[i].link_<<"\n";
00187         std::cout<<"CONNECTION TRANSPOSED\n";;
00188         std::cout<<connections_[i].link_.getSource1()->transpose()<<"\n";
00189         std::cout<<"TMPLINK\n";
00190         std::cout<<tmp_link<<"\n";
00191         std::cout<<"ADDED\n";
00192         std::cout<<(connections_[i].link_.getSource1()->transpose() + *tmp_link.getSource1())<<"\n";
00193 
00194         std::cout<<"LINK\n";
00195         std::cout<<link<<"\n";
00196         std::cout<<"SHOULD\n";
00197         std::cout<<(connections_[i].link_.getSource1()->getRotation().inverse()*tmp_link.getSource1()->getRotation())<<"\n";
00198       }
00199       else
00200       {
00201         ROS_INFO("rX was 2");
00202         //TODO:
00203         Eigen::Matrix4f M = connections_[i].link_.getTF4().inverse();
00204         Eigen::Matrix4f M2= M*tmp_link.getTF4();
00205         link.setVariance(tmp_link.getTranslationVariance()+connections_[i].link_.getTranslationVariance(),
00206                          M2.col(3).head<3>(),
00207                          tmp_link.getRotationVariance()+connections_[i].link_.getRotationVariance(),
00208                          (typename DOF6::TROTATION)( M2.topLeftCorner(3,3) ) );
00209 
00210         std::cout<<"CONNECTION\n";;
00211         std::cout<<connections_[i].link_<<"\n";
00212         std::cout<<"CONNECTION\n";;
00213         std::cout<<connections_[i].link_.getTF4()<<"\n";
00214         std::cout<<"CONNECTION TRANSPOSED\n";;
00215         std::cout<<M<<"\n";
00216         std::cout<<"TMPLINK\n";
00217         std::cout<<tmp_link<<"\n";
00218         std::cout<<"LINK\n";
00219         std::cout<<link<<"\n";
00220       }
00221 
00222       ret |= (link.getTranslationVariance()+link.getRotationVariance())<0.5f;
00223 
00224       ROS_INFO("sumvar %f",(link.getTranslationVariance()+link.getRotationVariance()));
00225     }
00226     else
00227       ROS_INFO("au backe");
00228 
00229   }
00230 
00231   ROS_INFO("ret was %d",ret);
00232 
00233   if(!ret && connections_.size()>0)
00234   {
00235     ROS_INFO("need a complete map");
00236 
00237     OBJCTXT map;
00238     map += ctxt_;
00239     for(size_t i=0; i<connections_.size(); i++)
00240     {
00241       map += connections_[i].node_->ctxt_.clone()->transform(connections_[i].link_);
00242     }
00243     ret = map.registration(ctxt, link, success_rate, error_rate);
00244 
00245     if(ret)
00246       ROS_INFO("worked :)");
00247     else
00248     {
00249       ROS_INFO("did not work :(");
00250 
00251       // restore link
00252       link.deepCopy(cp_link);
00253       ROS_INFO("restore link");
00254     }
00255   }
00256 
00257   return ret;
00258 }
00259 
00260 template<typename OBJECT_CONTEXT>
00261 void Node<OBJECT_CONTEXT>::_merge(const OBJCTXT &ctxt, const DOF6 &link, std::map<typename OBJECT::Ptr, bool> &used, std::map<const Node*,bool> &_visited_list_, const bool only_merge, const int depth)
00262 {
00263   //prevent endless loop
00264   if(_visited_list_.find(this)!=_visited_list_.end())
00265     return;
00266   _visited_list_[this] = true;
00267 
00268   const float noise = 0.06f;
00269 
00270   //check bounding box
00271   if(depth>1 && !(connections_.size()==0 && ctxt_.empty()) && !(ctxt_.getBoundingBox().transform(link.getRotation(),link.getTranslation())&ctxt.getBoundingBox().transform(Eigen::Matrix3f::Identity(), Eigen::Vector3f::Zero())))
00272   {
00273     ROS_INFO("no intersection");
00274     return;
00275   }
00276 
00277   for(size_t i=0; i<connections_.size(); i++)
00278   {
00279     DOF6 tmp_link;
00280     tmp_link.deepCopy(link);
00281     tmp_link.setVariance(link.getTranslationVariance()+connections_[i].link_.getTranslationVariance() + noise,
00282                          ((Eigen::Matrix3f)link.getRotation())*connections_[i].link_.getTranslation()+link.getTranslation(),
00283                          link.getRotationVariance()+connections_[i].link_.getRotationVariance() + noise,
00284                          (typename DOF6::TROTATION)( ((Eigen::Matrix3f)link.getRotation())*((Eigen::Matrix3f)connections_[i].link_.getRotation())) );
00285     tmp_link.getSource1()->reset();
00286     connections_[i].node_->_merge(ctxt, tmp_link, used, _visited_list_, true, depth+1);
00287   }
00288 
00289   BoundingBox::FoVBB<float> fov;
00290   Eigen::Vector3f mi,ma;
00291   mi(0)=-0.43f;
00292   mi(1)=-0.36f;
00293   ma(0)=0.43f;
00294   ma(1)=0.36f;
00295   mi(2)=ma(2)=1.f;
00296   mi*=0.4f;
00297   ma*=8.f;
00298   Eigen::Vector3f t;
00299   t(0)=t(1)=0;
00300   t(2)=0.4f;
00301   fov.update(mi,ma);
00302   DOF6 l=link.transpose();
00303   merge(ctxt, link, used, fov.transform(l.getRotation(),l.getTranslation()-t), only_merge);
00304 
00305   ctxt_.update();
00306 }
00307 
00308