Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
5d25e982
Commit
5d25e982
authored
Dec 07, 2010
by
hengli
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Improved rendering performance for Kinect data.
parent
899e0bae
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
with
70 additions
and
53 deletions
+70
-53
Freenect.cc
src/input/Freenect.cc
+30
-21
Freenect.h
src/input/Freenect.h
+9
-2
Pixhawk3DWidget.cc
src/ui/map3D/Pixhawk3DWidget.cc
+31
-27
Pixhawk3DWidget.h
src/ui/map3D/Pixhawk3DWidget.h
+0
-3
No files found.
src/input/Freenect.cc
View file @
5d25e982
...
...
@@ -39,6 +39,11 @@ 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
>
)
{
}
...
...
@@ -166,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
;
}
Q
Vector
<
QVector3D
>
Q
SharedPointer
<
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
)
{
...
...
@@ -206,28 +216,27 @@ Freenect::get3DPointCloudData(void)
QVector3D
ray
=
depthProjectionMatrix
[
i
];
ray
*=
range
;
pointCloud
.
push_back
(
QVector3D
(
ray
.
x
(),
ray
.
y
(),
ray
.
z
()));
pointCloud
3D
->
push_back
(
QVector3D
(
ray
.
x
(),
ray
.
y
(),
ray
.
z
()));
}
}
}
return
pointCloud
;
return
pointCloud
3D
;
}
Q
Vector
<
Freenect
::
Vector6D
>
Q
SharedPointer
<
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
);
...
...
@@ -250,11 +259,11 @@ Freenect::get6DPointCloudData(void)
point
.
g
=
pixel
[
1
];
point
.
b
=
pixel
[
2
];
pointCloud
.
push_back
(
point
);
pointCloud
6D
->
push_back
(
point
);
}
}
return
pointCloud
;
return
pointCloud
6D
;
}
int
...
...
src/input/Freenect.h
View file @
5d25e982
...
...
@@ -54,7 +54,7 @@ public:
QSharedPointer
<
QByteArray
>
getRgbData
(
void
);
QSharedPointer
<
QByteArray
>
getRawDepthData
(
void
);
QSharedPointer
<
QByteArray
>
getColoredDepthData
(
void
);
Q
Vector
<
QVector3D
>
get3DPointCloudData
(
void
);
Q
SharedPointer
<
QVector
<
QVector3D
>
>
get3DPointCloudData
(
void
);
typedef
struct
{
...
...
@@ -65,7 +65,7 @@ public:
unsigned
char
g
;
unsigned
char
b
;
}
Vector6D
;
Q
Vector
<
Vector6D
>
get6DPointCloudData
(
void
);
Q
SharedPointer
<
QVector
<
Vector6D
>
>
get6DPointCloudData
(
);
int
getTiltAngle
(
void
)
const
;
void
setTiltAngle
(
int
angle
);
...
...
@@ -142,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/ui/map3D/Pixhawk3DWidget.cc
View file @
5d25e982
...
...
@@ -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.0
f
,
pointCloud
[
i
]
.
g
/
255.0
f
,
pointCloud
[
i
]
.
b
/
255.0
f
,
(
*
colors
)[
i
].
set
(
pointCloud
->
at
(
i
)
.
r
/
255.0
f
,
pointCloud
->
at
(
i
)
.
g
/
255.0
f
,
pointCloud
->
at
(
i
)
.
b
/
255.0
f
,
1.0
f
);
}
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 @
5d25e982
...
...
@@ -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
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment