Stereo calibration using the OpenKinect Processing wrapper
in
Contributed Library Questions
•
1 year ago
I've noticed that SimpleOpenNI has a built-in function to map rgb coordinates on depth data, and so does the ofxKinect addon, but haven't found anything in Daniel Shiffman's OpenKinect Processing wrapper.
I've attempted to port the code from ofxKinect, but got lost in matrices.
Here is my attempt so far:
I am not entirely sure how this works, which doesn't help when debugging.
- import org.openkinect.*;
- import org.openkinect.processing.*;
- Kinect kinect;
- int w = 640;
- int h = 480;
- float factor = 300;
- float[] depthLookUp = new float[2048];
- //RGB
- PImage rgb;
- //calibration method from: http://nicolas.burrus.name/index.php/Research/KinectCalibration
- //code 'kindly borrowed' from ofxKinectCalibration.cpp
- double fx_rgb = 5.2921508098293293e+02;
- double fy_rgb = 5.2556393630057437e+02;
- float cx_rgb = 3.2894272028759258e+02;
- float cy_rgb = 2.6748068171871557e+02;
- PMatrix3D R_rgb = new PMatrix3D(9.9984628826577793e-01f, 1.2635359098409581e-03f, -1.7487233004436643e-02f, 0,
- -1.4779096108364480e-03f, 9.9992385683542895e-01f, -1.2251380107679535e-02f, 0,
- 1.7470421412464927e-02f, 1.2275341476520762e-02f, 9.9977202419716948e-01f, 0,
- 0,0,0,1);
- PVector T_rgb = new PVector( 1.9985242312092553e-02f, -7.4423738761617583e-04f, -1.0916736334336222e-02f );
- void setup() {
- size(800,600,P3D);
- kinect = new Kinect(this);
- kinect.start();
- kinect.enableDepth(true);
- kinect.enableRGB(true);
- kinect.processDepthImage(false);
- kinect.tilt(20);
- for (int i = 0; i < depthLookUp.length; i++) depthLookUp[i] = rawDepthToMeters(i);
- //rgb setup
- preMultTranslate(R_rgb,PVector.mult(T_rgb,-1));
- R_rgb.transpose();
- }
- void draw() {
- background(0);
- fill(255);
- text("Kinect FR: " + (int)kinect.getDepthFPS() + "\nProcessing FR: " + (int)frameRate,10,16);
- int[] depth = kinect.getRawDepth();
- rgb = kinect.getVideoImage();
- int skip = 6;
- translate(width/2,height/2,-50);
- rotateY(map(mouseX,0,width,TWO_PI,PI));
- for(int x=0; x<w; x+=skip) {
- for(int y=0; y<h; y+=skip) {
- int offset = x+y*w;
- int rawDepth = depth[offset];
- PVector v = depthToWorld(x,y,rawDepth);
- //stroke(255);
- /*
- ofxVec2f ofxKinectCalibration::getCalibratedColorCoordAt(int x, int y){
- //calibration method from: http://nicolas.burrus.name/index.php/Research/KinectCalibration
- ofxVec3f texcoord3d;
- ofxVec2f texcoord2d;
- texcoord3d = getWorldCoordinateFor(x,y);
- texcoord3d = R_rgb * texcoord3d;
- const float invZ = 1/ texcoord3d.z;
- texcoord2d.x = ofClamp((texcoord3d.x * fx_rgb *invZ) + cx_rgb,0,640);
- texcoord2d.y = ofClamp((texcoord3d.y * fy_rgb *invZ) + cy_rgb,0,480);
- return texcoord2d;
- }
- */
- //PVector tex3d = new PVector();
- //R_rgb.mult(v.get(),tex3d);
- PVector tex3d = preMult(R_rgb,v);
- float invZ = 1/tex3d.z;
- int tx = (int)constrain((float)((tex3d.x * fx_rgb *invZ) + cx_rgb),0,640);
- int ty = (int)constrain((float)((tex3d.y * fy_rgb *invZ) + cy_rgb),0,480);
- int rgbOffset = (640-tx)+ty*w;
- if(mousePressed) stroke(rgb.get(640-x,y));
- else stroke(rgb.pixels[rgbOffset]);
- point(v.x*factor,v.y*factor,factor-v.z*factor);
- }
- }
- }
- // These functions come from: http://graphics.stanford.edu/~mdfisher/Kinect.html
- float rawDepthToMeters(int depthValue) {
- if (depthValue < 2047) {
- return (float)(1.0 / ((double)(depthValue) * -0.0030711016 + 3.3309495161));
- }
- return 0.0f;
- }
- PVector depthToWorld(int x, int y, int depthValue) {
- final double fx_d = 1.0 / 5.9421434211923247e+02;
- final double fy_d = 1.0 / 5.9104053696870778e+02;
- final double cx_d = 3.3930780975300314e+02;
- final double cy_d = 2.4273913761751615e+02;
- PVector result = new PVector();
- double depth = depthLookUp[depthValue];//rawDepthToMeters(depthValue);
- result.x = (float)((x - cx_d) * depth * fx_d);
- result.y = (float)((y - cy_d) * depth * fy_d);
- result.z = (float)(depth);
- return result;
- }
- boolean areVecsEqual(PVector v1,PVector v2){
- float e = .01;
- return ( (abs(v1.x-v2.x) <= e) && (abs(v1.y-v2.y) <= e) && (abs(v1.z-v2.z) <= e) );
- }
- void preMultTranslate(PMatrix3D mat, PVector vec){
- float[] v = vec.array();
- float[] m = new float[16];
- mat.get(m);
- for (int i = 0; i < 3; ++i) {
- float tmp = v[i];
- if(tmp == 0) continue;
- m[12] += tmp * m[i+0*4];
- m[13] += tmp * m[i+1*4];
- m[14] += tmp * m[i+2*4];
- m[15] += tmp * m[i+3*4];
- }
- mat.set(m);
- }
- /*
- inline void ofxMatrix4x4::preMultTranslate( const ofxVec3f& v ) {
- for (unsigned i = 0; i < 3; ++i) {
- float tmp = v.v[i];
- if (tmp == 0)
- continue;
- _mat[3][0] += tmp * _mat[i][0];
- _mat[3][1] += tmp * _mat[i][1];
- _mat[3][2] += tmp * _mat[i][2];
- _mat[3][3] += tmp * _mat[i][3];
- }
- }
- */
- PVector preMult(PMatrix3D mat, PVector v){
- float[] _mat = new float[16];
- mat.get(_mat);
- float d = 1.0f / (_mat[3] * v.x + _mat[7] * v.y + _mat[11] * v.z + _mat[15]) ;
- return new PVector ( (_mat[0]*v.x + _mat[4]*v.y + _mat[8]*v.z + _mat[12])*d,
- (_mat[1]*v.x + _mat[5]*v.y + _mat[9]*v.z + _mat[13])*d,
- (_mat[2]*v.x + _mat[6]*v.y + _mat[10]*v.z + _mat[14])*d);
- }
- /*
- inline ofxVec3f ofxMatrix4x4::preMult( const ofxVec3f& v ) const {
- float d = 1.0f / (_mat[0][3] * v.x + _mat[1][3] * v.y + _mat[2][3] * v.z + _mat[3][3]) ;
- return ofxVec3f( (_mat[0][0]*v.x + _mat[1][0]*v.y + _mat[2][0]*v.z + _mat[3][0])*d,
- (_mat[0][1]*v.x + _mat[1][1]*v.y + _mat[2][1]*v.z + _mat[3][1])*d,
- (_mat[0][2]*v.x + _mat[1][2]*v.y + _mat[2][2]*v.z + _mat[3][2])*d);
- }
- */
- void stop() {
- kinect.quit();
- super.stop();
- }
So far
Can someone please explain how the stereo calibration works and where I'm going wrong ?
Thanks,
George
1
