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 ); }