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
+177
@@ -0,0 +1,177 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "icp.h"
|
||||
#include "opencv\cv.h"
|
||||
|
||||
void FindClosestPointForEach(PointCloud &sourceCloud, cv::Mat &destPoints, vector<float> &distances, vector<size_t> &indices)
|
||||
{
|
||||
int nVerts2 = destPoints.rows;
|
||||
typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, PointCloud>, PointCloud, 3> kdTree;
|
||||
kdTree tree(3, sourceCloud);
|
||||
tree.buildIndex();
|
||||
|
||||
#pragma omp parallel for
|
||||
for (int i = 0; i < nVerts2; i++)
|
||||
{
|
||||
nanoflann::KNNResultSet<float> resultSet(1);
|
||||
resultSet.init(&indices[i], &distances[i]);
|
||||
tree.findNeighbors(resultSet, (float*)destPoints.row(i).data, nanoflann::SearchParams());
|
||||
}
|
||||
}
|
||||
|
||||
float GetStandardDeviation(vector<float> &data)
|
||||
{
|
||||
float mean = 0;
|
||||
for (size_t i = 0; i < data.size(); i++)
|
||||
{
|
||||
mean += data[i];
|
||||
}
|
||||
mean /= data.size();
|
||||
|
||||
float std = 0;
|
||||
|
||||
for (size_t i = 0; i < data.size(); i++)
|
||||
{
|
||||
std += pow(data[i] - mean, 2);
|
||||
}
|
||||
|
||||
std /= data.size();
|
||||
std = sqrt(std);
|
||||
|
||||
return std;
|
||||
}
|
||||
|
||||
void RejectOutlierMatches(vector<Point3f> &matches1, vector<Point3f> &matches2, vector<float> &matchDistances, float maxStdDev)
|
||||
{
|
||||
float distanceStandardDev = GetStandardDeviation(matchDistances);
|
||||
|
||||
vector<Point3f> filteredMatches1;
|
||||
vector<Point3f> filteredMatches2;
|
||||
for (size_t i = 0; i < matches1.size(); i++)
|
||||
{
|
||||
if (matchDistances[i] > maxStdDev * distanceStandardDev)
|
||||
continue;
|
||||
|
||||
filteredMatches1.push_back(matches1[i]);
|
||||
filteredMatches2.push_back(matches2[i]);
|
||||
}
|
||||
|
||||
matches1 = filteredMatches1;
|
||||
matches2 = filteredMatches2;
|
||||
}
|
||||
|
||||
ICP_API float __stdcall ICP(Point3f *verts1, Point3f *verts2, int nVerts1, int nVerts2, float *R, float *t, int maxIter)
|
||||
{
|
||||
PointCloud cloud1;
|
||||
cloud1.pts = vector<Point3f>(verts1, verts1 + nVerts1);
|
||||
|
||||
cv::Mat matR(3, 3, CV_32F, R);
|
||||
cv::Mat matT(1, 3, CV_32F, t);
|
||||
|
||||
cv::Mat verts2Mat(nVerts2, 3, CV_32F, (float*)verts2);
|
||||
|
||||
float error = 1;
|
||||
|
||||
for (int iter = 0; iter < maxIter; iter++)
|
||||
{
|
||||
vector<Point3f> matched1, matched2;
|
||||
|
||||
vector<float> distances(nVerts2);
|
||||
vector<size_t> indices(nVerts2);
|
||||
FindClosestPointForEach(cloud1, verts2Mat, distances, indices);
|
||||
|
||||
vector<float> matchDistances;
|
||||
vector<int> matchIdxs(nVerts1, -1);
|
||||
for (int i = 0; i < nVerts2; i++)
|
||||
{
|
||||
int pos = matchIdxs[indices[i]];
|
||||
|
||||
if (pos != -1)
|
||||
{
|
||||
if (matchDistances[pos] < distances[i])
|
||||
continue;
|
||||
}
|
||||
|
||||
Point3f temp;
|
||||
temp.X = verts2Mat.at<float>(i, 0);
|
||||
temp.Y = verts2Mat.at<float>(i, 1);
|
||||
temp.Z = verts2Mat.at<float>(i, 2);
|
||||
|
||||
if (pos == -1)
|
||||
{
|
||||
matched1.push_back(verts1[indices[i]]);
|
||||
matched2.push_back(temp);
|
||||
|
||||
matchDistances.push_back(distances[i]);
|
||||
|
||||
matchIdxs[indices[i]] = matched1.size() - 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
matched2[pos] = temp;
|
||||
matchDistances[pos] = distances[i];
|
||||
}
|
||||
}
|
||||
|
||||
RejectOutlierMatches(matched1, matched2, matchDistances, 2.5);
|
||||
|
||||
//error = 0;
|
||||
//for (int i = 0; i < matchDistances.size(); i++)
|
||||
//{
|
||||
// error += sqrt(matchDistances[i]);
|
||||
//}
|
||||
//error /= matchDistances.size();
|
||||
//cout << error << endl;
|
||||
|
||||
cv::Mat matched1MatCv(matched1.size(), 3, CV_32F, matched1.data());
|
||||
cv::Mat matched2MatCv(matched2.size(), 3, CV_32F, matched2.data());
|
||||
cv::Mat tempT;
|
||||
cv::reduce(matched1MatCv - matched2MatCv, tempT, 0, CV_REDUCE_AVG);
|
||||
|
||||
for (int i = 0; i < verts2Mat.rows; i++)
|
||||
{
|
||||
verts2Mat.row(i) += tempT;
|
||||
}
|
||||
for (int i = 0; i < matched2MatCv.rows; i++)
|
||||
{
|
||||
matched2MatCv.row(i) += tempT;
|
||||
}
|
||||
|
||||
cv::Mat M = matched2MatCv.t() * matched1MatCv;
|
||||
cv::SVD svd;
|
||||
svd(M);
|
||||
cv::Mat tempR = svd.u * svd.vt;
|
||||
|
||||
double det = cv::determinant(tempR);
|
||||
if (det < 0)
|
||||
{
|
||||
cv::Mat temp = cv::Mat::eye(3, 3, CV_32F);
|
||||
temp.at<float>(2, 2) = -1;
|
||||
tempR = svd.u * temp * svd.vt;
|
||||
}
|
||||
|
||||
verts2Mat = verts2Mat * tempR;
|
||||
|
||||
matT += tempT * matR.t();
|
||||
matR = matR * tempR;
|
||||
}
|
||||
|
||||
memcpy(verts2, verts2Mat.data, verts2Mat.rows * sizeof(float) * 3);
|
||||
|
||||
memcpy(R, matR.data, 9 * sizeof(float));
|
||||
memcpy(t, matT.data, 3 * sizeof(float));
|
||||
|
||||
return error;
|
||||
}
|
||||
@@ -0,0 +1,128 @@
|
||||
// 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},
|
||||
// }
|
||||
#include <stdio.h>
|
||||
#include <vector>
|
||||
|
||||
#include "icp.h"
|
||||
#include "opencv\cv.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
struct RGB
|
||||
{
|
||||
unsigned char R, G, B;
|
||||
};
|
||||
|
||||
void savePLY(std::string filename, std::vector<Point3f> vertices, std::vector<RGB> colors)
|
||||
{
|
||||
|
||||
unsigned int numVertices = vertices.size();
|
||||
unsigned int numColors = colors.size();
|
||||
|
||||
// Open File
|
||||
FILE *meshFile = NULL;
|
||||
errno_t err = fopen_s(&meshFile, filename.c_str(), "wt");
|
||||
|
||||
|
||||
// Write the header line
|
||||
std::string header = "ply\nformat ascii 1.0\n";
|
||||
fwrite(header.c_str(), sizeof(char), header.length(), meshFile);
|
||||
|
||||
const unsigned int bufSize = 1000 * 3;
|
||||
char outStr[bufSize];
|
||||
int written = 0;
|
||||
|
||||
// Elements are: x,y,z, r,g,b
|
||||
written = sprintf_s(outStr, bufSize, "element vertex %u\nproperty float x\nproperty float y\nproperty float z\nproperty uchar red\nproperty uchar green\nproperty uchar blue\n", numVertices);
|
||||
fwrite(outStr, sizeof(char), written, meshFile);
|
||||
|
||||
written = sprintf_s(outStr, bufSize, "end_header\n");
|
||||
fwrite(outStr, sizeof(char), written, meshFile);
|
||||
|
||||
|
||||
// Sequentially write the 3 vertices of the triangle, for each triangle
|
||||
for (unsigned int vertexIndex = 0; vertexIndex < numVertices; vertexIndex++)
|
||||
{
|
||||
unsigned int color0 = colors[vertexIndex].R;
|
||||
unsigned int color1 = colors[vertexIndex].G;
|
||||
unsigned int color2 = colors[vertexIndex].B;
|
||||
|
||||
written = sprintf_s(outStr, bufSize, "%f %f %f %u %u %u\n",
|
||||
vertices[vertexIndex].X, vertices[vertexIndex].Y, vertices[vertexIndex].Z,
|
||||
((color0)& 255), ((color1)& 255), (color2 & 255));
|
||||
|
||||
fwrite(outStr, sizeof(char), written, meshFile);
|
||||
}
|
||||
|
||||
fflush(meshFile);
|
||||
fclose(meshFile);
|
||||
}
|
||||
|
||||
void loadPLY(string filename, vector<Point3f> &verts, vector<RGB> &colors)
|
||||
{
|
||||
FILE *f;
|
||||
int nVerts;
|
||||
|
||||
fopen_s(&f, filename.c_str(), "r");
|
||||
|
||||
char buffer[100];
|
||||
fgets(buffer, 100, f);
|
||||
fgets(buffer, 100, f);
|
||||
fgets(buffer, 100, f);
|
||||
sscanf_s(buffer, "element vertex %d\n", &nVerts);
|
||||
|
||||
for (int i = 0; i < 7; i++)
|
||||
fgets(buffer, 100, f);
|
||||
|
||||
for (int i = 0; i < nVerts; i++)
|
||||
{
|
||||
fgets(buffer, 100, f);
|
||||
|
||||
Point3f point;
|
||||
RGB rgb;
|
||||
int R, G, B;
|
||||
sscanf_s(buffer, "%f %f %f %d %d %d\n", &point.X, &point.Y, &point.Z, &R, &G, &B);
|
||||
|
||||
rgb.R = R;
|
||||
rgb.G = G;
|
||||
rgb.B = B;
|
||||
|
||||
verts.push_back(point);
|
||||
colors.push_back(rgb);
|
||||
}
|
||||
|
||||
fclose(f);
|
||||
}
|
||||
|
||||
|
||||
//This function here can be used to test the ICP functionality, it aligns the points clouds in "test1.ply" and "test2.ply"
|
||||
int main()
|
||||
{
|
||||
vector<Point3f> verts1, verts2;
|
||||
vector<RGB> colors1, colors2;
|
||||
|
||||
loadPLY("../test1.ply", verts1, colors1);
|
||||
loadPLY("../test2.ply", verts2, colors2);
|
||||
|
||||
cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
|
||||
cv::Mat t(1, 3, CV_32F, cv::Scalar(0));
|
||||
cv::Mat verts2Mat(verts2.size(), 3, CV_32F, (float*)verts2.data());
|
||||
|
||||
ICP(verts1.data(), verts2.data(), verts1.size(), verts2.size(), (float*)R.data, (float*)t.data, 1);
|
||||
|
||||
savePLY("../testResult.ply", verts2, colors2);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,257 @@
|
||||
// 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},
|
||||
// }
|
||||
|
||||
#include "calibration.h"
|
||||
#include "Kinect.h"
|
||||
#include "opencv\cv.h"
|
||||
|
||||
|
||||
|
||||
Calibration::Calibration()
|
||||
{
|
||||
bCalibrated = false;
|
||||
nSampleCounter = 0;
|
||||
nRequiredSamples = 20;
|
||||
|
||||
pDetector = new MarkerDetector();
|
||||
}
|
||||
|
||||
Calibration::~Calibration()
|
||||
{
|
||||
if (pDetector != NULL)
|
||||
{
|
||||
delete pDetector;
|
||||
pDetector = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
bool Calibration::Calibrate(RGB *pBuffer, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight)
|
||||
{
|
||||
MarkerInfo marker;
|
||||
|
||||
bool res = pDetector->GetMarker(pBuffer, cColorHeight, cColorWidth, marker);
|
||||
if (!res)
|
||||
return false;
|
||||
|
||||
int indexInPoses = -1;
|
||||
|
||||
for (unsigned int j = 0; j < markerPoses.size(); j++)
|
||||
{
|
||||
if (marker.id == markerPoses[j].markerId)
|
||||
{
|
||||
indexInPoses = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (indexInPoses == -1)
|
||||
return false;
|
||||
|
||||
MarkerPose markerPose = markerPoses[indexInPoses];
|
||||
iUsedMarkerId = markerPose.markerId;
|
||||
|
||||
vector<Point3f> marker3D(marker.corners.size());
|
||||
bool success = GetMarkerCorners3D(marker3D, marker, pCameraCoordinates, cColorWidth, cColorHeight);
|
||||
|
||||
if (!success)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
marker3DSamples.push_back(marker3D);
|
||||
nSampleCounter++;
|
||||
|
||||
if (nSampleCounter < nRequiredSamples)
|
||||
return false;
|
||||
|
||||
for (size_t i = 0; i < marker3D.size(); i++)
|
||||
{
|
||||
marker3D[i] = Point3f();
|
||||
for (int j = 0; j < nRequiredSamples; j++)
|
||||
{
|
||||
marker3D[i].X += marker3DSamples[j][i].X / (float)nRequiredSamples;
|
||||
marker3D[i].Y += marker3DSamples[j][i].Y / (float)nRequiredSamples;
|
||||
marker3D[i].Z += marker3DSamples[j][i].Z / (float)nRequiredSamples;
|
||||
}
|
||||
}
|
||||
|
||||
Procrustes(marker, marker3D, worldT, worldR);
|
||||
|
||||
vector<vector<float>> Rcopy = worldR;
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
worldR[i][j] = 0;
|
||||
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
worldR[i][j] += markerPose.R[i][k] * Rcopy[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
cameraR = worldR;
|
||||
cameraT = RotatePoint(worldT, cameraR);
|
||||
|
||||
vector<float> 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 = InverseRotatePoint(translationIncr, worldR);
|
||||
|
||||
worldT[0] += translationIncr[0];
|
||||
worldT[1] += translationIncr[1];
|
||||
worldT[2] += translationIncr[2];
|
||||
|
||||
bCalibrated = true;
|
||||
|
||||
marker3DSamples.clear();
|
||||
nSampleCounter = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Calibration::Procrustes(MarkerInfo &marker, vector<Point3f> &markerInWorld, vector<float> &worldToMarkerT, vector<vector<float>> &worldToMarkerR)
|
||||
{
|
||||
int nVertices = marker.points.size();
|
||||
|
||||
Point3f markerCenterInWorld;
|
||||
Point3f markerCenter;
|
||||
for (int i = 0; i < nVertices; i++)
|
||||
{
|
||||
markerCenterInWorld.X += markerInWorld[i].X / nVertices;
|
||||
markerCenterInWorld.Y += markerInWorld[i].Y / nVertices;
|
||||
markerCenterInWorld.Z += markerInWorld[i].Z / nVertices;
|
||||
|
||||
markerCenter.X += marker.points[i].X / nVertices;
|
||||
markerCenter.Y += marker.points[i].Y / nVertices;
|
||||
markerCenter.Z += marker.points[i].Z / nVertices;
|
||||
}
|
||||
|
||||
worldToMarkerT.resize(3);
|
||||
worldToMarkerT[0] = -markerCenterInWorld.X;
|
||||
worldToMarkerT[1] = -markerCenterInWorld.Y;
|
||||
worldToMarkerT[2] = -markerCenterInWorld.Z;
|
||||
|
||||
vector<Point3f> markerInWorldTranslated(nVertices);
|
||||
vector<Point3f> markerTranslated(nVertices);
|
||||
for (int i = 0; i < nVertices; i++)
|
||||
{
|
||||
markerInWorldTranslated[i].X = markerInWorld[i].X + worldToMarkerT[0];
|
||||
markerInWorldTranslated[i].Y = markerInWorld[i].Y + worldToMarkerT[1];
|
||||
markerInWorldTranslated[i].Z = markerInWorld[i].Z + worldToMarkerT[2];
|
||||
|
||||
markerTranslated[i].X = marker.points[i].X - markerCenter.X;
|
||||
markerTranslated[i].Y = marker.points[i].Y - markerCenter.Y;
|
||||
markerTranslated[i].Z = marker.points[i].Z - markerCenter.Z;
|
||||
}
|
||||
|
||||
cv::Mat A(nVertices, 3, CV_64F);
|
||||
cv::Mat B(nVertices, 3, CV_64F);
|
||||
|
||||
for (int i = 0; i < nVertices; i++)
|
||||
{
|
||||
A.at<double>(i, 0) = markerTranslated[i].X;
|
||||
A.at<double>(i, 1) = markerTranslated[i].Y;
|
||||
A.at<double>(i, 2) = markerTranslated[i].Z;
|
||||
|
||||
B.at<double>(i, 0) = markerInWorldTranslated[i].X;
|
||||
B.at<double>(i, 1) = markerInWorldTranslated[i].Y;
|
||||
B.at<double>(i, 2) = markerInWorldTranslated[i].Z;
|
||||
}
|
||||
|
||||
cv::Mat M = A.t() * B;
|
||||
|
||||
cv::SVD svd;
|
||||
svd(M);
|
||||
cv::Mat R = svd.u * svd.vt;
|
||||
|
||||
double det = cv::determinant(R);
|
||||
|
||||
if (det < 0)
|
||||
{
|
||||
cv::Mat temp = cv::Mat::eye(3, 3, CV_64F);
|
||||
temp.at<double>(2, 2) = -1;
|
||||
R = svd.u * temp * svd.vt;
|
||||
}
|
||||
|
||||
worldToMarkerR.resize(3);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
worldToMarkerR[i].resize(3);
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
worldToMarkerR[i][j] = static_cast<float>(R.at<double>(i, j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Calibration::GetMarkerCorners3D(vector<Point3f> &marker3D, MarkerInfo &marker, Point3f *pCameraCoordinates, int cColorWidth, int cColorHeight)
|
||||
{
|
||||
for (unsigned int i = 0; i < marker.corners.size(); i++)
|
||||
{
|
||||
int minX = static_cast<int>(marker.corners[i].X);
|
||||
int maxX = minX + 1;
|
||||
int minY = static_cast<int>(marker.corners[i].Y);
|
||||
int maxY = minY + 1;
|
||||
|
||||
float dx = marker.corners[i].X - minX;
|
||||
float dy = marker.corners[i].Y - minY;
|
||||
|
||||
Point3f pointMin = pCameraCoordinates[minX + minY * cColorWidth];
|
||||
Point3f pointXMaxYMin = pCameraCoordinates[maxX + minY * cColorWidth];
|
||||
Point3f pointXMinYMax = pCameraCoordinates[minX + maxY * cColorWidth];
|
||||
Point3f pointMax = pCameraCoordinates[maxX + maxY * cColorWidth];
|
||||
|
||||
if (pointMin.Z < 0 || pointXMaxYMin.Z < 0 || pointXMinYMax.Z < 0 || pointMax.Z < 0)
|
||||
return false;
|
||||
|
||||
marker3D[i].X = (1 - dx) * (1 - dy) * pointMin.X + dx * (1 - dy) * pointXMaxYMin.X + (1 - dx) * dy * pointXMinYMax.X + dx * dy * pointMax.X;
|
||||
marker3D[i].Y = (1 - dx) * (1 - dy) * pointMin.Y + dx * (1 - dy) * pointXMaxYMin.Y + (1 - dx) * dy * pointXMinYMax.Y + dx * dy * pointMax.Y;
|
||||
marker3D[i].Z = (1 - dx) * (1 - dy) * pointMin.Z + dx * (1 - dy) * pointXMaxYMin.Z + (1 - dx) * dy * pointXMinYMax.Z + dx * dy * pointMax.Z;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
vector<float> InverseRotatePoint(vector<float> &point, std::vector<std::vector<float>> &R)
|
||||
{
|
||||
vector<float> res(3);
|
||||
|
||||
res[0] = point[0] * R[0][0] + point[1] * R[1][0] + point[2] * R[2][0];
|
||||
res[1] = point[0] * R[0][1] + point[1] * R[1][1] + point[2] * R[2][1];
|
||||
res[2] = point[0] * R[0][2] + point[1] * R[1][2] + point[2] * R[2][2];
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
vector<float> RotatePoint(vector<float> &point, std::vector<std::vector<float>> &R)
|
||||
{
|
||||
vector<float> res(3);
|
||||
|
||||
res[0] = point[0] * R[0][0] + point[1] * R[0][1] + point[2] * R[0][2];
|
||||
res[1] = point[0] * R[1][0] + point[1] * R[1][1] + point[2] * R[1][2];
|
||||
res[2] = point[0] * R[2][0] + point[1] * R[2][1] + point[2] * R[2][2];
|
||||
|
||||
return res;
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "filter.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
vector<KNNeighborsResult> KNNeighbors(PointCloud &cloud, kdTree &tree, int k)
|
||||
{
|
||||
vector<KNNeighborsResult> result(cloud.pts.size());
|
||||
int nCloudPts = static_cast<int>(cloud.pts.size());
|
||||
|
||||
#pragma omp parallel for
|
||||
for (int i = 0; i < nCloudPts; i++)
|
||||
{
|
||||
result[i].neighbors.resize(k);
|
||||
result[i].distances.resize(k);
|
||||
tree.knnSearch((float*)(cloud.pts.data() + i), k, (size_t*)result[i].neighbors.data(), result[i].distances.data());
|
||||
result[i].kDistance = result[i].distances[k - 1];
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void filter(std::vector<Point3f> &vertices, std::vector<RGB> &colors, int k, float maxDist)
|
||||
{
|
||||
if (k <= 0 || maxDist <= 0)
|
||||
return;
|
||||
|
||||
PointCloud cloud;
|
||||
cloud.pts = vertices;
|
||||
|
||||
kdTree tree(3, cloud);
|
||||
tree.buildIndex();
|
||||
|
||||
vector<KNNeighborsResult> knn = KNNeighbors(cloud, tree, k);
|
||||
|
||||
vector<int> indicesToRemove;
|
||||
|
||||
float distThreshold = pow(maxDist, 2);
|
||||
for (unsigned int i = 0; i < cloud.pts.size(); i++)
|
||||
{
|
||||
if (knn[i].kDistance > distThreshold)
|
||||
indicesToRemove.push_back(i);
|
||||
}
|
||||
|
||||
int lastElemIdx = 0;
|
||||
unsigned int idxToCheck = 0;
|
||||
for (unsigned int i = 0; i < vertices.size(); i++)
|
||||
{
|
||||
if (idxToCheck < indicesToRemove.size() && i == indicesToRemove[idxToCheck])
|
||||
{
|
||||
idxToCheck++;
|
||||
continue;
|
||||
}
|
||||
vertices[lastElemIdx] = vertices[i];
|
||||
colors[lastElemIdx] = colors[i];
|
||||
|
||||
lastElemIdx++;
|
||||
}
|
||||
|
||||
vertices.resize(lastElemIdx);
|
||||
colors.resize(lastElemIdx);
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "ICapture.h"
|
||||
|
||||
ICapture::ICapture()
|
||||
{
|
||||
bInitialized = false;
|
||||
|
||||
nColorFrameHeight = 0;
|
||||
nColorFrameWidth = 0;
|
||||
nDepthFrameHeight = 0;
|
||||
nDepthFrameWidth = 0;
|
||||
|
||||
pDepth = NULL;
|
||||
pColorRGBX = NULL;
|
||||
}
|
||||
|
||||
ICapture::~ICapture()
|
||||
{
|
||||
if (pDepth != NULL)
|
||||
{
|
||||
delete[] pDepth;
|
||||
pDepth = NULL;
|
||||
}
|
||||
|
||||
if (pColorRGBX != NULL)
|
||||
{
|
||||
delete[] pColorRGBX;
|
||||
pColorRGBX = NULL;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "IMarker.h"
|
||||
@@ -0,0 +1,166 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <copyright file="ImageRenderer.cpp" company="Microsoft">
|
||||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// </copyright>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
#include "stdafx.h"
|
||||
#include "ImageRenderer.h"
|
||||
|
||||
/// <summary>
|
||||
/// Constructor
|
||||
/// </summary>
|
||||
ImageRenderer::ImageRenderer() :
|
||||
m_hWnd(0),
|
||||
m_sourceWidth(0),
|
||||
m_sourceHeight(0),
|
||||
m_sourceStride(0),
|
||||
m_pD2DFactory(NULL),
|
||||
m_pRenderTarget(NULL),
|
||||
m_pBitmap(0)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Destructor
|
||||
/// </summary>
|
||||
ImageRenderer::~ImageRenderer()
|
||||
{
|
||||
DiscardResources();
|
||||
SafeRelease(m_pD2DFactory);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Ensure necessary Direct2d resources are created
|
||||
/// </summary>
|
||||
/// <returns>indicates success or failure</returns>
|
||||
HRESULT ImageRenderer::EnsureResources()
|
||||
{
|
||||
HRESULT hr = S_OK;
|
||||
|
||||
if (NULL == m_pRenderTarget)
|
||||
{
|
||||
D2D1_SIZE_U size = D2D1::SizeU(m_sourceWidth, m_sourceHeight);
|
||||
|
||||
D2D1_RENDER_TARGET_PROPERTIES rtProps = D2D1::RenderTargetProperties();
|
||||
rtProps.pixelFormat = D2D1::PixelFormat(DXGI_FORMAT_B8G8R8A8_UNORM, D2D1_ALPHA_MODE_IGNORE);
|
||||
rtProps.usage = D2D1_RENDER_TARGET_USAGE_GDI_COMPATIBLE;
|
||||
|
||||
// Create a hWnd render target, in order to render to the window set in initialize
|
||||
hr = m_pD2DFactory->CreateHwndRenderTarget(
|
||||
rtProps,
|
||||
D2D1::HwndRenderTargetProperties(m_hWnd, size),
|
||||
&m_pRenderTarget
|
||||
);
|
||||
|
||||
if (FAILED(hr))
|
||||
{
|
||||
return hr;
|
||||
}
|
||||
|
||||
// Create a bitmap that we can copy image data into and then render to the target
|
||||
hr = m_pRenderTarget->CreateBitmap(
|
||||
size,
|
||||
D2D1::BitmapProperties(D2D1::PixelFormat(DXGI_FORMAT_B8G8R8A8_UNORM, D2D1_ALPHA_MODE_IGNORE)),
|
||||
&m_pBitmap
|
||||
);
|
||||
|
||||
if (FAILED(hr))
|
||||
{
|
||||
SafeRelease(m_pRenderTarget);
|
||||
return hr;
|
||||
}
|
||||
}
|
||||
|
||||
return hr;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dispose of Direct2d resources
|
||||
/// </summary>
|
||||
void ImageRenderer::DiscardResources()
|
||||
{
|
||||
SafeRelease(m_pRenderTarget);
|
||||
SafeRelease(m_pBitmap);
|
||||
}
|
||||
|
||||
/// <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 ImageRenderer::Initialize(HWND hWnd, ID2D1Factory* pD2DFactory, int sourceWidth, int sourceHeight, int sourceStride)
|
||||
{
|
||||
if (NULL == pD2DFactory)
|
||||
{
|
||||
return E_INVALIDARG;
|
||||
}
|
||||
|
||||
m_hWnd = hWnd;
|
||||
|
||||
// One factory for the entire application so save a pointer here
|
||||
m_pD2DFactory = pD2DFactory;
|
||||
|
||||
m_pD2DFactory->AddRef();
|
||||
|
||||
// Get the frame size
|
||||
m_sourceWidth = sourceWidth;
|
||||
m_sourceHeight = sourceHeight;
|
||||
m_sourceStride = sourceStride;
|
||||
|
||||
return S_OK;
|
||||
}
|
||||
|
||||
/// <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 ImageRenderer::Draw(BYTE* pImage, unsigned long cbImage)
|
||||
{
|
||||
// incorrectly sized image data passed in
|
||||
if (cbImage < ((m_sourceHeight - 1) * m_sourceStride) + (m_sourceWidth * 4))
|
||||
{
|
||||
return E_INVALIDARG;
|
||||
}
|
||||
|
||||
// create the resources for this draw device
|
||||
// they will be recreated if previously lost
|
||||
HRESULT hr = EnsureResources();
|
||||
|
||||
if (FAILED(hr))
|
||||
{
|
||||
return hr;
|
||||
}
|
||||
|
||||
// Copy the image that was passed in into the direct2d bitmap
|
||||
hr = m_pBitmap->CopyFromMemory(NULL, pImage, m_sourceStride);
|
||||
|
||||
if (FAILED(hr))
|
||||
{
|
||||
return hr;
|
||||
}
|
||||
|
||||
m_pRenderTarget->BeginDraw();
|
||||
|
||||
// Draw the bitmap stretched to the size of the window
|
||||
m_pRenderTarget->DrawBitmap(m_pBitmap);
|
||||
|
||||
hr = m_pRenderTarget->EndDraw();
|
||||
|
||||
// Device lost, need to recreate the render target
|
||||
// We'll dispose it now and retry drawing
|
||||
if (hr == D2DERR_RECREATE_TARGET)
|
||||
{
|
||||
hr = S_OK;
|
||||
DiscardResources();
|
||||
}
|
||||
|
||||
return hr;
|
||||
}
|
||||
@@ -0,0 +1,163 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "KinectCapture.h"
|
||||
#include <chrono>
|
||||
|
||||
KinectCapture::KinectCapture()
|
||||
{
|
||||
pKinectSensor = NULL;
|
||||
pCoordinateMapper = NULL;
|
||||
pMultiSourceFrameReader = NULL;
|
||||
}
|
||||
|
||||
KinectCapture::~KinectCapture()
|
||||
{
|
||||
SafeRelease(pKinectSensor);
|
||||
SafeRelease(pCoordinateMapper);
|
||||
SafeRelease(pMultiSourceFrameReader);
|
||||
}
|
||||
|
||||
bool KinectCapture::Initialize()
|
||||
{
|
||||
HRESULT hr;
|
||||
|
||||
hr = GetDefaultKinectSensor(&pKinectSensor);
|
||||
if (FAILED(hr))
|
||||
{
|
||||
bInitialized = false;
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
if (pKinectSensor)
|
||||
{
|
||||
pKinectSensor->get_CoordinateMapper(&pCoordinateMapper);
|
||||
hr = pKinectSensor->Open();
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
pKinectSensor->OpenMultiSourceFrameReader(FrameSourceTypes::FrameSourceTypes_Color | FrameSourceTypes::FrameSourceTypes_Depth, &pMultiSourceFrameReader);
|
||||
}
|
||||
}
|
||||
|
||||
bInitialized = SUCCEEDED(hr);
|
||||
|
||||
if (bInitialized)
|
||||
{
|
||||
std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
|
||||
bool bTemp;
|
||||
do
|
||||
{
|
||||
bTemp = AcquireFrame();
|
||||
|
||||
std::chrono::duration<double> elapsedSeconds = std::chrono::system_clock::now() - start;
|
||||
if (elapsedSeconds.count() > 5.0)
|
||||
{
|
||||
bInitialized = false;
|
||||
break;
|
||||
}
|
||||
|
||||
} while (!bTemp);
|
||||
}
|
||||
|
||||
return bInitialized;
|
||||
}
|
||||
|
||||
bool KinectCapture::AcquireFrame()
|
||||
{
|
||||
if (!bInitialized)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
//Multi frame
|
||||
IMultiSourceFrame* pMultiFrame = NULL;
|
||||
HRESULT hr = pMultiSourceFrameReader->AcquireLatestFrame(&pMultiFrame);
|
||||
|
||||
if (!SUCCEEDED(hr))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
//Depth frame
|
||||
IDepthFrameReference* pDepthFrameReference = NULL;
|
||||
IDepthFrame* pDepthFrame = NULL;
|
||||
pMultiFrame->get_DepthFrameReference(&pDepthFrameReference);
|
||||
hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
if (pDepth == NULL)
|
||||
{
|
||||
IFrameDescription* pFrameDescription = NULL;
|
||||
hr = pDepthFrame->get_FrameDescription(&pFrameDescription);
|
||||
pFrameDescription->get_Width(&nDepthFrameWidth);
|
||||
pFrameDescription->get_Height(&nDepthFrameHeight);
|
||||
pDepth = new UINT16[nDepthFrameHeight * nDepthFrameWidth];
|
||||
SafeRelease(pFrameDescription);
|
||||
}
|
||||
|
||||
UINT nBufferSize = nDepthFrameHeight * nDepthFrameWidth;
|
||||
hr = pDepthFrame->CopyFrameDataToArray(nBufferSize, pDepth);
|
||||
}
|
||||
SafeRelease(pDepthFrame);
|
||||
SafeRelease(pDepthFrameReference);
|
||||
|
||||
//Color frame
|
||||
IColorFrameReference* pColorFrameReference = NULL;
|
||||
IColorFrame* pColorFrame = NULL;
|
||||
pMultiFrame->get_ColorFrameReference(&pColorFrameReference);
|
||||
hr = pColorFrameReference->AcquireFrame(&pColorFrame);
|
||||
|
||||
if (SUCCEEDED(hr))
|
||||
{
|
||||
if (pColorRGBX == NULL)
|
||||
{
|
||||
IFrameDescription* pFrameDescription = NULL;
|
||||
hr = pColorFrame->get_FrameDescription(&pFrameDescription);
|
||||
hr = pFrameDescription->get_Width(&nColorFrameWidth);
|
||||
hr = pFrameDescription->get_Height(&nColorFrameHeight);
|
||||
pColorRGBX = new RGB[nColorFrameWidth * nColorFrameHeight];
|
||||
SafeRelease(pFrameDescription);
|
||||
}
|
||||
|
||||
UINT nBufferSize = nColorFrameWidth * nColorFrameHeight * sizeof(RGB);
|
||||
hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast<BYTE*>(pColorRGBX), ColorImageFormat_Bgra);
|
||||
}
|
||||
|
||||
SafeRelease(pColorFrame);
|
||||
SafeRelease(pColorFrameReference);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void KinectCapture::MapDepthFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapDepthFrameToCameraSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nDepthFrameWidth * nDepthFrameHeight, (CameraSpacePoint*)pCameraSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapColorFrameToCameraSpace(Point3f *pCameraSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapColorFrameToCameraSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (CameraSpacePoint*)pCameraSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapDepthFrameToColorSpace(Point2f *pColorSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapDepthFrameToColorSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nDepthFrameWidth * nDepthFrameHeight, (ColorSpacePoint*)pColorSpacePoints);
|
||||
}
|
||||
|
||||
void KinectCapture::MapColorFrameToDepthSpace(Point2f *pDepthSpacePoints)
|
||||
{
|
||||
pCoordinateMapper->MapColorFrameToDepthSpace(nDepthFrameWidth * nDepthFrameHeight, pDepth, nColorFrameWidth * nColorFrameHeight, (DepthSpacePoint*)pDepthSpacePoints);;
|
||||
}
|
||||
@@ -0,0 +1,735 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "stdafx.h"
|
||||
#include "resource.h"
|
||||
#include "LiveScanClient.h"
|
||||
#include "filter.h"
|
||||
#include <chrono>
|
||||
#include <strsafe.h>
|
||||
#include <fstream>
|
||||
|
||||
std::mutex m_mSocketThreadMutex;
|
||||
|
||||
int APIENTRY wWinMain(
|
||||
_In_ HINSTANCE hInstance,
|
||||
_In_opt_ HINSTANCE hPrevInstance,
|
||||
_In_ LPWSTR lpCmdLine,
|
||||
_In_ int nShowCmd
|
||||
)
|
||||
{
|
||||
UNREFERENCED_PARAMETER(hPrevInstance);
|
||||
UNREFERENCED_PARAMETER(lpCmdLine);
|
||||
|
||||
LiveScanClient application;
|
||||
application.Run(hInstance, nShowCmd);
|
||||
}
|
||||
|
||||
LiveScanClient::LiveScanClient() :
|
||||
m_hWnd(NULL),
|
||||
m_nLastCounter(0),
|
||||
m_nFramesSinceUpdate(0),
|
||||
m_fFreq(0),
|
||||
m_nNextStatusTime(0LL),
|
||||
m_pD2DFactory(NULL),
|
||||
m_pDrawColor(NULL),
|
||||
m_pDepthRGBX(NULL),
|
||||
m_pCameraSpaceCoordinates(NULL),
|
||||
m_bCalibrate(false),
|
||||
m_bFilter(false),
|
||||
m_bCaptureFrame(false),
|
||||
m_bConnected(false),
|
||||
m_bConfirmCaptured(false),
|
||||
m_bConfirmCalibrated(false),
|
||||
m_bShowDepth(false),
|
||||
m_bSocketThread(true),
|
||||
m_pClientSocket(NULL),
|
||||
m_nFilterNeighbors(10),
|
||||
m_fFilterThreshold(0.01f)
|
||||
{
|
||||
pCapture = new KinectCapture();
|
||||
|
||||
LARGE_INTEGER qpf = {0};
|
||||
if (QueryPerformanceFrequency(&qpf))
|
||||
{
|
||||
m_fFreq = double(qpf.QuadPart);
|
||||
}
|
||||
|
||||
m_vBounds.push_back(-0.5);
|
||||
m_vBounds.push_back(-0.5);
|
||||
m_vBounds.push_back(-0.5);
|
||||
m_vBounds.push_back(0.5);
|
||||
m_vBounds.push_back(0.5);
|
||||
m_vBounds.push_back(0.5);
|
||||
}
|
||||
|
||||
LiveScanClient::~LiveScanClient()
|
||||
{
|
||||
// clean up Direct2D renderer
|
||||
if (m_pDrawColor)
|
||||
{
|
||||
delete m_pDrawColor;
|
||||
m_pDrawColor = NULL;
|
||||
}
|
||||
|
||||
if (pCapture)
|
||||
{
|
||||
delete pCapture;
|
||||
pCapture = NULL;
|
||||
}
|
||||
|
||||
if (m_pDepthRGBX)
|
||||
{
|
||||
delete[] m_pDepthRGBX;
|
||||
m_pDepthRGBX = NULL;
|
||||
}
|
||||
|
||||
if (m_pCameraSpaceCoordinates)
|
||||
{
|
||||
delete[] m_pCameraSpaceCoordinates;
|
||||
m_pCameraSpaceCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pColorCoordinates)
|
||||
{
|
||||
delete[] m_pColorCoordinates;
|
||||
m_pColorCoordinates = NULL;
|
||||
}
|
||||
|
||||
if (m_pClientSocket)
|
||||
{
|
||||
delete m_pClientSocket;
|
||||
m_pClientSocket = NULL;
|
||||
}
|
||||
// clean up Direct2D
|
||||
SafeRelease(m_pD2DFactory);
|
||||
}
|
||||
|
||||
int LiveScanClient::Run(HINSTANCE hInstance, int nCmdShow)
|
||||
{
|
||||
MSG msg = {0};
|
||||
WNDCLASS wc;
|
||||
|
||||
// Dialog custom window class
|
||||
ZeroMemory(&wc, sizeof(wc));
|
||||
wc.style = CS_HREDRAW | CS_VREDRAW;
|
||||
wc.cbWndExtra = DLGWINDOWEXTRA;
|
||||
wc.hCursor = LoadCursorW(NULL, IDC_ARROW);
|
||||
wc.hIcon = LoadIconW(hInstance, MAKEINTRESOURCE(IDI_APP));
|
||||
wc.lpfnWndProc = DefDlgProcW;
|
||||
wc.lpszClassName = L"LiveScanClientAppDlgWndClass";
|
||||
|
||||
if (!RegisterClassW(&wc))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Create main application window
|
||||
HWND hWndApp = CreateDialogParamW(
|
||||
NULL,
|
||||
MAKEINTRESOURCE(IDD_APP),
|
||||
NULL,
|
||||
(DLGPROC)LiveScanClient::MessageRouter,
|
||||
reinterpret_cast<LPARAM>(this));
|
||||
|
||||
// Show window
|
||||
ShowWindow(hWndApp, nCmdShow);
|
||||
|
||||
std::thread t1(&LiveScanClient::SocketThreadFunction, this);
|
||||
// Main message loop
|
||||
while (WM_QUIT != msg.message)
|
||||
{
|
||||
//HandleSocket();
|
||||
UpdateFrame();
|
||||
|
||||
while (PeekMessageW(&msg, NULL, 0, 0, PM_REMOVE))
|
||||
{
|
||||
// If a dialog message will be taken care of by the dialog proc
|
||||
if (hWndApp && IsDialogMessageW(hWndApp, &msg))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
TranslateMessage(&msg);
|
||||
DispatchMessageW(&msg);
|
||||
}
|
||||
}
|
||||
|
||||
m_bSocketThread = false;
|
||||
t1.join();
|
||||
return static_cast<int>(msg.wParam);
|
||||
}
|
||||
|
||||
void LiveScanClient::UpdateFrame()
|
||||
{
|
||||
if (!pCapture->bInitialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
bool bNewFrameAcquired = pCapture->AcquireFrame();
|
||||
|
||||
if (!bNewFrameAcquired)
|
||||
return;
|
||||
|
||||
pCapture->MapDepthFrameToCameraSpace(m_pCameraSpaceCoordinates);
|
||||
pCapture->MapDepthFrameToColorSpace(m_pColorCoordinates);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
StoreFrame(m_pCameraSpaceCoordinates, m_pColorCoordinates, pCapture->pColorRGBX);
|
||||
|
||||
if (m_bCaptureFrame)
|
||||
{
|
||||
m_vGatheredVertices.push_back(m_vLastFrameVertices);
|
||||
m_vGatheredRGBPoints.push_back(m_vLastFrameRGB);
|
||||
m_bConfirmCaptured = true;
|
||||
m_bCaptureFrame = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bCalibrate)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
Point3f *pCameraCoordinates = new Point3f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
pCapture->MapColorFrameToCameraSpace(pCameraCoordinates);
|
||||
|
||||
bool res = calibration.Calibrate(pCapture->pColorRGBX, pCameraCoordinates, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight);
|
||||
|
||||
delete[] pCameraCoordinates;
|
||||
|
||||
if (res)
|
||||
{
|
||||
m_bConfirmCalibrated = true;
|
||||
m_bCalibrate = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (!m_bShowDepth)
|
||||
ProcessColor(pCapture->pColorRGBX, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight);
|
||||
else
|
||||
ProcessDepth(pCapture->pDepth, pCapture->nDepthFrameWidth, pCapture->nDepthFrameHeight);
|
||||
|
||||
ShowFPS();
|
||||
}
|
||||
|
||||
LRESULT CALLBACK LiveScanClient::MessageRouter(HWND hWnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
|
||||
{
|
||||
LiveScanClient* pThis = NULL;
|
||||
|
||||
if (WM_INITDIALOG == uMsg)
|
||||
{
|
||||
pThis = reinterpret_cast<LiveScanClient*>(lParam);
|
||||
SetWindowLongPtr(hWnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(pThis));
|
||||
}
|
||||
else
|
||||
{
|
||||
pThis = reinterpret_cast<LiveScanClient*>(::GetWindowLongPtr(hWnd, GWLP_USERDATA));
|
||||
}
|
||||
|
||||
if (pThis)
|
||||
{
|
||||
return pThis->DlgProc(hWnd, uMsg, wParam, lParam);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
LRESULT CALLBACK LiveScanClient::DlgProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
|
||||
{
|
||||
UNREFERENCED_PARAMETER(wParam);
|
||||
UNREFERENCED_PARAMETER(lParam);
|
||||
|
||||
switch (message)
|
||||
{
|
||||
case WM_INITDIALOG:
|
||||
{
|
||||
// Bind application window handle
|
||||
m_hWnd = hWnd;
|
||||
|
||||
// Init Direct2D
|
||||
D2D1CreateFactory(D2D1_FACTORY_TYPE_SINGLE_THREADED, &m_pD2DFactory);
|
||||
|
||||
// Get and initialize the default Kinect sensor
|
||||
bool res = pCapture->Initialize();
|
||||
if (res)
|
||||
{
|
||||
m_pDepthRGBX = new RGB[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
|
||||
m_pCameraSpaceCoordinates = new Point3f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
m_pColorCoordinates = new Point2f[pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight];
|
||||
}
|
||||
else
|
||||
{
|
||||
SetStatusMessage(L"Capture device failed to initialize!", 10000, true);
|
||||
}
|
||||
|
||||
// Create and initialize a new Direct2D image renderer (take a look at ImageRenderer.h)
|
||||
// We'll use this to draw the data we receive from the Kinect to the screen
|
||||
HRESULT hr;
|
||||
m_pDrawColor = new ImageRenderer();
|
||||
hr = m_pDrawColor->Initialize(GetDlgItem(m_hWnd, IDC_VIDEOVIEW), m_pD2DFactory, pCapture->nColorFrameWidth, pCapture->nColorFrameHeight, pCapture->nColorFrameWidth * sizeof(RGB));
|
||||
if (FAILED(hr))
|
||||
{
|
||||
SetStatusMessage(L"Failed to initialize the Direct2D draw device.", 10000, true);
|
||||
}
|
||||
|
||||
ReadIPFromFile();
|
||||
}
|
||||
break;
|
||||
|
||||
// If the titlebar X is clicked, destroy app
|
||||
case WM_CLOSE:
|
||||
WriteIPToFile();
|
||||
DestroyWindow(hWnd);
|
||||
break;
|
||||
case WM_DESTROY:
|
||||
// Quit the main message pump
|
||||
PostQuitMessage(0);
|
||||
break;
|
||||
|
||||
// Handle button press
|
||||
case WM_COMMAND:
|
||||
if (IDC_BUTTON_CONNECT == LOWORD(wParam) && BN_CLICKED == HIWORD(wParam))
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
if (m_bConnected)
|
||||
{
|
||||
delete m_pClientSocket;
|
||||
m_pClientSocket = NULL;
|
||||
|
||||
m_bConnected = false;
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_CONNECT, "Connect");
|
||||
}
|
||||
else
|
||||
{
|
||||
m_vGatheredVertices.clear();
|
||||
m_vGatheredRGBPoints.clear();
|
||||
try
|
||||
{
|
||||
char address[20];
|
||||
GetDlgItemTextA(m_hWnd, IDC_IP, address, 20);
|
||||
m_pClientSocket = new SocketClient(address, 48001);
|
||||
|
||||
m_bConnected = true;
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_CONNECT, "Disconnect");
|
||||
//Clear the status bar so that the "Failed to connect..." disappears.
|
||||
SetStatusMessage(L"", 1, true);
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
SetStatusMessage(L"Failed to connect. Did you start the server?", 10000, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (IDC_BUTTON_SWITCH == LOWORD(wParam) && BN_CLICKED == HIWORD(wParam))
|
||||
{
|
||||
m_bShowDepth = !m_bShowDepth;
|
||||
|
||||
if (m_bShowDepth)
|
||||
{
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_SWITCH, "Show color");
|
||||
}
|
||||
else
|
||||
{
|
||||
SetDlgItemTextA(m_hWnd, IDC_BUTTON_SWITCH, "Show depth");
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessDepth(const UINT16* pBuffer, int nWidth, int nHeight)
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
if (m_pDepthRGBX && pBuffer && (nWidth == pCapture->nDepthFrameWidth) && (nHeight == pCapture->nDepthFrameHeight))
|
||||
{
|
||||
// end pixel is start + width*height - 1
|
||||
const UINT16* pBufferEnd = pBuffer + (nWidth * nHeight);
|
||||
|
||||
Point2f *pDepthSpacePoints = new Point2f[pCapture->nColorFrameWidth * pCapture->nColorFrameHeight];
|
||||
pCapture->MapColorFrameToDepthSpace(pDepthSpacePoints);
|
||||
|
||||
for (int i = 0; i < pCapture->nColorFrameWidth * pCapture->nColorFrameHeight; i++)
|
||||
{
|
||||
Point2f depthPoint = pDepthSpacePoints[i];
|
||||
BYTE intensity = 0;
|
||||
|
||||
if (depthPoint.X >= 0 && depthPoint.Y >= 0)
|
||||
{
|
||||
int depthIdx = (int)(depthPoint.X + depthPoint.Y * pCapture->nDepthFrameWidth);
|
||||
USHORT depth = pBuffer[depthIdx];
|
||||
intensity = static_cast<BYTE>(depth % 256);
|
||||
}
|
||||
|
||||
m_pDepthRGBX[i].rgbRed = intensity;
|
||||
m_pDepthRGBX[i].rgbGreen = intensity;
|
||||
m_pDepthRGBX[i].rgbBlue = intensity;
|
||||
}
|
||||
|
||||
// Draw the data with Direct2D
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(m_pDepthRGBX), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB));
|
||||
|
||||
delete[] pDepthSpacePoints;
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::ProcessColor(RGB* pBuffer, int nWidth, int nHeight)
|
||||
{
|
||||
// Make sure we've received valid data
|
||||
if (pBuffer && (nWidth == pCapture->nColorFrameWidth) && (nHeight == pCapture->nColorFrameHeight))
|
||||
{
|
||||
// Draw the data with Direct2D
|
||||
m_pDrawColor->Draw(reinterpret_cast<BYTE*>(pBuffer), pCapture->nColorFrameWidth * pCapture->nColorFrameHeight * sizeof(RGB));
|
||||
}
|
||||
}
|
||||
|
||||
bool LiveScanClient::SetStatusMessage(_In_z_ WCHAR* szMessage, DWORD nShowTimeMsec, bool bForce)
|
||||
{
|
||||
INT64 now = GetTickCount64();
|
||||
|
||||
if (m_hWnd && (bForce || (m_nNextStatusTime <= now)))
|
||||
{
|
||||
SetDlgItemText(m_hWnd, IDC_STATUS, szMessage);
|
||||
m_nNextStatusTime = now + nShowTimeMsec;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void LiveScanClient::SocketThreadFunction()
|
||||
{
|
||||
while (m_bSocketThread)
|
||||
{
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
HandleSocket();
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::HandleSocket()
|
||||
{
|
||||
char byteToSend;
|
||||
std::lock_guard<std::mutex> lock(m_mSocketThreadMutex);
|
||||
|
||||
if (!m_bConnected)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
string received = m_pClientSocket->ReceiveBytes();
|
||||
for (unsigned int i = 0; i < received.length(); i++)
|
||||
{
|
||||
//capture a frame
|
||||
if (received[i] == MSG_CAPTURE_FRAME)
|
||||
m_bCaptureFrame = true;
|
||||
//calibrate
|
||||
else if (received[i] == MSG_CALIBRATE)
|
||||
m_bCalibrate = true;
|
||||
//receive settings
|
||||
//TODO: what if packet is split?
|
||||
else if (received[i] == MSG_RECEIVE_SETTINGS)
|
||||
{
|
||||
vector<float> bounds(6);
|
||||
i++;
|
||||
int nBytes = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
|
||||
for (int j = 0; j < 6; j++)
|
||||
{
|
||||
bounds[j] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
m_bFilter = (received[i]!=0);
|
||||
i++;
|
||||
|
||||
m_nFilterNeighbors = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
|
||||
m_fFilterThreshold = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
|
||||
m_vBounds = bounds;
|
||||
|
||||
int nMarkers = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
|
||||
calibration.markerPoses.resize(nMarkers);
|
||||
|
||||
for (int j = 0; j < nMarkers; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
for (int l = 0; l < 3; l++)
|
||||
{
|
||||
calibration.markerPoses[j].R[k][l] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
}
|
||||
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
calibration.markerPoses[j].t[k] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
|
||||
calibration.markerPoses[j].markerId = *(int*)(received.c_str() + i);
|
||||
i += sizeof(int);
|
||||
}
|
||||
|
||||
//so that we do not lose the next character in the stream
|
||||
i--;
|
||||
}
|
||||
//send stored frame
|
||||
else if (received[i] == MSG_REQUEST_STORED_FRAME)
|
||||
{
|
||||
byteToSend = MSG_STORED_FRAME;
|
||||
m_pClientSocket->SendBytes(&byteToSend, 1);
|
||||
|
||||
if (m_vGatheredRGBPoints.size() > 0)
|
||||
{
|
||||
int size = m_vGatheredRGBPoints[0].size() * (3 + 3 * sizeof(float));
|
||||
//m_pClientSocket->SendLine(to_string(size));
|
||||
m_pClientSocket->SendBytes((char*)&size, 4);
|
||||
|
||||
char *to_send = new char[size];
|
||||
char *ptr1 = (char*)m_vGatheredRGBPoints[0].data();
|
||||
char *ptr2 = (char*)m_vGatheredVertices[0].data();
|
||||
int pos = 0, initial_pos;
|
||||
for (unsigned int i = 0; i < m_vGatheredRGBPoints[0].size(); i++)
|
||||
{
|
||||
initial_pos = pos;
|
||||
to_send[pos++] = m_vGatheredRGBPoints[0][i].rgbRed;
|
||||
to_send[pos++] = m_vGatheredRGBPoints[0][i].rgbGreen;
|
||||
to_send[pos++] = m_vGatheredRGBPoints[0][i].rgbBlue;
|
||||
|
||||
|
||||
memcpy(to_send + pos, ptr2, sizeof(float)* 3);
|
||||
ptr2 += sizeof(float)* 3;
|
||||
pos += sizeof(float)* 3;
|
||||
}
|
||||
|
||||
m_pClientSocket->SendBytes(to_send, size);
|
||||
delete[]to_send;
|
||||
|
||||
m_vGatheredRGBPoints.erase(m_vGatheredRGBPoints.begin(), m_vGatheredRGBPoints.begin() + 1);
|
||||
m_vGatheredVertices.erase(m_vGatheredVertices.begin(), m_vGatheredVertices.begin() + 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
int size = 0;
|
||||
m_pClientSocket->SendBytes((char*)&size, 4);
|
||||
}
|
||||
}
|
||||
//send last frame
|
||||
else if (received[i] == MSG_REQUEST_LAST_FRAME)
|
||||
{
|
||||
byteToSend = MSG_LAST_FRAME;
|
||||
m_pClientSocket->SendBytes(&byteToSend, 1);
|
||||
|
||||
int size = m_vLastFrameRGB.size() * (3 + 3 * sizeof(float));
|
||||
//m_pClientSocket->SendLine(to_string(size));
|
||||
m_pClientSocket->SendBytes((char*)&size, 4);
|
||||
|
||||
char *buffer = new char[size];
|
||||
char *ptr2 = (char*)m_vLastFrameVertices.data();
|
||||
int pos = 0, initial_pos;
|
||||
for (unsigned int i = 0; i < m_vLastFrameRGB.size(); i++)
|
||||
{
|
||||
initial_pos = pos;
|
||||
buffer[pos++] = m_vLastFrameRGB[i].rgbRed;
|
||||
buffer[pos++] = m_vLastFrameRGB[i].rgbGreen;
|
||||
buffer[pos++] = m_vLastFrameRGB[i].rgbBlue;
|
||||
|
||||
|
||||
memcpy(buffer + pos, ptr2, sizeof(float)* 3);
|
||||
ptr2 += sizeof(float)* 3;
|
||||
pos += sizeof(float)* 3;
|
||||
}
|
||||
|
||||
m_pClientSocket->SendBytes(buffer, size);
|
||||
delete[]buffer;
|
||||
}
|
||||
//receive calibration data
|
||||
else if (received[i] == MSG_RECEIVE_CALIBRATION)
|
||||
{
|
||||
i++;
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
for (int k = 0; k < 3; k++)
|
||||
{
|
||||
calibration.worldR[j][k] = *(float*)(received.c_str() + i);
|
||||
i += sizeof(float);
|
||||
}
|
||||
}
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
calibration.worldT[j] = *(float*)(received.c_str() + i);
|
||||
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--;
|
||||
}
|
||||
else if (received[i] == MSG_CLEAR_STORED_FRAMES)
|
||||
{
|
||||
m_vGatheredVertices.clear();
|
||||
m_vGatheredRGBPoints.clear();
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bConfirmCaptured)
|
||||
{
|
||||
byteToSend = MSG_CONFIRM_CAPTURED;
|
||||
m_pClientSocket->SendBytes(&byteToSend, 1);
|
||||
m_bConfirmCaptured = false;
|
||||
}
|
||||
|
||||
if (m_bConfirmCalibrated)
|
||||
{
|
||||
int size = 2 * (9 + 3) * sizeof(float) + sizeof(int) + 1;
|
||||
char *buffer = new char[size];
|
||||
buffer[0] = MSG_CONFIRM_CALIBRATED;
|
||||
int i = 1;
|
||||
|
||||
memcpy(buffer + i, &calibration.iUsedMarkerId, 1 * sizeof(int));
|
||||
i += 1 * sizeof(int);
|
||||
memcpy(buffer + i, calibration.worldR[0].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.worldR[1].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
memcpy(buffer + i, calibration.worldR[2].data(), 3 * sizeof(float));
|
||||
i += 3 * sizeof(float);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::StoreFrame(Point3f *vertices, Point2f *mapping, RGB *color)
|
||||
{
|
||||
std::vector<Point3f> goodVertices;
|
||||
std::vector<RGB> goodColorPoints;
|
||||
|
||||
unsigned int nVertices = pCapture->nDepthFrameWidth * pCapture->nDepthFrameHeight;
|
||||
|
||||
for (unsigned int vertexIndex = 0; vertexIndex < nVertices; vertexIndex++)
|
||||
{
|
||||
if (vertices[vertexIndex].Z >= 0 && mapping[vertexIndex].Y >= 0 && mapping[vertexIndex].Y < pCapture->nColorFrameHeight)
|
||||
{
|
||||
Point3f temp = vertices[vertexIndex];
|
||||
RGB tempColor = color[(int)mapping[vertexIndex].X + (int)mapping[vertexIndex].Y * pCapture->nColorFrameWidth];
|
||||
if (calibration.bCalibrated)
|
||||
{
|
||||
temp.X += calibration.worldT[0];
|
||||
temp.Y += calibration.worldT[1];
|
||||
temp.Z += calibration.worldT[2];
|
||||
temp = RotatePoint(temp, calibration.worldR);
|
||||
|
||||
if (temp.X < m_vBounds[0] || temp.X > m_vBounds[3]
|
||||
|| temp.Y < m_vBounds[1] || temp.Y > m_vBounds[4]
|
||||
|| temp.Z < m_vBounds[2] || temp.Z > m_vBounds[5])
|
||||
continue;
|
||||
}
|
||||
|
||||
goodVertices.push_back(temp);
|
||||
goodColorPoints.push_back(tempColor);
|
||||
}
|
||||
}
|
||||
|
||||
if (m_bFilter)
|
||||
filter(goodVertices, goodColorPoints, m_nFilterNeighbors, m_fFilterThreshold);
|
||||
m_vLastFrameVertices = goodVertices;
|
||||
m_vLastFrameRGB = goodColorPoints;
|
||||
}
|
||||
|
||||
void LiveScanClient::ShowFPS()
|
||||
{
|
||||
if (m_hWnd)
|
||||
{
|
||||
double fps = 0.0;
|
||||
|
||||
LARGE_INTEGER qpcNow = { 0 };
|
||||
if (m_fFreq)
|
||||
{
|
||||
if (QueryPerformanceCounter(&qpcNow))
|
||||
{
|
||||
if (m_nLastCounter)
|
||||
{
|
||||
m_nFramesSinceUpdate++;
|
||||
fps = m_fFreq * m_nFramesSinceUpdate / double(qpcNow.QuadPart - m_nLastCounter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
WCHAR szStatusMessage[64];
|
||||
StringCchPrintf(szStatusMessage, _countof(szStatusMessage), L" FPS = %0.2f", fps);
|
||||
|
||||
if (SetStatusMessage(szStatusMessage, 1000, false))
|
||||
{
|
||||
m_nLastCounter = qpcNow.QuadPart;
|
||||
m_nFramesSinceUpdate = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::ReadIPFromFile()
|
||||
{
|
||||
ifstream file;
|
||||
file.open("lastIP.txt");
|
||||
if (file.is_open())
|
||||
{
|
||||
char lastUsedIPAddress[20];
|
||||
file.getline(lastUsedIPAddress, 20);
|
||||
file.close();
|
||||
SetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress);
|
||||
}
|
||||
}
|
||||
|
||||
void LiveScanClient::WriteIPToFile()
|
||||
{
|
||||
ofstream file;
|
||||
file.open("lastIP.txt");
|
||||
char lastUsedIPAddress[20];
|
||||
GetDlgItemTextA(m_hWnd, IDC_IP, lastUsedIPAddress, 20);
|
||||
file << lastUsedIPAddress;
|
||||
file.close();
|
||||
}
|
||||
@@ -0,0 +1,394 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "marker.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
using namespace std;
|
||||
|
||||
MarkerDetector::MarkerDetector()
|
||||
{
|
||||
nMinSize = 100;
|
||||
nMaxSize = 1000000000;
|
||||
nThreshold = 120;
|
||||
dApproxPolyCoef = 0.12;
|
||||
|
||||
dMarkerFrame = 0.4;
|
||||
nMarkerCorners = 5;
|
||||
|
||||
bDraw = true;
|
||||
|
||||
GetMarkerPointsForWarp(vPts);
|
||||
}
|
||||
|
||||
bool MarkerDetector::GetMarker(cv::Mat &img, MarkerInfo &marker)
|
||||
{
|
||||
vector<MarkerInfo> markers;
|
||||
cv::Mat img2, img3;
|
||||
cv::cvtColor(img, img2, CV_BGR2GRAY);
|
||||
cv::threshold(img2, img2, nThreshold, 255, CV_THRESH_BINARY);
|
||||
|
||||
img2.copyTo(img3);
|
||||
|
||||
vector<vector<cv::Point>> contours;
|
||||
cv::findContours(img3, contours, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
|
||||
|
||||
for (unsigned int i = 0; i < contours.size(); i++)
|
||||
{
|
||||
vector<cv::Point> corners;
|
||||
double area = cv::contourArea(contours[i]);
|
||||
|
||||
if (area < nMinSize || area > nMaxSize)
|
||||
continue;
|
||||
|
||||
cv::approxPolyDP(contours[i], corners, sqrt(area)*dApproxPolyCoef, true);
|
||||
|
||||
vector<cv::Point2f> cornersFloat;
|
||||
for (unsigned int j = 0; j < corners.size(); j++)
|
||||
{
|
||||
cornersFloat.push_back(cv::Point2f((float)corners[j].x, (float)corners[j].y));
|
||||
}
|
||||
|
||||
if (!cv::isContourConvex(corners) && corners.size() == nMarkerCorners && OrderCorners(cornersFloat))
|
||||
{
|
||||
bool order = true;
|
||||
|
||||
int code = GetCode(img2, vPts, cornersFloat);
|
||||
|
||||
if (code < 0)
|
||||
{
|
||||
reverse(cornersFloat.begin() + 1, cornersFloat.end());
|
||||
code = GetCode(img2, vPts, cornersFloat);
|
||||
|
||||
if (code < 0)
|
||||
continue;
|
||||
|
||||
order = false;
|
||||
}
|
||||
|
||||
float test = cornersFloat[0].x - cornersFloat[1].x;
|
||||
|
||||
CornersSubPix(cornersFloat, contours[i], order);
|
||||
|
||||
vector<Point2f> cornersFloat2(nMarkerCorners);
|
||||
vector<Point3f> points3D;
|
||||
|
||||
for (int i = 0; i < nMarkerCorners; i++)
|
||||
{
|
||||
cornersFloat2[i] = Point2f(cornersFloat[i].x, cornersFloat[i].y);
|
||||
}
|
||||
|
||||
GetMarkerPoints(points3D);
|
||||
|
||||
markers.push_back(MarkerInfo(code, cornersFloat2, points3D));
|
||||
|
||||
if (bDraw)
|
||||
{
|
||||
for (unsigned int j = 0; j < corners.size(); j++)
|
||||
{
|
||||
cv::circle(img, cornersFloat[j], 2, cv::Scalar(0, 50 * j, 0), 1);
|
||||
cv::line(img, cornersFloat[j], cornersFloat[(j + 1) % cornersFloat.size()], cv::Scalar(0, 0, 255), 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (markers.size() > 0)
|
||||
{
|
||||
double maxArea = 0;
|
||||
int maxInd = 0;
|
||||
|
||||
for (unsigned int i = 0; i < markers.size(); i++)
|
||||
{
|
||||
if (GetMarkerArea(markers[i]) > maxArea)
|
||||
{
|
||||
maxInd = i;
|
||||
maxArea = GetMarkerArea(markers[i]);
|
||||
}
|
||||
}
|
||||
|
||||
marker = markers[maxInd];
|
||||
if (bDraw)
|
||||
{
|
||||
for (int j = 0; j < nMarkerCorners; j++)
|
||||
{
|
||||
cv::Point2f pt1 = cv::Point2f(marker.corners[j].X, marker.corners[j].Y);
|
||||
cv::Point2f pt2 = cv::Point2f(marker.corners[(j + 1) % nMarkerCorners].X, marker.corners[(j + 1) % nMarkerCorners].Y);
|
||||
cv::line(img, pt1, pt2, cv::Scalar(0, 255, 0), 2);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
bool MarkerDetector::GetMarker(RGB *img, int height, int width, MarkerInfo &marker)
|
||||
{
|
||||
cv::Mat cvImg(height, width, CV_8UC3);
|
||||
|
||||
for (int i = 0; i < height; i++)
|
||||
{
|
||||
for (int j = 0; j < width; j++)
|
||||
{
|
||||
cvImg.at<cv::Vec3b>(i, j)[0] = img[j + width * i].rgbBlue;
|
||||
cvImg.at<cv::Vec3b>(i, j)[1] = img[j + width * i].rgbGreen;
|
||||
cvImg.at<cv::Vec3b>(i, j)[2] = img[j + width * i].rgbRed;
|
||||
}
|
||||
}
|
||||
|
||||
bool res = GetMarker(cvImg, marker);
|
||||
|
||||
for (int i = 0; i < height; i++)
|
||||
{
|
||||
for (int j = 0; j < width; j++)
|
||||
{
|
||||
img[j + width * i].rgbBlue = cvImg.at<cv::Vec3b>(i, j)[0];
|
||||
img[j + width * i].rgbGreen = cvImg.at<cv::Vec3b>(i, j)[1];
|
||||
img[j + width * i].rgbRed = cvImg.at<cv::Vec3b>(i, j)[2];
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
bool MarkerDetector::OrderCorners(vector<cv::Point2f> &corners)
|
||||
{
|
||||
vector<int> hull;
|
||||
|
||||
cv::convexHull(corners, hull);
|
||||
|
||||
if (hull.size() != corners.size() - 1)
|
||||
return false;
|
||||
|
||||
int index = -1;
|
||||
for (unsigned int i = 0; i < corners.size(); i++)
|
||||
{
|
||||
bool found = false;
|
||||
for (unsigned int j = 0; j < hull.size(); j++)
|
||||
{
|
||||
if (hull[j] == i)
|
||||
{
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!found)
|
||||
{
|
||||
index = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
vector<cv::Point2f> corners2;
|
||||
for (unsigned int i = 0; i < corners.size(); i++)
|
||||
{
|
||||
corners2.push_back(corners[(index + i)%corners.size()]);
|
||||
}
|
||||
|
||||
corners = corners2;
|
||||
return true;
|
||||
}
|
||||
|
||||
int MarkerDetector::GetCode(cv::Mat &img, vector<cv::Point2f> points, vector<cv::Point2f> corners)
|
||||
{
|
||||
cv::Mat H, img2;
|
||||
|
||||
int minX = 0, minY = 0;
|
||||
|
||||
double markerInterior = 2 - 2 * dMarkerFrame;
|
||||
|
||||
for (unsigned int i = 0; i < points.size(); i++)
|
||||
{
|
||||
points[i].x = static_cast<float>((points[i].x - dMarkerFrame + 1) * 50);
|
||||
points[i].y = static_cast<float>((points[i].y - dMarkerFrame + 1) * 50);
|
||||
}
|
||||
|
||||
H = cv::findHomography(corners, points);
|
||||
cv::warpPerspective(img, img2, H, cv::Size((int)(50 * markerInterior), (int)(50 * markerInterior)));
|
||||
|
||||
int xdiff = img2.cols / 3;
|
||||
int ydiff = img2.rows / 3;
|
||||
int tot = xdiff * ydiff;
|
||||
int vals[9];
|
||||
|
||||
cv::Mat integral;
|
||||
cv::integral(img2, integral);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
int temp;
|
||||
temp = integral.at<int>((i + 1) * xdiff, (j + 1) * ydiff);
|
||||
temp += integral.at<int>(i * xdiff, j * ydiff);
|
||||
temp -= integral.at<int>((i + 1) * xdiff, j * ydiff);
|
||||
temp -= integral.at<int>(i * xdiff, (j + 1) * ydiff);
|
||||
|
||||
temp = temp / tot;
|
||||
|
||||
if (temp < 128)
|
||||
vals[j + i * 3] = 0;
|
||||
else if (temp >= 128)
|
||||
vals[j + i * 3] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
int ones = 0;
|
||||
int code = 0;
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
if (vals[i] == vals[i + 4])
|
||||
return -1;
|
||||
else if (vals[i] == 1)
|
||||
{
|
||||
code += static_cast<int>(pow(2, (double)(3 - i)));
|
||||
ones++;
|
||||
}
|
||||
}
|
||||
|
||||
if (ones / 2 == (float)ones / 2.0)
|
||||
{
|
||||
if (vals[8] == 0)
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (ones / 2 != ones / 2.0)
|
||||
{
|
||||
if (vals[8] == 1)
|
||||
return -1;
|
||||
}
|
||||
|
||||
return code;
|
||||
}
|
||||
|
||||
void MarkerDetector::CornersSubPix(vector<cv::Point2f> &corners, vector<cv::Point> contour, bool order)
|
||||
{
|
||||
int *indices = new int[corners.size()];
|
||||
|
||||
for (unsigned int i = 0; i < corners.size(); i++)
|
||||
{
|
||||
for (unsigned int j = 0; j < contour.size(); j++)
|
||||
{
|
||||
if (corners[i].x == contour[j].x && corners[i].y == contour[j].y)
|
||||
{
|
||||
indices[i] = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vector<cv::Point> *pts = new vector<cv::Point>[corners.size()];
|
||||
|
||||
for (unsigned int i = 0; i < corners.size(); i++)
|
||||
{
|
||||
int index1, index2;
|
||||
if (order)
|
||||
{
|
||||
index1 = indices[i];
|
||||
index2 = indices[(i + 1) % corners.size()];
|
||||
}
|
||||
else
|
||||
{
|
||||
index1 = indices[(i + 1) % corners.size()];
|
||||
index2 = indices[i];
|
||||
}
|
||||
|
||||
if (index1 < index2)
|
||||
{
|
||||
pts[i].resize(index2 - index1);
|
||||
copy(contour.begin() + index1, contour.begin() + index2, pts[i].begin());
|
||||
}
|
||||
else
|
||||
{
|
||||
pts[i].resize(index2 + contour.size() - index1);
|
||||
copy(contour.begin() + index1, contour.end(), pts[i].begin());
|
||||
copy(contour.begin(), contour.begin() + index2, pts[i].end() - index2);
|
||||
}
|
||||
}
|
||||
|
||||
cv::Vec4f *lines = new cv::Vec4f[corners.size()];
|
||||
|
||||
for (unsigned int i = 0; i < corners.size(); i++)
|
||||
{
|
||||
cv::fitLine(pts[i], lines[i], CV_DIST_L2, 0, 0.01, 0.01);
|
||||
}
|
||||
|
||||
vector<cv::Point2f> corners2;
|
||||
for (unsigned int i = corners.size() - 1; i < 2 * corners.size() - 1; i++)
|
||||
corners2.push_back(GetIntersection(lines[(i + 1) % corners.size()], lines[i % corners.size()]));
|
||||
|
||||
corners = corners2;
|
||||
}
|
||||
|
||||
cv::Point2f MarkerDetector::GetIntersection(cv::Vec4f lin1, cv::Vec4f lin2)
|
||||
{
|
||||
float c1 = lin2[2] - lin1[2];
|
||||
float c2 = lin2[3] - lin1[3];
|
||||
float a1 = lin1[0];
|
||||
float a2 = lin1[1];
|
||||
float b1 = -lin2[0];
|
||||
float b2 = -lin2[1];
|
||||
|
||||
cv::Mat A(2, 2, CV_32F);
|
||||
cv::Mat b(2, 1, CV_32F);
|
||||
cv::Mat dst(2, 1, CV_32F);
|
||||
|
||||
A.at<float>(0, 0) = a1;
|
||||
A.at<float>(0, 1) = b1;
|
||||
A.at<float>(1, 0) = a2;
|
||||
A.at<float>(1, 1) = b2;
|
||||
b.at<float>(0, 0) = c1;
|
||||
b.at<float>(1, 0) = c2;
|
||||
|
||||
//rozwi¹zuje uk³ad równañ
|
||||
cv::solve(A, b, dst);
|
||||
|
||||
cv::Point2f res(dst.at<float>(0, 0) * lin1[0] + lin1[2], dst.at<float>(0, 0) * lin1[1] + lin1[3]);
|
||||
return res;
|
||||
}
|
||||
|
||||
void MarkerDetector::GetMarkerPoints(vector<Point3f> &pts)
|
||||
{
|
||||
pts.push_back(Point3f(0.0f, -1.0f, 0.0f));
|
||||
pts.push_back(Point3f(-1.0f, -1.6667f, 0.0f));
|
||||
pts.push_back(Point3f(-1.0f, 1.0f, 0.0f));
|
||||
pts.push_back(Point3f(1.0f, 1.0f, 0.0f));
|
||||
pts.push_back(Point3f(1.0f, -1.6667f, 0.0f));
|
||||
}
|
||||
|
||||
void MarkerDetector::GetMarkerPointsForWarp(vector<cv::Point2f> &pts)
|
||||
{
|
||||
pts.push_back(cv::Point2f(0, 1));
|
||||
pts.push_back(cv::Point2f(-1, 1.6667f));
|
||||
pts.push_back(cv::Point2f(-1, -1));
|
||||
pts.push_back(cv::Point2f(1, -1));
|
||||
pts.push_back(cv::Point2f(1, 1.6667f));
|
||||
}
|
||||
|
||||
double MarkerDetector::GetMarkerArea(MarkerInfo &marker)
|
||||
{
|
||||
cv::Mat hull;
|
||||
|
||||
vector<cv::Point2f> cvCorners(nMarkerCorners);
|
||||
for (int i = 0; i < nMarkerCorners; i++)
|
||||
{
|
||||
cvCorners[i] = cv::Point2f(marker.corners[i].X, marker.corners[i].Y);
|
||||
}
|
||||
|
||||
cv::convexHull(cvCorners, hull);
|
||||
return cv::contourArea(hull);
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
// 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},
|
||||
// }
|
||||
#include "utils.h"
|
||||
|
||||
Point3f RotatePoint(Point3f &point, std::vector<std::vector<float>> &R)
|
||||
{
|
||||
Point3f res;
|
||||
|
||||
res.X = point.X * R[0][0] + point.Y * R[0][1] + point.Z * R[0][2];
|
||||
res.Y = point.X * R[1][0] + point.Y * R[1][1] + point.Z * R[1][2];
|
||||
res.Z = point.X * R[2][0] + point.Y * R[2][1] + point.Z * R[2][2];
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Point3f InverseRotatePoint(Point3f &point, std::vector<std::vector<float>> &R)
|
||||
{
|
||||
Point3f res;
|
||||
|
||||
res.X = point.X * R[0][0] + point.Y * R[1][0] + point.Z * R[2][0];
|
||||
res.Y = point.X * R[0][1] + point.Y * R[1][1] + point.Z * R[2][1];
|
||||
res.Z = point.X * R[0][2] + point.Y * R[1][2] + point.Z * R[2][2];
|
||||
|
||||
return res;
|
||||
}
|
||||
@@ -0,0 +1,226 @@
|
||||
#define _WINSOCK_DEPRECATED_NO_WARNINGS
|
||||
|
||||
|
||||
#include "SocketCS.h"
|
||||
#pragma comment(lib, "ws2_32.lib")
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
|
||||
int Socket::nofSockets_= 0;
|
||||
|
||||
void Socket::Start() {
|
||||
if (!nofSockets_) {
|
||||
WSADATA info;
|
||||
if (WSAStartup(MAKEWORD(2,0), &info)) {
|
||||
throw "Could not start WSA";
|
||||
}
|
||||
}
|
||||
++nofSockets_;
|
||||
}
|
||||
|
||||
void Socket::End() {
|
||||
WSACleanup();
|
||||
}
|
||||
|
||||
Socket::Socket() : s_(0) {
|
||||
Start();
|
||||
// UDP: use SOCK_DGRAM instead of SOCK_STREAM
|
||||
s_ = socket(AF_INET,SOCK_STREAM,0);
|
||||
|
||||
if (s_ == INVALID_SOCKET) {
|
||||
throw "INVALID_SOCKET";
|
||||
}
|
||||
|
||||
refCounter_ = new int(1);
|
||||
}
|
||||
|
||||
Socket::Socket(SOCKET s) : s_(s) {
|
||||
Start();
|
||||
refCounter_ = new int(1);
|
||||
};
|
||||
|
||||
Socket::~Socket() {
|
||||
if (! --(*refCounter_)) {
|
||||
Close();
|
||||
delete refCounter_;
|
||||
}
|
||||
|
||||
--nofSockets_;
|
||||
if (!nofSockets_) End();
|
||||
}
|
||||
|
||||
Socket::Socket(const Socket& o) {
|
||||
refCounter_=o.refCounter_;
|
||||
(*refCounter_)++;
|
||||
s_ =o.s_;
|
||||
|
||||
nofSockets_++;
|
||||
}
|
||||
|
||||
Socket& Socket::operator=(Socket& o) {
|
||||
(*o.refCounter_)++;
|
||||
|
||||
refCounter_=o.refCounter_;
|
||||
s_ =o.s_;
|
||||
|
||||
nofSockets_++;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void Socket::Close() {
|
||||
closesocket(s_);
|
||||
}
|
||||
|
||||
std::string Socket::ReceiveBytes() {
|
||||
std::string ret;
|
||||
char buf[1024];
|
||||
|
||||
while (1) {
|
||||
u_long arg = 0;
|
||||
if (ioctlsocket(s_, FIONREAD, &arg) != 0)
|
||||
break;
|
||||
|
||||
if (arg == 0)
|
||||
break;
|
||||
|
||||
if (arg > 1024) arg = 1024;
|
||||
|
||||
int rv = recv (s_, buf, arg, 0);
|
||||
if (rv <= 0) break;
|
||||
|
||||
std::string t;
|
||||
|
||||
t.assign (buf, rv);
|
||||
ret += t;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
std::string Socket::ReceiveLine() {
|
||||
std::string ret;
|
||||
while (1) {
|
||||
char r;
|
||||
|
||||
switch(recv(s_, &r, 1, 0)) {
|
||||
case 0: // not connected anymore;
|
||||
// ... but last line sent
|
||||
// might not end in \n,
|
||||
// so return ret anyway.
|
||||
return ret;
|
||||
case -1:
|
||||
return "";
|
||||
// if (errno == EAGAIN) {
|
||||
// return ret;
|
||||
// } else {
|
||||
// // not connected anymore
|
||||
// return "";
|
||||
// }
|
||||
}
|
||||
|
||||
ret += r;
|
||||
if (r == '\n') return ret;
|
||||
}
|
||||
}
|
||||
|
||||
void Socket::SendLine(std::string s) {
|
||||
s += '\n';
|
||||
send(s_,s.c_str(),s.length(),0);
|
||||
}
|
||||
|
||||
void Socket::SendBytes(const char *buf, int len) {
|
||||
send(s_,buf,len,0);
|
||||
}
|
||||
|
||||
SocketServer::SocketServer(int port, int connections, TypeSocket type) {
|
||||
sockaddr_in sa;
|
||||
|
||||
memset(&sa, 0, sizeof(sa));
|
||||
|
||||
sa.sin_family = PF_INET;
|
||||
sa.sin_port = htons(port);
|
||||
s_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (s_ == INVALID_SOCKET) {
|
||||
throw "INVALID_SOCKET";
|
||||
}
|
||||
|
||||
if(type==NonBlockingSocket) {
|
||||
u_long arg = 1;
|
||||
ioctlsocket(s_, FIONBIO, &arg);
|
||||
}
|
||||
|
||||
/* bind the socket to the internet address */
|
||||
if (bind(s_, (sockaddr *)&sa, sizeof(sockaddr_in)) == SOCKET_ERROR) {
|
||||
closesocket(s_);
|
||||
throw "INVALID_SOCKET";
|
||||
}
|
||||
|
||||
listen(s_, connections);
|
||||
}
|
||||
|
||||
Socket* SocketServer::Accept() {
|
||||
SOCKET new_sock = accept(s_, 0, 0);
|
||||
if (new_sock == INVALID_SOCKET) {
|
||||
int rc = WSAGetLastError();
|
||||
if(rc==WSAEWOULDBLOCK) {
|
||||
return 0; // non-blocking call, no request pending
|
||||
}
|
||||
else {
|
||||
throw "Invalid Socket";
|
||||
}
|
||||
}
|
||||
|
||||
Socket* r = new Socket(new_sock);
|
||||
return r;
|
||||
}
|
||||
|
||||
SocketClient::SocketClient(const std::string& host, int port) : Socket() {
|
||||
std::string error;
|
||||
|
||||
hostent *he;
|
||||
if ((he = gethostbyname(host.c_str())) == 0) {
|
||||
error = strerror(errno);
|
||||
throw error;
|
||||
}
|
||||
|
||||
sockaddr_in addr;
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port);
|
||||
addr.sin_addr = *((in_addr *)he->h_addr);
|
||||
memset(&(addr.sin_zero), 0, 8);
|
||||
|
||||
if (::connect(s_, (sockaddr *) &addr, sizeof(sockaddr))) {
|
||||
error = strerror(WSAGetLastError());
|
||||
throw error;
|
||||
}
|
||||
}
|
||||
|
||||
SocketSelect::SocketSelect(Socket const * const s1, Socket const * const s2, TypeSocket type) {
|
||||
FD_ZERO(&fds_);
|
||||
FD_SET(const_cast<Socket*>(s1)->s_,&fds_);
|
||||
if(s2) {
|
||||
FD_SET(const_cast<Socket*>(s2)->s_,&fds_);
|
||||
}
|
||||
|
||||
TIMEVAL tval;
|
||||
tval.tv_sec = 0;
|
||||
tval.tv_usec = 1;
|
||||
|
||||
TIMEVAL *ptval;
|
||||
if(type==NonBlockingSocket) {
|
||||
ptval = &tval;
|
||||
}
|
||||
else {
|
||||
ptval = 0;
|
||||
}
|
||||
|
||||
if (select (0, &fds_, (fd_set*) 0, (fd_set*) 0, ptval) == SOCKET_ERROR)
|
||||
throw "Error in select";
|
||||
}
|
||||
|
||||
bool SocketSelect::Readable(Socket const* const s) {
|
||||
if (FD_ISSET(s->s_,&fds_)) return true;
|
||||
return false;
|
||||
}
|
||||
Reference in New Issue
Block a user