osg::StateSet* createColorToGreyscaleStateSet()
{
osg::StateSet* stateset = new osg::StateSet;
osg::Program* program = new osg::Program;
stateset->setAttribute(program);
const char* fragSource =
{
"uniform sampler2D baseTexture;\n"
"uniform mat4 colorMatrix;\n"
"void main(void)\n"
"{\n"
" vec4 color = texture2D( baseTexture, gl_TexCoord[0].st );\n"
" gl_FragColor = colorMatrix * color;\n"
"}\n"
};
program->addShader(new osg::Shader(osg::Shader::FRAGMENT, fragSource));
stateset->addUniform(new osg::Uniform("baseTexture",0));
osg::Matrixf colorMatrix(
0.3f, 0.3f, 0.3f, 0.0f,
0.59f, 0.59f, 0.59f, 0.0f,
0.11f, 0.11f, 0.11f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
stateset->addUniform(new osg::Uniform("colorMatrix",colorMatrix));
return stateset;
}
// virtual
void Image::OnRender(Graphics* pGraphics)
{
if (m_Source)
{
LDraw::Bitmap* bitmap = m_Source->GetBitmap();
double m_Opacity = get_Opacity();
if (m_Opacity > 0)
{
float opacity = m_Opacity;
if (opacity > 1) opacity = 1;
LDraw::CompositingMode oldCompositingMode = pGraphics->GetCompositingMode();
LDraw::CompositingMode compositingMode;
switch (get_CompositingMode())
{
case SourceOver: compositingMode = LDraw::CompositingModeSourceOver; break;
case SourceCopy: compositingMode = LDraw::CompositingModeSourceCopy; break;
case Xor: compositingMode = LDraw::CompositingModeXor; break;
case Multiply: compositingMode = LDraw::CompositingModeMultiply; break;
case Screen: compositingMode = LDraw::CompositingModeScreen; break;
default:
ASSERT(0);
}
pGraphics->SetCompositingMode(compositingMode);
#if 1
// opacity
LDraw::ColorMatrixF colorMatrix( 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, opacity, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
// Gdiplus::ImageAttributes imageAtt;
// imageAtt.SetColorMatrix(&colorMatrix, Gdiplus::ColorMatrixFlagsDefault, Gdiplus::ColorAdjustTypeBitmap);
#endif
pGraphics->DrawImage(bitmap,
LDraw::RectF(0, 0, float(m_computedSize.Width), float(m_computedSize.Height)),
0, 0, bitmap->GetWidth(), bitmap->GetHeight(), LDraw::WrapModeClamp, &colorMatrix/*,
Gdiplus::UnitPixel,
&imageAtt*/);
pGraphics->SetCompositingMode(oldCompositingMode);
}
}
}
void downloadCalibration(unsigned int index)
{
/* Open a Kinect camera: */
Kinect::Camera camera(usbContext,index);
std::cout<<"Downloading factory calibration data for Kinect "<<camera.getSerialNumber()<<"..."<<std::endl;
/* Retrieve the factory calibration data: */
Kinect::Camera::CalibrationParameters cal;
camera.getCalibrationParameters(cal);
/* Calculate the depth-to-distance conversion formula: */
double numerator=10.0*(4.0*cal.dcmosEmitterDist*cal.referenceDistance)/cal.referencePixelSize;
double denominator=4.0*cal.dcmosEmitterDist/cal.referencePixelSize+4.0*cal.constantShift+1.5;
std::cout<<"Depth conversion formula: dist[mm] = "<<numerator<<" / ("<<denominator<<" - depth)"<<std::endl;
/* Calculate the depth pixel unprojection matrix: */
double scale=2.0*cal.referencePixelSize/cal.referenceDistance;
Math::Matrix depthMatrix(4,4,0.0);
depthMatrix(0,0)=scale;
depthMatrix(0,3)=-scale*640.0/2.0;
depthMatrix(1,1)=scale;
depthMatrix(1,3)=-scale*480.0/2.0;
depthMatrix(2,3)=-1.0;
depthMatrix(3,2)=-1.0/numerator;
depthMatrix(3,3)=denominator/numerator;
{
std::cout<<std::endl<<"Depth unprojection matrix:"<<std::endl;
std::streamsize oldPrec=std::cout.precision(8);
std::cout.setf(std::ios::fixed);
for(int i=0;i<4;++i)
{
std::cout<<" ";
for(int j=0;j<4;++j)
std::cout<<' '<<std::setw(11)<<depthMatrix(i,j);
std::cout<<std::endl;
}
std::cout.unsetf(std::ios::fixed);
std::cout.precision(oldPrec);
}
/* Calculate the depth-to-color mapping: */
double colorShiftA=cal.dcmosRcmosDist/(cal.referencePixelSize*2.0)+0.375;
double colorShiftB=cal.dcmosRcmosDist*cal.referenceDistance*10.0/(cal.referencePixelSize*2.0);
std::cout<<"Depth-to-color pixel shift formula: Shift = "<<colorShiftA<<" - "<<colorShiftB<<"/dist[mm]"<<std::endl;
/* Formula to go directly from depth to displacement: */
double dispA=colorShiftA-colorShiftB*denominator/numerator;
double dispB=colorShiftB/numerator;
std::cout<<"Or: Shift = "<<dispA<<" + "<<dispB<<"*depth"<<std::endl;
/* Tabulate the bivariate pixel mapping polynomial using forward differencing: */
double* colorx=new double[640*480];
double* colory=new double[640*480];
double ax=cal.ax;
double bx=cal.bx;
double cx=cal.cx;
double dx=cal.dx;
double ay=cal.ay;
double by=cal.by;
double cy=cal.cy;
double dy=cal.dy;
double dx0=(cal.dxStart<<13)>>4;
double dy0=(cal.dyStart<<13)>>4;
double dxdx0=(cal.dxdxStart<<11)>>3;
double dxdy0=(cal.dxdyStart<<11)>>3;
double dydx0=(cal.dydxStart<<11)>>3;
double dydy0=(cal.dydyStart<<11)>>3;
double dxdxdx0=(cal.dxdxdxStart<<5)<<3;
double dydxdx0=(cal.dydxdxStart<<5)<<3;
double dxdxdy0=(cal.dxdxdyStart<<5)<<3;
double dydxdy0=(cal.dydxdyStart<<5)<<3;
double dydydx0=(cal.dydydxStart<<5)<<3;
double dydydy0=(cal.dydydyStart<<5)<<3;
for(int y=0;y<480;++y)
{
dxdxdx0+=cx;
dxdx0 +=dydxdx0/256.0;
dydxdx0+=dx;
dx0 +=dydx0/64.0;
dydx0 +=dydydx0/256.0;
dydydx0+=bx;
dxdxdy0+=cy;
dxdy0 +=dydxdy0/256.0;
dydxdy0+=dy;
dy0 +=dydy0/64.0;
dydy0 +=dydydy0/256.0;
dydydy0+=by;
double coldxdxdy=dxdxdy0;
double coldxdy=dxdy0;
double coldy=dy0;
double coldxdxdx=dxdxdx0;
double coldxdx=dxdx0;
double coldx=dx0;
for(int x=0;x<640;++x)
{
colorx[y*640+x]=double(x)+coldx/131072.0+1.0;
colory[y*640+x]=double(y)+coldy/131072.0+1.0;
#if 0
if(x%40==20&&y%40==20)
std::cout<<x<<", "<<y<<": "<<colorx[y*640+x]<<", "<<colory[y*640+x]<<std::endl;
#endif
coldx+=coldxdx/64.0;
coldxdx+=coldxdxdx/256.0;
coldxdxdx+=ax;
coldy+=coldxdy/64.0;
coldxdy+=coldxdxdy/256.0;
coldxdxdy+=ay;
}
}
/* Calculate the texture projection matrix by sampling the pixel mapping polynomial: */
int minDepth=Math::ceil(denominator-numerator/500.0); // 500mm is minimum viewing distance
int maxDepth=Math::ceil(denominator-numerator/3000.0); // 3000mm is maximum reasonable viewing distance
std::cout<<"Calculating best-fit color projection matrix for depth value range "<<minDepth<<" - "<<maxDepth<<"..."<<std::endl;
Math::Matrix colorSystem(12,12,0.0);
Math::Matrix depthPoint(4,1,0.0);
depthPoint(3)=1.0;
for(int y=20;y<480;y+=40)
{
depthPoint(1)=(double(y)+0.5)/480.0;
for(int x=20;x<640;x+=40)
{
depthPoint(0)=(double(x)+0.5)/640.0;
for(int depth=minDepth;depth<=maxDepth;depth+=20)
{
/* Transform the depth point to color image space using the non-linear transformation: */
// int xdisp=x+Math::floor(dispA+dispB*double(depth)+0.5);
// if(xdisp>=0&&xdisp<640)
{
// double colX=double(xdisp)/640.0;
// double colY=double(y)/480.0;
// double colX=colorx[(479-y)*640+xdisp]/640.0;
// double colY=(479.0-colory[(479-y)*640+xdisp])/480.0;
double colX=(colorx[(479-y)*640+x]+dispA+dispB*double(depth))/640.0;
double colY=(479.0-colory[(479-y)*640+x])/480.0;
/* Calculate the 3D point based on the depth unprojection matrix: */
depthPoint(2)=double(depth)/double(maxDepth);
Math::Matrix worldPoint=depthMatrix*depthPoint;
/* Make the world point affine: */
for(int i=0;i<3;++i)
worldPoint(i)/=worldPoint(3);
/* Enter the world point / color point pair into the color projection system: */
double eq[2][12];
eq[0][0]=depthPoint(0);
eq[0][1]=depthPoint(1);
eq[0][2]=depthPoint(2);
eq[0][3]=1.0;
eq[0][4]=0.0;
eq[0][5]=0.0;
eq[0][6]=0.0;
eq[0][7]=0.0;
eq[0][8]=-colX*depthPoint(0);
eq[0][9]=-colX*depthPoint(1);
eq[0][10]=-colX*depthPoint(2);
eq[0][11]=-colX;
eq[1][0]=0.0;
eq[1][1]=0.0;
eq[1][2]=0.0;
eq[1][3]=0.0;
eq[1][4]=depthPoint(0);
eq[1][5]=depthPoint(1);
eq[1][6]=depthPoint(2);
eq[1][7]=1.0;
eq[1][8]=-colY*depthPoint(0);
eq[1][9]=-colY*depthPoint(1);
eq[1][10]=-colY*depthPoint(2);
eq[1][11]=-colY;
for(int row=0;row<2;++row)
for(unsigned int i=0;i<12;++i)
for(unsigned int j=0;j<12;++j)
colorSystem(i,j)+=eq[row][i]*eq[row][j];
}
}
}
}
/* Find the linear system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> qe=colorSystem.jacobiIteration();
unsigned int minEIndex=0;
double minE=Math::abs(qe.second(0,0));
for(unsigned int i=1;i<12;++i)
{
if(minE>Math::abs(qe.second(i,0)))
{
minEIndex=i;
minE=Math::abs(qe.second(i,0));
}
}
std::cout<<"Smallest eigenvalue of color system = "<<minE<<std::endl;
/* Create the normalized homography and extend it to a 4x4 matrix: */
Math::Matrix colorMatrix(4,4);
double cmScale=qe.first(11,minEIndex);
for(int i=0;i<2;++i)
for(int j=0;j<4;++j)
colorMatrix(i,j)=qe.first(i*4+j,minEIndex)/cmScale;
for(int j=0;j<4;++j)
colorMatrix(2,j)=j==2?1.0:0.0;
for(int j=0;j<4;++j)
colorMatrix(3,j)=qe.first(2*4+j,minEIndex)/cmScale;
/* Un-normalize the color matrix: */
for(int i=0;i<4;++i)
{
colorMatrix(i,0)/=640.0;
colorMatrix(i,1)/=480.0;
colorMatrix(i,2)/=double(maxDepth);
}
/* Calculate the approximation error: */
double rms=0.0;
unsigned int numPoints=0;
depthPoint(3)=1.0;
for(int y=20;y<480;y+=40)
{
depthPoint(1)=double(y)+0.5;
for(int x=20;x<640;x+=40)
{
depthPoint(0)=double(x)+0.5;
for(int depth=minDepth;depth<=maxDepth;depth+=20)
{
/* Transform the depth point to color image space using the non-linear transformation: */
// int xdisp=x+Math::floor(dispA+dispB*double(depth)+0.5);
// if(xdisp>=0&&xdisp<640)
{
// double colX=double(xdisp)/640.0;
// double colY=double(y)/480.0;
// double colX=colorx[y*640+xdisp]/640.0;
// double colY=(colory[y*640+xdisp]-50.0)/480.0;
double colX=(colorx[y*640+x]+dispA+dispB*double(depth))/640.0;
double colY=(colory[y*640+x]-0.0)/480.0;
/* Transform the depth image point to color space using the color matrix: */
depthPoint(2)=double(depth);
Math::Matrix colorPoint=colorMatrix*depthPoint;
/* Calculate the approximation error: */
double dist=Math::sqr(colorPoint(0)/colorPoint(3)-colX)+Math::sqr(colorPoint(1)/colorPoint(3)-colY);
rms+=dist;
++numPoints;
}
}
}
}
/* Print the RMS: */
std::cout<<"Color matrix approximation RMS = "<<Math::sqrt(rms/double(numPoints))<<std::endl;
/* Make the color matrix compatible with Kinect::Projector's way of doing things: */
// colorMatrix*=depthMatrix;
{
std::cout<<"Optimal homography from world space to color image space:"<<std::endl;
std::streamsize oldPrec=std::cout.precision(8);
std::cout.setf(std::ios::fixed);
for(int i=0;i<4;++i)
{
std::cout<<" ";
for(int j=0;j<4;++j)
std::cout<<' '<<std::setw(11)<<colorMatrix(i,j);
std::cout<<std::endl;
}
std::cout.unsetf(std::ios::fixed);
std::cout.precision(oldPrec);
}
delete[] colorx;
delete[] colory;
/* Convert the depth matrix from mm to cm: */
Math::Matrix scaleMatrix(4,4,1.0);
for(int i=0;i<3;++i)
scaleMatrix(i,i)=0.1;
depthMatrix=scaleMatrix*depthMatrix;
/* Write the depth and color matrices to an intrinsic parameter file: */
std::string calibFileName=KINECT_CONFIG_DIR;
calibFileName.push_back('/');
calibFileName.append(KINECT_CAMERA_INTRINSICPARAMETERSFILENAMEPREFIX);
calibFileName.push_back('-');
calibFileName.append(camera.getSerialNumber());
calibFileName.append(".dat");
if(!Misc::doesPathExist(calibFileName.c_str()))
{
std::cout<<"Writing full intrinsic camera parameters to "<<calibFileName<<std::endl;
IO::FilePtr calibFile=IO::openFile(calibFileName.c_str(),IO::File::WriteOnly);
calibFile->setEndianness(Misc::LittleEndian);
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
calibFile->write<double>(depthMatrix(i,j));
for(int i=0;i<4;++i)
for(int j=0;j<4;++j)
calibFile->write<double>(colorMatrix(i,j));
}
else
std::cout<<"Intrinsic camera parameter file "<<calibFileName<<" already exists"<<std::endl;
}
// Playlist
void *display_loop(void * unused) {
while (1) {
plasmaWave();
plasmaSea();
plasmaBall();
testBlur();
uart_putstr("plasmaTest()\n");
plasmaTest();
uart_putstr("spirale()\n");
spirale();
playPlaylist("anims/playlist.apl");
playAnimFile("anim1.prn");
uart_putstr("colorSnakle()\n");
colorSnake();
uart_putstr("colorMatrix()\n");
colorMatrix();
//uart_putstr("flashLight()\n");
//flashLight();
uart_putstr("plasmaSnake()\n");
plasmaSnake();
//uart_putstr("funkyBeat()\n");
//funkyBeat();
//uart_putstr("symetricRoutes()\n");
//symetricRoutes();
uart_putstr("plasmaSnake()\n");
plasmaSnake();
//uart_putstr("shiftTest()\n");
//shiftTest();
uart_putstr("fadeTest()\n");
fadeTest();
//uart_putstr("symetricRoutes()\n");
//symetricRoutes();
//uart_putstr("cubes()\n");
//cubes();
//uart_putstr("brightnesTest()\n");
//brightnesTest();
uart_putstr("movingArrows()\n");
movingArrows();
uart_putstr("plasmaSnake()\n");
plasmaSnake();
uart_putstr("upgoingRandom()\n");
upgoingRandom();
uart_putstr("planeBall()\n");
planeBall();
uart_putstr("wobbeln()\n");
wobbeln();
uart_putstr("plasmaSnake()\n");
plasmaSnake();
//uart_putstr("snake()\n");
//snake();
uart_putstr("movingCubes()\n");
movingCubes();
uart_putstr("plasmaSnake()\n");
plasmaSnake();
uart_putstr("symetricRandom()\n");
symetricRandom();
uart_putstr("testAnim()\n");
testAnim();
//uart_putstr("fnordLicht()\n");
//fnordLicht();
}
return 0;
}
