mat4f DX11CameraTrackingMultiRes::applyCT(ID3D11DeviceContext* context, ID3D11ShaderResourceView* inputSRV, ID3D11ShaderResourceView* inputNormalsSRV, ID3D11ShaderResourceView* inputColorsSRV, ID3D11ShaderResourceView* modelSRV, ID3D11ShaderResourceView* modelNormalsSRV, ID3D11ShaderResourceView* modelColorsSRV, 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, float condThres, float angleThres, const mat4f& deltaTransformEstimate, const std::vector<float>& earlyOutResidual, ICPErrorLog* errorLog)
{
// Input
m_inputTextureFloat4SRV[0] = inputSRV;
m_inputNormalTextureFloat4SRV[0] = inputNormalsSRV;
m_inputColorTextureFloat4SRV[0] = inputColorsSRV;
m_modelTextureFloat4SRV[0] = modelSRV;
m_modelNormalTextureFloat4SRV[0] = modelNormalsSRV;
m_modelColorTextureFloat4SRV[0] = modelColorsSRV;
// Start query for timing
if(GlobalAppState::getInstance().s_timingsDetailledEnabled)
{
GlobalAppState::getInstance().WaitForGPU();
m_timer.start();
}
for(unsigned int i = 0; i<GlobalCameraTrackingState::getInstance().s_maxLevels-1; i++)
{
// Downsample Depth Maps directly ? -> better ?
DX11ImageHelper::applyDownsampling(context, m_inputTextureFloat4SRV[i], m_inputTextureFloat4UAV[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
DX11ImageHelper::applyDownsampling(context, m_modelTextureFloat4SRV[i], m_modelTextureFloat4UAV[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
DX11ImageHelper::applyNormalComputation(context, m_inputTextureFloat4SRV[i+1], m_inputNormalTextureFloat4UAV[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
DX11ImageHelper::applyNormalComputation(context, m_modelTextureFloat4SRV[i+1], m_modelNormalTextureFloat4UAV[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
DX11ImageHelper::applyDownsampling(context, m_inputColorTextureFloat4SRV[i], m_inputColorTextureFloat4UAV[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
DX11ImageHelper::applyDownsampling(context, m_modelColorTextureFloat4SRV[i], m_modelColorTextureFloat4UAV[i+1], m_imageWidth[i+1], m_imageHeight[i+1]);
}
Eigen::Matrix4f deltaTransform;
//deltaTransform.setIdentity();
deltaTransform = MatToEig(deltaTransformEstimate);
for(int level = GlobalCameraTrackingState::getInstance().s_maxLevels-1; level>=0; level--)
{
if (errorLog) {
errorLog->newICPFrame(level);
}
deltaTransform = align(context, m_inputTextureFloat4SRV[level], m_inputNormalTextureFloat4SRV[level], m_inputColorTextureFloat4SRV[level], m_modelTextureFloat4SRV[level], m_modelNormalTextureFloat4SRV[level], m_modelColorTextureFloat4SRV[level], deltaTransform, level, maxInnerIter[level], maxOuterIter[level], distThres[level], normalThres[level], condThres, angleThres, earlyOutResidual[level], errorLog);
if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity()) {
return EigToMat(m_matrixTrackingLost);
}
}
// Wait for query
if(GlobalAppState::getInstance().s_timingsDetailledEnabled)
{
GlobalAppState::getInstance().WaitForGPU();
TimingLog::totalTimeTrackCamera += m_timer.getElapsedTimeMS(); TimingLog::countTrackCamera++;
}
return lastTransform*EigToMat(deltaTransform);
}
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);
}
