Initial commit

include/LiveScanClient/calibration.h

@@ -0,0 +1,58 @@
+//   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},
+//    }
+#pragma once
+#include "stdafx.h"
+#include "marker.h"
+#include "utils.h"
+
+vector<float> RotatePoint(vector<float> &point, std::vector<std::vector<float>> &R);
+vector<float> InverseRotatePoint(vector<float> &point, std::vector<std::vector<float>> &R);
+
+struct MarkerPose
+{
+	int markerId;
+	float R[3][3];
+	float t[3];
+};
+
+class Calibration
+{
+public:
+	vector<float> worldT;
+	vector<vector<float>> worldR;
+	int iUsedMarkerId;
+
+	vector<float> cameraT;
+	vector<vector<float>> cameraR;
+
+	vector<MarkerPose> markerPoses;
+
+	bool bCalibrated;
+
+	Calibration();
+	~Calibration();
+
+	bool Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight);
+private:
+	IMarker *pDetector;
+	int nSampleCounter;
+	int nRequiredSamples;
+
+	vector<vector<Point3f>> marker3DSamples;
+
+	void Procrustes(MarkerInfo &marker, vector<Point3f> &markerInWorld, vector<float> &markerT, vector<vector<float>> &markerR);
+	bool GetMarkerCorners3D(vector<Point3f> &marker3D, MarkerInfo &marker, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight);
+};
+

include/LiveScanClient/filter.h

@@ -0,0 +1,62 @@
+//   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},
+//    }
+#pragma once
+
+#include <vector>
+
+#include "nanoflann.h"
+#include "utils.h"
+
+struct KNNeighborsResult
+{
+	std::vector<size_t> neighbors;
+	std::vector<float> distances;
+	float kDistance;
+};
+
+struct PointCloud
+{
+	std::vector<Point3f> pts;
+
+	// Must return the number of data points
+	inline size_t kdtree_get_point_count() const { return pts.size(); }
+	// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
+	inline float kdtree_distance(const float *p1, const size_t idx_p2, size_t /*size*/) const
+	{
+		const float d0 = p1[0] - pts[idx_p2].X;
+		const float d1 = p1[1] - pts[idx_p2].Y;
+		const float d2 = p1[2] - pts[idx_p2].Z;
+
+		return d0*d0 + d1*d1 + d2*d2;
+	}
+	// Returns the dim'th component of the idx'th point in the class:
+	// Since this is inlined and the "dim" argument is typically an immediate value, the
+	// "if/else's" are actually solved at compile time.
+	inline float kdtree_get_pt(const size_t idx, int dim) const
+	{
+		if (dim == 0) return pts[idx].X;
+		else if (dim == 1) return pts[idx].Y;
+		else return pts[idx].Z;
+	}
+	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
+	// Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
+	// Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
+	template <class BBOX>
+	bool kdtree_get_bbox(BBOX& /*bb*/) const { return false; }
+};
+
+typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, PointCloud>, PointCloud, 3> kdTree;
+
+void filter(std::vector<Point3f> &vertices, std::vector<RGB> &colors, int k = 10, float maxDist = 0.01);

include/LiveScanClient/iCapture.h

@@ -0,0 +1,39 @@
+//   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},
+//    }
+#pragma once
+
+#include "utils.h"
+
+class ICapture
+{
+public:
+	ICapture();
+	~ICapture();
+
+	virtual bool Initialize() = 0;
+	virtual bool AcquireFrame() = 0;
+	virtual void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0;
+	virtual void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints) = 0;
+	virtual void MapDepthFrameToColorSpace(Point2f *pColorSpacePoints) = 0;
+	virtual void MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints) = 0;
+
+	bool bInitialized;
+
+	int nColorFrameHeight, nColorFrameWidth;
+	int nDepthFrameHeight, nDepthFrameWidth;
+
+	UINT16 *pDepth;
+	RGB *pColorRGBX;
+};

include/LiveScanClient/iMarker.h

@@ -0,0 +1,49 @@
+//   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},
+//    }
+#pragma once
+
+#include "utils.h"
+
+//struktura przechowywuj¹ca wszystkie dane markera
+typedef struct MarkerStruct
+{
+	int id;
+	//po³o¿enie naro¿ników markera w obrazie
+	std::vector<Point2f> corners;
+	//te same punkty w uk³adzie wspó³rzêdnych markera
+	std::vector<Point3f> points;
+
+	MarkerStruct()
+	{
+		id = -1;
+	}
+
+	MarkerStruct(int id, std::vector<Point2f> corners, std::vector<Point3f> points)
+	{
+		this->id = id;
+
+		this->corners = corners;
+		this->points = points;
+	}
+} MarkerInfo;
+
+class IMarker
+{
+public:
+	IMarker() {};
+
+	//znajduje wszystkie markery w obrazie i zapisuje je w zmiennej markers
+	virtual bool GetMarker(RGB *img, int height, int width, MarkerInfo &marker) = 0;
+};

include/LiveScanClient/imageRenderer.h

@@ -0,0 +1,69 @@
+//------------------------------------------------------------------------------
+// <copyright file="ImageRenderer.h" company="Microsoft">
+//     Copyright (c) Microsoft Corporation.  All rights reserved.
+// </copyright>
+//------------------------------------------------------------------------------
+
+// Manages the drawing of image data
+
+#pragma once
+
+#include <d2d1.h>
+
+class ImageRenderer
+{
+public:
+    /// <summary>
+    /// Constructor
+    /// </summary>
+    ImageRenderer();
+
+    /// <summary>
+    /// Destructor
+    /// </summary>
+    virtual ~ImageRenderer();
+
+    /// <summary>
+    /// Set the window to draw to as well as the video format
+    /// Implied bits per pixel is 32
+    /// </summary>
+    /// <param name="hWnd">window to draw to</param>
+    /// <param name="pD2DFactory">already created D2D factory object</param>
+    /// <param name="sourceWidth">width (in pixels) of image data to be drawn</param>
+    /// <param name="sourceHeight">height (in pixels) of image data to be drawn</param>
+    /// <param name="sourceStride">length (in bytes) of a single scanline</param>
+    /// <returns>indicates success or failure</returns>
+    HRESULT Initialize(HWND hwnd, ID2D1Factory* pD2DFactory, int sourceWidth, int sourceHeight, int sourceStride);
+
+    /// <summary>
+    /// Draws a 32 bit per pixel image of previously specified width, height, and stride to the associated hwnd
+    /// </summary>
+    /// <param name="pImage">image data in RGBX format</param>
+    /// <param name="cbImage">size of image data in bytes</param>
+    /// <returns>indicates success or failure</returns>
+    HRESULT Draw(BYTE* pImage, unsigned long cbImage);
+
+private:
+    HWND                     m_hWnd;
+
+    // Format information
+    UINT                     m_sourceHeight;
+    UINT                     m_sourceWidth;
+    LONG                     m_sourceStride;
+
+     // Direct2D 
+    ID2D1Factory*            m_pD2DFactory;
+    ID2D1HwndRenderTarget*   m_pRenderTarget;
+    ID2D1Bitmap*             m_pBitmap;
+
+    /// <summary>
+    /// Ensure necessary Direct2d resources are created
+    /// </summary>
+    /// <returns>indicates success or failure</returns>
+    HRESULT EnsureResources();
+
+    /// <summary>
+    /// Dispose of Direct2d resources 
+    /// </summary>
+    void DiscardResources();
+};

include/LiveScanClient/kinectCapture.h

@@ -0,0 +1,38 @@
+//   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},
+//    }
+#pragma once
+
+#include "stdafx.h"
+#include "ICapture.h"
+#include "Kinect.h"
+#include "utils.h"
+
+class KinectCapture : public ICapture
+{
+public:
+	KinectCapture();
+	~KinectCapture();
+
+	bool Initialize();
+	bool AcquireFrame();
+	void MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints);
+	void MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints);
+	void MapDepthFrameToColorSpace(Point2f *pColorSpacePoints);
+	void MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints);
+private:	
+	ICoordinateMapper* pCoordinateMapper;
+	IKinectSensor* pKinectSensor;
+	IMultiSourceFrameReader* pMultiSourceFrameReader;
+};

include/LiveScanClient/liveScanClient.h

@@ -0,0 +1,90 @@
+//   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},
+//    }
+#pragma once
+
+#include "resource.h"
+#include "ImageRenderer.h"
+#include "SocketCS.h"
+#include "calibration.h"
+#include "utils.h"
+#include "KinectCapture.h"
+#include <thread>
+#include <mutex>
+
+class LiveScanClient
+{
+public:
+    LiveScanClient();
+    ~LiveScanClient();
+
+
+    static LRESULT CALLBACK MessageRouter(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam);
+    LRESULT CALLBACK        DlgProc(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam);
+    int                     Run(HINSTANCE hInstance, int nCmdShow);
+
+	bool m_bSocketThread;
+private:
+	Calibration calibration;
+
+	bool m_bCalibrate;
+	bool m_bFilter;
+
+	ICapture *pCapture;
+
+	int m_nFilterNeighbors;
+	float m_fFilterThreshold;
+
+	bool m_bCaptureFrame;
+	bool m_bConnected;
+	bool m_bConfirmCaptured;
+	bool m_bConfirmCalibrated;
+	bool m_bShowDepth;
+	
+	SocketClient *m_pClientSocket;
+	std::vector<float> m_vBounds;
+
+	std::vector<Point3f> m_vLastFrameVertices;
+	std::vector<RGB> m_vLastFrameRGB;
+	std::vector<std::vector<Point3f>> m_vGatheredVertices;
+	std::vector<std::vector<RGB>> m_vGatheredRGBPoints;
+
+	HWND m_hWnd;
+    INT64 m_nLastCounter;
+    double m_fFreq;
+    INT64 m_nNextStatusTime;
+    DWORD m_nFramesSinceUpdate;	
+
+	Point3f* m_pCameraSpaceCoordinates;
+	Point2f* m_pColorCoordinates;
+
+    // Direct2D
+    ImageRenderer* m_pDrawColor;
+    ID2D1Factory* m_pD2DFactory;
+	RGB* m_pDepthRGBX;
+
+	void UpdateFrame();
+    void ProcessColor(RGB* pBuffer, int nWidth, int nHeight);
+	void ProcessDepth(const UINT16* pBuffer, int nHeight, int nWidth);
+
+    bool SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce);
+
+	void HandleSocket();
+	void SocketThreadFunction();
+	void StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color);
+	void ShowFPS();
+	void ReadIPFromFile();
+	void WriteIPToFile();
+};
+

include/LiveScanClient/marker.h

@@ -0,0 +1,49 @@
+//   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},
+//    }
+#pragma once
+
+#include "stdafx.h"
+#include <opencv2/opencv.hpp>
+#include "utils.h"
+#include "IMarker.h"
+
+using namespace std;
+
+class MarkerDetector : public IMarker
+{
+public:
+	MarkerDetector();
+
+	bool GetMarker(RGB *img, int height, int width, MarkerInfo &marker);
+private:
+	int nMarkerCorners;
+	vector<cv::Point2f> vPts;
+
+	int nMinSize;
+	int nMaxSize;
+	int nThreshold;
+	double dApproxPolyCoef;
+	double dMarkerFrame;
+	bool bDraw;
+
+	bool GetMarker(cv::Mat &img, MarkerInfo &marker);
+	bool OrderCorners(vector<cv::Point2f> &corners);
+	int GetCode(cv::Mat &img, vector<cv::Point2f> points, vector<cv::Point2f> corners);
+	void CornersSubPix(vector<cv::Point2f> &corners, vector<cv::Point> contour, bool order);
+	cv::Point2f GetIntersection(cv::Vec4f lin1, cv::Vec4f lin2);
+	void GetMarkerPoints(vector<Point3f> &pts);
+	void GetMarkerPointsForWarp(vector<cv::Point2f> &pts);
+	double GetMarkerArea(MarkerInfo &marker);
+};

include/LiveScanClient/utils.h

@@ -0,0 +1,85 @@
+//   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},
+//    }
+#pragma once
+
+#include "stdafx.h"
+#include <stdio.h>
+#include <string>
+#include <vector>
+
+enum INCOMING_MESSAGE_TYPE
+{
+	MSG_CAPTURE_FRAME,
+	MSG_CALIBRATE,
+	MSG_RECEIVE_SETTINGS,
+	MSG_REQUEST_STORED_FRAME,
+	MSG_REQUEST_LAST_FRAME,
+	MSG_RECEIVE_CALIBRATION,
+	MSG_CLEAR_STORED_FRAMES
+};
+
+enum OUTGOING_MESSAGE_TYPE
+{
+	MSG_CONFIRM_CAPTURED,
+	MSG_CONFIRM_CALIBRATED,
+	MSG_STORED_FRAME,
+	MSG_LAST_FRAME
+};
+
+typedef struct Point3f
+{
+	Point3f()
+	{
+		this->X = 0;
+		this->Y = 0;
+		this->Z = 0;
+	}
+	Point3f(float X, float Y, float Z)
+	{
+		this->X = X;
+		this->Y = Y;
+		this->Z = Z;
+	}
+	float X;
+	float Y;
+	float Z;
+} Point3f;
+
+typedef struct Point2f
+{
+	Point2f()
+	{
+		this->X = 0;
+		this->Y = 0;
+	}
+	Point2f(float X, float Y)
+	{
+		this->X = X;
+		this->Y = Y;
+	}
+	float X;
+	float Y;
+} Point2f;
+
+typedef struct RGB
+{
+	BYTE    rgbBlue;
+	BYTE    rgbGreen;
+	BYTE    rgbRed;
+	BYTE    rgbReserved;
+} RGB;
+
+Point3f RotatePoint(Point3f &point, std::vector<std::vector<float>> &R);
+Point3f InverseRotatePoint(Point3f &point, std::vector<std::vector<float>> &R);