Point Cloud Library (PCL)  1.14.0-dev
warp_point_rigid.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/memory.h>
44 #include <pcl/pcl_macros.h>
45 
46 namespace pcl {
47 namespace registration {
48 /** \brief Base warp point class.
49  *
50  * \note The class is templated on the source and target point types as well as on the
51  * output scalar of the transformation matrix (i.e., float or double). Default: float.
52  * \author Radu B. Rusu
53  * \ingroup registration
54  */
55 template <typename PointSourceT, typename PointTargetT, typename Scalar = float>
57 public:
58  using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
59  using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
60  using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
61 
62  using Ptr = shared_ptr<WarpPointRigid<PointSourceT, PointTargetT, Scalar>>;
63  using ConstPtr = shared_ptr<const WarpPointRigid<PointSourceT, PointTargetT, Scalar>>;
64 
65  /** \brief Constructor
66  * \param[in] nr_dim the number of dimensions
67  */
68  WarpPointRigid(int nr_dim) : nr_dim_(nr_dim), transform_matrix_(Matrix4::Zero())
69  {
70  transform_matrix_(3, 3) = 1.0;
71  };
72 
73  /** \brief Destructor. */
74  virtual ~WarpPointRigid() = default;
75 
76  /** \brief Set warp parameters. Pure virtual.
77  * \param[in] p warp parameters
78  */
79  virtual void
80  setParam(const VectorX& p) = 0;
81 
82  /** \brief Warp a point given a transformation matrix
83  * \param[in] pnt_in the point to warp (transform)
84  * \param[out] pnt_out the warped (transformed) point
85  */
86  inline void
87  warpPoint(const PointSourceT& pnt_in, PointSourceT& pnt_out) const
88  {
89  pnt_out.x = static_cast<float>(
90  transform_matrix_(0, 0) * pnt_in.x + transform_matrix_(0, 1) * pnt_in.y +
91  transform_matrix_(0, 2) * pnt_in.z + transform_matrix_(0, 3));
92  pnt_out.y = static_cast<float>(
93  transform_matrix_(1, 0) * pnt_in.x + transform_matrix_(1, 1) * pnt_in.y +
94  transform_matrix_(1, 2) * pnt_in.z + transform_matrix_(1, 3));
95  pnt_out.z = static_cast<float>(
96  transform_matrix_(2, 0) * pnt_in.x + transform_matrix_(2, 1) * pnt_in.y +
97  transform_matrix_(2, 2) * pnt_in.z + transform_matrix_(2, 3));
98  // pnt_out.getVector3fMap () = transform_matrix_.topLeftCorner<3, 3> () *
99  // pnt_in.getVector3fMap () +
100  // transform_matrix_.block<3, 1> (0, 3);
101  // pnt_out.data[3] = pnt_in.data[3];
102  }
103 
104  /** \brief Warp a point given a transformation matrix
105  * \param[in] pnt_in the point to warp (transform)
106  * \param[out] pnt_out the warped (transformed) point
107  */
108  inline void
109  warpPoint(const PointSourceT& pnt_in, Vector4& pnt_out) const
110  {
111  pnt_out[0] = static_cast<Scalar>(
112  transform_matrix_(0, 0) * pnt_in.x + transform_matrix_(0, 1) * pnt_in.y +
113  transform_matrix_(0, 2) * pnt_in.z + transform_matrix_(0, 3));
114  pnt_out[1] = static_cast<Scalar>(
115  transform_matrix_(1, 0) * pnt_in.x + transform_matrix_(1, 1) * pnt_in.y +
116  transform_matrix_(1, 2) * pnt_in.z + transform_matrix_(1, 3));
117  pnt_out[2] = static_cast<Scalar>(
118  transform_matrix_(2, 0) * pnt_in.x + transform_matrix_(2, 1) * pnt_in.y +
119  transform_matrix_(2, 2) * pnt_in.z + transform_matrix_(2, 3));
120  pnt_out[3] = 0.0;
121  }
122 
123  /** \brief Get the number of dimensions. */
124  inline int
125  getDimension() const
126  {
127  return (nr_dim_);
128  }
129 
130  /** \brief Get the Transform used. */
131  inline const Matrix4&
132  getTransform() const
133  {
134  return (transform_matrix_);
135  }
136 
137 public:
139 
140 protected:
141  int nr_dim_;
143 };
144 } // namespace registration
145 } // namespace pcl
void warpPoint(const PointSourceT &pnt_in, Vector4 &pnt_out) const
Warp a point given a transformation matrix.
WarpPointRigid(int nr_dim)
Constructor.
shared_ptr< WarpPointRigid< PointSourceT, PointTargetT, Scalar > > Ptr
virtual ~WarpPointRigid()=default
Destructor.
void warpPoint(const PointSourceT &pnt_in, PointSourceT &pnt_out) const
Warp a point given a transformation matrix.
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Eigen::Matrix< Scalar, 4, 1 > Vector4
virtual void setParam(const VectorX &p)=0
Set warp parameters.
const Matrix4 & getTransform() const
Get the Transform used.
shared_ptr< const WarpPointRigid< PointSourceT, PointTargetT, Scalar > > ConstPtr
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
int getDimension() const
Get the number of dimensions.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
Defines functions, macros and traits for allocating and using memory.
Defines all the PCL and non-PCL macros used.