registration_correspondence.hpp
Go to the documentation of this file.
00001 /****************************************************************
00002  *
00003  * Copyright (c) 2011
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_vision
00012  * ROS package name: registration
00013  * Description:
00014  *
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *
00017  * Author: goa-jh
00018  * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00019  *
00020  * Date of creation: Nov 29, 2011
00021  * ToDo:
00022  *
00023  *
00024  *
00025  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00026  *
00027  * Redistribution and use in source and binary forms, with or without
00028  * modification, are permitted provided that the following conditions are met:
00029  *
00030  *     * Redistributions of source code must retain the above copyright
00031  *       notice, this list of conditions and the following disclaimer.
00032  *     * Redistributions in binary form must reproduce the above copyright
00033  *       notice, this list of conditions and the following disclaimer in the
00034  *       documentation and/or other materials provided with the distribution.
00035  *     * Neither the name of the Fraunhofer Institute for Manufacturing
00036  *       Engineering and Automation (IPA) nor the names of its
00037  *       contributors may be used to endorse or promote products derived from
00038  *       this software without specific prior written permission.
00039  *
00040  * This program is free software: you can redistribute it and/or modify
00041  * it under the terms of the GNU Lesser General Public License LGPL as
00042  * published by the Free Software Foundation, either version 3 of the
00043  * License, or (at your option) any later version.
00044  *
00045  * This program is distributed in the hope that it will be useful,
00046  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00047  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00048  * GNU Lesser General Public License LGPL for more details.
00049  *
00050  * You should have received a copy of the GNU Lesser General Public
00051  * License LGPL along with this program.
00052  * If not, see <http://www.gnu.org/licenses/>.
00053  *
00054  ****************************************************************/
00055 
00056 namespace cob_3d_registration {
00057 
00058   template <typename Point>
00059   bool Registration_Corrospondence<Point>::compute_features() {
00060     if(register_.size()<1) {
00061       register_ = *this->input_org_;
00062       return true;
00063     }
00064 
00065     return keypoints_ && keypoints_->compute(register_, *this->input_org_);
00066   }
00067 
00068   template <typename Point>
00069   bool Registration_Corrospondence<Point>::compute_corrospondences() {
00070 
00071     all_correspondences_.reset(new pcl::registration::Correspondences);
00072     keypoints_->getCorrespondences (*all_correspondences_);
00073 
00074     //  ROS_INFO("rejectBadCorrespondences %d %d",all_correspondences->size(), remaining_correspondences.size());
00075 
00076     return true;
00077   }
00078 
00079   template <typename Point>
00080   bool Registration_Corrospondence<Point>::compute_transformation() {
00081     Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
00082 
00083     // Find correspondences between keypoints in FPFH space
00084     pcl::registration::CorrespondencesPtr good_correspondences (new pcl::registration::Correspondences);
00085 
00086     rejectBadCorrespondences(all_correspondences_, *good_correspondences);
00087 
00088     if(good_correspondences->size()<3) {
00089       ROS_ERROR("not enough coresspondences");
00090       return false;
00091     }
00092 
00093     // Obtain the best transformation between the two sets of keypoints given the remaining correspondences
00094     pcl::registration::TransformationEstimationSVD<Point, Point> trans_est;
00095     trans_est.estimateRigidTransformation (*keypoints_->getSourcePoints(), *keypoints_->getTargetPoints(), *good_correspondences, transform);
00096 
00097     this->transformation_ = transform*this->transformation_;
00098 
00099     std::cout<<"transf\n"<<this->transformation_<<"\n";
00100 
00101     register_ = *this->input_org_;
00102 
00103     return true;
00104   }
00105 
00106 
00107   template <typename Point>
00108   void Registration_Corrospondence<Point>::rejectBadCorrespondences (const pcl::registration::CorrespondencesPtr &all_correspondences,
00109                                                                      pcl::registration::Correspondences &remaining_correspondences)
00110                                                                      {
00111     pcl::registration::CorrespondenceRejectorDistance rej;
00112     //rej.setInputCloud(keypoints_src);
00113     //rej.setInputTarget(keypoints_tgt);
00114     rej.setMaximumDistance (rejection_dis_);
00115     rej.setInputCorrespondences (all_correspondences);
00116     rej.getCorrespondeces (remaining_correspondences);
00117 
00118     ROS_INFO("rejectBadCorrespondences %d %d", (int)all_correspondences->size(), (int)remaining_correspondences.size());
00119                                                                      }
00120 
00121 
00122 }
00123 
00124 #define PCL_INSTANTIATE_Registration_Corrospondence(T) template class PCL_EXPORTS cob_3d_registration::Registration_Corrospondence<T>;
00125 


cob_3d_registration
Author(s): Joshua Hampp
autogenerated on Wed Aug 26 2015 11:02:36