diff --git a/.gitignore b/.gitignore index 78ff283..5c3b6cd 100644 --- a/.gitignore +++ b/.gitignore @@ -6,6 +6,7 @@ *.user *.userosscache *.sln.docstates +*.tlog # User-specific files (MonoDevelop/Xamarin Studio) *.userprefs diff --git a/LICENSE b/LICENSE index 96c4c98..3ed139e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,6 +1,6 @@ The MIT License (MIT) -Copyright (c) 2015 Marek +Copyright (c) 2015 Marek Kowalski, Jacek Naruniec Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/LiveScanServer/KinectSocket.cs b/LiveScanServer/KinectSocket.cs index 39b8d6d..32c9ca8 100644 --- a/LiveScanServer/KinectSocket.cs +++ b/LiveScanServer/KinectSocket.cs @@ -99,7 +99,7 @@ namespace KinectServer public void SendCalibrationData() { - int size = 1 + 2 * (9 + 3) * sizeof(float); + int size = 1 + (9 + 3) * sizeof(float); byte[] data = new byte[size]; int i = 0; @@ -111,12 +111,6 @@ namespace KinectServer Buffer.BlockCopy(oWorldTransform.t, 0, data, i, 3 * sizeof(float)); i += 3 * sizeof(float); - Buffer.BlockCopy(oCameraPose.R, 0, data, i, 9 * sizeof(float)); - i += 9 * sizeof(float); - Buffer.BlockCopy(oCameraPose.t, 0, data, i, 3 * sizeof(float)); - i += 3 * sizeof(float); - - if (SocketConnected()) oSocket.Send(data); } @@ -141,11 +135,15 @@ namespace KinectServer buffer = Receive(sizeof(float) * 3); Buffer.BlockCopy(buffer, 0, oWorldTransform.t, 0, sizeof(float) * 3); - buffer = Receive(sizeof(float) * 9); - Buffer.BlockCopy(buffer, 0, oCameraPose.R, 0, sizeof(float) * 9); - - buffer = Receive(sizeof(float) * 3); - Buffer.BlockCopy(buffer, 0, oCameraPose.t, 0, sizeof(float) * 3); + oCameraPose.R = oWorldTransform.R; + for (int i = 0; i < 3; i++) + { + oCameraPose.t[i] = 0.0f; + for (int j = 0; j < 3; j++) + { + oCameraPose.t[i] += oWorldTransform.t[j] * oWorldTransform.R[i, j]; + } + } UpdateSocketState(); } diff --git a/README.md b/README.md index c662092..8487244 100644 --- a/README.md +++ b/README.md @@ -28,6 +28,7 @@ While all of our code is licensed under the MIT license, the 3rd party libraries * nanoflann - https://github.com/jlblancoc/nanoflann, BSD license * OpenCV - https://github.com/Itseez/opencv, 3-clause BSD license * OpenTK - https://github.com/opentk/opentk, MIT/X11 license + * ZSTD - https://github.com/facebook/zstd, BSD license * SocketCS - http://www.adp-gmbh.ch/win/misc/sockets.html If you use this software in your research, then please use the following citation: diff --git a/include/LiveScanClient/calibration.h b/include/LiveScanClient/calibration.h index 767be2b..c16be34 100644 --- a/include/LiveScanClient/calibration.h +++ b/include/LiveScanClient/calibration.h @@ -34,9 +34,6 @@ public: vector> worldR; int iUsedMarkerId; - vector cameraT; - vector> cameraR; - vector markerPoses; bool bCalibrated; @@ -45,6 +42,8 @@ public: ~Calibration(); bool Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight); + bool LoadCalibration(); + void SaveCalibration(); private: IMarker *pDetector; int nSampleCounter; diff --git a/src/LiveScanClient/calibration.cpp b/src/LiveScanClient/calibration.cpp index d1f2162..4a2f3dd 100644 --- a/src/LiveScanClient/calibration.cpp +++ b/src/LiveScanClient/calibration.cpp @@ -17,7 +17,7 @@ #include "Kinect.h" #include "opencv\cv.h" - +#include Calibration::Calibration() { @@ -25,6 +25,13 @@ Calibration::Calibration() nSampleCounter = 0; nRequiredSamples = 20; + worldT = vector(3, 0.0f); + for (int i = 0; i < 3; i++) + { + worldR.push_back(vector(3, 0.0f)); + worldR[i][i] = 1.0f; + } + pDetector = new MarkerDetector(); } @@ -102,17 +109,10 @@ bool Calibration::Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColo } } - cameraR = worldR; - cameraT = RotatePoint(worldT, cameraR); - vector translationIncr(3); translationIncr[0] = markerPose.t[0]; translationIncr[1] = markerPose.t[1]; - translationIncr[2] = markerPose.t[2]; - - cameraT[0] += translationIncr[0]; - cameraT[1] += translationIncr[1]; - cameraT[2] += translationIncr[2]; + translationIncr[2] = markerPose.t[2];; translationIncr = InverseRotatePoint(translationIncr, worldR); @@ -125,9 +125,50 @@ bool Calibration::Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColo marker3DSamples.clear(); nSampleCounter = 0; + SaveCalibration(); + return true; } +bool Calibration::LoadCalibration() +{ + ifstream file; + file.open("calibration.txt"); + if (!file.is_open()) + return false; + + for (int i = 0; i < 3; i++) + file >> worldT[i]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + file >> worldR[i][j]; + } + file >> iUsedMarkerId; + file >> bCalibrated; + + return true; +} + +void Calibration::SaveCalibration() +{ + ofstream file; + file.open("calibration.txt"); + for (int i = 0; i < 3; i++) + file << worldT[i] << " "; + file << endl; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + file << worldR[i][j]; + file << endl; + } + file << iUsedMarkerId << endl; + file << bCalibrated << endl; + + file.close(); +} + void Calibration::Procrustes(MarkerInfo &marker, vector &markerInWorld, vector &worldToMarkerT, vector> &worldToMarkerR) { int nVertices = marker.points.size(); @@ -232,8 +273,6 @@ bool Calibration::GetMarkerCorners3D(vector &marker3D, MarkerInfo &mark return true; } - - vector InverseRotatePoint(vector &point, std::vector> &R) { vector res(3); diff --git a/src/LiveScanClient/liveScanClient.cpp b/src/LiveScanClient/liveScanClient.cpp index 1eaa269..28f2721 100644 --- a/src/LiveScanClient/liveScanClient.cpp +++ b/src/LiveScanClient/liveScanClient.cpp @@ -79,6 +79,8 @@ LiveScanClient::LiveScanClient() : m_vBounds.push_back(0.5); m_vBounds.push_back(0.5); m_vBounds.push_back(0.5); + + calibration.LoadCalibration(); } LiveScanClient::~LiveScanClient() @@ -347,6 +349,9 @@ LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, m_pClientSocket = new SocketClient(address, 48001); m_bConnected = true; + if (calibration.bCalibrated) + m_bConfirmCalibrated = true; + SetDlgItemTextA(m_hWnd, IDC_BUTTON_CONNECT, "Disconnect"); //Clear the status bar so that the "Failed to connect..." disappears. SetStatusMessage(L"", 1, true); @@ -581,20 +586,6 @@ void LiveScanClient::HandleSocket() i += sizeof(float); } - for (int j = 0; j < 3; j++) - { - for (int k = 0; k < 3; k++) - { - calibration.cameraR[j][k] = *(float*)(received.c_str() + i); - i += sizeof(float); - } - } - for (int j = 0; j < 3; j++) - { - calibration.cameraT[j] = *(float*)(received.c_str() + i); - i += sizeof(float); - } - //so that we do not lose the next character in the stream i--; } @@ -614,7 +605,7 @@ void LiveScanClient::HandleSocket() if (m_bConfirmCalibrated) { - int size = 2 * (9 + 3) * sizeof(float) + sizeof(int) + 1; + int size = (9 + 3) * sizeof(float) + sizeof(int) + 1; char *buffer = new char[size]; buffer[0] = MSG_CONFIRM_CALIBRATED; int i = 1; @@ -630,14 +621,6 @@ void LiveScanClient::HandleSocket() memcpy(buffer + i, calibration.worldT.data(), 3 * sizeof(float)); i += 3 * sizeof(float); - memcpy(buffer + i, calibration.cameraR[0].data(), 3 * sizeof(float)); - i += 3 * sizeof(float); - memcpy(buffer + i, calibration.cameraR[1].data(), 3 * sizeof(float)); - i += 3 * sizeof(float); - memcpy(buffer + i, calibration.cameraR[2].data(), 3 * sizeof(float)); - i += 3 * sizeof(float); - memcpy(buffer + i, calibration.cameraT.data(), 3 * sizeof(float)); - m_pClientSocket->SendBytes(buffer, size); m_bConfirmCalibrated = false; }