/// <summary>
/// creates a mesh from the current volume and tries to save it to a file
/// </summary>
/// <param name="volume">the volume</param>
/// <param name="pkdp">the data package the mesh origined from</param>
/// <param name="flipAxes">should achses be flipped?</param>
static void exportMesh(ColorReconstruction volume, KinectDataPackage pkdp, bool flipAxes)
{
ColorMesh mesh = volume.CalculateMesh(1);
Microsoft.Win32.SaveFileDialog dialog = new Microsoft.Win32.SaveFileDialog();
dialog.FileName = "KinectFusionMesh_" + pkdp.usedConfig.name + DateTime.UtcNow.ToShortDateString() + ".stl";
dialog.Filter = "STL Mesh Files|*.stl|All Files|*.*";
if (true == dialog.ShowDialog())
{
using (BinaryWriter writer = new BinaryWriter(dialog.OpenFile()))
{
if (null == mesh || null == writer)
{
return;
}
var vertices = mesh.GetVertices();
var normals = mesh.GetNormals();
var indices = mesh.GetTriangleIndexes();
// Check mesh arguments
if (0 == vertices.Count || 0 != vertices.Count % 3 || vertices.Count != indices.Count)
{
throw new Exception("Invalid Mesh Arguments");
}
char[] header = new char[80];
writer.Write(header);
// Write number of triangles
int triangles = vertices.Count / 3;
writer.Write(triangles);
// Sequentially write the normal, 3 vertices of the triangle and attribute, for each triangle
for (int i = 0; i < triangles; i++)
{
// Write normal
var normal = normals[i * 3];
writer.Write(normal.X);
writer.Write(flipAxes ? -normal.Y : normal.Y);
writer.Write(flipAxes ? -normal.Z : normal.Z);
// Write vertices
for (int j = 0; j < 3; j++)
{
var vertex = vertices[(i * 3) + j];
writer.Write(vertex.X);
writer.Write(flipAxes ? -vertex.Y : vertex.Y);
writer.Write(flipAxes ? -vertex.Z : vertex.Z);
}
ushort attribute = 0;
writer.Write(attribute);
}
}
}
}
/// <summary>
/// starts a data request to the server
/// </summary>
/// <param name="pDataPackage">the data package</param>
public void startDataRequest(String pTargetIP, KinectDataPackage pDataPackage)
{
Task <String> t = new Task <String>(() => sendRequestThread(@"http://" +
pTargetIP + @"/KINECTDATA",
pDataPackage, 300000));
t.ContinueWith(TaskFaultedHandler, TaskContinuationOptions.OnlyOnFaulted);
//t.ContinueWith(ConfigRequestSuccessfulHandler, TaskContinuationOptions.OnlyOnRanToCompletion);
t.Start();
}
/// <summary>
/// gets fired when the server recieves a kinect data package from the client
/// </summary>
/// <param name="input">input stream</param>
/// <returns>a status message</returns>
public String responseKinectData(Stream input)
{
// make the input stream an object
BinaryFormatter formatter = new BinaryFormatter();
KinectDataPackage KdP = (KinectDataPackage)formatter.Deserialize(input);
LogManager.writeLogDebug("[Webservice:ServerDefinition] Kinect Data Package recieved from ClientID " + KdP.usedConfig.ID);
OnKinectDataPackageEvent(KdP);
return("KINECT DATA RECIEVED");
}
/// <summary>
/// sends the kinect data package to the server
/// </summary>
/// <param name="pKinectDataPackage">the kinect data package</param>
internal void sendDataToServer(KinectDataPackage pKinectDataPackage)
{
if (ConfigManager._ClientConfigObject.clientRequestObject.isConnected)
{
if (pKinectDataPackage.usedConfig.clientConnectionConfig.targetGateway != String.Empty)
{
_WebserviceSender.startDataRequest(pKinectDataPackage.usedConfig.clientConnectionConfig.targetGateway, pKinectDataPackage);
}
else
{
_WebserviceSender.startDataRequest(pKinectDataPackage.usedConfig.clientConnectionConfig.targetIP, pKinectDataPackage);
}
}
}
/// <summary>
/// updates the database with the client kinect data
/// </summary>
/// <param name="input">the KDP</param>
public void updateClient(KinectDataPackage input)
{
DataTable currentDataTable = getClientDataTable();
DataColumn[] primaryKeyArray = new DataColumn[1];
primaryKeyArray[0] = currentDataTable.Columns["ClientId"];
currentDataTable.PrimaryKey = primaryKeyArray;
try
{
DataRow row = currentDataTable.Rows.Find(input.usedConfig.ID);
MemoryStream memStream = new MemoryStream();
try
{
BinaryFormatter bF = new BinaryFormatter();
bF.Serialize(memStream, input);
byte[] serInput = memStream.ToArray();
row[4] = serInput;
}
catch (Exception e)
{
Log.LogManager.writeLog("[DataManager:DBManager] Update of Client " + input.usedConfig.name + " in database not possible. Reason: " + e.Message);
}
finally
{
memStream.Close();
}
Log.LogManager.writeLog("[DataManager:DBManager] Update of Kinect Data of Client " + input.usedConfig.name + " in database successful.");
}
catch (Exception ex)
{
Log.LogManager.writeLog("[DataManager:DBManager] Update of Client with ID " + input.usedConfig.name + " in database not possible. Reason: " + ex.Message);
}
updateKinectObjectInDB(currentDataTable);
}
/// <summary>
/// creates a new data package to be filled with data
/// </summary>
/// <param name="pCco">the ClientConfigObject with the configuration data</param>
internal void initializeDataPackage(ClientConfigObject pCco)
{
currentPackage = new KinectDataPackage(pCco);
dataSent = false;
acceptsData = true;
}
/// <summary>
/// throw the event that tells the data manager that new kinect data was recieved
/// </summary>
/// <param name="EventArgs"></param>
private void ServerWebserviceContract_OnKinectDataPackageEvent(KinectDataPackage EventArgs)
{
OnKinectDataPackageSendEvent.BeginInvoke(EventArgs, null, null);
}
/// <summary>
/// processes the depth data package into the kinect fusion volume
/// </summary>
/// <param name="pKdp">the data package</param>
void processDepthData(KinectDataPackage pKdp, System.Threading.CancellationToken pCancelToken)
{
lock (canWorkLock)
{
Log.LogManager.updateAlgorithmStatus("Kinect Fusion integration");
this.volume.ResetReconstruction(Matrix4.Identity);
int picturesIntegrated = 0;
foreach (ushort[] pDepth in pKdp.rawDepthData)
{
pCancelToken.ThrowIfCancellationRequested();
WriteableBitmap bitmap = new WriteableBitmap(this.depthFloatFrame.Width, this.depthFloatFrame.Height, 96.0, 96.0, PixelFormats.Bgr32, null);
FusionFloatImageFrame depthFloatBuffer = new FusionFloatImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height);
FusionPointCloudImageFrame pointCloudBuffer = new FusionPointCloudImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height);
FusionColorImageFrame shadedSurfaceColorFrame = new FusionColorImageFrame(this.depthFloatFrame.Width, this.depthFloatFrame.Height);
int[] voxelPixels = new int[this.depthFloatFrame.Width * this.depthFloatFrame.Height];
this.volume.DepthToDepthFloatFrame(
pDepth,
depthFloatBuffer,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.minDepthClip,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.maxDepthClip,
false);
float alignmentValue;
bool trackingSucceeded = this.volume.ProcessFrame(depthFloatBuffer,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.iterationCount,
Config.ServerConfigManager._ServerConfigObject.serverKinectFusionConfig.integrationWeight,
out alignmentValue,
volume.GetCurrentWorldToCameraTransform());
// If camera tracking failed, no data integration or raycast for reference
// point cloud will have taken place, and the internal camera pose
// will be unchanged.
if (!trackingSucceeded)
{
trackingErrorCount++;
}
else
{
Matrix4 calculatedCameraPose = volume.GetCurrentWorldToCameraTransform();
// Set the camera pose and reset tracking errors
worldToCameraTransform = calculatedCameraPose;
trackingErrorCount = 0;
}
// Calculate the point cloud
volume.CalculatePointCloud(pointCloudBuffer, worldToCameraTransform);
// Shade point cloud and render
FusionDepthProcessor.ShadePointCloud(
pointCloudBuffer,
worldToCameraTransform,
null,
shadedSurfaceColorFrame
);
shadedSurfaceColorFrame.CopyPixelDataTo(voxelPixels);
bitmap.WritePixels(
new Int32Rect(0, 0, bitmap.PixelWidth, bitmap.PixelHeight),
voxelPixels,
bitmap.PixelWidth * sizeof(int),
0);
bitmap.Freeze();
OnNewFusionPictureEvent.BeginInvoke(pKdp.usedConfig.ID, bitmap, null, null);
picturesIntegrated++;
Log.LogManager.writeLogDebug("[DataIntegration:Reconstruction] " + picturesIntegrated + " of " + pKdp.rawDepthData.Count + " Pictures integrated");
}
//if request was calibration request, export meshes
if (pKdp.usedConfig.clientRequestObject.requestType == ClientConfigObject.RequestType.calibration)
{
exportMesh(volume, pKdp, false);
Log.LogManager.writeLog("[DataIntegration:Reconstruction] Mesh of " + pKdp.usedConfig.name + " exported.");
return;
}
//broadcast new point cloud
PointCloud p = new PointCloud(volume);
p.ConfigObject = pKdp.usedConfig;
OnNewPointCloudEvent.BeginInvoke(p, null, null);
Log.LogManager.writeLog("[DataIntegration:Reconstruction] All pictures of " + pKdp.usedConfig.name + " integrated");
Log.LogManager.updateAlgorithmStatus("Done");
}
}
/// <summary>
/// external function to accept the kinect data package; starts deparate thread to operate on the volume
/// </summary>
/// <param name="pKdp">the data package</param>
internal void acceptKinectDataPackage(KinectDataPackage pKdp, System.Threading.CancellationToken pCancelToken)
{
Task.Factory.StartNew(() => processDepthData(pKdp, pCancelToken), pCancelToken);
}
/// <summary>
/// recieves the KinectDataPackage by adding it to the Database and creating a point cloud from it
/// </summary>
/// <param name="input"></param>
private void _Webservice_OnKinectDataPackageSendEvent(KinectDataPackage input)
{
this._DataIntegrationManager.recieveDataPackage(input, this._CancelTokenSource.Token);
}
