/* fix narrow-lane ambiguity by rounding -------------------------------------*/
static int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n)
{
double C1,C2,B1,v1,BC,v,vc,*NC,*var,lam_NL=lam_LC(1,1,0),lam1,lam2;
int i,j,k,m=0,N1,stat;
lam1=lam_carr[0];
lam2=lam_carr[1];
C1= SQR(lam2)/(SQR(lam2)-SQR(lam1));
C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1));
NC=zeros(n,1);
var=zeros(n,1);
for (i=0; i<n; i++) {
j=IB(sat1[i],&rtk->opt);
k=IB(sat2[i],&rtk->opt);
/* narrow-lane ambiguity */
B1=(rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL;
N1=ROUND(B1);
/* variance of narrow-lane ambiguity */
var[m]=rtk->P[j+j*rtk->nx]+rtk->P[k+k*rtk->nx]-2.0*rtk->P[j+k*rtk->nx];
v1=var[m]/SQR(lam_NL);
/* validation of narrow-lane ambiguity */
if (fabs(N1-B1)>rtk->opt.thresar[2]||
conffunc(N1,B1,sqrt(v1))<rtk->opt.thresar[1]) {
continue;
}
/* iono-free ambiguity (m) */
BC=C1*lam1*N1+C2*lam2*(N1-NW[i]);
/* check residuals */
v=rtk->ssat[sat1[i]-1].resc[0]-rtk->ssat[sat2[i]-1].resc[0];
vc=v+(BC-(rtk->x[j]-rtk->x[k]));
if (fabs(vc)>THRES_RES) continue;
sat1[m]=sat1[i];
sat2[m]=sat2[i];
NC[m++]=BC;
}
/* select fixed ambiguities by dependancy check */
m=sel_amb(sat1,sat2,NC,var,m);
/* fixed solution */
stat=fix_sol(rtk,sat1,sat2,NC,m);
free(NC);
free(var);
return stat&&m>=3;
}
/* ionosphere residuals ------------------------------------------------------*/
static int res_iono(const obsd_t *obs, int n, const nav_t *nav,
const double *rs, const double *rr, const double *pos,
const double *azel, const pcv_t *pcv, const ekf_t *ekf,
double *phw, double *v, double *H, double *R)
{
double *sig,P1,P2,L1,L2,c_iono=1.0-SQR(lam[1]/lam[0]);
double LG,PG,antdel[3]={0},dant[NFREQ]={0};
int i,j,nv=0,sat;
sig=mat(1,2*n);
for (i=0;i<n;i++) {
if (!raw_obs(obs+i,nav,&P1,&P2,&L1,&L2)||azel[i*2+1]<MIN_EL) continue;
sat=obs[i].sat;
/* ionosphere-LC model */
LG=-c_iono*ekf->x[II(sat)]+ekf->x[IB(sat)];
PG= c_iono*ekf->x[II(sat)]+nav->cbias[sat-1][0];
/* receiver antenna phase center offset and variation */
if (pcv) {
antmodel(pcv,antdel,azel+i*2,dant);
LG+=dant[0]-dant[1];
PG+=dant[0]-dant[1];
}
/* phase windup correction */
windupcorr(obs[i].time,rs+i*6,rr,phw+obs[i].sat-1);
LG+=(lam[0]-lam[1])*phw[obs[i].sat-1];
/* residuals of ionosphere (geometriy-free) LC */
v[nv ]=(L1-L2)-LG;
#if 0
v[nv+1]=(P1-P2)-PG;
#else
v[nv+1]=0.0;
#endif
for (j=0;j<ekf->nx*2;j++) H[ekf->nx*nv+j]=0.0;
H[ekf->nx*nv +II(sat)]=-c_iono;
H[ekf->nx*nv +IB(sat)]=1.0;
H[ekf->nx*(nv+1)+II(sat)]=c_iono;
sig[nv ]=sig_err(azel+i*2);
sig[nv+1]=RATIO_ERR*sig[nv];
nv+=2;
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?SQR(sig[i]):0.0;
}
free(sig);
return nv;
}
/* fixed solution ------------------------------------------------------------*/
static int fix_sol(rtk_t *rtk, const int *sat1, const int *sat2,
const double *NC, int n)
{
double *v,*H,*R;
int i,j,k,info;
if (n<=0) return 0;
v=zeros(n,1);
H=zeros(rtk->nx,n);
R=zeros(n,n);
/* constraints to fixed ambiguities */
for (i=0; i<n; i++) {
j=IB(sat1[i],&rtk->opt);
k=IB(sat2[i],&rtk->opt);
v[i]=NC[i]-(rtk->x[j]-rtk->x[k]);
H[j+i*rtk->nx]= 1.0;
H[k+i*rtk->nx]=-1.0;
R[i+i*n]=SQR(CONST_AMB);
}
/* update states with constraints */
if ((info=filter(rtk->x,rtk->P,H,v,R,rtk->nx,n))) {
trace(1,"filter error (info=%d)\n",info);
free(v);
free(H);
free(R);
return 0;
}
/* set solution */
for (i=0; i<rtk->na; i++) {
rtk->xa[i]=rtk->x[i];
for (j=0; j<rtk->na; j++) {
rtk->Pa[i+j*rtk->na]=rtk->P[i+j*rtk->nx];
rtk->Pa[j+i*rtk->na]=rtk->P[i+j*rtk->nx];
}
}
/* set flags */
for (i=0; i<n; i++) {
rtk->ambc[sat1[i]-1].flags[sat2[i]-1]=1;
rtk->ambc[sat2[i]-1].flags[sat1[i]-1]=1;
}
free(v);
free(H);
free(R);
return 1;
}
/* initizlize bias parameter --------------------------------------------------*/
static void init_bias(const obsd_t *obs, double *x, double *P, int nx)
{
double bias;
if (obs->L[0]==0||obs->L[1]==0||obs->P[0]==0||obs->P[1]==0) return;
bias=(obs->L[0]-obs->L[1])-(obs->P[0]-obs->P[1]);
initx(x,P,nx,IB(obs->sat),bias,VAR_BIAS);
}
::libmaus2::bitio::IndexedBitVector::unique_ptr_type bpsVector() const
{
::libmaus2::bitio::IndexedBitVector::unique_ptr_type IB ( new ::libmaus2::bitio::IndexedBitVector(2*n) );
::libmaus2::bitio::BitWriter8 BW8(IB->A.get());
bps(BW8);
BW8.flush();
IB->setupIndex();
return UNIQUE_PTR_MOVE(IB);
}
/* ionosphere residuals ------------------------------------------------------*/
static int res_iono(const obsd_t *obs, int n, const double *azel,
const double *x, int nx, double *v, double *H, double *R)
{
double *sig,L1,L2,P1,P2,map;
int i,j,nv=0,sat;
sig=mat(1,2*n);
for (i=0;i<n;i++) {
sat=obs[i].sat;
L1=obs->L[0]*lam[0];
L2=obs->L[1]*lam[1];
P1=obs->P[0];
P2=obs->P[1];
if (L1==0||L2==0||P1==0||P2==0) continue;
/* ionosphere mapping function */
map=ionmapf(pos,azel+i*2);
/* residuals of ionosphere (geometriy-free) LC */
v[nv ]=(L1-L2)+map*x[II(sat)]-x[IB(sat)];
v[nv+1]=(P1-P2)-map*x[II(sat)];
/* partial derivatives */
for (j=0;j<nx;j++) H[nx*nv+j]=0.0;
H[nx*nv +II(sat)]=-map;
H[nx*nv +IB(sat)]=1.0;
H[nx*(nv+1)+IB(sat)]=map;
/* standard deviation of error */
sig[nv ]=std_err(azel);
sig[nv+1]=sig[nv]*RATIO_ERR;
nv+=2;
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?SQR(sig[i]):0.0;
}
free(sig);
return nv;
}
/* temporal update of states --------------------------------------------------*/
static void ud_state(const obsd_t *obs, int n, const nav_t *nav,
const double *pos, const double *azel, ekf_t *ekf,
sstat_t *sstat)
{
double P1,P2,L1,L2,PG,LG,tt,F[4]={0},Q[2]={0};
double x[2]={0},P[2],c_iono=1.0-SQR(lam[1]/lam[0]);
int i,sat,slip;
for (i=0;i<n;i++) {
/* raw pseudorange and phase range */
if (!raw_obs(obs+i,nav,&P1,&P2,&L1,&L2)||azel[i*2+1]<MIN_EL) continue;
sat=obs[i].sat;
tt=timediff(obs[i].time,sstat[sat-1].time);
LG=L1-L2;
PG=P1-P2;
slip=(obs[i].LLI[0]&3)||(obs[i].LLI[1]&3);
slip|=fabs(LG-sstat[sat-1].LG)>THRES_LG;
if (fabs(tt)>MAXGAP_IONO) {
#if 1
x[0]=PG/c_iono;
#else
x[0]=ionmodel(obs[i].time,nav->ion_gps,pos,azel+i*2);
#endif
x[1]=1E-6;
P[0]=VAR_IONO;
P[1]=VAR_IONR;
ekf_init(ekf,x,P,II(sat),2);
}
else {
F[0]=F[3]=1.0;
F[2]=tt;
Q[0]=PRN_IONO*fabs(tt);
Q[1]=PRN_IONR*fabs(tt);
ekf_pred(ekf,F,Q,II(sat),2);
}
if (tt>MAXGAP_BIAS||slip) {
x[0]=LG+PG;
P[0]=VAR_BIAS;
ekf_init(ekf,x,P,IB(sat),1);
}
sstat[sat-1].time=obs[i].time;
sstat[sat-1].azel[0]=azel[i*2];
sstat[sat-1].azel[1]=azel[i*2+1];
sstat[sat-1].slip=slip;
sstat[sat-1].LG=LG;
sstat[sat-1].PG=PG;
}
}
/* hold integer ambiguity ----------------------------------------------------*/
static void holdamb(rtk_t *rtk, const double *xa,char **msg)
{
double *v,*H,*R;
int i,n,f,info,index[MAX_SAT],nb=rtk->nx-rtk->na,nv=0;
v=mat(nb,1);
H=zeros(nb,rtk->nx);//neu loi thi stack luon
if ((!v) || (!H))
{
*msg += sprintf(*msg,"holdamb mat error");
free(v);
free(H);
return;
}
for (n=i=0; i<MAX_SAT; i++) {
if (rtk->ssat[i].fix!=2||
rtk->ssat[i].azel[1]<rtk->opt.elmaskhold) continue;
index[n++]=IB(i+1,f,&rtk->opt);
rtk->ssat[i].fix=3; /* hold */
}
/* constraint to fixed ambiguity */
for (i=1; i<n; i++) {
v[nv]=(xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]);
H[index[0]+nv*rtk->nx]= 1.0;
H[index[i]+nv*rtk->nx]=-1.0;
nv++;
}
if (nv>0) {
R=zeros(nv,nv);
for (i=0; i<nv; i++) R[i+i*nv]=VAR_HOLDAMB;
/* update states with constraints */
if ((info=filter(rtk->x,rtk->P,H,v,R,rtk->nx,nv))) {
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"filter error (info=%d)\n",info);
#endif
}
free(R);
}
free(v);
free(H);
}
static void restamb(rtk_t *rtk, const double *bias, int nb, double *xa)
{
int i,n,f,index[MAX_SAT],nv=0;
for (i=0; i<rtk->nx; i++) xa[i]=rtk->x [i];
for (i=0; i<rtk->na; i++) xa[i]=rtk->xa[i];
f=0;
for (n=i=0; i<MAX_SAT; i++) {
if (rtk->ssat[i].fix!=2) continue;
index[n++]=IB(i+1,f,&rtk->opt);
}
if (n>=2)
{
xa[index[0]]=rtk->x[index[0]];
for (i=1; i<n; i++) {
xa[index[i]]=xa[index[0]]-bias[nv++];
}
}
}
static int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
const double *P, const int *sat, double *y, double *e,
double *azel, const int *iu, const int *ir, int ns, double *v,
double *H, double *R, int *vflg,char **msg)
{
prcopt_t *opt=&rtk->opt;
double bl,dr[3],posu[3],posr[3],*im;
double *tropr,*tropu,*dtdxr,*dtdxu,*Ri,*Rj,s,lami,lamj,*Hi=NULL;
int i,j,k,f,nv=0,nb[6]= {0},b=0,sysi,sysj,nf=1;
bl=baseline(x,rtk->rb,dr);
ecef2pos(x,posu);
ecef2pos(rtk->rb,posr);
sysi=sysj=SYS_GPS;
Ri=mat(ns*2+2,1);
Rj=mat (ns*2+2,1);
im=mat(ns,1);
tropu=mat(ns,1);
tropr=mat(ns,1);
dtdxu=mat(ns,3);
dtdxr=mat(ns,3);
for (i=0; i<MAX_SAT; i++) {
rtk->ssat[i].resp=rtk->ssat[i].resc=0.0;
}
for (f=0; f<2; f++)
{
/* search reference satellite with highest elevation */
for (i=-1,j=0; j<ns; j++) {
if (validobs(iu[j],ir[j],f,1,y)) continue;
if (i<0||azel[1+iu[j]*2]>=azel[1+iu[i]*2]) i=j;
}
if (i<0) continue;
/* make double difference */
for (j=0; j<ns; j++) {
if (i==j) continue;
if (validobs(iu[j],ir[j],f,1,y)) continue;
lami=lamj=LAM1;
if (H) Hi=H+nv*rtk->nx;
/* double-differenced residual */
v[nv]=(y[f+iu[i]*2]-y[f+ir[i]*2])-
(y[f+iu[j]*2]-y[f+ir[j]*2]);
/* partial derivatives by rover position */
if (H) {
for (k=0; k<3; k++) {
Hi[k]=-e[k+iu[i]*3]+e[k+iu[j]*3];
}
}
/* double-differenced phase-bias term */
if (f<1) {
v[nv]-=lami*x[IB(sat[i],f,opt)]-lamj*x[IB(sat[j],f,opt)];
if (H)
{
Hi[IB(sat[i],f,opt)]= lami;
Hi[IB(sat[j],f,opt)]=-lamj;
}
}
if (f<1) rtk->ssat[sat[j]-1].resc=v[nv];
else rtk->ssat[sat[j]-1].resp=v[nv];
/* test innovation */
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
if (f<1) {
rtk->ssat[sat[i]-1].rejc++;
rtk->ssat[sat[j]-1].rejc++;
}
#ifdef _DEBUG_MSG
(*msg)+=sprintf(*msg,"outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n",
sat[i],sat[j],f<1?"L":"P",f+1,v[nv]);
#endif
continue;
}
/* single-differenced measurement error variances */
Ri[nv]=varerr(sat[i],sysi,azel[1+iu[i]*2],bl,dt,f,opt);
Rj[nv]=varerr(sat[j],sysj,azel[1+iu[j]*2],bl,dt,f,opt);
/* set valid data flags */
if (f<1) rtk->ssat[sat[i]-1].vsat=rtk->ssat[sat[j]-1].vsat=1;
//trace(4,"sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n",sat[i],
// sat[j],f<nf?"L":"P",f%nf+1,v[nv],Ri[nv],Rj[nv]);
vflg[nv++]=(sat[i]<<16)|(sat[j]<<8)|((f<nf?0:1)<<4);
nb[b]++;
}
/* restore single-differenced residuals assuming sum equal zero */
if (f<nf) {
for (j=0,s=0.0; j<MAX_SAT; j++) s+=rtk->ssat[j].resc;
s/=nb[b]+1;
for (j=0; j<MAX_SAT; j++) {
if (j==sat[i]-1||rtk->ssat[j].resc!=0.0) rtk->ssat[j].resc-=s;
}
}
else {
for (j=0,s=0.0; j<MAX_SAT; j++) s+=rtk->ssat[j].resp;
s/=nb[b]+1;
for (j=0; j<MAX_SAT; j++) {
if (j==sat[i]-1||rtk->ssat[j].resp!=0.0)
rtk->ssat[j].resp-=s;
}
}
b++;
}
/* end of system loop */
/* baseline length constraint for moving baseline */
/* double-differenced measurement error covariance */
ddcov(nb,b,Ri,Rj,nv,R);
free(Ri);
free(Rj);
free(im);
free(tropu);
free(tropr);
free(dtdxu);
free(dtdxr);
return nv;
}
static void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat,
const int *iu, const int *ir, int ns, const nav_t *nav,char **msg)
{
double cp,pr,*bias,offset;
int i,j,slip,reset;
for (i=0; i<ns; i++)
{
/* detect cycle slip by LLI */
rtk->ssat[sat[i]-1].slip&=0xFC;
detslp_ll(rtk,obs,iu[i],1,msg);
detslp_ll(rtk,obs,ir[i],2,msg);
}
/* reset phase-bias if expire obs outage counter */
for (i=1; i<=MAX_SAT; i++) {
reset=++rtk->ssat[i-1].outc>(unsigned int)rtk->opt.maxout;
if (reset&&rtk->x[IB(i,0,&rtk->opt)]!=0.0) {
initx(rtk,0.0,0.0,IB(i,0,&rtk->opt));
}
if (reset) {
rtk->ssat[i-1].lock=-rtk->opt.minlock;
}
}
/* reset phase-bias if instantaneous AR or expire obs outage counter */
for (i=1; i<=MAX_SAT; i++) {
reset=++rtk->ssat[i-1].outc>(unsigned int)rtk->opt.maxout;
if (reset&&rtk->x[IB(i,0,&rtk->opt)]!=0.0) {
initx(rtk,0.0,0.0,IB(i,0,&rtk->opt));
}
if (rtk->opt.modear!=ARMODE_INST&&reset) {
rtk->ssat[i-1].lock=-rtk->opt.minlock;
}
}
/* reset phase-bias if detecting cycle slip */
for (i=0; i<ns; i++) {
j=IB(sat[i],0,&rtk->opt);
rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*tt;
slip=rtk->ssat[sat[i]-1].slip;
if (!(slip&1)) continue;
rtk->x[j]=0.0;
rtk->ssat[sat[i]-1].lock=-rtk->opt.minlock;
}
bias=zeros(ns,1);
/* estimate approximate phase-bias by phase - code */
for (i=j=0,offset=0.0; i<ns; i++)
{
cp=sdobs(obs,iu[i],ir[i],0); /* cycle */
pr=sdobs(obs,iu[i],ir[i],1);
if (cp==0.0||pr==0.0) continue;
bias[i]=cp-pr*FREQ1/CLIGHT;
if (rtk->x[IB(sat[i],0,&rtk->opt)]!=0.0) {
offset+=bias[i]-rtk->x[IB(sat[i],0,&rtk->opt)];
j++;
}
}
/* correct phase-bias offset to enssure phase-code coherency */
if (j>0)
{
for (i=1; i<=MAX_SAT; i++)
{
if (rtk->x[IB(i,0,&rtk->opt)]!=0.0)
rtk->x[IB(i,0,&rtk->opt)]+=offset/j;
}
}
/* set initial states of phase-bias */
for (i=0; i<ns; i++)
{
if (bias[i]==0.0||rtk->x[IB(sat[i],0,&rtk->opt)]!=0.0) continue;
initx(rtk,bias[i],SQR(rtk->opt.std[0]),IB(sat[i],0,&rtk->opt));
}
free(bias);
}
/* phase and code residuals --------------------------------------------------*/
static int res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, rtk_t *rtk, double *v,
double *H, double *R, double *azel)
{
prcopt_t *opt=&rtk->opt;
double r,rr[3],disp[3],pos[3],e[3],meas[2],dtdx[3],dantr[NFREQ]={0};
double dants[NFREQ]={0},var[MAXOBS*2],dtrp=0.0,vart=0.0,varm[2]={0};
int i,j,k,sat,sys,nv=0,nx=rtk->nx,brk,tideopt;
trace(3,"res_ppp : n=%d nx=%d\n",n,nx);
for (i=0;i<MAXSAT;i++) rtk->ssat[i].vsat[0]=0;
for (i=0;i<3;i++) rr[i]=x[i];
/* earth tides correction */
if (opt->tidecorr) {
tideopt=opt->tidecorr==1?1:7; /* 1:solid, 2:solid+otl+pole */
tidedisp(gpst2utc(obs[0].time),rr,tideopt,&nav->erp,opt->odisp[0],
disp);
for (i=0;i<3;i++) rr[i]+=disp[i];
}
ecef2pos(rr,pos);
for (i=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
if (!(sys=satsys(sat,NULL))||!rtk->ssat[sat-1].vs) continue;
/* geometric distance/azimuth/elevation angle */
if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin) continue;
/* excluded satellite? */
if (satexclude(obs[i].sat,svh[i],opt)) continue;
/* tropospheric delay correction */
if (opt->tropopt==TROPOPT_SAAS) {
dtrp=tropmodel(obs[i].time,pos,azel+i*2,REL_HUMI);
vart=SQR(ERR_SAAS);
}
else if (opt->tropopt==TROPOPT_SBAS) {
dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart);
}
else if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {
dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x+IT(opt),dtdx,&vart);
}
else if (opt->tropopt==TROPOPT_COR||opt->tropopt==TROPOPT_CORG) {
dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x,dtdx,&vart);
}
/* satellite antenna model */
if (opt->posopt[0]) {
satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants);
}
/* receiver antenna model */
antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr);
/* phase windup correction */
if (opt->posopt[2]) {
windupcorr(rtk->sol.time,rs+i*6,rr,&rtk->ssat[sat-1].phw);
}
/* ionosphere and antenna phase corrected measurements */
if (!corrmeas(obs+i,nav,pos,azel+i*2,&rtk->opt,dantr,dants,
rtk->ssat[sat-1].phw,meas,varm,&brk)) {
continue;
}
/* satellite clock and tropospheric delay */
r+=-CLIGHT*dts[i*2]+dtrp;
trace(5,"sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n",
sat,azel[i*2]*R2D,azel[1+i*2]*R2D,dtrp,dantr[0],dantr[1],dants[0],
dants[1],rtk->ssat[sat-1].phw);
for (j=0;j<2;j++) { /* for phase and code */
if (meas[j]==0.0) continue;
for (k=0;k<nx;k++) H[k+nx*nv]=0.0;
v[nv]=meas[j]-r;
for (k=0;k<3;k++) H[k+nx*nv]=-e[k];
if (sys!=SYS_GLO) {
v[nv]-=x[IC(0,opt)];
H[IC(0,opt)+nx*nv]=1.0;
}
else {
v[nv]-=x[IC(1,opt)];
H[IC(1,opt)+nx*nv]=1.0;
}
if (opt->tropopt>=TROPOPT_EST) {
for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) {
H[IT(opt)+k+nx*nv]=dtdx[k];
}
}
if (j==0) {
v[nv]-=x[IB(obs[i].sat,opt)];
H[IB(obs[i].sat,opt)+nx*nv]=1.0;
}
var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j,opt)+varm[j]+vare[i]+vart;
if (j==0) rtk->ssat[sat-1].resc[0]=v[nv];
else rtk->ssat[sat-1].resp[0]=v[nv];
/* test innovation */
#if 0
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
#else
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno&&sys!=SYS_GLO) {
#endif
trace(2,"ppp outlier rejected %s sat=%2d type=%d v=%.3f\n",
time_str(obs[i].time,0),sat,j,v[nv]);
rtk->ssat[sat-1].rejc[0]++;
continue;
}
if (j==0) rtk->ssat[sat-1].vsat[0]=1;
nv++;
}
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?var[i]:0.0;
}
trace(5,"x=\n"); tracemat(5,x, 1,nx,8,3);
trace(5,"v=\n"); tracemat(5,v, 1,nv,8,3);
trace(5,"H=\n"); tracemat(5,H,nx,nv,8,3);
trace(5,"R=\n"); tracemat(5,R,nv,nv,8,5);
return nv;
}
/* number of estimated states ------------------------------------------------*/
extern int pppnx(const prcopt_t *opt)
{
return NX(opt);
}
/* precise point positioning -------------------------------------------------*/
extern void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
const prcopt_t *opt=&rtk->opt;
double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp;
int i,nv,info,svh[MAXOBS],stat=SOLQ_SINGLE;
trace(3,"pppos : nx=%d n=%d\n",rtk->nx,n);
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n);
for (i=0;i<MAXSAT;i++) rtk->ssat[i].fix[0]=0;
/* temporal update of states */
udstate_ppp(rtk,obs,n,nav);
trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
/* satellite positions and clocks */
satposs(obs[0].time,obs,n,nav,rtk->opt.sateph,rs,dts,var,svh);
/* exclude measurements of eclipsing satellite */
if (rtk->opt.posopt[3]) {
testeclipse(obs,n,nav,rs);
}
xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx);
matcpy(xp,rtk->x,rtk->nx,1);
nv=n*rtk->opt.nf*2; v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv);
for (i=0;i<rtk->opt.niter;i++) {
/* phase and code residuals */
if ((nv=res_ppp(i,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel))<=0) break;
/* measurement update */
matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
trace(2,"ppp filter error %s info=%d\n",time_str(rtk->sol.time,0),
info);
break;
}
trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4);
stat=SOLQ_PPP;
}
if (stat==SOLQ_PPP) {
/* postfit residuals */
res_ppp(1,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel);
/* update state and covariance matrix */
matcpy(rtk->x,xp,rtk->nx,1);
matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
/* ambiguity resolution in ppp */
if (opt->modear==ARMODE_PPPAR||opt->modear==ARMODE_PPPAR_ILS) {
if (pppamb(rtk,obs,n,nav,azel)) stat=SOLQ_FIX;
}
/* update solution status */
rtk->sol.ns=0;
for (i=0;i<n&&i<MAXOBS;i++) {
if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue;
rtk->ssat[obs[i].sat-1].lock[0]++;
rtk->ssat[obs[i].sat-1].outc[0]=0;
rtk->ssat[obs[i].sat-1].fix [0]=4;
rtk->sol.ns++;
}
rtk->sol.stat=stat;
for (i=0;i<3;i++) {
rtk->sol.rr[i]=rtk->x[i];
rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx];
}
rtk->sol.qr[3]=(float)rtk->P[1];
rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx];
rtk->sol.qr[5]=(float)rtk->P[2];
rtk->sol.dtr[0]=rtk->x[IC(0,opt)];
rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)];
for (i=0;i<n&&i<MAXOBS;i++) {
rtk->ssat[obs[i].sat-1].snr[0]=MIN(obs[i].SNR[0],obs[i].SNR[1]);
}
for (i=0;i<MAXSAT;i++) {
if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++;
}
}
free(rs); free(dts); free(var); free(azel);
free(xp); free(Pp); free(v); free(H); free(R);
}
/* temporal update of phase biases -------------------------------------------*/
static void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
double meas[2],var[2],bias[MAXOBS]={0},offset=0.0,pos[3]={0};
int i,j,k,sat,brk=0;
trace(3,"udbias : n=%d\n",n);
for (i=0;i<MAXSAT;i++) for (j=0;j<rtk->opt.nf;j++) {
rtk->ssat[i].slip[j]=0;
}
/* detect cycle slip by LLI */
detslp_ll(rtk,obs,n);
/* detect cycle slip by geometry-free phase jump */
detslp_gf(rtk,obs,n,nav);
/* reset phase-bias if expire obs outage counter */
for (i=0;i<MAXSAT;i++) {
if (++rtk->ssat[i].outc[0]>(unsigned int)rtk->opt.maxout) {
initx(rtk,0.0,0.0,IB(i+1,&rtk->opt));
}
}
ecef2pos(rtk->sol.rr,pos);
for (i=k=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
j=IB(sat,&rtk->opt);
if (!corrmeas(obs+i,nav,pos,rtk->ssat[sat-1].azel,&rtk->opt,NULL,NULL,
0.0,meas,var,&brk)) continue;
if (brk) {
rtk->ssat[sat-1].slip[0]=1;
trace(2,"%s: sat=%2d correction break\n",time_str(obs[i].time,0),sat);
}
bias[i]=meas[0]-meas[1];
if (rtk->x[j]==0.0||
rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]) continue;
offset+=bias[i]-rtk->x[j];
k++;
}
/* correct phase-code jump to enssure phase-code coherency */
if (k>=2&&fabs(offset/k)>0.0005*CLIGHT) {
for (i=0;i<MAXSAT;i++) {
j=IB(i+1,&rtk->opt);
if (rtk->x[j]!=0.0) rtk->x[j]+=offset/k;
}
trace(2,"phase-code jump corrected: %s n=%2d dt=%12.9fs\n",
time_str(rtk->sol.time,0),k,offset/k/CLIGHT);
}
for (i=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
j=IB(sat,&rtk->opt);
rtk->P[j+j*rtk->nx]+=SQR(rtk->opt.prn[0])*fabs(rtk->tt);
if (rtk->x[j]!=0.0&&
!rtk->ssat[sat-1].slip[0]&&!rtk->ssat[sat-1].slip[1]) continue;
if (bias[i]==0.0) continue;
/* reinitialize phase-bias if detecting cycle slip */
initx(rtk,bias[i],VAR_BIAS,IB(sat,&rtk->opt));
trace(5,"udbias_ppp: sat=%2d bias=%.3f\n",sat,meas[0]-meas[1]);
}
}
/** Cache J2000 rotation quaternion over a time range.
*
* This method will load an internal cache with frames over a time
* range. This prevents the NAIF kernels from being read over-and-over
* again and slowing an application down due to I/O performance. Once the
* cache has been loaded then the kernels can be unloaded from the NAIF
* system.
*
* @internal
* @history 2010-12-23 Debbie A. Cook Added set of full cache time
* parameters
*/
void LineScanCameraRotation::LoadCache() {
NaifStatus::CheckErrors();
double startTime = p_cacheTime[0];
int size = p_cacheTime.size();
double endTime = p_cacheTime[size-1];
SetFullCacheParameters(startTime, endTime, size);
// TODO Add a label value to indicate pointing is already decomposed to line scan angles
// and set p_pointingDecomposition=none,framing angles, or line scan angles.
// Also add a label value to indicate jitterOffsets=jitterFileName
// Then we can decide whether to simply grab the crot angles or do new decomposition and whether
// to apply jitter or throw an error because jitter has already been applied.
// *** May need to do a frame trace and load the frames (at least the constant ones) ***
// Loop and load the cache
double state[6];
double lt;
NaifStatus::CheckErrors();
double R[3]; // Direction of radial axis of line scan camera
double C[3]; // Direction of cross-track axis
double I[3]; // Direction of in-track axis
double *velocity;
std::vector<double> IB(9);
std::vector<double> CI(9);
SpiceRotation *prot = p_spi->bodyRotation();
SpiceRotation *crot = p_spi->instrumentRotation();
for(std::vector<double>::iterator i = p_cacheTime.begin(); i < p_cacheTime.end(); i++) {
double et = *i;
prot->SetEphemerisTime(et);
crot->SetEphemerisTime(et);
// The following code will be put into method LoadIBcache()
spkezr_c("MRO", et, "IAU_MARS", "NONE", "MARS", state, <);
NaifStatus::CheckErrors();
// Compute the direction of the radial axis (3) of the line scan camera
vscl_c(1. / vnorm_c(state), state, R); // vscl and vnorm only operate on first 3 members of state
// Compute the direction of the cross-track axis (2) of the line scan camera
velocity = state + 3;
vscl_c(1. / vnorm_c(velocity), velocity, C);
vcrss_c(R, C, C);
// Compute the direction of the in-track axis (1) of the line scan camera
vcrss_c(C, R, I);
// Load the matrix IB and enter it into the cache
vequ_c(I, (SpiceDouble( *)) &IB[0]);
vequ_c(C, (SpiceDouble( *)) &IB[3]);
vequ_c(R, (SpiceDouble( *)) &IB[6]);
p_cacheIB.push_back(IB);
// end IB code
// Compute the CIcr matrix - in-track, cross-track, radial frame to constant frame
mxmt_c((SpiceDouble( *)[3]) & (crot->TimeBasedMatrix())[0], (SpiceDouble( *)[3]) & (prot->Matrix())[0],
(SpiceDouble( *)[3]) &CI[0]);
// Put CI into parent cache to use the parent class methods on it
mxmt_c((SpiceDouble( *)[3]) &CI[0], (SpiceDouble( *)[3]) &IB[0], (SpiceDouble( *)[3]) &CI[0]);
p_cache.push_back(CI);
}
p_cachesLoaded = true;
SetSource(Memcache);
NaifStatus::CheckErrors();
}
static void generate_round_keys(int rnds,uint32_t* K, uint32_t* x, uint32_t* z)
{
COMPUTE_Z;
K[1]=S5[IA(z[2])]^S6[IB(z[2])]^S7[ID(z[1])]^S8[IC(z[1])]^S5[IC(z[0])];
K[2]=S5[IC(z[2])]^S6[ID(z[2])]^S7[IB(z[1])]^S8[IA(z[1])]^S6[IC(z[1])];
K[3]=S5[IA(z[3])]^S6[IB(z[3])]^S7[ID(z[0])]^S8[IC(z[0])]^S7[IB(z[2])];
K[4]=S5[IC(z[3])]^S6[ID(z[3])]^S7[IB(z[0])]^S8[IA(z[0])]^S8[IA(z[3])];
COMPUTE_X;
K[5]=S5[ID(x[0])]^S6[IC(x[0])]^S7[IA(x[3])]^S8[IB(x[3])]^S5[IA(x[2])];
K[6]=S5[IB(x[0])]^S6[IA(x[0])]^S7[IC(x[3])]^S8[ID(x[3])]^S6[IB(x[3])];
K[7]=S5[ID(x[1])]^S6[IC(x[1])]^S7[IA(x[2])]^S8[IB(x[2])]^S7[ID(x[0])];
K[8]=S5[IB(x[1])]^S6[IA(x[1])]^S7[IC(x[2])]^S8[ID(x[2])]^S8[ID(x[1])];
COMPUTE_Z;
K[9]=S5[ID(z[0])]^S6[IC(z[0])]^S7[IA(z[3])]^S8[IB(z[3])]^S5[IB(z[2])];
K[10]=S5[IB(z[0])]^S6[IA(z[0])]^S7[IC(z[3])]^S8[ID(z[3])]^S6[IA(z[3])];
K[11]=S5[ID(z[1])]^S6[IC(z[1])]^S7[IA(z[2])]^S8[IB(z[2])]^S7[IC(z[0])];
K[12]=S5[IB(z[1])]^S6[IA(z[1])]^S7[IC(z[2])]^S8[ID(z[2])]^S8[IC(z[1])];
COMPUTE_X;
if (rnds==16) {
K[13]=S5[IA(x[2])]^S6[IB(x[2])]^S7[ID(x[1])]^S8[IC(x[1])]^S5[ID(x[0])];
K[14]=S5[IC(x[2])]^S6[ID(x[2])]^S7[IB(x[1])]^S8[IA(x[1])]^S6[ID(x[1])];
K[15]=S5[IA(x[3])]^S6[IB(x[3])]^S7[ID(x[0])]^S8[IC(x[0])]^S7[IA(x[2])];
K[16]=S5[IC(x[3])]^S6[ID(x[3])]^S7[IB(x[0])]^S8[IA(x[0])]^S8[IB(x[3])];
}
}
/* fix narrow-lane ambiguity by ILS ------------------------------------------*/
static int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n)
{
double C1,C2,*B1,*N1,*NC,*D,*E,*Q,s[2],lam_NL=lam_LC(1,1,0),lam1,lam2;
int i,j,k,m=0,info,stat,flgs[MAXSAT]= {0},max_flg=0;
lam1=lam_carr[0];
lam2=lam_carr[1];
C1= SQR(lam2)/(SQR(lam2)-SQR(lam1));
C2=-SQR(lam1)/(SQR(lam2)-SQR(lam1));
B1=zeros(n,1);
N1=zeros(n,2);
D=zeros(rtk->nx,n);
E=mat(n,rtk->nx);
Q=mat(n,n);
NC=mat(n,1);
for (i=0; i<n; i++) {
/* check linear independency */
if (!is_depend(sat1[i],sat2[i],flgs,&max_flg)) continue;
j=IB(sat1[i],&rtk->opt);
k=IB(sat2[i],&rtk->opt);
/* float narrow-lane ambiguity (cycle) */
B1[m]=(rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL;
N1[m]=ROUND(B1[m]);
/* validation of narrow-lane ambiguity */
if (fabs(N1[m]-B1[m])>rtk->opt.thresar[2]) continue;
/* narrow-lane ambiguity transformation matrix */
D[j+m*rtk->nx]= 1.0/lam_NL;
D[k+m*rtk->nx]=-1.0/lam_NL;
sat1[m]=sat1[i];
sat2[m]=sat2[i];
NW[m++]=NW[i];
}
if (m<3) return 0;
/* covariance of narrow-lane ambiguities */
matmul("TN",m,rtk->nx,rtk->nx,1.0,D,rtk->P,0.0,E);
matmul("NN",m,m,rtk->nx,1.0,E,D,0.0,Q);
/* integer least square */
if ((info=lambda(m,2,B1,Q,N1,s))) {
trace(2,"lambda error: info=%d\n",info);
return 0;
}
if (s[0]<=0.0) return 0;
rtk->sol.ratio=(float)(MIN(s[1]/s[0],999.9));
/* varidation by ratio-test */
if (rtk->opt.thresar[0]>0.0&&rtk->sol.ratio<rtk->opt.thresar[0]) {
trace(2,"varidation error: n=%2d ratio=%8.3f\n",m,rtk->sol.ratio);
return 0;
}
trace(2,"varidation ok: %s n=%2d ratio=%8.3f\n",time_str(rtk->sol.time,0),m,
rtk->sol.ratio);
/* narrow-lane to iono-free ambiguity */
for (i=0; i<m; i++) {
NC[i]=C1*lam1*N1[i]+C2*lam2*(N1[i]-NW[i]);
}
/* fixed solution */
stat=fix_sol(rtk,sat1,sat2,NC,m);
free(B1);
free(N1);
free(D);
free(E);
free(Q);
free(NC);
return stat;
}
void cvUpdateTracks(CvBlobs const &blobs, CvTracks &tracks, const double thDistance, const unsigned int thInactive, const unsigned int thActive)
{
CV_FUNCNAME("cvUpdateTracks");
__CV_BEGIN__;
unsigned int nBlobs = blobs.size();
unsigned int nTracks = tracks.size();
// Proximity matrix:
// Last row/column is for ID/label.
// Last-1 "/" is for accumulation.
CvID *close = new unsigned int[(nBlobs+2)*(nTracks+2)]; // XXX Must be same type than CvLabel.
try
{
// Inicialization:
unsigned int i=0;
for (CvBlobs::const_iterator it = blobs.begin(); it!=blobs.end(); ++it, i++)
{
AB(i) = 0;
IB(i) = it->second->label;
}
CvID maxTrackID = 0;
unsigned int j=0;
for (CvTracks::const_iterator jt = tracks.begin(); jt!=tracks.end(); ++jt, j++)
{
AT(j) = 0;
IT(j) = jt->second->id;
if (jt->second->id > maxTrackID)
maxTrackID = jt->second->id;
}
// Proximity matrix calculation and "used blob" list inicialization:
for (i=0; i<nBlobs; i++)
for (j=0; j<nTracks; j++)
if (C(i, j) = (distantBlobTrack(B(i), T(j)) < thDistance))
{
AB(i)++;
AT(j)++;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Detect inactive tracks
for (j=0; j<nTracks; j++)
{
unsigned int c = AT(j);
if (c==0)
{
//cout << "Inactive track: " << j << endl;
// Inactive track.
CvTrack *track = T(j);
track->inactive++;
track->label = 0;
}
}
// Detect new tracks
for (i=0; i<nBlobs; i++)
{
unsigned int c = AB(i);
if (c==0)
{
//cout << "Blob (new track): " << maxTrackID+1 << endl;
//cout << *B(i) << endl;
// New track.
maxTrackID++;
CvBlob *blob = B(i);
CvTrack *track = new CvTrack;
track->id = maxTrackID;
track->label = blob->label;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
track->centroid = blob->centroid;
track->lifetime = 0;
track->active = 0;
track->inactive = 0;
tracks.insert(CvIDTrack(maxTrackID, track));
}
}
// Clustering
for (j=0; j<nTracks; j++)
{
unsigned int c = AT(j);
if (c)
{
list<CvTrack*> tt; tt.push_back(T(j));
list<CvBlob*> bb;
getClusterForTrack(j, close, nBlobs, nTracks, blobs, tracks, bb, tt);
// Select track
CvTrack *track;
unsigned int area = 0;
for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it)
{
CvTrack *t = *it;
unsigned int a = (t->maxx-t->minx)*(t->maxy-t->miny);
if (a>area)
{
area = a;
track = t;
}
}
// Select blob
CvBlob *blob;
area = 0;
//cout << "Matching blobs: ";
for (list<CvBlob*>::const_iterator it=bb.begin(); it!=bb.end(); ++it)
{
CvBlob *b = *it;
//cout << b->label << " ";
if (b->area>area)
{
area = b->area;
blob = b;
}
}
//cout << endl;
// Update track
//cout << "Matching: track=" << track->id << ", blob=" << blob->label << endl;
track->label = blob->label;
track->centroid = blob->centroid;
track->minx = blob->minx;
track->miny = blob->miny;
track->maxx = blob->maxx;
track->maxy = blob->maxy;
if (track->inactive)
track->active = 0;
track->inactive = 0;
// Others to inactive
for (list<CvTrack*>::const_iterator it=tt.begin(); it!=tt.end(); ++it)
{
CvTrack *t = *it;
if (t!=track)
{
//cout << "Inactive: track=" << t->id << endl;
t->inactive++;
t->label = 0;
}
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for (CvTracks::iterator jt=tracks.begin(); jt!=tracks.end();)
if ((jt->second->inactive>=thInactive)||((jt->second->inactive)&&(thActive)&&(jt->second->active<thActive)))
{
delete jt->second;
tracks.erase(jt++);
}
else
{
jt->second->lifetime++;
if (!jt->second->inactive)
jt->second->active++;
++jt;
}
}
catch (...)
{
delete[] close;
throw; // TODO: OpenCV style.
}
delete[] close;
__CV_END__;
}
template <class inputType, unsigned int Dimension> medAbstractJob::medJobExitStatus medItkBiasCorrectionProcess::N4BiasCorrectionCore()
{
medJobExitStatus eRes = medAbstractJob::MED_JOB_EXIT_SUCCESS;
typedef itk::Image<inputType, Dimension > ImageType;
typedef itk::Image <float, Dimension> OutputImageType;
typedef itk::Image<unsigned char, Dimension> MaskImageType;
typedef itk::N4BiasFieldCorrectionImageFilter<OutputImageType, MaskImageType, OutputImageType> BiasFilter;
typedef itk::ConstantPadImageFilter<OutputImageType, OutputImageType> PadderType;
typedef itk::ConstantPadImageFilter<MaskImageType, MaskImageType> MaskPadderType;
typedef itk::ShrinkImageFilter<OutputImageType, OutputImageType> ShrinkerType;
typedef itk::ShrinkImageFilter<MaskImageType, MaskImageType> MaskShrinkerType;
typedef itk::BSplineControlPointImageFilter<typename BiasFilter::BiasFieldControlPointLatticeType, typename BiasFilter::ScalarImageType> BSplinerType;
typedef itk::ExpImageFilter<OutputImageType, OutputImageType> ExpFilterType;
typedef itk::DivideImageFilter<OutputImageType, OutputImageType, OutputImageType> DividerType;
typedef itk::ExtractImageFilter<OutputImageType, OutputImageType> CropperType;
unsigned int uiThreadNb = static_cast<unsigned int>(m_poUIThreadNb->value());
unsigned int uiShrinkFactors = static_cast<unsigned int>(m_poUIShrinkFactors->value());
unsigned int uiSplineOrder = static_cast<unsigned int>(m_poUISplineOrder->value());
float fWienerFilterNoise = static_cast<float>(m_poFWienerFilterNoise->value());
float fbfFWHM = static_cast<float>(m_poFbfFWHM->value());
float fConvergenceThreshold = static_cast<float>(m_poFConvergenceThreshold->value());
float fSplineDistance = static_cast<float>(m_poFSplineDistance->value());
float fProgression = 0;
QStringList oListValue = m_poSMaxIterations->value().split("x");
std::vector<unsigned int> oMaxNumbersIterationsVector(oListValue.size());
std::vector<float> oInitialMeshResolutionVect(Dimension);
for (int i=0; i<oMaxNumbersIterationsVector.size(); ++i)
{
oMaxNumbersIterationsVector[i] = (unsigned int)oListValue[i].toInt();
}
oInitialMeshResolutionVect[0] = static_cast<float>(m_poFInitialMeshResolutionVect1->value());
oInitialMeshResolutionVect[1] = static_cast<float>(m_poFInitialMeshResolutionVect2->value());
oInitialMeshResolutionVect[2] = static_cast<float>(m_poFInitialMeshResolutionVect3->value());
typename ImageType::Pointer image = dynamic_cast<ImageType *>((itk::Object*)(this->input()->data()));
typedef itk::CastImageFilter <ImageType, OutputImageType> CastFilterType;
typename CastFilterType::Pointer castFilter = CastFilterType::New();
castFilter->SetInput(image);
/********************************************************************************/
/***************************** PREPARING STARTING *******************************/
/********************************************************************************/
/*** 0 ******************* Create filter and accessories ******************/
ABORT_CHECKING(m_bAborting);
typename BiasFilter::Pointer filter = BiasFilter::New();
typename BiasFilter::ArrayType oNumberOfControlPointsArray;
m_filter = filter;
/*** 1 ******************* Read input image *******************************/
ABORT_CHECKING(m_bAborting);
fProgression = 1;
updateProgression(fProgression);
/*** 2 ******************* Creating Otsu mask *****************************/
ABORT_CHECKING(m_bAborting);
itk::TimeProbe timer;
timer.Start();
typename MaskImageType::Pointer maskImage = ITK_NULLPTR;
typedef itk::OtsuThresholdImageFilter<OutputImageType, MaskImageType> ThresholderType;
typename ThresholderType::Pointer otsu = ThresholderType::New();
m_filter = otsu;
otsu->SetInput(castFilter->GetOutput());
otsu->SetNumberOfHistogramBins(200);
otsu->SetInsideValue(0);
otsu->SetOutsideValue(1);
otsu->SetNumberOfThreads(uiThreadNb);
otsu->Update();
updateProgression(fProgression);
maskImage = otsu->GetOutput();
/*** 3A *************** Set Maximum number of Iterations for the filter ***/
ABORT_CHECKING(m_bAborting);
typename BiasFilter::VariableSizeArrayType itkTabMaximumIterations;
itkTabMaximumIterations.SetSize(oMaxNumbersIterationsVector.size());
for (int i = 0; i < oMaxNumbersIterationsVector.size(); ++i)
{
itkTabMaximumIterations[i] = oMaxNumbersIterationsVector[i];
}
filter->SetMaximumNumberOfIterations(itkTabMaximumIterations);
/*** 3B *************** Set Fitting Levels for the filter *****************/
typename BiasFilter::ArrayType oFittingLevelsTab;
oFittingLevelsTab.Fill(oMaxNumbersIterationsVector.size());
filter->SetNumberOfFittingLevels(oFittingLevelsTab);
updateProgression(fProgression);
/*** 4 ******************* Save image's index, size, origine **************/
ABORT_CHECKING(m_bAborting);
typename ImageType::IndexType oImageIndex = image->GetLargestPossibleRegion().GetIndex();
typename ImageType::SizeType oImageSize = image->GetLargestPossibleRegion().GetSize();
typename ImageType::PointType newOrigin = image->GetOrigin();
typename OutputImageType::Pointer outImage = castFilter->GetOutput();
if (fSplineDistance > 0)
{
/*** 5 ******************* Compute number of control points **************/
ABORT_CHECKING(m_bAborting);
itk::SizeValueType lowerBound[3];
itk::SizeValueType upperBound[3];
for (unsigned int i = 0; i < 3; i++)
{
float domain = static_cast<float>(image->GetLargestPossibleRegion().GetSize()[i] - 1) * image->GetSpacing()[i];
unsigned int numberOfSpans = static_cast<unsigned int>(std::ceil(domain / fSplineDistance));
unsigned long extraPadding = static_cast<unsigned long>((numberOfSpans * fSplineDistance - domain) / image->GetSpacing()[i] + 0.5);
lowerBound[i] = static_cast<unsigned long>(0.5 * extraPadding);
upperBound[i] = extraPadding - lowerBound[i];
newOrigin[i] -= (static_cast<float>(lowerBound[i]) * image->GetSpacing()[i]);
oNumberOfControlPointsArray[i] = numberOfSpans + filter->GetSplineOrder();
}
updateProgression(fProgression);
/*** 6 ******************* Padder ****************************************/
ABORT_CHECKING(m_bAborting);
typename PadderType::Pointer imagePadder = PadderType::New();
m_filter = imagePadder;
imagePadder->SetInput(castFilter->GetOutput());
imagePadder->SetPadLowerBound(lowerBound);
imagePadder->SetPadUpperBound(upperBound);
imagePadder->SetConstant(0);
imagePadder->SetNumberOfThreads(uiThreadNb);
imagePadder->Update();
updateProgression(fProgression);
outImage = imagePadder->GetOutput();
/*** 7 ******************** Handle the mask image *************************/
ABORT_CHECKING(m_bAborting);
typename MaskPadderType::Pointer maskPadder = MaskPadderType::New();
m_filter = maskPadder;
maskPadder->SetInput(maskImage);
maskPadder->SetPadLowerBound(lowerBound);
maskPadder->SetPadUpperBound(upperBound);
maskPadder->SetConstant(0);
maskPadder->SetNumberOfThreads(uiThreadNb);
maskPadder->Update();
updateProgression(fProgression);
maskImage = maskPadder->GetOutput();
/*** 8 ******************** SetNumber Of Control Points *******************/
ABORT_CHECKING(m_bAborting);
filter->SetNumberOfControlPoints(oNumberOfControlPointsArray);
}
else if (oInitialMeshResolutionVect.size() == 3)
{
/*** 9 ******************** SetNumber Of Control Points alternative *******/
ABORT_CHECKING(m_bAborting);
for (unsigned i = 0; i < 3; i++)
{
oNumberOfControlPointsArray[i] = static_cast<unsigned int>(oInitialMeshResolutionVect[i]) + filter->GetSplineOrder();
}
filter->SetNumberOfControlPoints(oNumberOfControlPointsArray);
updateProgression(fProgression, 3);
}
else
{
fProgression = 0;
updateProgression(fProgression);
std::cout << "No BSpline distance and Mesh Resolution is ignored because not 3 dimensions" << std::endl;
}
/*** 10 ******************* Shrinker image ********************************/
ABORT_CHECKING(m_bAborting);
typename ShrinkerType::Pointer imageShrinker = ShrinkerType::New();
m_filter = imageShrinker;
imageShrinker->SetInput(outImage);
/*** 11 ******************* Shrinker mask *********************************/
ABORT_CHECKING(m_bAborting);
typename MaskShrinkerType::Pointer maskShrinker = MaskShrinkerType::New();
m_filter = maskShrinker;
maskShrinker->SetInput(maskImage);
/*** 12 ******************* Shrink mask and image *************************/
ABORT_CHECKING(m_bAborting);
imageShrinker->SetShrinkFactors(uiShrinkFactors);
maskShrinker->SetShrinkFactors(uiShrinkFactors);
imageShrinker->SetNumberOfThreads(uiThreadNb);
maskShrinker->SetNumberOfThreads(uiThreadNb);
imageShrinker->Update();
updateProgression(fProgression);
maskShrinker->Update();
updateProgression(fProgression);
/*** 13 ******************* Filter setings ********************************/
ABORT_CHECKING(m_bAborting);
filter->SetSplineOrder(uiSplineOrder);
filter->SetWienerFilterNoise(fWienerFilterNoise);
filter->SetBiasFieldFullWidthAtHalfMaximum(fbfFWHM);
filter->SetConvergenceThreshold(fConvergenceThreshold);
filter->SetInput(imageShrinker->GetOutput());
filter->SetMaskImage(maskShrinker->GetOutput());
/*** 14 ******************* Apply filter **********************************/
ABORT_CHECKING(m_bAborting);
try
{
filter->SetNumberOfThreads(uiThreadNb);
filter->Update();
updateProgression(fProgression, 5);
}
catch (itk::ExceptionObject & err)
{
std::cerr << "ExceptionObject caught !" << std::endl;
std::cerr << err << std::endl;
eRes = medAbstractJob::MED_JOB_EXIT_FAILURE;
return eRes;
}
/**
* Reconstruct the bias field at full image resolution. Divide
* the original input image by the bias field to get the final
* corrected image.
*/
ABORT_CHECKING(m_bAborting);
typename BSplinerType::Pointer bspliner = BSplinerType::New();
m_filter = bspliner;
bspliner->SetInput(filter->GetLogBiasFieldControlPointLattice());
bspliner->SetSplineOrder(filter->GetSplineOrder());
bspliner->SetSize(image->GetLargestPossibleRegion().GetSize());
bspliner->SetOrigin(newOrigin);
bspliner->SetDirection(image->GetDirection());
bspliner->SetSpacing(image->GetSpacing());
bspliner->SetNumberOfThreads(uiThreadNb);
bspliner->Update();
updateProgression(fProgression);
/*********************** Logarithm phase ***************************/
ABORT_CHECKING(m_bAborting);
typename OutputImageType::Pointer logField = OutputImageType::New();
logField->SetOrigin(image->GetOrigin());
logField->SetSpacing(image->GetSpacing());
logField->SetRegions(image->GetLargestPossibleRegion());
logField->SetDirection(image->GetDirection());
logField->Allocate();
itk::ImageRegionIterator<typename BiasFilter::ScalarImageType> IB(bspliner->GetOutput(), bspliner->GetOutput()->GetLargestPossibleRegion());
itk::ImageRegionIterator<OutputImageType> IF(logField, logField->GetLargestPossibleRegion());
for (IB.GoToBegin(), IF.GoToBegin(); !IB.IsAtEnd(); ++IB, ++IF)
{
IF.Set(IB.Get()[0]);
}
/*********************** Exponential phase *************************/
ABORT_CHECKING(m_bAborting);
typename ExpFilterType::Pointer expFilter = ExpFilterType::New();
m_filter = expFilter;
expFilter->SetInput(logField);
expFilter->SetNumberOfThreads(uiThreadNb);
expFilter->Update();
updateProgression(fProgression);
/************************ Dividing phase ***************************/
ABORT_CHECKING(m_bAborting);
typename DividerType::Pointer divider = DividerType::New();
m_filter = divider;
divider->SetInput1(castFilter->GetOutput());
divider->SetInput2(expFilter->GetOutput());
divider->SetNumberOfThreads(uiThreadNb);
divider->Update();
updateProgression(fProgression);
/******************** Prepare cropping phase ***********************/
ABORT_CHECKING(m_bAborting);
typename ImageType::RegionType inputRegion;
inputRegion.SetIndex(oImageIndex);
inputRegion.SetSize(oImageSize);
/************************ Cropping phase ***************************/
ABORT_CHECKING(m_bAborting);
typename CropperType::Pointer cropper = CropperType::New();
m_filter = cropper;
cropper->SetInput(divider->GetOutput());
cropper->SetExtractionRegion(inputRegion);
cropper->SetDirectionCollapseToSubmatrix();
cropper->SetNumberOfThreads(uiThreadNb);
cropper->Update();
updateProgression(fProgression);
/********************** Write output image *************************/
ABORT_CHECKING(m_bAborting);
medAbstractImageData *out = qobject_cast<medAbstractImageData *>(medAbstractDataFactory::instance()->create("itkDataImageFloat3"));
out->setData(cropper->GetOutput());
this->setOutput(out);
m_filter = 0;
return eRes;
}