void ITMColorTracker_CPU::F_oneLevel(float *f, ITMPose *pose)
{
int noTotalPoints = trackingState->pointCloud->noTotalPoints;
Vector4f projParams;
if (type == ITMLibSettings::TRACKER_ODOMETRY_COLOR) {
projParams.setValues(view->calib->intrinsics_d.projectionParamsSimple.all);
} else {
projParams.setValues(view->calib->intrinsics_rgb.projectionParamsSimple.all);
}
projParams.x /= 1 << levelId; projParams.y /= 1 << levelId;
projParams.z /= 1 << levelId; projParams.w /= 1 << levelId;
Matrix4f M = pose->GetM();
Vector2i imgSize = viewHierarchy->levels[levelId]->rgb->noDims;
float scaleForOcclusions, final_f;
Vector4f *locations = trackingState->pointCloud->locations->GetData(MEMORYDEVICE_CPU);
Vector4f *colours = trackingState->pointCloud->colours->GetData(MEMORYDEVICE_CPU);
Vector4u *rgb = viewHierarchy->levels[levelId]->rgb->GetData(MEMORYDEVICE_CPU);
final_f = 0; countedPoints_valid = 0;
for (int locId = 0; locId < noTotalPoints; locId++)
{
float colorDiffSq = getColorDifferenceSq(locations, colours, rgb, imgSize, locId, projParams, M);
if (colorDiffSq >= 0) { final_f += colorDiffSq; countedPoints_valid++; }
}
if (countedPoints_valid == 0) { final_f = MY_INF; scaleForOcclusions = 1.0; }
else { scaleForOcclusions = (float)noTotalPoints / countedPoints_valid; }
f[0] = final_f * scaleForOcclusions;
}
void ITMColorTracker_CPU::G_oneLevel(float *gradient, float *hessian, ITMPose *pose) const
{
int noTotalPoints = trackingState->pointCloud->noTotalPoints;
Vector4f projParams;
if (type == ITMLibSettings::TRACKER_ODOMETRY_COLOR) {
projParams.setValues(view->calib->intrinsics_d.projectionParamsSimple.all);
} else {
projParams.setValues(view->calib->intrinsics_rgb.projectionParamsSimple.all);
}
projParams.x /= 1 << levelId; projParams.y /= 1 << levelId;
projParams.z /= 1 << levelId; projParams.w /= 1 << levelId;
Matrix4f M = pose->GetM();
Vector2i imgSize = viewHierarchy->levels[levelId]->rgb->noDims;
float scaleForOcclusions;
bool rotationOnly = iterationType == TRACKER_ITERATION_ROTATION;
int numPara = rotationOnly ? 3 : 6, startPara = rotationOnly ? 3 : 0, numParaSQ = rotationOnly ? 3 + 2 + 1 : 6 + 5 + 4 + 3 + 2 + 1;
float globalGradient[6], globalHessian[21];
for (int i = 0; i < numPara; i++) globalGradient[i] = 0.0f;
for (int i = 0; i < numParaSQ; i++) globalHessian[i] = 0.0f;
Vector4f *locations = trackingState->pointCloud->locations->GetData(MEMORYDEVICE_CPU);
Vector4f *colours = trackingState->pointCloud->colours->GetData(MEMORYDEVICE_CPU);
Vector4u *rgb = viewHierarchy->levels[levelId]->rgb->GetData(MEMORYDEVICE_CPU);
Vector4s *gx = viewHierarchy->levels[levelId]->gradientX_rgb->GetData(MEMORYDEVICE_CPU);
Vector4s *gy = viewHierarchy->levels[levelId]->gradientY_rgb->GetData(MEMORYDEVICE_CPU);
for (int locId = 0; locId < noTotalPoints; locId++)
{
float localGradient[6], localHessian[21];
bool isValidPoint = computePerPointGH_rt_Color(localGradient, localHessian, locations, colours, rgb, imgSize, locId,
projParams, M, gx, gy, numPara, startPara);
if (isValidPoint)
{
for (int i = 0; i < numPara; i++) globalGradient[i] += localGradient[i];
for (int i = 0; i < numParaSQ; i++) globalHessian[i] += localHessian[i];
}
}
scaleForOcclusions = (float)noTotalPoints / countedPoints_valid;
if (countedPoints_valid == 0) { scaleForOcclusions = 1.0f; }
for (int para = 0, counter = 0; para < numPara; para++)
{
gradient[para] = globalGradient[para] * scaleForOcclusions;
for (int col = 0; col <= para; col++, counter++) hessian[para + col * numPara] = globalHessian[counter] * scaleForOcclusions;
}
for (int row = 0; row < numPara; row++)
{
for (int col = row + 1; col < numPara; col++) hessian[row + col * numPara] = hessian[col + row * numPara];
}
}
