PLearn 0.1
SurfaceTemplate/geometry.cc
Go to the documentation of this file.
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 :
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines