mat4f CUDACameraTrackingMultiResRGBD::applyCT(
float4* dInputPos, float4* dInputNormal, float4* dInputColor,
float4* dTargetPos,float4* dTargetNormal, float4* dTargetColor,
const mat4f& lastTransform, const std::vector<unsigned int>& maxInnerIter, const std::vector<unsigned int>& maxOuterIter,
const std::vector<float>& distThres, const std::vector<float>& normalThres,
const std::vector<float>& colorGradiantMin,
const std::vector<float>& colorThres,
float condThres, float angleThres,
const mat4f& deltaTransformEstimate,
const std::vector<float>& weightsDepth,
const std::vector<float>& weightsColor,
const std::vector<float>& earlyOutResidual,
const mat4f& intrinsic,
const DepthCameraData& depthCameraData,
ICPErrorLog* errorLog)
{
// Input
d_input[0] = dInputPos;
d_inputNormal[0] = dInputNormal;
d_model[0] = dTargetPos;
d_modelNormal[0] = dTargetNormal;
//Start Timing
if(GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.start(); }
convertColorToIntensityFloat(d_inputIntensity[0], dInputColor, m_imageWidth[0], m_imageHeight[0]);
convertColorToIntensityFloat(d_modelIntensity[0], dTargetColor, m_imageWidth[0], m_imageHeight[0]);
computeIntensityAndDerivatives(d_modelIntensity[0], m_imageWidth[0], m_imageHeight[0], d_modelIntensityAndDerivatives[0]);
copyFloatMap(d_inputIntensityFiltered[0], d_inputIntensity[0], m_imageWidth[0], m_imageHeight[0]);
for (unsigned int i = 0; i < m_levels-1; i++)
{
float sigmaD = 3.0f; float sigmaR = 1.0f;
resampleFloat4Map(d_input[i+1], m_imageWidth[i+1], m_imageHeight[i+1], d_input[i], m_imageWidth[i], m_imageHeight[i]);
computeNormals(d_inputNormal[i+1], d_input[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
resampleFloatMap(d_inputIntensity[i+1], m_imageWidth[i+1], m_imageHeight[i+1], d_inputIntensity[i], m_imageWidth[i], m_imageHeight[i]);
gaussFilterFloatMap(d_inputIntensityFiltered[i+1], d_inputIntensity[i+1], sigmaD, sigmaR, m_imageWidth[i+1], m_imageHeight[i+1]);
resampleFloat4Map(d_model[i+1], m_imageWidth[i+1], m_imageHeight[i+1], d_model[i], m_imageWidth[i], m_imageHeight[i]);
computeNormals(d_modelNormal[i+1], d_model[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
resampleFloatMap(d_modelIntensity[i+1], m_imageWidth[i+1], m_imageHeight[i+1], d_modelIntensity[i], m_imageWidth[i], m_imageHeight[i]);
gaussFilterFloatMap(d_modelIntensityFiltered[i+1], d_modelIntensity[i+1], sigmaD, sigmaR, m_imageWidth[i+1], m_imageHeight[i+1]);
computeIntensityAndDerivatives(d_modelIntensityFiltered[i+1], m_imageWidth[i+1], m_imageHeight[i+1], d_modelIntensityAndDerivatives[i+1]);
}
//DX11QuadDrawer::RenderQuadDynamic(DXUTGetD3D11Device(), DXUTGetD3D11DeviceContext(), (float*)d_modelIntensityAndDerivatives[0], 4, m_imageWidth[0], m_imageHeight[0], 100.0f);
Eigen::Matrix4f deltaTransform; deltaTransform = MatToEig(deltaTransformEstimate);
for (int level = m_levels-1; level>=0; level--)
{
if (errorLog) {
errorLog->newICPFrame(level);
}
float levelFactor = pow(2.0f, (float)level);
mat4f intrinsicNew = intrinsic;
intrinsicNew(0, 0) /= levelFactor; intrinsicNew(1, 1) /= levelFactor; intrinsicNew(0, 2) /= levelFactor; intrinsicNew(1, 2) /= levelFactor;
CameraTrackingInput input;
input.d_inputPos = d_input[level];
input.d_inputNormal = d_inputNormal[level];
input.d_inputIntensity = d_inputIntensityFiltered[level];
input.d_targetPos = d_model[level];
input.d_targetNormal = d_modelNormal[level];
input.d_targetIntensityAndDerivatives = d_modelIntensityAndDerivatives[level];
CameraTrackingParameters parameters;
parameters.weightColor = weightsColor[level];
parameters.weightDepth = weightsDepth[level];
parameters.distThres = distThres[level];
parameters.normalThres = normalThres[level];
parameters.sensorMaxDepth = GlobalAppState::get().s_sensorDepthMax;
parameters.colorGradiantMin = colorGradiantMin[level];
parameters.colorThres = colorThres[level];
deltaTransform = align(input, deltaTransform, level, parameters, maxInnerIter[level], maxOuterIter[level], condThres, angleThres, earlyOutResidual[level], intrinsicNew, depthCameraData, errorLog);
if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity()) {
return EigToMat(m_matrixTrackingLost);
}
}
//End Timing
if(GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.stop(); TimingLog::totalTimeTracking += m_timer.getElapsedTimeMS(); TimingLog::countTimeTracking++; }
return lastTransform*EigToMat(deltaTransform);
}
HRESULT CUDARGBDSensor::process(ID3D11DeviceContext* context)
{
HRESULT hr = S_OK;
if (m_RGBDAdapter->process(context) == S_FALSE) return S_FALSE;
////////////////////////////////////////////////////////////////////////////////////
// Process Color
////////////////////////////////////////////////////////////////////////////////////
//Start Timing
if (GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.start(); }
if (m_bFilterIntensityValues) gaussFilterFloat4Map(m_depthCameraData.d_colorData, m_RGBDAdapter->getColorMapResampledFloat4(), m_fBilateralFilterSigmaDIntensity, m_fBilateralFilterSigmaRIntensity, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
else copyFloat4Map(m_depthCameraData.d_colorData, m_RGBDAdapter->getColorMapResampledFloat4(), m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
// Stop Timing
if (GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.stop(); TimingLog::totalTimeFilterColor += m_timer.getElapsedTimeMS(); TimingLog::countTimeFilterColor++; }
////////////////////////////////////////////////////////////////////////////////////
// Process Depth
////////////////////////////////////////////////////////////////////////////////////
//Start Timing
if (GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.start(); }
if (m_bFilterDepthValues) gaussFilterFloatMap(d_depthMapFilteredFloat, m_RGBDAdapter->getDepthMapResampledFloat(), m_fBilateralFilterSigmaD, m_fBilateralFilterSigmaR, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
else copyFloatMap(d_depthMapFilteredFloat, m_RGBDAdapter->getDepthMapResampledFloat(), m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
//TODO this call seems not needed as the depth map is overwriten later anyway later anyway...
setInvalidFloatMap(m_depthCameraData.d_depthData, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
// Stop Timing
if (GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.stop(); TimingLog::totalTimeFilterDepth += m_timer.getElapsedTimeMS(); TimingLog::countTimeFilterDepth++; }
////////////////////////////////////////////////////////////////////////////////////
// Render to Color Space
////////////////////////////////////////////////////////////////////////////////////
//Start Timing
if (GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.start(); }
if (GlobalAppState::get().s_bUseCameraCalibration)
{
mat4f depthExt = m_RGBDAdapter->getDepthExtrinsics();
g_CustomRenderTarget.Clear(context);
g_CustomRenderTarget.Bind(context);
g_RGBDRenderer.RenderDepthMap(context,
d_depthMapFilteredFloat, m_depthCameraData.d_colorData, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight(),
m_RGBDAdapter->getDepthIntrinsicsInv(), depthExt, m_RGBDAdapter->getColorIntrinsics(),
g_CustomRenderTarget.getWidth(), g_CustomRenderTarget.getHeight(),
GlobalAppState::get().s_remappingDepthDiscontinuityThresOffset, GlobalAppState::get().s_remappingDepthDiscontinuityThresLin);
g_CustomRenderTarget.Unbind(context);
g_CustomRenderTarget.copyToCuda(m_depthCameraData.d_depthData, 0);
//Util::writeToImage(m_depthCameraData.d_depthData, getDepthWidth(), getDepthHeight(), "depth.png");
//Util::writeToImage(m_depthCameraData.d_colorData, getDepthWidth(), getDepthHeight(), "color.png");
}
else
{
copyFloatMap(m_depthCameraData.d_depthData, d_depthMapFilteredFloat, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
}
bool bErode = false;
if (bErode) {
unsigned int numIter = 20;
numIter = 2 * ((numIter + 1) / 2);
for (unsigned int i = 0; i < numIter; i++) {
if (i % 2 == 0) {
erodeDepthMap(d_depthErodeHelper, m_depthCameraData.d_depthData, 5, getDepthWidth(), getDepthHeight(), 0.05f, 0.3f);
}
else {
erodeDepthMap(m_depthCameraData.d_depthData, d_depthErodeHelper, 5, getDepthWidth(), getDepthHeight(), 0.05f, 0.3f);
}
}
}
//TODO check whether the intensity is actually used
convertColorToIntensityFloat(d_intensityMapFilteredFloat, m_depthCameraData.d_colorData, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
float4x4 M((m_RGBDAdapter->getColorIntrinsicsInv()).ptr());
m_depthCameraData.updateParams(getDepthCameraParams());
convertDepthFloatToCameraSpaceFloat4(d_cameraSpaceFloat4, m_depthCameraData.d_depthData, M, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight(), m_depthCameraData); // !!! todo
computeNormals(d_normalMapFloat4, d_cameraSpaceFloat4, m_RGBDAdapter->getWidth(), m_RGBDAdapter->getHeight());
float4x4 Mintrinsics((m_RGBDAdapter->getColorIntrinsics()).ptr());
cudaMemcpyToArray(m_depthCameraData.d_depthArray, 0, 0, m_depthCameraData.d_depthData, sizeof(float)*m_depthCameraParams.m_imageHeight*m_depthCameraParams.m_imageWidth, cudaMemcpyDeviceToDevice);
cudaMemcpyToArray(m_depthCameraData.d_colorArray, 0, 0, m_depthCameraData.d_colorData, sizeof(float4)*m_depthCameraParams.m_imageHeight*m_depthCameraParams.m_imageWidth, cudaMemcpyDeviceToDevice);
// Stop Timing
if (GlobalAppState::get().s_timingsDetailledEnabled) { cutilSafeCall(cudaDeviceSynchronize()); m_timer.stop(); TimingLog::totalTimeRemapDepth += m_timer.getElapsedTimeMS(); TimingLog::countTimeRemapDepth++; }
return hr;
}
