Go to the documentation of this file.00001
00002 #ifndef vgl_norm_trans_3d_txx_
00003 #define vgl_norm_trans_3d_txx_
00004
00005
00006
00007 #include "vgl_norm_trans_3d.h"
00008 #include <vgl/vgl_point_3d.h>
00009 #include <vnl/vnl_vector_fixed.h>
00010 #include <vcl_iostream.h>
00011
00012
00013
00014
00015
00016 template <class T>
00017 vgl_norm_trans_3d<T>::vgl_norm_trans_3d() : vgl_h_matrix_3d<T>()
00018 {
00019 }
00020
00021
00022 template <class T>
00023 vgl_norm_trans_3d<T>::vgl_norm_trans_3d(const vgl_norm_trans_3d<T>& M)
00024 : vgl_h_matrix_3d<T>(M)
00025 {
00026 }
00027
00028
00029
00030 template <class T>
00031 vgl_norm_trans_3d<T>::vgl_norm_trans_3d(vcl_istream& s)
00032 : vgl_h_matrix_3d<T>(s)
00033 {
00034 }
00035
00036
00037 template <class T>
00038 vgl_norm_trans_3d<T>::vgl_norm_trans_3d(char const* filename)
00039 : vgl_h_matrix_3d<T>(filename)
00040 {
00041 }
00042
00043
00044
00045 template <class T>
00046 vgl_norm_trans_3d<T>::vgl_norm_trans_3d(vnl_matrix_fixed<T,4,4> const& M)
00047 : vgl_h_matrix_3d<T>(M)
00048 {
00049 }
00050
00051
00052
00053 template <class T>
00054 vgl_norm_trans_3d<T>::vgl_norm_trans_3d(const T* H)
00055 : vgl_h_matrix_3d<T>(H)
00056 {
00057 }
00058
00059
00060 template <class T>
00061 vgl_norm_trans_3d<T>::~vgl_norm_trans_3d()
00062 {
00063 }
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073 template <class T>
00074 bool vgl_norm_trans_3d<T>::
00075 compute_from_points(vcl_vector<vgl_homg_point_3d<T> > const& points)
00076 {
00077 T cx, cy, cz, radius;
00078 this->center_of_mass(points, cx, cy, cz);
00079 vgl_h_matrix_3d<T>::set_identity().set_translation(-cx,-cy,-cz);
00080 vcl_vector<vgl_homg_point_3d<T> > temp;
00081 for (typename vcl_vector<vgl_homg_point_3d<T> >::const_iterator
00082 pit = points.begin(); pit != points.end(); pit++)
00083 {
00084 vgl_homg_point_3d<T> p((*this)(*pit));
00085 temp.push_back(p);
00086 }
00087
00088 if (!this->scale_xyzroot2(temp, radius))
00089 return false;
00090 vgl_h_matrix_3d<T>::set_scale(T(1)/radius);
00091 return true;
00092 }
00093
00094
00095
00096
00097 template <class T>
00098 void vgl_norm_trans_3d<T>::
00099 center_of_mass(vcl_vector<vgl_homg_point_3d<T> > const& in,
00100 T& cx, T& cy, T& cz)
00101 {
00102 T cog_x = 0;
00103 T cog_y = 0;
00104 T cog_z = 0;
00105 T cog_count = 0;
00106 T tol = 1e-06f;
00107 unsigned n = in.size();
00108 for (unsigned i = 0; i < n; ++i)
00109 {
00110 if (in[i].ideal(tol))
00111 continue;
00112 vgl_point_3d<T> p(in[i]);
00113 T x = p.x();
00114 T y = p.y();
00115 T z = p.z();
00116 cog_x += x;
00117 cog_y += y;
00118 cog_z += z;
00119 ++cog_count;
00120 }
00121 if (cog_count > 0)
00122 {
00123 cog_x /= cog_count;
00124 cog_y /= cog_count;
00125 cog_z /= cog_count;
00126 }
00127
00128 cx = cog_x;
00129 cy = cog_y;
00130 cz = cog_z;
00131 }
00132
00133
00134
00135
00136 template <class T>
00137 bool vgl_norm_trans_3d<T>::
00138 scale_xyzroot2(vcl_vector<vgl_homg_point_3d<T> > const& in, T& radius)
00139 {
00140 T magnitude = T(0);
00141 int numfinite = 0;
00142 T tol = T(1e-06);
00143 radius = T(0);
00144 for (unsigned i = 0; i < in.size(); ++i)
00145 {
00146 if (in[i].ideal(tol))
00147 continue;
00148 vgl_point_3d<T> p(in[i]);
00149 vnl_vector_fixed<T, 3> v(p.x(), p.y(), p.z());
00150 magnitude += v.magnitude();
00151 ++numfinite;
00152 }
00153
00154 if (numfinite > 0)
00155 {
00156 radius = magnitude / numfinite;
00157 return radius>=tol;
00158 }
00159 return false;
00160 }
00161
00162
00163 #undef VGL_NORM_TRANS_3D_INSTANTIATE
00164 #define VGL_NORM_TRANS_3D_INSTANTIATE(T) \
00165 template class vgl_norm_trans_3d<T >
00166
00167 #endif // vgl_norm_trans_3d_txx_