icp_registration_fitter.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: model_registration.h 36182 2010-04-27 16:17:14Z rusu $
00035  *
00036  */
00037 #ifndef MODEL_REGISTRATION_H_
00038 #define MODEL_REGISTRATION_H_
00039 
00040 #include <pcl/registration/registration.h>
00041 #include <pcl/registration/icp.h>
00042 #include <pcl/registration/icp_nl.h>
00043 
00044 #include "tabletop_object_detector/model_fitter.h"
00045 
00046 namespace tabletop_object_detector 
00047 {
00049   template <typename T>
00050   class ICPRegistrationFitter : public ModelToCloudFitter
00051   {
00052     public:
00053 
00054     void initializeFromMesh(const arm_navigation_msgs::Shape &mesh) 
00055     {
00056       std::vector<tf::Vector3> btVectors;
00057       sampleMesh(mesh, btVectors, 0.002 );
00058       cloudmodel_.points.resize (btVectors.size ());
00059       for (int i = 0; i < (int)btVectors.size (); ++i)
00060       {
00061         cloudmodel_.points[i].x = btVectors[i].x ();
00062         cloudmodel_.points[i].y = btVectors[i].y ();
00063         cloudmodel_.points[i].z = btVectors[i].z ();
00064       }
00065     }
00066     
00067     ModelFitInfo fitPointCloud (const T &cloud)
00068     {
00069       pcl::IterativeClosestPoint<T, T> reg;
00070       reg.setMaximumIterations (10000);
00071       reg.setTransformationEpsilon (1e-5);
00072       //T::Ptr cloudptr;
00073       //cloudptr.reset (new T (cloud));
00074       //reg.setInputCloud (cloudptr);
00075       //reg.setInputModel (cloudmodel_);
00076       T cloud_out;
00077       reg.compute (cloud_out);
00078       
00079       double score = reg.getFitnessScore ();
00080 
00081       Eigen::Matrix4f tma = reg.getFinalTransformationMatrix (); 
00082       geometry_msgs::Pose p;
00083       p.position.x = tma (0, 3);
00084       p.position.y = tma (1, 3);
00085       p.position.z = tma (2, 3);
00086       Eigen::Quaternion<double> q (tma.block<3,3>(0,0));
00087       p.orientation.x = q.x ();
00088       p.orientation.y = q.y ();
00089       p.orientation.z = q.z ();
00090       p.orientation.w = q.w ();
00091 
00092       return ModelFitInfo(model_id_, p, score);
00093     }
00094 
00095   private:
00096     T cloudmodel_;
00097   };
00098 } //namespace tabletop_object_detector
00099 
00100 #endif


tabletop_object_detector
Author(s): Marius Muja and Matei Ciocarlie
autogenerated on Fri Jan 3 2014 11:48:47