Freenect.cc 13.8 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Definition of the class Freenect.
 *
 *   @author Lionel Heng <hengli@student.ethz.ch>
 *
 */

32 33
#include "Freenect.h"

34
#include <cmath>
35
#include <string.h>
36
#include <QSettings>
37 38 39 40 41

Freenect::Freenect()
    : context(NULL)
    , device(NULL)
    , tiltAngle(0)
42 43 44 45 46
    , rgbData(new QByteArray)
    , rawDepthData(new QByteArray)
    , coloredDepthData(new QByteArray)
    , pointCloud3D(new QVector<QVector3D>)
    , pointCloud6D(new QVector<Vector6D>)
47
{
48

49 50 51 52
}

Freenect::~Freenect()
{
53
    if (device != NULL) {
54 55 56 57 58 59 60 61 62 63 64 65 66 67
        freenect_stop_depth(device);
        freenect_stop_video(device);
    }

    freenect_close_device(device);

    freenect_shutdown(context);
}

bool
Freenect::init(int userDeviceNumber)
{
    // read in settings
    readConfigFile();
68

69
    // populate gamma lookup table
70
    for (int i = 0; i < 2048; ++i) {
71 72 73 74
        float v = static_cast<float>(i) / 2048.0f;
        v = powf(v, 3.0f) * 6.0f;
        gammaTable[i] = static_cast<unsigned short>(v * 6.0f * 256.0f);
    }
75 76

    // populate depth projection matrix
77 78
    for (int i = 0; i < FREENECT_FRAME_H; ++i) {
        for (int j = 0; j < FREENECT_FRAME_W; ++j) {
79 80 81 82 83 84 85 86
            QVector2D originalPoint(j, i);
            QVector2D rectifiedPoint;
            rectifyPoint(originalPoint, rectifiedPoint, depthCameraParameters);

            QVector3D rectifiedRay;
            projectPixelTo3DRay(rectifiedPoint, rectifiedRay, depthCameraParameters);

            depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay;
87 88 89

            rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters);
            rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint;
90 91
        }
    }
92

93
    if (freenect_init(&context, NULL) < 0) {
94 95 96 97 98
        return false;
    }

    freenect_set_log_level(context, FREENECT_LOG_DEBUG);

99
    if (freenect_num_devices(context) < 1) {
100 101 102
        return false;
    }

103
    if (freenect_open_device(context, &device, userDeviceNumber) < 0) {
104 105 106 107 108
        return false;
    }

    freenect_set_user(device, this);

109 110
    memset(rgb, 0, FREENECT_VIDEO_RGB_SIZE);
    memset(depth, 0, FREENECT_DEPTH_11BIT_SIZE);
111 112

    // set Kinect parameters
113
    if (freenect_set_tilt_degs(device, tiltAngle) != 0) {
114 115
        return false;
    }
116
    if (freenect_set_led(device, LED_RED) != 0) {
117 118
        return false;
    }
119
    if (freenect_set_video_format(device, FREENECT_VIDEO_RGB) != 0) {
120 121
        return false;
    }
122
    if (freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) != 0) {
123 124
        return false;
    }
125
    freenect_set_video_callback(device, videoCallback);
126 127
    freenect_set_depth_callback(device, depthCallback);

128
    if (freenect_start_depth(device) != 0) {
129 130
        return false;
    }
131
    if (freenect_start_video(device) != 0) {
132 133 134 135 136 137 138 139 140 141 142 143
        return false;
    }

    thread.reset(new FreenectThread(device));
    thread->start();

    return true;
}

bool
Freenect::process(void)
{
144
    if (freenect_process_events(context) < 0) {
145 146 147
        return false;
    }

148 149 150 151
    freenect_raw_tilt_state* state;
    freenect_update_tilt_state(device);
    state = freenect_get_tilt_state(device);
    freenect_get_mks_accel(state, &ax, &ay, &az);
152 153 154 155 156 157 158 159

    return true;
}

QSharedPointer<QByteArray>
Freenect::getRgbData(void)
{
    QMutexLocker locker(&rgbMutex);
160 161 162 163
    rgbData->clear();
    rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE);

    return rgbData;
164 165 166
}

QSharedPointer<QByteArray>
167
Freenect::getRawDepthData(void)
168 169
{
    QMutexLocker locker(&depthMutex);
170 171 172 173
    rawDepthData->clear();
    rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE);

    return rawDepthData;
174 175 176 177 178 179
}

QSharedPointer<QByteArray>
Freenect::getColoredDepthData(void)
{
    QMutexLocker locker(&coloredDepthMutex);
180 181 182 183
    coloredDepthData->clear();
    coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE);

    return coloredDepthData;
184 185
}

186
QSharedPointer< QVector<QVector3D> >
187
Freenect::get3DPointCloudData(void)
188 189 190
{
    QMutexLocker locker(&depthMutex);

191
    pointCloud3D->clear();
192
    unsigned short* data = reinterpret_cast<unsigned short*>(depth);
193 194
    for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
        if (data[i] > 0 && data[i] <= 2048) {
195 196 197
            double range = baseline * depthCameraParameters.fx
                           / (1.0 / 8.0 * (disparityOffset
                                           - static_cast<double>(data[i])));
198

199
            if (range > 0.0f) {
200 201 202
                QVector3D ray = depthProjectionMatrix[i];
                ray *= range;

203
                pointCloud3D->push_back(QVector3D(ray.x(), ray.y(), ray.z()));
204 205 206 207
            }
        }
    }

208
    return pointCloud3D;
209 210
}

211
QSharedPointer< QVector<Freenect::Vector6D> >
212 213
Freenect::get6DPointCloudData(void)
{
214
    get3DPointCloudData();
215

216
    pointCloud6D->clear();
217
    for (int i = 0; i < pointCloud3D->size(); ++i) {
218 219
        Vector6D point;

220 221 222
        point.x = pointCloud3D->at(i).x();
        point.y = pointCloud3D->at(i).y();
        point.z = pointCloud3D->at(i).z();
223 224 225 226 227 228 229 230 231 232 233

        QVector4D transformedPoint = transformMatrix * QVector4D(point.x, point.y, point.z, 1.0);

        float iz = 1.0 / transformedPoint.z();
        QVector2D rectifiedPoint(transformedPoint.x() * iz * rgbCameraParameters.fx + rgbCameraParameters.cx,
                                 transformedPoint.y() * iz * rgbCameraParameters.fy + rgbCameraParameters.cy);

        QVector2D originalPoint;
        unrectifyPoint(rectifiedPoint, originalPoint, rgbCameraParameters);

        if (originalPoint.x() >= 0.0 && originalPoint.x() < FREENECT_FRAME_W &&
234
                originalPoint.y() >= 0.0 && originalPoint.y() < FREENECT_FRAME_H) {
235 236 237 238 239 240 241 242 243
            int x = static_cast<int>(originalPoint.x());
            int y = static_cast<int>(originalPoint.y());
            unsigned char* pixel = reinterpret_cast<unsigned char*>(rgb)
                                   + (y * FREENECT_FRAME_W + x) * 3;

            point.r = pixel[0];
            point.g = pixel[1];
            point.b = pixel[2];

244
            pointCloud6D->push_back(point);
245 246 247
        }
    }

248
    return pointCloud6D;
249 250
}

251 252 253 254 255 256 257 258 259
int
Freenect::getTiltAngle(void) const
{
    return tiltAngle;
}

void
Freenect::setTiltAngle(int angle)
{
260
    if (angle > 30) {
261 262
        angle = 30;
    }
263
    if (angle < -30) {
264 265 266 267 268 269 270 271 272 273 274 275 276 277 278
        angle = -30;
    }

    tiltAngle = angle;
}

Freenect::FreenectThread::FreenectThread(freenect_device* _device)
{
    device = _device;
}

void
Freenect::FreenectThread::run(void)
{
    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
279
    while (1) {
280 281 282 283
        freenect->process();
    }
}

284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321
void
Freenect::readConfigFile(void)
{
    QSettings settings("data/kinect.cal", QSettings::IniFormat, 0);

    rgbCameraParameters.cx = settings.value("rgb/principal_point/x").toDouble();
    rgbCameraParameters.cy = settings.value("rgb/principal_point/y").toDouble();
    rgbCameraParameters.fx = settings.value("rgb/focal_length/x").toDouble();
    rgbCameraParameters.fy = settings.value("rgb/focal_length/y").toDouble();
    rgbCameraParameters.k[0] = settings.value("rgb/distortion/k1").toDouble();
    rgbCameraParameters.k[1] = settings.value("rgb/distortion/k2").toDouble();
    rgbCameraParameters.k[2] = settings.value("rgb/distortion/k3").toDouble();
    rgbCameraParameters.k[3] = settings.value("rgb/distortion/k4").toDouble();
    rgbCameraParameters.k[4] = settings.value("rgb/distortion/k5").toDouble();

    depthCameraParameters.cx = settings.value("depth/principal_point/x").toDouble();
    depthCameraParameters.cy = settings.value("depth/principal_point/y").toDouble();
    depthCameraParameters.fx = settings.value("depth/focal_length/x").toDouble();
    depthCameraParameters.fy = settings.value("depth/focal_length/y").toDouble();
    depthCameraParameters.k[0] = settings.value("depth/distortion/k1").toDouble();
    depthCameraParameters.k[1] = settings.value("depth/distortion/k2").toDouble();
    depthCameraParameters.k[2] = settings.value("depth/distortion/k3").toDouble();
    depthCameraParameters.k[3] = settings.value("depth/distortion/k4").toDouble();
    depthCameraParameters.k[4] = settings.value("depth/distortion/k5").toDouble();

    transformMatrix = QMatrix4x4(settings.value("transform/R11").toDouble(),
                                 settings.value("transform/R12").toDouble(),
                                 settings.value("transform/R13").toDouble(),
                                 settings.value("transform/Tx").toDouble(),
                                 settings.value("transform/R21").toDouble(),
                                 settings.value("transform/R22").toDouble(),
                                 settings.value("transform/R23").toDouble(),
                                 settings.value("transform/Ty").toDouble(),
                                 settings.value("transform/R31").toDouble(),
                                 settings.value("transform/R32").toDouble(),
                                 settings.value("transform/R33").toDouble(),
                                 settings.value("transform/Tz").toDouble(),
                                 0.0, 0.0, 0.0, 1.0);
322
    transformMatrix = transformMatrix.inverted();
323 324 325

    baseline = settings.value("transform/baseline").toDouble();
    disparityOffset = settings.value("transform/disparity_offset").toDouble();
326 327
}

328
void
329 330
Freenect::rectifyPoint(const QVector2D& originalPoint,
                       QVector2D& rectifiedPoint,
331 332 333 334 335 336 337 338 339
                       const IntrinsicCameraParameters& params)
{
    double x = (originalPoint.x() - params.cx) / params.fx;
    double y = (originalPoint.y() - params.cy) / params.fy;

    double x0 = x;
    double y0 = y;

    // eliminate lens distortion iteratively
340
    for (int i = 0; i < 4; ++i) {
341 342 343 344 345 346 347 348 349 350 351 352 353 354 355
        double r2 = x * x + y * y;

        // tangential distortion vector [dx dy]
        double dx = 2 * params.k[2] * x * y + params.k[3] * (r2 + 2 * x * x);
        double dy = params.k[2] * (r2 + 2 * y * y) + 2 * params.k[3] * x * y;

        double icdist = 1.0 / (1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4])));
        x = (x0 - dx) * icdist;
        y = (y0 - dy) * icdist;
    }

    rectifiedPoint.setX(x * params.fx + params.cx);
    rectifiedPoint.setY(y * params.fy + params.cy);
}

356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377
void
Freenect::unrectifyPoint(const QVector2D& rectifiedPoint,
                         QVector2D& originalPoint,
                         const IntrinsicCameraParameters& params)
{
    double x = (rectifiedPoint.x() - params.cx) / params.fx;
    double y = (rectifiedPoint.y() - params.cy) / params.fy;

    double r2 = x * x + y * y;

    // tangential distortion vector [dx dy]
    double dx = 2 * params.k[2] * x * y + params.k[3] * (r2 + 2 * x * x);
    double dy = params.k[2] * (r2 + 2 * y * y) + 2 * params.k[3] * x * y;

    double cdist = 1.0 + r2 * (params.k[0] + r2 * (params.k[1] + r2 * params.k[4]));
    x = x * cdist + dx;
    y = y * cdist + dy;

    originalPoint.setX(x * params.fx + params.cx);
    originalPoint.setY(y * params.fy + params.cy);
}

378 379 380 381 382 383 384 385 386
void
Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
                              const IntrinsicCameraParameters& params)
{
    ray.setX((pixel.x() - params.cx) / params.fx);
    ray.setY((pixel.y() - params.cy) / params.fy);
    ray.setZ(1.0);
}

387
void
388
Freenect::videoCallback(freenect_device* device, void* video, uint32_t timestamp)
389 390 391 392
{
    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));

    QMutexLocker locker(&freenect->rgbMutex);
393
    memcpy(freenect->rgb, video, FREENECT_VIDEO_RGB_SIZE);
394 395 396
}

void
397
Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
398 399
{
    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
400
    uint16_t* data = reinterpret_cast<uint16_t *>(depth);
401

402
    QMutexLocker depthLocker(&freenect->depthMutex);
403
    memcpy(freenect->depth, data, FREENECT_DEPTH_11BIT_SIZE);
404 405 406 407

    QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
    unsigned short* src = reinterpret_cast<unsigned short *>(data);
    unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);
408
    for (int i = 0; i < FREENECT_FRAME_PIX; ++i) {
409 410
        unsigned short pval = freenect->gammaTable[src[i]];
        unsigned short lb = pval & 0xFF;
411
        switch (pval >> 8) {
412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448
        case 0:
            dst[3 * i] = 255;
            dst[3 * i + 1] = 255 - lb;
            dst[3 * i + 2] = 255 - lb;
            break;
        case 1:
            dst[3 * i] = 255;
            dst[3 * i + 1] = lb;
            dst[3 * i + 2] = 0;
            break;
        case 2:
            dst[3 * i] = 255 - lb;
            dst[3 * i + 1] = 255;
            dst[3 * i + 2] = 0;
            break;
        case 3:
            dst[3 * i] = 0;
            dst[3 * i + 1] = 255;
            dst[3 * i + 2] = lb;
            break;
        case 4:
            dst[3 * i] = 0;
            dst[3 * i + 1] = 255 - lb;
            dst[3 * i + 2] = 255;
            break;
        case 5:
            dst[3 * i] = 0;
            dst[3 * i + 1] = 0;
            dst[3 * i + 2] = 255 - lb;
            break;
        default:
            dst[3 * i] = 0;
            dst[3 * i + 1] = 0;
            dst[3 * i + 2] = 0;
            break;
        }
    }
449
}