KWStyle - itkEuler3DTransform.txx

Matrix View

Description

---

1		/*=========================================================================
2
3		Program:   Insight Segmentation & Registration Toolkit
4		Module:    $RCSfile: itkEuler3DTransform.txx.html,v $
5		Language:  C++
6		Date:      $Date: 2006/01/17 19:15:35 $
7		Version:   $Revision: 1.4 $
8
9		Copyright (c) Insight Software Consortium. All rights reserved.
10		See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.
11
12		This software is distributed WITHOUT ANY WARRANTY; without even
13		the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
14		PURPOSE.  See the above copyright notices for more information.
15
16		=========================================================================*/
17	DEF	#ifndef _itkEuler3DTransform_txx
18	DEF	#define _itkEuler3DTransform_txx
19
20		#include "itkEuler3DTransform.h"
21
22
23		namespace itk
24		{
25
26		// Constructor with default arguments
27		template <class TScalarType>
28		Euler3DTransform<TScalarType>
29		::Euler3DTransform():
30	IND	**Superclass(SpaceDimension, ParametersDimension)
31		{
32		m_ComputeZYX = false;
33		m_AngleX = m_AngleY = m_AngleZ = 0.0;
34		}
35
36		// Constructor with arguments
37		template <class TScalarType>
38		Euler3DTransform<TScalarType>
39		::Euler3DTransform(unsigned int SpaceDimension,
40		unsigned int ParametersDimension):
41	IND	**Superclass(SpaceDimension, ParametersDimension)
42		{
43		m_ComputeZYX = false;
44		m_AngleX = m_AngleY = m_AngleZ = 0.0;
45		}
46
47		// Set Parameters
48		template <class TScalarType>
49		void
50		Euler3DTransform<TScalarType>
51		::SetParameters( const ParametersType & parameters )
52		{
53		itkDebugMacro( << "Setting paramaters " << parameters );
54
55		// Set angles with parameters
56		m_AngleX = parameters[0];
57		m_AngleY = parameters[1];
58		m_AngleZ = parameters[2];
59		this->ComputeMatrix();
60
61		// Transfer the translation part
62		OutputVectorType newTranslation;
63		newTranslation[0] = parameters[3];
64		newTranslation[1] = parameters[4];
65		newTranslation[2] = parameters[5];
66		this->SetVarTranslation(newTranslation);
67		this->ComputeOffset();
68
69		itkDebugMacro(<<"After setting paramaters ");
70		}
71
72
73		// Get Parameters
74		template <class TScalarType>
75		const typename Euler3DTransform<TScalarType>::ParametersType &
76		Euler3DTransform<TScalarType>
77		::GetParameters( void ) const
78		{
79		this->m_Parameters[0] = m_AngleX;
80		this->m_Parameters[1] = m_AngleY;
81		this->m_Parameters[2] = m_AngleZ;
82		this->m_Parameters[3] = this->GetTranslation()[0];
83		this->m_Parameters[4] = this->GetTranslation()[1];
84		this->m_Parameters[5] = this->GetTranslation()[2];
85
86		return this->m_Parameters;
87		}
88
89		// Set Rotational Part
90		template <class TScalarType>
91		void
92		Euler3DTransform<TScalarType>
93		::SetRotation(ScalarType angleX,ScalarType angleY,ScalarType angleZ)
94		{
95		m_AngleX = angleX;
96		m_AngleY = angleY;
97		m_AngleZ = angleZ;
98		this->ComputeMatrix();
99		this->ComputeOffset();
100		}
101
102		// Compose
103		template <class TScalarType>
104		void
105		Euler3DTransform<TScalarType>
106		::SetIdentity(void)
107		{
108		Superclass::SetIdentity();
109		m_AngleX = 0;
110		m_AngleY = 0;
111		m_AngleZ = 0;
112		}
113
114
115		// Compute angles from the rotation matrix
116		template <class TScalarType>
117		void
118		Euler3DTransform<TScalarType>
119		::ComputeMatrixParameters(void)
120		{
121		if(m_ComputeZYX)
122		{
123		m_AngleY = -asin(this->GetMatrix()[2][0]);
124		double C = cos(m_AngleY);
125		if(fabs(C)>0.00005)
126		{
127		double x = this->GetMatrix()[2][2] / C;
128		double y = this->GetMatrix()[2][1] / C;
129		m_AngleX = atan2(y,x);
130		x = this->GetMatrix()[0][0] / C;
131		y = this->GetMatrix()[1][0] / C;
132		m_AngleZ = atan2(y,x);
133		}
134		else
135		{
136		m_AngleX = 0;
137		double x = this->GetMatrix()[1][1];
138		double y = -this->GetMatrix()[0][1];
139		m_AngleZ = atan2(y,x);
140		}
141		}
142		else
143		{
144		m_AngleX = asin(this->GetMatrix()[2][1]);
145		double A = cos(m_AngleX);
146		if(fabs(A)>0.00005)
147		{
148		double x = this->GetMatrix()[2][2] / A;
149		double y = -this->GetMatrix()[2][0] / A;
150		m_AngleY = atan2(y,x);
151
152		x = this->GetMatrix()[1][1] / A;
153		y = -this->GetMatrix()[0][1] / A;
154		m_AngleZ = atan2(y,x);
155		}
156		else
157		{
158		m_AngleZ = 0;
159		double x = this->GetMatrix()[0][0];
160		double y = this->GetMatrix()[1][0];
161		m_AngleY = atan2(y,x);
162		}
163		}
164		this->ComputeMatrix();
165		}
166
167
168		// Compute the matrix
169		template <class TScalarType>
170		void
171		Euler3DTransform<TScalarType>
172		::ComputeMatrix( void )
173		{
174		// need to check if angles are in the right order
175		const double cx = cos(m_AngleX);
176		const double sx = sin(m_AngleX);
177		const double cy = cos(m_AngleY);
178		const double sy = sin(m_AngleY);
179		const double cz = cos(m_AngleZ);
180		const double sz = sin(m_AngleZ);
181
182		Matrix<TScalarType,3,3> RotationX;
183		RotationX[0][0]=1;RotationX[0][1]=0;RotationX[0][2]=0;
184		RotationX[1][0]=0;RotationX[1][1]=cx;RotationX[1][2]=-sx;
185		RotationX[2][0]=0;RotationX[2][1]=sx;RotationX[2][2]=cx;
186
187
188		Matrix<TScalarType,3,3> RotationY;
189		RotationY[0][0]=cy;RotationY[0][1]=0;RotationY[0][2]=sy;
190		RotationY[1][0]=0;RotationY[1][1]=1;RotationY[1][2]=0;
191		RotationY[2][0]=-sy;RotationY[2][1]=0;RotationY[2][2]=cy;
192
193
194		Matrix<TScalarType,3,3> RotationZ;
195		RotationZ[0][0]=cz;RotationZ[0][1]=-sz;RotationZ[0][2]=0;
196		RotationZ[1][0]=sz;RotationZ[1][1]=cz;RotationZ[1][2]=0;
197		RotationZ[2][0]=0;RotationZ[2][1]=0;RotationZ[2][2]=1;
198
199		/** Aply the rotation first around Y then X then Z */
200		if(m_ComputeZYX)
201		{
202		this->SetVarMatrix(RotationZ*RotationY*RotationX);
203		}
204		else
205		{
206		// Like VTK transformation order
207		this->SetVarMatrix(RotationZ*RotationX*RotationY);
208		}
209		}
210
211
212		// Set parameters
213		template<class TScalarType>
214		const typename Euler3DTransform<TScalarType>::JacobianType &
215		Euler3DTransform<TScalarType>::
216		GetJacobian( const InputPointType & p ) const
217		{
218		// need to check if angles are in the right order
219		const double cx = cos(m_AngleX);
220		const double sx = sin(m_AngleX);
221		const double cy = cos(m_AngleY);
222		const double sy = sin(m_AngleY);
223		const double cz = cos(m_AngleZ);
224		const double sz = sin(m_AngleZ);
225
226		this->m_Jacobian.Fill(0.0);
227
228		const double px = p[0] - this->GetCenter()[0];
229		const double py = p[1] - this->GetCenter()[1];
230		const double pz = p[2] - this->GetCenter()[2];
231
232
233		if ( m_ComputeZYX )
234		{
235		this->m_Jacobian[0][0] = (cz*sy*cx+sz*sx)*py+(-cz*sy*sx+sz*cx)*pz;
236		this->m_Jacobian[1][0] = (sz*sy*cx-cz*sx)*py+(-sz*sy*sx-cz*cx)*pz;
237		this->m_Jacobian[2][0] = (cy*cx)*py+(-cy*sx)*pz;
238
239		this->m_Jacobian[0][1] = (-cz*sy)*px+(cz*cy*sx)*py+(cz*cy*cx)*pz;
240		this->m_Jacobian[1][1] = (-sz*sy)*px+(sz*cy*sx)*py+(sz*cy*cx)*pz;
241		this->m_Jacobian[2][1] = (-cy)*px+(-sy*sx)*py+(-sy*cx)*pz;
242
243		this->m_Jacobian[0][2] = (-sz*cy)*px+(-sz*sy*sx-cz*cx)*py
244	IND	****************************************+(-sz*sy*cx+cz*sx)*pz;
245	LEN	this->m_Jacobian[1][2] = (cz*cy)*px+(cz*sy*sx-sz*cx)*py+(cz*sy*cx+sz*sx)*pz;
246		this->m_Jacobian[2][2] = 0;
247		}
248		else
249		{
250		this->m_Jacobian[0][0] = (-sz*cx*sy)*px + (sz*sx)*py + (sz*cx*cy)*pz;
251		this->m_Jacobian[1][0] = (cz*cx*sy)*px + (-cz*sx)*py + (-cz*cx*cy)*pz;
252		this->m_Jacobian[2][0] = (sx*sy)*px + (cx)*py + (-sx*cy)*pz;
253
254		this->m_Jacobian[0][1] = (-cz*sy-sz*sx*cy)*px + (cz*cy-sz*sx*sy)*pz;
255		this->m_Jacobian[1][1] = (-sz*sy+cz*sx*cy)*px + (sz*cy+cz*sx*sy)*pz;
256		this->m_Jacobian[2][1] = (-cx*cy)*px + (-cx*sy)*pz;
257
258		this->m_Jacobian[0][2] = (-sz*cy-cz*sx*sy)*px + (-cz*cx)*py
259	IND	**************************************************+ (-sz*sy+cz*sx*cy)*pz;
260		this->m_Jacobian[1][2] = (cz*cy-sz*sx*sy)*px + (-sz*cx)*py
261	IND	*************************************************+ (cz*sy+sz*sx*cy)*pz;
262		this->m_Jacobian[2][2] = 0;
263		}
264
265		// compute derivatives for the translation part
266		unsigned int blockOffset = 3;
267		for(unsigned int dim=0; dim < SpaceDimension; dim++ )
268		{
269		this->m_Jacobian[ dim ][ blockOffset + dim ] = 1.0;
270		}
271
272		return this->m_Jacobian;
273
274		}
275
276		// Print self
277		template<class TScalarType>
278		void
279		Euler3DTransform<TScalarType>::
280		PrintSelf(std::ostream &os, Indent indent) const
281		{
282		Superclass::PrintSelf(os,indent);
283
284		os << indent << "Euler's angles: AngleX=" << m_AngleX
285	IND	*****<< " AngleY=" << m_AngleY
286	IND	*****<< " AngleZ=" << m_AngleZ
287	IND	*****<< std::endl;
288		os << indent << "m_ComputeZYX = " << m_ComputeZYX << std::endl;
289		}
290
291		} // namespace
292
293		#endif
294

---

Generated by KWStyle 1.0b on Tuesday January,17 at 02:14:07PM

© Kitware Inc.