Skip to content

Commit 65b1f33

Browse files
thundercarrotthundercarrot
committed
Unify PlaneFit versions; clean up warnings
1 parent f44c96d commit 65b1f33

File tree

6 files changed

+35
-84
lines changed

6 files changed

+35
-84
lines changed

‎ProCamCalibration/KinectServer/KinectServer2.cs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ public KinectHandler()
6060
kinectSensor.Open();
6161
}
6262

63-
CoordinateMappingExample cooordinateMappingExample;
63+
//CoordinateMappingExample cooordinateMappingExample;
6464

6565
void CoordinateMapper_CoordinateMappingChanged(object sender, CoordinateMappingChangedEventArgs e)
6666
{

‎ProCamCalibration/ProCamEnsembleCalibration/CameraMath.cs

Lines changed: 18 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -394,7 +394,7 @@ public static void TestPlanarDLT()
394394
Console.WriteLine("test\n" + test);
395395
}
396396

397-
static public double PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t)
397+
static public void PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
398398
{
399399
int n = X.Count;
400400

@@ -426,6 +426,11 @@ static public double PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t)
426426
V2[i, 0] = V[i, 2];
427427
}
428428

429+
d2 = new Matrix(3, 1);
430+
d2[2] = d[0];
431+
d2[1] = d[1];
432+
d2[0] = d[2];
433+
429434
R = new Matrix(3, 3);
430435
R.Transpose(V2);
431436

@@ -436,8 +441,17 @@ static public double PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t)
436441
t.Mult(R, mu);
437442
t.Scale(-1);
438443

439-
// min eigenvalue is the sum of squared distances to the plane
440-
return d[0];
444+
// eigenvalues are the sum of squared distances in each direction
445+
// i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]
446+
447+
// compute the distance to the plane by transforming to the plane and take z-coordinate:
448+
// xPlane = R*x + t; distance = xPlane[2]
449+
}
450+
451+
static public void PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t)
452+
{
453+
Matrix d;
454+
PlaneFit(X, out R, out t, out d);
441455
}
442456

443457
public static Matrix Homography(List<Matrix> worldPoints, List<System.Drawing.PointF> imagePoints)
@@ -556,6 +570,7 @@ public static Matrix GaussianSample(Matrix mu, double sigma)
556570

557571
public static double NextGaussianSample(double mu, double sigma)
558572
{
573+
// Box-Muller transform
559574
const double epsilon = double.MinValue;
560575
const double tau = 2.0 * Math.PI;
561576

‎ProCamCalibration/ProCamEnsembleCalibration/ProjectorCameraEnsemble.cs

Lines changed: 10 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -702,15 +702,17 @@ public void CalibrateProjectorGroups(string directory)
702702
imagePointSubset.Add(pointSet.imagePoints[k]);
703703
}
704704

705-
// check that points are not coplanar
706-
Matrix X;
707-
double D;
708-
double ssdToPlane = PlaneFit(worldPointSubset, out X, out D);
705+
Matrix R, t;
706+
CameraMath.PlaneFit(worldPointSubset, out R, out t);
707+
709708
int numOutliers = 0;
709+
var x = new Matrix(3, 1);
710710
foreach (var point in worldPointSubset)
711711
{
712-
double distanceFromPlane = X.Dot(point) + D;
713-
if (Math.Abs(distanceFromPlane) > 0.1f)
712+
x.Mult(R, point);
713+
x.Add(t);
714+
double distanceFromPlane = Math.Abs(x[2]);
715+
if (distanceFromPlane > 0.1f)
714716
numOutliers++;
715717
}
716718
nonCoplanar = (numOutliers > worldPointSubset.Count * 0.10f);
@@ -799,7 +801,6 @@ public void CalibrateProjectorGroups(string directory)
799801
var worldPointInlierSet = new List<Matrix>();
800802
var imagePointInlierSet = new List<System.Drawing.PointF>();
801803

802-
//var R = Vision.Orientation.Rodrigues(rotations[setIndex]);
803804
var R = RotationMatrixFromRotationVector(rotations[setIndex]);
804805
var t = translations[setIndex];
805806
var p = new Matrix(3, 1);
@@ -1004,7 +1005,6 @@ public void OptimizePose()
10041005
R[ii, jj] = T[ii, jj];
10051006
}
10061007

1007-
//var r = Vision.Orientation.RotationVector(R);
10081008
var r = ProjectorCameraEnsemble.RotationVectorFromRotationMatrix(R);
10091009

10101010
for (int ii = 0; ii < 3; ii++)
@@ -1025,7 +1025,6 @@ public void OptimizePose()
10251025
R[ii, jj] = T[ii, jj];
10261026
}
10271027

1028-
//var r = Vision.Orientation.RotationVector(R);
10291028
var r = ProjectorCameraEnsemble.RotationVectorFromRotationMatrix(R);
10301029

10311030
for (int ii = 0; ii < 3; ii++)
@@ -1057,7 +1056,6 @@ public void OptimizePose()
10571056
r[0] = p[pi++];
10581057
r[1] = p[pi++];
10591058
r[2] = p[pi++];
1060-
//var R = Vision.Orientation.Rodrigues(r);
10611059
var R = RotationMatrixFromRotationVector(r);
10621060

10631061
var t = new Matrix(3, 1);
@@ -1082,7 +1080,6 @@ public void OptimizePose()
10821080
r[0] = p[pi++];
10831081
r[1] = p[pi++];
10841082
r[2] = p[pi++];
1085-
//var R = Vision.Orientation.Rodrigues(r);
10861083
var R = RotationMatrixFromRotationVector(r);
10871084

10881085
var t = new Matrix(3, 1);
@@ -1185,7 +1182,6 @@ public void OptimizePose()
11851182
r[0] = parameters[pi++];
11861183
r[1] = parameters[pi++];
11871184
r[2] = parameters[pi++];
1188-
//var R = Vision.Orientation.Rodrigues(r);
11891185
var R = RotationMatrixFromRotationVector(r);
11901186

11911187
var t = new Matrix(3, 1);
@@ -1210,7 +1206,6 @@ public void OptimizePose()
12101206
r[0] = parameters[pi++];
12111207
r[1] = parameters[pi++];
12121208
r[2] = parameters[pi++];
1213-
//var R = Vision.Orientation.Rodrigues(r);
12141209
var R = RotationMatrixFromRotationVector(r);
12151210

12161211
var t = new Matrix(3, 1);
@@ -1257,7 +1252,6 @@ static double CalibrateCamera(List<List<Matrix>> worldPointSets, List<List<Syste
12571252
Matrix R, t;
12581253
CameraMath.DLT(cameraMatrix, distCoeffs, worldPointSets[i], imagePointSets[i], out R, out t);
12591254

1260-
//var r = Vision.Orientation.RotationVector(R);
12611255
var r = ProjectorCameraEnsemble.RotationVectorFromRotationMatrix(R);
12621256

12631257
rotations.Add(r);
@@ -1319,7 +1313,6 @@ static double CalibrateCamera(List<List<Matrix>> worldPointSets, List<List<Syste
13191313
rotation[0] = p[pi++];
13201314
rotation[1] = p[pi++];
13211315
rotation[2] = p[pi++];
1322-
//var R = Vision.Orientation.Rodrigues(rotation);
13231316
var R = RotationMatrixFromRotationVector(rotation);
13241317

13251318
var t = new Matrix(3, 1);
@@ -1674,9 +1667,6 @@ static public void SaveToPly(string filename, Float3Image pts3D)
16741667
}
16751668
#endregion
16761669

1677-
1678-
1679-
16801670
public static Matrix RotationMatrixFromRotationVector(Matrix rotationVector)
16811671
{
16821672
double angle = rotationVector.Norm();
@@ -1709,56 +1699,8 @@ public static Matrix RotationVectorFromRotationMatrix(Matrix R)
17091699
return rotationVector;
17101700
}
17111701

1712-
static public double PlaneFit(IList<Matrix> points, out Matrix X, out double D)
1713-
{
1714-
X = new Matrix(3, 1);
1715-
1716-
var mu = new RoomAliveToolkit.Matrix(3, 1);
1717-
for (int i = 0; i < points.Count; i++)
1718-
mu.Add(points[i]);
1719-
mu.Scale(1f / (float)points.Count);
1720-
1721-
var A = new RoomAliveToolkit.Matrix(3, 3);
1722-
var pc = new RoomAliveToolkit.Matrix(3, 1);
1723-
var M = new RoomAliveToolkit.Matrix(3, 3);
1724-
for (int i = 0; i < points.Count; i++)
1725-
{
1726-
var p = points[i];
1727-
pc.Sub(p, mu);
1728-
M.Outer(pc, pc);
1729-
A.Add(M);
1730-
}
1731-
1732-
var V = new RoomAliveToolkit.Matrix(3, 3);
1733-
var d = new RoomAliveToolkit.Matrix(3, 1);
1734-
A.Eig(V, d); // TODO: replace with 3x3 version?
1735-
1736-
//Console.WriteLine("------");
1737-
//Console.WriteLine(A);
1738-
//Console.WriteLine(V);
1739-
//Console.WriteLine(d);
1740-
1741-
double minEigenvalue = Double.MaxValue;
1742-
int minEigenvaluei = 0;
1743-
for (int i = 0; i < 3; i++)
1744-
if (d[i] < minEigenvalue)
1745-
{
1746-
minEigenvalue = d[i];
1747-
minEigenvaluei = i;
1748-
}
1749-
1750-
X.CopyCol(V, minEigenvaluei);
1751-
1752-
D = -X.Dot(mu);
1753-
1754-
// min eigenvalue is the sum of squared distances to the plane
1755-
// signed distance is: double distance = X.Dot(point) + D;
1756-
1757-
return minEigenvalue;
1758-
}
1759-
1760-
1761-
1702+
1703+
17621704
//[STAThread]
17631705
//public static unsafe void Main()
17641706
//{

‎ProCamCalibration/ProjectionMappingSample/DepthAndColorShader.cs

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -293,7 +293,7 @@ public void Render(DeviceContext deviceContext, ShaderResourceView depthImageTex
293293
SamplerState colorSamplerState;
294294
SharpDX.Direct3D11.Buffer constantBuffer;
295295
BilateralFilter bilateralFilter;
296-
FromUIntPS fromUIntPS;
296+
//FromUIntPS fromUIntPS;
297297
Texture2D filteredDepthImageTexture, filteredDepthImageTexture2;
298298
RenderTargetView filteredRenderTargetView, filteredRenderTargetView2;
299299
ShaderResourceView filteredDepthImageSRV, filteredDepthImageSRV2;

‎ProCamCalibration/ProjectionMappingSample/ProjectionMappingSample.cs

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -240,7 +240,7 @@ void RenderLoop()
240240
float dz = handLeftCameraSpacePoint.Z - handRightCameraSpacePoint.Z;
241241
distanceSquared = dx * dx + dy * dy + dz * dz;
242242
}
243-
var transform = SharpDX.Matrix.RotationY((float)Math.PI) * SharpDX.Matrix.Translation(-0.25f, 0.45f, 0);
243+
var transform = SharpDX.Matrix.RotationY((float)Math.PI) * SharpDX.Matrix.Translation(0, 0.45f, 0);
244244
headPosition = SharpDX.Vector3.TransformCoordinate(headPosition, transform);
245245

246246
if (trackingValid && (distanceSquared < 0.02f) && (alpha > 1))
@@ -724,18 +724,18 @@ public void Render(DeviceContext deviceContext)
724724
deviceContext.Draw((Kinect2Calibration.depthImageWidth - 1) * (Kinect2Calibration.depthImageHeight - 1) * 6, 0);
725725
}
726726

727-
bool live = false;
727+
//bool live = false;
728728

729729
public void StartLive()
730730
{
731-
live = true;
731+
//live = true;
732732
//new System.Threading.Thread(ColorCameraLoop).Start();
733733
new System.Threading.Thread(DepthCameraLoop).Start();
734734
}
735735

736736
public void StopLive()
737737
{
738-
live = false;
738+
//live = false;
739739
}
740740

741741

‎ProCamCalibration/ProjectionMappingSample/ProjectiveTexturingShader.cs

Lines changed: 1 addition & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,7 @@ public void Render(DeviceContext deviceContext, ShaderResourceView depthImageTex
105105
{
106106
deviceContext.InputAssembler.InputLayout = vertexInputLayout;
107107
deviceContext.InputAssembler.PrimitiveTopology = SharpDX.Direct3D.PrimitiveTopology.TriangleList;
108-
deviceContext.InputAssembler.SetVertexBuffers(0, new VertexBufferBinding(vertexBuffer, VertexPosition.SizeInBytes, 0)); // bytes per vertex
108+
deviceContext.InputAssembler.SetVertexBuffers(0, new VertexBufferBinding(vertexBuffer, 16, 0)); // bytes per vertex
109109
deviceContext.OutputMerger.SetTargets(depthStencilView, renderTargetView);
110110
deviceContext.OutputMerger.DepthStencilState = depthStencilState;
111111
deviceContext.Rasterizer.State = rasterizerState;
@@ -123,12 +123,6 @@ public void Render(DeviceContext deviceContext, ShaderResourceView depthImageTex
123123
deviceContext.PixelShader.SetShaderResource(0, null);
124124
}
125125

126-
struct VertexPosition
127-
{
128-
public SharpDX.Vector4 position;
129-
static public int SizeInBytes { get { return 4 * 4; } }
130-
}
131-
132126
VertexShader vertexShader;
133127
GeometryShader geometryShader;
134128
PixelShader pixelShader;

0 commit comments

Comments
 (0)