int MetricAnIso::IntersectWith(const MetricAnIso M2)
{
//cerr << " - " << *this << M2 << endl;
int r=0;
MetricAnIso & M1 = *this;
D2xD2 M;
double l1,l2;
ReductionSimultanee(*this,M2,l1,l2,M);
// cerr << M << endl;
R2 v1(M.x.x,M.y.x);
R2 v2(M.x.y,M.y.y);
double l11=M1(v1,v1);
double l12=M1(v2,v2);
double l21=M2(v1,v1);
double l22=M2(v2,v2);
if ( l11 < l21 ) r=1,l11=l21;
if ( l12 < l22 ) r=1,l12=l22;
// cerr << r << endl;
if (r) { // change
D2xD2 M_1(M.inv());
D2xD2 D(l11,0,0,l12);
D2xD2 Mi(M_1.t()*D*M_1);
a11=Mi.x.x;
a21=0.5*(Mi.x.y+Mi.y.x);
a22=Mi.y.y; }
return r;
}
QSolventBox::QSolventBox(QWidget *parent) :
QComboBox(parent), ActiveDir(NULL)
{
setFocusPolicy(Qt::ClickFocus);
Map.insert("Custom", "");
Map.insert("TestSpec", ":/GS_Spectra/TestSpec.txt");
Map.insert("Acetonitrile", ":/GS_Spectra/Acetonitrile.txt");
Map.insert("Acetone", ":/GS_Spectra/Acetone.txt");
Map.insert("Methanol", ":/GS_Spectra/Methanol.txt");
Map.insert("Toluene", ":/GS_Spectra/Toluene.txt");
Map.insert("Chloroform", ":/GS_Spectra/Chloroform.txt");
Map.insert("N-Hexane", ":/GS_Spectra/N-Hexane.txt");
Map.insert("THF", ":/GS_Spectra/THF.txt");
int i=0; // Sunumeravimas nebutinas.
QMapIterator<QString, QString> Mi(Map);
while (Mi.hasNext())
{
Mi.next();
if (Mi.key()!="TestSpec" && Mi.key()!="Custom") addItem(Mi.key(),i++);
// Testas - pasleptas, custom - paskutinis.
}
addItem("Custom",i);
connect(this,SIGNAL(currentIndexChanged(int)),this,SLOT(solventIndexChanged(int)));
}
MetricAnIso Intersection(const MetricAnIso M1,const MetricAnIso M2)
{
D2xD2 M;
double l1,l2;
ReductionSimultanee(M1,M2,l1,l2,M);
R2 v0(M.x.x,M.y.x);
R2 v1(M.x.y,M.y.y);
D2xD2 M_1(M.inv());
D2xD2 D(Max(M1(v0,v0),M2(v0,v0)),0,0,Max(M1(v1,v1),M2(v1,v1)));
D2xD2 Mi(M_1.t()*D*M_1);
return MetricAnIso(Mi.x.x,0.5*(Mi.x.y+Mi.y.x),Mi.y.y);
}
void Suiji(char* Path)
{
SYSTEMTIME stLocal;
GetLocalTime(&stLocal);
TCHAR suiji[256];
wsprintf(suiji,"%i%i%i%i%i%i",stLocal.wYear,stLocal.wMonth,stLocal.wDay,stLocal.wHour,stLocal.wMinute,stLocal.wSecond);
DeleteFile("C:\\1.txt");
HANDLE hFile1;
DWORD dwBytesWritten;
hFile1 = CreateFile("C:\\1.txt",GENERIC_WRITE,FILE_SHARE_READ,NULL,CREATE_ALWAYS,NULL,NULL);
if (hFile1 == INVALID_HANDLE_VALUE) return ;
int i;
for (i=0;i<10;i++)
{
WriteFile(hFile1, Mi(suiji), strlen(suiji), &dwBytesWritten, NULL);
}
CloseHandle(hFile1);
HANDLE hFile;
hFile = CreateFile("C:\\1.txt", GENERIC_READ, NULL, NULL, OPEN_EXISTING,FILE_ATTRIBUTE_NORMAL, NULL);
DWORD nSizeOfSrcFile = GetFileSize( hFile, &nSizeOfSrcFile );
char *szSrcFileBuf = new char[ nSizeOfSrcFile ];
ReadFile( hFile, szSrcFileBuf, nSizeOfSrcFile, &nSizeOfSrcFile, NULL);
HANDLE hUpdate;
BOOL ret;
hUpdate = BeginUpdateResource(Path, false);
char suiji1[256];
char suiji2[256];
lstrcpy(suiji1,suiji);
lstrcpy(suiji2,suiji);
ret = UpdateResource(hUpdate, Mi2(suiji1), Mi(suiji2+1), 0, szSrcFileBuf, nSizeOfSrcFile);
if (!ret)
{
CloseHandle(hFile);
}
EndUpdateResource( hUpdate, false );
CloseHandle(hFile);
DeleteFile("C:\\1.txt");
}
Vector sampleVNormal(const Vector &wi, Sampler *sampler, Float Sxx, Float Syy, Float Szz,
Float Sxy, Float Sxz, Float Syz) const {
Float u1 = sampler->next1D();
Float u2 = sampler->next1D();
Float r = sqrtf(u1);
Float phi = 2.f * M_PI * u2;
Float u = r * cosf(phi);
Float v = r * sinf(phi);
Float w = sqrtf(1.f - u * u - v * v);
Vector wk, wj;
Frame frame(wi);
wk = frame.s;
wj = frame.t;
// project S in this basis
Float Skk = wk.x * wk.x * Sxx + wk.y * wk.y * Syy + wk.z * wk.z * Szz +
2.f * (wk.x * wk.y * Sxy + wk.x * wk.z * Sxz + wk.y * wk.z * Syz);
Float Sjj = wj.x * wj.x * Sxx + wj.y * wj.y * Syy + wj.z * wj.z * Szz +
2.f * (wj.x * wj.y * Sxy + wj.x * wj.z * Sxz + wj.y * wj.z * Syz);
Float Sii = wi.x * wi.x * Sxx + wi.y * wi.y * Syy + wi.z * wi.z * Szz +
2.f * (wi.x * wi.y * Sxy + wi.x * wi.z * Sxz + wi.y * wi.z * Syz);
Float Skj = wk.x * wj.x * Sxx + wk.y * wj.y * Syy + wk.z * wj.z * Szz +
(wk.x * wj.y + wk.y * wj.x) * Sxy + (wk.x * wj.z + wk.z * wj.x) * Sxz +
(wk.y * wj.z + wk.z * wj.y) * Syz;
Float Ski = wk.x * wi.x * Sxx + wk.y * wi.y * Syy + wk.z * wi.z * Szz +
(wk.x * wi.y + wk.y * wi.x) * Sxy + (wk.x * wi.z + wk.z * wi.x) * Sxz +
(wk.y * wi.z + wk.z * wi.y) * Syz;
Float Sji = wj.x * wi.x * Sxx + wj.y * wi.y * Syy + wj.z * wi.z * Szz +
(wj.x * wi.y + wj.y * wi.x) * Sxy + (wj.x * wi.z + wj.z * wi.x) * Sxz +
(wj.y * wi.z + wj.z * wi.y) * Syz;
Float sqrtDetSkji = sqrtf(fabs(Skk * Sjj * Sii - Skj * Skj * Sii - Ski * Ski * Sjj -
Sji * Sji * Skk + 2.f * Skj * Ski * Sji));
Float invSqrtSii = 1.f / sqrt(Sii);
Float tmp = sqrtf(Sjj * Sii - Sji * Sji);
Vector Mk(sqrtDetSkji / tmp, 0.f, 0.f);
Vector Mj(-invSqrtSii * (Ski * Sji - Skj * Sii) / tmp, invSqrtSii * tmp, 0);
Vector Mi(invSqrtSii * Ski, invSqrtSii * Sji, invSqrtSii * Sii);
Vector wm_kji = normalize(u * Mk + v * Mj + w * Mi);
return normalize(wm_kji.x * wk + wm_kji.y * wj + wm_kji.z * wi);
}
vec3 sample_VNDF(const vec3 wi,
const float S_xx, const float S_yy, const float S_zz,
const float S_xy, const float S_xz, const float S_yz,
const float U1, const float U2)
{
// generate sample (u, v, w)
const float r = sqrtf(U1);
const float phi = 2.0f*M_PI*U2;
const float u = r*cosf(phi);
const float v = r*sinf(phi);
const float w = sqrtf(1.0f - u*u - v*v);
// build orthonormal basis
vec3 wk, wj;
buildOrthonormalBasis(wk, wj, wi);
// project S in this basis
const float S_kk = wk.x*wk.x*S_xx + wk.y*wk.y*S_yy + wk.z*wk.z*S_zz
+ 2.0f * (wk.x*wk.y*S_xy + wk.x*wk.z*S_xz + wk.y*wk.z*S_yz);
const float S_jj = wj.x*wj.x*S_xx + wj.y*wj.y*S_yy + wj.z*wj.z*S_zz
+ 2.0f * (wj.x*wj.y*S_xy + wj.x*wj.z*S_xz + wj.y*wj.z*S_yz);
const float S_ii = wi.x*wi.x*S_xx + wi.y*wi.y*S_yy + wi.z*wi.z*S_zz
+ 2.0f * (wi.x*wi.y*S_xy + wi.x*wi.z*S_xz + wi.y*wi.z*S_yz);
const float S_kj = wk.x*wj.x*S_xx + wk.y*wj.y*S_yy + wk.z*wj.z*S_zz
+ (wk.x*wj.y + wk.y*wj.x)*S_xy
+ (wk.x*wj.z + wk.z*wj.x)*S_xz
+ (wk.y*wj.z + wk.z*wj.y)*S_yz;
const float S_ki = wk.x*wi.x*S_xx + wk.y*wi.y*S_yy + wk.z*wi.z*S_zz
+ (wk.x*wi.y + wk.y*wi.x)*S_xy + (wk.x*wi.z + wk.z*wi.x)*S_xz + (wk.y*wi.z + wk.z*wi.y)*S_yz;
const float S_ji = wj.x*wi.x*S_xx + wj.y*wi.y*S_yy + wj.z*wi.z*S_zz
+ (wj.x*wi.y + wj.y*wi.x)*S_xy
+ (wj.x*wi.z + wj.z*wi.x)*S_xz
+ (wj.y*wi.z + wj.z*wi.y)*S_yz;
// compute normal
float sqrtDetSkji = sqrtf(fabsf(S_kk*S_jj*S_ii - S_kj*S_kj*S_ii - S_ki*S_ki*S_jj - S_ji*S_ji*S_kk + 2.0f*S_kj*S_ki*S_ji));
float inv_sqrtS_ii = 1.0f / sqrtf(S_ii);
float tmp = sqrtf(S_jj*S_ii - S_ji*S_ji);
vec3 Mk(sqrtDetSkji / tmp, 0.0f, 0.0f);
vec3 Mj(-inv_sqrtS_ii*(S_ki*S_ji - S_kj*S_ii) / tmp, inv_sqrtS_ii*tmp, 0);
vec3 Mi(inv_sqrtS_ii*S_ki, inv_sqrtS_ii*S_ji, inv_sqrtS_ii*S_ii);
vec3 wm_kji = normalize(u*Mk + v*Mj + w*Mi);
// rotate back to world basis
return wm_kji.x * wk + wm_kji.y * wj + wm_kji.z * wi;
}
double cisstAlgorithmICP_IMLP::ICP_EvaluateErrorFunction()
{
#ifdef COMPUTE_ERROR_FUNCTION
// TODO: do something different if "REMOVE_OUTLIERS" enabled?
//
// Compute negative log-likelihood of match probabilities for error function
//
// likelihood function: Product_i[ (1/sqrt((2*pi)^3*|Mi|) * exp( (-1/2)*(Yi-R*Xi-t)'*inv(Mi)*(Yi-R*Xi-t) ) ]
// where Mi = covariance of error in Yi-R*Xi-t (Mi = R*Mxi*R' + Myi)
// Mxi = covariance of measurement error for source point xi
// Myi = covariance of measurement error for target point yi
//
// If C's are same:
// -loglik: -Sum_i[ log(Ci) ] + (1/2)*Sum_i[ di'*inv(M)*di ]
// where Ci = 1/(sqrt((2*pi)^3*|Mi|)
//
// -loglik = (3/2)*N*log(2*pi) + (1/2)*Sum_i[log(Mi)] + (1/2)*Sum_i[ di'*inv(M)*di ]
//
// Note: Removing constant terms and constant factors, the error functions simplifies
// to the error below. If using an error tolerance as the termination criteria,
// the constant term could create a lot of bias at low magnitudes of the variable
// error component => it is best to not include the constant in the error.
// On the other hand, it may be better to include the constant term in the error
// if using error tolerance as a termination condition if the algorithm has
// a hard time terminating, as this would ensure termination happens below
// some threshold (otherwise the error could keep reducing by some fixed
// percentage even though its basically zero, and never terminate).
// Note: It is best to avoid this problem entirely, and base termination on the change
// in the registration parameter estimates.
//
// Simplified Error = Sum_i[log(Mi) + di'*inv(M)*di]
//
vctDynamicVector<vct3x3> Mi(nSamples); // noise covariances of match (R*Mxi*Rt + Myi)
vctDynamicVector<vct3x3> inv_Mi(nSamples); // inverse noise covariances of match (R*Mxi*Rt + Myi)^-1
vctDynamicVector<double> det_Mi(nSamples); // determinant of noise covariances of match |R*Mxi*Rt + Myi|
vctDynamicVector<double> SqrMahalDist(nSamples); // square Mahalanobis distance of matches = (yi-Rxi-t)'*inv(Mi)*(yi-Rxi-t)
// compute mahalanobis distances of the matches
vct3 residual;
for (unsigned int s = 0; s < nSamples; s++)
{
residual = samplePtsXfmd.Element(s) - matchPts.Element(s);
// match covariance
Mi.Element(s) = R_Mxi_Rt.Element(s) + Myi_sigma2.Element(s);
// match covariance decomposition
ComputeCovDecomposition_NonIter(Mi.Element(s), inv_Mi.Element(s), det_Mi.Element(s));
// match square Mahalanobis distance
SqrMahalDist.Element(s) = residual*inv_Mi.Element(s)*residual;
}
//// This is not general enough since we want to computer a correct error
//// both post-match and post-registration steps; this method assumes
//// error evaluation always happens post-match
//// compute mahalanobis distances of the matches
//for (unsigned int s = 0; s < nSamples; s++)
//{
// // match covariance
// Mi.Element(s) = R_Mxi_Rt.Element(s) + Myi_sigma2.Element(s);
// // match covariance decomposition
// ComputeCovDecomposition(Mi.Element(s), inv_Mi.Element(s), det_Mi.Element(s));
// // match square Mahalanobis distance
// SqrMahalDist.Element(s) = residuals_PostMatch.Element(s)*inv_Mi.Element(s)*residuals_PostMatch.Element(s);
//}
//-- Here We Compute the Full Negative Log-Likelihood --//
static double nklog2PI = nSamples*3.0*log(2.0*cmnPI);
double logCost = 0.0;
double expCost = 0.0;
for (unsigned int i = 0; i<nSamples; i++)
{
// Compute error contribution for this sample
// error: log(detM) + di'*inv(Mi)*di
expCost += SqrMahalDist.Element(i);
logCost += log(det_Mi.Element(i));
}
prevCostFuncValue = costFuncValue;
costFuncValue = (nklog2PI + logCost + expCost) / 2.0;
//double costFunctionValue = (logCost + expCost);
//-- Test for algorithm-specific termination --//
// remove last iteration from monitoring variable
// by bit shifting one bit to the right
costFuncIncBits >>= 1;
if (costFuncValue > prevCostFuncValue)
{
// set 4th bit in monitoring variable
// (since we want to monitor up to 4 iterations)
costFuncIncBits |= 0x08;
// signal termination if cost function increased another time within
// the past 3 trials and if the value has not decreased since that time
// TODO: better to test if the value is not the same value as before?
if (costFuncIncBits > 0x08 && abs(prevIncCostFuncValue - costFuncValue) < 1e-10)
{
bTerminateAlgorithm = true;
}
prevIncCostFuncValue = costFuncValue;
}
else
{ // record last time the cost function was non-increasing
Fdec = Freg;
}
return costFuncValue;
#else
return 0.0;
#endif
}
DFSCorrection::DFSCorrection(const DFS_Segment &aSegment, Accelerator::Plane xy)
: itsSegment(aSegment), itsBPMdataFilter(0), bpms(), correctors(),svd(0),refdata()
{
// First we use theReferenceModel to calculate the design
// response model matrix which will be used to calculate
// future corrections on simulated data using theSimulationModel.
dfs_trace(dfs_trace::level_1)<<"constructing DFS correction for segment "<<itsSegment<<' ';
switch(xy)
{
case Accelerator::x_only:
dfs_trace(dfs_trace::level_1)<<"X plane only";
break;
case Accelerator::y_only:
dfs_trace(dfs_trace::level_1)<<"Y plane only";
break;
case Accelerator::x_and_y:
dfs_trace(dfs_trace::level_1)<<"X and Y planes";
break;
}
dfs_trace(dfs_trace::level_1)<<endl;
theReferenceModel->SetActiveBeamlineSegment(itsSegment);
theEnergyAdjustmentPolicy->SetActiveBeamlineSegment(itsSegment);
size_t nbpms = theReferenceModel->GetMonitorChannels(xy,bpms);
size_t ncors = theReferenceModel->GetCorrectorChannels(xy,correctors);
dfs_trace(dfs_trace::level_2)<<"No. of BPMs: "<<nbpms<<endl;
dfs_trace(dfs_trace::level_2)<<"No. of correctors: "<<ncors<<endl;
size_t nstates = theEnergyAdjustmentPolicy->GetNumEnergyStates();
dfs_trace(dfs_trace::level_2)<<nstates<<"energy states"<<endl;
RealMatrix M0(nbpms,ncors,0.0);
RealMatrix Mi(nbpms,ncors,0.0);
RealMatrix M(nstates*nbpms,ncors,0.0);
refdata.reserve(nstates);
dfs_trace(dfs_trace::level_1)<<"Constructing DFS model matrix"<<endl;
ResponseMatrixGenerator rmg(theReferenceModel,bpms,correctors);
for(size_t nes=0; nes<nstates; nes++)
{
dfs_trace(dfs_trace::level_2)<<"Constructing response matrix for state "<<nes<<endl;
SubMatrix<double> m = M(Range(nes*nbpms,(nes+1)*nbpms-1),Range(0,ncors-1));
theEnergyAdjustmentPolicy->SetEnergyState(nes);
rmg.Generate(nes);
refdata.push_back(rmg.GetReference());
if(nes==0)
{
m = M0 = rmg.GetMatrix(); // constraint on absolute orbit
}
else
{
// For the off-energy states we take the
// difference matrix
m = rmg.GetMatrix() - M0;
refdata[nes] -= refdata[0]; // for off-energy states we need the difference orbit
}
}
theEnergyAdjustmentPolicy->Restore();
// Set up weight vector for SVD
RealVector w(nstates*nbpms);
w[Range(0,nbpms-1)] = w_abs;
w[Range(nbpms,nstates*nbpms-1)] = w_diff;
// Now construct the SVD of the complete model matrix
dfs_trace(dfs_trace::level_2)<<"Performing SVD..."<<flush;
svd = new TLAS::SVDMatrix<double>(M,w);
dfs_trace(dfs_trace::level_2)<<"succesful"<<endl;
// Finally we set set up the necessary channels
// for the actual simulated correction
theSimulationModel->SetActiveBeamlineSegment(itsSegment);
theSimulationModel->GetMonitorChannels(xy,bpms);
theSimulationModel->GetCorrectorChannels(xy,correctors);
cData.redim(nstates*nbpms);
cCorr.redim(ncors);
}
double cisstAlgorithmICP_IMLP_MahalDist::ICP_EvaluateErrorFunction()
{
#ifdef COMPUTE_ERROR_FUNCTION
// TODO: do something different if "REMOVE_OUTLIERS" enabled?
//
// Compute sum of square Mahalanobis Distances of Matches
//
vctDynamicVector<vct3x3> Mi(nSamples); // noise covariances of match (R*Mxi*Rt + Myi)
vctDynamicVector<vct3x3> inv_Mi(nSamples); // inverse noise covariances of match (R*Mxi*Rt + Myi)^-1
vctDynamicVector<double> SqrMahalDist(nSamples); // square Mahalanobis distance of matches = (yi-Rxi-t)'*inv(Mi)*(yi-Rxi-t)
// compute mahalanobis distances of the matches
vct3 residual;
double expCost = 0.0;
for (unsigned int s = 0; s < nSamples; s++)
{
residual = samplePtsXfmd.Element(s) - matchPts.Element(s);
// match covariance
Mi.Element(s) = R_Mxi_Rt.Element(s) + Myi_sigma2.Element(s);
// match covariance decomposition
ComputeCovInverse_NonIter(Mi.Element(s), inv_Mi.Element(s));
// match square Mahalanobis distance
SqrMahalDist.Element(s) = residual*inv_Mi.Element(s)*residual;
// sum error contribution for this sample
// error: di'*inv(Mi)*di
expCost += SqrMahalDist.Element(s);
}
prevCostFuncValue = costFuncValue;
costFuncValue = expCost;
//-- Test for algorithm-specific termination --//
// remove last iteration from monitoring variable
// by bit shifting one bit to the right
costFuncIncBits >>= 1;
if (costFuncValue > prevCostFuncValue)
{
// set 4th bit in monitoring variable
// (since we want to monitor up to 4 iterations)
costFuncIncBits |= 0x08;
// signal termination if cost function increased another time within
// the past 3 trials and if the value has not decreased since that time
// TODO: better to test if the value is not the same value as before?
if (costFuncIncBits > 0x08 && abs(prevIncCostFuncValue - costFuncValue) < 1e-10)
{
bTerminateAlgorithm = true;
}
prevIncCostFuncValue = costFuncValue;
}
else
{ // record last time the cost function was non-increasing
Fdec = Freg;
}
return costFuncValue;
#else
return 0.0;
#endif
}
