HRESULT KinectSensor::processColor()
{
if (! (WAIT_OBJECT_0 == WaitForSingleObject(m_hNextColorFrameEvent, 0)) ) return S_FALSE;
NUI_IMAGE_FRAME imageFrame;
HRESULT hr = S_OK;
hr = m_pNuiSensor->NuiImageStreamGetNextFrame(m_pColorStreamHandle, 0, &imageFrame);
if ( FAILED(hr) ) { return hr; }
NUI_LOCKED_RECT LockedRect;
hr = imageFrame.pFrameTexture->LockRect(0, &LockedRect, NULL, 0);
if ( FAILED(hr) ) { return hr; }
// loop over each row and column of the color
#pragma omp parallel for
for (int yi = 0; yi < (int)getColorHeight(); ++yi) {
LONG y = yi;
LONG* pDest = ((LONG*)m_colorRGBX) + (int)getColorWidth() * y;
for (LONG x = 0; x < (int)getColorWidth(); ++x) {
// calculate index into depth array
//int depthIndex = x/m_colorToDepthDivisor + y/m_colorToDepthDivisor * getDepthWidth(); //TODO x flip
int depthIndex = (getDepthWidth() - 1 - x/m_colorToDepthDivisor) + y/m_colorToDepthDivisor * getDepthWidth();
// retrieve the depth to color mapping for the current depth pixel
LONG colorInDepthX = m_colorCoordinates[depthIndex * 2];
LONG colorInDepthY = m_colorCoordinates[depthIndex * 2 + 1];
// make sure the depth pixel maps to a valid point in color space
if ( colorInDepthX >= 0 && colorInDepthX < (int)getColorWidth() && colorInDepthY >= 0 && colorInDepthY < (int)getColorHeight() ) {
// calculate index into color array
LONG colorIndex = colorInDepthY * (int)getColorWidth() + colorInDepthX; //TODO x flip
//LONG colorIndex = colorInDepthY * (int)getColorWidth() + (getColorWidth() - 1 - colorInDepthX);
// set source for copy to the color pixel
LONG* pSrc = ((LONG *)LockedRect.pBits) + colorIndex;
LONG tmp = *pSrc;
tmp|=0xFF000000; // Flag for is valid
*pDest = tmp;
} else {
*pDest = 0x00000000;
}
pDest++;
}
}
hr = imageFrame.pFrameTexture->UnlockRect(0);
if ( FAILED(hr) ) { return hr; };
hr = m_pNuiSensor->NuiImageStreamReleaseFrame(m_pColorStreamHandle, &imageFrame);
return hr;
}
HRESULT BinaryDumpReader::createFirstConnected()
{
std::string filename = GlobalAppState::getInstance().s_BinaryDumpReaderSourceFile;
std::cout << "Start loading binary dump" << std::endl;
//BinaryDataStreamZLibFile inputStream(filename, false);
BinaryDataStreamFile inputStream(filename, false);
CalibratedSensorData sensorData;
inputStream >> sensorData;
std::cout << "Loading finished" << std::endl;
std::cout << sensorData << std::endl;
DepthSensor::init(sensorData.m_DepthImageWidth, sensorData.m_DepthImageHeight, std::max(sensorData.m_ColorImageWidth,1u), std::max(sensorData.m_ColorImageHeight,1u));
mat4f intrinsics(sensorData.m_CalibrationDepth.m_Intrinsic);
initializeIntrinsics(sensorData.m_CalibrationDepth.m_Intrinsic(0,0), sensorData.m_CalibrationDepth.m_Intrinsic(1,1), sensorData.m_CalibrationDepth.m_Intrinsic(0,2), sensorData.m_CalibrationDepth.m_Intrinsic(1,2));
m_NumFrames = sensorData.m_DepthNumFrames;
assert(sensorData.m_ColorNumFrames == sensorData.m_DepthNumFrames || sensorData.m_ColorNumFrames == 0);
releaseData();
m_DepthD16Array = new USHORT*[m_NumFrames];
for (unsigned int i = 0; i < m_NumFrames; i++) {
m_DepthD16Array[i] = new USHORT[getDepthWidth()*getDepthHeight()];
for (unsigned int k = 0; k < getDepthWidth()*getDepthHeight(); k++) {
m_DepthD16Array[i][k] = (USHORT)(sensorData.m_DepthImages[i][k]*1000.0f + 0.5f);
}
}
std::cout << "loading depth done" << std::endl;
if (sensorData.m_ColorImages.size() > 0) {
m_bHasColorData = true;
m_ColorRGBXArray = new BYTE*[m_NumFrames];
for (unsigned int i = 0; i < m_NumFrames; i++) {
m_ColorRGBXArray[i] = new BYTE[getColorWidth()*getColorHeight()*getColorBytesPerPixel()];
for (unsigned int k = 0; k < getColorWidth()*getColorHeight(); k++) {
const BYTE* c = (BYTE*)&(sensorData.m_ColorImages[i][k]);
m_ColorRGBXArray[i][k*getColorBytesPerPixel()+0] = c[0];
m_ColorRGBXArray[i][k*getColorBytesPerPixel()+1] = c[1];
m_ColorRGBXArray[i][k*getColorBytesPerPixel()+2] = c[2];
m_ColorRGBXArray[i][k*getColorBytesPerPixel()+3] = 255;
//I don't know really why this has to be swapped...
}
//std::string outFile = "colorout//color" + std::to_string(i) + ".png";
//ColorImageR8G8B8A8 image(getColorHeight(), getColorWidth(), (vec4uc*)m_ColorRGBXArray[i]);
//FreeImageWrapper::saveImage(outFile, image);
}
} else {
m_bHasColorData = false;
}
sensorData.deleteData();
std::cout << "loading color done" << std::endl;
return S_OK;
}
HRESULT BinaryDumpReader::processDepth()
{
if(m_CurrFrame >= m_NumFrames)
{
GlobalAppState::get().s_playData = false;
std::cout << "binary dump sequence complete - press space to run again" << std::endl;
m_CurrFrame = 0;
}
if(GlobalAppState::get().s_playData) {
float* depth = getDepthFloat();
memcpy(depth, m_data.m_DepthImages[m_CurrFrame], sizeof(float)*getDepthWidth()*getDepthHeight());
incrementRingbufIdx();
if (m_bHasColorData) {
memcpy(m_colorRGBX, m_data.m_ColorImages[m_CurrFrame], sizeof(vec4uc)*getColorWidth()*getColorHeight());
}
m_CurrFrame++;
return S_OK;
} else {
return S_FALSE;
}
}
void RGBDSensor::computePointCurrentPointCloud(PointCloudf& pc, const mat4f& transform /*= mat4f::identity()*/) const
{
if (!(getColorWidth() == getDepthWidth() && getColorHeight() == getDepthHeight())) throw MLIB_EXCEPTION("invalid dimensions");
for (unsigned int i = 0; i < getDepthWidth()*getDepthHeight(); i++) {
unsigned int x = i % getDepthWidth();
unsigned int y = i / getDepthWidth();
vec3f p = depthToSkeleton(x,y);
if (p.x != -std::numeric_limits<float>::infinity() && p.x != 0.0f) {
vec3f n = getNormal(x,y);
if (n.x != -FLT_MAX) {
pc.m_points.push_back(p);
pc.m_normals.push_back(n);
vec4uc c = m_colorRGBX[i];
pc.m_colors.push_back(vec4f(c.z/255.0f, c.y/255.0f, c.x/255.0f, 1.0f)); //there's a swap... dunno why really
}
}
}
for (auto& p : pc.m_points) {
p = transform * p;
}
mat4f invTranspose = transform.getInverse().getTranspose();
for (auto& n : pc.m_normals) {
n = invTranspose * n;
n.normalize();
}
}
void RGBDSensor::recordFrame()
{
m_recordedDepthData.push_back(m_depthFloat[m_currentRingBufIdx]);
m_recordedColorData.push_back(m_colorRGBX);
m_depthFloat[m_currentRingBufIdx] = new float[getDepthWidth()*getDepthHeight()];
m_colorRGBX = new vec4uc[getColorWidth()*getColorHeight()];
}
HRESULT BinaryDumpReader::processDepth()
{
if (m_CurrFrame < m_NumFrames) {
std::cout << "curr Frame " << m_CurrFrame << std::endl;
memcpy(m_depthD16, m_DepthD16Array[m_CurrFrame], sizeof(USHORT)*getDepthWidth()*getDepthHeight());
if (m_bHasColorData) {
memcpy(m_colorRGBX, m_ColorRGBXArray[m_CurrFrame], getColorBytesPerPixel()*getColorWidth()*getColorHeight());
}
m_CurrFrame++;
return S_OK;
} else {
return S_FALSE;
}
}
bool StructureSensor::processDepth()
{
std::pair<float*,UCHAR*> frames = m_server.process(m_oldDepth, m_oldColor);
if (frames.first == NULL || frames.second == NULL) return false;
// depth
memcpy(m_depthFloat[m_currentRingBufIdx], frames.first, sizeof(float)*getDepthWidth()*getDepthHeight());
// color
memcpy(m_colorRGBX, (vec4uc*)frames.second, sizeof(vec4uc)*getColorWidth()*getColorHeight());
m_oldDepth = frames.first;
m_oldColor = frames.second;
return true;
}
bool SensorDataReader::processDepth()
{
if (m_currFrame >= m_numFrames)
{
GlobalAppState::get().s_playData = false;
//std::cout << "binary dump sequence complete - press space to run again" << std::endl;
stopReceivingFrames();
std::cout << "binary dump sequence complete - stopped receiving frames" << std::endl;
m_currFrame = 0;
}
if (GlobalAppState::get().s_playData) {
float* depth = getDepthFloat();
//TODO check why the frame cache is not used?
ml::SensorData::RGBDFrameCacheRead::FrameState frameState = m_sensorDataCache->getNext();
//ml::SensorData::RGBDFrameCacheRead::FrameState frameState;
//frameState.m_colorFrame = m_sensorData->decompressColorAlloc(m_currFrame);
//frameState.m_depthFrame = m_sensorData->decompressDepthAlloc(m_currFrame);
for (unsigned int i = 0; i < getDepthWidth()*getDepthHeight(); i++) {
if (frameState.m_depthFrame[i] == 0) depth[i] = -std::numeric_limits<float>::infinity();
else depth[i] = (float)frameState.m_depthFrame[i] / m_sensorData->m_depthShift;
}
incrementRingbufIdx();
if (m_bHasColorData) {
for (unsigned int i = 0; i < getColorWidth()*getColorHeight(); i++) {
m_colorRGBX[i] = vec4uc(frameState.m_colorFrame[i]);
}
}
frameState.free();
m_currFrame++;
return true;
}
else {
return false;
}
}
void RGBDSensor::saveRecordedFramesToFile( const std::string& filename )
{
if (m_recordedDepthData.size() == 0 || m_recordedColorData.size() == 0) return;
CalibratedSensorData cs;
cs.m_DepthImageWidth = getDepthWidth();
cs.m_DepthImageHeight = getDepthHeight();
cs.m_ColorImageWidth = getColorWidth();
cs.m_ColorImageHeight = getColorHeight();
cs.m_DepthNumFrames = (unsigned int)m_recordedDepthData.size();
cs.m_ColorNumFrames = (unsigned int)m_recordedColorData.size();
cs.m_CalibrationDepth.m_Intrinsic = getDepthIntrinsics();
cs.m_CalibrationDepth.m_Extrinsic = getDepthExtrinsics();
cs.m_CalibrationDepth.m_IntrinsicInverse = cs.m_CalibrationDepth.m_Intrinsic.getInverse();
cs.m_CalibrationDepth.m_ExtrinsicInverse = cs.m_CalibrationDepth.m_Extrinsic.getInverse();
cs.m_CalibrationColor.m_Intrinsic = getColorIntrinsics();
cs.m_CalibrationColor.m_Extrinsic = getColorExtrinsics();
cs.m_CalibrationColor.m_IntrinsicInverse = cs.m_CalibrationColor.m_Intrinsic.getInverse();
cs.m_CalibrationColor.m_ExtrinsicInverse = cs.m_CalibrationColor.m_Extrinsic.getInverse();
cs.m_DepthImages.resize(cs.m_DepthNumFrames);
cs.m_ColorImages.resize(cs.m_ColorNumFrames);
unsigned int dFrame = 0;
for (auto& a : m_recordedDepthData) {
cs.m_DepthImages[dFrame] = a;
dFrame++;
}
unsigned int cFrame = 0;
for (auto& a : m_recordedColorData) {
cs.m_ColorImages[cFrame] = a;
cFrame++;
}
cs.m_trajectory = m_recordedTrajectory;
std::cout << cs << std::endl;
std::cout << "dumping recorded frames... ";
std::string actualFilename = filename;
while (util::fileExists(actualFilename)) {
std::string path = util::directoryFromPath(actualFilename);
std::string curr = util::fileNameFromPath(actualFilename);
std::string ext = util::getFileExtension(curr);
curr = util::removeExtensions(curr);
std::string base = util::getBaseBeforeNumericSuffix(curr);
unsigned int num = util::getNumericSuffix(curr);
if (num == (unsigned int)-1) {
num = 0;
}
actualFilename = path + base + std::to_string(num+1) + "." + ext;
}
BinaryDataStreamFile outStream(actualFilename, true);
//BinaryDataStreamZLibFile outStream(filename, true);
outStream << cs;
std::cout << "done" << std::endl;
m_recordedDepthData.clear();
m_recordedColorData.clear(); //destructor of cs frees all allocated data
//m_recordedTrajectory.clear();
}