diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 742de0cd4b5e3073fff2a7a913944961d4062776..d4bcfcf572443241a9defea2ba0732b6a0ecf37d 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -135,14 +135,15 @@ macx { # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. - #QMAKE_CXXFLAGS += -Wl, -E + #QMAKE_CXXFLAGS += -Wl,-E } # GNU/Linux linux-g++ { - + debug { DESTDIR = $$BUILDDIR/debug + CONFIG += debug } release { @@ -157,18 +158,8 @@ linux-g++ { # $$BASEDIR/lib/flite/lang - HARDWARE_PLATFORM = $$system(uname -a) - contains( HARDWARE_PLATFORM, x86_64 ) { - # 64-bit Linux - #LIBS += \ - #-L$$BASEDIR/lib/flite/linux64 - message(Building for GNU/Linux 64bit/x64) - } else { - # 32-bit Linux - #LIBS += \ - #-L$$BASEDIR/lib/flite/linux32 - message(Building for GNU/Linux 32bit/i386) - } + message(Building for GNU/Linux 32bit/i386) + LIBS += \ -L/usr/lib \ -lm \ @@ -198,7 +189,7 @@ linux-g++ { } exists(/usr/local/include/libfreenect) { - message("Building suplocport for libfreenect") + message("Building support for libfreenect") DEPENDENCIES_PRESENT += libfreenect INCLUDEPATH += /usr/include/libusb-1.0 # Include libfreenect libraries @@ -213,14 +204,14 @@ linux-g++ { # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl, -E + QMAKE_CXXFLAGS += -Wl,-E } linux-g++-64 { - CONFIG += debug debug { DESTDIR = $$BUILDDIR/debug + CONFIG += debug } release { @@ -228,24 +219,16 @@ linux-g++-64 { } QMAKE_PRE_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. + INCLUDEPATH += /usr/include \ /usr/include/qt4/phonon # $$BASEDIR/lib/flite/include \ # $$BASEDIR/lib/flite/lang - HARDWARE_PLATFORM = $$system(uname -a) - contains( HARDWARE_PLATFORM, x86_64 ) { - # 64-bit Linux - #LIBS += \ - #-L$$BASEDIR/lib/flite/linux64 - message(Building for GNU/Linux 64bit/x64) - } else { - # 32-bit Linux - #LIBS += \ - #-L$$BASEDIR/lib/flite/linux32 - message(Building for GNU/Linux 32bit/i386) - } + # 64-bit Linux + message(Building for GNU/Linux 64bit/x64 (g++-64)) + LIBS += \ -L/usr/lib \ -lm \ @@ -285,7 +268,7 @@ linux-g++-64 { # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl, -E + QMAKE_CXXFLAGS += -Wl,-E } # Windows (32bit) @@ -356,7 +339,7 @@ win32-g++ { # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl, -E + QMAKE_CXXFLAGS += -Wl,-E } # Windows (64bit) @@ -394,5 +377,5 @@ win64-g++ { # osg/osgEarth dynamic casts might fail without this compiler option. # see http://osgearth.org/wiki/FAQ for details. - QMAKE_CXXFLAGS += -Wl, -E + QMAKE_CXXFLAGS += -Wl,-E } diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 020e128180ad3d01f5e5533d7558fe68c28f8bfa..aec966e8c76faa43457094ffd454eabddabcedd4 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -37,8 +37,7 @@ BASEDIR = $$IN_PWD TARGETDIR = $$OUT_PWD BUILDDIR = $$TARGETDIR/build LANGUAGE = C++ -CONFIG += debug_and_release \ - console +CONFIG += console OBJECTS_DIR = $$BUILDDIR/obj MOC_DIR = $$BUILDDIR/moc UI_HEADERS_DIR = src/ui/generated @@ -353,9 +352,9 @@ contains(DEPENDENCIES_PRESENT, osgearth) { } } -contains(DEPENDENCIES_PRESENT, libfreenect) { +contains(DEPENDENCIES_PRESENT, libfreenect) { message("Including sources for libfreenect") - + # Enable only if libfreenect is available SOURCES += src/input/Freenect.cc } diff --git a/src/Waypoint.cc b/src/Waypoint.cc index d84b76ecfa7636af9fc63acb6408ace5094a2d91..7a1c295b977927584620afac41c69a680da57aa7 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -40,12 +40,12 @@ Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _ y(_y), z(_z), yaw(_yaw), + frame(_frame), + action(_action), autocontinue(_autocontinue), current(_current), orbit(_orbit), - holdTime(_holdTime), - frame(_frame), - action(_action) + holdTime(_holdTime) { } diff --git a/src/input/Freenect.cc b/src/input/Freenect.cc index a60f094f8600eb7f53eacdb6d13c002338a71132..55951f94ddd21d7cdc899c6fb7315ef1013acb21 100644 --- a/src/input/Freenect.cc +++ b/src/input/Freenect.cc @@ -2,19 +2,57 @@ #include #include -#include Freenect::Freenect() : context(NULL) , device(NULL) , tiltAngle(0) { + // default rgb camera parameters + rgbCameraParameters.cx = 3.2894272028759258e+02; + rgbCameraParameters.cy = 2.6748068171871557e+02; + rgbCameraParameters.fx = 5.2921508098293293e+02; + rgbCameraParameters.fy = 5.2556393630057437e+02; + rgbCameraParameters.k[0] = 2.6451622333009589e-01; + rgbCameraParameters.k[1] = -8.3990749424620825e-01; + rgbCameraParameters.k[2] = -1.9922302173693159e-03; + rgbCameraParameters.k[3] = 1.4371995932897616e-03; + rgbCameraParameters.k[4] = 9.1192465078713847e-01; + + // default depth camera parameters + depthCameraParameters.cx = 3.3930780975300314e+02; + depthCameraParameters.cy = 2.4273913761751615e+02; + depthCameraParameters.fx = 5.9421434211923247e+02; + depthCameraParameters.fy = 5.9104053696870778e+02; + depthCameraParameters.k[0] = -2.6386489753128833e-01; + depthCameraParameters.k[1] = 9.9966832163729757e-01; + depthCameraParameters.k[2] = -7.6275862143610667e-04; + depthCameraParameters.k[3] = 5.0350940090814270e-03; + depthCameraParameters.k[4] = -1.3053628089976321e+00; + + // populate gamma lookup table for (int i = 0; i < 2048; ++i) { float v = static_cast(i) / 2048.0f; v = powf(v, 3.0f) * 6.0f; gammaTable[i] = static_cast(v * 6.0f * 256.0f); } + + // 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; + } + } } Freenect::~Freenect() @@ -98,8 +136,14 @@ Freenect::process(void) return false; } - freenect_get_raw_accel(device, &ax, &ay, &az); - freenect_get_mks_accel(device, &dx, &dy, &dz); + //libfreenect changed some access functions in one of the new revisions + freenect_raw_device_state state; + freenect_get_mks_accel(&state, &ax, &ay, &az); + //tiltAngle = freenect_get_tilt_degs(&state); + + //these are the old access functions + //freenect_get_raw_accel(device, &ax, &ay, &az); + //freenect_get_mks_accel(device, &dx, &dy, &dz); return true; } @@ -120,15 +164,6 @@ Freenect::getRawDepthData(void) new QByteArray(depth, FREENECT_DEPTH_SIZE)); } -QSharedPointer -Freenect::getDistanceData(void) -{ - QMutexLocker locker(&distanceMutex); - return QSharedPointer( - new QByteArray(reinterpret_cast(distance), - FREENECT_FRAME_PIX * sizeof(float))); -} - QSharedPointer Freenect::getColoredDepthData(void) { @@ -137,6 +172,34 @@ Freenect::getColoredDepthData(void) new QByteArray(coloredDepth, FREENECT_RGB_SIZE)); } +QVector +Freenect::getPointCloudData(void) +{ + QMutexLocker locker(&depthMutex); + + QVector pointCloud; + + unsigned short* data = reinterpret_cast(depth); + for (int i = 0; i < FREENECT_FRAME_PIX; ++i) + { + if (data[i] <= 2048) + { + // see www.ros.org/wiki/kinect_node for details + double range = 1.0f / (-0.00307f * static_cast(data[i]) + 3.33f); + + if (range > 0.0f) + { + QVector3D ray = depthProjectionMatrix[i]; + ray *= range; + + pointCloud.push_back(QVector3D(ray.x(), ray.y(), ray.z())); + } + } + } + + return pointCloud; +} + int Freenect::getTiltAngle(void) const { @@ -173,6 +236,43 @@ Freenect::FreenectThread::run(void) } } +void +Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, + 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); +} + +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); +} + void Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp) { @@ -183,7 +283,7 @@ Freenect::rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t tim } void -Freenect::depthCallback(freenect_device* device, freenect_depth* depth, uint32_t timestamp) +Freenect::depthCallback(freenect_device* device, void* depth, uint32_t timestamp) { Freenect* freenect = static_cast(freenect_get_user(device)); freenect_depth* data = reinterpret_cast(depth); @@ -191,13 +291,6 @@ Freenect::depthCallback(freenect_device* device, freenect_depth* depth, uint32_t QMutexLocker depthLocker(&freenect->depthMutex); memcpy(freenect->depth, data, FREENECT_DEPTH_SIZE); - QMutexLocker distanceLocker(&freenect->distanceMutex); - for (int i = 0; i < FREENECT_FRAME_PIX; ++i) - { - freenect->distance[i] = - 100.f / (-0.00307f * static_cast(data[i]) + 3.33f); - } - QMutexLocker coloredDepthLocker(&freenect->coloredDepthMutex); unsigned short* src = reinterpret_cast(data); unsigned char* dst = reinterpret_cast(freenect->coloredDepth); diff --git a/src/input/Freenect.h b/src/input/Freenect.h index e3e3925c5cfd732419f606343fae38bf6378010c..7424addd1c14eb259d18044d21a4b0c32b6b12b7 100644 --- a/src/input/Freenect.h +++ b/src/input/Freenect.h @@ -1,11 +1,14 @@ #ifndef FREENECT_H #define FREENECT_H -#include +#include #include #include #include #include +#include +#include +#include class Freenect { @@ -18,15 +21,35 @@ public: QSharedPointer getRgbData(void); QSharedPointer getRawDepthData(void); - QSharedPointer getDistanceData(void); QSharedPointer getColoredDepthData(void); + QVector getPointCloudData(void); int getTiltAngle(void) const; void setTiltAngle(int angle); private: + typedef struct + { + // coordinates of principal point + double cx; + double cy; + + // focal length in pixels + double fx; + double fy; + + // distortion parameters + double k[5]; + + } IntrinsicCameraParameters; + + void rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint, + const IntrinsicCameraParameters& params); + void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray, + const IntrinsicCameraParameters& params); + static void rgbCallback(freenect_device* device, freenect_pixel* rgb, uint32_t timestamp); - static void depthCallback(freenect_device* device, freenect_depth* depth, uint32_t timestamp); + static void depthCallback(freenect_device* device, void* depth, uint32_t timestamp); freenect_context* context; freenect_device* device; @@ -43,6 +66,9 @@ private: }; QScopedPointer thread; + IntrinsicCameraParameters rgbCameraParameters; + IntrinsicCameraParameters depthCameraParameters; + // tilt angle of Kinect camera int tiltAngle; @@ -53,18 +79,17 @@ private: char depth[FREENECT_DEPTH_SIZE]; QMutex depthMutex; - float distance[FREENECT_FRAME_PIX]; - QMutex distanceMutex; - char coloredDepth[FREENECT_RGB_SIZE]; QMutex coloredDepthMutex; // accelerometer data - short ax, ay, az; + double ax, ay, az; double dx, dy, dz; // gamma map unsigned short gammaTable[2048]; + + QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX]; }; #endif // FREENECT_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 30ce495af6d0a4789524eca8e1677197d69fd0bb..3ca6c54ac8b2a7afd6e73e2d973519b3f90922f6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -559,22 +559,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit textMessageReceived(uasId, message.compid, severity, text); } break; -#ifdef MAVLINK_ENABLED_PIXHAWK - case MAVLINK_MSG_ID_POINT_OF_INTEREST: - { - mavlink_point_of_interest_t poi; - mavlink_msg_point_of_interest_decode(&message, &poi); - emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z); - } - break; - case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION: - { - mavlink_point_of_interest_connection_t poi; - mavlink_msg_point_of_interest_connection_decode(&message, &poi); - emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2); - } - break; -#endif +//#ifdef MAVLINK_ENABLED_PIXHAWK +// case MAVLINK_MSG_ID_POINT_OF_INTEREST: +// { +// mavlink_point_of_interest_t poi; +// mavlink_msg_point_of_interest_decode(&message, &poi); +// emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z); +// } +// break; +// case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION: +// { +// mavlink_point_of_interest_connection_t poi; +// mavlink_msg_point_of_interest_connection_decode(&message, &poi); +// emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2); +// } +// break; +//#endif #ifdef MAVLINK_ENABLED_UALBERTA case MAVLINK_MSG_ID_NAV_FILTER_BIAS: { diff --git a/src/ui/map3D/GCManipulator.cc b/src/ui/map3D/GCManipulator.cc index 83b0b61684a99cecb1e8e9a8b787e2742304263a..cdabc966533ea9f53448713144bcbd9b584f5e6b 100644 --- a/src/ui/map3D/GCManipulator.cc +++ b/src/ui/map3D/GCManipulator.cc @@ -254,7 +254,7 @@ GCManipulator::calcMovement() if (buttonMask == GUIEventAdapter::LEFT_MOUSE_BUTTON) { // rotate camera - osg::Vec3d axis; + osg::Vec3 axis; float angle; float px0 = _ga_t0->getXnormalized(); diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index af91684a11ea712a2092fe50bc4657b9539495f7..8a15cde4de99db874c70df04e58cecd85b7b49c2 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -224,8 +224,7 @@ Pixhawk3DWidget::findVehicleModels(void) if (node) { - - nodes.push_back(node); + nodes.push_back(node); } else { @@ -809,12 +808,175 @@ Pixhawk3DWidget::updateWaypoints(void) } } +float colormap_jet[128][3] = +{ + {0.0f,0.0f,0.53125f}, + {0.0f,0.0f,0.5625f}, + {0.0f,0.0f,0.59375f}, + {0.0f,0.0f,0.625f}, + {0.0f,0.0f,0.65625f}, + {0.0f,0.0f,0.6875f}, + {0.0f,0.0f,0.71875f}, + {0.0f,0.0f,0.75f}, + {0.0f,0.0f,0.78125f}, + {0.0f,0.0f,0.8125f}, + {0.0f,0.0f,0.84375f}, + {0.0f,0.0f,0.875f}, + {0.0f,0.0f,0.90625f}, + {0.0f,0.0f,0.9375f}, + {0.0f,0.0f,0.96875f}, + {0.0f,0.0f,1.0f}, + {0.0f,0.03125f,1.0f}, + {0.0f,0.0625f,1.0f}, + {0.0f,0.09375f,1.0f}, + {0.0f,0.125f,1.0f}, + {0.0f,0.15625f,1.0f}, + {0.0f,0.1875f,1.0f}, + {0.0f,0.21875f,1.0f}, + {0.0f,0.25f,1.0f}, + {0.0f,0.28125f,1.0f}, + {0.0f,0.3125f,1.0f}, + {0.0f,0.34375f,1.0f}, + {0.0f,0.375f,1.0f}, + {0.0f,0.40625f,1.0f}, + {0.0f,0.4375f,1.0f}, + {0.0f,0.46875f,1.0f}, + {0.0f,0.5f,1.0f}, + {0.0f,0.53125f,1.0f}, + {0.0f,0.5625f,1.0f}, + {0.0f,0.59375f,1.0f}, + {0.0f,0.625f,1.0f}, + {0.0f,0.65625f,1.0f}, + {0.0f,0.6875f,1.0f}, + {0.0f,0.71875f,1.0f}, + {0.0f,0.75f,1.0f}, + {0.0f,0.78125f,1.0f}, + {0.0f,0.8125f,1.0f}, + {0.0f,0.84375f,1.0f}, + {0.0f,0.875f,1.0f}, + {0.0f,0.90625f,1.0f}, + {0.0f,0.9375f,1.0f}, + {0.0f,0.96875f,1.0f}, + {0.0f,1.0f,1.0f}, + {0.03125f,1.0f,0.96875f}, + {0.0625f,1.0f,0.9375f}, + {0.09375f,1.0f,0.90625f}, + {0.125f,1.0f,0.875f}, + {0.15625f,1.0f,0.84375f}, + {0.1875f,1.0f,0.8125f}, + {0.21875f,1.0f,0.78125f}, + {0.25f,1.0f,0.75f}, + {0.28125f,1.0f,0.71875f}, + {0.3125f,1.0f,0.6875f}, + {0.34375f,1.0f,0.65625f}, + {0.375f,1.0f,0.625f}, + {0.40625f,1.0f,0.59375f}, + {0.4375f,1.0f,0.5625f}, + {0.46875f,1.0f,0.53125f}, + {0.5f,1.0f,0.5f}, + {0.53125f,1.0f,0.46875f}, + {0.5625f,1.0f,0.4375f}, + {0.59375f,1.0f,0.40625f}, + {0.625f,1.0f,0.375f}, + {0.65625f,1.0f,0.34375f}, + {0.6875f,1.0f,0.3125f}, + {0.71875f,1.0f,0.28125f}, + {0.75f,1.0f,0.25f}, + {0.78125f,1.0f,0.21875f}, + {0.8125f,1.0f,0.1875f}, + {0.84375f,1.0f,0.15625f}, + {0.875f,1.0f,0.125f}, + {0.90625f,1.0f,0.09375f}, + {0.9375f,1.0f,0.0625f}, + {0.96875f,1.0f,0.03125f}, + {1.0f,1.0f,0.0f}, + {1.0f,0.96875f,0.0f}, + {1.0f,0.9375f,0.0f}, + {1.0f,0.90625f,0.0f}, + {1.0f,0.875f,0.0f}, + {1.0f,0.84375f,0.0f}, + {1.0f,0.8125f,0.0f}, + {1.0f,0.78125f,0.0f}, + {1.0f,0.75f,0.0f}, + {1.0f,0.71875f,0.0f}, + {1.0f,0.6875f,0.0f}, + {1.0f,0.65625f,0.0f}, + {1.0f,0.625f,0.0f}, + {1.0f,0.59375f,0.0f}, + {1.0f,0.5625f,0.0f}, + {1.0f,0.53125f,0.0f}, + {1.0f,0.5f,0.0f}, + {1.0f,0.46875f,0.0f}, + {1.0f,0.4375f,0.0f}, + {1.0f,0.40625f,0.0f}, + {1.0f,0.375f,0.0f}, + {1.0f,0.34375f,0.0f}, + {1.0f,0.3125f,0.0f}, + {1.0f,0.28125f,0.0f}, + {1.0f,0.25f,0.0f}, + {1.0f,0.21875f,0.0f}, + {1.0f,0.1875f,0.0f}, + {1.0f,0.15625f,0.0f}, + {1.0f,0.125f,0.0f}, + {1.0f,0.09375f,0.0f}, + {1.0f,0.0625f,0.0f}, + {1.0f,0.03125f,0.0f}, + {1.0f,0.0f,0.0f}, + {0.96875f,0.0f,0.0f}, + {0.9375f,0.0f,0.0f}, + {0.90625f,0.0f,0.0f}, + {0.875f,0.0f,0.0f}, + {0.84375f,0.0f,0.0f}, + {0.8125f,0.0f,0.0f}, + {0.78125f,0.0f,0.0f}, + {0.75f,0.0f,0.0f}, + {0.71875f,0.0f,0.0f}, + {0.6875f,0.0f,0.0f}, + {0.65625f,0.0f,0.0f}, + {0.625f,0.0f,0.0f}, + {0.59375f,0.0f,0.0f}, + {0.5625f,0.0f,0.0f}, + {0.53125f,0.0f,0.0f}, + {0.5f,0.0f,0.0f} +}; + #ifdef QGC_LIBFREENECT_ENABLED void Pixhawk3DWidget::updateRGBD(void) { rgb = freenect->getRgbData(); coloredDepth = freenect->getColoredDepthData(); + pointCloud = freenect->getPointCloudData(); + + osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); + + osg::Vec3Array* vertices = static_cast(geometry->getVertexArray()); + osg::Vec4Array* colors = static_cast(geometry->getColorArray()); + for (int i = 0; i < pointCloud.size(); ++i) + { + double x = pointCloud[i].x(); + double y = pointCloud[i].y(); + double z = pointCloud[i].z(); + (*vertices)[i].set(x, z, -y); + + double dist = sqrt(x * x + y * y + z * z); + int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); + (*colors)[i].set(colormap_jet[colorIndex][0], + colormap_jet[colorIndex][1], + colormap_jet[colorIndex][2], + 1.0f); + } + + if (geometry->getNumPrimitiveSets() == 0) + { + geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, + 0, pointCloud.size())); + } + else + { + osg::DrawArrays* drawarrays = static_cast(geometry->getPrimitiveSet(0)); + drawarrays->setCount(pointCloud.size()); + } } #endif diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index be4e66d298d3464716e42654da545fa7797ef652..0d8074093da1fd326aaa5c66e1f5062bec56a97a 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -139,6 +139,7 @@ private: #endif QSharedPointer rgb; QSharedPointer coloredDepth; + QVector pointCloud; QVector< osg::ref_ptr > vehicleModels; diff --git a/src/ui/map3D/Q3DWidget.cc b/src/ui/map3D/Q3DWidget.cc index 0e08d85ac6d8b32e44b316c2a16b1f3af584d805..21300459923b90d477531cf7545424936d62e799 100644 --- a/src/ui/map3D/Q3DWidget.cc +++ b/src/ui/map3D/Q3DWidget.cc @@ -34,7 +34,9 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#ifdef Q_OS_MACX #include +#endif Q3DWidget::Q3DWidget(QWidget* parent) : QGLWidget(parent)