void ofxKinectCalibration::calculateLookups() {
if(!lookupsCalculated) {
ofLog(OF_LOG_VERBOSE, "Setting up LUT for distance and depth values.");
for(int i = 0; i < 2048; i++){
if(i == 2047) {
distancePixelsLookup[i] = 0;
depthPixelsLookupNearWhite[i] = 0;
depthPixelsLookupFarWhite[i] = 0;
} else {
distancePixelsLookup[i] = rawToCentimeters(i);
depthPixelsLookupFarWhite[i] = ofMap(distancePixelsLookup[i], nearClipping, farClipping, 0, 255, true);
depthPixelsLookupNearWhite[i] = 255 - depthPixelsLookupFarWhite[i];
}
}
}
lookupsCalculated = true;
}
int main (int argc, char * const argv[]) {
short *depth;
uint32_t timestamp;
while (1) {
// 深度データの取得
freenect_sync_get_depth((void**)&depth, ×tamp, 0, FREENECT_DEPTH_11BIT);
// RAWデータをセンチメートルに変換
int centerDepth = rawToCentimeters(depth[240*320]);
std::cout << "Center Depth:" << centerDepth << "cm" << std::endl;
sleep(1);
}
return 0;
}
void testApp::updatePointCloud() {
pointCloud.clear();
const unsigned int Xres = 640;
const unsigned int Yres = 480;
Point2d fov = kinectCalibration.getUndistortedIntrinsics().getFov();
float fx = tanf(ofDegToRad(fov.x) / 2) * 2;
float fy = tanf(ofDegToRad(fov.y) / 2) * 2;
Point2d principalPoint = kinectCalibration.getUndistortedIntrinsics().getPrincipalPoint();
cv::Size imageSize = kinectCalibration.getUndistortedIntrinsics().getImageSize();
int w = 640;
int h = 480;
// int w = curKinect.getWidth();
// int h = curKinect.getHeight();
// float* pixels = curKinect.getPixels();
Mat pixels = curKinect;
int i = 0;
/*
principalPoint.x += ofMap(mouseX, 0, ofGetWidth(), -4, 4);
principalPoint.y += ofMap(mouseY, 0, ofGetHeight(), -4, 4);
cout << "fudge: " << ofMap(mouseX, 0, ofGetWidth(), -4, 4) << "x" << ofMap(mouseY, 0, ofGetHeight(), -4, 4) << endl;
*/
for(int y = 0; y < h; y++) {
for(int j = 0; j < w; j++) {
float pixel = curKinect.at<float>(y, j);
if(pixel < 1000) { // the rest is basically noise
int x = Xres - j - 1; // x axis is flipped from depth image
float z = rawToCentimeters(pixel);
float xReal = (((float) x - principalPoint.x) / imageSize.width) * z * fx;
float yReal = (((float) y - principalPoint.y) / imageSize.height) * z * fy;
// add each point into pointCloud
pointCloud.push_back(Point3f(xReal, yReal, z));
}
i++;
}
}
}
/* * ofxKinectCalibration.cpp * * Created on: 03/01/2011 * Author: arturo */ #include "ofxKinectCalibration.h" /* these values constrain the maximum distance in the depthPixels image to: - as near as possible (raw value of 0) - 4 meters away, maximum both near and far clipping planes should be user-settable */ float ofxKinectCalibration::nearClipping = rawToCentimeters(0), ofxKinectCalibration::farClipping = 400; bool ofxKinectCalibration::lookupsCalculated = false; float ofxKinectCalibration::distancePixelsLookup[2048]; unsigned char ofxKinectCalibration::depthPixelsLookupNearWhite[2048]; unsigned char ofxKinectCalibration::depthPixelsLookupFarWhite[2048]; double ofxKinectCalibration::fx_d = 1.0 / 5.9421434211923247e+02; double ofxKinectCalibration::fy_d = 1.0 / 5.9104053696870778e+02; float ofxKinectCalibration::cx_d = 3.3930780975300314e+02; float ofxKinectCalibration::cy_d = 2.4273913761751615e+02; double ofxKinectCalibration::fx_rgb = 5.2921508098293293e+02; double ofxKinectCalibration::fy_rgb = 5.2556393630057437e+02; float ofxKinectCalibration::cx_rgb = 3.2894272028759258e+02; float ofxKinectCalibration::cy_rgb = 2.6748068171871557e+02;
