void buildOpenGLProjectionForIntrinsics_OpenGLColumnMajorD
(
double * frustum,
int * viewport ,
double fx,
double fy,
double skew,
double cx, double cy,
unsigned int imageWidth, unsigned int imageHeight,
double nearPlane,
double farPlane
)
{
// fprintf(stderr,"buildOpenGLProjectionForIntrinsicsD according to old Ammar code Image ( %u x %u )\n",imageWidth,imageHeight);
// fprintf(stderr,"fx %0.2f fy %0.2f , cx %0.2f , cy %0.2f , skew %0.2f \n",fx,fy,cx,cy,skew);
// fprintf(stderr,"Near %0.2f Far %0.2f \n",nearPlane,farPlane);
if (farPlane==0.0)
{
fprintf(stderr,RED "Far plane is zero, argument bug..? \n" NORMAL);
exit(1);
}
// These parameters define the final viewport that is rendered into by
// the camera.
// Left Bottom Right Top
double L = 0.0 , B = 0.0 , R = imageWidth , T = imageHeight;
// near and far clipping planes, these only matter for the mapping from
// world-space z-coordinate into the depth coordinate for OpenGL
double N = nearPlane , F = farPlane;
double R_sub_L = R-L , T_sub_B = T-B , F_sub_N = F-N , F_plus_N = F+N , F_mul_N = F*N;
if ( (R_sub_L==0) || (R_sub_L-1.0f==0) ||
(T_sub_B==0) || (T_sub_B-1.0f==0) ||
(F_sub_N==0) ) { fprintf(stderr,"Problem with image limits R-L=%f , T-B=%f , F-N=%f\n",R_sub_L,T_sub_B,F_sub_N); }
// set the viewport parameters
viewport[0] = L; viewport[1] = B; viewport[2] = R_sub_L; viewport[3] = T_sub_B;
//OpenGL Projection Matrix ready for loading ( column-major ) , also axis compensated
frustum[0] = -2.0f*fx/R_sub_L; frustum[1] = 0.0f; frustum[2] = 0.0f; frustum[3] = 0.0f;
frustum[4] = 0.0f; frustum[5] = 2.0f*fy/T_sub_B; frustum[6] = 0.0f; frustum[7] = 0.0f;
frustum[8] = 2.0f*cx/R_sub_L-1.0f; frustum[9] = 2.0f*cy/T_sub_B-1.0f; frustum[10]=-1.0*(F_plus_N/F_sub_N); frustum[11] = -1.0f;
frustum[12]= 0.0f; frustum[13]= 0.0f; frustum[14]=-2.0f*F_mul_N/(F_sub_N); frustum[15] = 0.0f;
//Matrix already in OpenGL column major format
//TROUBLESHOOTING Left To Right Hand conventions , Thanks Damien 24-06-15
double identMat[16];
double finalFrutstrum[16];
create4x4IdentityMatrix(identMat);
identMat[10]=-1;
multiplyTwo4x4Matrices(finalFrutstrum,identMat,frustum);
copy4x4DMatrix(frustum,finalFrutstrum);
}
int simpleRendererInitialize(struct simpleRenderer * sr)
{
sr->viewport[0]=0;
sr->viewport[1]=0;
sr->viewport[2]=sr->width;
sr->viewport[3]=sr->height;
buildOpenGLProjectionForIntrinsics(
sr->projectionMatrix ,
sr->viewport ,
sr->fx,
sr->fy,
sr->skew,
sr->cx,
sr->cy,
sr->width,
sr->height,
sr->near,
sr->far
);
double viewMatrixD[16];
create4x4IdentityMatrix(viewMatrixD);
create4x4ScalingMatrix(viewMatrixD,0.01,0.01,-0.01);
copy4x4DMatrixToF(sr->viewMatrix,viewMatrixD);
//Initialization of matrices not yet used
create4x4IdentityMatrixF(sr->modelMatrix);
create4x4IdentityMatrixF(sr->modelViewMatrix);
return 1;
}
void changeYandZAxisOpenGL4x4Matrix(double * result,double * matrix)
{
#if PRINT_MATRIX_DEBUGGING
fprintf(stderr,"Invert Y and Z axis\n");
#endif // PRINT_MATRIX_DEBUGGING
double * invertOp = (double * ) malloc ( sizeof(double) * 16 );
if (invertOp==0) { return; }
create4x4IdentityMatrix(invertOp);
invertOp[5]=-1; invertOp[10]=-1;
multiplyTwo4x4Matrices(result, matrix, invertOp);
free(invertOp);
}
unsigned char * selectSegmentationForDepthFrame(unsigned short * source , unsigned int width , unsigned int height , struct SegmentationFeaturesDepth * segConf , struct calibration * calib)
{
unsigned short * sourceCopy = (unsigned short *) malloc( width * height * sizeof(unsigned short));
if ( sourceCopy == 0) { return 0; }
memcpy(sourceCopy,source,width*height*sizeof(unsigned short));
unsigned short * target = (unsigned short *) malloc( width * height * sizeof(unsigned short));
if ( target == 0) { free(sourceCopy); return 0; }
memset(target,0,width*height*sizeof(short));
removeDepthFloodFillBeforeProcessing(sourceCopy,target,width,height,segConf);
unsigned int sourceWidthStep = width;
unsigned int targetWidthStep = width;
unsigned int posX = segConf->minX;
unsigned int posY = segConf->minY;
width = segConf->maxX-segConf->minX;
height = segConf->maxY-segConf->minY;
unsigned short * sourcePixelsStart = (unsigned short*) sourceCopy + ( (posX) + posY * sourceWidthStep );
unsigned short * sourcePixelsLineEnd = sourcePixelsStart + (width);
unsigned short * sourcePixelsEnd = sourcePixelsLineEnd + ((height-1) * sourceWidthStep );
unsigned short * sourcePixels = sourcePixelsStart;
unsigned char * selectedDepth = (unsigned char*) malloc(width*height*sizeof(unsigned char));
if (selectedDepth==0) { fprintf(stderr,"Could not allocate memory for RGB Selection\n"); return 0; }
memset(selectedDepth,0,width*height*sizeof(unsigned char));
unsigned char * selectedPtr = selectedDepth;
unsigned int x =0;
unsigned int y =0;
unsigned short * depth=0;
while (sourcePixels<sourcePixelsEnd)
{
while (sourcePixels<sourcePixelsLineEnd)
{
depth = sourcePixels++;
if (*depth != 0)
{ //If there is a depth given for point
if ( (segConf->minDepth <= *depth) && (*depth <= segConf->maxDepth) ) { *selectedPtr=1; } else
{ *selectedPtr=0; }
}
++selectedPtr;
++x;
if (x>=width) { x=0; ++y;}
}
sourcePixelsLineEnd+=sourceWidthStep;
}
// -------------------------------------------------------------------------------------------------
// --------------------------------- BOUNDING BOX SEGMENTATION -------------------------------------
// -------------------------------------------------------------------------------------------------
if (segConf->enableBBox)
{
fprintf(stderr,"Selecting Bounding Box %0.2f %0.2f %0.2f -> %0.2f %0.2f %0.2f \n",segConf->bboxX1,segConf->bboxY1,segConf->bboxZ1,segConf->bboxX2,segConf->bboxY2,segConf->bboxZ2);
float fx = 537.479600 , fy = 536.572920 , cx = 317.389787 ,cy = 236.118093;
if ( calib->intrinsicParametersSet )
{
fx = calib->intrinsic[CALIB_INTR_FX];
fy = calib->intrinsic[CALIB_INTR_FY];
cx = calib->intrinsic[CALIB_INTR_CX];
cy = calib->intrinsic[CALIB_INTR_CY];
if (fx==0) { fx=1;}
if (fy==0) { fy=1;}
} else {fprintf(stderr,"No intrinsic parameters provided , bounding box segmentation will use default intrinsic values ( you probably dont want this )\n"); }
double * m = alloc4x4Matrix();
if (m==0) {fprintf(stderr,"Could not allocate a 4x4 matrix , cannot perform bounding box selection\n"); } else
{
create4x4IdentityMatrix(m);
if ( calib->extrinsicParametersSet ) { convertRodriguezAndTranslationToOpenGL4x4DMatrix(m, calib->extrinsicRotationRodriguez , calib->extrinsicTranslation); }
else {fprintf(stderr,"No extrinsic parameters provided , bounding box segmentation will use default coordinate system \n"); }
double raw3D[4]={0};
double world3D[4]={0};
sourcePixelsStart = (unsigned short*) sourceCopy + ( (posX) + posY * sourceWidthStep );
sourcePixelsLineEnd = sourcePixelsStart + (width);
sourcePixelsEnd = sourcePixelsLineEnd + ((height-1) * sourceWidthStep );
sourcePixels = sourcePixelsStart;
selectedPtr = selectedDepth;
sourcePixels = sourcePixelsStart;
sourcePixelsLineEnd = sourcePixelsStart + (width);
x=0; y=0;
depth=0;
while (sourcePixels<sourcePixelsEnd)
{
while (sourcePixels<sourcePixelsLineEnd)
{
depth = sourcePixels++;
if ( (*selectedPtr!=0) ) // && (*depth != 0)
{
raw3D[0] = (double) (x - cx) * (*depth) / fx;
raw3D[1] = (double) (y - cy) * (*depth) / fy;
raw3D[2] = (double) *depth;
raw3D[3] = (double) 1.0;
transform3DPointUsing4x4Matrix(world3D,m,raw3D);
if (
(segConf->bboxX1<world3D[0])&& (segConf->bboxX2>world3D[0]) &&
(segConf->bboxY1<world3D[1])&& (segConf->bboxY2>world3D[1]) &&
(segConf->bboxZ1<world3D[2])&& (segConf->bboxZ2>world3D[2])
)
{ } else // If it was selected keep it selected
{ *selectedPtr=0; } //Denied
}//If it was selected and not null project it into 3d Space
++selectedPtr;
++x;
if (x>=width) { x=0; ++y;}
}
sourcePixelsLineEnd+=sourceWidthStep;
}
free4x4Matrix(&m); // This is the same as free(m); m=0;
} //End of M allocated!
}
// -------------------------------------------------------------------------------------------------
// --------------------------------- BOUNDING BOX SEGMENTATION -------------------------------------
// -------------------------------------------------------------------------------------------------
free(sourceCopy);
return selectedDepth;
}
int simpleRendererRender(
struct simpleRenderer * sr ,
float * position3D,
float * center3D,
float * objectRotation,
unsigned int rotationOrder,
///---------------
float * output3DX,
float * output3DY,
float * output3DZ,
///---------------
float * output2DX,
float * output2DY,
float * output2DW
)
{
double modelTransformationD[16];
float modelTransformationF[16];
///--------------------------------------------------------------------
/// CAMERA MATRICES ETC
///--------------------------------------------------------------------
create4x4ModelTransformation(
modelTransformationD,
//Rotation Component
(double) sr->cameraOffsetRotation[0],//heading,
(double) sr->cameraOffsetRotation[1],//pitch,
(double) sr->cameraOffsetRotation[2],//roll,
ROTATION_ORDER_RPY,
//Translation Component
(double) sr->cameraOffsetPosition[0],
(double) sr->cameraOffsetPosition[1],
(double) sr->cameraOffsetPosition[2],
//Scale Component
(double) 1.0,
(double) 1.0,
(double) 1.0
);
///--------------------------------------------------------------------
copy4x4DMatrixToF(modelTransformationF,modelTransformationD);
multiplyTwo4x4FMatrices(sr->modelViewMatrix,sr->viewMatrix,modelTransformationF);
///--------------------------------------------------------------------
///--------------------------------------------------------------------
/// OBJECT MATRICES ETC
///--------------------------------------------------------------------
double objectMatrixRotation[16];
if (objectRotation==0)
{
create4x4IdentityMatrix(objectMatrixRotation);
} else
if ( (objectRotation[0]==0) && (objectRotation[1]==0) && (objectRotation[2]==0) )
{
create4x4IdentityMatrix(objectMatrixRotation);
} else
if (rotationOrder==ROTATION_ORDER_RPY)
{
//This is the old way to do this rotation
doRPYTransformation(
objectMatrixRotation,
(double) objectRotation[0],
(double) objectRotation[1],
(double) objectRotation[2]
);
} else
{
//fprintf(stderr,"Using new model transform code\n");
create4x4MatrixFromEulerAnglesWithRotationOrder(
objectMatrixRotation ,
(double) objectRotation[0],
(double) objectRotation[1],
(double) objectRotation[2],
rotationOrder
);
}
double point3D[4];
double resultPoint3D[4];
point3D[0]=(double) (position3D[0]-center3D[0]);
point3D[1]=(double) (position3D[1]-center3D[1]);
point3D[2]=(double) (position3D[2]-center3D[2]);
point3D[3]=(double) (1.0);
transform3DPointVectorUsing4x4Matrix(
resultPoint3D,
objectMatrixRotation,
point3D
);
float final3DPosition[4];
if (sr->removeObjectPosition)
{
final3DPosition[0]=(float) resultPoint3D[0]+sr->cameraOffsetPosition[0];
final3DPosition[1]=(float) resultPoint3D[1]+sr->cameraOffsetPosition[1];
final3DPosition[2]=(float) resultPoint3D[2]+sr->cameraOffsetPosition[2];
} else
{
final3DPosition[0]=(float) resultPoint3D[0]+center3D[0]+sr->cameraOffsetPosition[0];
final3DPosition[1]=(float) resultPoint3D[1]+center3D[1]+sr->cameraOffsetPosition[1];
final3DPosition[2]=(float) resultPoint3D[2]+center3D[2]+sr->cameraOffsetPosition[2];
}
final3DPosition[3]=(float) 0.0;//resultPoint3D[3];
///--------------------------------------------------------------------
///--------------------------------------------------------------------
/// FINAL PROJECTION
///--------------------------------------------------------------------
float windowCoordinates[3]={0};
if (
!_glhProjectf(
final3DPosition,
sr->modelViewMatrix,
sr->projectionMatrix,
sr->viewport,
windowCoordinates
)
)
{ fprintf(stderr,"Could not project 3D Point (%0.2f,%0.2f,%0.2f)\n",final3DPosition[0],final3DPosition[1],final3DPosition[2]); }
///--------------------------------------------------------------------
*output2DX = windowCoordinates[0];//windowCoordinates[2];
*output2DY = windowCoordinates[1];//windowCoordinates[2];
*output2DW = windowCoordinates[2];
return 1;
}
