warp_point_rigid_6d.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 
00042 #ifndef PCL_WARP_POINT_RIGID_6D_H_
00043 #define PCL_WARP_POINT_RIGID_6D_H_
00044 
00045 #include <pcl/registration/warp_point_rigid.h>
00046 
00047 namespace pcl
00048 {
00049   namespace registration
00050   {
00058     template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
00059     class WarpPointRigid6D : public WarpPointRigid<PointSourceT, PointTargetT, Scalar>
00060     {
00061       public:
00062         using WarpPointRigid<PointSourceT, PointTargetT, Scalar>::transform_matrix_;
00063 
00064         typedef typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::Matrix4 Matrix4;
00065         typedef typename WarpPointRigid<PointSourceT, PointTargetT, Scalar>::VectorX VectorX;
00066 
00067         typedef boost::shared_ptr<WarpPointRigid6D<PointSourceT, PointTargetT, Scalar> > Ptr;
00068         typedef boost::shared_ptr<const WarpPointRigid6D<PointSourceT, PointTargetT, Scalar> > ConstPtr;
00069 
00070         WarpPointRigid6D () : WarpPointRigid<PointSourceT, PointTargetT, Scalar> (6) {}
00071       
00073         virtual ~WarpPointRigid6D () {}
00074 
00079         virtual void 
00080         setParam (const VectorX& p)
00081         {
00082           assert (p.rows () == this->getDimension ());
00083 
00084           // Copy the rotation and translation components
00085           transform_matrix_.setZero ();
00086           transform_matrix_ (0, 3) = p[0];
00087           transform_matrix_ (1, 3) = p[1];
00088           transform_matrix_ (2, 3) = p[2];
00089           transform_matrix_ (3, 3) = 1;
00090           
00091           // Compute w from the unit quaternion
00092           Eigen::Quaternion<Scalar> q (0, p[3], p[4], p[5]);
00093           q.w () = static_cast<Scalar> (sqrt (1 - q.dot (q)));
00094           q.normalize ();
00095           transform_matrix_.topLeftCorner (3, 3) = q.toRotationMatrix ();
00096         }
00097     };
00098   }
00099 }
00100 
00101 #endif
00102 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:38:43