bool Calibration::Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight)
{
MarkerInfo marker;
bool res = pDetector->GetMarker(pBuffer, cColorHeight, cColorWidth, marker);
if (!res)
return false;
int indexInPoses = -1;
for (unsigned int j = 0; j < markerPoses.size(); j++)
{
if (marker.id == markerPoses[j].markerId)
{
indexInPoses = j;
break;
}
}
if (indexInPoses == -1)
return false;
MarkerPose markerPose = markerPoses[indexInPoses];
iUsedMarkerId = markerPose.markerId;
vector<Point3f> marker3D(marker.corners.size());
bool success = GetMarkerCorners3D(marker3D, marker, pCameraCoordinates, cColorWidth, cColorHeight);
if (!success)
{
return false;
}
marker3DSamples.push_back(marker3D);
nSampleCounter++;
if (nSampleCounter < nRequiredSamples)
return false;
for (size_t i = 0; i < marker3D.size(); i++)
{
marker3D[i] = Point3f();
for (int j = 0; j < nRequiredSamples; j++)
{
marker3D[i].X += marker3DSamples[j][i].X / (float)nRequiredSamples;
marker3D[i].Y += marker3DSamples[j][i].Y / (float)nRequiredSamples;
marker3D[i].Z += marker3DSamples[j][i].Z / (float)nRequiredSamples;
}
}
Procrustes(marker, marker3D, worldT, worldR);
vector<vector<float>> Rcopy = worldR;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
worldR[i][j] = 0;
for (int k = 0; k < 3; k++)
{
worldR[i][j] += markerPose.R[i][k] * Rcopy[k][j];
}
}
}
cameraR = worldR;
cameraT = RotatePoint(worldT, cameraR);
vector<float> translationIncr(3);
translationIncr[0] = markerPose.t[0];
translationIncr[1] = markerPose.t[1];
translationIncr[2] = markerPose.t[2];
cameraT[0] += translationIncr[0];
cameraT[1] += translationIncr[1];
cameraT[2] += translationIncr[2];
translationIncr = InverseRotatePoint(translationIncr, worldR);
worldT[0] += translationIncr[0];
worldT[1] += translationIncr[1];
worldT[2] += translationIncr[2];
bCalibrated = true;
marker3DSamples.clear();
nSampleCounter = 0;
return true;
}
// lmfile (landmarks file) - This is a text file with format:
// i1 j1 k1
// i2 j2 k2
// i3 j3 k3
// where
// (i1, j1, k1) are the coordinates of the AC
// (i2, j2, k2) are the coordinates of the PC, and
// (i3, j3, k3) are the coordinates of the RP.
// subfile (subject file) - 3D T1W volume of type short in NIFTI format
// TPIL - output 4x4 rigid-body transformation matrix that would transform
// subfile into a standardized PIL orientation
void new_PIL_transform(const char *subfile,const char *lmfile,char *orient,float *TPIL, int SAVE_MRX_FLAG)
{
float Qavg[3]; // average of rows of Q
float Pavg[3]; // average of rows of P
float TPIL0[16]; // transforms the original image to MSP/AC-PC aligned PIL orientation
int n; // number of landmarks
float *LM; // (3 x n) matrix of detected landmarks
float *invT;
char filename[1024];
SHORTIM subimPIL;
nifti_1_header PILbraincloud_hdr;
// subfile without the directory structure and extension
char subfile_prefix[1024];
char imagedir[1024];
char modelfile[1024]="";
if( niftiFilename(subfile_prefix, subfile)==0 ) exit(0);
getDirectoryName(subfile, imagedir);
getARTHOME();
// initial TPIL0 using old PIL transformation
char opt_CENTER_AC_old;
opt_CENTER_AC_old = opt_CENTER_AC; // record opt_CENTER_AC
opt_CENTER_AC = NO; // temporarily set opt_CENTER_AC to NO
standard_PIL_transformation(subfile, lmfile, orient, 0, TPIL0);
opt_CENTER_AC = opt_CENTER_AC_old; // restore opt_CENTER_AC
/////////////////////////////////////////////////////////
// Read input volume from subfile
SHORTIM subim;
nifti_1_header subim_hdr;
subim.v = (short *)read_nifti_image(subfile, &subim_hdr);
if(subim.v==NULL)
{
printf("Error reading %s, aborting ...\n", subfile);
exit(1);
}
set_dim(subim, subim_hdr);
/////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////
// Reslice subim to PIL space
sprintf(filename,"%s/PILbrain.nii",ARTHOME);
PILbraincloud_hdr=read_NIFTI_hdr(filename);
set_dim(subimPIL, PILbraincloud_hdr);
invT = inv4(TPIL0);
resliceImage(subim, subimPIL, invT, LIN);
free(invT);
// Detect 8 landmarks on the MSP on the subimPIL volume
sprintf(modelfile,"%s/orion.mdl",ARTHOME);
LM=detect_landmarks(subimPIL, modelfile, n);
convert_to_xyz(LM, n, subimPIL);
// This block outputs the locations of the detected landmarks
// in (i,j,k) coordinates of the native space of the input volume
if(opt_txt)
{
FILE *fp;
float landmark[4];
invT = inv4(TPIL0);
sprintf(filename,"%s/%s_orion.txt",imagedir,subfile_prefix);
fp=fopen(filename,"w");
if(fp==NULL) file_open_error(filename);
for(int i=0; i<n; i++)
{
landmark[0]=LM[0*n + i];
landmark[1]=LM[1*n + i];
landmark[2]=LM[2*n + i];
landmark[3]=1;
multi(invT,4,4,landmark,4,1,landmark);
convert_to_ijk(landmark, 1, subim);
fprintf(fp,"%5.1f %5.1f %5.1f\n",landmark[0], landmark[1], landmark[2]);
}
fclose(fp);
free(invT);
}
float *P;
float *Q; // Image using Q insted of P' in Eq. (1) of Arun et al. 1987
float TLM[16];
Q = (float *)calloc(3*n,sizeof(float));
P = (float *)calloc(3*n,sizeof(float));
for(int i=0; i<3*n; i++) P[i]=LM[i];
delete subimPIL.v;
/////////////////////////////////////////////////////////
// read Q
{
FILE *fp;
int r, r2;
int cm[3];
fp=fopen(modelfile, "r");
if(fp==NULL) file_open_error(modelfile);
fread(&n, sizeof(int), 1, fp);
fread(&r, sizeof(int), 1, fp);
fread(&r2, sizeof(int), 1, fp);
SPH refsph(r);
for(int i=0; i<n; i++)
{
fread(cm, sizeof(int), 3, fp);
fread(refsph.v, sizeof(float), refsph.n, fp);
Q[0*n + i]=cm[0];
Q[1*n + i]=cm[1];
Q[2*n + i]=cm[2];
}
fclose(fp);
convert_to_xyz(Q, n, subimPIL);
}
Procrustes(Q, Qavg, n, P, Pavg, TLM);
multi(TLM,4,4,TPIL0,4,4,TPIL);
// create the *LM.ppm image
if(opt_ppm || opt_png)
{
int *lmx, *lmy;
float lm[4];
invT = inv4(TPIL);
resliceImage(subim, subimPIL, invT, LIN);
free(invT);
lmx = (int *)calloc(n,sizeof(int));
lmy = (int *)calloc(n,sizeof(int));
for(int i=0; i<n; i++)
{
lm[0] = P[i] + Pavg[0];
lm[1] = P[n+i] + Pavg[1];
lm[2] = P[2*n+i] + Pavg[2];
lm[3] = 1.0;
multi(TLM,4,4,lm,4,1,lm);
convert_to_ijk(lm, 1, subimPIL);
lmx[i]=(int)( lm[0] + 0.5 );
lmy[i]=(int)( lm[1] + 0.5 );
}
sprintf(filename,"%s/%s_orion.ppm",imagedir, subfile_prefix);
mspPPM(subimPIL, lmx, lmy, n, filename);
delete lmx;
delete lmy;
delete subimPIL.v;
}
if(opt_CENTER_AC)
{
float ac[4];
float Ttmp[16];
ac[0] = P[1] + Pavg[0]; // landmark #1 is AC
ac[1] = P[n+1] + Pavg[1];
ac[2] = P[2*n+1] + Pavg[2];
ac[3] = 1.0;
multi(TLM,4,4,ac,4,1,ac);
Ttmp[0]=1.0; Ttmp[1]=0.0; Ttmp[2]=0.0; Ttmp[3]=-ac[0]; // makes ac the center of the FOV
Ttmp[4]=0.0; Ttmp[5]=1.0; Ttmp[6]=0.0; Ttmp[7]=-ac[1]; // ac[1] should be equal to pc[1]
Ttmp[8]=0.0; Ttmp[9]=0.0; Ttmp[10]=1.0; Ttmp[11]=0;
Ttmp[12]=0.0; Ttmp[13]=0.0; Ttmp[14]=0.0; Ttmp[15]=1.0;
multi(Ttmp,4,4,TPIL,4,4,TPIL);
}
// save the PIL transformation in <subfile_prefix>_PIL.mrx
if(SAVE_MRX_FLAG == 1)
{
FILE *fp;
sprintf(filename,"%s/%s_PIL.mrx",imagedir, subfile_prefix);
fp=fopen(filename,"w");
if(fp==NULL) file_open_error(filename);
printMatrix(TPIL,4,4,"",fp);
fclose(fp);
}
{
float ssd1=0.0;
float ssd3=0.0;
float x[4], y[4];
x[3]=y[3]=1.0;
for(int i=0; i<n; i++)
{
x[0] = Q[i] + Qavg[0];
x[1] = Q[i+n] + Qavg[1];
x[2] = Q[i+2*n] + Qavg[2];
y[0] = P[i]+Pavg[0];
y[1] = P[i+n]+Pavg[1];
y[2] = P[i+2*n]+Pavg[2];
ssd1 += (x[0]-y[0])*(x[0]-y[0]);
ssd1 += (x[1]-y[1])*(x[1]-y[1]);
ssd1 += (x[2]-y[2])*(x[2]-y[2]);
multi(TLM,4,4,y,4,1,y);
ssd3 += (x[0]-y[0])*(x[0]-y[0]);
ssd3 += (x[1]-y[1])*(x[1]-y[1]);
ssd3 += (x[2]-y[2])*(x[2]-y[2]);
}
//if(opt_v) printf("SSD (MSP + AC/PC transformation) = %f\n",ssd1);
//if(opt_v) printf("SSD (MSP + AC/PC + LM transformation) = %f\n",ssd3);
}
delete subim.v;
free(P);
free(Q);
}
