/// <summary>
/// lableForegroundPixels to label pixels as forground or background for better processing :)
/// This labeling is done on the frame Mask
/// </summary>
/// <param name="state"></param>
void labelMaskForegroundPixels(trackerState state)
{
int count = frame.Width * frame.Height;
float dt;
//for (int i = 0; i < count; i++)
//{
for (int i = 0; i < frame.Height; i++)
{
for (int j = 0; j < frame.Width; j++)
{
int idx = i * frame.Width + j;
// Keep track of useless pixels on the mask and check if we're inside the bb
if ((frame.PfVec[i] > 0)) //&& (frame.PfVec[i] != defines.OUTSIDE_BB)) //TODO-alexa
{
dt = findPerPixelDT(frame.Camera3DPoints[idx], frame.PfVec[idx], state.getPoseList(), defines.NUM_OBJ);
if (Mathf.Abs(dt) <= 5)
{
frame.Mask[i] = defines.HIST_FG_PIXEL;
Color pixel = frame.ColorTexture.GetPixel(j, i);
frame.ColorTextureVisual.SetPixel(j, i, Color.Lerp(pixel, Color.red, 0.2f));
}
else
{
frame.Mask[i] = defines.HIST_BG_PIXEL;
}
}
//}
}
}
}
/// <summary>
/// evaluates the total energy of a given frame
/// </summary>
/// <param name="energy"></param>
/// <param name="state"></param>
private void evaluateEnergy(out float energy, trackerState state)
{
int count = frame.Width * frame.Height; //frame.Camera3DPoints.Length; TODO Michael changed this
CameraSpacePoint[] ptcloud_ptr = frame.Camera3DPoints;
objectPose[] poses = state.getPoseList();
int objCount = state.numPoses();
float e = 0, es = 0;
int totalpix = 0;
int totalpfpix = 0;
//int testcount = 0;
//int testcount2 = 0;
for (int i = 0; i < count; i++)
{
es = computePerPixelEnergy(ptcloud_ptr[i], frame.PfVec[i], poses, objCount);
if (es > 0)
{
e += es; totalpix++;
if (frame.PfVec[i] > 0.5)
{
totalpfpix++;
}
}
}
energy = totalpfpix > 100 ? e / totalpix : 0.0f;
}
public bool TestEvaluateEnergy(string ptcloudFile)
{
//int npoints = width * height;
CameraSpacePoint[] pointCloud = new CameraSpacePoint[13568];
float[] pfvec = new float[13568];
// Read the file and parse data into point cloud
using (BinaryReader file = new BinaryReader(File.Open(ptcloudFile, FileMode.Open)))
{
for (int k = 0; k < 13568; k++)
{
pointCloud[k] = new CameraSpacePoint();
pointCloud[k].X = file.ReadSingle();
pointCloud[k].Y = file.ReadSingle();
pointCloud[k].Z = file.ReadSingle();
pfvec[k] = file.ReadSingle();
}
}
int count = 13568;
CameraSpacePoint[] ptcloud_ptr = pointCloud;
float[] pfArray = pfvec;
Matrix4x4 pose = new Matrix4x4();
pose.m00 = 0.760526f; pose.m10 = 0.523887f; pose.m20 = 0.38359f; pose.m30 = 0.0f;
pose.m01 = 0.631573f; pose.m11 = -0.459739f; pose.m21 = -0.624303f; pose.m31 = 0.0f;
pose.m02 = -0.150713f; pose.m12 = 0.717064f; pose.m22 = -0.680517f; pose.m32 = 0.0f;
pose.m03 = 0.007287f; pose.m13 = -0.011503f; pose.m23 = 0.583796f; pose.m33 = 1.0f;
trackerState state = new trackerState(1);
state.getPose(0).setFromH(pose);
objectPose[] poses = state.getPoseList();
int objCount = state.numPoses();
float e = 0, es = 0;
int totalpix = 0;
int totalpfpix = 0;
for (int i = 0; i < count; i++)
{
es = computePerPixelEnergy(ptcloud_ptr[i], pfArray[i], poses, objCount);
if (es > 0)
{
e += es; totalpix++;
if (pfArray[i] > 0.5)
{
totalpfpix++;
}
}
}
float energy = totalpfpix > 100 ? e / totalpix : 0.0f;
//Debug.Log( "energy is " + energy.ToString( "F6" ) );
return(true);
}
/// <summary>
/// Function to set this trackerState from another trackerState.
/// </summary>
public void setFrom(trackerState inposes)
{
int count = inposes.numPoses();
boundingBox = inposes.boundingBox;
for (int i = 0; i < count; i++)
{
this.getPose(i).setFromH(inposes.getPose(i).getH());
}
}
/// <summary>
/// computeJacobianAndHessian helper function to get jacobian and hessian used for the next step. This function should not change
/// anything in the tracker object.
/// </summary>
/// <param name="gradient"></param>
/// <param name="hessian"></param>
/// <param name="tracker"></param>
private void computeJacobianAndHessian(float[] gradient, float[] hessian, trackerState tracker)
{
int count = frame.Camera3DPoints.Length; //frame.Width * frame.Height; //
CameraSpacePoint[] ptcloud = frame.Camera3DPoints; // 3D voxels in meters
float[] pfArray = frame.PfVec; // pf of each voxel
objectPose[] poses = tracker.getPoseList();
int objCount = tracker.numPoses();
int paramNum = objCount * 6;
int paramNumSq = paramNum * paramNum;
float[] globalGradient = new float[paramNum];
float[] globalHessian = new float[paramNumSq];
float[] jacobian = new float[paramNum];
Array.Clear(globalGradient, 0, paramNum);
Array.Clear(globalHessian, 0, paramNumSq);
for (int i = 0; i < count; i++)
{
if (pfArray[i] != defines.OUTSIDE_BB)
{
if (computePerPixelJacobian(out jacobian, ptcloud[i], pfArray[i], poses, objCount))
{
for (int a = 0, counter = 0; a < paramNum; a++)
{
globalGradient[a] += jacobian[a];
for (int b = 0; b <= a; b++, counter++)
{
globalHessian[counter] += jacobian[a] * jacobian[b];
}
}
}
}
}
Array.Copy(globalGradient, gradient, paramNum);
for (int r = 0, counter = 0; r < paramNum; r++)
{
for (int c = 0; c <= r; c++, counter++)
{
hessian[r + c * paramNum] = globalHessian[counter];
}
}
for (int r = 0; r < paramNum; ++r)
{
for (int c = r + 1; c < paramNum; c++)
{
hessian[r + c * paramNum] = hessian[c + r * paramNum];
}
}
}
public trackerRGBD(int nObjs, string[] fileNames)
{
nObjects = nObjs;
shapes = new shapeSDF[nObjects];
for (int i = 0; i < nObjects; i++)
{
shapes[i] = new shapeSDF(fileNames[i]);
}
ATb_size = nObjs * 6;
ATA_size = ATb_size * ATb_size;
ATb = new float[ATb_size];
ATA = new float[ATA_size];
tmpATA = new float[ATb_size * ATb_size];
trackingState = new trackerState(nObjs);
tempState = new trackerState(nObjs);
}
