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; }