double Radical::DoRadical2D(const mat& matX)
{
mat matXMod;
CopyAndPerturb(matXMod, matX);
mat matJacobi(2,2);
mat candidateY;
vec thetas = linspace<vec>(0, nAngles - 1, nAngles) /
((double) nAngles) * M_PI / 2;
vec values(nAngles);
for(size_t i = 0; i < nAngles; i++) {
double cosTheta = cos(thetas(i));
double sinTheta = sin(thetas(i));
matJacobi(0,0) = cosTheta;
matJacobi(1,0) = -sinTheta;
matJacobi(0,1) = sinTheta;
matJacobi(1,1) = cosTheta;
candidateY = matXMod * matJacobi;
vec candidateY1 = candidateY.unsafe_col(0);
vec candidateY2 = candidateY.unsafe_col(1);
values(i) = Vasicek(candidateY1) + Vasicek(candidateY2);
}
uword indOpt;
values.min(indOpt); // we ignore the return value; we don't care about it
return thetas(indOpt);
}
// thetas = 0 : 0.1 * pi : pi;
// phis = 0 : 0.1 * pi : 2 * pi;
// actualValues = zeros(length(thetas), length(phis));
// expectedValues = zeros(length(thetas), length(phis));
//
// l = 1; m = -1;
// for i = 1 : length(thetas)
// for j = 1 : length(phis)
// theta = thetas(i);
// phi = phis(j);
// currentValues = SphericalHarmonicsComputer.GetSphericalHarmonicValues(theta, phi, l);
// actualValues(i, j) = currentValues(m + 1 + l);
// expectedValues(i, j) = SphericalHarmonicsComputer.GetAnalyticSphericalHarmonicValue(theta, phi, l, m);
// end
// end
// difference = actualValues - expectedValues;
// norm(difference)
FLOAT_TYPE SphericalHarmonicsComputerTests::GetSphericalHarmonicsError(int l, int m)
{
const int thetasCount = 100;
const int phisCount = 100;
vector<FLOAT_TYPE> thetas(thetasCount);
vector<FLOAT_TYPE> phis(phisCount);
for (int i = 0; i < thetasCount; i++)
{
thetas[i] = PI * i / thetasCount;
}
for (int i = 0; i < phisCount; i++)
{
phis[i] = PI * i / phisCount;
}
FLOAT_TYPE error = 0.0;
int sphericalHarmonicsCount = 2 * l + 1;
vector< complex<FLOAT_TYPE> > harmonicValues(sphericalHarmonicsCount);
for (int i = 0; i < thetasCount; i++)
{
for (int j = 0; j < phisCount; j++)
{
FLOAT_TYPE theta = thetas[i];
FLOAT_TYPE phi = phis[j];
SphericalHarmonicsComputer::FillSphericalHarmonicValues(theta, phi, l, &harmonicValues);
complex<FLOAT_TYPE> actualValue = harmonicValues[m + l];
complex<FLOAT_TYPE> expectedValue = SphericalHarmonicsComputer::GetAnalyticalSphericalHarmonicValue(theta, phi, l, m);
error += norm(actualValue - expectedValue);
}
}
return error;
}
/**
* A map detector ID and Q ranges
* This method looks unnecessary as it could be calculated on the fly but
* the parallelization means that lazy instantation slows it down due to the
* necessary CRITICAL sections required to update the cache. The Q range
* values are required very frequently so the total time is more than
* offset by this precaching step
*/
DetectorAngularCache initAngularCaches(const MatrixWorkspace *const workspace) {
const size_t nhist = workspace->getNumberHistograms();
std::vector<double> thetas(nhist);
std::vector<double> thetaWidths(nhist);
std::vector<double> detectorHeights(nhist);
auto inst = workspace->getInstrument();
const auto samplePos = inst->getSample()->getPos();
const V3D upDirVec = inst->getReferenceFrame()->vecPointingUp();
for (size_t i = 0; i < nhist; ++i) // signed for OpenMP
{
IDetector_const_sptr det;
try {
det = workspace->getDetector(i);
} catch (Exception::NotFoundError &) {
// Catch if no detector. Next line tests whether this happened - test
// placed
// outside here because Mac Intel compiler doesn't like 'continue' in a
// catch
// in an openmp block.
}
// If no detector found, skip onto the next spectrum
if (!det || det->isMonitor()) {
thetas[i] = -1.0; // Indicates a detector to skip
thetaWidths[i] = -1.0;
continue;
}
// We have to convert theta from radians to degrees
const double theta = workspace->detectorSignedTwoTheta(*det) * rad2deg;
thetas[i] = theta;
/**
* Determine width from shape geometry. A group is assumed to contain
* detectors with the same shape & r, theta value, i.e. a ring mapped-group
* The shape is retrieved and rotated to match the rotation of the detector.
* The angular width is computed using the l2 distance from the sample
*/
if (auto group = boost::dynamic_pointer_cast<const DetectorGroup>(det)) {
// assume they all have same shape and same r,theta
auto dets = group->getDetectors();
det = dets[0];
}
const auto pos = det->getPos() - samplePos;
double l2(0.0), t(0.0), p(0.0);
pos.getSpherical(l2, t, p);
// Get the shape
auto shape =
det->shape(); // Defined in its own reference frame with centre at 0,0,0
BoundingBox bbox = shape->getBoundingBox();
auto maxPoint(bbox.maxPoint());
auto minPoint(bbox.minPoint());
auto span = maxPoint - minPoint;
detectorHeights[i] = span.scalar_prod(upDirVec);
thetaWidths[i] = 2.0 * std::fabs(std::atan((detectorHeights[i] / 2) / l2)) *
180.0 / M_PI;
}
DetectorAngularCache cache;
cache.thetas = thetas;
cache.thetaWidths = thetaWidths;
cache.detectorHeights = detectorHeights;
return cache;
}
//EAP estimates used in multipleGroup
RcppExport SEXP EAPgroup(SEXP Rlogitemtrace, SEXP Rtabdata, SEXP RTheta, SEXP Rprior, SEXP Rmu,
SEXP Rfull, SEXP Rr, SEXP Rncores)
{
BEGIN_RCPP
const int ncores = as<int>(Rncores);
const int full = as<int>(Rfull);
const vector<int> r = as< vector<int> >(Rr);
#ifdef SUPPORT_OPENMP
omp_set_num_threads(ncores);
#endif
const NumericMatrix logitemtrace(Rlogitemtrace);
const IntegerMatrix tabdata(Rtabdata);
const NumericMatrix Theta(RTheta);
const vector<double> prior = as< vector<double> >(Rprior);
const vector<double> mu = as< vector<double> >(Rmu);
const int n = prior.size(); //nquad
const int N = tabdata.nrow();
const int nitems = tabdata.ncol();
const int nfact = Theta.ncol();
vector<double> scores(N * nfact);
vector<double> scores2(N * nfact*(nfact + 1)/2);
#pragma omp parallel for
for(int pat = 0; pat < N; ++pat){
vector<double> L(n, 0.0);
for(int j = 0; j < n; ++j){
for(int i = 0; i < nitems; ++i)
if(tabdata(pat, i))
L[j] = L[j] + logitemtrace(j, i);
}
vector<double> thetas(nfact, 0.0);
double denom = 0.0;
for(int j = 0; j < n; ++j){
L[j] = exp(L[j] + log(prior[j]));
denom += L[j];
}
for(int k = 0; k < nfact; ++k){
for(int j = 0; j < n; ++j)
thetas[k] += Theta(j, k) * L[j] / denom;
scores[pat + k*N] = thetas[k];
}
int ind = 0;
vector<double> thetas2(nfact*(nfact+1)/2, 0.0);
for(int i = 0; i < nfact; ++i){
for(int k = 0; k < nfact; ++k){
if(i <= k){
for(int j = 0; j < n; ++j)
thetas2[ind] += (Theta(j, i) - thetas[i]) * (Theta(j, k) - thetas[k]) *
L[j] / denom;
thetas2[ind] += (thetas[i] - mu[i]) * (thetas[k] - mu[k]);
scores2[pat + ind*N] = thetas2[ind];
ind += 1;
}
}
}
}
if(full){
const int NN = std::accumulate(r.begin(), r.end(), 0);
NumericMatrix fullscores(NN, nfact);
NumericMatrix scoresmat = vec2mat(scores, N, nfact);
int ind = 0;
for(int pat = 0; pat < N; ++pat){
for(int j = 0; j < r[pat]; ++j){
for(int i = 0; i < nfact; ++i)
fullscores(ind, i) = scoresmat(pat, i);
++ind;
}
}
return(fullscores);
}
List ret;
ret["scores"] = vec2mat(scores, N, nfact);
ret["scores2"] = vec2mat(scores2, N, nfact*(nfact + 1)/2);
return(ret);
END_RCPP
}
SEXP invariantSimilarityHelper(
typename itk::Image< float , ImageDimension >::Pointer image1,
typename itk::Image< float , ImageDimension >::Pointer image2,
SEXP r_thetas, SEXP r_lsits, SEXP r_WM, SEXP r_scale,
SEXP r_doreflection, SEXP r_txfn )
{
unsigned int mibins = 20;
unsigned int localSearchIterations =
Rcpp::as< unsigned int >( r_lsits ) ;
std::string whichMetric = Rcpp::as< std::string >( r_WM );
std::string txfn = Rcpp::as< std::string >( r_txfn );
bool useprincaxis = true;
typedef typename itk::ImageMaskSpatialObject<ImageDimension>::ImageType
maskimagetype;
typename maskimagetype::Pointer mask = ITK_NULLPTR;
Rcpp::NumericVector thetas( r_thetas );
Rcpp::NumericVector vector_r( r_thetas ) ;
Rcpp::IntegerVector dims( 1 );
Rcpp::IntegerVector doReflection( r_doreflection );
unsigned int vecsize = thetas.size();
dims[0]=0;
typedef float PixelType;
typedef double RealType;
RealType bestscale = Rcpp::as< RealType >( r_scale ) ;
typedef itk::Image< PixelType , ImageDimension > ImageType;
if( image1.IsNotNull() & image2.IsNotNull() )
{
typedef typename itk::ImageMomentsCalculator<ImageType> ImageCalculatorType;
typedef itk::AffineTransform<RealType, ImageDimension> AffineType0;
typedef itk::AffineTransform<RealType, ImageDimension> AffineType;
typedef typename ImageCalculatorType::MatrixType MatrixType;
typedef itk::Vector<float, ImageDimension> VectorType;
VectorType ccg1;
VectorType cpm1;
MatrixType cpa1;
VectorType ccg2;
VectorType cpm2;
MatrixType cpa2;
typename ImageCalculatorType::Pointer calculator1 =
ImageCalculatorType::New();
typename ImageCalculatorType::Pointer calculator2 =
ImageCalculatorType::New();
calculator1->SetImage( image1 );
calculator2->SetImage( image2 );
typename ImageCalculatorType::VectorType fixed_center;
fixed_center.Fill(0);
typename ImageCalculatorType::VectorType moving_center;
moving_center.Fill(0);
try
{
calculator1->Compute();
fixed_center = calculator1->GetCenterOfGravity();
ccg1 = calculator1->GetCenterOfGravity();
cpm1 = calculator1->GetPrincipalMoments();
cpa1 = calculator1->GetPrincipalAxes();
try
{
calculator2->Compute();
moving_center = calculator2->GetCenterOfGravity();
ccg2 = calculator2->GetCenterOfGravity();
cpm2 = calculator2->GetPrincipalMoments();
cpa2 = calculator2->GetPrincipalAxes();
}
catch( ... )
{
fixed_center.Fill(0);
}
}
catch( ... )
{
// Rcpp::Rcerr << " zero image1 error ";
}
if ( vnl_math_abs( bestscale - 1.0 ) < 1.e-6 )
{
RealType volelt1 = 1;
RealType volelt2 = 1;
for ( unsigned int d=0; d<ImageDimension; d++)
{
volelt1 *= image1->GetSpacing()[d];
volelt2 *= image2->GetSpacing()[d];
}
bestscale =
( calculator2->GetTotalMass() * volelt2 )/
( calculator1->GetTotalMass() * volelt1 );
RealType powlev = 1.0 / static_cast<RealType>(ImageDimension);
bestscale = vcl_pow( bestscale , powlev );
}
unsigned int eigind1 = 1;
unsigned int eigind2 = 1;
if( ImageDimension == 3 )
{
eigind1 = 2;
}
typedef vnl_vector<RealType> EVectorType;
typedef vnl_matrix<RealType> EMatrixType;
EVectorType evec1_2ndary = cpa1.GetVnlMatrix().get_row( eigind2 );
EVectorType evec1_primary = cpa1.GetVnlMatrix().get_row( eigind1 );
EVectorType evec2_2ndary = cpa2.GetVnlMatrix().get_row( eigind2 );
EVectorType evec2_primary = cpa2.GetVnlMatrix().get_row( eigind1 );
/** Solve Wahba's problem http://en.wikipedia.org/wiki/Wahba%27s_problem */
EMatrixType B = outer_product( evec2_primary, evec1_primary );
if( ImageDimension == 3 )
{
B = outer_product( evec2_2ndary, evec1_2ndary )
+ outer_product( evec2_primary, evec1_primary );
}
vnl_svd<RealType> wahba( B );
vnl_matrix<RealType> A_solution = wahba.V() * wahba.U().transpose();
A_solution = vnl_inverse( A_solution );
RealType det = vnl_determinant( A_solution );
if( ( det < 0 ) )
{
vnl_matrix<RealType> id( A_solution );
id.set_identity();
for( unsigned int i = 0; i < ImageDimension; i++ )
{
if( A_solution( i, i ) < 0 )
{
id( i, i ) = -1.0;
}
}
A_solution = A_solution * id.transpose();
}
if ( doReflection[0] == 1 || doReflection[0] == 3 )
{
vnl_matrix<RealType> id( A_solution );
id.set_identity();
id = id - 2.0 * outer_product( evec2_primary , evec2_primary );
A_solution = A_solution * id;
}
if ( doReflection[0] > 1 )
{
vnl_matrix<RealType> id( A_solution );
id.set_identity();
id = id - 2.0 * outer_product( evec1_primary , evec1_primary );
A_solution = A_solution * id;
}
typename AffineType::Pointer affine1 = AffineType::New();
typename AffineType::OffsetType trans = affine1->GetOffset();
itk::Point<RealType, ImageDimension> trans2;
for( unsigned int i = 0; i < ImageDimension; i++ )
{
trans[i] = moving_center[i] - fixed_center[i];
trans2[i] = fixed_center[i] * ( 1 );
}
affine1->SetIdentity();
affine1->SetOffset( trans );
if( useprincaxis )
{
affine1->SetMatrix( A_solution );
}
affine1->SetCenter( trans2 );
if( ImageDimension > 3 )
{
return EXIT_SUCCESS;
}
vnl_vector<RealType> evec_tert;
if( ImageDimension == 3 )
{ // try to rotate around tertiary and secondary axis
evec_tert = vnl_cross_3d( evec1_primary, evec1_2ndary );
}
if( ImageDimension == 2 )
{ // try to rotate around tertiary and secondary axis
evec_tert = evec1_2ndary;
evec1_2ndary = evec1_primary;
}
itk::Vector<RealType, ImageDimension> axis2;
itk::Vector<RealType, ImageDimension> axis1;
for( unsigned int d = 0; d < ImageDimension; d++ )
{
axis1[d] = evec_tert[d];
axis2[d] = evec1_2ndary[d];
}
typename AffineType::Pointer simmer = AffineType::New();
simmer->SetIdentity();
simmer->SetCenter( trans2 );
simmer->SetOffset( trans );
typename AffineType0::Pointer affinesearch = AffineType0::New();
affinesearch->SetIdentity();
affinesearch->SetCenter( trans2 );
typedef itk::MultiStartOptimizerv4 OptimizerType;
typename OptimizerType::MetricValuesListType metricvalues;
typename OptimizerType::Pointer mstartOptimizer = OptimizerType::New();
typedef itk::CorrelationImageToImageMetricv4
<ImageType, ImageType, ImageType> GCMetricType;
typedef itk::MattesMutualInformationImageToImageMetricv4
<ImageType, ImageType, ImageType> MetricType;
typename MetricType::ParametersType newparams( affine1->GetParameters() );
typename GCMetricType::Pointer gcmetric = GCMetricType::New();
gcmetric->SetFixedImage( image1 );
gcmetric->SetVirtualDomainFromImage( image1 );
gcmetric->SetMovingImage( image2 );
gcmetric->SetMovingTransform( simmer );
gcmetric->SetParameters( newparams );
typename MetricType::Pointer mimetric = MetricType::New();
mimetric->SetNumberOfHistogramBins( mibins );
mimetric->SetFixedImage( image1 );
mimetric->SetMovingImage( image2 );
mimetric->SetMovingTransform( simmer );
mimetric->SetParameters( newparams );
if( mask.IsNotNull() )
{
typename itk::ImageMaskSpatialObject<ImageDimension>::Pointer so =
itk::ImageMaskSpatialObject<ImageDimension>::New();
so->SetImage( const_cast<maskimagetype *>( mask.GetPointer() ) );
mimetric->SetFixedImageMask( so );
gcmetric->SetFixedImageMask( so );
}
typedef itk::ConjugateGradientLineSearchOptimizerv4 LocalOptimizerType;
typename LocalOptimizerType::Pointer localoptimizer =
LocalOptimizerType::New();
RealType localoptimizerlearningrate = 0.1;
localoptimizer->SetLearningRate( localoptimizerlearningrate );
localoptimizer->SetMaximumStepSizeInPhysicalUnits(
localoptimizerlearningrate );
localoptimizer->SetNumberOfIterations( localSearchIterations );
localoptimizer->SetLowerLimit( 0 );
localoptimizer->SetUpperLimit( 2 );
localoptimizer->SetEpsilon( 0.1 );
localoptimizer->SetMaximumLineSearchIterations( 50 );
localoptimizer->SetDoEstimateLearningRateOnce( true );
localoptimizer->SetMinimumConvergenceValue( 1.e-6 );
localoptimizer->SetConvergenceWindowSize( 5 );
if( true )
{
typedef typename MetricType::FixedSampledPointSetType PointSetType;
typedef typename PointSetType::PointType PointType;
typename PointSetType::Pointer pset(PointSetType::New());
unsigned int ind=0;
unsigned int ct=0;
itk::ImageRegionIteratorWithIndex<ImageType> It(image1,
image1->GetLargestPossibleRegion() );
for( It.GoToBegin(); !It.IsAtEnd(); ++It )
{
// take every N^th point
if ( ct % 10 == 0 )
{
PointType pt;
image1->TransformIndexToPhysicalPoint( It.GetIndex(), pt);
pset->SetPoint(ind, pt);
ind++;
}
ct++;
}
mimetric->SetFixedSampledPointSet( pset );
mimetric->SetUseFixedSampledPointSet( true );
gcmetric->SetFixedSampledPointSet( pset );
gcmetric->SetUseFixedSampledPointSet( true );
}
if ( whichMetric.compare("MI") == 0 ) {
mimetric->Initialize();
typedef itk::RegistrationParameterScalesFromPhysicalShift<MetricType>
RegistrationParameterScalesFromPhysicalShiftType;
typename RegistrationParameterScalesFromPhysicalShiftType::Pointer
shiftScaleEstimator =
RegistrationParameterScalesFromPhysicalShiftType::New();
shiftScaleEstimator->SetMetric( mimetric );
shiftScaleEstimator->SetTransformForward( true );
typename RegistrationParameterScalesFromPhysicalShiftType::ScalesType
movingScales( simmer->GetNumberOfParameters() );
shiftScaleEstimator->EstimateScales( movingScales );
mstartOptimizer->SetScales( movingScales );
mstartOptimizer->SetMetric( mimetric );
localoptimizer->SetMetric( mimetric );
localoptimizer->SetScales( movingScales );
}
if ( whichMetric.compare("MI") != 0 ) {
gcmetric->Initialize();
typedef itk::RegistrationParameterScalesFromPhysicalShift<GCMetricType>
RegistrationParameterScalesFromPhysicalShiftType;
typename RegistrationParameterScalesFromPhysicalShiftType::Pointer
shiftScaleEstimator =
RegistrationParameterScalesFromPhysicalShiftType::New();
shiftScaleEstimator->SetMetric( gcmetric );
shiftScaleEstimator->SetTransformForward( true );
typename RegistrationParameterScalesFromPhysicalShiftType::ScalesType
movingScales( simmer->GetNumberOfParameters() );
shiftScaleEstimator->EstimateScales( movingScales );
mstartOptimizer->SetScales( movingScales );
mstartOptimizer->SetMetric( gcmetric );
localoptimizer->SetMetric( gcmetric );
localoptimizer->SetScales( movingScales );
}
typename OptimizerType::ParametersListType parametersList =
mstartOptimizer->GetParametersList();
affinesearch->SetIdentity();
affinesearch->SetCenter( trans2 );
affinesearch->SetOffset( trans );
for ( unsigned int i = 0; i < vecsize; i++ )
{
RealType ang1 = thetas[i];
RealType ang2 = 0; // FIXME should be psi
vector_r[ i ]=0;
if( ImageDimension == 3 )
{
for ( unsigned int jj = 0; jj < vecsize; jj++ )
{
ang2=thetas[jj];
affinesearch->SetIdentity();
affinesearch->SetCenter( trans2 );
affinesearch->SetOffset( trans );
if( useprincaxis )
{
affinesearch->SetMatrix( A_solution );
}
affinesearch->Rotate3D(axis1, ang1, 1);
affinesearch->Rotate3D(axis2, ang2, 1);
affinesearch->Scale( bestscale );
simmer->SetMatrix( affinesearch->GetMatrix() );
parametersList.push_back( simmer->GetParameters() );
}
}
if( ImageDimension == 2 )
{
affinesearch->SetIdentity();
affinesearch->SetCenter( trans2 );
affinesearch->SetOffset( trans );
if( useprincaxis )
{
affinesearch->SetMatrix( A_solution );
}
affinesearch->Rotate2D( ang1, 1);
affinesearch->Scale( bestscale );
simmer->SetMatrix( affinesearch->GetMatrix() );
typename AffineType::ParametersType pp =
simmer->GetParameters();
//pp[1]=ang1;
//pp[0]=bestscale;
parametersList.push_back( simmer->GetParameters() );
}
}
mstartOptimizer->SetParametersList( parametersList );
if( localSearchIterations > 0 )
{
mstartOptimizer->SetLocalOptimizer( localoptimizer );
}
mstartOptimizer->StartOptimization();
typename AffineType::Pointer bestaffine = AffineType::New();
bestaffine->SetCenter( trans2 );
bestaffine->SetParameters( mstartOptimizer->GetBestParameters() );
if ( txfn.length() > 3 )
{
typename AffineType::Pointer bestaffine = AffineType::New();
bestaffine->SetCenter( trans2 );
bestaffine->SetParameters( mstartOptimizer->GetBestParameters() );
typedef itk::TransformFileWriter TransformWriterType;
typename TransformWriterType::Pointer transformWriter =
TransformWriterType::New();
transformWriter->SetInput( bestaffine );
transformWriter->SetFileName( txfn.c_str() );
transformWriter->Update();
}
metricvalues = mstartOptimizer->GetMetricValuesList();
for ( unsigned int k = 0; k < metricvalues.size(); k++ )
{
vector_r[k] = metricvalues[k];
}
dims[0] = vecsize;
vector_r.attr( "dim" ) = vecsize;
return Rcpp::wrap( vector_r );
}
else
{
return Rcpp::wrap( vector_r );
}
}
