On the bottom is the full code for what i am making.
What to look for is this part:
// Reject points outside the range [0..1]
if ((dX >= 0) && (dX <= 1) && (dY >= 0) && (dY <= 1)) {
//Denormalize from [0..1] to [0..width] or [0..height]
denormalizedX = dX * width;
denormalizedY = dY * height;
//look up the brightness value for the target pixel
destination[i][j] = raw[denormalizedX + denormalizedY * width];
}
else {
destination[i][j] = -1;
}
This "if" statement is always false, and I am getting destination[i][j] all -1, which is white.
I tried looking at the interpolation algorithm, but couldn't find any solution.
What can possibly be wrong here?
#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <iostream>
#include <vector>
#include "Leap.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// GLEW
#define GLEW_STATIC
#include <GL/glew.h>
#include <GL/glut.h>
// GLFW
#include <GLFW/glfw3.h>
GLFWwindow* _window;
GLint windowWidth = 1280; // 640 * 2
GLint windowHeight = 240; // 240
GLfloat fieldOfView = 45.0f; // FoV
GLfloat zNear = 0.1f; // Near clip plane
GLfloat zFar = 200.0f; // Far clip plane
GLfloat padding = 0.2f;
// Frame counting and limiting
int frameCount = 0;
double frameStartTime, frameEndTime, frameDrawTime;
cv::Mat _mats[2];
const char* WIN_LEFT_CAM = "Left Camera";
const char* WIN_RIGHT_CAM = "Right Camera";
// OpenGL GSGL Shader
GLuint _distortionShaderProgram;
GLuint _distortionFbo;
GLuint _outputTexture;
void distortionFix(Leap::Image *image, cv::Mat *destinationMat);
class SampleListener : public Leap::Listener {
public:
virtual void onConnect(const Leap::Controller&); // LM is plugged in
virtual void onFrame(const Leap::Controller&); // New frame of hand and finger tracking data is available
private:
};
void SampleListener::onConnect(const Leap::Controller& controller) {
std::cout << "LeapMotion Connected" << std::endl;
}
void SampleListener::onFrame(const Leap::Controller& controller) {
// Get the most recent frame and report some basic information
Leap::Frame frame = controller.frame();
Leap::ImageList images = frame.images();
for (int i = 0; i < 2; i++) {
Leap::Image image = images[i];
const char* window;
cv::Mat *result = &_mats[i];
if (i == 0) {
// Left Image
window = WIN_LEFT_CAM;
}
else {
// Right Image
window = WIN_RIGHT_CAM;
}
if (image.isValid()) {
// Get the Raw Images
int rows = image.height(); // 240
int cols = image.width(); // 640
result->create(rows, cols, CV_8UC1); // allocate if necessary
//const uchar* image_buffer = image.data(); // brightness values
//memcpy(result->data, image_buffer, sizeof(uchar)*rows*cols*image.bytesPerPixel());
distortionFix(&image, result);
cv::namedWindow(window);
if (i == 0) {
cv::moveWindow(window, 0, 0);
}
else {
cv::moveWindow(window, cols, 0);
cv::waitKey(1);
}
//if (_distortionFbo == 0) {
// glGenFramebuffers(1, &_distortionFbo);
//}
//glBindFramebuffer(GL_FRAMEBUFFER, _distortionFbo);
//_outputTexture = matToTexture(result, GL_NEAREST, GL_NEAREST, GL_CLAMP);
//glBindFramebuffer(GL_FRAMEBUFFER, 0);
cv::imshow(window, *result);
}
}
}
void distortionFix(Leap::Image *image, cv::Mat *destinationMat) {
const int destinationWidth = 640;
const int destinationHeight = 240;
unsigned char destination[destinationWidth][destinationHeight];
//define needed variables outside the inner loop
float calibrationX, calibrationY;
float weightX, weightY;
float dX, dX1, dX2, dX3, dX4;
float dY, dY1, dY2, dY3, dY4;
int x1, x2, y1, y2;
int denormalizedX, denormalizedY;
int i, j;
const unsigned char* raw = image->data();
const float* distortion_buffer = image->distortion();
//Local variables for values needed in loop
const int distortionWidth = image->distortionWidth();
const int width = image->width();
const int height = image->height();
for (i = 0; i < destinationWidth; i += 1) {
for (j = 0; j < destinationHeight; j += 1) {
//Calculate the position in the calibration map (still with a fractional part)
calibrationX = 63 * i / destinationWidth;
calibrationY = 62 * (1 - j / destinationHeight); // The y origin is at the bottom
//Save the fractional part to use as the weight for interpolation
weightX = calibrationX - truncf(calibrationX);
weightY = calibrationY - truncf(calibrationY);
//Get the x,y coordinates of the closest calibration map points to the target pixel
x1 = calibrationX; //Note truncation to int
y1 = calibrationY;
x2 = x1 + 1;
y2 = y1 + 1;
//Look up the x and y values for the 4 calibration map points around the target
dX1 = distortion_buffer[x1 * 2 + y1 * distortionWidth];
dX2 = distortion_buffer[x2 * 2 + y1 * distortionWidth];
dX3 = distortion_buffer[x1 * 2 + y2 * distortionWidth];
dX4 = distortion_buffer[x2 * 2 + y2 * distortionWidth];
dY1 = distortion_buffer[x1 * 2 + y1 * distortionWidth + 1];
dY2 = distortion_buffer[x2 * 2 + y1 * distortionWidth + 1];
dY3 = distortion_buffer[x1 * 2 + y2 * distortionWidth + 1];
dY4 = distortion_buffer[x2 * 2 + y2 * distortionWidth + 1];
/*
std::cout << i << ", " << j << " -- " << x1 << ", " << y1 << ", " << x2 << ", " << y2 << " -- "
<< (x1 * 2 + y1 * distortionWidth) << ", "
<< (x1 * 2 + y1 * distortionWidth + 1) << " -- "
<< (x2 * 2 + y2 * distortionWidth) << ", "
<< (x2 * 2 + y2 * distortionWidth + 1) << std::endl;
*/
//Bilinear interpolation of the looked-up values:
// X value
dX = dX1 * (1 - weightX) * (1 - weightY) +
dX2 * weightX * (1 - weightY) +
dX3 * (1 - weightX) * weightY +
dX4 * weightX * weightY;
// Y value
dY = dY1 * (1 - weightX) * (1 - weightY) +
dY2 * weightX * (1 - weightY) +
dY3 * (1 - weightX) * weightY +
dY4 * weightX * weightY;
// Reject points outside the range [0..1]
if ((dX >= 0) && (dX <= 1) && (dY >= 0) && (dY <= 1)) {
//Denormalize from [0..1] to [0..width] or [0..height]
denormalizedX = dX * width;
denormalizedY = dY * height;
//look up the brightness value for the target pixel
destination[i][j] = raw[denormalizedX + denormalizedY * width];
}
else {
destination[i][j] = -1;
}
destination[i][j] = raw[i + j * width];
}
}
for (int i1 = 0; i1 < destinationHeight; i1++)
for (int j1 = 0; j1 < destinationWidth; j1++)
destinationMat->at<unsigned char>(i1, j1) = destination[j1][i1];
}
// Function turn a cv::Mat into a texture, and return the texture ID as a GLuint for use
GLuint matToTexture(cv::Mat &mat, GLenum minFilter, GLenum magFilter, GLenum wrapFilter)
{
// Generate a number for our textureID's unique handle
GLuint textureID;
glGenTextures(1, &textureID);
// Bind to our texture handle
glBindTexture(GL_TEXTURE_2D, textureID);
// Catch silly-mistake texture interpolation method for magnification
if (magFilter == GL_LINEAR_MIPMAP_LINEAR ||
magFilter == GL_LINEAR_MIPMAP_NEAREST ||
magFilter == GL_NEAREST_MIPMAP_LINEAR ||
magFilter == GL_NEAREST_MIPMAP_NEAREST)
{
std::cout << "You can't use MIPMAPs for magnification - setting filter to GL_LINEAR" << std::endl;
magFilter = GL_LINEAR;
}
// Set texture interpolation methods for minification and magnification
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, minFilter);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, magFilter);
// Set texture clamping method
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, wrapFilter);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, wrapFilter);
// Set incoming texture format to:
// GL_BGR for CV_CAP_OPENNI_BGR_IMAGE,
// GL_LUMINANCE for CV_CAP_OPENNI_DISPARITY_MAP,
// Work out other mappings as required ( there's a list in comments in main() )
GLenum inputColourFormat = GL_BGR;
if (mat.channels() == 1)
{
inputColourFormat = GL_LUMINANCE;
}
// Create the texture
glTexImage2D(GL_TEXTURE_2D, // Type of texture
0, // Pyramid level (for mip-mapping) - 0 is the top level
GL_RGB, // Internal colour format to convert to
mat.cols, // Image width i.e. 640 for Kinect in standard mode
mat.rows, // Image height i.e. 480 for Kinect in standard mode
0, // Border width in pixels (can either be 1 or 0)
inputColourFormat, // Input image format (i.e. GL_RGB, GL_RGBA, GL_BGR etc.)
GL_UNSIGNED_BYTE, // Image data type
mat.ptr()); // The actual image data itself
// If we're using mipmaps then generate them. Note: This requires OpenGL 3.0 or higher
if (minFilter == GL_LINEAR_MIPMAP_LINEAR ||
minFilter == GL_LINEAR_MIPMAP_NEAREST ||
minFilter == GL_NEAREST_MIPMAP_LINEAR ||
minFilter == GL_NEAREST_MIPMAP_NEAREST)
{
glGenerateMipmap(GL_TEXTURE_2D);
}
return textureID;
}
void draw(cv::Mat &frame0, cv::Mat &frame1)
{
// Clear the screen and depth buffer, and reset the ModelView matrix to identity
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
// Move things back into the screen
glTranslatef(0.0f, 0.0f, -8.0f);
glEnable(GL_TEXTURE_2D);
// Quad width and height
float w = windowWidth / 40.f;
float h = windowHeight / 20.f;
// Convert image and depth data to OpenGL textures
GLuint frameTex0 = matToTexture(frame0, GL_LINEAR_MIPMAP_LINEAR, GL_LINEAR, GL_CLAMP);
GLuint frameTex1 = matToTexture(frame1, GL_LINEAR_MIPMAP_LINEAR, GL_LINEAR, GL_CLAMP);
// Draw the textures
// Note: Window co-ordinates origin is top left, texture co-ordinate origin is bottom left.
// Frame 0
glTranslatef(-w / 2 - padding / 2, 0, 0);
glBindTexture(GL_TEXTURE_2D, frameTex0);
glBegin(GL_QUADS);
glTexCoord2f(1, 1);
glVertex2f(w / 2, -h / 2);
glTexCoord2f(1, 0);
glVertex2f(w / 2, h / 2);
glTexCoord2f(0, 0);
glVertex2f(-w / 2, h / 2);
glTexCoord2f(0, 1);
glVertex2f(-w / 2, -h / 2);
glEnd();
glTranslatef(w + padding, 0, 0);
// Frame 1
glBindTexture(GL_TEXTURE_2D, frameTex1);
glBegin(GL_QUADS);
glTexCoord2f(1, 1);
glVertex2f(w / 2, -h / 2);
glTexCoord2f(1, 0);
glVertex2f(w / 2, h / 2);
glTexCoord2f(0, 0);
glVertex2f(-w / 2, h / 2);
glTexCoord2f(0, 1);
glVertex2f(-w / 2, -h / 2);
glEnd();
// Free the texture memory
glDeleteTextures(1, &frameTex0);
glDeleteTextures(1, &frameTex1);
glDisable(GL_TEXTURE_2D);
}
void key_(int theKey, int theAction)
{
if (theAction == GLFW_PRESS)
{
switch (theKey)
{
case GLFW_KEY_ESCAPE:
break;
default:
break;
} // End of switch statement
} // End of GLFW_PRESS
}
void initGL()
{
// Define our buffer settings
int redBits = 8, greenBits = 8, blueBits = 8;
int alphaBits = 8, depthBits = 24, stencilBits = 8;
// Initialise glfw
glfwInit();
// Create a window
_window = glfwCreateWindow(windowWidth, windowHeight, "HyoJoonPark 2017-24482", nullptr, nullptr);
if(_window == nullptr)
{
std::cout << "Failed to open window!" << std::endl;
glfwTerminate();
exit(-1);
}
glfwMakeContextCurrent(_window);
// Specify the callback function for key presses/releases
//glfwSetKeyCallback(handleKeypress);
// Initialise glew (must occur AFTER window creation or glew will error)
GLenum err = glewInit();
if (GLEW_OK != err)
{
std::cout << "GLEW initialisation error: " << glewGetErrorString(err) << std::endl;
exit(-1);
}
std::cout << "GLEW okay - using version: " << glewGetString(GLEW_VERSION) << std::endl;
// Setup our viewport to be the entire size of the window
glViewport(0, 0, (GLsizei) windowWidth, (GLsizei) windowHeight);
// Change to the projection matrix and set our viewing volume
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(fieldOfView, windowWidth / windowHeight, zNear, zFar);
// ----- OpenGL settings -----
glDepthFunc(GL_LEQUAL); // Specify depth function to use
glEnable(GL_DEPTH_TEST); // Enable the depth buffer
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); // Ask for nicest perspective correction
glEnable(GL_CULL_FACE); // Cull back facing polygons
glfwSwapInterval(1); // Lock screen updates to vertical refresh
// Switch to ModelView matrix and reset
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void lockFramerate(double framerate)
{
// Note: frameStartTime is called first thing in the main loop
// Our allowed frame time is 1 second divided by the desired FPS
static double allowedFrameTime = 1.0 / framerate;
// Get current time
frameEndTime = glfwGetTime();
// Calc frame draw time
frameDrawTime = frameEndTime - frameStartTime;
double sleepTime = 0.0;
// Sleep if we've got time to kill before the next frame
if (frameDrawTime < allowedFrameTime)
{
sleepTime = allowedFrameTime - frameDrawTime;
//glfwSleep(sleepTime);
}
// Debug stuff
double potentialFPS = 1.0 / frameDrawTime;
double lockedFPS = 1.0 / (glfwGetTime() - frameStartTime);
std::cout << "Draw: " << frameDrawTime << " Sleep: " << sleepTime;
std::cout << " Pot. FPS: " << potentialFPS << " Locked FPS: " << lockedFPS << std::endl;
}
int main(int argc, char** argv) {
// Set up our OpenGL window, projection and options
initGL();
SampleListener listener;
// Add a controler object that it's automatically connected to the LM service
Leap::Controller controller;
controller.addListener(listener);
if (argc > 1 && strcmp(argv[1], "--bg") == 0) {
controller.setPolicy(Leap::Controller::POLICY_BACKGROUND_FRAMES);
}
controller.setPolicy(Leap::Controller::POLICY_IMAGES);
while (!glfwWindowShouldClose(_window))
{
glEnable(GL_DEPTH_TEST);
// Check and call events
glfwPollEvents();
// Reset buffers
glClearColor(0.3f, 0.1f, 0.1f, 1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
draw(_mats[0], _mats[1]);
frameCount++;
// Swap the buffers
glfwSwapBuffers(_window);
}
glfwTerminate();
return 0;
}