CropFilter.hpp 15.9 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
#include "ofxTimeMeasurements.h"

12
13
#include "ofEasingParameterFloat.hpp"

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

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)));
28
        add(ease_dim_.set("ease dim", glm::vec3(.01), glm::vec3(.01), glm::vec3(10)));
29
30
31
        add(slide_down_.set("filter (down)", .5, 0, 1));
        add(capacity_.set("capacity", 200, 1, 5000));
        add(count_.set("count", 0, 0, 1000));
32
33
34
        add(rms_.set("rms", 0, 0, 1));
        add(height_.set("height", 0, 0, 1));
        add(thresh_.set("thresh", 0.06, 0.06, .5));
35

36
37
        dim_.addListener(this, &CropFilterParameterGroup::ease);

38
39
40
41
        add_to_string_mapping(&(ofAbstractParameter &)pos_);
        add_to_string_mapping(&(ofAbstractParameter &)dim_);
    }

42
43
44
45
46
47
    auto ease(glm::vec3 &to) -> void
    {
        
        ease_dim_.ease(to);
    }

48
49
50
51
52
53
54
55
56
    void force_trigger(ofEvent<ofAbstractParameter> event)
    {
        ofNotifyEvent(event, pos_);
        ofNotifyEvent(event, dim_);
    }

    void reset()
    {
        pos_        = glm::vec3(0, 0, .5);
57
        dim_.set(glm::vec3(.5, .5, 1));
58
59
60
61
62
63
64
        capacity_   = 200;
        slide_down_ = .5;
        count_      = 0;
        enabled_    = false;
    }

    ofParameter<float> slide_down_;
65
    ofEasingParameterVec3 ease_dim_;
66
    ofParameter<glm::vec3> pos_;
67
    ofParameter<glm::vec3> dim_;
68
69
70
    ofParameter<bool> state_;
    ofParameter<bool> newstate_;
    ofParameter<float> thresh_;
71
72
73
    ofParameter<bool> enabled_;
    ofParameter<float> capacity_;
    ofParameter<int> count_;
74
75
    ofParameter<float> rms_;
    ofParameter<float> height_;
76
77
78
79
80
};

class CropFilter
{
  public:
81
    CropFilter(int i, int total = 8)
82
83
84
    {
        index_ = i;
        result_.reset(new pcl::PointCloud<pcl::PointXYZ>);
85
        color_.setHsb((float(i) / float(total)) * 255, 255, 128, 200);
artificiel's avatar
artificiel committed
86
87
        material_.setAmbientColor(ofColor(20, 20, 20, 0));
        material_.setSpecularColor(ofColor(0, 0, 0, 0));
88
89
90
91
    }

    float get_fill()
    {
92
        auto fill = ofClamp(parameters_.count_.get() / parameters_.capacity_.get(), 0, 1);
93
94
95
96
97
98
99
100
101
102
103
104
        fill_ = fill;
        if (fill < 0.02) {
            fill_ = 0;
        } else {
            fill_ = fill;
        }
        return fill_;

    }

    float get_pan() {
        return ofMap(parameters_.pos_->x, -4, 4, 0, 1);
105
106
    }

107
108
109
110
111
112
113
114
115
116
    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();
    }

117
    virtual void draw()                       = 0;
118
119
120
121
    virtual bool apply_rule(pcl::PointXYZ &p) = 0;

    void perform(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
    {
122
        if (parameters_.enabled_ && input_cloud->size() > 0) {
123
124

            result_->clear();
125
126
127
128
129
130
            parameters_.height_ = 0;
            float new_height    = 0;

            // apply_rule from here

            // perhaps only on param_changes? (setup_rules()?)
131
132
            min_ = parameters_.pos_.get() - (parameters_.ease_dim_.get() / 2);
            max_ = parameters_.pos_.get() + (parameters_.ease_dim_.get() / 2);
133
            // end perhaps
134

135
136
            for (int i = 0; i < input_cloud->size(); i++) {
                auto p = input_cloud->points[i];
137
138
                if (apply_rule(p)) {
                    result_->push_back(p);
139
140

                    // perhaps another loop outside apply_rule?
141
142
143
                    if (p.y > new_height) new_height = ofMap(p.y, min_.y, max_.y, 0, 1, true);
                }
            }
144
145
            // end apply_rule

146
147
148
            if (new_height > parameters_.height_) {
                parameters_.height_ = new_height;
            } else {
149
                parameters_.height_ = new_height * parameters_.slide_down_ + parameters_.height_ * (1.0 - parameters_.slide_down_);
150
151
152
            }

            if (!parameters_.state_) {
153
                if (get_fill() > parameters_.thresh_ + .03) {
154
155
156
                    parameters_.newstate_ = true;
                }
            } else {
157
                if (get_fill() < parameters_.thresh_ - .03) {
158
159
160
                    parameters_.newstate_ = false;
                    parameters_.height_   = 0;
                }
161
162
163
164
165
166
167
            }

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

169
        } else {
170
171
172
            parameters_.newstate_ = false;
            parameters_.count_    = 0;
            parameters_.height_   = 0;
173
174
175
176
177
178
179
180
181
182
183
        }
    }

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

    pcl::PointCloud<pcl::PointXYZ>::Ptr result_;
    CropFilterParameterGroup parameters_;
    glm::vec3 min_, max_;
184
    ofColor color_;
185
    ofParameter<float> fill_;
186
    size_t index_;
187
    float distance_ = 10000;
188
    ofMaterial material_;
189
190
191
192
193
};

class CropBoxFilter : public CropFilter
{
  public:
194
195
196
197
    CropBoxFilter(int i, int total = 8) : CropFilter(i, total){};

    void draw() override
    {
198
        glm::vec3 size = parameters_.ease_dim_.get();
199
200
201
202
203

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

205
206
207
208
209
210
211
212
            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);

213
            if (parameters_.count_ > 10) {
214
                glm::vec3 fillsize = parameters_.ease_dim_.get() * glm::vec3(1, get_fill(), 1);
215
216
217
218
                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);
            }
219
            /*
220
221
222
223
224
225
            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);
            }
226
            */
227
            if (parameters_.state_) {
228
                c.a = 128;
229
            } else {
230
                c.a = 64;
231
            }
232
            ofSetColor(c);
233
234
235
236
237
238
239
240
241
242
243
            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();
    }
244
245
246
247
248
249
250
251
252
253
254

    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:
255
256
257
258
    CropCylinderFilter(int i, int total = 8) : CropFilter(i, total){};

    void draw() override
    {
259
        glm::vec3 size = parameters_.ease_dim_.get();
260
261
262
263
264
265
266

        ofPushStyle();
        ofEnableBlendMode(OF_BLENDMODE_ADD);
        ofDisableDepthTest();
        if (parameters_.enabled_) {
            ofSetColor(color_);
            ofNoFill();
267
            // ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
artificiel's avatar
artificiel committed
268
269
            ofDrawCylinder(parameters_.pos_->x, parameters_.pos_->y, parameters_.pos_->z, size.x * .707, size.y);
            ofFill();
270
271

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

274
275

            if (parameters_.count_ > 0) {
276
                glm::vec3 fillsize = parameters_.ease_dim_.get() * glm::vec3(1, get_fill(), 1);
277
278
                glm::vec3 fillpos  = parameters_.pos_.get() - glm::vec3(0, (size.y * (1.0 - get_fill())) / 2, 0);
                ofSetColor(c);
279
280
281
282
283
                //  ofDrawBox(fillpos, fillsize.x, fillsize.y, fillsize.z);
                material_.setDiffuseColor(c);
                material_.begin();
                ofDrawCylinder(fillpos, fillsize.x * .707, fillsize.y);
                material_.end();
284
285
            }
            if (parameters_.rms_ > 0) {
286
                /*
287
288
289
290
                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);
291
                */
292
293
294
            }
            if (parameters_.state_) {
                ofSetColor(color_);
295
                material_.setDiffuseColor(color_);
296
297
298
299
300
301

            } else {
                auto c = color_;
                c.a    = 128;
                ofSetColor(color_);
            }
302
            material_.setDiffuseColor(color_);
303
            material_.begin();
artificiel's avatar
artificiel committed
304
            ofDrawCylinder(parameters_.pos_->x, parameters_.pos_->y, parameters_.pos_->z, size.x * .707, size.y);
305
            material_.end();
306
307

        } else {
308
309
            /*
            //    a mettre dans un debug quelconque?
310
311
312
313
            ofSetColor(200, 200, 200, 128);
            ofNoFill();
            ofDrawBox(parameters_.pos_, size.x, size.y, size.z);
            ofFill();
314
            */
315
316
317
        }
        ofPopStyle();
    }
318
319
320

    bool apply_rule(pcl::PointXYZ &p) override
    {
321
        // THIS FOR BOX!
322
323
324
325
326
327
328
329
330
331
332
        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;
333

334
335
        parameters_.setName("FilterManager");
        parameters_.add(enabled_.set("enabled", true));
336

337
        for (size_t i = 0; i < num_filters; i++) {
338
339

            // TODO: figure out parametrisation of type (same type for all manager?)
340
            filters_.push_back(new CropBoxFilter(i));
341
342
343
            filters_.back()->parameters_.setName("Filter #" + ofToString(i));
            parameters_.add(filters_.back()->parameters_);
        }
344

345
346
347
        // uniquement pour presets!
        zones_gui_.setup();
        zones_gui_.add(parameters_);
348
    }
349

350
351
    void save_preset_id(int num)
    {
artificiel's avatar
artificiel committed
352
        ofLogNotice("filter_manager") << "saving preset" << ofToString(num) << ".json";
353
        zones_gui_.saveToFile("presets/zones/preset" + ofToString(num) + ".json");
354
355
356
        ofJson json;
        ofSerialize(json, parameters_);
        ofSaveJson("presets/zones/preset" + ofToString(num) + "-group.json", json);
357
    }
358

359
    void load_preset_id(int num)
360
    {
361
        ofLogNotice("filter_manager") << "loading preset" << ofToString(num) << ".json";
362
        zones_gui_.loadFromFile("presets/zones/preset" + ofToString(num) + ".json");
363
364
365
366
367


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

370
371
372
373
374
375
376
377
    void clear_preset_id(int num) {
        ofFile::removeFile("presets/zones/preset" + ofToString(num) + ".json");
    }

    bool is_there_such_a_preset_id(int num) {
        return ofFile::doesFileExist("presets/zones/preset" + ofToString(num) + ".json");
    }

378
379
    void update(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud)
    {
380

381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
        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;
                    // }
                }
404

405
406
407
408
                //      m.addIntArg(filter->parameters_.count_);
                if (excitated_) {
                    m_height.addFloatArg(filter->parameters_.height_);
                }
409
                m.addFloatArg(filter->get_fill());
410
            }
411
            sender_->sendMessage(m);
412
413
            //      sender_->sendMessage(m_height);
            TS_STOP("FilterManager::update()");
414
415
        }
    }
416

417
418
419
420
421
422
423
    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));
424
                filter->parameters_.dim_.set(glm::vec3(m.getArgAsFloat(4), m.getArgAsFloat(5), m.getArgAsFloat(6)));
425
                filter->parameters_.slide_down_ = m.getArgAsFloat(7);
426
427
                filter->parameters_.thresh_     = m.getArgAsFloat(8);
                filter->parameters_.capacity_   = m.getArgAsInt(9);
428
429
430
            }
        } else if (m.getAddress() == "/filter_manager/filter/enable") {
            int filter_num = m.getArgAsInt(0);
artificiel's avatar
artificiel committed
431

432
433
434
435
            if (filter_num >= 0 && filter_num < filters_.size()) {
                auto filter                  = filters_[filter_num];
                filter->parameters_.enabled_ = m.getArgAsBool(1);
            }
artificiel's avatar
artificiel committed
436

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

439
440
441
442
            for (int i = 0; i < 4; i++) {
                auto filter              = filters_[i];
                filter->parameters_.rms_ = 5 * (m.getArgAsFloat(i));
            }
artificiel's avatar
artificiel committed
443

444
        } else if (m.getAddress() == "/filter_manager/enable") {
artificiel's avatar
artificiel committed
445
446
447
            ofLogNotice("crop_manager") << "filter enable";

            enabled_ = m.getArgAsInt(0);
448
        } else if (m.getAddress() == "/filter_manager/save") {
artificiel's avatar
artificiel committed
449
            save_preset_id(m.getArgAsInt(0));
450
        } else if (m.getAddress() == "/filter_manager/load") {
artificiel's avatar
artificiel committed
451
            load_preset_id(m.getArgAsInt(0));
452
453
454
        }
    }
    std::vector<CropFilter *> &get_filters() { return filters_; }
455

456
457
    void draw()
    {
458
        if (enabled_) {
459

460
461
462
463
464
465
466
467
468
469
470
            ofPushMatrix();
            ofPopStyle();
            ofEnableLighting();
            for (const auto &filter : filters_) {
                filter->draw();
            }
            ofDisableLighting();

            ofPopStyle();
            ofPopMatrix();
        }
471
472
    }

473
    void draw_floor(size_t index, ofColor color, float size, std::string label)
474
    {
475
        filters_[index]->draw_floor(color, size, label);
476
    }
477
    void draw_gui()
478
    {
479
        zones_gui_.draw();
480
481
    }

482
483
    ofParameterGroup parameters_;
    ofParameter<bool> enabled_;
484
    ofxPanel zones_gui_;
485
486

  private:
487
    bool excitated_ = false;
488
489
490
491
    std::vector<CropFilter *> filters_;
    ofxOscSender *sender_;
};
}
alexandre burton's avatar
alexandre burton committed
492
#endif // CLOUDLIB_CROPFILTER_HPP_