void
patchcase(Inst *i, int n, int *jmptbl)
{
Patch *p;
p = newPatch(P_CASE, i);
p->n = n;
p->jmptbl = jmptbl;
}
FeatureModel::FeatureModel( const CamModel & _camera,
const MotionModel & _motion,
const FeaturesState & _featuresState,
const Mat & frame,
const Point2f & pf,
int patchSize,
float rho_0,
float sigma_rho):
camera(_camera),
motion(_motion),
features(_featuresState)
{
Mat newPatch(cv::Mat(frame, cv::Rect(pf.x-patchSize/2, pf.y-patchSize/2, patchSize,patchSize)));
this->imageLocation << pf.x, pf.y;
this->Patch = newPatch.clone();
Vector2f hd = (Vector2f << (float)pf.x, (float)pf.y);
// TODO: convert using motionmodel
Vector3f r = motion->get_r();
Quatenionf q = motion->get_q();
Vector3f hC = this->camera.unproject(hd, true);
Matrix3f Jac_hCHd = this->camera.getUnprojectionJacobian();
Matrix3f Rot = quat2rot(q);
Vector3f hW = Rot*hC;
float hx = hW(0);
float hy = hW(1);
float hz = hW(2);
float theta = atan2(hx,hz);
float phi = atan2(-hy, sqrt(hx*hx+hz*hz));
// Updating state and Sigma
MatrixXf Jx = this->computeJx();
MatrixXf Jm = this->computeJm();
Matrix2f Sigma_mm = this->computeSigma_mm(sigma_pixel, sigma_pixel);
this->f = (VectorXf(6) << r, theta, phi, rho_0);
this->Sigma_ii = Jm*Sigma_mm*Jm.transpose() + Jx*motion->getSigma_xx()*Jx.transpose();
this->motion->updateVariaceBlocks(Jx);
this->features->updateVariaceBlocks(Jx);
this->features.push_back((*this));
return 1;
}
void
patchop(Inst *i)
{
newPatch(P_OTHER, i);
}
int main (int argc, char *argv [])
{
/*Structures: these are passed to the sub-routines which need them.*/
patch *p1,*p2;
satellite *s;
rangeRef *r;
getRec *signalGetRec;
file *f;
/*Variables.*/
int n_az,n_range;/*Region to be processed.*/
int patchNo;/*Loop counter.*/
double pixShift,dopDel,old_dop;
int refLen,lineToBeRead;
int saved_NLA;
struct ARDOP_PARAMS params,old_params;
meta_parameters *meta;
StartWatch();
if (!quietflag) printf(" Doppler Determination:\n");
if (logflag) printLog(" Doppler Determination:\n");
if (!parse_cla(argc,argv,¶ms,&meta))
printErr(" ERROR: Usage: dop_prf as ardop\n");
/*Set the initial doppler parameters.*/
if (!quietflag) printf(" Processing to doppler at %f prf...\n",params.fd);
if (logflag) {
sprintf(logbuf," Processing to doppler at %f prf...\n",params.fd);
printLog(logbuf);
}
read_params(params.in1,&old_params);/*Read in user's old parameters*/
/*Set params so we're processing at least 300 samples and 500 lines.*/
refLen=params.fs*params.pulsedur;
params.nla=smallestPow2(300+refLen)-refLen;
params.na_valid=500;
ardop_setup(¶ms,meta,&n_az,&n_range,&s,&r,&f,&signalGetRec);
if (!quietflag) {
printf(" Processing a %d az by %d range patch...\n",n_az,n_range);
printf(" Of the %d azimuth lines, only %d are valid.\n",n_az,f->n_az_valid);
}
/*
Create "patch"es of data.
*/
p1=newPatch(n_az,n_range);
p2=newPatch(n_az,n_range);
/*Process both patches.*/
lineToBeRead = f->firstLineToProcess;
ac_direction=-1;
setPatchLoc(p1,s,f->skipFile,f->skipSamp,lineToBeRead);/*Update patch parameters for location.*/
processPatch(p1,signalGetRec,r,s);/*SAR Process patch.*/
ac_direction=1;
setPatchLoc(p2,s,f->skipFile,f->skipSamp,lineToBeRead);/*Update patch parameters for location.*/
processPatch(p2,signalGetRec,r,s);/*SAR Process patch.*/
/*Do range amplitude correlation.*/
pixShift=amp_corr(p1,p2,f);
StopWatch();
dopDel=2*pixShift*f->rngpix/(s->wavl*s->refPerRange/2*(p1->slantToFirst+p1->slantPer*p1->n_range/2));
if (!quietflag) printf(" The image's doppler is off by %.2f pixels, or %.2f PRF.\n",
pixShift,dopDel);
if (logflag) {
sprintf(logbuf," The image's doppler is off by %.2f pixels, or %.2f PRF.\n",
pixShift,dopDel);
printLog(logbuf);
}
/*dopDel-=0.2; <-- Constant frequency offset?*/
dopDel=floor(dopDel+0.5);
if (!quietflag) printf(" The doppler coefficients should be:\n"
" %.10f %.10f %.10f\n\n",s->orig_fd-dopDel,s->orig_fdd,s->orig_fddd);
if (!quietflag) printf(" These coefficients have been written into '%s'\n",appendExt(params.in1,".in"));
if (logflag) {
sprintf(logbuf," The doppler coefficients should be:\n"
" %.10f %.10f %.10f\n\n",s->orig_fd-dopDel,s->orig_fdd,s->orig_fddd);
printLog(logbuf);
sprintf(logbuf," These coefficients have been written into '%s'\n",appendExt(params.in1,".in"));
printLog(logbuf);
}
old_params.fd=s->orig_fd-dopDel;
old_params.fdd=s->orig_fdd;
old_params.fddd=s->orig_fddd;
print_params(params.in1,&old_params,"dop_prf");
destroyPatch(p1);
destroyPatch(p2);
return(0);
}