Alignment::Alignment(PharmacophoreMap& fMap) :
_refMap(),
_dbMap(),
_refCenter(),
_dbCenter(),
_refRotMat(3,3,0.0),
_dbRotMat(3,3,0.0),
_dCdq(4,0.0),
_d2Cdq2(4,4,0.0),
_grad(4,0.0),
_AkA(fMap.size()),
_nbrPoints(0),
_nbrExcl(0)
{
// compute centers of the two pharmacophore sets
PharmacophoreMap::iterator mi;
unsigned int nbrMatch(0);
// compute the centroid of the pharmacophores
double V1(0.0), v1(0.0), V2(0.0), v2(0.0);
for (mi = fMap.begin(); mi != fMap.end(); ++mi)
{
if (mi->first->func == EXCL)
{
_nbrExcl++;
continue; // do not use exclusion spheres when computing the center
}
_nbrPoints++;
v1 = GCI * pow(PI/mi->first->alpha,1.5);
V1 += v1;
_refCenter.x += v1 * mi->first->point.x;
_refCenter.y += v1 * mi->first->point.y;
_refCenter.z += v1 * mi->first->point.z;
v2 = GCI * pow(PI/mi->second->alpha,1.5);
V2 += v2;
_dbCenter.x += v2 * mi->second->point.x;
_dbCenter.y += v2 * mi->second->point.y;
_dbCenter.z += v2 * mi->second->point.z;
++nbrMatch;
}
_refCenter.x /= V1;
_refCenter.y /= V1;
_refCenter.z /= V1;
_dbCenter.x /= V2;
_dbCenter.y /= V2;
_dbCenter.z /= V2;
// translate the pharmacophores to the centers
// and compute center of mass matrix
SiMath::Matrix mass1(3,3,0.0);
SiMath::Matrix mass2(3,3,0.0);
for (mi = fMap.begin(); mi != fMap.end(); ++mi)
{
PharmacophorePoint p1, p2;
p1.point.x = mi->first->point.x - _refCenter.x;
p1.point.y = mi->first->point.y - _refCenter.y;
p1.point.z = mi->first->point.z - _refCenter.z;
p1.func = mi->first->func;
p1.alpha = mi->first->alpha;
p1.normal.x = mi->first->normal.x - mi->first->point.x;
p1.normal.y = mi->first->normal.y - mi->first->point.y;
p1.normal.z = mi->first->normal.z - mi->first->point.z;
p2.point.x = mi->second->point.x - _dbCenter.x;
p2.point.y = mi->second->point.y - _dbCenter.y;
p2.point.z = mi->second->point.z - _dbCenter.z;
p2.func = mi->second->func;
p2.alpha = mi->second->alpha;
p2.normal.x = mi->second->normal.x - mi->second->point.x;
p2.normal.y = mi->second->normal.y - mi->second->point.y;
p2.normal.z = mi->second->normal.z - mi->second->point.z;
if (mi->first->func != EXCL)
{
v1 = GCI * pow(PI/mi->first->alpha,1.5);
mass1[0][0] += v1 * p1.point.x * p1.point.x;
mass1[0][1] += v1 * p1.point.x * p1.point.y;
mass1[0][2] += v1 * p1.point.x * p1.point.z;
mass1[1][1] += v1 * p1.point.y * p1.point.y;
mass1[1][2] += v1 * p1.point.y * p1.point.z;
mass1[2][2] += v1 * p1.point.z * p1.point.z;
v2 = GCI * pow(PI/mi->second->alpha,1.5);
mass2[0][0] += v2 * p2.point.x * p2.point.x;
mass2[0][1] += v2 * p2.point.x * p2.point.y;
mass2[0][2] += v2 * p2.point.x * p2.point.z;
mass2[1][1] += v2 * p2.point.y * p2.point.y;
mass2[1][2] += v2 * p2.point.y * p2.point.z;
mass2[2][2] += v2 * p2.point.z * p2.point.z;
}
// add new points to local maps
_refMap.push_back(p1);
_dbMap.push_back(p2);
}
// use SVD to compute best rotations
// set lower triangle
mass1[1][0] = mass1[0][1];
mass1[2][0] = mass1[0][2];
mass1[2][1] = mass1[1][2];
// normalize mass matrix
mass1 /= V1;
// compute SVD of the mass matrix
SiMath::SVD svd(mass1, true, true);
_refRotMat = svd.getU();
// check if determinant is 1, otherwise it is a mirroring instead of rotation
double det = _refRotMat[0][0]*_refRotMat[1][1]*_refRotMat[2][2]
+ _refRotMat[2][1]*_refRotMat[1][0]*_refRotMat[0][2]
+ _refRotMat[0][1]*_refRotMat[1][2]*_refRotMat[2][0]
- _refRotMat[0][0]*_refRotMat[1][2]*_refRotMat[2][1]
- _refRotMat[1][1]*_refRotMat[2][0]*_refRotMat[0][2]
- _refRotMat[2][2]*_refRotMat[0][1]*_refRotMat[1][0];
// check if it is a rotation matrix and not a mirroring
if (det < 0)
{
// switch sign of third column
_refRotMat[0][2] = -_refRotMat[0][2];
_refRotMat[1][2] = -_refRotMat[1][2];
_refRotMat[2][2] = -_refRotMat[2][2];
}
// set lower triangle
mass2[1][0] = mass2[0][1];
mass2[2][0] = mass2[0][2];
mass2[2][1] = mass2[1][2];
// normalize mass matrix
mass2 /= V2;
// compute SVD of the mass matrix
SiMath::SVD svd2(mass2, true, true);
_dbRotMat = svd2.getU();
// check if determinant is 1, otherwise it is a mirroring instead of rotation
det = _dbRotMat[0][0]*_dbRotMat[1][1]*_dbRotMat[2][2]
+ _dbRotMat[2][1]*_dbRotMat[1][0]*_dbRotMat[0][2]
+ _dbRotMat[0][1]*_dbRotMat[1][2]*_dbRotMat[2][0]
- _dbRotMat[0][0]*_dbRotMat[1][2]*_dbRotMat[2][1]
- _dbRotMat[1][1]*_dbRotMat[2][0]*_dbRotMat[0][2]
- _dbRotMat[2][2]*_dbRotMat[0][1]*_dbRotMat[1][0];
// checif if it is a rotation matrix and not a mirroring
if (det < 0)
{
// switch sign of third column
_dbRotMat[0][2] = -_dbRotMat[0][2];
_dbRotMat[1][2] = -_dbRotMat[1][2];
_dbRotMat[2][2] = -_dbRotMat[2][2];
}
// rotate points towards main axes
for (unsigned int i(0); i < _refMap.size(); ++i)
{
// Rotate points
double x = _refMap[i].point.x;
double y = _refMap[i].point.y;
double z = _refMap[i].point.z;
_refMap[i].point.x = _refRotMat[0][0]*x + _refRotMat[1][0]*y + _refRotMat[2][0]*z;
_refMap[i].point.y = _refRotMat[0][1]*x + _refRotMat[1][1]*y + _refRotMat[2][1]*z;
_refMap[i].point.z = _refRotMat[0][2]*x + _refRotMat[1][2]*y + _refRotMat[2][2]*z;
x = _dbMap[i].point.x;
y = _dbMap[i].point.y;
z = _dbMap[i].point.z;
_dbMap[i].point.x = _dbRotMat[0][0]*x + _dbRotMat[1][0]*y + _dbRotMat[2][0]*z;
_dbMap[i].point.y = _dbRotMat[0][1]*x + _dbRotMat[1][1]*y + _dbRotMat[2][1]*z;
_dbMap[i].point.z = _dbRotMat[0][2]*x + _dbRotMat[1][2]*y + _dbRotMat[2][2]*z;
double dx = _refMap[i].point.x - _dbMap[i].point.x;
double dx2 = dx * dx;
double dy = _refMap[i].point.y - _dbMap[i].point.y;
double dy2 = dy * dy;
double dz = _refMap[i].point.z - _dbMap[i].point.z;
double dz2 = dz * dz;
double sx = _refMap[i].point.x + _dbMap[i].point.x;
double sx2 = sx * sx;
double sy = _refMap[i].point.y + _dbMap[i].point.y;
double sy2 = sy * sy;
double sz = _refMap[i].point.z + _dbMap[i].point.z;
double sz2 = sz * sz;
_AkA[i] = new SiMath::Matrix(4,4,0.0);
(*_AkA[i])[0][0] = dx2 + dy2 + dz2;
(*_AkA[i])[0][1] = dy*sz - sy*dz;
(*_AkA[i])[0][2] = sx*dz - dx*sz;
(*_AkA[i])[0][3] = dx*sy - sx*dy;
(*_AkA[i])[1][0] = (*_AkA[i])[0][1];
(*_AkA[i])[1][1] = dx2 + sy2 + sz2;
(*_AkA[i])[1][2] = dx*dy - sx*sy;
(*_AkA[i])[1][3] = dx*dz - sx*sz;
(*_AkA[i])[2][0] = (*_AkA[i])[0][2];
(*_AkA[i])[2][1] = (*_AkA[i])[1][2];
(*_AkA[i])[2][2] = sx2 + dy2 + sz2;
(*_AkA[i])[2][3] = dy*dz - sy*sz;
(*_AkA[i])[3][0] = (*_AkA[i])[0][3];
(*_AkA[i])[3][1] = (*_AkA[i])[1][3];
(*_AkA[i])[3][2] = (*_AkA[i])[2][3];
(*_AkA[i])[3][3] = sx2 + sy2 + dz2;
// Rotate normals
x = _refMap[i].normal.x;
y = _refMap[i].normal.y;
z = _refMap[i].normal.z;
_refMap[i].normal.x = _refRotMat[0][0]*x + _refRotMat[1][0]*y + _refRotMat[2][0]*z;
_refMap[i].normal.y = _refRotMat[0][1]*x + _refRotMat[1][1]*y + _refRotMat[2][1]*z;
_refMap[i].normal.z = _refRotMat[0][2]*x + _refRotMat[1][2]*y + _refRotMat[2][2]*z;
x = _dbMap[i].normal.x;
y = _dbMap[i].normal.y;
z = _dbMap[i].normal.z;
_dbMap[i].normal.x = _dbRotMat[0][0]*x + _dbRotMat[1][0]*y + _dbRotMat[2][0]*z;
_dbMap[i].normal.y = _dbRotMat[0][1]*x + _dbRotMat[1][1]*y + _dbRotMat[2][1]*z;
_dbMap[i].normal.z = _dbRotMat[0][2]*x + _dbRotMat[1][2]*y + _dbRotMat[2][2]*z;
}
return;
}
void qrstep (gsl_vector * d, gsl_vector * f, gsl_matrix * U,
gsl_matrix * V) {
const size_t M=U->size1;
const size_t N=V->size1;
const size_t n=d->size;
double y, z;
double ak, bk, zk, ap, bp, aq, bq;
size_t i, k;
//std::cout << "M,N,n: " << M << " " << N << " " << n << std::endl;
if (n == 1)
return; /* shouldn't happen */
/* Compute 2x2 svd directly */
if (n == 2)
{
svd2 (d, f, U, V);
return;
}
/* Chase out any zeroes on the diagonal */
for (i=0; i < n - 1; i++)
{
double d_i=gsl_vector_get (d, i);
//std::cout << "d_i: " << i << " " << n << " "
//<< d_i << std::endl;
if (d_i == 0.0)
{
chase_out_intermediate_zero (d, f, U, i);
return;
}
}
/* Chase out any zero at the end of the diagonal */
{
double d_nm1=gsl_vector_get (d, n - 1);
//std::cout << "d_nm1: " << d_nm1 << std::endl;
if (d_nm1 == 0.0)
{
chase_out_trailing_zero (d, f, V);
return;
}
}
/* Apply QR reduction steps to the diagonal and offdiagonal */
{
double d0=gsl_vector_get (d, 0);
double f0=gsl_vector_get (f, 0);
double d1=gsl_vector_get (d, 1);
double f1=gsl_vector_get (f, 1);
//std::cout << "d0,f0,d1,f1: " << d0 << " " << f0 << " " << d1 << " "
//<< f1 << std::endl;
{
double mu=trailing_eigenvalue (d, f);
y=d0 * d0 - mu;
z=d0 * f0;
}
/* Set up the recurrence for Givens rotations on a bidiagonal matrix */
ak=0;
bk=0;
ap=d0;
bp=f0;
aq=d1;
bq=f1;
}
for (k=0; k < n - 1; k++)
{
double c, s;
create_givens (y, z, &c, &s);
/* Compute V <= V G */
for (i=0; i < N; i++)
{
double Vip=gsl_matrix_get (V, i, k);
double Viq=gsl_matrix_get (V, i, k + 1);
//std::cout << "Vip,Viq: " << Vip << " " << Viq << std::endl;
gsl_matrix_set (V, i, k, c * Vip - s * Viq);
gsl_matrix_set (V, i, k + 1, s * Vip + c * Viq);
}
/* compute B <= B G */
{
double bk1=c * bk - s * z;
double ap1=c * ap - s * bp;
double bp1=s * ap + c * bp;
double zp1=-s * aq;
double aq1=c * aq;
if (k > 0)
{
gsl_vector_set (f, k - 1, bk1);
}
ak=ap1;
bk=bp1;
zk=zp1;
ap=aq1;
if (k < n - 2)
{
bp=gsl_vector_get (f, k + 1);
}
else
{
bp=0.0;
}
y=ak;
z=zk;
}
create_givens (y, z, &c, &s);
/* Compute U <= U G */
for (i=0; i < M; i++)
{
double Uip=gsl_matrix_get (U, i, k);
double Uiq=gsl_matrix_get (U, i, k + 1);
//std::cout << "Uip2,Uiq2: " << Uip << " " << Uiq << std::endl;
gsl_matrix_set (U, i, k, c * Uip - s * Uiq);
gsl_matrix_set (U, i, k + 1, s * Uip + c * Uiq);
}
/* compute B <= G^T B */
//std::cout << "k,bk,ap2: " << k << " " << bk << " " << ap << std::endl;
//std::cout << "ak,zk,bp: " << ak << " " << zk << " "
// << bp << std::endl;
{
//std::cout << "prod1: " << c*ak << " " << s*zk << std::endl;
//std::cout << "prod2: " << c*bk << " " << s*ap << std::endl;
//std::cout << "prod3: " << s*bk << " " << c*ap << std::endl;
double ak1=c * ak - s * zk;
double bk1=c * bk - s * ap;
double zk1=-s * bp;
double ap1=s * bk + c * ap;
double bp1=c * bp;
gsl_vector_set (d, k, ak1);
ak=ak1;
bk=bk1;
zk=zk1;
ap=ap1;
bp=bp1;
//std::cout << "c,s: " << c << " " << s << std::endl;
//std::cout << "k,bk,ap: " << k << " " << bk << " " << ap << std::endl;
if (k < n - 2)
{
aq=gsl_vector_get (d, k + 2);
}
else
{
aq=0.0;
}
y=bk;
z=zk;
}
}
gsl_vector_set (f, n - 2, bk);
gsl_vector_set (d, n - 1, ap);
//std::cout << "bk,ap: " << bk << " " << ap << std::endl;
}
matf GetExtrinsicsFromEssential(const matf & essMat_, const TrackedPoint & one_point,
bool correct_essMat, int c) {
matf essMat;
if (correct_essMat) {
SVD svd2(essMat_);
matf D = matf(3,3,0.0f);
D(0,0) = D(1,1) = (svd2.w.at<float>(0,0) + svd2.w.at<float>(1,0)) * 0.5f;
essMat = svd2.u * D * svd2.vt;
} else {
matf essMat = essMat_;
}
SVD svd(essMat);
//assert(epsEqual(D(0,0) / D(1,0), 1.0, 0.1) && epsEqual(D(2,0), 0.0f));
matf W(3,3,0.0f); //TODO do not recreate at each call
W(0,1) = -1.0f;
W(1,0) = W(2,2) = 1.0f;
matf extr(3,4);
matf tmp;
//case 1
tmp = svd.u*W*svd.vt;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
extr(i,j) = tmp(i,j);
for (int i = 0; i < 3; ++i)
extr(i, 3) = svd.u.at<float>(i, 2);
//cout << extr << endl;
if ((c == 1) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 1" << endl;
return extr;
}
//case 2
for (int i = 0; i < 3; ++i)
extr(i, 3) = -svd.u.at<float>(i, 2);
//cout << extr << endl;
if ((c == 2) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 2" << endl;
return extr;
}
//case 3
tmp = svd.u*W.t()*svd.vt;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
extr(i,j) = tmp(i,j);
//cout << extr << endl;
if ((c == 3) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 3" << endl;
return extr;
}
//case 4
for (int i = 0; i < 3; ++i)
extr(i, 3) = svd.u.at<float>(i, 2);
//cout << extr << endl;
if ((c == 4) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 4" << endl;
return extr;
}
//assert(false); //this should not happen. If it does, sth is wrong
// (the point might be the on the principal plane, or there is a bug)
return matf(0,0); // remove warning
}
