39 #include <Eigen/Dense>
65 float a = -0.0030711f;
80 m_grabber->setDepthFormat(FREENECT_DEPTH_11BIT);
93 std::vector<short> depthImage(480 * 680, 0);
96 std::vector<uint8_t> colorImage(480 * 680 * 3, 0);
100 for(
size_t i = 0; i < depthImage.size(); i++)
102 if(
isnan(depthImage[i])) nans.insert(i);
108 size_t numPoints = depthImage.size() - nans.size();
112 floatArr points(
new float[numPoints * 3]);
118 for (i = 0; i < 480; i++) {
119 for (j = 0; j < 640; j++) {
121 if(nans.find(c) == nans.end())
124 v << j, i, (float)(depthImage[i * 640 + j]), 1.0f;
127 points[3 * index ] = v(0) / v(3);
128 points[3 * index + 1] = v(1) / v(3);
129 points[3 * index + 2] = v(2) / v(3);
131 colors[3 * index ] = colorImage[3 * c ];
132 colors[3 * index + 1] = colorImage[3 * c + 1];
133 colors[3 * index + 2] = colorImage[3 * c + 2];
140 buffer->setPointArray(points, numPoints);
141 buffer->setColorArray(
colors, numPoints);