void SensorDataReader::createFirstConnected() { releaseData(); std::string filename = GlobalAppState::get().s_binaryDumpSensorFile; std::cout << "Start loading binary dump... "; m_sensorData = new SensorData; m_sensorData->loadFromFile(filename); std::cout << "DONE!" << std::endl; std::cout << *m_sensorData << std::endl; //std::cout << "depth intrinsics:" << std::endl; //std::cout << m_sensorData->m_calibrationDepth.m_intrinsic << std::endl; //std::cout << "color intrinsics:" << std::endl; //std::cout << m_sensorData->m_calibrationColor.m_intrinsic << std::endl; RGBDSensor::init(m_sensorData->m_depthWidth, m_sensorData->m_depthHeight, std::max(m_sensorData->m_colorWidth, 1u), std::max(m_sensorData->m_colorHeight, 1u), 1); initializeDepthIntrinsics(m_sensorData->m_calibrationDepth.m_intrinsic(0, 0), m_sensorData->m_calibrationDepth.m_intrinsic(1, 1), m_sensorData->m_calibrationDepth.m_intrinsic(0, 2), m_sensorData->m_calibrationDepth.m_intrinsic(1, 2)); initializeColorIntrinsics(m_sensorData->m_calibrationColor.m_intrinsic(0, 0), m_sensorData->m_calibrationColor.m_intrinsic(1, 1), m_sensorData->m_calibrationColor.m_intrinsic(0, 2), m_sensorData->m_calibrationColor.m_intrinsic(1, 2)); initializeDepthExtrinsics(m_sensorData->m_calibrationDepth.m_extrinsic); initializeColorExtrinsics(m_sensorData->m_calibrationColor.m_extrinsic); m_numFrames = (unsigned int)m_sensorData->m_frames.size(); if (m_numFrames > GlobalBundlingState::get().s_maxNumImages * GlobalBundlingState::get().s_submapSize) { throw MLIB_EXCEPTION("sens file #frames = " + std::to_string(m_numFrames) + ", please change param file to accommodate"); //std::cout << "WARNING: sens file #frames = " << m_numFrames << ", please change param file to accommodate" << std::endl; //std::cout << "(press key to continue)" << std::endl; //getchar(); } if (m_numFrames > 0 && m_sensorData->m_frames[0].getColorCompressed()) { m_bHasColorData = true; } else { m_bHasColorData = false; } const unsigned int cacheSize = 10; m_sensorDataCache = new ml::SensorData::RGBDFrameCacheRead(m_sensorData, cacheSize); }
KinectSensor::KinectSensor() { // get resolution as DWORDS, but store as LONGs to avoid casts later DWORD width = 0; DWORD height = 0; NuiImageResolutionToSize(cDepthResolution, width, height); unsigned int depthWidth = static_cast<unsigned int>(width); unsigned int depthHeight = static_cast<unsigned int>(height); NuiImageResolutionToSize(cColorResolution, width, height); unsigned int colorWidth = static_cast<unsigned int>(width); unsigned int colorHeight = static_cast<unsigned int>(height); RGBDSensor::init(depthWidth, depthHeight, colorWidth, colorHeight); m_colorToDepthDivisor = colorWidth/depthWidth; m_hNextDepthFrameEvent = INVALID_HANDLE_VALUE; m_pDepthStreamHandle = INVALID_HANDLE_VALUE; m_hNextColorFrameEvent = INVALID_HANDLE_VALUE; m_pColorStreamHandle = INVALID_HANDLE_VALUE; m_colorCoordinates = new LONG[depthWidth*depthHeight*2]; m_bDepthImageIsUpdated = false; m_bDepthImageCameraIsUpdated = false; m_bNormalImageCameraIsUpdated = false; initializeDepthIntrinsics(2.0f*NUI_CAMERA_SKELETON_TO_DEPTH_IMAGE_MULTIPLIER_320x240, 2.0f*NUI_CAMERA_SKELETON_TO_DEPTH_IMAGE_MULTIPLIER_320x240, 320.0f, 240.0f); initializeDepthExtrinsics(mat4f::identity()); //MLIB_WARNING("TODO initialize color intrs/extr"); initializeColorIntrinsics(2.0f*NUI_CAMERA_SKELETON_TO_DEPTH_IMAGE_MULTIPLIER_320x240, 2.0f*NUI_CAMERA_SKELETON_TO_DEPTH_IMAGE_MULTIPLIER_320x240, 320.0f, 240.0f); initializeColorExtrinsics(mat4f::identity()); }
HRESULT BinaryDumpReader::createFirstConnected() { releaseData(); if (GlobalAppState::get().s_binaryDumpSensorFile.size() == 0) throw MLIB_EXCEPTION("need to specific s_binaryDumpSensorFile[0]"); std::string filename = GlobalAppState::get().s_binaryDumpSensorFile[0]; std::cout << "Start loading binary dump" << std::endl; //BinaryDataStreamZLibFile inputStream(filename, false); BinaryDataStreamFile inputStream(filename, false); inputStream >> m_data; std::cout << "Loading finished" << std::endl; std::cout << m_data << std::endl; std::cout << "intrinsics:" << std::endl; std::cout << m_data.m_CalibrationDepth.m_Intrinsic << std::endl; RGBDSensor::init(m_data.m_DepthImageWidth, m_data.m_DepthImageHeight, std::max(m_data.m_ColorImageWidth,1u), std::max(m_data.m_ColorImageHeight,1u), 1); initializeDepthIntrinsics(m_data.m_CalibrationDepth.m_Intrinsic(0,0), m_data.m_CalibrationDepth.m_Intrinsic(1,1), m_data.m_CalibrationDepth.m_Intrinsic(0,2), m_data.m_CalibrationDepth.m_Intrinsic(1,2)); initializeColorIntrinsics(m_data.m_CalibrationColor.m_Intrinsic(0,0), m_data.m_CalibrationColor.m_Intrinsic(1,1), m_data.m_CalibrationColor.m_Intrinsic(0,2), m_data.m_CalibrationColor.m_Intrinsic(1,2)); initializeDepthExtrinsics(m_data.m_CalibrationDepth.m_Extrinsic); initializeColorExtrinsics(m_data.m_CalibrationColor.m_Extrinsic); m_NumFrames = m_data.m_DepthNumFrames; assert(m_data.m_ColorNumFrames == m_data.m_DepthNumFrames || m_data.m_ColorNumFrames == 0); if (m_data.m_ColorImages.size() > 0) { m_bHasColorData = true; } else { m_bHasColorData = false; } return S_OK; }
HRESULT PrimeSenseSensor::createFirstConnected() { HRESULT hr = S_OK; openni::Status rc = openni::STATUS_OK; const char* deviceURI = openni::ANY_DEVICE; rc = openni::OpenNI::initialize(); std::cout << "After initialization: " << openni::OpenNI::getExtendedError() << std::endl; // Create Device rc = m_device.open(deviceURI); if (rc != openni::STATUS_OK) { std::cout << "Device open failed: " << openni::OpenNI::getExtendedError() << std::endl; openni::OpenNI::shutdown(); return S_FALSE; } openni::PlaybackControl* pc = m_device.getPlaybackControl(); // Create Depth Stream rc = m_depthStream.create(m_device, openni::SENSOR_DEPTH); if (rc == openni::STATUS_OK) { rc = m_depthStream.start(); if (rc != openni::STATUS_OK) { std::cout << "Couldn't start depth stream: " << openni::OpenNI::getExtendedError() << std::endl; m_depthStream.destroy(); } } else { std::cout << "Couldn't find depth stream: " << openni::OpenNI::getExtendedError() << std::endl; } // Create Color Stream rc = m_colorStream.create(m_device, openni::SENSOR_COLOR); if (rc == openni::STATUS_OK) { rc = m_colorStream.start(); if (rc != openni::STATUS_OK) { std::cout << "Couldn't start color stream: " << openni::OpenNI::getExtendedError() << " Return code: " << rc << std::endl; m_colorStream.destroy(); } } else { std::cout << "Couldn't find color stream: " << openni::OpenNI::getExtendedError() << std::endl; } if (GlobalAppState::get().s_enableColorCropping) { if (m_colorStream.isCroppingSupported()) { m_colorStream.setCropping(GlobalAppState::get().s_colorCropX, GlobalAppState::get().s_colorCropY, GlobalAppState::get().s_colorCropWidth, GlobalAppState::get().s_colorCropHeight); } else { std::cout << "cropping is not supported for this color stream" << std::endl; } } // Check Streams if (!m_depthStream.isValid() || !m_colorStream.isValid()) { std::cout << "No valid streams. Exiting" << std::endl; openni::OpenNI::shutdown(); return S_FALSE; } openni::CameraSettings* cs = m_colorStream.getCameraSettings(); //cs->setAutoWhiteBalanceEnabled(false); //cs->setAutoExposureEnabled(false); //cs->setGain(1); //cs->setExposure(10); std::cout << "getGain: " << cs->getGain() << std::endl; std::cout << "getExposure: " << cs->getExposure() << std::endl; std::cout << "getAutoExposureEnabled: " << cs->getAutoExposureEnabled() << std::endl; std::cout << "getAutoWhiteBalanceEnabled: " << cs->getAutoWhiteBalanceEnabled() << std::endl; // Get Dimensions m_depthVideoMode = m_depthStream.getVideoMode(); m_colorVideoMode = m_colorStream.getVideoMode(); int depthWidth = m_depthVideoMode.getResolutionX(); int depthHeight = m_depthVideoMode.getResolutionY(); int colorWidth = m_colorVideoMode.getResolutionX(); int colorHeight = m_colorVideoMode.getResolutionY(); RGBDSensor::init(depthWidth, depthHeight, colorWidth, colorHeight, 1); m_streams = new openni::VideoStream*[2]; m_streams[0] = &m_depthStream; m_streams[1] = &m_colorStream; if (rc != openni::STATUS_OK) { openni::OpenNI::shutdown(); return 3; } m_device.setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR); float focalLengthX = (depthWidth/2.0f)/tan(m_depthStream.getHorizontalFieldOfView()/2.0f); float focalLengthY = (depthHeight/2.0f)/tan(m_depthStream.getVerticalFieldOfView()/2.0f); initializeDepthIntrinsics(focalLengthX, focalLengthY, depthWidth/2.0f, depthHeight/2.0f); focalLengthX = (colorWidth/2.0f)/tan(m_colorStream.getHorizontalFieldOfView()/2.0f); focalLengthY = (colorHeight/2.0f)/tan(m_colorStream.getVerticalFieldOfView()/2.0f); initializeColorIntrinsics(focalLengthX, focalLengthY, colorWidth/2.0f, colorHeight/2.0f); Matrix3f R; R.setIdentity(); Vector3f t; t.setZero(); initializeColorExtrinsics(R, t); R(0, 0) = 9.9991741106823473e-001; R(0, 1) = 3.0752530258331304e-003; R(0, 2) = -1.2478536028949385e-002; R(1, 0) = -3.0607678272497924e-003; R(1, 1) = 9.9999461994140826e-001; R(1, 2) = 1.1797408808971066e-003; R(2, 0) = 1.2482096895408091e-002; R(2, 1) = -1.1414495457493831e-003; R(2, 2) = 9.9992144408949846e-001; //t[0] = -2.5331974929667012e+001; t[1] = 6.1798287248283634e-001; t[2] = 3.8510108109251804e+000; //t[0] /= 1000.0f; t[1] /= 1000.0f; t[2] /= 1000.0f; // //initializeDepthExtrinsics(R, t); return hr; }