Skip to content

Commit

Permalink
Merge branch 'ahundt-master'
Browse files Browse the repository at this point in the history
  • Loading branch information
Giacomo Dabisias committed Dec 17, 2015
2 parents 649ede3 + e27003a commit 98a7464
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 80 deletions.
3 changes: 1 addition & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,5 +39,4 @@ include_directories(${FREENECT2_INCLUDE_DIRS})

add_executable(Kinect2Grabber test.cpp)

target_link_libraries(Kinect2Grabber ${OpenCV_LIBS} ${FREENECT2_LIBRARY} ${PCL_LIBRARIES})

target_link_libraries(Kinect2Grabber ${OpenCV_LIBS} ${FREENECT2_LIBRARY} ${PCL_LIBRARIES})
109 changes: 42 additions & 67 deletions k2g.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@ via Luigi Alamanni 13D, San Giuliano Terme 56010 (PI), Italy
#include <opencv2/opencv.hpp>
#include <signal.h>
#include <cstdlib>
#include <limits>
#include <string>
#include <iostream>
#include <Eigen/Core>
Expand All @@ -49,7 +48,8 @@ class K2G {

public:

K2G(processor p): undistorted_(512, 424, 4), registered_(512, 424, 4), big_mat_(1920, 1082, 4), listener_(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth), qnan_(std::numeric_limits<float>::quiet_NaN()){
K2G(processor p, bool mirror = false): mirror_(mirror), listener_(libfreenect2::Frame::Color | libfreenect2::Frame::Ir | libfreenect2::Frame::Depth),
undistorted_(512, 424, 4), registered_(512, 424, 4), big_mat_(1920, 1082, 4), qnan_(std::numeric_limits<float>::quiet_NaN()){

signal(SIGINT,sigint_handler);

Expand All @@ -65,10 +65,12 @@ class K2G {
std::cout << "creating CPU processor" << std::endl;
pipeline_ = new libfreenect2::CpuPacketPipeline();
break;
#ifdef HAVE_OPENCL
case OPENCL:
std::cout << "creating OpenCL processor" << std::endl;
pipeline_ = new libfreenect2::OpenCLPacketPipeline();
break;
#endif
case OPENGL:
std::cout << "creating OpenGL processor" << std::endl;
pipeline_ = new libfreenect2::OpenGLPacketPipeline();
Expand Down Expand Up @@ -99,87 +101,48 @@ class K2G {
return rgb;
}

// Allocates the cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr getCloud(){

listener_.waitForNewFrame(frames_);
libfreenect2::Frame * rgb = frames_[libfreenect2::Frame::Color];
libfreenect2::Frame * depth = frames_[libfreenect2::Frame::Depth];

registration_->apply(rgb, depth, &undistorted_, &registered_, true, &big_mat_, map_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr getCloud(){
const short w = undistorted_.width;
const short h = undistorted_.height;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>(w, h));

const float * itD0 = (float *)undistorted_.data;
const char * itRGB0 = (char *)registered_.data;
pcl::PointXYZRGB * itP = &cloud->points[0];
bool is_dense = true;

for(int y = 0; y < h; ++y){

const unsigned int offset = y * w;
const float * itD = itD0 + offset;
const char * itRGB = itRGB0 + offset * 4;
const float dy = rowmap(y);

for(size_t x = 0; x < w; ++x, ++itP, ++itD, itRGB += 4 )
{
const float depth_value = *itD / 1000.0f;

if(!std::isnan(depth_value) && !(std::abs(depth_value) < 0.0001)){

const float rx = colmap(x) * depth_value;
const float ry = dy * depth_value;
itP->z = depth_value;
itP->x = rx;
itP->y = ry;

itP->b = itRGB[0];
itP->g = itRGB[1];
itP->r = itRGB[2];
} else {
itP->z = qnan_;
itP->x = qnan_;
itP->y = qnan_;

itP->b = qnan_;
itP->g = qnan_;
itP->r = qnan_;
is_dense = false;
}
}
}
cloud->is_dense = is_dense;
listener_.release(frames_);
return cloud;

return updateCloud(cloud);
}

// Does not allocate the cloud
void getCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr updateCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud){

listener_.waitForNewFrame(frames_);
libfreenect2::Frame * rgb = frames_[libfreenect2::Frame::Color];
libfreenect2::Frame * depth = frames_[libfreenect2::Frame::Depth];

registration_->apply(rgb, depth, &undistorted_, &registered_, true, &big_mat_, map_);
const short w = undistorted_.width;
const short h = undistorted_.height;
bool is_dense = true;
const std::size_t w = undistorted_.width;
const std::size_t h = undistorted_.height;

cv::Mat tmp_itD0(undistorted_.height, undistorted_.width, CV_8UC4, undistorted_.data);
cv::Mat tmp_itRGB0(registered_.height, registered_.width, CV_8UC4, registered_.data);

if (mirror_ == true){

cv::flip(tmp_itD0,tmp_itD0,1);
cv::flip(tmp_itRGB0,tmp_itRGB0,1);

const float * itD0 = (float *)undistorted_.data;
const char * itRGB0 = (char *)registered_.data;
}

const float * itD0 = (float *) tmp_itD0.ptr();
const char * itRGB0 = (char *) tmp_itRGB0.ptr();

pcl::PointXYZRGB * itP = &cloud->points[0];
bool is_dense = true;

for(int y = 0; y < h; ++y){
for(std::size_t y = 0; y < h; ++y){

const unsigned int offset = y * w;
const float * itD = itD0 + offset;
const char * itRGB = itRGB0 + offset * 4;
const float dy = rowmap(y);

for(size_t x = 0; x < w; ++x, ++itP, ++itD, itRGB += 4 )
for(std::size_t x = 0; x < w; ++x, ++itP, ++itD, itRGB += 4 )
{
const float depth_value = *itD / 1000.0f;

Expand Down Expand Up @@ -208,6 +171,7 @@ class K2G {
}
cloud->is_dense = is_dense;
listener_.release(frames_);
return cloud;
}

void shutDown(){
Expand All @@ -219,7 +183,10 @@ class K2G {
listener_.waitForNewFrame(frames_);
libfreenect2::Frame * rgb = frames_[libfreenect2::Frame::Color];
cv::Mat tmp(rgb->height, rgb->width, CV_8UC4, rgb->data);
cv::Mat r = tmp.clone();
cv::Mat r;
if (mirror_ == true) {cv::flip(tmp,r,1);}
else {r = tmp.clone();}

listener_.release(frames_);
return std::move(r);
}
Expand All @@ -228,7 +195,10 @@ class K2G {
listener_.waitForNewFrame(frames_);
libfreenect2::Frame * depth = frames_[libfreenect2::Frame::Depth];
cv::Mat tmp(depth->height, depth->width, CV_8UC4, depth->data);
cv::Mat r = tmp.clone();
cv::Mat r;
if (mirror_ == true) {cv::flip(tmp,r,1);}
else {r = tmp.clone();}

listener_.release(frames_);
return std::move(r);
}
Expand All @@ -242,6 +212,10 @@ class K2G {
cv::Mat tmp_color(registered_.height, registered_.width, CV_8UC4, registered_.data);
cv::Mat r = tmp_color.clone();
cv::Mat d = tmp_depth.clone();
if (mirror_ == true) {
cv::flip(tmp_depth,d,1);
cv::flip(tmp_color,r,1);
}
listener_.release(frames_);
return std::move(std::pair<cv::Mat, cv::Mat>(r,d));
}
Expand Down Expand Up @@ -274,6 +248,7 @@ class K2G {
Eigen::Matrix<float,512,1> colmap;
Eigen::Matrix<float,424,1> rowmap;
std::string serial_;
int map_[512 * 424]; // will be used in the next libfreenect2 update
float qnan_;
int map_[512 * 424];
float qnan_;
bool mirror_;
};
76 changes: 65 additions & 11 deletions test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,34 +20,88 @@ via Luigi Alamanni 13D, San Giuliano Terme 56010 (PI), Italy
#include <pcl/visualization/cloud_viewer.h>
#include <chrono>

// extra headers for writing out ply file
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/io/ply_io.h>


struct PlySaver{

PlySaver(boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud, bool binary, bool use_camera):
cloud_(cloud), binary_(binary), use_camera_(use_camera){}

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud_;
bool binary_;
bool use_camera_;

};


void
KeyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void * data)
{
std::string pressed;
pressed = event.getKeySym();
PlySaver * s = (PlySaver*)data;
if(event.keyDown ())
{
if(pressed == "s")
{

pcl::PLYWriter writer;
std::chrono::high_resolution_clock::time_point p = std::chrono::high_resolution_clock::now();
std::string now = std::to_string((long)std::chrono::duration_cast<std::chrono::milliseconds>(p.time_since_epoch()).count());
writer.write ("cloud_" + now, *(s->cloud_), s->binary_, s->use_camera_);

std::cout << "saved " << "cloud_" + now + ".ply" << std::endl;
}
}
}

int main(int argc, char *argv[])
{

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>(512, 424));
K2G k2g(OPENGL);
std::cout << "Syntax is: " << argv[0] << " [-processor 0|1|2] -processor options 0,1,2 correspond to CPU, OPENCL, and OPENGL respectively\n";
processor freenectprocessor = OPENGL;
std::vector<int> ply_file_indices;

if(argc > 1){
freenectprocessor = static_cast<processor>(atoi(argv[1]));
}

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>> cloud;
K2G k2g(freenectprocessor);
std::cout << "getting cloud" << std::endl;
k2g.getCloud(cloud);
cloud = k2g.getCloud();

cloud->sensor_orientation_.w() = 0.0;
cloud->sensor_orientation_.x() = 1.0;
cloud->sensor_orientation_.y() = 0.0;
cloud->sensor_orientation_.z() = 0.0;

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->setBackgroundColor (0, 0, 0);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud, rgb, "sample cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");

while(!viewer->wasStopped()){
viewer->spinOnce();

std::chrono::high_resolution_clock::time_point tnow = std::chrono::high_resolution_clock::now();
k2g.getCloud(cloud);
PlySaver ps(cloud, false, false);
viewer->registerKeyboardCallback(KeyboardEventOccurred, (void*)&ps);

bool done = false;
while((!viewer->wasStopped()) && (!done)){

viewer->spinOnce ();
std::chrono::high_resolution_clock::time_point tnow = std::chrono::high_resolution_clock::now();

cloud = k2g.updateCloud(cloud);

std::chrono::high_resolution_clock::time_point tpost = std::chrono::high_resolution_clock::now();
std::cout << "delta " << std::chrono::duration_cast<std::chrono::duration<double>>(tpost-tnow).count() * 1000 << std::endl;
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
viewer->updatePointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");
viewer->updatePointCloud<pcl::PointXYZRGB> (cloud, rgb, "sample cloud");

}

k2g.shutDown();
Expand Down

0 comments on commit 98a7464

Please sign in to comment.