Skip to content

Q
qgroundcontrol
Commit a8e94b3f authored by pixhawk's avatar pixhawk
Browse files
Options

Merged

parents e240bb91 2adb1f82
Hide whitespace changes
Inline Side-by-side
Showing with 139 additions and 19 deletions
qgroundcontrol.pri
View file @ a8e94b3f
......@@ -189,6 +189,7 @@ linux-g++ {
}
exists(/usr/local/include/libfreenect) {
exists(/usr/local/include/libfreenect/libfreenect.h) {
message("Building support for libfreenect")
DEPENDENCIES_PRESENT += libfreenect
INCLUDEPATH += /usr/include/libusb-1.0
......
src/comm/MAVLinkProtocol.cc
View file @ a8e94b3f
......@@ -258,7 +258,7 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
//if ()
// If a new loss was detected or we just hit one 128th packet step
if (lastLoss != totalLossCounter || (totalReceiveCounter == 128))
if (lastLoss != totalLossCounter || (totalReceiveCounter % 64 == 0))
{
// Calculate new loss ratio
// Receive loss
......@@ -325,6 +325,8 @@ void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message
{
// Create buffer
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// Rewriting header to ensure correct link ID is set
if (link->getId() != 0) mavlink_finalize_message_chan(&message, this->getSystemId(), this->getComponentId(), link->getId(), message.len);
// Write message into buffer, prepending start sign
int len = mavlink_msg_to_send_buffer(buffer, &message);
// If link is connected
......
src/comm/MAVLinkXMLParser.cc
View file @ a8e94b3f
......@@ -341,7 +341,8 @@ bool MAVLinkXMLParser::generate()
QString decode("static inline void mavlink_msg_%1_decode(const mavlink_message_t* msg, %2* %1)\n{\n%3}\n");
QString pack("static inline uint16_t mavlink_msg_%1_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message(msg, system_id, component_id, i);\n}\n\n");
QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif");
QString packChan("static inline uint16_t mavlink_msg_%1_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg%2)\n{\n\tuint16_t i = 0;\n\tmsg->msgid = MAVLINK_MSG_ID_%3;\n\n%4\n\treturn mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);\n}\n\n");
QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif");
//QString compactStructSend = "#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_struct_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_encode(mavlink_system.sysid, mavlink_system.compid, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif";
QString unpacking;
QString prepends;
......@@ -506,10 +507,11 @@ bool MAVLinkXMLParser::generate()
cStruct = cStruct.arg(cStructName, cStructLines);
lcmStructDefs.append("\n").append(cStruct).append("\n");
pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines);
packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines);
encode = encode.arg(messageName).arg(cStructName).arg(packArguments);
decode = decode.arg(messageName).arg(cStructName).arg(decodeLines);
compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters);
QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + arrayDefines + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode;
QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine + "\n\n" + cStruct + "\n\n" + arrayDefines + "\n\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + packChan + encode + "\n" + compactSend + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + decode;
cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile));
} // Check if tag = message
} // Check if e = NULL
......
src/input/Freenect.cc
View file @ a8e94b3f
......@@ -30,6 +30,18 @@ Freenect::Freenect()
depthCameraParameters.k[3] = 5.0350940090814270e-03;
depthCameraParameters.k[4] = -1.3053628089976321e+00;
// relative rotation/translation between cameras with depth camera as reference
transformMatrix = QMatrix4x4(9.9984628826577793e-01, 1.2635359098409581e-03,
-1.7487233004436643e-02, 1.9985242312092553e-02,
-1.4779096108364480e-03, 9.9992385683542895e-01,
-1.2251380107679535e-02, -7.4423738761617583e-04,
1.7470421412464927e-02, 1.2275341476520762e-02,
9.9977202419716948e-01, -1.0916736334336222e-02,
0.0, 0.0, 0.0, 1.0);
// relative rotation/translation between cameras with rgb camera as reference
transformMatrix = transformMatrix.transposed();
// populate gamma lookup table
for (int i = 0; i < 2048; ++i)
{
......@@ -51,6 +63,9 @@ Freenect::Freenect()
projectPixelTo3DRay(rectifiedPoint, rectifiedRay, depthCameraParameters);
depthProjectionMatrix[i * FREENECT_FRAME_W + j] = rectifiedRay;
rectifyPoint(originalPoint, rectifiedPoint, rgbCameraParameters);
rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint;
}
}
}
......@@ -173,7 +188,7 @@ Freenect::getColoredDepthData(void)
}
QVector<QVector3D>
Freenect::getPointCloudData(void)
Freenect::get3DPointCloudData(void)
{
QMutexLocker locker(&depthMutex);
......@@ -200,6 +215,49 @@ Freenect::getPointCloudData(void)
return pointCloud;
}
QVector<Freenect::Vector6D>
Freenect::get6DPointCloudData(void)
{
QVector<QVector3D> rawPointCloud = get3DPointCloudData();
QVector<Freenect::Vector6D> pointCloud;
for (int i = 0; i < rawPointCloud.size(); ++i)
{
Vector6D point;
point.x = rawPointCloud[i].x();
point.y = rawPointCloud[i].y();
point.z = rawPointCloud[i].z();
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];
pointCloud.push_back(point);
}
}
return pointCloud;
}
int
Freenect::getTiltAngle(void) const
{
......@@ -237,7 +295,8 @@ Freenect::FreenectThread::run(void)
}
void
Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint,
Freenect::rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params)
{
double x = (originalPoint.x() - params.cx) / params.fx;
......@@ -264,6 +323,28 @@ Freenect::rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint
rectifiedPoint.setY(y * params.fy + params.cy);
}
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);
}
void
Freenect::projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params)
......
src/input/Freenect.h
View file @ a8e94b3f
......@@ -2,6 +2,7 @@
#define FREENECT_H
#include <libfreenect/libfreenect.h>
#include <QMatrix4x4>
#include <QMutex>
#include <QScopedPointer>
#include <QSharedPointer>
......@@ -22,7 +23,18 @@ public:
QSharedPointer<QByteArray> getRgbData(void);
QSharedPointer<QByteArray> getRawDepthData(void);
QSharedPointer<QByteArray> getColoredDepthData(void);
QVector<QVector3D> getPointCloudData(void);
QVector<QVector3D> get3DPointCloudData(void);
typedef struct
{
double x;
double y;
double z;
unsigned char r;
unsigned char g;
unsigned char b;
} Vector6D;
QVector<Vector6D> get6DPointCloudData(void);
int getTiltAngle(void) const;
void setTiltAngle(int angle);
......@@ -43,8 +55,12 @@ private:
} IntrinsicCameraParameters;
void rectifyPoint(const QVector2D& originalPoint, QVector2D& rectifiedPoint,
void rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params);
void unrectifyPoint(const QVector2D& rectifiedPoint,
QVector2D& originalPoint,
const IntrinsicCameraParameters& params);
void projectPixelTo3DRay(const QVector2D& pixel, QVector3D& ray,
const IntrinsicCameraParameters& params);
......@@ -69,6 +85,8 @@ private:
IntrinsicCameraParameters rgbCameraParameters;
IntrinsicCameraParameters depthCameraParameters;
QMatrix4x4 transformMatrix;
// tilt angle of Kinect camera
int tiltAngle;
......@@ -90,6 +108,7 @@ private:
unsigned short gammaTable[2048];
QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX];
QVector2D rgbRectificationMap[FREENECT_FRAME_PIX];
};
#endif // FREENECT_H
src/ui/map3D/Pixhawk3DWidget.cc
View file @ a8e94b3f
......@@ -53,6 +53,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
, displayWaypoints(true)
, displayRGBD2D(false)
, displayRGBD3D(false)
, enableRGBDColor(true)
, followCamera(true)
, lastRobotX(0.0f)
, lastRobotY(0.0f)
......@@ -228,7 +229,7 @@ Pixhawk3DWidget::findVehicleModels(void)
}
else
{
printf(QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
printf("%s\n", QString("ERROR: Could not load file " + directory.absoluteFilePath(files[i]) + "\n").toStdString().c_str());
}
}
......@@ -396,6 +397,9 @@ Pixhawk3DWidget::keyPressEvent(QKeyEvent* event)
case '2':
displayRGBD3D = !displayRGBD3D;
break;
case 'c': case 'C':
enableRGBDColor = !enableRGBDColor;
break;
}
}
......@@ -946,7 +950,7 @@ Pixhawk3DWidget::updateRGBD(void)
{
rgb = freenect->getRgbData();
coloredDepth = freenect->getColoredDepthData();
pointCloud = freenect->getPointCloudData();
pointCloud = freenect->get6DPointCloudData();
osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
......@@ -954,17 +958,27 @@ Pixhawk3DWidget::updateRGBD(void)
osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(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();
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<int>(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 (enableRGBDColor)
{
(*colors)[i].set(pointCloud[i].r / 255.0f,
pointCloud[i].g / 255.0f,
pointCloud[i].b / 255.0f,
1.0f);
}
else
{
double dist = sqrt(x * x + y * y + z * z);
int colorIndex = static_cast<int>(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)
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ a8e94b3f
......@@ -110,6 +110,7 @@ private:
bool displayWaypoints;
bool displayRGBD2D;
bool displayRGBD3D;
bool enableRGBDColor;
bool followCamera;
......@@ -136,10 +137,10 @@ private:
osg::ref_ptr<osg::Geode> rgbd3DNode;
#ifdef QGC_LIBFREENECT_ENABLED
QScopedPointer<Freenect> freenect;
QVector<Freenect::Vector6D> pointCloud;
#endif
QSharedPointer<QByteArray> rgb;
QSharedPointer<QByteArray> coloredDepth;
QVector<QVector3D> pointCloud;
QVector< osg::ref_ptr<osg::Node> > vehicleModels;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment