private void PerformExpectationMaximization(float3[] _Directions, FitLobe[] _Lobes)
{
int n = _Directions.Length;
double invN = 1.0 / n;
int k = _Lobes.Length;
double[,] probabilities = new double[n, k];
// 1] Initialize lobes
for (int h = 0; h < k; h++)
{
_Lobes[h].Direction = _Directions[(int)((h + 0.5f) * n / k)];
_Lobes[h].Concentration = 0.5;
_Lobes[h].Alpha = 1.0 / k;
}
// 2] Iterate
int iterationsCount = 0;
while (++iterationsCount < 1000)
{
// 2.1] Compute Expectation (the E step of the algorithm)
for (int i = 0; i < n; i++)
{
float3 dir = _Directions[i];
// 2.1.1) Compute weighted probability for each direction to belong to each lobe
double weightedSumProbabilities = 0.0;
for (int h = 0; h < k; h++)
{
FitLobe Lobe = _Lobes[h];
double kappa = Lobe.Concentration;
double dot = dir.Dot(Lobe.Direction);
double f = kappa / (2.0 * Math.PI * (Math.Exp(kappa) - Math.Exp(-kappa))) * Math.Exp(kappa * dot);
f *= Lobe.Alpha;
probabilities[i, h] = f;
weightedSumProbabilities += f;
}
// 2.1.2) Normalize
double normalizer = weightedSumProbabilities > 1e-12 ? 1.0 / weightedSumProbabilities : 0.0;
for (int h = 0; h < k; h++)
{
probabilities[i, h] *= normalizer;
}
}
// 2.2] Compute Maximization (the M step of the algorithm)
double sqConvergenceRate = 0.0;
for (int h = 0; h < k; h++)
{
FitLobe Lobe = _Lobes[h];
// Accumulate new alpha and average direction
Lobe.Alpha = 0.0;
double mu_x = 0.0;
double mu_y = 0.0;
double mu_z = 0.0;
for (int i = 0; i < n; i++)
{
float3 dir = _Directions[i];
double p = probabilities[i, h];
double p_over_N = invN * p;
Lobe.Alpha += p_over_N;
mu_x += p_over_N * dir.x;
mu_y += p_over_N * dir.y;
mu_z += p_over_N * dir.z;
}
// Compute new direction
double mu_length = Math.Sqrt(mu_x * mu_x + mu_y * mu_y + mu_z * mu_z);
double r = Lobe.Alpha > 1e-12 ? mu_length / Lobe.Alpha : 0.0;
// Normalize direction
mu_length = mu_length > 1e-12 ? 1.0 / mu_length : 0.0;
Lobe.Direction.x = (float)(mu_length * mu_x);
Lobe.Direction.y = (float)(mu_length * mu_y);
Lobe.Direction.z = (float)(mu_length * mu_z);
// Compute new concentration
double oldConcentration = Lobe.Concentration;
double newConcentration = (3.0 * r - r * r * r) / (1.0 - r * r);
Lobe.Concentration = newConcentration;
sqConvergenceRate += (newConcentration - oldConcentration) * (newConcentration - oldConcentration);
}
sqConvergenceRate /= k * k;
if (sqConvergenceRate < 1e-6)
{
break;
}
}
}
// public float3x3 MakeRot( float3 _from, float3 _to ) {
// float3 v = _from.Cross( _to );
// float c = _from.Dot( _to );
// float k = 1.0f / (1.0f + c);
//
// float3x3 R = new float3x3();
// R.m[0, 0] = v.x*v.x*k + c; R.m[0, 1] = v.y*v.x*k - v.z; R.m[0, 2] = v.z*v.x*k + v.y;
// R.m[1, 0] = v.x*v.y*k + v.z; R.m[1, 1] = v.y*v.y*k + c; R.m[1, 2] = v.z*v.y*k - v.x;
// R.m[2, 0] = v.x*v.z*k - v.y; R.m[2, 1] = v.y*v.z*k + v.x; R.m[2, 2] = v.z*v.z*k + c;
//
// return R;
// }
public FittingForm()
{
// Random RNG = new Random();
// float3 From = new float3( 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f ).Normalized;
// float3 To = new float3( 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f ).Normalized;
// float3x3 Pipo = MakeRot( From, To );
// float3 Test = Pipo * From;
//TestChromaRanges();
//TestSHRGBEEncoding();
//TestSquareFilling();
InitializeComponent();
// Create the random points
List <float3> RandomDirections = new List <float3>();
List <float> RandomThetas = new List <float>();
for (int LobeIndex = 0; LobeIndex < m_RandomLobes.Length; LobeIndex++)
{
float MainPhi = (float)(m_RandomLobes[LobeIndex].Phi * Math.PI / 180.0f);
float MainTheta = (float)(m_RandomLobes[LobeIndex].Theta * Math.PI / 180.0f);
float Concentration = m_RandomLobes[LobeIndex].Concentration;
int PointsCount = m_RandomLobes[LobeIndex].RandomPointsCount;
// Build the main direction for the target lobe
float3 MainDirection = new float3(
(float)(Math.Sin(MainTheta) * Math.Sin(MainPhi)),
(float)(Math.Cos(MainTheta)),
(float)(Math.Sin(MainTheta) * Math.Cos(MainPhi))
);
// Build the transform to bring Y-aligned points to the main direction
float3x3 Rot = new float3x3();
Rot.BuildRot(float3.UnitY, MainDirection);
BuildDistributionMapping(Concentration, 0.0);
// Draw random points in the Y-aligned hemisphere and transform them into the main direction
for (int PointIndex = 0; PointIndex < PointsCount; PointIndex++)
{
double Theta = GetTheta();
float CosTheta = (float)Math.Cos(Theta);
// float SinTheta = (float) Math.Sqrt( 1.0f - CosTheta*CosTheta );
float SinTheta = (float)Math.Sin(Theta);
float Phi = (float)(SimpleRNG.GetUniform() * Math.PI);
float3 RandomDirection = new float3(
(float)(SinTheta * Math.Sin(Phi)),
CosTheta,
(float)(SinTheta * Math.Cos(Phi))
);
float3 FinalDirection = RandomDirection * Rot;
RandomDirections.Add(FinalDirection);
RandomThetas.Add(CosTheta);
}
}
m_RandomDirections = RandomDirections.ToArray();
m_RandomThetas = RandomThetas.ToArray();
panelOutput.UpdateBitmap();
panelOutputNormalDistribution.UpdateBitmap();
// Do it!
FitLobe[] Result = new FitLobe[FITTING_LOBES_COUNT];
for (int h = 0; h < Result.Length; h++)
{
Result[h] = new FitLobe();
}
PerformExpectationMaximization(m_RandomDirections, Result);
}
private void PerformExpectationMaximization( Vector[] _Directions, FitLobe[] _Lobes )
{
int n = _Directions.Length;
double invN = 1.0 / n;
int k = _Lobes.Length;
double[,] probabilities = new double[n,k];
// 1] Initialize lobes
for ( int h=0; h < k; h++ ) {
_Lobes[h].Direction = _Directions[(int) ((h+0.5f) * n / k)];
_Lobes[h].Concentration = 0.5;
_Lobes[h].Alpha = 1.0 / k;
}
// 2] Iterate
int iterationsCount = 0;
while ( ++iterationsCount < 1000 ) {
// 2.1] Compute Expectation (the E step of the algorithm)
for ( int i=0; i < n; i++ ) {
Vector dir = _Directions[i];
// 2.1.1) Compute weighted probability for each direction to belong to each lobe
double weightedSumProbabilities = 0.0;
for ( int h=0; h < k; h++ ) {
FitLobe Lobe = _Lobes[h];
double kappa = Lobe.Concentration;
double dot = dir.Dot( Lobe.Direction );
double f = kappa / (2.0 * Math.PI * (Math.Exp( kappa ) - Math.Exp( -kappa ))) * Math.Exp( kappa * dot );
f *= Lobe.Alpha;
probabilities[i,h] = f;
weightedSumProbabilities += f;
}
// 2.1.2) Normalize
double normalizer = weightedSumProbabilities > 1e-12 ? 1.0 / weightedSumProbabilities : 0.0;
for ( int h=0; h < k; h++ ) {
probabilities[i,h] *= normalizer;
}
}
// 2.2] Compute Maximization (the M step of the algorithm)
double sqConvergenceRate = 0.0;
for ( int h=0; h < k; h++ ) {
FitLobe Lobe = _Lobes[h];
// Accumulate new alpha and average direction
Lobe.Alpha = 0.0;
double mu_x = 0.0;
double mu_y = 0.0;
double mu_z = 0.0;
for ( int i=0; i < n; i++ ) {
Vector dir = _Directions[i];
double p = probabilities[i,h];
double p_over_N = invN * p;
Lobe.Alpha += p_over_N;
mu_x += p_over_N * dir.x;
mu_y += p_over_N * dir.y;
mu_z += p_over_N * dir.z;
}
// Compute new direction
double mu_length = Math.Sqrt( mu_x*mu_x + mu_y*mu_y + mu_z*mu_z );
double r = Lobe.Alpha > 1e-12 ? mu_length / Lobe.Alpha : 0.0;
// Normalize direction
mu_length = mu_length > 1e-12 ? 1.0 / mu_length : 0.0;
Lobe.Direction.x = (float) (mu_length * mu_x);
Lobe.Direction.y = (float) (mu_length * mu_y);
Lobe.Direction.z = (float) (mu_length * mu_z);
// Compute new concentration
double oldConcentration = Lobe.Concentration;
double newConcentration = (3.0 * r - r*r*r) / (1.0 - r*r);
Lobe.Concentration = newConcentration;
sqConvergenceRate += (newConcentration - oldConcentration) * (newConcentration - oldConcentration);
}
sqConvergenceRate /= k * k;
if ( sqConvergenceRate < 1e-6 )
break;
}
}
// Solves the best planes for the room
void SolveRoom()
{
//////////////////////////////////////////////////////////////////////////
// Use EM to obtain principal directions
List <float3> directions = new List <float3>(PIXELS_COUNT);
for (int i = 0; i < PIXELS_COUNT; i++)
{
// if ( m_Pixels[i].Distance < 1e3f )
directions.Add(new float3(m_Pixels[i].Normal.x, m_Pixels[i].Normal.y, 0.0f));
}
int planesCount = integerTrackbarControlResultPlanesCount.Value;
m_Planes = new Plane[planesCount];
m_Lobes = new FitLobe[planesCount];
for (int i = 0; i < planesCount; i++)
{
m_Lobes[i] = new FitLobe();
}
PerformExpectationMaximization(directions.ToArray(), m_Lobes, 1000, 1e-6);
for (int i = 0; i < planesCount; i++)
{
m_Planes[i].m_Weight = (float)m_Lobes[i].Alpha;
}
//////////////////////////////////////////////////////////////////////////
// Remove similar planes
for (int i = 0; i < planesCount - 1; i++)
{
FitLobe P0 = m_Lobes[i];
if (m_Planes[i].m_Dismissed)
{
continue; // Already dismissed...
}
float3 averageDirection = (float)P0.Alpha * P0.Direction;
double maxKappa = P0.Concentration;
for (int j = i + 1; j < planesCount; j++)
{
FitLobe P1 = m_Lobes[j];
if (m_Planes[j].m_Dismissed)
{
continue; // Already dismissed...
}
float dot = P0.Direction.Dot(P1.Direction);
if (dot < floatTrackbarControlSimilarPlanes.Value)
{
continue;
}
averageDirection += (float)P1.Alpha * P1.Direction;
maxKappa = Math.Max(maxKappa, P1.Concentration);
m_Planes[j].m_Dismissed = true; // Dismiss
m_Planes[j].m_DismissalReason += " SIMILAR" + i;
}
P0.Direction = averageDirection.Normalized;
P0.Concentration = maxKappa;
}
//////////////////////////////////////////////////////////////////////////
// Place planes at the best positions
float maxOrthoDistance = 0.0f;
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
float2 Normal = new float2(m_Lobes[planeIndex].Direction.x, m_Lobes[planeIndex].Direction.y);
m_Planes[planeIndex].m_Normal = Normal;
float sumOrthoDistances0 = 0.0f;
float sumWeights0 = 0.0f;
float sumOrthoDistances1 = 0.0f;
float sumWeights1 = 0.0f;
for (int i = 0; i < PIXELS_COUNT; i++)
{
float orthoDistance = -(m_Pixels[i].Position - m_boxCenter).Dot(Normal);
// Use computed probabilities
float weight = (float)probabilities[i, planeIndex];
sumOrthoDistances0 += weight * orthoDistance;
sumWeights0 += weight;
// Use weighted sum
float dot = Math.Max(0.0f, Normal.Dot(m_Pixels[i].Normal));
dot = (float)Math.Pow(dot, floatTrackbarControlWeightExponent.Value);
weight = dot;
sumOrthoDistances1 += weight * orthoDistance;
sumWeights1 += weight;
}
float averageOrthoDistance0 = sumOrthoDistances0 / sumWeights0;
float averageOrthoDistance1 = sumOrthoDistances1 / sumWeights1;
// Choose whichever ortho distance is best
float finalOrthoDistance;
if (radioButtonBest.Checked)
{
finalOrthoDistance = Math.Max(averageOrthoDistance0, averageOrthoDistance1);
}
else
{
finalOrthoDistance = radioButtonProbabilities.Checked ? averageOrthoDistance0 : averageOrthoDistance1;
}
maxOrthoDistance = Math.Max(maxOrthoDistance, finalOrthoDistance);
m_Planes[planeIndex].m_OrthoDistance = finalOrthoDistance;
m_Planes[planeIndex].m_Position = m_boxCenter - finalOrthoDistance * Normal;
if (finalOrthoDistance <= 0.0f)
{
m_Planes[planeIndex].m_Dismissed = true;
m_Planes[planeIndex].m_DismissalReason += " BEHIND";
}
}
//////////////////////////////////////////////////////////////////////////
// Reconstruct weight
if (radioButtonNormalAffinity.Checked)
{
// Reconstruct weights by normal affinity
// We do the same operation as the normal weighting to compute ortho distances: dot each pixel with each plane normal and use that as a weight
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
float2 Normal = new float2(m_Lobes[planeIndex].Direction.x, m_Lobes[planeIndex].Direction.y);
// Use weighted sum
float sumWeights = 0.0f;
for (int i = 0; i < PIXELS_COUNT; i++)
{
float weight = Math.Max(0.0f, Normal.Dot(m_Pixels[i].Normal));
weight = (float)Math.Pow(weight, floatTrackbarControlWeightExponent.Value);
sumWeights += weight;
}
float finalWeight = sumWeights / PIXELS_COUNT;
m_Lobes[planeIndex].Alpha = finalWeight;
m_Planes[planeIndex].m_Weight = finalWeight;
}
}
else if (radioButtonLargestD.Checked)
{
// Choose largest ortho distances
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
float weight = Math.Max(0.0f, m_Planes[planeIndex].m_OrthoDistance / maxOrthoDistance);
m_Lobes[planeIndex].Alpha = weight;
m_Planes[planeIndex].m_Weight = weight;
}
}
else if (radioButtonWeightHybrid.Checked)
{
// Hybrid method combining ortho distance and normal affinity
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
float2 Normal = new float2(m_Lobes[planeIndex].Direction.x, m_Lobes[planeIndex].Direction.y);
// Use weighted sum
float sumWeights = 0.0f;
for (int i = 0; i < PIXELS_COUNT; i++)
{
float weight = Math.Max(0.0f, Normal.Dot(m_Pixels[i].Normal));
weight = (float)Math.Pow(weight, floatTrackbarControlWeightExponent.Value);
sumWeights += weight;
}
float finalWeight = sumWeights / PIXELS_COUNT;
finalWeight *= Math.Max(0.0f, m_Planes[planeIndex].m_OrthoDistance / maxOrthoDistance);
m_Lobes[planeIndex].Alpha = finalWeight;
m_Planes[planeIndex].m_Weight = finalWeight;
}
}
//////////////////////////////////////////////////////////////////////////
// Dismiss unimportant planes
float averageWeight = 0.0f, harmonicAverageWeight = 0.0f, averageConcentration = 0.0f, harmonicAverageConcentration = 0.0f;
float maxWeight = 0.0f, maxConcentration = 0.0f;
float minWeight = float.MaxValue, minConcentration = float.MaxValue;
int validPlanesCount = 0;
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
if (m_Planes[planeIndex].m_Dismissed)
{
continue;
}
float weight = (float)m_Lobes[planeIndex].Alpha;
minWeight = Math.Min(minWeight, weight);
maxWeight = Math.Max(maxWeight, weight);
averageWeight += weight;
harmonicAverageWeight += 1.0f / Math.Max(1e-4f, weight);
float concentration = (float)m_Lobes[planeIndex].Concentration;
minConcentration = Math.Min(minConcentration, concentration);
maxConcentration = Math.Max(maxConcentration, concentration);
averageConcentration += concentration;
harmonicAverageConcentration += 1.0f / Math.Max(1e-4f, concentration);
validPlanesCount++;
}
validPlanesCount = Math.Max(1, validPlanesCount);
averageWeight /= validPlanesCount;
harmonicAverageWeight = validPlanesCount / harmonicAverageWeight;
averageConcentration /= validPlanesCount;
harmonicAverageConcentration = validPlanesCount / harmonicAverageConcentration;
// float dismissWeight = 0.5f / validPlanesCount;
float dismissWeight = floatTrackbarControlDismissFactor.Value * harmonicAverageWeight;
float dismissConcentration = floatTrackbarControlDismissFactor.Value * harmonicAverageConcentration;
// Select the N best planes/Dismiss others
if (!radioButtonUseBest.Checked)
{
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
bool dismissed = radioButtonDismissKappa.Checked ? m_Lobes[planeIndex].Concentration < dismissConcentration : m_Lobes[planeIndex].Alpha < dismissWeight;
if (dismissed)
{
m_Planes[planeIndex].m_Dismissed = true;
m_Planes[planeIndex].m_DismissalReason += " REJECTED";
}
}
}
// Dismiss planes that are totally clipped by others
DismissClippedPlanes();
// Dismiss planes above target count
List <SortedPlane> SortedPlanes = new List <SortedPlane>(planesCount);
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
SortedPlanes.Add(new SortedPlane()
{
planeIndex = planeIndex, weight = m_Planes[planeIndex].m_Dismissed ? 0.0f : m_Planes[planeIndex].m_Weight
});
}
SortedPlanes.Sort();
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
SortedPlane SP = SortedPlanes[planeIndex];
if (!m_Planes[SP.planeIndex].m_Dismissed && planeIndex >= integerTrackbarControlKeepBestPlanesCount.Value)
{
m_Planes[SP.planeIndex].m_Dismissed = true;
m_Planes[SP.planeIndex].m_DismissalReason += " LIMIT";
}
}
//////////////////////////////////////////////////////////////////////////
// Display info
string text = "";
text += "Min, Max, Avg, HAvg Weight = " + minWeight.ToString("G4") + ", " + maxWeight.ToString("G4") + ", " + averageWeight.ToString("G4") + ", " + harmonicAverageWeight.ToString("G4") + " > Dismiss weight = " + dismissWeight + "\r\n";
text += "Min, Max, Avg, HAvg Kappa = " + minConcentration.ToString("G4") + ", " + maxConcentration.ToString("G4") + ", " + averageConcentration.ToString("G4") + ", " + harmonicAverageConcentration.ToString("G4") + " > Dismiss kappa = " + dismissConcentration + "\r\n\r\n";
text += planesCount + " Planes:\r\n";
for (int planeIndex = 0; planeIndex < planesCount; planeIndex++)
{
text += "#" + planeIndex + " => { " + m_Lobes[planeIndex].Direction.x.ToString("G4") + ", " + m_Lobes[planeIndex].Direction.y.ToString("G4") + "} D = " + m_Planes[planeIndex].m_OrthoDistance.ToString("G4") + " - k=" + m_Lobes[planeIndex].Concentration.ToString("G4") + " - weight = " + m_Lobes[planeIndex].Alpha.ToString("G4") + m_Planes[planeIndex].m_DismissalReason + " " + (m_Planes[planeIndex].m_Dismissed ? "(DIS)" : "") + "\r\n";
}
textBoxPlanes.Text = text;
// Refresh
panelOutput.UpdateBitmap();
panelHistogram.UpdateBitmap();
}
public FittingForm()
{
// Random RNG = new Random();
// Vector From = new Vector( 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f ).Normalized;
// Vector To = new Vector( 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f, 2.0f * (float) RNG.NextDouble() - 1.0f ).Normalized;
// Matrix3x3 Pipo = MakeRot( From, To );
// Vector Test = Pipo * From;
//TestChromaRanges();
//TestSHRGBEEncoding();
TestSquareFilling();
InitializeComponent();
// Create the random points
List< Vector > RandomDirections = new List< Vector >();
List< float > RandomThetas = new List< float >();
for ( int LobeIndex=0; LobeIndex < m_RandomLobes.Length; LobeIndex++ )
{
float MainPhi = (float) (m_RandomLobes[LobeIndex].Phi * Math.PI / 180.0f);
float MainTheta = (float) (m_RandomLobes[LobeIndex].Theta * Math.PI / 180.0f);
float Concentration = m_RandomLobes[LobeIndex].Concentration;
int PointsCount = m_RandomLobes[LobeIndex].RandomPointsCount;
// Build the main direction for the target lobe
Vector MainDirection = new Vector(
(float) (Math.Sin( MainTheta ) * Math.Sin( MainPhi )),
(float) (Math.Cos( MainTheta )),
(float) (Math.Sin( MainTheta ) * Math.Cos( MainPhi ))
);
// Build the transform to bring Y-aligned points to the main direction
Matrix3x3 Rot = new Matrix3x3();
Rot.MakeRot( Vector.UnitY, MainDirection );
BuildDistributionMapping( Concentration, 0.0 );
// Draw random points in the Y-aligned hemisphere and transform them into the main direction
for ( int PointIndex=0; PointIndex < PointsCount; PointIndex++ )
{
double Theta = GetTheta();
float CosTheta = (float) Math.Cos( Theta );
// float SinTheta = (float) Math.Sqrt( 1.0f - CosTheta*CosTheta );
float SinTheta = (float) Math.Sin( Theta );
float Phi = (float) (WMath.SimpleRNG.GetUniform() * Math.PI);
Vector RandomDirection = new Vector(
(float) (SinTheta * Math.Sin( Phi )),
CosTheta,
(float) (SinTheta * Math.Cos( Phi ))
);
Vector FinalDirection = RandomDirection * Rot;
RandomDirections.Add( FinalDirection );
RandomThetas.Add( CosTheta );
}
}
m_RandomDirections = RandomDirections.ToArray();
m_RandomThetas = RandomThetas.ToArray();
panelOutput.UpdateBitmap();
panelOutputNormalDistribution.UpdateBitmap();
// Do it!
FitLobe[] Result = new FitLobe[FITTING_LOBES_COUNT];
for ( int h=0; h < Result.Length; h++ )
Result[h] = new FitLobe();
PerformExpectationMaximization( m_RandomDirections, Result );
}
