RigidTransformation.h

Go to the documentation of this file.

//     ____   ______ __
//    / __ \ / ____// /
//   / /_/ // /    / /
//  / ____// /___ / /___   PixInsight Class Library
// /_/     \____//_____/   PCL 2.7.0
// ----------------------------------------------------------------------------
// pcl/RigidTransformation.h - Released 2024-06-18T15:48:54Z
// ----------------------------------------------------------------------------
// This file is part of the PixInsight Class Library (PCL).
// PCL is a multiplatform C++ framework for development of PixInsight modules.
//
// Copyright (c) 2003-2024 Pleiades Astrophoto S.L. All Rights Reserved.
//
// Redistribution and use in both source and binary forms, with or without
// modification, is permitted provided that the following conditions are met:
//
// 1. All redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. All redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//
// 3. Neither the names "PixInsight" and "Pleiades Astrophoto", nor the names
//    of their contributors, may be used to endorse or promote products derived
//    from this software without specific prior written permission. For written
//    permission, please contact info@pixinsight.com.
//
// 4. All products derived from this software, in any form whatsoever, must
//    reproduce the following acknowledgment in the end-user documentation
//    and/or other materials provided with the product:
//
//    "This product is based on software from the PixInsight project, developed
//    by Pleiades Astrophoto and its contributors (https://pixinsight.com/)."
//
//    Alternatively, if that is where third-party acknowledgments normally
//    appear, this acknowledgment must be reproduced in the product itself.
//
// THIS SOFTWARE IS PROVIDED BY PLEIADES ASTROPHOTO AND ITS CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL PLEIADES ASTROPHOTO OR ITS
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
// EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, BUSINESS
// INTERRUPTION; PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; AND LOSS OF USE,
// DATA OR PROFITS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
// ----------------------------------------------------------------------------
 
#ifndef __PCL_RigidTransformation_h
#define __PCL_RigidTransformation_h
 
 
#include <pcl/Defs.h>
#include <pcl/Diagnostics.h>
 
#include <pcl/Algebra.h>
#include <pcl/Array.h>
#include <pcl/Matrix.h>
#include <pcl/Point.h>
 
namespace pcl
{
 
// ----------------------------------------------------------------------------
 
class PCL_CLASS RigidTransformation
{
public:
 
   RigidTransformation()
      : m_H( Matrix::UnitMatrix( 3 ) )
   {
   }
 
   RigidTransformation( const Matrix& H )
      : m_H( H )
   {
   }
 
   template <class point_list1, class point_list2>
   RigidTransformation( const point_list1& P1, const point_list2& P2 )
      : m_H( LS_SVD( P1, P2 ) )
   {
   }
 
   RigidTransformation( const RigidTransformation& ) = default;
 
   RigidTransformation& operator =( const RigidTransformation& ) = default;
 
   RigidTransformation( RigidTransformation&& ) = default;
 
   RigidTransformation& operator =( RigidTransformation&& ) = default;
 
   template <typename T1, typename T2>
   void Apply( T1& x, T2& y ) const
   {
      double fx = double( x );
      double fy = double( y );
      x = T1( m_H[0][0]*fx + m_H[0][1]*fy + m_H[0][2] );
      y = T2( m_H[1][0]*fx + m_H[1][1]*fy + m_H[1][2] );
   }
 
   template <typename T>
   GenericPoint<T>& Apply( GenericPoint<T>& p ) const
   {
      Apply( p.x, p.y );
      return p;
   }
 
   DPoint operator ()( double x, double y ) const
   {
      DPoint p( x, y );
      Apply( p.x, p.y );
      return p;
   }
 
   template <typename T>
   DPoint operator ()( const GenericPoint<T>& p ) const
   {
      return operator ()( double( p.x ), double( p.y ) );
   }
 
   RigidTransformation Inverse() const
   {
      return RigidTransformation( m_H.Inverse() );
   }
 
   const Matrix& TransformationMatrix() const
   {
      return m_H;
   }
 
   operator const Matrix&() const
   {
      return TransformationMatrix();
   }
 
   bool IsValid() const
   {
      return !m_H.IsEmpty();
   }
 
   void EnsureUnique()
   {
      m_H.EnsureUnique();
   }
 
private:
 
   Matrix m_H;
 
   /*
    * Implementation of the SVD-based least squares method to compute an
    * optimal rigid transformation matrix.
    */
   template <class point_list1, class point_list2>
   static Matrix LS_SVD( const point_list1& P1, const point_list2& P2 )
   {
      int n = int( Min( P1.Length(), P2.Length() ) );
      if ( n < 3 )
         throw Error( "RigidTransformation::LS_SVD(): Less than three points specified." );
 
      /*
       * Centroid coordinates
       */
      DPoint p10( 0 );
      for ( int i = 0; i < n; ++i )
         p10 += P1[i];
      p10 /= n;
      DPoint p20( 0 );
      for ( int i = 0; i < n; ++i )
         p20 += P2[i];
      p20 /= n;
 
      /*
       * Covariance matrix
       */
      Matrix M1( 2, n );
      for ( int i = 0; i < n; ++i )
      {
         M1[0][i] = P1[i].x - p10.x;
         M1[1][i] = P1[i].y - p10.y;
      }
      Matrix M2( 2, n );
      for ( int i = 0; i < n; ++i )
      {
         M2[0][i] = P2[i].x - p20.x;
         M2[1][i] = P2[i].y - p20.y;
      }
      Matrix H = M1 * M2.Transpose();
 
      /*
       * Rotation matrix R
       */
      InPlaceSVD svd( H );
      Matrix R = svd.V * H.Transpose();
 
      /*
       * Rotation + translation matrix
       */
      return Matrix( R[0][0], R[0][1], p20.x - R[0][0]*p10.x - R[0][1]*p10.y,
                     R[1][0], R[1][1], p20.y - R[1][0]*p10.x - R[1][1]*p10.y,
                     0.0,     0.0,     1.0 );
   }
};
 
// ----------------------------------------------------------------------------
 
} // pcl
 
#endif   // __PCL_RigidTransformation_h
 
// ----------------------------------------------------------------------------
// EOF pcl/RigidTransformation.h - Released 2024-06-18T15:48:54Z