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