Wrong definition of the floor plane from a Kinect point cloud. Help me please!
in
Contributed Library Questions
•
4 months ago
Hello,
I am using the simpleOpenni library in order to retrieve the point cloud from a Kinect. Then I take three random real world points and I calculate the parameters of the plane equations on me own using this method from this site:
http://www.had2know.com/academics/equation-plane-through-3-points.html (Actually I use Matlab for the calculations). Afterwards I put every real world point form the point cloud to the equation hoping that I took a result close to zero...The problem is that I obtain very large number instead....Can you help me please?
- import SimpleOpenNI.*;
- SimpleOpenNI context;
- float zoomF =0.3f;
- float rotX = radians(180); // by default rotate the hole scene 180deg around the x-axis,
- // the data from openni comes upside down
- float rotY = radians(0);
- int index;
- PVector [] p=new PVector [304635];
- void setup()
- {
- frameRate(300);
- size(640,480,P3D); // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem
- //context = new SimpleOpenNI(this,SimpleOpenNI.RUN_MODE_SINGLE_THREADED);
- context = new SimpleOpenNI(this);
- // disable mirror
- context.setMirror(false);
- // enable depthMap generation
- if(context.enableDepth() == false)
- {
- println("Can't open the depthMap, maybe the camera is not connected!");
- exit();
- return;
- }
- stroke(255,255,255);
- smooth();
- perspective(radians(45),
- float(width)/float(height),
- 10,150000);
- }
- void draw()
- {
- // update the cam
- context.update();
- background(0,0,0);
- translate(width/2, height/2, 0);
- rotateX(rotX);
- rotateY(rotY);
- scale(zoomF);
- int[] depthMap = context.depthMap();
- int steps =10; // to speed up the drawing, draw every third point
- PVector realWorldPoint;
- int z=0;
- translate(0,0,-1000); // set the rotation center of the scene 1000 infront of the camera
- stroke(255);
- PVector[] realWorldMap = context.depthMapRealWorld();
- for(int y=80;y < context.depthHeight();y+=steps)
- {
- for(int x=0;x < context.depthWidth();x+=steps)
- {
- index = x + y * context.depthWidth();
- if(depthMap[index] > 0)
- {
- // draw the projected point
- // realWorldPoint = context.depthMapRealWorld()[index];
- realWorldPoint = realWorldMap[index];
- stroke(240,240,240);//grey
- point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z); // make realworld z negative, in the 3d drawing coordsystem +z points in the direction of the eye
- /* realWorldPoint = realWorldMap[300 + 450 * context.depthWidth()];
- stroke(95,158,160);
- point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
- println(realWorldPoint.x+","+realWorldPoint.y+","+realWorldPoint.z);
- */
- double plane=( 9.5721e+03*realWorldPoint.z)- ( 3.8451e+04*realWorldPoint.y) - (1.3451e+03*realWorldPoint.x) - 1.6337e+07;
- println(plane);
- if(plane==0){
- stroke(95,158,160); //blue
- point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
- }
- }
- }
- // draw the kinect cam
- // context.drawCamFrustum();
- }
- /*PVector v1=new PVector(0,0,0);
- for (int i=0; i<16; i++){
- v1=p[i];
- println(v1);
- */
- noLoop();
- }
- void keyPressed()
- {
- switch(key)
- {
- case ' ':
- context.setMirror(!context.mirror());
- break;
- }
- switch(keyCode)
- {
- case LEFT:
- rotY += 0.1f;
- break;
- case RIGHT:
- // zoom out
- rotY -= 0.1f;
- break;
- case UP:
- if(keyEvent.isShiftDown())
- zoomF += 0.02f;
- else
- rotX += 0.1f;
- break;
- case DOWN:
- if(keyEvent.isShiftDown())
- {
- zoomF -= 0.02f;
- if(zoomF < 0.01)
- zoomF = 0.01;
- }
- else
- rotX -= 0.1f;
- break;
- }
- }
1