Freenect.cc 14 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 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68
}

Freenect::~Freenect()
{
    if (device != NULL)
    {
        freenect_stop_depth(device);
        freenect_stop_video(device);
    }

    freenect_close_device(device);

    freenect_shutdown(context);
}

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

70
    // populate gamma lookup table
71 72 73 74 75 76
    for (int i = 0; i < 2048; ++i)
    {
        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);
    }
77 78 79 80 81 82 83 84 85 86 87 88 89 90

    // populate depth projection matrix
    for (int i = 0; i < FREENECT_FRAME_H; ++i)
    {
        for (int j = 0; j < FREENECT_FRAME_W; ++j)
        {
            QVector2D originalPoint(j, i);
            QVector2D rectifiedPoint;
            rectifyPoint(originalPoint, rectifiedPoint, depthCameraParameters);

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

            depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay;
91 92 93

            rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters);
            rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint;
94 95
        }
    }
96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115

    if (freenect_init(&context, NULL) < 0)
    {
        return false;
    }

    freenect_set_log_level(context, FREENECT_LOG_DEBUG);

    if (freenect_num_devices(context) < 1)
    {
        return false;
    }

    if (freenect_open_device(context, &device, userDeviceNumber) < 0)
    {
        return false;
    }

    freenect_set_user(device, this);

116 117
    memset(rgb, 0, FREENECT_VIDEO_RGB_SIZE);
    memset(depth, 0, FREENECT_DEPTH_11BIT_SIZE);
118 119 120 121 122 123 124 125 126 127

    // set Kinect parameters
    if (freenect_set_tilt_degs(device, tiltAngle) != 0)
    {
        return false;
    }
    if (freenect_set_led(device, LED_RED) != 0)
    {
        return false;
    }
128
    if (freenect_set_video_format(device, FREENECT_VIDEO_RGB) != 0)
129 130 131
    {
        return false;
    }
132
    if (freenect_set_depth_format(device, FREENECT_DEPTH_11BIT) != 0)
133 134 135
    {
        return false;
    }
136
    freenect_set_video_callback(device, videoCallback);
137 138
    freenect_set_depth_callback(device, depthCallback);

139
    if (freenect_start_depth(device) != 0)
140 141 142
    {
        return false;
    }
143
    if (freenect_start_video(device) != 0)
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161
    {
        return false;
    }

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

    return true;
}

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

162 163 164 165
    freenect_raw_tilt_state* state;
    freenect_update_tilt_state(device);
    state = freenect_get_tilt_state(device);
    freenect_get_mks_accel(state, &ax, &ay, &az);
166 167 168 169 170 171 172 173

    return true;
}

QSharedPointer<QByteArray>
Freenect::getRgbData(void)
{
    QMutexLocker locker(&rgbMutex);
174 175 176 177
    rgbData->clear();
    rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE);

    return rgbData;
178 179 180
}

QSharedPointer<QByteArray>
181
Freenect::getRawDepthData(void)
182 183
{
    QMutexLocker locker(&depthMutex);
184 185 186 187
    rawDepthData->clear();
    rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE);

    return rawDepthData;
188 189 190 191 192 193
}

QSharedPointer<QByteArray>
Freenect::getColoredDepthData(void)
{
    QMutexLocker locker(&coloredDepthMutex);
194 195 196 197
    coloredDepthData->clear();
    coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE);

    return coloredDepthData;
198 199
}

200
QSharedPointer< QVector<QVector3D> >
201
Freenect::get3DPointCloudData(void)
202 203 204
{
    QMutexLocker locker(&depthMutex);

205
    pointCloud3D->clear();
206 207 208
    unsigned short* data = reinterpret_cast<unsigned short*>(depth);
    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
    {
209
        if (data[i] > 0 && data[i] <= 2048)
210
        {
211 212 213
            double range = baseline * depthCameraParameters.fx
                           / (1.0 / 8.0 * (disparityOffset
                                           - static_cast<double>(data[i])));
214 215 216 217 218 219

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

220
                pointCloud3D->push_back(QVector3D(ray.x(), ray.y(), ray.z()));
221 222 223 224
            }
        }
    }

225
    return pointCloud3D;
226 227
}

228
QSharedPointer< QVector<Freenect::Vector6D> >
229 230
Freenect::get6DPointCloudData(void)
{
231
    get3DPointCloudData();
232

233 234
    pointCloud6D->clear();
    for (int i = 0; i < pointCloud3D->size(); ++i)
235 236 237
    {
        Vector6D point;

238 239 240
        point.x = pointCloud3D->at(i).x();
        point.y = pointCloud3D->at(i).y();
        point.z = pointCloud3D->at(i).z();
241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262

        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 &&
            originalPoint.y() >= 0.0 && originalPoint.y() < FREENECT_FRAME_H)
        {
            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];

263
            pointCloud6D->push_back(point);
264 265 266
        }
    }

267
    return pointCloud6D;
268 269
}

270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305
int
Freenect::getTiltAngle(void) const
{
    return tiltAngle;
}

void
Freenect::setTiltAngle(int angle)
{
    if (angle > 30)
    {
        angle = 30;
    }
    if (angle < -30)
    {
        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));
    while (1)
    {
        freenect->process();
    }
}

306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344
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);
    transformMatrix = transformMatrix.transposed();
345 346 347

    baseline = settings.value("transform/baseline").toDouble();
    disparityOffset = settings.value("transform/disparity_offset").toDouble();
348 349
}

350
void
351 352
Freenect::rectifyPoint(const QVector2D& originalPoint,
                       QVector2D& rectifiedPoint,
353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378
                       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
    for (int i = 0; i < 4; ++i)
    {
        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);
}

379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400
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);
}

401 402 403 404 405 406 407 408 409
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);
}

410
void
411
Freenect::videoCallback(freenect_device* device, void* video, uint32_t timestamp)
412 413 414 415
{
    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));

    QMutexLocker locker(&freenect->rgbMutex);
416
    memcpy(freenect->rgb, video, FREENECT_VIDEO_RGB_SIZE);
417 418 419
}

void
420
Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp)
421 422
{
    Freenect* freenect = static_cast<Freenect *>(freenect_get_user(device));
423
    uint16_t* data = reinterpret_cast<uint16_t *>(depth);
424

425
    QMutexLocker depthLocker(&freenect->depthMutex);
426
    memcpy(freenect->depth, data, FREENECT_DEPTH_11BIT_SIZE);
427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473

    QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex);
    unsigned short* src = reinterpret_cast<unsigned short *>(data);
    unsigned char* dst = reinterpret_cast<unsigned char *>(freenect->coloredDepth);
    for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
    {
        unsigned short pval = freenect->gammaTable[src[i]];
        unsigned short lb = pval & 0xFF;
        switch (pval >> 8)
        {
        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;
        }
    }
474
}