1 /* 2 * Copyright (C) 2011 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 /* $Id: db_framestitching.cpp,v 1.2 2011/06/17 14:03:30 mbansal Exp $ */ 18 19 #include "db_utilities.h" 20 #include "db_framestitching.h" 21 22 23 24 /***************************************************************** 25 * Lean and mean begins here * 26 *****************************************************************/ 27 28 inline void db_RotationFromMOuterProductSum(double R[9],double *score,double M[9]) 29 { 30 double N[16],q[4],lambda[4],lambda_max; 31 double y[4]; 32 int nr_roots; 33 34 N[0]= M[0]+M[4]+M[8]; 35 N[5]= M[0]-M[4]-M[8]; 36 N[10]= -M[0]+M[4]-M[8]; 37 N[15]= -M[0]-M[4]+M[8]; 38 N[1] =N[4] =M[5]-M[7]; 39 N[2] =N[8] =M[6]-M[2]; 40 N[3] =N[12]=M[1]-M[3]; 41 N[6] =N[9] =M[1]+M[3]; 42 N[7] =N[13]=M[6]+M[2]; 43 N[11]=N[14]=M[5]+M[7]; 44 45 /*get the quaternion representing the rotation 46 by finding the eigenvector corresponding to the most 47 positive eigenvalue. Force eigenvalue solutions, since the matrix 48 is symmetric and solutions might otherwise be lost 49 when the data is planar*/ 50 db_RealEigenvalues4x4(lambda,&nr_roots,N,1); 51 if(nr_roots) 52 { 53 lambda_max=lambda[0]; 54 if(nr_roots>=2) 55 { 56 if(lambda[1]>lambda_max) lambda_max=lambda[1]; 57 if(nr_roots>=3) 58 { 59 if(lambda[2]>lambda_max) lambda_max=lambda[2]; 60 { 61 if(nr_roots>=4) if(lambda[3]>lambda_max) lambda_max=lambda[3]; 62 } 63 } 64 } 65 } 66 else lambda_max=1.0; 67 db_EigenVector4x4(q,lambda_max,N); 68 69 /*Compute the rotation matrix*/ 70 db_QuaternionToRotation(R,q); 71 72 if(score) 73 { 74 /*Compute score=transpose(q)*N*q */ 75 db_Multiply4x4_4x1(y,N,q); 76 *score=db_ScalarProduct4(q,y); 77 } 78 } 79 80 void db_StitchSimilarity3DRaw(double *scale,double R[9],double t[3], 81 double **Xp,double **X,int nr_points,int orientation_preserving, 82 int allow_scaling,int allow_rotation,int allow_translation) 83 { 84 int i; 85 double c[3],cp[3],r[3],rp[3],M[9],s,sp,sc; 86 double Rr[9],score_p,score_r; 87 double *temp,*temp_p; 88 89 if(allow_translation) 90 { 91 db_PointCentroid3D(c,X,nr_points); 92 db_PointCentroid3D(cp,Xp,nr_points); 93 } 94 else 95 { 96 db_Zero3(c); 97 db_Zero3(cp); 98 } 99 100 db_Zero9(M); 101 s=sp=0; 102 for(i=0;i<nr_points;i++) 103 { 104 temp= *X++; 105 temp_p= *Xp++; 106 r[0]=(*temp++)-c[0]; 107 r[1]=(*temp++)-c[1]; 108 r[2]=(*temp++)-c[2]; 109 rp[0]=(*temp_p++)-cp[0]; 110 rp[1]=(*temp_p++)-cp[1]; 111 rp[2]=(*temp_p++)-cp[2]; 112 113 M[0]+=r[0]*rp[0]; 114 M[1]+=r[0]*rp[1]; 115 M[2]+=r[0]*rp[2]; 116 M[3]+=r[1]*rp[0]; 117 M[4]+=r[1]*rp[1]; 118 M[5]+=r[1]*rp[2]; 119 M[6]+=r[2]*rp[0]; 120 M[7]+=r[2]*rp[1]; 121 M[8]+=r[2]*rp[2]; 122 123 s+=db_sqr(r[0])+db_sqr(r[1])+db_sqr(r[2]); 124 sp+=db_sqr(rp[0])+db_sqr(rp[1])+db_sqr(rp[2]); 125 } 126 127 /*Compute scale*/ 128 if(allow_scaling) sc=sqrt(db_SafeDivision(sp,s)); 129 else sc=1.0; 130 *scale=sc; 131 132 /*Compute rotation*/ 133 if(allow_rotation) 134 { 135 if(orientation_preserving) 136 { 137 db_RotationFromMOuterProductSum(R,0,M); 138 } 139 else 140 { 141 /*Try preserving*/ 142 db_RotationFromMOuterProductSum(R,&score_p,M); 143 /*Try reversing*/ 144 M[6]= -M[6]; 145 M[7]= -M[7]; 146 M[8]= -M[8]; 147 db_RotationFromMOuterProductSum(Rr,&score_r,M); 148 if(score_r>score_p) 149 { 150 /*Reverse is better*/ 151 R[0]=Rr[0]; R[1]=Rr[1]; R[2]= -Rr[2]; 152 R[3]=Rr[3]; R[4]=Rr[4]; R[5]= -Rr[5]; 153 R[6]=Rr[6]; R[7]=Rr[7]; R[8]= -Rr[8]; 154 } 155 } 156 } 157 else db_Identity3x3(R); 158 159 /*Compute translation*/ 160 if(allow_translation) 161 { 162 t[0]=cp[0]-sc*(R[0]*c[0]+R[1]*c[1]+R[2]*c[2]); 163 t[1]=cp[1]-sc*(R[3]*c[0]+R[4]*c[1]+R[5]*c[2]); 164 t[2]=cp[2]-sc*(R[6]*c[0]+R[7]*c[1]+R[8]*c[2]); 165 } 166 else db_Zero3(t); 167 } 168 169 170
