int main( int argc, char** argv)
{
cv::Mat A = ( cv::Mat_<double>(3,3) << 0.5,0,1,0,1,0,1,0,0);
cv::Mat B = orth(A);
return 0;
}
void Polar::bvmg(double bt_longitude,double bt_latitude, double wp_longitude, double wp_latitude,
double w_angle, double w_speed,
double *heading, double *wangle)
{
double maxwangle,twaOrtho;
double maxheading;
double wanted_heading;
w_angle=degToRad(w_angle);
Orthodromie orth(bt_longitude,bt_latitude,wp_longitude,wp_latitude);
wanted_heading=orth.getAzimutRad();
twaOrtho = w_angle - wanted_heading;
myBvmgWind(twaOrtho,w_speed,&maxwangle);
maxheading = fmod((wanted_heading-maxwangle+twaOrtho), TWO_PI);
if (maxheading < 0)
{
maxheading += TWO_PI;
}
maxwangle = fmod(maxwangle, TWO_PI);
if (maxwangle > PI)
{
maxwangle -= TWO_PI;
} else if (maxwangle < -PI)
{
maxwangle += TWO_PI;
}
#if 0
qWarning() << "BVMG: Wind " << w_speed << "kts " << radToDeg(w_angle);
qWarning() << "BVMG Wind Angle : wanted_heading " << radToDeg(wanted_heading);
qWarning() << "BVMG Wind Angle : heading " << radToDeg(maxheading) << ", wind angle " << radToDeg(maxwangle);
#endif /* DEBUG */
*heading = radToDeg(maxheading);
*wangle = radToDeg(maxwangle);
}
vector<sp_mat> make_freebie_flow_bases_ignore_q(const sp_mat & value_basis,
const vector<sp_mat> blocks){
vector<sp_mat> flow_bases;
uint A = blocks.size();
for(uint a = 0; a < A; a++){
sp_mat raw_basis = blocks.at(a).t() * value_basis;
flow_bases.push_back(sp_mat(orth(mat(raw_basis))));
// Orthonorm (TODO: do directly in sparse?)
}
return flow_bases;
}
inline
bool
orth(Mat<typename T1::elem_type>& out, const Base<typename T1::elem_type, T1>& X, const typename T1::pod_type tol = 0.0, const typename arma_blas_type_only<typename T1::elem_type>::result* junk = 0)
{
arma_extra_debug_sigprint();
arma_ignore(junk);
try { out = orth(X,tol); } catch (std::runtime_error&) { return false; }
return true;
}
vector<sp_mat> make_freebie_flow_bases(const sp_mat & value_basis,
const vector<sp_mat> blocks,
const mat & Q){
vector<sp_mat> flow_bases;
uint A = blocks.size();
assert((A+1) == Q.n_cols);
for(uint a = 0; a < A; a++){
mat raw_basis = join_horiz(mat(blocks.at(a).t() * value_basis),
Q.col(a+1));
flow_bases.push_back(sp_mat(orth(raw_basis)));
// Orthonorm (TODO: do directly in sparse?)
}
return flow_bases;
}
void BattleField::addBorder(const Coord& pos)
{
Ship* ship = get(pos).parent();
if (ship) {
Coord inc = ship->increment();
Coord orth(inc.y, inc.x);
Coord p = pos - inc;
set(p, Element::BORDER);
for (; p != pos + inc * (ship->size() + 1); p += inc) {
set(p + orth, Element::BORDER);
set(p - orth, Element::BORDER);
}
p -= inc;
set(p, Element::BORDER);
}
}
sp_mat make_radial_fourier_basis(const Points & points,
uint K,double max_freq){
uint N = points.n_rows;
mat basis = mat(N,2*K+1);
vec r = lp_norm(points,2,1);
double omega;
for(uint k = 0; k < K; k++){
omega = ((double)k+1.0) * max_freq / (double) K;
omega *= 2.0*datum::pi;
basis.col(K-k-1) = sin(omega*r);
basis.col(K+k+1) = cos(omega*r);
}
basis.col(K).fill(1/sqrt(N));
basis = orth(basis); // Explicitly orthonormalize
return sp_mat(basis);
}
int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
{
int m=-1;
while(m==-1)
{
m = maxdirfiltered(p,count,dir,allow);
if(allow[m]==3) return m;
T u = orth(dir);
T v = cross(u,dir);
int ma=-1;
for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
{
btScalar s = sinf(SIMD_RADS_PER_DEG*(x));
btScalar c = cosf(SIMD_RADS_PER_DEG*(x));
int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
if(ma==m && mb==m)
{
allow[m]=3;
return m;
}
if(ma!=-1 && ma!=mb) // Yuck - this is really ugly
{
int mc = ma;
for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
{
btScalar s = sinf(SIMD_RADS_PER_DEG*(xx));
btScalar c = cosf(SIMD_RADS_PER_DEG*(xx));
int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
if(mc==m && md==m)
{
allow[m]=3;
return m;
}
mc=md;
}
}
ma=mb;
}
allow[m]=0;
m=-1;
}
btAssert(0);
return m;
}
mat make_rbf_basis(const Points & points,
const Points & centers,
double bandwidth,
double cutoff_thresh){
uint N = points.n_rows;
uint K = centers.n_rows;
mat basis = zeros<mat>(N,K+1);
basis.col(K) = ones<vec>(N);
for(uint k = 0; k < K; k++){
basis.col(k) = gaussian(points,centers.row(k).t(),bandwidth);
}
//basis(find(basis < cutoff_thresh)).fill(0);
basis = orth(basis); // Not ortho at all; need to do explicitly
if(basis.n_cols < (K+1)){
cerr << "WARNING: Basis degenerate..." << endl;
}
return basis;
}
sp_mat make_fourier_basis(const Points & points,
uint K,double max_freq){
uint N = points.n_rows;
mat basis = mat(N,K);
basis.col(0) = ones<vec>(N);
basis.col(1) = sin(datum::pi*sum(points,1));
basis.col(2) = sin(datum::pi*(points.col(0) - points.col(1)));
for(uint i = 3; i < K; i++){
vec freq = 2.0*datum::pi * randi<vec>(2, distr_param(1,(uint)max_freq));
vec flip = randn<vec>(1);
if(flip(0) > 0.5)
basis.col(i) = sin(points * freq);
else
basis.col(i) = cos(points * freq);
}
basis = orth(basis); // Explicitly orthonormalize
return sp_mat(basis);
}
void boatReal::setWP(double la, double lo, double w)
{
WP=QPointF(lo,la);
this->WPHd=w;
Orthodromie orth(this->lon,this->lat,lo,la);
this->dnm=qRound(orth.getDistance()*100)/100.00;
this->loxo=qRound(orth.getLoxoCap()*100)/100.00;
this->ortho=qRound(orth.getAzimutDeg()*100)/100.00;
if(loxo<0)
loxo+=360;
if(ortho<0)
ortho+=360;
this->vmg=qRound(this->speed*cos(degToRad(qAbs(this->heading-this->loxo)))*100)/100.00;
if(vmg<=0)
eta=-1;
else
{
eta=QDateTime::currentDateTimeUtc().toTime_t();
eta=eta+(3600/vmg)*dnm;
}
this->drawEstime();
}
void BattleField::addBorderSecondaryBoard(Ship* ship)
{
if ( !m_allow_adjacent_ships )
{
Coord inc = ship->increment();
Coord orth(inc.y, inc.x);
Coord p = ship->position() - inc;
if ( valid (p) ) {
m_secondary_board[p]=true;
}
for (; p != ship->position() + inc * (ship->size() + 1); p += inc) {
if ( valid( p + orth ) ) {
m_secondary_board[p + orth]=true;
}
if ( valid( p - orth ) ) {
m_secondary_board[p - orth]=true;
}
}
p -= inc;
if ( valid (p) ) {
m_secondary_board[p]=true;
}
}
}
int zgesv_idrs(
const size_t n,
// A is a function which multiplies the matrix by the first argument
// and returns the result in the second. The second argument must
// be manually cleared. The third parameter is user data, passed in
// through Adata.
void (*A)(const std::complex<double>*, std::complex<double>*, void*),
std::complex<double>* b,
std::complex<double>* x,
// Optional parameters
void *Adata = NULL,
size_t maxit = 0, // default is min(2*n,1000)
const size_t s = 4,
const double tol = 1e-8,
bool x_initialized = false,
// P is a precondition which simply solves P*x' = x,
// where x i the first argument. The second parameter is user data,
// which is passed in through Pdata.
void (*P)(std::complex<double>*, void*) = NULL,
void *Pdata = NULL,
double angle = 0.7
){
double normb = vecnorm(n, b);
if(0 == normb){
for(size_t i = 0; i < n; ++i){ x[i] = 0; }
return 0;
}
const double tolb = tol*normb; // compute tolerance
// Set initial x
if(!x_initialized){
for(size_t i = 0; i < n; ++i){ x[i] = 0; }
}
std::complex<double> *r = new std::complex<double>[n];
A(x,r,Adata);
for(size_t i = 0; i < n; ++i){ r[i] = b[i]-r[i]; }
double normr = vecnorm(n, r);
// Now, r = b-A*x
std::complex<double> *Q = new std::complex<double>[n*s];
{ // set up shadow space
for(size_t j = 0; j < s; ++j){
for(size_t i = 0; i < n; ++i){
Q[i+j*n] = (double)rand()/(double)RAND_MAX - 0.5;
}
}
// Orthogonalize Q
orth(n, s, Q);
}
std::complex<double> *G = new std::complex<double>[n*s];
std::complex<double> *U = new std::complex<double>[n*s];
std::complex<double> *M = new std::complex<double>[s*s];
std::complex<double> *Mcopy = new std::complex<double>[s*s];
size_t *pivots = new size_t[s];
for(size_t j = 0; j < s; ++j){
for(size_t i = 0; i < n; ++i){
G[i+j*n] = 0;
U[i+j*n] = 0;
}
for(size_t i = 0; i < s; ++i){
if(i == j){
M[i+j*s] = 1;
}else{
M[i+j*s] = 0;
}
}
}
std::complex<double> *f = new std::complex<double>[s];
std::complex<double> *c = new std::complex<double>[s];
std::complex<double> *v = new std::complex<double>[n];
std::complex<double> *t = new std::complex<double>[n];
size_t iter = 0;
std::complex<double> om = 1;
if(0 == maxit){
maxit = 2*n;
if(1000 < maxit){ maxit = 1000; }
}
int ret = 0;
while(normr > tolb && iter < maxit){
std::cout << "iter = " << iter << std::endl;
// generate RHS for small system
for(size_t j = 0; j < s; ++j){
std::complex<double> sum = 0;
for(size_t i = 0; i < n; ++i){
sum += r[i] * std::conj(Q[i+j*n]);
}
f[j] = sum;
}
for(size_t k = 0; k < s; ++k){
// solve small systems of M(k:s,k:s)*c(k:s) = f(k:s)
{
// Copy over stuff for a destructive LU solve in Mcopy
for(size_t j = k; j < s; ++j){
for(size_t i = k; i < s; ++i){
Mcopy[i+j*s] = M[i+j*s];
}
c[j] = f[j];
}
// Perform LU solve...
lu(s-k, s-k, s, &Mcopy[k+k*s], pivots);
lu_solve(s-k, s-k, s, &Mcopy[k+k*s], pivots, &c[k]);
}
// v = r - G(:,k:s)*c;
for(size_t i = 0; i < n; ++i){
std::complex<double> sum = 0;
for(size_t j = k; j < s; ++j){
sum += G[i+j*n]*c[j];
}
v[i] = r[i] - sum;
}
if(NULL != P){
P(v, Pdata);
}
//U(:,k) = U(:,k:s)*c + om*v;
for(size_t i = 0; i < n; ++i){
std::complex<double> sum = 0;
for(size_t j = k; j < s; ++j){
sum += U[i+j*n]*c[j];
}
U[i+k*n] = sum + om*v[i];
}
//G(:,k) = A*U(:,k);
A(&U[0+k*n], &G[0+k*n], Adata);
// Bi-Orthogonalise the new basis vectors
for(size_t j = 0; j < k; ++j){
std::complex<double> alpha = 0;
for(size_t i = 0; i < n; ++i){
alpha += std::conj(Q[i+j*n])*G[i+k*n];
}
alpha /= M[j+j*s];
for(size_t i = 0; i < n; ++i){
G[i+k*n] -= alpha*G[i+j*n];
}
for(size_t i = 0; i < n; ++i){
U[i+k*n] -= alpha*U[i+j*n];
}
}
// New column of M = (Q'*G)' (first k-1 entries are zero)
for(size_t j = k; j < s; ++j){
std::complex<double> sum = 0;
for(size_t i = 0; i < n; ++i){
sum += G[i+k*n]*std::conj(Q[i+j*n]);
}
M[j+k*s] = sum;
}
// Make r orthogonal to p_i, i = 1..k
std::complex<double> beta = f[k]/M[k+k*s];
for(size_t i = 0; i < n; ++i){
r[i] -= beta*G[i+k*n];
}
for(size_t i = 0; i < n; ++i){
x[i] += beta*U[i+k*n];
}
++iter;
normr = vecnorm(n, r);
if(normr < tolb || iter == maxit){ break; }
// New f = Q'*r (first k components are zero)
for(size_t j = k+1; j < s; ++j){
f[j] -= beta*M[j+k*s];
}
} // end k loop
// If we break'd out of the inner loop, do so again
if(normr < tolb){ break; }
// Now we have sufficient vectors in G_j to compute residual in G_j+1
// Note: r is already perpendicular to Q so v = r
for(size_t i = 0; i < n; ++i){ v[i] = r[i]; }
if(NULL != P){
P(v, Pdata);
}
A(v, t, Adata);
{ // compute new omega
double norms = vecnorm(n, r), normt = vecnorm(n, t);
std::complex<double> ts = 0;
for(size_t i = 0; i < n; ++i){
ts += std::conj(t[i])*r[i];
}
double rho = std::abs(ts/(normt*norms));
om = ts/(normt*normt);
if(rho < angle){
om *= angle/rho;
}
}
for(size_t i = 0; i < n; ++i){ r[i] -= om*t[i]; }
for(size_t i = 0; i < n; ++i){ x[i] += om*v[i]; }
normr = vecnorm(n, r);
++iter;
}
delete [] r;
delete [] G;
delete [] U;
delete [] M;
delete [] Mcopy;
delete [] f;
delete [] c;
delete [] v;
delete [] t;
return ret;
}
void mesh::output(file *xmshfile, int matid, int *allindices, int *allindpos, int *numprintedverts, float *min, float *max)
{
min[0] = 0; //-y
min[1] = 0; //z
min[2] = 0; //x
max[0] = 0;
max[1] = 0;
max[2] = 0;
struct record
{
int type;
int refer;
float x;
float y;
float z;
};
float *v;
float *tv;
int vertposinface = 0;
float vertnorm[3];
float tangent[3];
float tangent2[3];
int isrighthanded;
int isrighthanded2;
int tvert;
int tvert2;
int c;
int *allind = &allindices[allindpos[0]];
int numnewprintedverts = 0;
char *container;
//int *face;
int fmatid;
float *vnorm;
int vert;
record *rdrec;
record *rdrec2;
//float rdfnorm[3];
record rec;
twodim verts(((sizeof(record) + sizeof(int)) * 3 * facespermat[matid]), numverts);
for (int i = 0;i < numfaces;i++)
{
fmatid = getfmatid(i);
if (fmatid == matid)
{
rec.refer = i;
for (int j = 0;j < 3;j++)
{
vert = faces[5 * i + j];
vnorm = &vnormals[vnfaces[i * 3 + j] * 3];
rec.x = vnorm[0];
rec.y = vnorm[1];
rec.z = vnorm[2];
rec.type = j;
verts.adddata(vert, sizeof(record), ((char *) &rec));
}
}
}
container = verts.getcontainer();
for (int i = 0;i < numverts;i++)
{
c = verts.getcount(i);
for (int k = 0;k < c;k++)
{
rdrec = ((record *) verts.getdata(i, k));
if (rdrec->type >= 0)
{
vertnorm[0] = rdrec->x;
vertnorm[1] = rdrec->y;
vertnorm[2] = rdrec->z;
tvert = tvfaces[3 * rdrec->refer + rdrec->type];
getftangent(tangent, rdrec->refer, &isrighthanded);
for (int l = (k + 1);l < c;l++)
{
rdrec2 = ((record *) verts.getdata(i, l));
if (rdrec2->type >= 0)
{
tvert2 = tvfaces[3 * rdrec2->refer + rdrec2->type];
if ((abs(tverts[3 * tvert] - tverts[3 * tvert2]) < 0.00001) && (abs(tverts[3 * tvert + 1] - tverts[3 * tvert2 + 1]) < 0.00001))
//if (1 == 1)//
{
if (cosofangle(vertnorm,&rdrec2->x,1,1) > 0.98)
//if (1 == 1)//
{
getftangent(tangent2, rdrec2->refer, &isrighthanded2);
//if ((isrighthanded == isrighthanded2) && (cosofangle(tangent, tangent2, 1, 1) > 0.6))
if (1 == 1)//
{
if (rdrec->type < 3)
{
if (isrighthanded)
{
rdrec->type = 101;
}
else
{
rdrec->type = 100;
}
rdrec2->refer = rdrec->refer;
rdrec->refer = verts.getcontainerpos(i, l);
rdrec->x = tangent[0];
rdrec->y = tangent[1];
rdrec->z = tangent[2];
}
rdrec2->type = ((verts.getcontainerpos(i, k)) * (-1) - 1);
rdrec->x += tangent2[0];
rdrec->y += tangent2[1];
rdrec->z += tangent2[2];
}
}
}
}
}
if (rdrec->type < 3)
{
rdrec->type = isrighthanded;
}
}
}
}
for (int i = 0;i < numverts;i++)//
{
if (verts.getcount(i) == 0)
{
i = i;
}
}
for (int i = 0;i < 3 * facespermat[matid];i++)
{
rdrec = ((record *) &container[i * (sizeof(record) + sizeof(int)) + sizeof(int)]);
if (rdrec->type < 0)
{
rdrec2 = ((record *) &container[rdrec->type * (-1) - 1 + sizeof(int)]);
allind[i] = rdrec2->refer;
}
else
{
if (rdrec->type > 1)
{
rdrec2 = ((record *) &container[rdrec->refer + sizeof(int)]);
normalize(&rdrec->x);
orth(&rdrec->x, &rdrec2->x, tangent);
v = &this->verts[3 * faces[5 * rdrec2->refer + vertposinface]];
tv = &tverts[3 * tvfaces[3 * rdrec2->refer + vertposinface]];
xmshfile->writefloat(v[0]);
xmshfile->writefloat(v[2]);
xmshfile->writefloat(v[1]);
//min / max
if (i == 0)
{
min[0] = v[0];
min[1] = v[2];
min[2] = v[1];
max[0] = v[0];
max[1] = v[2];
max[2] = v[1];
}
if (v[1] < min[2])
{
min[2] = v[1];
}
if (v[0] < min[0])
{
min[0] = v[0];
}
if (v[2] < min[1])
{
min[1] = v[2];
}
if (v[1] > max[2])
{
max[2] = v[1];
}
if (v[0] > max[0])
{
max[0] = v[0];
}
if (v[2] > max[1])
{
max[1] = v[2];
}
xmshfile->writefloat(rdrec2->x);
xmshfile->writefloat(rdrec2->z);
xmshfile->writefloat(rdrec2->y);
if (rdrec->type == 101)
{
xmshfile->writefloat(tangent[0]);
xmshfile->writefloat(tangent[2]);
xmshfile->writefloat(tangent[1]);
xmshfile->writelong(4294967295); //0
}
else
{
xmshfile->writefloat(tangent[0] * (-1));
xmshfile->writefloat(tangent[2] * (-1));
xmshfile->writefloat(tangent[1] * (-1));
xmshfile->writelong(4294967295); //16711680
}
xmshfile->writelong(4278190080);
xmshfile->writefloat(tv[0]);
xmshfile->writefloat(tv[1] * (-1));
rdrec->refer = (numnewprintedverts + numprintedverts[0]);
}
else
{
getftangent(tangent, rdrec->refer);
memcpy(tangent2, tangent, 12);
orth(tangent2, &rdrec->x, tangent);
v = &this->verts[3 * faces[5 * rdrec->refer + vertposinface]];
tv = &tverts[3 * tvfaces[3 * rdrec->refer + vertposinface]];
xmshfile->writefloat(v[0]);
xmshfile->writefloat(v[2]);
xmshfile->writefloat(v[1]);
//min / max
if (i == 0)
{
min[0] = v[0];
min[1] = v[2];
min[2] = v[1];
max[0] = v[0];
max[1] = v[2];
max[2] = v[1];
}
if (v[1] < min[2])
{
min[2] = v[1];
}
if (v[0] < min[0])
{
min[0] = v[0];
}
if (v[2] < min[1])
{
min[1] = v[2];
}
if (v[1] > max[2])
{
max[2] = v[1];
}
if (v[0] > max[0])
{
max[0] = v[0];
}
if (v[2] > max[1])
{
max[1] = v[2];
}
/*
xmshfile->writefloat(rdrec->x);
xmshfile->writefloat(rdrec->z);
xmshfile->writefloat(rdrec->y);
*/
xmshfile->writefloat(0.0);
xmshfile->writefloat(1.0);
xmshfile->writefloat(0.0);
if (rdrec->type == 1)
{
xmshfile->writefloat(tangent[0]);
xmshfile->writefloat(tangent[2]);
xmshfile->writefloat(tangent[1]);
xmshfile->writelong(4294967295); //0
}
else
{
xmshfile->writefloat(tangent[0] * (-1));
xmshfile->writefloat(tangent[2] * (-1));
xmshfile->writefloat(tangent[1] * (-1));
xmshfile->writelong(4294967295); //16711680
}
xmshfile->writelong(4278190080);
xmshfile->writefloat(tv[0]);
xmshfile->writefloat(tv[1] * (-1));
}
allind[i] = (numnewprintedverts + numprintedverts[0]);
numnewprintedverts++;
}
vertposinface++;
if (vertposinface == 3)
{
vertposinface = 0;
}
}
allindpos[0] += (3 * facespermat[matid]);
numprintedverts[0] += numnewprintedverts;
}
//TODO Must make sure we always use 32F or 64F.
void PCAMerge::computeAdd()
{
// Add checks for errors, Exceptions
// TODO : Add checks against model, number of observations. Create eigenVals and eigenVecs accordingly. Refer Matlab code for possible checks to be implemented
//
// From here, going forward assuming no other errors possible
//
float eps = 1e-3;
// New number of Observations
nObs = N + M;
// TODO add check for nObs being zero
// New model's mean.
mean = ( (N * m1.mean) + (M * m2.mean) ) / nObs;
// Vector joining the centres
cv::Mat dorg = m1.mean - m2.mean;
// Note:
// dorg : n x 1
// m1.eigenvectors : n x p
// m2.eigenvectors : n x q
// G : p x q
// H : n x q
// g : p x 1
// h : n x 1
//Note Eigenvectors from cv::PCA are stored as rows. We need to transpose them.
//Note We should probably put the transposed vectors in other matrices, in order to leave the original models untouched (Luca)
m1.eigenvectors = m1.eigenvectors.t();
m2.eigenvectors = m2.eigenvectors.t();
//New Basis
cv::Mat G = m1.eigenvectors.t() * m2.eigenvectors;
cv::Mat H = m2.eigenvectors - ( m1.eigenvectors * G );// H is orthogonal to m1.eigenvectors
cv::Mat g = m1.eigenvectors.t() * dorg;
cv::Mat h = dorg - (m1.eigenvectors * g); // h is orthogonal to dorg
//Some vectors in H can be zero vectors. Must be removed
cv::Mat sumH = cv::Mat::zeros( 1, H.cols, CV_64FC1 );
cv::reduce( H.mul(H), sumH, 0, cv::REDUCE_SUM );
// Even h can be a zero vector. Must not be used if so
double sumh = 0;
sumh = h.dot(h);
//
// Get indices of sumH > eps. use it to construct vector nu
cv::Mat newH;
for( int i=0; i < sumH.cols; i++ )
{
if( sumH.at<double>(i) > eps )
newH.push_back( H.col(i).t() );
}
if (sumh > eps)
newH.push_back( h.t() );
newH = newH.t();
// Dimension of newH must be n x t
std::cout << newH.size() << std::endl;
//TODO : Implement Gram Schmidt Orthonormalization. DONE
cv::Mat nu = orth( newH );
//TODO : Forgetting about residues at the moment.
//Residues are the eigenvalues which were not used in the model m1 / m2.
//The following was used in matlab for including residues
//resn1 = size( m1.vct, 1) - size(m1.vct,2 );
/* if resn1 > 0
rpern1 = m1.residue / resn1;
else
rpern1 = 0;
end
resn2 = size( m2.vct, 1) - size(m2.vct,2 );
if resn2 > 0
rpern2 = m2.residue / resn2;
else
rpern2 = 0;
end
*/
//First part of the matrix in equation (20) in paper - Correlation of m1
//
int n,p,t,q;
n = m1.eigenvectors.rows; // = m2.eigenvectors.rows
t = nu.cols; //
p = m1.eigenvalues.rows;
q = m2.eigenvalues.rows;
cv::Mat tempeval = cv::Mat::zeros( (p + t) , 1, m1.eigenvalues.type() );
m1.eigenvalues.copyTo( tempeval.rowRange(0,m1.eigenvalues.rows) );
cv::Mat A1 = ( N / nObs ) * cv::Mat::diag(tempeval);
// Correlation of m2
cv::Mat Gamma = nu.t() * m2.eigenvectors;
cv::Mat D = G * cv::Mat::diag( m2.eigenvalues );
cv::Mat E = Gamma * cv::Mat::diag( m2.eigenvalues);
cv::Mat A2 = cv::Mat::zeros( A1.size(), A1.type() );
A2( cv::Range(0,p), cv::Range(0, p) ) = D * G.t();
A2( cv::Range(0,p), cv::Range(p, A1.cols) ) = D * Gamma.t();
A2( cv::Range(p, A1.rows), cv::Range(0,p) ) = E * G.t();
A2( cv::Range(p, A1.rows), cv::Range(p, A1.cols) ) = E * Gamma.t();
A2 = A2 * ( M / nObs );
//Third Part : term for diff between means
cv::Mat gamma = nu.t() * dorg;
cv::Mat A3 = cv::Mat( A1.size(), A1.type() );
A3( cv::Range(0,p), cv::Range(0,p) ) = g * g.t();
A3( cv::Range(0,p), cv::Range(p, A1.cols) ) = g * gamma.t();
A3( cv::Range(p, A1.rows), cv::Range(0,p) ) = gamma * g.t();
A3( cv::Range(p, A1.rows), cv::Range(p, A1.cols) ) = gamma * gamma.t();
A3 = ( N * M / (nObs*nObs) ) * A3;
// Guard against rounding errors
cv::Mat A = A1 + A2 + A3;
A = ( A + A.t() ) / 2.0;
/* (Luca)
Is this step right? Because PCA expects A to have samples inside, not correlation matrices. We should probably call
cv::eigen instead
[m3.vct m3.val] = eig( A ); % the eigen-solution
m3.vct = [m1.vct nu]* m3.vct; % rotate the basis set into place - can fail for v.high dim data
m3.val = diag(m3.val); % keep only the diagonal
*/
m3 = cv::PCA( A, cv::noArray(), cv::PCA::DATA_AS_ROW );
eigenVals = m3.eigenvalues;
m3.eigenvectors = m3.eigenvectors.t();
cv::Mat m3Temp = cv::Mat::zeros( n, A.cols, A.type() );
m3Temp( cv::Range::all(), cv::Range(0,p)) = m1.eigenvectors;
m3Temp( cv::Range::all(), cv::Range(p, A.cols)) = nu;
eigenVecs = m3Temp * m3.eigenvectors;
//Look at how many eigenvalues must be returned. Call that function as required.
//Calling the function like the matlab code.
//I'd try not to transpose the eigenvectors in order to be more OpenCV friendly.
int nValsToKeep = keepVals(KEEP_T,eigenVals,eps);
eigenVals = eigenVals(cv::Range(0,nValsToKeep),cv::Range::all()).clone();
eigenVecs = eigenVecs(cv::Range::all(),cv::Range(0,nValsToKeep)).clone();
}
void GRASTA_training(const mat &D,
mat &Uhat,
struct STATUS &status,
const struct GRASTA_OPT &options,
mat &W,
mat &Outlier
)
{
int rows, cols;
rows = D.n_rows; cols = D.n_cols;
if ( !status.init ){
status.init = 1;
status.curr_iter = 0;
status.last_mu = options.MIN_MU;
status.level = 0;
status.step_scale = 0.0;
status.last_w = zeros(options.RANK, 1);
status.last_gamma = zeros(options.DIM, 1);
if (!Uhat.is_finite()){
Uhat = orth(randn(options.DIM, options.RANK));
}
}
Outlier = zeros<mat>(rows, cols);
W = zeros<mat>(options.RANK, cols);
mat U_Omega, y_Omega, y_t, s, w, dual, gt;
uvec idx, col_order;
ADMM_OPT admm_opt;
double SCALE, t, rel;
bool bRet;
admm_opt.lambda = options.lambda;
//if (!options.QUIET)
int maxIter = options.maxCycles * cols; // 20 passes through the data set
status.hist_rel.reserve( maxIter);
// Order of examples to process
arma_rng::set_seed_random();
col_order = conv_to<uvec>::from(floor(cols*randu(maxIter, 1)));
for (int k=0; k<maxIter; k++){
int iCol = col_order(k);
//PRINTF("%d / %d\n",iCol, cols);
y_t = D.col(iCol);
idx = find_finite(y_t);
y_Omega = y_t.elem(idx);
SCALE = norm(y_Omega);
y_Omega = y_Omega/SCALE;
// the following for-loop is for U_Omega = U(idx,:) in matlab
U_Omega = zeros<mat>(idx.n_elem, Uhat.n_cols);
for (int i=0; i<idx.n_elem; i++)
U_Omega.row(i) = Uhat.row(idx(i));
// solve L-1 regression
admm_opt.MAX_ITER = options.MAX_ITER;
if (options.NORM_TYPE == L1_NORM)
bRet = ADMM_L1(U_Omega, y_Omega, admm_opt, s, w, dual);
else if (options.NORM_TYPE == L21_NORM){
w = solve(U_Omega, y_Omega);
s = y_Omega - U_Omega*w;
dual = -s/norm(s, 2);
}
else {
PRINTF("Error: norm type does not support!\n");
return;
}
vec tmp_col = zeros<vec>(rows);
tmp_col.elem(idx) = SCALE * s;
Outlier.col(iCol) = tmp_col;
W.col(iCol) = SCALE * w;
// take gradient step over Grassmannian
t = GRASTA_update(Uhat, status, w, dual, idx, options);
if (!options.QUIET){
rel = subspace(options.GT_mat, Uhat);
status.hist_rel.push_back(rel);
if (rel < options.TOL){
PRINTF("%d/%d: subspace angle %.2e\n",k,maxIter, rel);
break;
}
}
if (k % cols ==0){
if (!options.QUIET) PRINTF("Pass %d/%d: step-size %.2e, level %d, last mu %.2f\n",
k % cols, options.maxCycles, t, status.level, status.last_mu);
}
if (status.level >= options.convergeLevel){
// Must cycling around the dataset twice to get the correct regression weight W
if (!options.QUIET) PRINTF("Converge at level %d, last mu %.2f\n",status.level,status.last_mu);
break;
}
}
}
bool Polyangle::split(Polyangle & p1, Polyangle & p2, const Droite & d) const
{
Vector2D pointIntersection1;
int iBeforeInter1;
int iAfterInter1;
Vector2D pointIntersection2;
int iBeforeInter2 = -1;
int iAfterInter2 = -1;
bool inter2=false;
for(int i = 0; i < lesPoints.size(); i++)
{
Vector2D pointIntersection;
Vector2D pointCote1 = lesPoints[i];
Vector2D pointCote2 = lesPoints[(i+1)%lesPoints.size()];
Droite cote(pointCote1, (pointCote2-pointCote1));
if(d.getIntersection(cote, pointIntersection))
{
if(pointCote1.distanceToPoint2DSquared(pointIntersection) < pointCote1.distanceToPoint2DSquared(pointCote2)
&& pointCote2.distanceToPoint2DSquared(pointIntersection) < pointCote1.distanceToPoint2DSquared(pointCote2))
{
if(!inter2)
{
pointIntersection1 = pointIntersection;
iBeforeInter1=i;
iAfterInter1 = (i+1)%lesPoints.size();
inter2=true;
}
else
{
pointIntersection2 = pointIntersection;
iBeforeInter2=i;
iAfterInter2 = (i+1)%lesPoints.size();
break;
}
}
}
}
if(!inter2)
{
return false;
}
QVector<Vector2D> lesPoints1;
if(iAfterInter1 <= iBeforeInter2)
{
for(int i = iAfterInter1; i <= iBeforeInter2; i++)
{
lesPoints1.push_back(lesPoints[i]);
}
}
else
{
for(int i = iAfterInter1; i < lesPoints.size(); i++)
{
lesPoints1.push_back(lesPoints[i]);
}
for(int i = 0; i < iBeforeInter2; i++)
{
lesPoints1.push_back(lesPoints[i]);
}
}
lesPoints1.push_back(pointIntersection2);
lesPoints1.push_back(pointIntersection1);
QVector<Vector2D> lesPoints2;
if(iAfterInter2 <= iBeforeInter1)
{
for(int i = iAfterInter2; i <= iBeforeInter1; i++)
{
lesPoints2.push_back(lesPoints[i]);
}
}
else
{
for(int i = iAfterInter2; i < lesPoints.size(); i++)
{
lesPoints2.push_back(lesPoints[i]);
}
for(int i = 0; i <= iBeforeInter1; i++)
{
lesPoints2.push_back(lesPoints[i]);
}
}
lesPoints2.push_back(pointIntersection1);
lesPoints2.push_back(pointIntersection2);
Vector2D orth(-d.getD().y(), d.getD().x());
Vector2D point1(lesPoints1[0]- d.getO());
if(orth*point1 > 0)
{
p1 = Polyangle(lesPoints1);
p2 = Polyangle(lesPoints2);
}
else
{
p2 = Polyangle(lesPoints1);
p1 = Polyangle(lesPoints2);
}
return true;
}