Eigen::Matrix4f CUDACameraTrackingMultiResRGBD::computeBestRigidAlignment(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, Eigen::Matrix4f& globalDeltaTransform, unsigned int level, CameraTrackingParameters cameraTrackingParameters, unsigned int maxInnerIter, float condThres, float angleThres, LinearSystemConfidence& conf)
{
Eigen::Matrix4f deltaTransform = globalDeltaTransform;
conf.reset();
Matrix6x7f system;
Eigen::Matrix3f ROld = deltaTransform.block(0, 0, 3, 3);
Eigen::Vector3f eulerAngles = ROld.eulerAngles(2, 1, 0);
float3 anglesOld; anglesOld.x = eulerAngles.x(); anglesOld.y = eulerAngles.y(); anglesOld.z = eulerAngles.z();
float3 translationOld; translationOld.x = deltaTransform(0, 3); translationOld.y = deltaTransform(1, 3); translationOld.z = deltaTransform(2, 3);
m_CUDABuildLinearSystem->applyBL(cameraTrackingInput, intrinsics, cameraTrackingParameters, anglesOld, translationOld, m_imageWidth[level], m_imageHeight[level], level, system, conf);
Matrix6x6f ATA = system.block(0, 0, 6, 6);
Vector6f ATb = system.block(0, 6, 6, 1);
if (ATA.isZero()) {
return m_matrixTrackingLost;
}
Eigen::JacobiSVD<Matrix6x6f> SVD(ATA, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vector6f x = SVD.solve(ATb);
//computing the matrix condition
Vector6f evs = SVD.singularValues();
conf.matrixCondition = evs[0]/evs[5];
Vector6f xNew; xNew.block(0, 0, 3, 1) = eulerAngles; xNew.block(3, 0, 3, 1) = deltaTransform.block(0, 3, 3, 1);
xNew += x;
deltaTransform = delinearizeTransformation(xNew, Eigen::Vector3f(0.0f, 0.0f, 0.0f), 1.0f, level);
if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity())
{
conf.trackingLostTresh = true;
return m_matrixTrackingLost;
}
return deltaTransform;
}
Eigen::Matrix4f DX11CameraTrackingMultiRes::computeBestRigidAlignment(ID3D11DeviceContext* context, ID3D11ShaderResourceView* inputSRV, ID3D11ShaderResourceView* inputNormalsSRV, D3DXVECTOR3& mean, float meanStDev, float nValidCorres, const Eigen::Matrix4f& globalDeltaTransform, unsigned int level, unsigned int maxInnerIter, float condThres, float angleThres, LinearSystemConfidence& conf)
{
Eigen::Matrix4f deltaTransform = globalDeltaTransform;
for(unsigned int i = 0; i<maxInnerIter; i++)
{
conf.reset();
Matrix6x7f system;
DX11BuildLinearSystem::applyBL(context, inputSRV, m_pCorrespondenceTextureFloat4SRV[level], m_pCorrespondenceNormalTextureFloat4SRV[level], mean, meanStDev, deltaTransform, m_imageWidth[level], m_imageHeight[level], level, system, conf);
Matrix6x6f ATA = system.block(0, 0, 6, 6);
Vector6f ATb = system.block(0, 6, 6, 1);
Eigen::JacobiSVD<Matrix6x6f> SVD(ATA, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vector6f x = SVD.solve(ATb);
//computing the matrix condition
Vector6f evs = SVD.singularValues();
conf.matrixCondition = evs[0]/evs[5];
Eigen::Matrix4f t = delinearizeTransformation(x, Eigen::Vector3f(mean.x, mean.y, mean.z), meanStDev, level);
if(t(0, 0) == -std::numeric_limits<float>::infinity())
{
conf.trackingLostTresh = true;
return m_matrixTrackingLost;
}
deltaTransform = t*deltaTransform;
//Eigen::Matrix3f R = deltaTransform.block(0, 0, 3, 3);
//Eigen::Vector3f v = deltaTransform.block(0, 3, 3, 1);
}
return deltaTransform;
}
