private static void SetRealVertices(Workspace workspace)
{
Vector<double> fittedPlaneVector = GeometryHelper.FitPlaneToPoints(workspace.PointCloud.ToArray());
if (fittedPlaneVector == null)
{
return;
}
Point3D projectedPoint = GeometryHelper.ProjectPoint3DToPlane(workspace.PointCloud.First(), fittedPlaneVector);
Vector<double> planeNormal = new DenseVector(new[] { fittedPlaneVector[0], fittedPlaneVector[1], fittedPlaneVector[2] });
Point3D[] vertices3D = workspace.Vertices3D;
Point[] vertices = workspace.Vertices.ToArray();
for (int i = 0; i < vertices.Length; i++)
{
Vector<double> pointOnPlane = new DenseVector(new[] { projectedPoint.X, projectedPoint.Y, projectedPoint.Z });
Vector<double> pointOnLine = new DenseVector(new double[] { vertices3D[i].X, vertices3D[i].Y, vertices3D[i].Z });
double d = (pointOnPlane.Subtract(pointOnLine)).DotProduct(planeNormal) / (pointOnLine.DotProduct(planeNormal));
Vector<double> intersection = pointOnLine + pointOnLine.Multiply(d);
workspace.FittedVertices[i] = new Point3D(intersection[0], intersection[1], intersection[2]);
}
workspace.PlaneVector = fittedPlaneVector;
}
public void SetWorkspace(Workspace workspaceSource)
{
ActiveWorkspace = workspaceSource;
Dispatcher.Invoke(() =>
{
SetCameraCenterAndShowCloud(MainViewPort, ActiveWorkspace);
DrawFittedPlane();
});
}
public void AddOrUpdateWorkspace(string workspaceId, Workspace workspace, KinectClient client)
{
if (!workspaceDictionary.Keys.Contains(workspaceId))
{
workspaceDictionary.Add(workspaceId, workspace);
workspaceClientDictionary.Add(workspaceId, client);
}
else
{
workspaceDictionary[workspace.ID] = workspace;
}
}
public void AddWorkspace(Workspace workspace, KinectClient client)
{
StateObject state = clientStateObjectDictionary[client];
if (state == null)
{
state = stateObjectClientDictionary.Keys.First();
}
WorkspaceMessage message = new WorkspaceMessage()
{
ID = workspace.ID,
Name = workspace.Name,
Vertices = workspace.Vertices.ToArray()
};
SerializeAndSendMessage(message, state.WorkSocket);
}
public MainWindow()
{
InitializeComponent();
kinectServer = KinectServer.Instance;
serverMessageProcessor = ServerMessageProcessor.Instance;
serverMessageProcessor.WorkspaceUpdated += kinectServer_WorkspaceUpdated;
serverMessageProcessor.DepthMessageArrived += kinectServer_DepthDataArrived;
serverMessageProcessor.TextMessageArrived += kinectServer_TextMessageArrived;
activeWorkspace = new Workspace()
{
Name = "asd",
Vertices = new ObservableCollection<Point>
{
new Point(0,0),
new Point(0,50),
new Point(50,50),
new Point(50,0)
}
};
AddCameraWorkspace();
cloudView = new CloudView();
WorkspacePointCloudHolder.Children.Add(cloudView);
bodyView = new BodyView();
//HandCheckBodyViewHolder.Children.Add(bodyView);
roomPointCloudView = new RoomPointCloudView();
RoomPointCloudHolder.Children.Add(roomPointCloudView);
calibrationView = new CalibrationView();
CalibrationViewHolder.Children.Add(calibrationView);
WorkspaceList.ItemsSource = workspaceList;
EditWorkspace.DataContext = activeWorkspace;
}
public static void SetWorkspaceCloudRealVerticesAndCenter(Workspace workspace, FrameSize depthFrameSize)
{
dataStore = DataStore.Instance;
KinectClient client = dataStore.GetClientForWorkspace(workspace.ID);
double sumX = 0;
double sumY = 0;
double sumZ = 0;
double numberOfPoints = 0;
Point[] workspaceVertices = {
new Point(workspace.Vertices[0].X, workspace.Vertices[0].Y),
new Point(workspace.Vertices[1].X, workspace.Vertices[1].Y),
new Point(workspace.Vertices[2].X, workspace.Vertices[2].Y),
new Point(workspace.Vertices[3].X, workspace.Vertices[3].Y),
};
List<Point3D> pointCloud = new List<Point3D>();
NullablePoint3D[] clientPointCloud = dataStore.GetPointCloudForClient(client);
for (int i = 0; i < clientPointCloud.Count(); i++)
{
if (GeometryHelper.InsidePolygon(workspaceVertices, new Point(i%depthFrameSize.Width, i/depthFrameSize.Width)))
{
NullablePoint3D point = clientPointCloud[i];
if (point != null)
{
double x = point.X;
double y = point.Y;
double z = point.Z;
sumX += x;
sumY += y;
sumZ += z;
numberOfPoints += 1;
pointCloud.Add(new Point3D(point.X, point.Y, point.Z));
}
}
}
workspace.Center = new Point3D(sumX / numberOfPoints, sumY / numberOfPoints, sumZ / numberOfPoints);
workspace.PointCloud = pointCloud.ToArray();
SetRealVertices(workspace);
}
private static void SetRealVertices(Workspace workspace)
{
Vector<double> fittedPlaneVector = GeometryHelper.FitPlaneToPoints(workspace.PointCloud.ToArray());
if (fittedPlaneVector == null)
{
return;
}
Point3D projectedPoint = GeometryHelper.ProjectPoint3DToPlane(workspace.PointCloud.First(), fittedPlaneVector);
Vector<double> planeNormal = new DenseVector(new[] { fittedPlaneVector[0], fittedPlaneVector[1], fittedPlaneVector[2] });
CameraSpacePoint[] csps = { new CameraSpacePoint() };
Point[] vertices = workspace.Vertices.ToArray();
for (int i = 0; i < vertices.Length; i++)
{
Point vertex = vertices[i];
KinectStreamer.Instance.CoordinateMapper.MapDepthPointsToCameraSpace(
new[] {
new DepthSpacePoint {
X = (float)vertex.X,
Y = (float)vertex.Y
}
},
new ushort[] { 1 }, csps);
Vector<double> pointOnPlane = new DenseVector(new[] { projectedPoint.X, projectedPoint.Y, projectedPoint.Z });
Vector<double> pointOnLine = new DenseVector(new double[] { csps[0].X, csps[0].Y, csps[0].Z });
double d = (pointOnPlane.Subtract(pointOnLine)).DotProduct(planeNormal) / (pointOnLine.DotProduct(planeNormal));
Vector<double> intersection = pointOnLine + pointOnLine.Multiply(d);
workspace.FittedVertices[i] = new Point3D(intersection[0], intersection[1], intersection[2]);
}
workspace.PlaneVector = fittedPlaneVector;
}
public static Workspace ProcessWorkspace(Workspace workspace)
{
Workspace newWorkspace = workspace;
//if (newWorkspace.PointCloud == null)
//{
// SetWorkspaceCloudAndCenter(newWorkspace);
//}
//SetRealVertices(newWorkspace);
Point[] vertices = workspace.Vertices.ToArray();
workspace.VertexDepths = new ushort[vertices.Length];
CameraSpacePoint[] csps = { new CameraSpacePoint() };
for (int i = 0; i < vertices.Length; i++)
{
Point vertex = vertices[i];
workspace.VertexDepths[i] = (ushort) (
KinectStreamer.Instance.DepthPixels[
(int)(vertex.Y*KinectStreamer.Instance.DepthFrameDescription.Width) + (int)vertex.X] * MapDepthToByte);
KinectStreamer.Instance.CoordinateMapper.MapDepthPointsToCameraSpace(
new[] {
new DepthSpacePoint {
X = (float)vertex.X,
Y = (float)vertex.Y
}
},
new ushort[] { workspace.VertexDepths[i] }, csps);
newWorkspace.Vertices3D[i] = new Point3D(csps[0].X, csps[0].Y, csps[0].Z);
}
return newWorkspace;
}
private void WorkspaceList_SelectionChanged(object sender, SelectionChangedEventArgs e)
{
if ((Workspace)WorkspaceList.SelectedItem == null)
{
return;
}
activeWorkspace = (Workspace)WorkspaceList.SelectedItem;
cloudView.SetWorkspace(activeWorkspace);
EditWorkspace.DataContext = activeWorkspace;
}
private void RemoveWorkspace(object sender, RoutedEventArgs e)
{
Workspace workspace = (Workspace)(((ListBoxItem)WorkspaceList.ContainerFromElement((Button)sender)).Content);
workspaceList.Remove(workspace);
dataStore.DeleteWorkspace(workspace);
activeWorkspace = new Workspace();
cloudView.ClearScreen();
}
private void AddWorkspace(object sender, RoutedEventArgs e)
{
KinectClient activeClient = defineWorkspaceView.ActiveClient;
if (!workspaceList.Contains(activeWorkspace))
{
workspaceList.Add(activeWorkspace);
dataStore.AddOrUpdateWorkspace(activeWorkspace.ID, activeWorkspace, activeClient);
}
kinectServer.AddWorkspace(activeWorkspace, activeClient);
activeWorkspace = new Workspace();
EditWorkspace.DataContext = activeWorkspace;
WorkspaceList.Items.Refresh();
}
public void DeleteWorkspace(Workspace workspace)
{
workspaceDictionary.Remove(workspace.ID);
workspaceClientDictionary.Remove(workspace.ID);
}
public CloudView()
{
ActiveWorkspace = new Workspace();
InitializeComponent();
}
private void SetCameraCenterAndShowCloud(Viewport3D viewport, Workspace workspace)
{
ClearScreen();
if (workspace.PointCloud == null)
{
MessageBox.Show("Workspace has no point cloud");
return;
}
foreach (Point3D point in workspace.PointCloud)
{
DrawTriangle(viewport, point, Colors.Black);
}
Point3D center = workspace.Center;
XRotation.CenterX = center.X;
XRotation.CenterY = center.Y;
XRotation.CenterZ = center.Z;
YRotation.CenterX = center.X;
YRotation.CenterY = center.Y;
YRotation.CenterZ = center.Z;
Scale.CenterX = center.X;
Scale.CenterY = center.Y;
Scale.CenterZ = center.Z;
Camera.Position = new Point3D(center.X, center.Y, -5);
}
private static void SetWorkspaceCloudAndCenter(Workspace workspace)
{
//AllCameraSpacePoints = KinectStreamer.Instance.GenerateFullPointCloud();
//double sumX = 0;
//double sumY = 0;
//double sumZ = 0;
//double numberOfPoints = 0;
//List<Point3D> cameraSpacePoints = new List<Point3D>();
//List<DepthSpacePoint> dspList = new List<DepthSpacePoint>();
//foreach (CameraSpacePoint csp in AllCameraSpacePoints)
//{
// if (GeometryHelper.IsValidPoint(csp))
// {
// DepthSpacePoint dsp = KinectStreamer.Instance.CoordinateMapper.MapCameraPointToDepthSpace(csp);
// dspList.Add(dsp);
// if (GeometryHelper.InsidePolygon(workspace.Vertices.ToArray(), new Point(dsp.X, dsp.Y)))
// {
// double x = csp.X;
// double y = csp.Y;
// double z = csp.Z;
// sumX += x;
// sumY += y;
// sumZ += z;
// numberOfPoints += 1;
// cameraSpacePoints.Add(new Point3D(csp.X, csp.Y, csp.Z));
// }
// }
//}
//workspace.Center = new Point3D(sumX / numberOfPoints, sumY / numberOfPoints, sumZ / numberOfPoints);
//workspace.PointCloud = cameraSpacePoints.ToArray();
}
