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