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); }