Eigen::Matrix4f CUDACameraTrackingMultiResRGBD::align(CameraTrackingInput cameraTrackingInput, Eigen::Matrix4f& deltaTransform, unsigned int level, CameraTrackingParameters cameraTrackingParameters, unsigned int maxInnerIter, unsigned maxOuterIter, float condThres, float angleThres, float earlyOut, const mat4f& intrinsic, const DepthCameraData& depthCameraData, ICPErrorLog* errorLog)
{
float lastICPError = -1.0f;
for(unsigned int i = 0; i<maxOuterIter; i++)
{
LinearSystemConfidence currConf;
Eigen::Matrix4f intrinsics4x4 = MatrixConversion::MatToEig(intrinsic);
Eigen::Matrix3f intrinsics = intrinsics4x4.block(0, 0, 3, 3);
deltaTransform = computeBestRigidAlignment(cameraTrackingInput, intrinsics, deltaTransform, level, cameraTrackingParameters, maxInnerIter, condThres, angleThres, currConf);
if (errorLog) {
errorLog->addCurrentICPIteration(currConf, level);
}
if (std::abs(lastICPError - currConf.sumRegError) < earlyOut) {
// std::cout << lastICPError << " " << currConf.sumRegError << " ICP aboarted because no further convergence... " << i << std::endl;
break;
}
lastICPError = currConf.sumRegError;
}
return deltaTransform;
}
Eigen::Matrix4f DX11CameraTrackingMultiRes::align(ID3D11DeviceContext* context, ID3D11ShaderResourceView* inputSRV, ID3D11ShaderResourceView* inputNormalsSRV, ID3D11ShaderResourceView* inputColorsSRV,
ID3D11ShaderResourceView* modelSRV, ID3D11ShaderResourceView* modelNormalsSRV, ID3D11ShaderResourceView* modelColorsSRV, Eigen::Matrix4f& deltaTransform,
unsigned int level, unsigned int maxInnerIter, unsigned maxOuterIter, float distThres, float normalThres, float condThres, float angleThres, float earlyOut, ICPErrorLog* errorLog)
{
float lastICPError = -1.0f;
for(unsigned int i = 0; i<maxOuterIter; i++)
{
D3DXVECTOR3 mean;
float meanStDev;
float nValidCorres;
LinearSystemConfidence currConfWiReject;
LinearSystemConfidence currConfNoReject;
if (errorLog) {
//run ICP without correspondence rejection (must be run before because it needs the old delta transform)
float dThresh = 1000.0f; float nThresh = 0.0f;
computeCorrespondences(context, inputSRV, inputNormalsSRV, inputColorsSRV, modelSRV, modelNormalsSRV, modelColorsSRV, mean, meanStDev, nValidCorres, deltaTransform, level, dThresh, nThresh);
computeBestRigidAlignment(context, inputSRV, inputNormalsSRV, mean, meanStDev, nValidCorres, deltaTransform, level, maxInnerIter, condThres, angleThres, currConfNoReject);
errorLog->addCurrentICPIteration(currConfNoReject, level);
}
//standard correspondence search and alignment
computeCorrespondences(context, inputSRV, inputNormalsSRV, inputColorsSRV, modelSRV, modelNormalsSRV, modelColorsSRV, mean, meanStDev, nValidCorres, deltaTransform, level, distThres, normalThres);
deltaTransform = computeBestRigidAlignment(context, inputSRV, inputNormalsSRV, mean, meanStDev, nValidCorres, deltaTransform, level, maxInnerIter, condThres, angleThres, currConfWiReject);
if (std::abs(lastICPError - currConfWiReject.sumRegError) < earlyOut) {
//std::cout << "ICP aboarted because no further convergence... " << i << std::endl;
break;
}
lastICPError = currConfWiReject.sumRegError;
//std::cout << currConfWiReject.numCorr << std::endl;
//std::cout << "i " << i << std::endl;
//currConf.print();
//if(level == 0) std::cout << deltaTransform << std::endl;
//deltaTransform.setIdentity();
/*if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity())
{
return m_matrixTrackingLost;
}*/
}
return deltaTransform;
}
