int convertRodriguezAndTranslationTo4x4DUnprojectionMatrix(double * result4x4, double * rodriguez , double * translation , double scaleToDepthUnit)
{
double * matrix3x3Rotation = alloc4x4Matrix(); if (matrix3x3Rotation==0) { return 0; }
//Our translation vector is ready to be used!
#if PRINT_MATRIX_DEBUGGING
fprintf(stderr,"translation %f %f %f\n ",translation[0],translation[1],translation[2]);
#endif // PRINT_MATRIX_DEBUGGING
//Our rodriguez vector should be first converted to a 3x3 Rotation matrix
convertRodriguezTo3x3((double*) matrix3x3Rotation , rodriguez);
//Shorthand variables for readable code :P
double * m = result4x4;
double * rm = matrix3x3Rotation;
double * tm = translation;
//double scaleToDepthUnit = 1000.0; //Convert Unit to milimeters
double Tx = tm[0]*scaleToDepthUnit;
double Ty = tm[1]*scaleToDepthUnit;
double Tz = tm[2]*scaleToDepthUnit;
/*
Here what we want to do is generate a 4x4 matrix that does the inverse transformation that our
rodriguez and translation vector define
In order to do that we should have the following be true
(note the minus under)
( R | T ) ( R trans | - R trans * T ) ( I | 0 )
( --------- ) . ( -------------------------- ) = ( ----------- )
( 0 | 1 ) ( 0 | 1 ) ( 0 | I )
Using matlab to do the calculations we get the following matrix
*/
m[0]= rm[0]; m[1]= rm[3]; m[2]= rm[6]; m[3]= -1.0 * ( rm[0]*Tx + rm[3]*Ty + rm[6]*Tz );
m[4]= rm[1]; m[5]= rm[4]; m[6]= rm[7]; m[7]= -1.0 * ( rm[1]*Tx + rm[4]*Ty + rm[7]*Tz );
m[8]= rm[2]; m[9]= rm[5]; m[10]= rm[8]; m[11]=-1.0 * ( rm[2]*Tx + rm[5]*Ty + rm[8]*Tz );
m[12]= 0.0; m[13]= 0.0; m[14]=0.0; m[15]=1.0;
print4x4DMatrix("ModelView", result4x4,0);
free4x4Matrix(&matrix3x3Rotation);
return 1;
}
int convertRodriguezAndTranslationToOpenGL4x4DProjectionMatrix(double * result4x4, double * rodriguez , double * translation , double scaleToDepthUnit )
{
double * matrix3x3Rotation = alloc4x4Matrix(); if (matrix3x3Rotation==0) { return 0; }
//Our translation vector is ready to be used!
#if PRINT_MATRIX_DEBUGGING
fprintf(stderr,"translation %f %f %f\n ",translation[0],translation[1],translation[2]);
#endif // PRINT_MATRIX_DEBUGGING
//Our rodriguez vector should be first converted to a 3x3 Rotation matrix
convertRodriguezTo3x3((double*) matrix3x3Rotation , rodriguez);
//Shorthand variables for readable code :P
double * m = result4x4;
double * rm = matrix3x3Rotation;
double * tm = translation;
//double scaleToDepthUnit = 1000.0; //Convert Unit to milimeters
double Tx = tm[0]*scaleToDepthUnit;
double Ty = tm[1]*scaleToDepthUnit;
double Tz = tm[2]*scaleToDepthUnit;
/*
Here what we want to do is generate a 4x4 matrix that does the normal transformation that our
rodriguez and translation vector define
*/
m[0]= rm[0]; m[1]= rm[1]; m[2]= rm[2]; m[3]= -Tx;
m[4]= rm[3]; m[5]= rm[4]; m[6]= rm[5]; m[7]= -Ty;
m[8]= rm[6]; m[9]= rm[7]; m[10]= rm[8]; m[11]=-Tz;
m[12]= 0.0; m[13]= 0.0; m[14]=0.0; m[15]=1.0;
#if PRINT_MATRIX_DEBUGGING
print4x4DMatrix("ModelView", result4x4);
fprintf(stderr,"Matrix will be transposed to become OpenGL format ( i.e. column major )\n");
#endif // PRINT_MATRIX_DEBUGGING
transpose4x4MatrixD(result4x4);
free4x4Matrix(&matrix3x3Rotation);
return 1;
}
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;
}
