PLearn 0.1
|
00001 // -*- C++ -*- 00002 00003 // geometry.cc 00004 // 00005 // Copyright (C) 1996 Andrew E. Johnson (aej@ri.cmu.edu) 00006 // Copyright (C) 2006 Pascal Lamblin 00007 // 00008 // Redistribution and use in source and binary forms, with or without 00009 // modification, are permitted provided that the following conditions are met: 00010 // 00011 // 1. Redistributions of source code must retain the above copyright 00012 // notice, this list of conditions and the following disclaimer. 00013 // 00014 // 2. Redistributions in binary form must reproduce the above copyright 00015 // notice, this list of conditions and the following disclaimer in the 00016 // documentation and/or other materials provided with the distribution. 00017 // 00018 // 3. The name of the authors may not be used to endorse or promote 00019 // products derived from this software without specific prior written 00020 // permission. 00021 // 00022 // THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR 00023 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 00024 // OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN 00025 // NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 00026 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00027 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 00028 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00029 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 00030 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00031 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00032 // 00033 // This file is part of the PLearn library. For more information on the PLearn 00034 // library, go to the PLearn Web site at www.plearn.org 00035 00036 // These functions come from .... 00037 00041 #include "geometry.h" 00042 #include <plearn/math/TMat_maths.h> 00043 #include <plearn/math/plapack.h> 00044 00045 namespace PLearn { 00046 using namespace std; 00047 00048 // Put function implementations here. 00049 00050 Vec anglesFromRotationMatrix( const Mat& rot ) 00051 { 00052 Vec angle( 3 ); 00053 angle[1] = atan2( -rot(2,0), sqrt( rot(0,0)*rot(0,0)+rot(1,0)*rot(1,0) ) ); 00054 angle[2] = atan2( rot(1,0) / cos( angle[1] ), rot(0,0) / cos( angle[1] ) ); 00055 angle[0] = atan2( rot(2,1) / cos( angle[1] ), rot(2,2) / cos( angle[1] ) ); 00056 00057 if( angle[1] * 180 / Pi > 89.9 ) 00058 { 00059 angle[1] = Pi / 2; 00060 angle[2] = 0; 00061 angle[0] = atan2( rot(0,1), rot(1,1) ); 00062 } 00063 else if( angle[1] * 180.0 / Pi < -89.9 ) 00064 { 00065 angle[1] = -Pi / 2; 00066 angle[2] = 0; 00067 angle[0] = -atan2( rot(0,1), rot(1,1) ); 00068 } 00069 00070 return( angle * ( real(180.0) / real(Pi) ) ); 00071 } 00072 00073 Mat rotationMatrixFromAngles( real rx, real ry, real rz ) 00074 { 00075 Mat rot( 3, 3 ); 00076 rx *= DEG2RAD; 00077 ry *= DEG2RAD; 00078 rz *= DEG2RAD; 00079 00080 rot(0, 0) = cos( rz ) * cos( ry ); 00081 rot(1, 0) = sin( rz ) * cos( ry ); 00082 rot(2, 0) = -sin( ry ); 00083 00084 rot(0, 1) = cos( rz ) * sin( ry ) * sin( rx ) - sin( rz ) * cos( rx ); 00085 rot(1, 1) = sin( rz ) * sin( ry ) * sin( rx ) + cos( rz ) * cos( rx ); 00086 rot(2, 1) = cos( ry ) * sin( rx ); 00087 00088 rot(0, 2) = cos( rz ) * sin( ry ) * cos( rx ) + sin( rz ) * sin( rx ); 00089 rot(1, 2) = sin( rz ) * sin( ry ) * cos( rx ) - cos( rz ) * sin( rx ); 00090 rot(2, 2) = cos( ry ) * cos( rx ); 00091 00092 return rot; 00093 } 00094 00095 Mat rotationMatrixFromAngles( const Vec& angles ) 00096 { 00097 if( angles.size() != 3 ) 00098 PLERROR( "rotationMatrixFromAngles - angles size should be 3 (is %d)." 00099 "\n", angles.size() ); 00100 00101 return rotationMatrixFromAngles( angles[0], angles[1], angles[0] ); 00102 } 00103 00104 Mat rotationFromAxisAngle( const Vec& K, real th ) 00105 { 00106 Mat R( 3, 3 ); 00107 real c = cos( th ); 00108 real s = sin( th ); 00109 real v = 1 - c; 00110 00111 R( 0, 0 ) = K[0]*K[0]*v + c; 00112 R( 0, 1 ) = K[0]*K[1]*v - K[2]*s; 00113 R( 0, 2 ) = K[0]*K[2]*v + K[1]*s; 00114 R( 1, 0 ) = K[0]*K[1]*v + K[2]*s; 00115 R( 1, 1 ) = K[1]*K[1]*v + c; 00116 R( 1, 2 ) = K[1]*K[2]*v - K[0]*s; 00117 R( 2, 0 ) = K[0]*K[2]*v - K[1]*s; 00118 R( 2, 1 ) = K[1]*K[2]*v + K[0]*s; 00119 R( 2, 2 ) = K[2]*K[2]*v + c; 00120 00121 return R; 00122 } 00123 00124 00125 void applyGeomTransformation( const Mat& rot, const Vec& trans, 00126 const Mat& points_in, Mat& points_out ) 00127 { 00128 points_out.resize( points_in.length(), 3 ); 00129 productTranspose( points_out, points_in, rot ); 00130 points_out += trans; 00131 } 00132 00133 void transformationFromWeightedMatchedPoints( const Mat& template_points, 00134 const Mat& mol_points, 00135 const Vec& weights, 00136 const Mat& rot, const Vec& trans, 00137 real& error ) 00138 { 00139 Vec t_centroid = weightedCentroid( template_points, weights ); 00140 Vec m_centroid = weightedCentroid( mol_points, weights ); 00141 00142 Mat origin_tp = template_points - t_centroid; 00143 Mat origin_mp = mol_points - m_centroid; 00144 00145 rot << rotationFromWeightedMatchedPoints( origin_tp, origin_mp, 00146 weights, error ); 00147 00148 // trans = m_centroid - rot * t_centroid 00149 trans << m_centroid; 00150 productAcc( trans, rot, -t_centroid ); 00151 } 00152 00153 Mat rotationFromWeightedMatchedPoints( const Mat& template_points, 00154 const Mat& mol_points, 00155 const Vec& weights, 00156 real& error ) 00157 { 00158 Mat M( 4, 4 ); 00159 Mat A( 4, 4 ); 00160 Mat rot( 3, 3 ); 00161 00162 int n = template_points.length(); 00163 Vec tp( 3 ); 00164 Vec mp( 3 ); 00165 00166 for( int i=0 ; i<n ; i++ ) 00167 { 00168 tp = template_points( i ); 00169 mp = mol_points( i ); 00170 00171 real weight = weights[ i ]; 00172 M(0,1) = tp[2]+mp[2]; 00173 M(0,2) = -tp[1]-mp[1]; 00174 M(0,3) = tp[0]-mp[0]; 00175 00176 M(1,0) = -tp[2]-mp[2]; 00177 M(1,2) = tp[0]+mp[0]; 00178 M(1,3) = tp[1]-mp[1]; 00179 00180 M(2,0) = tp[1]+mp[1]; 00181 M(2,1) = -tp[0]-mp[0]; 00182 M(2,3) = tp[2]-mp[2]; 00183 00184 M(3,0) = -tp[0]+mp[0]; 00185 M(3,1) = -tp[1]+mp[1]; 00186 M(3,2) = -tp[2]+mp[2]; 00187 00188 // A = A + transpose(M) * M * weight 00189 transposeProductAcc( A, M, M * weight ); 00190 } 00191 00192 Vec eigen_vals( 4 ); 00193 Mat eigen_vecs( 4, 4 ); 00194 00195 eigenVecOfSymmMat( A, 4, eigen_vals, eigen_vecs, false ); 00196 00197 error = eigen_vals[ 3 ]; 00198 real theta = 2.0 * acos( eigen_vecs( 3, 3 ) ); 00199 00200 if( theta !=0 ) 00201 { 00202 Vec axis = eigen_vecs.subMat( 3, 0, 1, 3 ).toVecCopy(); 00203 axis /= real(sin( theta/2.0 )); 00204 rot << rotationFromAxisAngle( axis, theta ); 00205 } 00206 else 00207 rot << diagonalmatrix( Vec( 3, 1 ) ); 00208 00209 return rot; 00210 } 00211 00212 Vec weightedCentroid( const Mat& points, const Vec& weights ) 00213 { 00214 Vec centroid( 3 ); 00215 int n = points.length(); 00216 real w_tot = 0; 00217 00218 for( int i=0 ; i<n ; i++ ) 00219 { 00220 real w = weights[i]; 00221 centroid += points(i) * w; 00222 w_tot += w; 00223 } 00224 00225 if( w_tot == 0 ) 00226 centroid.fill(0); 00227 else 00228 centroid /= w_tot; 00229 00230 return centroid; 00231 } 00232 00233 00234 } // end of namespace PLearn 00235 00236 00237 /* 00238 Local Variables: 00239 mode:c++ 00240 c-basic-offset:4 00241 c-file-style:"stroustrup" 00242 c-file-offsets:((innamespace . 0)(inline-open . 0)) 00243 indent-tabs-mode:nil 00244 fill-column:79 00245 End: 00246 */ 00247 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=79 :