mirror of
https://github.com/storytold/LiveScan3D.git
synced 2026-10-09 00:09:58 +00:00
Initial commit
This commit is contained in:
marek
@@ -0,0 +1,524 @@
|
||||
// Copyright (C) 2015 Marek Kowalski (M.Kowalski@ire.pw.edu.pl), Jacek Naruniec (J.Naruniec@ire.pw.edu.pl)
|
||||
// License: MIT Software License See LICENSE.txt for the full license.
|
||||
|
||||
// If you use this software in your research, then please use the following citation:
|
||||
|
||||
// Kowalski, M.; Naruniec, J.; Daniluk, M.: "LiveScan3D: A Fast and Inexpensive 3D Data
|
||||
// Acquisition System for Multiple Kinect v2 Sensors". in 3D Vision (3DV), 2015 International Conference on, Lyon, France, 2015
|
||||
|
||||
// @INPROCEEDINGS{Kowalski15,
|
||||
// author={Kowalski, M. and Naruniec, J. and Daniluk, M.},
|
||||
// booktitle={3D Vision (3DV), 2015 International Conference on},
|
||||
// title={LiveScan3D: A Fast and Inexpensive 3D Data Acquisition System for Multiple Kinect v2 Sensors},
|
||||
// year={2015},
|
||||
// }
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.ComponentModel;
|
||||
using System.Data;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using System.Threading;
|
||||
using System.Windows.Forms;
|
||||
using System.Globalization;
|
||||
using System.Runtime.Serialization;
|
||||
|
||||
using System.IO;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Timers;
|
||||
|
||||
|
||||
namespace KinectServer
|
||||
{
|
||||
public partial class MainWindowForm : Form
|
||||
{
|
||||
[DllImport("ICP.dll")]
|
||||
static extern float ICP(IntPtr verts1, IntPtr verts2, int nVerts1, int nVerts2, float[] R, float[] t, int maxIter = 200);
|
||||
|
||||
KinectServer oServer;
|
||||
|
||||
//Those three variables are shared with the OpenGLWindow class and are used to exchange data with it.
|
||||
//Vertices from all of the sensors
|
||||
List<float> lAllVertices = new List<float>();
|
||||
//Color data from all of the sensors
|
||||
List<byte> lAllColors = new List<byte>();
|
||||
//Sensor poses from all of the sensors
|
||||
List<AffineTransform> lAllCameraPoses = new List<AffineTransform>();
|
||||
|
||||
bool bServerRunning = false;
|
||||
bool bRecording = false;
|
||||
bool bSaving = false;
|
||||
|
||||
//Live view open or not
|
||||
bool bLiveViewRunning = false;
|
||||
|
||||
System.Timers.Timer oStatusBarTimer = new System.Timers.Timer();
|
||||
|
||||
KinectSettings oSettings = new KinectSettings();
|
||||
//The live view window class
|
||||
OpenGLWindow oOpenGLWindow;
|
||||
|
||||
public MainWindowForm()
|
||||
{
|
||||
//This tries to read the settings from "settings.bin", if it failes the settings stay at default values.
|
||||
try
|
||||
{
|
||||
IFormatter formatter = new System.Runtime.Serialization.Formatters.Binary.BinaryFormatter();
|
||||
Stream stream = new FileStream("settings.bin", FileMode.Open, FileAccess.Read);
|
||||
oSettings = (KinectSettings)formatter.Deserialize(stream);
|
||||
stream.Close();
|
||||
}
|
||||
catch(Exception)
|
||||
{
|
||||
}
|
||||
|
||||
oServer = new KinectServer(oSettings);
|
||||
oServer.eSocketListChanged += new SocketListChangedHandler(UpdateListView);
|
||||
|
||||
InitializeComponent();
|
||||
}
|
||||
|
||||
private void Form1_FormClosing(object sender, FormClosingEventArgs e)
|
||||
{
|
||||
//The current settings are saved to a files.
|
||||
IFormatter formatter = new System.Runtime.Serialization.Formatters.Binary.BinaryFormatter();
|
||||
|
||||
Stream stream = new FileStream("settings.bin", FileMode.Create, FileAccess.Write);
|
||||
formatter.Serialize(stream, oSettings);
|
||||
stream.Close();
|
||||
|
||||
oServer.StopServer();
|
||||
}
|
||||
|
||||
//Starts the server
|
||||
private void btStart_Click(object sender, EventArgs e)
|
||||
{
|
||||
bServerRunning = !bServerRunning;
|
||||
|
||||
if (bServerRunning)
|
||||
{
|
||||
oServer.StartServer();
|
||||
btStart.Text = "Stop server";
|
||||
}
|
||||
else
|
||||
{
|
||||
oServer.StopServer();
|
||||
btStart.Text = "Start server";
|
||||
}
|
||||
}
|
||||
|
||||
//Opens the settings form
|
||||
private void btSettings_Click(object sender, EventArgs e)
|
||||
{
|
||||
SettingsForm form = new SettingsForm();
|
||||
form.oSettings = oSettings;
|
||||
form.oServer = oServer;
|
||||
form.Show();
|
||||
}
|
||||
|
||||
//Performs recording which is synchronized frame capture.
|
||||
//The frames are downloaded from the clients and saved once recording is finished.
|
||||
private void recordingWorker_DoWork(object sender, DoWorkEventArgs e)
|
||||
{
|
||||
oServer.ClearStoredFrames();
|
||||
|
||||
int nCaptured = 0;
|
||||
BackgroundWorker worker = (BackgroundWorker)sender;
|
||||
while (!worker.CancellationPending)
|
||||
{
|
||||
oServer.CaptureSynchronizedFrame();
|
||||
|
||||
nCaptured++;
|
||||
SetStatusBarOnTimer("Captured frame " + (nCaptured).ToString() + ".", 5000);
|
||||
}
|
||||
}
|
||||
|
||||
private void recordingWorker_RunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e)
|
||||
{
|
||||
//After recording has been terminated it is time to begin saving the frames.
|
||||
//Saving is downloading the frames from clients and saving them locally.
|
||||
bSaving = true;
|
||||
|
||||
btRecord.Text = "Stop saving";
|
||||
btRecord.Enabled = true;
|
||||
|
||||
savingWorker.RunWorkerAsync();
|
||||
}
|
||||
|
||||
//Opens the live view window
|
||||
private void OpenGLWorker_DoWork(object sender, DoWorkEventArgs e)
|
||||
{
|
||||
bLiveViewRunning = true;
|
||||
oOpenGLWindow = new OpenGLWindow();
|
||||
|
||||
//The variables below are shared between this class and the OpenGLWindow.
|
||||
lock (lAllVertices)
|
||||
{
|
||||
oOpenGLWindow.vertices = lAllVertices;
|
||||
oOpenGLWindow.colors = lAllColors;
|
||||
oOpenGLWindow.cameraPoses = lAllCameraPoses;
|
||||
oOpenGLWindow.settings = oSettings;
|
||||
}
|
||||
oOpenGLWindow.Run();
|
||||
}
|
||||
|
||||
private void OpenGLWorker_RunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e)
|
||||
{
|
||||
bLiveViewRunning = false;
|
||||
updateWorker.CancelAsync();
|
||||
}
|
||||
|
||||
private void savingWorker_DoWork(object sender, DoWorkEventArgs e)
|
||||
{
|
||||
int nFrames = 0;
|
||||
|
||||
string outDir = "out" + "\\" + txtSeqName.Text + "\\";
|
||||
DirectoryInfo di = Directory.CreateDirectory(outDir);
|
||||
|
||||
BackgroundWorker worker = (BackgroundWorker)sender;
|
||||
//This loop is running till it is either cancelled (using the btRecord button), or till there are no more stored frames.
|
||||
while (!worker.CancellationPending)
|
||||
{
|
||||
List<List<byte>> lFrameRGBAllDevices = new List<List<byte>>();
|
||||
List<List<float>> lFrameVertsAllDevices = new List<List<float>>();
|
||||
|
||||
oServer.GetStoredFrame(lFrameRGBAllDevices, lFrameVertsAllDevices);
|
||||
|
||||
nFrames++;
|
||||
int nVerticesTotal = 0;
|
||||
for (int i = 0; i < lFrameRGBAllDevices.Count; i++)
|
||||
{
|
||||
nVerticesTotal += lFrameVertsAllDevices[i].Count;
|
||||
}
|
||||
|
||||
//This indicates that there are no more stored frames.
|
||||
if (nVerticesTotal == 0)
|
||||
break;
|
||||
|
||||
List<byte> lFrameRGB = new List<byte>();
|
||||
List<Single> lFrameVerts = new List<Single>();
|
||||
|
||||
SetStatusBarOnTimer("Saving frame " + (nFrames).ToString() + ".", 5000);
|
||||
for (int i = 0; i < lFrameRGBAllDevices.Count; i++)
|
||||
{
|
||||
lFrameRGB.AddRange(lFrameRGBAllDevices[i]);
|
||||
lFrameVerts.AddRange(lFrameVertsAllDevices[i]);
|
||||
|
||||
//This is ran if the frames from each client are to be placed in separate files.
|
||||
if (!oSettings.bMergeScansForSave)
|
||||
{
|
||||
string outputFilename = outDir + "\\" + nFrames.ToString().PadLeft(5, '0') + i.ToString() + ".ply";
|
||||
saveToPly(outputFilename, lFrameVertsAllDevices[i], lFrameRGBAllDevices[i]);
|
||||
}
|
||||
}
|
||||
|
||||
//This is ran if the frames from all clients are to be placed in a single file.
|
||||
if (oSettings.bMergeScansForSave)
|
||||
{
|
||||
string outputFilename = outDir + "\\" + nFrames.ToString().PadLeft(5, '0') + ".ply";
|
||||
saveToPly(outputFilename, lFrameVerts, lFrameRGB);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void savingWorker_RunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e)
|
||||
{
|
||||
oServer.ClearStoredFrames();
|
||||
bSaving = false;
|
||||
|
||||
//If the live view window was open, we need to restart the UpdateWorker.
|
||||
if (bLiveViewRunning)
|
||||
RestartUpdateWorker();
|
||||
|
||||
btRecord.Enabled = true;
|
||||
btRecord.Text = "Start recording";
|
||||
btRefineCalib.Enabled = true;
|
||||
btCalibrate.Enabled = true;
|
||||
}
|
||||
|
||||
//Continually requests frames that will be displayed in the live view window.
|
||||
private void updateWorker_DoWork(object sender, DoWorkEventArgs e)
|
||||
{
|
||||
List<List<byte>> lFramesRGB = new List<List<byte>>();
|
||||
List<List<Single>> lFramesVerts = new List<List<Single>>();
|
||||
|
||||
BackgroundWorker worker = (BackgroundWorker)sender;
|
||||
while (!worker.CancellationPending)
|
||||
{
|
||||
Thread.Sleep(1);
|
||||
oServer.GetLatestFrame(lFramesRGB, lFramesVerts);
|
||||
|
||||
//Update the vertex and color lists that are common between this class and the OpenGLWindow.
|
||||
lock (lAllVertices)
|
||||
{
|
||||
lAllVertices.Clear();
|
||||
lAllColors.Clear();
|
||||
lAllCameraPoses.Clear();
|
||||
|
||||
for (int i = 0; i < lFramesRGB.Count; i++)
|
||||
{
|
||||
lAllVertices.AddRange(lFramesVerts[i]);
|
||||
lAllColors.AddRange(lFramesRGB[i]);
|
||||
lAllCameraPoses.Add(oServer.lCameraPoses[i]);
|
||||
}
|
||||
}
|
||||
|
||||
//Notes the fact that a new frame was downloaded, this is used to estimate the FPS.
|
||||
if (oOpenGLWindow != null)
|
||||
oOpenGLWindow.CloudUpdateTick();
|
||||
}
|
||||
}
|
||||
|
||||
//Performs the ICP based pose refinement.
|
||||
private void refineWorker_DoWork(object sender, DoWorkEventArgs e)
|
||||
{
|
||||
if (oServer.bAllCalibrated == false)
|
||||
{
|
||||
SetStatusBarOnTimer("Not all of the devices are calibrated.", 5000);
|
||||
return;
|
||||
}
|
||||
|
||||
//Download a frame from each client.
|
||||
List<List<float>> lAllFrameVertices = new List<List<float>>();
|
||||
List<List<byte>> lAllFrameColors = new List<List<byte>>();
|
||||
oServer.GetLatestFrame(lAllFrameColors, lAllFrameVertices);
|
||||
|
||||
//Initialize containers for the poses.
|
||||
List<float[]> Rs = new List<float[]>();
|
||||
List<float[]> Ts = new List<float[]>();
|
||||
for (int i = 0; i < lAllFrameVertices.Count; i++)
|
||||
{
|
||||
float[] tempR = new float[9];
|
||||
float[] tempT = new float[3];
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
tempT[j] = 0;
|
||||
tempR[j + j * 3] = 1;
|
||||
}
|
||||
|
||||
Rs.Add(tempR);
|
||||
Ts.Add(tempT);
|
||||
}
|
||||
|
||||
//Use ICP to refine the sensor poses.
|
||||
//This part is explained in more detail in our article (name on top of this file).
|
||||
float error = 1;
|
||||
float oldError = 1;
|
||||
bool firstPass = true;
|
||||
int iter = 0;
|
||||
while (true)
|
||||
{
|
||||
if (iter > oSettings.nMaxNumRefineIters)
|
||||
break;
|
||||
|
||||
error = 0;
|
||||
for (int i = 0; i < lAllFrameVertices.Count; i++)
|
||||
{
|
||||
List<float> otherFramesVertices = new List<float>();
|
||||
for (int j = 0; j < lAllFrameVertices.Count; j++)
|
||||
{
|
||||
if (j == i)
|
||||
continue;
|
||||
otherFramesVertices.AddRange(lAllFrameVertices[j]);
|
||||
}
|
||||
|
||||
float[] verts1 = otherFramesVertices.ToArray();
|
||||
float[] verts2 = lAllFrameVertices[i].ToArray();
|
||||
|
||||
IntPtr pVerts1 = Marshal.AllocHGlobal(otherFramesVertices.Count * sizeof(float));
|
||||
IntPtr pVerts2 = Marshal.AllocHGlobal(lAllFrameVertices[i].Count * sizeof(float));
|
||||
|
||||
Marshal.Copy(verts1, 0, pVerts1, verts1.Length);
|
||||
Marshal.Copy(verts2, 0, pVerts2, verts2.Length);
|
||||
|
||||
error += ICP(pVerts1, pVerts2, otherFramesVertices.Count / 3, lAllFrameVertices[i].Count / 3, Rs[i], Ts[i], oSettings.nNumICPIterations);
|
||||
|
||||
Marshal.Copy(pVerts2, verts2, 0, verts2.Length);
|
||||
lAllFrameVertices[i].Clear();
|
||||
lAllFrameVertices[i].AddRange(verts2);
|
||||
}
|
||||
|
||||
if (firstPass)
|
||||
{
|
||||
firstPass = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Math.Abs(oldError - error) / error < 0.01)
|
||||
break;
|
||||
}
|
||||
oldError = error;
|
||||
|
||||
iter++;
|
||||
}
|
||||
|
||||
//Update the calibration data in client machines.
|
||||
List<AffineTransform> worldTransforms = oServer.lWorldTransforms;
|
||||
|
||||
for (int i = 0; i < worldTransforms.Count; i++)
|
||||
{
|
||||
float[] tempT = new float[3];
|
||||
float[,] tempR = new float[3, 3];
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
tempT[j] += Ts[i][k] * worldTransforms[i].R[k, j];
|
||||
}
|
||||
|
||||
worldTransforms[i].t[j] += tempT[j];
|
||||
}
|
||||
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
for (int l = 0; l < 3; l++)
|
||||
{
|
||||
tempR[j, k] += Rs[i][l * 3 + j] * worldTransforms[i].R[l, k];
|
||||
}
|
||||
|
||||
worldTransforms[i].R[j, k] = tempR[j, k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
oServer.lWorldTransforms = worldTransforms;
|
||||
|
||||
oServer.SendCalibrationData();
|
||||
}
|
||||
|
||||
private void refineWorker_RunWorkerCompleted(object sender, RunWorkerCompletedEventArgs e)
|
||||
{
|
||||
//Re-enable all of the buttons after refinement.
|
||||
btRefineCalib.Enabled = true;
|
||||
btCalibrate.Enabled = true;
|
||||
btRecord.Enabled = true;
|
||||
}
|
||||
|
||||
//This is used for: starting/stopping the recording worker, stopping the saving worker
|
||||
private void btRecord_Click(object sender, EventArgs e)
|
||||
{
|
||||
if (oServer.nClientCount < 1)
|
||||
{
|
||||
SetStatusBarOnTimer("At least one client needs to be connected for recording.", 5000);
|
||||
return;
|
||||
}
|
||||
|
||||
//If we are saving frames right now, this button stops saving.
|
||||
if (bSaving)
|
||||
{
|
||||
btRecord.Enabled = false;
|
||||
savingWorker.CancelAsync();
|
||||
return;
|
||||
}
|
||||
|
||||
bRecording = !bRecording;
|
||||
if (bRecording)
|
||||
{
|
||||
//Stop the update worker to reduce the network usage (provides better synchronization).
|
||||
updateWorker.CancelAsync();
|
||||
|
||||
recordingWorker.RunWorkerAsync();
|
||||
btRecord.Text = "Stop recording";
|
||||
btRefineCalib.Enabled = false;
|
||||
btCalibrate.Enabled = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
btRecord.Enabled = false;
|
||||
recordingWorker.CancelAsync();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private void btCalibrate_Click(object sender, EventArgs e)
|
||||
{
|
||||
oServer.Calibrate();
|
||||
}
|
||||
|
||||
private void btRefineCalib_Click(object sender, EventArgs e)
|
||||
{
|
||||
if (oServer.nClientCount < 2)
|
||||
{
|
||||
SetStatusBarOnTimer("To refine calibration you need at least 2 connected devices.", 5000);
|
||||
return;
|
||||
}
|
||||
|
||||
btRefineCalib.Enabled = false;
|
||||
btCalibrate.Enabled = false;
|
||||
btRecord.Enabled = false;
|
||||
|
||||
refineWorker.RunWorkerAsync();
|
||||
}
|
||||
|
||||
private void saveToPly(string filename, List<Single> vertices, List<byte> colors)
|
||||
{
|
||||
int nVertices = vertices.Count / 3;
|
||||
|
||||
System.IO.StreamWriter file = new System.IO.StreamWriter(filename);
|
||||
|
||||
//PLY file header is written here.
|
||||
file.WriteLine("ply\nformat ascii 1.0");
|
||||
file.WriteLine("element vertex " + nVertices.ToString());
|
||||
file.WriteLine("property float x\nproperty float y\nproperty float z\nproperty uchar red\nproperty uchar green\nproperty uchar blue\nend_header");
|
||||
|
||||
//Vertex and color data are written here.
|
||||
for (int j = 0; j < vertices.Count / 3; j++)
|
||||
{
|
||||
string s = "";
|
||||
for (int k = 0; k < 3; k++) s += vertices[j * 3 + k].ToString(CultureInfo.InvariantCulture) + " ";
|
||||
for (int k = 0; k < 3; k++) s += colors[j * 3 + k].ToString(CultureInfo.InvariantCulture) + " ";
|
||||
file.WriteLine(s);
|
||||
}
|
||||
|
||||
file.Close();
|
||||
}
|
||||
|
||||
void RestartUpdateWorker()
|
||||
{
|
||||
if (!updateWorker.IsBusy)
|
||||
updateWorker.RunWorkerAsync();
|
||||
}
|
||||
|
||||
private void btShowLive_Click(object sender, EventArgs e)
|
||||
{
|
||||
RestartUpdateWorker();
|
||||
|
||||
//Opens the live view window if it is not open yet.
|
||||
if (!OpenGLWorker.IsBusy)
|
||||
OpenGLWorker.RunWorkerAsync();
|
||||
}
|
||||
|
||||
private void SetStatusBarOnTimer(string message, int milliseconds)
|
||||
{
|
||||
statusLabel.Text = message;
|
||||
|
||||
oStatusBarTimer.Stop();
|
||||
oStatusBarTimer = new System.Timers.Timer();
|
||||
|
||||
oStatusBarTimer.Interval = milliseconds;
|
||||
oStatusBarTimer.Elapsed += delegate(object sender, System.Timers.ElapsedEventArgs e)
|
||||
{
|
||||
oStatusBarTimer.Stop();
|
||||
statusLabel.Text = "";
|
||||
};
|
||||
oStatusBarTimer.Start();
|
||||
}
|
||||
|
||||
//Updates the ListBox contaning the connected clients, called by events inside KinectServer.
|
||||
private void UpdateListView(List<KinectSocket> socketList)
|
||||
{
|
||||
List<string> listBoxItems = new List<string>();
|
||||
|
||||
for (int i = 0; i < socketList.Count; i++)
|
||||
listBoxItems.Add(socketList[i].sSocketState);
|
||||
|
||||
lClientListBox.DataSource = listBoxItems;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user