Write out a triangulation result to an OBJ file in CGAL - cgal

I'm trying to make use of a 2D triangulation using CGAL, and create an obj file. I'm able to create a 2D triangulation. I now want to make the 3rd coordinate 0, ie z=0, and create a obj file out of the result of the triangulation. The samples of CGAL seem quite confusing, and I'm not sure how to go about this.

Here is how I did it. Hope it helps someone.
// A modifier creating a triangle with the incremental builder.
template<class HDS>
class polyhedron_builder : public CGAL::Modifier_base<HDS> {
public:
std::vector<Triangulation>& t_;
polyhedron_builder(std::vector<Triangulation>& t) : t_(t) {}
void operator()(HDS& hds) {
typedef typename HDS::Vertex Vertex;
typedef typename Vertex::Point Point3;
// create a cgal incremental builder
CGAL::Polyhedron_incremental_builder_3<HDS> B(hds, true);
// calculte total vertices
int face_num = 0;
int vertice_num = 0;
for (auto& tri : t_) {
face_num += tri.number_of_faces();
vertice_num += tri.number_of_vertices();
}
std::cout << face_num << ", " << vertice_num << ", " << t_.size() << "\n";
B.begin_surface(face_num, vertice_num);
// add the polyhedron vertices
for (auto& tri : t_) {
for (auto itr = tri.finite_vertices_begin(); itr != tri.finite_vertices_end(); ++itr) {
B.add_vertex(Point3(itr->point().x(), itr->point().y(), 0));
}
}
// add the polyhedron triangles
for (auto& tri : t_) {
for (auto itr = tri.finite_faces_begin(); itr != tri.finite_faces_end(); ++itr) {
B.begin_facet();
B.add_vertex_to_facet(itr->vertex(0)->info());
B.add_vertex_to_facet(itr->vertex(1)->info());
B.add_vertex_to_facet(itr->vertex(2)->info());
B.end_facet();
}
}
// finish up the surface
B.end_surface();
}
};
void OBJfile::write_obj_file(const std::string& filename) {
CGAL::Polyhedron_3<CGAL::Simple_cartesian<double>> polyhedron;
unsigned index = 0;
std::vector<Triangulation> t_vector;
// here, contours is an internal object that tracks the polygon outlines
for (auto& contour : contours_) {
Triangulation t;
std::vector < std::pair<Point, unsigned> > polygon;
for (auto& pt : contour) {
Point point(pt.x(), pt.y());
polygon.push_back(std::make_pair(point, index++));
}
triangulate(polygon, t);
t_vector.push_back(t);
}
polyhedron_builder<HalfedgeDS> builder(t_vector);
polyhedron.delegate(builder);
// write the polyhedron out as a .OFF file
std::ofstream os("test.obj");
CGAL::File_writer_wavefront writer;
CGAL::generic_print_polyhedron(os, polyhedron, writer);
os.close();
}
void OBJfile::triangulate(const std::vector<std::pair<Point, unsigned>>& polygon_points, Triangulation& t) {
auto begin = polygon_points.begin();
auto end = polygon_points.end();
//std::istream_iterator<Point> end;
t.insert(begin, end);
}

Related

How to clear the std::map<K,V*> container and delete all the pointed objects safely?

None of the standard library containers will call delete on contained raw pointers. I have checked for a solution on SO for C++98 but have not found the answer.
I have created template <typename K, typename V> void clearAndDestroy( std::map<K, V*> *&myMap) as a replacement function for std::clear() (remove all elements and call the destructors).
It works for maps with pointers to objects std::map(key,V*). The function works also for cases when map contains same V* pointers for the different keys.
#include <iostream>
#include <map>
#include <string>
#include <set>
using namespace std;
// clearAndDestroy deletes all objects and remove them from the std::map(K,V*) container.
template <typename K, typename V>
void clearAndDestroy( std::map<K, V*> *&myMap)
{
if(myMap == NULL)
return;
std::set<V*> mySet;
typename std::map<K,V*>::iterator itr;
typename std::set<V*>::iterator sitr;
itr = myMap->begin();
while (itr != myMap->end()) {
mySet.insert(itr->second);
++itr;
}
sitr = mySet.begin();
while (sitr != mySet.end()) {
delete(*sitr);
++sitr;
}
myMap->clear();
}
template <typename K, typename V> void clear1( std::map<K, V*> *myMap)
{
if(myMap == NULL) return;
typename std::map<K, V*>::iterator itr = myMap->begin();
while (itr != myMap->end()) {
typename std::map<K, V*>::iterator toErase = itr;
++itr;
myMap->erase(toErase);
delete(toErase->second);
}
}
template <typename M> void clear2( M *myMap )
{
if(myMap == NULL) return;
for ( typename M::iterator it = myMap->begin(); it != myMap->end(); ++it ) {
delete it->second;
}
myMap->clear();
}
class MY_CLASS
{
public:
int counter;
string *message;
MY_CLASS(int c, string *m):counter(c), message(m) {
std::cout << "Constructor MY_CLASS " << this << std::endl;
};
~MY_CLASS()
{
if(message) {
cout << "Being destroyed MY_CLASS: " << *message << " this = " << this <<endl;
}
else {
cout << "Being destoyed MY_CLASS: " << " this = " << this <<endl;
}
if(message) {
delete message;
message = NULL;
}
}
MY_CLASS(const MY_CLASS & other)
{
std::cout << "Copy Constructor MY_CLASS " << this << std::endl;
//1.
counter = other.counter;
//2.
if(other.message) {
message = new string;
*message = *other.message; // copy the value
}
else {
message = NULL;
}
}
};
void print(const string *str,MY_CLASS *& value, void *)
{
if (value && value->message)
cout << value->counter << " ! " << *(value->message) << endl;
}
int main() {
std::map<std::string, MY_CLASS *> *mpa = new std::map<std::string, MY_CLASS *>;
MY_CLASS *p = new MY_CLASS(2, new string("abc"));
mpa->insert(std::pair<std::string, MY_CLASS *>("1", p));
mpa->insert(std::pair<std::string, MY_CLASS *>("2", p));
clearAndDestroy(mpa);
delete mpa;
return 0;
}
Output:
Constructor MY_CLASS 0x111ccb0
Being destroyed MY_CLASS: abc this = 0x111ccb0
Being restricted to C++98 is clearAndDestroy my best option? Thank you!
Another approach you can take is using an object wrapper. Place the pointer you want within an object and have the destructor call the delete on the pointer. Basically building a simple "smart-pointer".
class AutoDeletePtr {
MY_CLASS* pointer;
AutoDeletePtr(MY_CLASS* myObjectPtr) {pointer = myObjectPtr};
~AutoDeletePtr() {delete(pointer)};
}
You can insert these objects into the std::map.

OpenKinect acquire raw depth image

I am trying to use the example code from here.
I have made some changes in order to save the images to the computer. When I read the data in MATLAB it seems like values that should be 0 are set to 2047, and overall it does not seem to be correct when I reconstruct the 3D points using the default intrinsic camera parameters.
What I want to achieve is to save the images so that I can use
img = single(imread(depth.png'))/ 1000
and have the depth values in meters, and pixels with no measurements should be zero.
It is the Kinect V1 by the way.
Here is the code with comments where I have tried to change.
#include "libfreenect.hpp"
#include <iostream>
#include <vector>
#include <cmath>
#include <pthread.h>
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
using namespace cv;
using namespace std;
class myMutex {
public:
myMutex() {
pthread_mutex_init( &m_mutex, NULL );
}
void lock() {
pthread_mutex_lock( &m_mutex );
}
void unlock() {
pthread_mutex_unlock( &m_mutex );
}
private:
pthread_mutex_t m_mutex;
};
// Should one use FREENECT_DEPTH_REGISTERED instead of FREENECT_DEPTH_11BIT?
class MyFreenectDevice : public Freenect::FreenectDevice {
public:
MyFreenectDevice(freenect_context *_ctx, int _index)
: Freenect::FreenectDevice(_ctx, _index), m_buffer_depth(FREENECT_DEPTH_11BIT),
m_buffer_rgb(FREENECT_VIDEO_RGB), m_gamma(2048), m_new_rgb_frame(false),
m_new_depth_frame(false), depthMat(Size(640,480),CV_16UC1),
rgbMat(Size(640,480), CV_8UC3, Scalar(0)),
ownMat(Size(640,480),CV_8UC3,Scalar(0)) {
for( unsigned int i = 0 ; i < 2048 ; i++) {
float v = i/2048.0;
v = std::pow(v, 3)* 6;
m_gamma[i] = v*6*256;
}
}
// Do not call directly even in child
void VideoCallback(void* _rgb, uint32_t timestamp) {
std::cout << "RGB callback" << std::endl;
m_rgb_mutex.lock();
uint8_t* rgb = static_cast<uint8_t*>(_rgb);
rgbMat.data = rgb;
m_new_rgb_frame = true;
m_rgb_mutex.unlock();
};
// Do not call directly even in child
void DepthCallback(void* _depth, uint32_t timestamp) {
std::cout << "Depth callback" << std::endl;
m_depth_mutex.lock();
uint16_t* depth = static_cast<uint16_t*>(_depth);
// Here I use memcpy instead so I can use uint16
// memcpy(depthMat.data,depth,depthMat.rows*depthMat.cols*sizeof(uint16_t));
depthMat.data = (uchar*) depth;
m_new_depth_frame = true;
m_depth_mutex.unlock();
}
bool getVideo(Mat& output) {
m_rgb_mutex.lock();
if(m_new_rgb_frame) {
cv::cvtColor(rgbMat, output, CV_RGB2BGR);
m_new_rgb_frame = false;
m_rgb_mutex.unlock();
return true;
} else {
m_rgb_mutex.unlock();
return false;
}
}
bool getDepth(Mat& output) {
m_depth_mutex.lock();
if(m_new_depth_frame) {
depthMat.copyTo(output);
m_new_depth_frame = false;
m_depth_mutex.unlock();
return true;
} else {
m_depth_mutex.unlock();
return false;
}
}
private:
// Should it be uint16_t instead or even higher?
std::vector<uint8_t> m_buffer_depth;
std::vector<uint8_t> m_buffer_rgb;
std::vector<uint16_t> m_gamma;
Mat depthMat;
Mat rgbMat;
Mat ownMat;
myMutex m_rgb_mutex;
myMutex m_depth_mutex;
bool m_new_rgb_frame;
bool m_new_depth_frame;
};
int main(int argc, char **argv) {
bool die(false);
string filename("snapshot");
string suffix(".png");
int i_snap(0),iter(0);
Mat depthMat(Size(640,480),CV_16UC1);
Mat depthf (Size(640,480),CV_8UC1);
Mat rgbMat(Size(640,480),CV_8UC3,Scalar(0));
Mat ownMat(Size(640,480),CV_8UC3,Scalar(0));
// The next two lines must be changed as Freenect::Freenect
// isn't a template but the method createDevice:
// Freenect::Freenect<MyFreenectDevice> freenect;
// MyFreenectDevice& device = freenect.createDevice(0);
// by these two lines:
Freenect::Freenect freenect;
MyFreenectDevice& device = freenect.createDevice<MyFreenectDevice>(0);
namedWindow("rgb",CV_WINDOW_AUTOSIZE);
namedWindow("depth",CV_WINDOW_AUTOSIZE);
device.startVideo();
device.startDepth();
while (!die) {
device.getVideo(rgbMat);
device.getDepth(depthMat);
// Here I save the depth images
std::ostringstream file;
file << filename << i_snap << suffix;
cv::imwrite(file.str(),depthMat);
cv::imshow("rgb", rgbMat);
depthMat.convertTo(depthf, CV_8UC1, 255.0/2048.0);
cv::imshow("depth",depthf);
if(iter >= 1000) break;
iter++;
}
device.stopVideo();
device.stopDepth();
return 0;
}
Thanks in advance!
Erik
I dont have any experience with OpenKinect in particular; but should your depth buffer be uint16?
std::vector<uint8_t> m_buffer_depth;
Also; for Matlab, do check if the image that you are reading is a uint16 or uint8. If its the latter then convert it to uint16
uint16(imread('depth.png'));
Sorry couldn't help more. Hope this helps.
The values you have are the raw depth values. You need to remap those into MM for the numbers to make sense. Kinect 1 can see up to 10 meters. So I would go with raw_values/2407*10000.
If the values are saturated at 2047, you are probably using the FREENECT_DEPTH_11BIT_PACKED depth format.
For work in Matlab, it is always easier to use FREENECT_DEPTH_MM or FREENECT_DEPTH_REGISTERED.
Enjoy.

Tagging (or coloring) CGAL objects

I am trying to use CGAL to perform some simple 2D CSG operations. Here is an example of an intersection of two polygons.
The actual problem is tracking down the origin (marked with color) of each segment in resulting polygon.
I would like to know if that is possible, maybe with some hacking on the CGAL itself. Any suggestion will be highly appreciated.
Unfortunately, there is no out-of-the-box way doing it. However, it doesn't require too much (famous last words...). You need to do two things described below. The first is supported by the API. The second is not, so you will need to patch a source file. A simple example is provided further bellow. Notice that the data you need, that is, the specification of the origin of each edge, ends up in a 2D arrangement data structure. If you want to obtain the polygons with this data, you need to extract them from the arrangement. You can obtain the header pgn_print.h, used in the example, from the 2D-Arrangement book.
Use an instance of CGAL::Polygon_set_2<Kernel, Container, Dcel>, where the Dcel is substituted with an extended Dcel, the halfedge of which is extended with a label that indicates the origin of the halfedge (i.e., first polygon, second polygon, or both in case of an overlap).
Patch the header file Boolean_set_operations_2/Gps_base_functor.h. In particular, add to the body of the three functions called create_edge() statements that set the label of the resulting halfedges according to their origin:
void create_edge(Halfedge_const_handle h1, Halfedge_const_handle h2,
Halfedge_handle h)
{
h->set_label(3);
h->twin()->set_label(3);
}
void create_edge(Halfedge_const_handle h1, Face_const_handle f2,
Halfedge_handle h)
{
h->set_label(1);
h->twin()->set_label(1);
}
void create_edge(Face_const_handle f1, Halfedge_const_handle h2,
Halfedge_handle h)
{
h->set_label(2);
h->twin()->set_label(2);
}
#include <list>
#include <vector>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
#include <CGAL/Boolean_set_operations_2.h>
#include <CGAL/Polygon_set_2.h>
#include "pgn_print.h"
/*! Extend the arrangement halfedge */
template <typename X_monotone_curve_2>
class Arr_labeled_halfedge :
public CGAL::Arr_halfedge_base<X_monotone_curve_2>
{
private:
unsigned m_label;
public:
Arr_labeled_halfedge() : m_label(0) {}
unsigned label() const { return m_label; }
void set_label(unsigned label) { m_label = label; }
virtual void assign(const Arr_labeled_halfedge& he)
{
CGAL::Arr_halfedge_base<X_monotone_curve_2>::assign(he);
m_label = he.m_label;
}
};
template <typename Traits>
class Arr_labeled_dcel :
public CGAL::Arr_dcel_base<CGAL::Arr_vertex_base<typename Traits::Point_2>,
Arr_labeled_halfedge<typename Traits::
X_monotone_curve_2>,
CGAL::Gps_face_base>
{
public:
Arr_labeled_dcel() {}
};
typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;
typedef Kernel::Point_2 Point_2;
typedef CGAL::Polygon_2<Kernel> Polygon_2;
typedef CGAL::Polygon_with_holes_2<Kernel> Polygon_with_holes_2;
typedef std::vector<Point_2> Container;
typedef CGAL::Gps_segment_traits_2<Kernel, Container> Traits_2;
typedef Arr_labeled_dcel<Traits_2> Dcel;
typedef CGAL::Polygon_set_2<Kernel, Container, Dcel> Polygon_set_2;
typedef std::list<Polygon_with_holes_2> Pwh_list_2;
typedef Polygon_set_2::Arrangement_2 Arrangement_2;
typedef Arrangement_2::Edge_const_iterator Edge_const_iterator;
void print_result(const Polygon_set_2& S)
{
std::cout << "The result contains " << S.number_of_polygons_with_holes()
<< " components:" << std::endl;
Pwh_list_2 res;
S.polygons_with_holes(std::back_inserter(res));
for (Pwh_list_2::const_iterator hit = res.begin(); hit != res.end(); ++hit) {
std::cout << "--> ";
print_polygon_with_holes(*hit);
}
const Arrangement_2& arr = S.arrangement();
for (Edge_const_iterator it = arr.edges_begin(); it != arr.edges_end(); ++it) {
std::cout << it->curve()
<< ", " << it->label()
<< std::endl;
}
}
int main()
{
// Construct the two input polygons.
Polygon_2 P;
P.push_back(Point_2(0, 0));
P.push_back(Point_2(5, 0));
P.push_back(Point_2(3.5, 1.5));
P.push_back(Point_2(2.5, 0.5));
P.push_back(Point_2(1.5, 1.5));
std::cout << "P = "; print_polygon(P);
Polygon_2 Q;
Q.push_back(Point_2(0, 2));
Q.push_back(Point_2(1.5, 0.5));
Q.push_back(Point_2(2.5, 1.5));
Q.push_back(Point_2(3.5, 0.5));
Q.push_back(Point_2(5, 2));
std::cout << "Q = "; print_polygon(Q);
// Compute the union of P and Q.
Polygon_set_2 intersection_set;
intersection_set.insert(P);
intersection_set.intersection(Q);
print_result(intersection_set);
// Compute the intersection of P and Q.
Polygon_set_2 union_set;
union_set.insert(P);
union_set.join(Q);
print_result(union_set);
return 0;
}

Using Point Cloud Library to store Point Clouds from Kinect

Using Point Cloud Library on Ubuntu, I am trying to take multiple point clouds from the Kinect and store them in memory for later use in the program. My code shown at the bottom of this post is designed to store the first Point Cloud from the Kinect and output its width and height. The program gives me a runtime error:
/usr/include/boost/smart_ptr/shared_ptr.hpp:418: T* boost::shared_ptr<T>::operator->() const [with T = pcl::PointCloud<pcl::PointXYZ>]: Assertion `px != 0' failed.
All help is greatly appreciated and I always accept an answer!
The code:
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
class SimpleOpenNIViewer
{
public:
SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {}
pcl::PointCloud<pcl::PointXYZ>::Ptr prevCloud;
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud)
{
if (!viewer.wasStopped())
viewer.showCloud (cloud);
//ICP start
if(!prevCloud) {
pcl::PointCloud<pcl::PointXYZ>::Ptr prevCloud( new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud<pcl::PointXYZ, pcl::PointXYZ>(*cloud, *prevCloud);
}
cout << prevCloud->width << " by " << prevCloud->height << endl;
}
void run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
interface->registerCallback (f);
interface->start ();
while (!viewer.wasStopped())
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
}
interface->stop ();
}
pcl::visualization::CloudViewer viewer;
};
int main ()
{
SimpleOpenNIViewer v;
v.run ();
return 0;
}
Try this, I don't have the Kinect drivers installed so I can't test. Basically in my version prevCloud is instantiated in the constructor so (!prevCloud) will always equal 'false'. Which is to say prevCloud.get() != NULL.
#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
class SimpleOpenNIViewer
{
typedef pcl::PointXYZ Point;
typedef pcl::PointCloud<Point> PointCloud;
public:
SimpleOpenNIViewer () : viewer ("PCL OpenNI Viewer") {
prevCloud = PointCloud::Ptr(NULL);
}
void cloud_cb_ (const PointCloud::ConstPtr &cloud)
{
if (!viewer.wasStopped())
viewer.showCloud (cloud);
if (!prevCloud) // init previous cloud if first frame
prevCloud = PointCloud::Ptr(new PointCloud);
else. // else RunICP between cloud and prevCloud
//RunICP(cloud,prevCloud);
//Copy new frame in to prevCloud
pcl::copyPointCloud<Point, Point>(*cloud, *prevCloud);
cout << prevCloud->width << " by " << prevCloud->height << endl;
}
void run ()
{
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const PointCloud::ConstPtr&)> f =
boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);
interface->registerCallback (f);
interface->start ();
while (!viewer.wasStopped())
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
}
interface->stop ();
}
PointCloud::Ptr prevCloud;
pcl::visualization::CloudViewer viewer;
};
int main ()
{
SimpleOpenNIViewer v;
v.run ();
return 0;
}
You're creating a new local variable prevCloud and copying cloud into it, rather than copying into the prevCloud field. So, if the field's value was null before the if {}, it is still null after the if {} and so it throws an error when you try to dereference it.
May be this code can help you, the cloud is saved in a "pcd" file, take a look here
And other option is work with the "Kinfu" project from PCL

What is the most reliable way to record a Kinect stream for later playback?

I have been working with Processing and Cinder to modify Kinect input on the fly. However, I would also like to record the full stream (depth+color+accelerometer values, and whatever else is in there). I'm recording so I can try out different effects/treatments on the same material.
Because I am still just learning Cinder and Processing is quite slow/laggy, I have had trouble finding advice on a strategy for capturing the stream - anything (preferably in Cinder, oF, or Processing) would be really helpful.
I've tried both Processing and OpenFrameworks. Processing is slower when displaying both images (depth and colour). OpenFrameworks slows a bit while writing the data to disk, but here's the basic approach:
Setup Openframeworks (open and compile any sample to make sure you're up and running)
Download the ofxKinect addon and copy the example project as described on github.
Once you've got OF and the ofxKinect example running, it's just a matter of adding a few variable to save your data:
In this basic setup, I've created a couple of ofImage instances and a boolean to toggle saving. In the example the depth and RGB buffers are saved into ofxCvGrayscaleImage instances, but I haven't used OF and OpenCV enough to know how to do something as simple as saving an image to disk, which is why I've used two ofImage instances.
I don't know how comfortable you are with Processing, OF, Cinder, so, for arguments' sake I'll assume you know you're way around Processing, but you're still tackling C++.
OF is pretty similar to Processing, but there are a few differences:
In Processing you have variables declaration and they're usage in the same file. In OF you've got a .h file where you declare you're variables and the .cpp file where you initialize and use your variables.
In Processing you have the setup()(initialize variables) and draw()(update variables and draw to screen) methods, while in OF you have setup() (same as in Processing), update() (update variables only, nothing visual) and draw() (draw to screen using updated values)
When working with images, you since you're coding in C++, you need to allocate memory first, as opposed to Processing/Java where you have memory management.
There's more differences that I won'te detail here. Do check out OF for Processing Users on the wiki
Back to the exampleKinect example, here my basic setup:
.h file:
#pragma once
#include "ofMain.h"
#include "ofxOpenCv.h"
#include "ofxKinect.h"
class testApp : public ofBaseApp {
public:
void setup();
void update();
void draw();
void exit();
void drawPointCloud();
void keyPressed (int key);
void mouseMoved(int x, int y );
void mouseDragged(int x, int y, int button);
void mousePressed(int x, int y, int button);
void mouseReleased(int x, int y, int button);
void windowResized(int w, int h);
ofxKinect kinect;
ofxCvColorImage colorImg;
ofxCvGrayscaleImage grayImage;
ofxCvGrayscaleImage grayThresh;
ofxCvGrayscaleImage grayThreshFar;
ofxCvContourFinder contourFinder;
ofImage colorData;//to save RGB data to disk
ofImage grayData;//to save depth data to disk
bool bThreshWithOpenCV;
bool drawPC;
bool saveData;//save to disk toggle
int nearThreshold;
int farThreshold;
int angle;
int pointCloudRotationY;
int saveCount;//counter used for naming 'frames'
};
and the .cpp file:
#include "testApp.h"
//--------------------------------------------------------------
void testApp::setup() {
//kinect.init(true); //shows infrared image
kinect.init();
kinect.setVerbose(true);
kinect.open();
colorImg.allocate(kinect.width, kinect.height);
grayImage.allocate(kinect.width, kinect.height);
grayThresh.allocate(kinect.width, kinect.height);
grayThreshFar.allocate(kinect.width, kinect.height);
//allocate memory for these ofImages which will be saved to disk
colorData.allocate(kinect.width, kinect.height, OF_IMAGE_COLOR);
grayData.allocate(kinect.width, kinect.height, OF_IMAGE_GRAYSCALE);
nearThreshold = 230;
farThreshold = 70;
bThreshWithOpenCV = true;
ofSetFrameRate(60);
// zero the tilt on startup
angle = 0;
kinect.setCameraTiltAngle(angle);
// start from the front
pointCloudRotationY = 180;
drawPC = false;
saveCount = 0;//init frame counter
}
//--------------------------------------------------------------
void testApp::update() {
ofBackground(100, 100, 100);
kinect.update();
if(kinect.isFrameNew()) // there is a new frame and we are connected
{
grayImage.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height);
if(saveData){
//if toggled, set depth and rgb pixels to respective ofImage, save to disk and update the 'frame' counter
grayData.setFromPixels(kinect.getDepthPixels(), kinect.width, kinect.height,true);
colorData.setFromPixels(kinect.getCalibratedRGBPixels(), kinect.width, kinect.height,true);
grayData.saveImage("depth"+ofToString(saveCount)+".png");
colorData.saveImage("color"+ofToString(saveCount)+".png");
saveCount++;
}
//we do two thresholds - one for the far plane and one for the near plane
//we then do a cvAnd to get the pixels which are a union of the two thresholds.
if( bThreshWithOpenCV ){
grayThreshFar = grayImage;
grayThresh = grayImage;
grayThresh.threshold(nearThreshold, true);
grayThreshFar.threshold(farThreshold);
cvAnd(grayThresh.getCvImage(), grayThreshFar.getCvImage(), grayImage.getCvImage(), NULL);
}else{
//or we do it ourselves - show people how they can work with the pixels
unsigned char * pix = grayImage.getPixels();
int numPixels = grayImage.getWidth() * grayImage.getHeight();
for(int i = 0; i < numPixels; i++){
if( pix[i] < nearThreshold && pix[i] > farThreshold ){
pix[i] = 255;
}else{
pix[i] = 0;
}
}
}
//update the cv image
grayImage.flagImageChanged();
// find contours which are between the size of 20 pixels and 1/3 the w*h pixels.
// also, find holes is set to true so we will get interior contours as well....
contourFinder.findContours(grayImage, 10, (kinect.width*kinect.height)/2, 20, false);
}
}
//--------------------------------------------------------------
void testApp::draw() {
ofSetColor(255, 255, 255);
if(drawPC){
ofPushMatrix();
ofTranslate(420, 320);
// we need a proper camera class
drawPointCloud();
ofPopMatrix();
}else{
kinect.drawDepth(10, 10, 400, 300);
kinect.draw(420, 10, 400, 300);
grayImage.draw(10, 320, 400, 300);
contourFinder.draw(10, 320, 400, 300);
}
ofSetColor(255, 255, 255);
stringstream reportStream;
reportStream << "accel is: " << ofToString(kinect.getMksAccel().x, 2) << " / "
<< ofToString(kinect.getMksAccel().y, 2) << " / "
<< ofToString(kinect.getMksAccel().z, 2) << endl
<< "press p to switch between images and point cloud, rotate the point cloud with the mouse" << endl
<< "using opencv threshold = " << bThreshWithOpenCV <<" (press spacebar)" << endl
<< "set near threshold " << nearThreshold << " (press: + -)" << endl
<< "set far threshold " << farThreshold << " (press: < >) num blobs found " << contourFinder.nBlobs
<< ", fps: " << ofGetFrameRate() << endl
<< "press c to close the connection and o to open it again, connection is: " << kinect.isConnected() << endl
<< "press s to toggle saving depth and color data. currently saving: " << saveData << endl
<< "press UP and DOWN to change the tilt angle: " << angle << " degrees";
ofDrawBitmapString(reportStream.str(),20,656);
}
void testApp::drawPointCloud() {
ofScale(400, 400, 400);
int w = 640;
int h = 480;
ofRotateY(pointCloudRotationY);
float* distancePixels = kinect.getDistancePixels();
glBegin(GL_POINTS);
int step = 2;
for(int y = 0; y < h; y += step) {
for(int x = 0; x < w; x += step) {
ofPoint cur = kinect.getWorldCoordinateFor(x, y);
ofColor color = kinect.getCalibratedColorAt(x,y);
glColor3ub((unsigned char)color.r,(unsigned char)color.g,(unsigned char)color.b);
glVertex3f(cur.x, cur.y, cur.z);
}
}
glEnd();
}
//--------------------------------------------------------------
void testApp::exit() {
kinect.setCameraTiltAngle(0); // zero the tilt on exit
kinect.close();
}
//--------------------------------------------------------------
void testApp::keyPressed (int key) {
switch (key) {
case ' ':
bThreshWithOpenCV = !bThreshWithOpenCV;
break;
case'p':
drawPC = !drawPC;
break;
case '>':
case '.':
farThreshold ++;
if (farThreshold > 255) farThreshold = 255;
break;
case '<':
case ',':
farThreshold --;
if (farThreshold < 0) farThreshold = 0;
break;
case '+':
case '=':
nearThreshold ++;
if (nearThreshold > 255) nearThreshold = 255;
break;
case '-':
nearThreshold --;
if (nearThreshold < 0) nearThreshold = 0;
break;
case 'w':
kinect.enableDepthNearValueWhite(!kinect.isDepthNearValueWhite());
break;
case 'o':
kinect.setCameraTiltAngle(angle); // go back to prev tilt
kinect.open();
break;
case 'c':
kinect.setCameraTiltAngle(0); // zero the tilt
kinect.close();
break;
case 's'://s to toggle saving data
saveData = !saveData;
break;
case OF_KEY_UP:
angle++;
if(angle>30) angle=30;
kinect.setCameraTiltAngle(angle);
break;
case OF_KEY_DOWN:
angle--;
if(angle<-30) angle=-30;
kinect.setCameraTiltAngle(angle);
break;
}
}
//--------------------------------------------------------------
void testApp::mouseMoved(int x, int y) {
pointCloudRotationY = x;
}
//--------------------------------------------------------------
void testApp::mouseDragged(int x, int y, int button)
{}
//--------------------------------------------------------------
void testApp::mousePressed(int x, int y, int button)
{}
//--------------------------------------------------------------
void testApp::mouseReleased(int x, int y, int button)
{}
//--------------------------------------------------------------
void testApp::windowResized(int w, int h)
{}
This is a very basic setup. Feel free to modify (add tilt angle to the saved data, etc.)
I'm pretty sure there are ways to improve this speedwise (e.g. don't update ofxCvGrayscaleImage instances and don't draw images to screen while saving, or stack a few frames and write them at interval as opposed to on every frame, etc.)
Goodluck