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
28inline 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
80void 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