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
db_RotationFromMOuterProductSum(double R[9],double * score,double M[9])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
db_StitchSimilarity3DRaw(double * scale,double R[9],double t[3],double ** Xp,double ** X,int nr_points,int orientation_preserving,int allow_scaling,int allow_rotation,int allow_translation)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