Skip to content

Q
qgroundcontrol
Commit c70d7417 authored by lm's avatar lm
Browse files
Options

Merge branch 'dev' of pixhawk.ethz.ch:qgroundcontrol into dev

parents b232d1e1 116c0f5c
Hide whitespace changes
Inline Side-by-side
Showing with 183 additions and 115 deletions
data/kinect.cal 0 → 100644
View file @ c70d7417
[rgb]
principal_point\x=313.67245
principal_point\y=264.06175
focal_length\x=527.44654
focal_length\y=527.40652
distortion\k1=0.20780
distortion\k2=-0.34320
distortion\k3=0.00139
distortion\k4=0.00061
distortion\k5=0.00000
[depth]
principal_point\x=313.23400
principal_point\y=246.13447
focal_length\x=587.62150
focal_length\y=587.51184
distortion\k1=0.01063
distortion\k2=-0.04479
distortion\k3=-0.00073
distortion\k4=0.00081
distortion\k5=0.00000
[transform]
R11=0.99994
R12=0.00098102
R13=0.010900
R21=-0.00097894
R22=1.0
R23=-0.00019534
R33=-0.010900
R32=0.00018466
R33=0.99994
Tx=-0.02581986
Ty=-0.0130948
Tz=-0.0047681
src/Waypoint.cc
View file @ c70d7417
......@@ -33,8 +33,7 @@ This file is part of the QGROUNDCONTROL project
#include "Waypoint.h"
#include <QStringList>
Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime,
MAV_FRAME _frame, MAV_ACTION _action)
Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime, MAV_FRAME _frame, MAV_ACTION _action)
: id(_id),
x(_x),
y(_y),
......
src/input/Freenect.cc
View file @ c70d7417
......@@ -33,45 +33,39 @@ This file is part of the QGROUNDCONTROL project
#include <cmath>
#include <string.h>
#include <QSettings>
Freenect::Freenect()
: context(NULL)
, device(NULL)
, tiltAngle(0)
, rgbData(new QByteArray)
, rawDepthData(new QByteArray)
, coloredDepthData(new QByteArray)
, pointCloud3D(new QVector<QVector3D>)
, pointCloud6D(new QVector<Vector6D>)
{
// 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;
// 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();
}
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();
// populate gamma lookup table
for (int i = 0; i < 2048; ++i)
......@@ -99,24 +93,7 @@ Freenect::Freenect()
rgbRectificationMap[i * FREENECT_FRAME_W + j] = rectifiedPoint;
}
}
}
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)
{
if (freenect_init(&context, NULL) < 0)
{
return false;
......@@ -194,33 +171,38 @@ QSharedPointer<QByteArray>
Freenect::getRgbData(void)
{
QMutexLocker locker(&rgbMutex);
return QSharedPointer<QByteArray>(
new QByteArray(rgb, FREENECT_VIDEO_RGB_SIZE));
rgbData->clear();
rgbData->append(rgb, FREENECT_VIDEO_RGB_SIZE);
return rgbData;
}
QSharedPointer<QByteArray>
Freenect::getRawDepthData(void)
{
QMutexLocker locker(&depthMutex);
return QSharedPointer<QByteArray>(
new QByteArray(depth, FREENECT_DEPTH_11BIT_SIZE));
rawDepthData->clear();
rawDepthData->append(depth, FREENECT_DEPTH_11BIT_SIZE);
return rawDepthData;
}
QSharedPointer<QByteArray>
Freenect::getColoredDepthData(void)
{
QMutexLocker locker(&coloredDepthMutex);
return QSharedPointer<QByteArray>(
new QByteArray(coloredDepth, FREENECT_VIDEO_RGB_SIZE));
coloredDepthData->clear();
coloredDepthData->append(coloredDepth, FREENECT_VIDEO_RGB_SIZE);
return coloredDepthData;
}
QVector<QVector3D>
QSharedPointer< QVector<QVector3D> >
Freenect::get3DPointCloudData(void)
{
QMutexLocker locker(&depthMutex);
QVector<QVector3D> pointCloud;
pointCloud3D->clear();
unsigned short* data = reinterpret_cast<unsigned short*>(depth);
for (int i = 0; i < FREENECT_FRAME_PIX; ++i)
{
......@@ -234,28 +216,27 @@ Freenect::get3DPointCloudData(void)
QVector3D ray = depthProjectionMatrix[i];
ray *= range;
pointCloud.push_back(QVector3D(ray.x(), ray.y(), ray.z()));
pointCloud3D->push_back(QVector3D(ray.x(), ray.y(), ray.z()));
}
}
}
return pointCloud;
return pointCloud3D;
}
QVector<Freenect::Vector6D>
QSharedPointer< QVector<Freenect::Vector6D> >
Freenect::get6DPointCloudData(void)
{
QVector<QVector3D> rawPointCloud = get3DPointCloudData();
QVector<Freenect::Vector6D> pointCloud;
get3DPointCloudData();
for (int i = 0; i < rawPointCloud.size(); ++i)
pointCloud6D->clear();
for (int i = 0; i < pointCloud3D->size(); ++i)
{
Vector6D point;
point.x = rawPointCloud.at(i).x();
point.y = rawPointCloud.at(i).y();
point.z = rawPointCloud.at(i).z();
point.x = pointCloud3D->at(i).x();
point.y = pointCloud3D->at(i).y();
point.z = pointCloud3D->at(i).z();
QVector4D transformedPoint = transformMatrix * QVector4D(point.x, point.y, point.z, 1.0);
......@@ -278,11 +259,11 @@ Freenect::get6DPointCloudData(void)
point.g = pixel[1];
point.b = pixel[2];
pointCloud.push_back(point);
pointCloud6D->push_back(point);
}
}
return pointCloud;
return pointCloud6D;
}
int
......@@ -321,6 +302,47 @@ Freenect::FreenectThread::run(void)
}
}
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();
}
void
Freenect::rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
......
src/input/Freenect.h
View file @ c70d7417
......@@ -54,7 +54,7 @@ public:
QSharedPointer<QByteArray> getRgbData(void);
QSharedPointer<QByteArray> getRawDepthData(void);
QSharedPointer<QByteArray> getColoredDepthData(void);
QVector<QVector3D> get3DPointCloudData(void);
QSharedPointer< QVector<QVector3D> > get3DPointCloudData(void);
typedef struct
{
......@@ -65,7 +65,7 @@ public:
unsigned char g;
unsigned char b;
} Vector6D;
QVector<Vector6D> get6DPointCloudData(void);
QSharedPointer< QVector<Vector6D> > get6DPointCloudData();
int getTiltAngle(void) const;
void setTiltAngle(int angle);
......@@ -86,6 +86,8 @@ private:
} IntrinsicCameraParameters;
void readConfigFile(void);
void rectifyPoint(const QVector2D& originalPoint,
QVector2D& rectifiedPoint,
const IntrinsicCameraParameters& params);
......@@ -140,6 +142,13 @@ private:
QVector3D depthProjectionMatrix[FREENECT_FRAME_PIX];
QVector2D rgbRectificationMap[FREENECT_FRAME_PIX];
// variables for use outside class
QSharedPointer<QByteArray> rgbData;
QSharedPointer<QByteArray> rawDepthData;
QSharedPointer<QByteArray> coloredDepthData;
QSharedPointer< QVector<QVector3D> > pointCloud3D;
QSharedPointer< QVector<Vector6D> > pointCloud6D;
};
#endif // FREENECT_H
src/uas/UASWaypointManager.cc
View file @ c70d7417
......@@ -131,7 +131,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if(wp->seq == current_wp_id)
{
Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->yaw << wp->autocontinue << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action);
addWaypoint(lwp);
//get next waypoint
......
src/ui/WaypointList.cc
View file @ c70d7417
......@@ -203,8 +203,6 @@ void WaypointList::add()
Waypoint *wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(),
0.0, true, true, 0.15, 2000);
uas->getWaypointManager().addWaypoint(wp);
}
}
}
......
src/ui/WaypointView.cc
View file @ c70d7417
......@@ -60,13 +60,13 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
m_ui->comboBox_frame->addItem("Global",MAV_FRAME_GLOBAL);
m_ui->comboBox_frame->addItem("Local",MAV_FRAME_LOCAL);
// defaults
changedAction(0);
changedFrame(wp->getFrame());
// Read values and set user interface
updateValues();
// defaults
//changedAction(wp->getAction());
//changedFrame(wp->getFrame());
connect(m_ui->posNSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double)));
connect(m_ui->posESpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double)));
connect(m_ui->posDSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double)));
......@@ -133,7 +133,7 @@ void WaypointView::changedAction(int index)
m_ui->holdTimeSpinBox->hide();
// set waypoint action
MAV_ACTION action = (MAV_ACTION)m_ui->comboBox_action->itemData(index).toUInt();
MAV_ACTION action = (MAV_ACTION) m_ui->comboBox_action->itemData(index).toUInt();
wp->setAction(action);
// expose ui based on action
......@@ -206,7 +206,8 @@ void WaypointView::updateValues()
{
// update frame
MAV_FRAME frame = wp->getFrame();
changedFrame(m_ui->comboBox_frame->findData(frame));
int frame_index = m_ui->comboBox_frame->findData(frame);
m_ui->comboBox_frame->setCurrentIndex(frame_index);
switch(frame)
{
case(MAV_FRAME_LOCAL):
......@@ -220,11 +221,12 @@ void WaypointView::updateValues()
m_ui->altSpinBox->setValue(wp->getZ());
break;
}
changedFrame(frame_index);
// update action
MAV_ACTION action = wp->getAction();
changedFrame(m_ui->comboBox_frame->findData(frame));
changedAction(m_ui->comboBox_action->findData(action));
int action_index = m_ui->comboBox_action->findData(action);
m_ui->comboBox_action->setCurrentIndex(action_index);
switch(action)
{
case MAV_ACTION_TAKEOFF:
......@@ -238,6 +240,7 @@ void WaypointView::updateValues()
default:
std::cerr << "unknown action" << std::endl;
}
changedAction(action_index);
m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
m_ui->selectedBox->setChecked(wp->getCurrent());
......
src/ui/map3D/Pixhawk3DWidget.cc
View file @ c70d7417
......@@ -975,21 +975,6 @@ Pixhawk3DWidget::updateHUD(double robotX, double robotY, double robotZ,
scaleGeode->update(height(), cameraParams.cameraFov,
cameraManipulator->getDistance(), darkBackground);
if (!rgb.isNull())
{
rgbImage->setImage(640, 480, 1,
GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(rgb->data()),
osg::Image::NO_DELETE);
rgbImage->dirty();
depthImage->setImage(640, 480, 1,
GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(coloredDepth->data()),
osg::Image::NO_DELETE);
depthImage->dirty();
}
}
void
......@@ -1260,26 +1245,45 @@ float colormap_jet[128][3] =
void
Pixhawk3DWidget::updateRGBD(void)
{
rgb = freenect->getRgbData();
coloredDepth = freenect->getColoredDepthData();
pointCloud = freenect->get6DPointCloudData();
QSharedPointer<QByteArray> rgb = freenect->getRgbData();
if (!rgb.isNull())
{
rgbImage->setImage(640, 480, 1,
GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(rgb->data()),
osg::Image::NO_DELETE);
rgbImage->dirty();
}
QSharedPointer<QByteArray> coloredDepth = freenect->getColoredDepthData();
if (!coloredDepth.isNull())
{
depthImage->setImage(640, 480, 1,
GL_RGB, GL_RGB, GL_UNSIGNED_BYTE,
reinterpret_cast<unsigned char *>(coloredDepth->data()),
osg::Image::NO_DELETE);
depthImage->dirty();
}
QSharedPointer< QVector<Freenect::Vector6D> > pointCloud =
freenect->get6DPointCloudData();
osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry();
osg::Vec3Array* vertices = static_cast<osg::Vec3Array*>(geometry->getVertexArray());
osg::Vec4Array* colors = static_cast<osg::Vec4Array*>(geometry->getColorArray());
for (int i = 0; i < pointCloud.size(); ++i)
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->at(i).x;
double y = pointCloud->at(i).y;
double z = pointCloud->at(i).z;
(*vertices)[i].set(x, z, -y);
if (enableRGBDColor)
{
(*colors)[i].set(pointCloud[i].r / 255.0f,
pointCloud[i].g / 255.0f,
pointCloud[i].b / 255.0f,
(*colors)[i].set(pointCloud->at(i).r / 255.0f,
pointCloud->at(i).g / 255.0f,
pointCloud->at(i).b / 255.0f,
1.0f);
}
else
......@@ -1296,12 +1300,12 @@ Pixhawk3DWidget::updateRGBD(void)
if (geometry->getNumPrimitiveSets() == 0)
{
geometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS,
0, pointCloud.size()));
0, pointCloud->size()));
}
else
{
osg::DrawArrays* drawarrays = static_cast<osg::DrawArrays*>(geometry->getPrimitiveSet(0));
drawarrays->setCount(pointCloud.size());
drawarrays->setCount(pointCloud->size());
}
}
#endif
......
src/ui/map3D/Pixhawk3DWidget.h
View file @ c70d7417
......@@ -157,11 +157,8 @@ private:
osg::ref_ptr<osg::Geode> rgbd3DNode;
#ifdef QGC_LIBFREENECT_ENABLED
QScopedPointer<Freenect> freenect;
QVector<Freenect::Vector6D> pointCloud;
#endif
bool enableFreenect;
QSharedPointer<QByteArray> rgb;
QSharedPointer<QByteArray> coloredDepth;
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