CropFilter.hpp 15 KB
Newer Older
1

2
3
4
#ifndef CLOUDLIB_CROPFILTER_HPP_
#define CLOUDLIB_CROPFILTER_HPP_

5
6
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>
7

8
#include "ofxGui.h"
9
#include "ofxOsc.h"
10
11
12
#include "ofxTimeMeasurements.h"

#include "ofxCloudLib/ParsedParameterGroup.hpp"
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

namespace ofxCloudLib
{

class CropFilterParameterGroup : public ofxCloudLib::ParsedParameterGroup
{
  public:
    CropFilterParameterGroup() : ParsedParameterGroup()
    {
        setName("Crop Filter");
        add(enabled_.set("enable", false));
        add(pos_.set("position", glm::vec3(-10), glm::vec3(-10), glm::vec3(10)));
        add(dim_.set("dimension", glm::vec3(.01), glm::vec3(.01), glm::vec3(10)));
        add(slide_down_.set("filter (down)", .5, 0, 1));
        add(capacity_.set("capacity", 200, 1, 5000));
        add(count_.set("count", 0, 0, 1000));
29
30
31
        add(rms_.set("rms", 0, 0, 1));
        add(height_.set("height", 0, 0, 1));
        add(thresh_.set("thresh", 0.06, 0.06, .5));
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55

        add_to_string_mapping(&(ofAbstractParameter &)pos_);
        add_to_string_mapping(&(ofAbstractParameter &)dim_);
    }

    void force_trigger(ofEvent<ofAbstractParameter> event)
    {
        ofNotifyEvent(event, pos_);
        ofNotifyEvent(event, dim_);
    }

    void reset()
    {
        pos_        = glm::vec3(0, 0, .5);
        dim_        = glm::vec3(.5, .5, 1);
        capacity_   = 200;
        slide_down_ = .5;
        count_      = 0;
        enabled_    = false;
    }

    ofParameter<float> slide_down_;
    ofParameter<glm::vec3> dim_;
    ofParameter<glm::vec3> pos_;
56
57
58
    ofParameter<bool> state_;
    ofParameter<bool> newstate_;
    ofParameter<float> thresh_;
59
60
61
    ofParameter<bool> enabled_;
    ofParameter<float> capacity_;
    ofParameter<int> count_;
62
63
    ofParameter<float> rms_;
    ofParameter<float> height_;
64
65
66
67
68
};

class CropFilter
{
  public:
69
    CropFilter(int i, int total = 8)
70
71
72
    {
        index_ = i;
        result_.reset(new pcl::PointCloud<pcl::PointXYZ>);
73
        color_.setHsb((float(i) / float(total)) * 255, 255, 128, 200);
artificiel's avatar
artificiel committed
74
75
        material_.setAmbientColor(ofColor(20, 20, 20, 0));
        material_.setSpecularColor(ofColor(0, 0, 0, 0));
76
77
78
79
    }

    float get_fill()
    {
80
        auto fill = ofClamp(parameters_.count_.get() / parameters_.capacity_.get(), 0, 1);
artificiel's avatar
artificiel committed
81
82
        if (fill > .02) return fill;
        return 0;
83
84
    }

85
86
87
88
89
90
91
92
93
94
    void draw_floor(ofColor color, float size, std::string label)
    {
        ofPushStyle();
        ofSetColor(color);
        ofDrawBox(parameters_.pos_.get().x, 0, parameters_.pos_.get().z, size, .01, size);
        ofSetColor(ofColor::white);
        ofDrawBitmapString(label, parameters_.pos_.get().x, .25, parameters_.pos_.get().z);
        ofPopStyle();
    }

95
    virtual void draw()                       = 0;
96
97
98
99
    virtual bool apply_rule(pcl::PointXYZ &p) = 0;

    void perform(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
    {
100
        if (parameters_.enabled_ && input_cloud->size() > 0) {
101
102

            result_->clear();
103
104
105
106
107
108
            parameters_.height_ = 0;
            float new_height    = 0;

            // apply_rule from here

            // perhaps only on param_changes? (setup_rules()?)
109
110
            min_ = parameters_.pos_.get() - (parameters_.dim_.get() / 2);
            max_ = parameters_.pos_.get() + (parameters_.dim_.get() / 2);
111
            // end perhaps
112

113
114
            for (int i = 0; i < input_cloud->size(); i++) {
                auto p = input_cloud->points[i];
115
116
                if (apply_rule(p)) {
                    result_->push_back(p);
117
118

                    // perhaps another loop outside apply_rule?
119
120
121
                    if (p.y > new_height) new_height = ofMap(p.y, min_.y, max_.y, 0, 1, true);
                }
            }
122
123
            // end apply_rule

124
125
126
            if (new_height > parameters_.height_) {
                parameters_.height_ = new_height;
            } else {
127
                parameters_.height_ = new_height * parameters_.slide_down_ + parameters_.height_ * (1.0 - parameters_.slide_down_);
128
129
130
            }

            if (!parameters_.state_) {
131
                if (get_fill() > parameters_.thresh_ + .03) {
132
133
134
                    parameters_.newstate_ = true;
                }
            } else {
135
                if (get_fill() < parameters_.thresh_ - .03) {
136
137
138
                    parameters_.newstate_ = false;
                    parameters_.height_   = 0;
                }
139
140
141
142
143
144
145
            }

            if (result_->size() > parameters_.count_) {
                parameters_.count_ = result_->size();
            } else {
                parameters_.count_ = result_->size() * parameters_.slide_down_ + parameters_.count_ * (1.0 - parameters_.slide_down_);
            }
146

147
        } else {
148
149
150
            parameters_.newstate_ = false;
            parameters_.count_    = 0;
            parameters_.height_   = 0;
151
152
153
154
155
156
157
158
159
160
161
        }
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr get()
    {
        return result_;
    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr result_;
    CropFilterParameterGroup parameters_;
    glm::vec3 min_, max_;
162
    ofColor color_;
163
    size_t index_;
164
    float distance_ = 10000;
165
    ofMaterial material_;
166
167
168
169
170
};

class CropBoxFilter : public CropFilter
{
  public:
171
172
173
174
175
176
177
178
179
180
    CropBoxFilter(int i, int total = 8) : CropFilter(i, total){};

    void draw() override
    {
        glm::vec3 size = parameters_.dim_.get();

        ofPushStyle();
        ofEnableBlendMode(OF_BLENDMODE_ADD);
        ofDisableDepthTest();
        if (parameters_.enabled_) {
181

182
183
184
185
186
187
188
189
            ofSetColor(color_);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();

            auto c = color_;
            c.a    = ofMap(get_fill(), 0, .25, 10, 50, true);

190
            if (parameters_.count_ > 10) {
191
192
193
194
195
                glm::vec3 fillsize = parameters_.dim_.get() * glm::vec3(1, get_fill(), 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - get_fill())) / 2, 0);
                ofSetColor(c);
                ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
            }
196
            /*
197
198
199
200
201
202
            if (parameters_.rms_ > 0) {
                glm::vec3 fillsize = parameters_.dim_.get() * glm::vec3(1, parameters_.rms_, 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - parameters_.rms_)) / 2, 0);
                ofSetColor(255, 0, 0, ofMap(parameters_.rms_, 0, .25, 10, 150, true));
                ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
            }
203
            */
204
            if (parameters_.state_) {
205
                c.a = 128;
206
            } else {
207
                c.a = 64;
208
            }
209
            ofSetColor(c);
210
211
212
213
214
215
216
217
218
219
220
            ofDrawBox(parameters_.pos_->x, 0, parameters_.pos_->z, size.x, .01, size.z);


        } else {
            ofSetColor(200, 200, 200, 128);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();
        }
        ofPopStyle();
    }
221
222
223
224
225
226
227
228
229
230
231

    bool apply_rule(pcl::PointXYZ &p) override
    {
        return (p.x > min_[0] && p.y > min_[1] && p.z > min_[2] &&
                p.x < max_[0] && p.y < max_[1] && p.z < max_[2]);
    }
};

class CropCylinderFilter : public CropFilter
{
  public:
232
233
234
235
236
237
238
239
240
241
242
243
    CropCylinderFilter(int i, int total = 8) : CropFilter(i, total){};

    void draw() override
    {
        glm::vec3 size = parameters_.dim_.get();

        ofPushStyle();
        ofEnableBlendMode(OF_BLENDMODE_ADD);
        ofDisableDepthTest();
        if (parameters_.enabled_) {
            ofSetColor(color_);
            ofNoFill();
244
            // ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
artificiel's avatar
artificiel committed
245
246
            ofDrawCylinder(parameters_.pos_->x, parameters_.pos_->y, parameters_.pos_->z, size.x * .707, size.y);
            ofFill();
247
248

            auto c = color_;
artificiel's avatar
artificiel committed
249
            c.a    = 128; //ofMap(get_fill(), 0, .1, 0, 1, true);
250

251
252
253
254
255

            if (parameters_.count_ > 0) {
                glm::vec3 fillsize = parameters_.dim_.get() * glm::vec3(1, get_fill(), 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - get_fill())) / 2, 0);
                ofSetColor(c);
256
257
258
259
260
                //  ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
                material_.setDiffuseColor(c);
                material_.begin();
                ofDrawCylinder(fillpos, fillsize.x * .707, fillsize.y);
                material_.end();
261
262
            }
            if (parameters_.rms_ > 0) {
263
                /*
264
265
266
267
                glm::vec3 fillsize = parameters_.dim_.get() * glm::vec3(1, parameters_.rms_, 1);
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - parameters_.rms_)) / 2, 0);
                ofSetColor(255, 0, 0, ofMap(parameters_.rms_, 0, .25, 10, 150, true));
                ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
268
                */
269
270
271
            }
            if (parameters_.state_) {
                ofSetColor(color_);
272
                material_.setDiffuseColor(color_);
273
274
275
276
277
278

            } else {
                auto c = color_;
                c.a    = 128;
                ofSetColor(color_);
            }
279
            material_.setDiffuseColor(color_);
280
            material_.begin();
artificiel's avatar
artificiel committed
281
            ofDrawCylinder(parameters_.pos_->x, parameters_.pos_->y, parameters_.pos_->z, size.x * .707, size.y);
282
            material_.end();
283
284

        } else {
285
286
            /*
            //    a mettre dans un debug quelconque?
287
288
289
290
            ofSetColor(200, 200, 200, 128);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();
291
            */
292
293
294
        }
        ofPopStyle();
    }
295
296
297

    bool apply_rule(pcl::PointXYZ &p) override
    {
298
        // THIS FOR BOX!
299
300
301
302
303
304
305
306
307
308
309
        return (p.x > min_[0] && p.y > min_[1] && p.z > min_[2] &&
                p.x < max_[0] && p.y < max_[1] && p.z < max_[2]);
    }
};

class FilterManager
{
  public:
    FilterManager(size_t num_filters, ofxOscSender *sender)
    {
        sender_ = sender;
310

311
312
        parameters_.setName("FilterManager");
        parameters_.add(enabled_.set("enabled", true));
313

314
        for (size_t i = 0; i < num_filters; i++) {
315
316

            // TODO: figure out parametrisation of type (same type for all manager?)
317
            filters_.push_back(new CropBoxFilter(i));
318
319
320
            filters_.back()->parameters_.setName("Filter #" + ofToString(i));
            parameters_.add(filters_.back()->parameters_);
        }
321

322
323
324
        // uniquement pour presets!
        zones_gui_.setup();
        zones_gui_.add(parameters_);
325
    }
326

327
328
    void save_preset_id(int num)
    {
artificiel's avatar
artificiel committed
329
        ofLogNotice("filter_manager") << "saving preset" << ofToString(num) << ".json";
330
331
        zones_gui_.saveToFile("presets/zones/preset" + ofToString(num) + ".json");
    }
332

333
    void load_preset_id(int num)
334
    {
335
        ofLogNotice("filter_manager") << "loading preset" << ofToString(num) << ".json";
336
        zones_gui_.loadFromFile("presets/zones/preset" + ofToString(num) + ".json");
337
338
339
340
341


        for (const auto &filter : filters_) {
            zones_gui_.getGroup(parameters_.getName()).getGroup(filter->parameters_.getName()).minimize();
        }
342
343
    }

344
345
    void update(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
    {
346

347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
        if (enabled_) {
            TS_START("FilterManager::update()");

            ofxOscMessage m, m_height;
            m.setAddress("/filter_manager/counts");
            m_height.setAddress("/filter_manager/height");
            for (const auto &filter : filters_) {
                filter->perform(input_cloud);

                if (filter->parameters_.state_ != filter->parameters_.newstate_) {
                    excitated_ = true;
                    ofxOscMessage m2;
                    m2.setAddress("/filter_manager/event/state");
                    m2.addIntArg(filter->index_);
                    m2.addIntArg(filter->parameters_.newstate_);
                    sender_->sendMessage(m2);
                    filter->parameters_.state_ = filter->parameters_.newstate_;

                    // // exception hard-codee: si une zone ext est released avant la int, releasera la int
                    // if (filter->index_ > =8 && filter->parameters_.state_ == false) {
                    //     filters_[filter->index_-1].newstate_ = false;
                    // }
                }
370

371
372
373
374
                //      m.addIntArg(filter->parameters_.count_);
                if (excitated_) {
                    m_height.addFloatArg(filter->parameters_.height_);
                }
375
                m.addFloatArg(filter->get_fill());
376
            }
377
            sender_->sendMessage(m);
378
379
            //      sender_->sendMessage(m_height);
            TS_STOP("FilterManager::update()");
380
381
        }
    }
382

383
384
385
386
387
388
389
390
391
    void process_osc(ofxOscMessage &m)
    {
        if (m.getAddress() == "/filter_manager/filter/set") {
            int filter_num = m.getArgAsInt(0);
            if (filter_num >= 0 && filter_num < filters_.size()) {
                auto filter                     = filters_[filter_num];
                filter->parameters_.pos_        = glm::vec3(m.getArgAsFloat(1), m.getArgAsFloat(2), m.getArgAsFloat(3));
                filter->parameters_.dim_        = glm::vec3(m.getArgAsFloat(4), m.getArgAsFloat(5), m.getArgAsFloat(6));
                filter->parameters_.slide_down_ = m.getArgAsFloat(7);
392
393
                filter->parameters_.thresh_     = m.getArgAsFloat(8);
                filter->parameters_.capacity_   = m.getArgAsInt(9);
394
395
396
            }
        } else if (m.getAddress() == "/filter_manager/filter/enable") {
            int filter_num = m.getArgAsInt(0);
artificiel's avatar
artificiel committed
397

398
399
400
401
            if (filter_num >= 0 && filter_num < filters_.size()) {
                auto filter                  = filters_[filter_num];
                filter->parameters_.enabled_ = m.getArgAsBool(1);
            }
artificiel's avatar
artificiel committed
402

403
        } else if (m.getAddress() == "/filter_manager/rms") {
artificiel's avatar
artificiel committed
404

405
406
407
408
            for (int i = 0; i < 4; i++) {
                auto filter              = filters_[i];
                filter->parameters_.rms_ = 5 * (m.getArgAsFloat(i));
            }
artificiel's avatar
artificiel committed
409

410
        } else if (m.getAddress() == "/filter_manager/enable") {
artificiel's avatar
artificiel committed
411
412
413
            ofLogNotice("crop_manager") << "filter enable";

            enabled_ = m.getArgAsInt(0);
414
        } else if (m.getAddress() == "/filter_manager/save") {
artificiel's avatar
artificiel committed
415
            save_preset_id(m.getArgAsInt(0));
416
        } else if (m.getAddress() == "/filter_manager/load") {
artificiel's avatar
artificiel committed
417
            load_preset_id(m.getArgAsInt(0));
418
419
420
        }
    }
    std::vector<CropFilter *> &get_filters() { return filters_; }
421

422
423
    void draw()
    {
424
        if (enabled_) {
425

426
427
428
429
430
431
432
433
434
435
436
            ofPushMatrix();
            ofPopStyle();
            ofEnableLighting();
            for (const auto &filter : filters_) {
                filter->draw();
            }
            ofDisableLighting();

            ofPopStyle();
            ofPopMatrix();
        }
437
438
    }

439
    void draw_floor(size_t index, ofColor color, float size, std::string label)
440
    {
441
        filters_[index]->draw_floor(color, size, label);
442
    }
443
    void draw_gui()
444
    {
445
        zones_gui_.draw();
446
447
    }

448
449
    ofParameterGroup parameters_;
    ofParameter<bool> enabled_;
450
    ofxPanel zones_gui_;
451
452

  private:
453
    bool excitated_ = false;
454
455
456
457
    std::vector<CropFilter *> filters_;
    ofxOscSender *sender_;
};
}
alexandre burton's avatar
alexandre burton committed
458
#endif // CLOUDLIB_CROPFILTER_HPP_