-
Notifications
You must be signed in to change notification settings - Fork 782
Expand file tree
/
Copy pathlibfreenect2.cpp
More file actions
1043 lines (844 loc) · 29.8 KB
/
libfreenect2.cpp
File metadata and controls
1043 lines (844 loc) · 29.8 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
/*
* This file is part of the OpenKinect Project. http://www.openkinect.org
*
* Copyright (c) 2014 individual OpenKinect contributors. See the CONTRIB file
* for details.
*
* This code is licensed to you under the terms of the Apache License, version
* 2.0, or, at your option, the terms of the GNU General Public License,
* version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
* or the following URLs:
* http://www.apache.org/licenses/LICENSE-2.0
* http://www.gnu.org/licenses/gpl-2.0.txt
*
* If you redistribute this file in source form, modified or unmodified, you
* may:
* 1) Leave this header intact and distribute it under the same terms,
* accompanying it with the APACHE20 and GPL20 files, or
* 2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
* 3) Delete the GPL v2 clause and accompany it with the APACHE20 file
* In all cases you must keep the copyright notice intact and include a copy
* of the CONTRIB file.
*
* Binary distributions must follow the binary distribution requirements of
* either License.
*/
/** @file libfreenect2.cpp Freenect2 devices and processing implementation. */
#include <string>
#include <vector>
#include <algorithm>
#include <libusb.h>
#include <limits>
#include <cmath>
#define WRITE_LIBUSB_ERROR(__RESULT) libusb_error_name(__RESULT) << " " << libusb_strerror((libusb_error)__RESULT)
#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/usb/event_loop.h>
#include <libfreenect2/usb/transfer_pool.h>
#include <libfreenect2/depth_packet_processor.h>
#include <libfreenect2/rgb_packet_processor.h>
#include <libfreenect2/protocol/usb_control.h>
#include <libfreenect2/protocol/command.h>
#include <libfreenect2/protocol/response.h>
#include <libfreenect2/protocol/command_transaction.h>
#include <libfreenect2/logging.h>
#ifdef __APPLE__
#define PKTS_PER_XFER 128
#define NUM_XFERS 4
#else
#define PKTS_PER_XFER 8
#define NUM_XFERS 60
#endif
namespace libfreenect2
{
using namespace libfreenect2;
using namespace libfreenect2::usb;
using namespace libfreenect2::protocol;
/*
For detailed analysis see https://github.com/OpenKinect/libfreenect2/issues/144
The following discussion is in no way authoritative. It is the current best
explanation considering the hardcoded parameters and decompiled code.
p0 tables are the "initial shift" of phase values, as in US8587771 B2.
Three p0 tables are used for "disamgibuation" in the first half of stage 2
processing.
At the end of stage 2 processing:
phase_final is the phase shift used to compute the travel distance.
What is being measured is max_depth (d), the total travel distance of the
reflected ray.
But what we want is depth_fit (z), the distance from reflection to the XY
plane. There are two issues: the distance before reflection is not needed;
and the measured ray is not normal to the XY plane.
Suppose L is the distance between the light source and the focal point (a
fixed constant), and xu,yu is the undistorted and normalized coordinates for
each measured pixel at unit depth.
Through some derivation, we have
z = (d*d - L*L)/(d*sqrt(xu*xu + yu*yu + 1) - xu*L)/2.
The expression in stage 2 processing is a variant of this, with the term
`-L*L` removed. Detailed derivation can be found in the above issue.
Here, the two terms `sqrt(xu*xu + yu*yu + 1)` and `xu` requires undistorted
coordinates, which is hard to compute in real-time because the inverse of
radial and tangential distortion has no analytical solutions and requires
numeric methods to solve. Thus these two terms are precomputed once and
their variants are stored as ztable and xtable respectively.
Even though x/ztable is derived with undistortion, they are only used to
correct the effect of distortion on the z value. Image warping is needed for
correcting distortion on x-y value, which happens in registration.cpp.
*/
struct IrCameraTables: Freenect2Device::IrCameraParams
{
std::vector<float> xtable;
std::vector<float> ztable;
std::vector<short> lut;
IrCameraTables(const Freenect2Device::IrCameraParams &parent):
Freenect2Device::IrCameraParams(parent),
xtable(DepthPacketProcessor::TABLE_SIZE),
ztable(DepthPacketProcessor::TABLE_SIZE),
lut(DepthPacketProcessor::LUT_SIZE)
{
const double scaling_factor = 8192;
const double unambigious_dist = 6250.0/3;
size_t divergence = 0;
for (size_t i = 0; i < DepthPacketProcessor::TABLE_SIZE; i++)
{
size_t xi = i % 512;
size_t yi = i / 512;
double xd = (xi + 0.5 - cx)/fx;
double yd = (yi + 0.5 - cy)/fy;
double xu, yu;
divergence += !undistort(xd, yd, xu, yu);
xtable[i] = scaling_factor*xu;
ztable[i] = unambigious_dist/sqrt(xu*xu + yu*yu + 1);
}
if (divergence > 0)
LOG_ERROR << divergence << " pixels in x/ztable have incorrect undistortion.";
short y = 0;
for (int x = 0; x < 1024; x++)
{
unsigned inc = 1 << (x/128 - (x>=128));
lut[x] = y;
lut[1024 + x] = -y;
y += inc;
}
lut[1024] = 32767;
}
//x,y: undistorted, normalized coordinates
//xd,yd: distorted, normalized coordinates
void distort(double x, double y, double &xd, double &yd) const
{
double x2 = x * x;
double y2 = y * y;
double r2 = x2 + y2;
double xy = x * y;
double kr = ((k3 * r2 + k2) * r2 + k1) * r2 + 1.0;
xd = x*kr + p2*(r2 + 2*x2) + 2*p1*xy;
yd = y*kr + p1*(r2 + 2*y2) + 2*p2*xy;
}
//The inverse of distort() using Newton's method
//Return true if converged correctly
//This function considers tangential distortion with double precision.
bool undistort(double x, double y, double &xu, double &yu) const
{
double x0 = x;
double y0 = y;
double last_x = x;
double last_y = y;
const int max_iterations = 100;
int iter;
for (iter = 0; iter < max_iterations; iter++) {
double x2 = x*x;
double y2 = y*y;
double x2y2 = x2 + y2;
double x2y22 = x2y2*x2y2;
double x2y23 = x2y2*x2y22;
//Jacobian matrix
double Ja = k3*x2y23 + (k2+6*k3*x2)*x2y22 + (k1+4*k2*x2)*x2y2 + 2*k1*x2 + 6*p2*x + 2*p1*y + 1;
double Jb = 6*k3*x*y*x2y22 + 4*k2*x*y*x2y2 + 2*k1*x*y + 2*p1*x + 2*p2*y;
double Jc = Jb;
double Jd = k3*x2y23 + (k2+6*k3*y2)*x2y22 + (k1+4*k2*y2)*x2y2 + 2*k1*y2 + 2*p2*x + 6*p1*y + 1;
//Inverse Jacobian
double Jdet = 1/(Ja*Jd - Jb*Jc);
double a = Jd*Jdet;
double b = -Jb*Jdet;
double c = -Jc*Jdet;
double d = Ja*Jdet;
double f, g;
distort(x, y, f, g);
f -= x0;
g -= y0;
x -= a*f + b*g;
y -= c*f + d*g;
const double eps = std::numeric_limits<double>::epsilon()*16;
if (fabs(x - last_x) <= eps && fabs(y - last_y) <= eps)
break;
last_x = x;
last_y = y;
}
xu = x;
yu = y;
return iter < max_iterations;
}
};
/** Freenect2 device implementation. */
class Freenect2DeviceImpl : public Freenect2Device
{
private:
enum State
{
Created,
Open,
Streaming,
Closed
};
State state_;
bool has_usb_interfaces_;
Freenect2Impl *context_;
libusb_device *usb_device_;
libusb_device_handle *usb_device_handle_;
BulkTransferPool rgb_transfer_pool_;
IsoTransferPool ir_transfer_pool_;
UsbControl usb_control_;
CommandTransaction command_tx_;
int command_seq_;
const PacketPipeline *pipeline_;
std::string serial_, firmware_;
Freenect2Device::IrCameraParams ir_camera_params_;
Freenect2Device::ColorCameraParams rgb_camera_params_;
public:
Freenect2DeviceImpl(Freenect2Impl *context, const PacketPipeline *pipeline, libusb_device *usb_device, libusb_device_handle *usb_device_handle, const std::string &serial);
virtual ~Freenect2DeviceImpl();
bool isSameUsbDevice(libusb_device* other);
virtual std::string getSerialNumber();
virtual std::string getFirmwareVersion();
virtual Freenect2Device::ColorCameraParams getColorCameraParams();
virtual Freenect2Device::IrCameraParams getIrCameraParams();
virtual void setColorCameraParams(const Freenect2Device::ColorCameraParams ¶ms);
virtual void setIrCameraParams(const Freenect2Device::IrCameraParams ¶ms);
virtual void setConfiguration(const Freenect2Device::Config &config);
int nextCommandSeq();
bool open();
virtual void setColorFrameListener(libfreenect2::FrameListener* rgb_frame_listener);
virtual void setIrAndDepthFrameListener(libfreenect2::FrameListener* ir_frame_listener);
virtual void start();
virtual void stop();
virtual void close();
};
struct PrintBusAndDevice
{
libusb_device *dev_;
int status_;
PrintBusAndDevice(libusb_device *dev, int status = 0) : dev_(dev), status_(status) {}
};
std::ostream &operator<<(std::ostream &out, const PrintBusAndDevice& dev)
{
out << "@" << int(libusb_get_bus_number(dev.dev_)) << ":" << int(libusb_get_device_address(dev.dev_));
if (dev.status_)
out << " " << WRITE_LIBUSB_ERROR(dev.status_);
return out;
}
/** Freenect2 device storage and control. */
class Freenect2Impl
{
private:
bool managed_usb_context_;
libusb_context *usb_context_;
EventLoop usb_event_loop_;
public:
struct UsbDeviceWithSerial
{
libusb_device *dev;
std::string serial;
};
typedef std::vector<UsbDeviceWithSerial> UsbDeviceVector;
typedef std::vector<Freenect2DeviceImpl *> DeviceVector;
bool has_device_enumeration_;
UsbDeviceVector enumerated_devices_;
DeviceVector devices_;
bool initialized;
Freenect2Impl(void *usb_context) :
managed_usb_context_(usb_context == 0),
usb_context_(reinterpret_cast<libusb_context *>(usb_context)),
initialized(false),
has_device_enumeration_(false)
{
#ifdef __linux__
if (libusb_get_version()->nano < 10952)
{
LOG_ERROR << "Your libusb does not support large iso buffer!";
return;
}
#endif
if(managed_usb_context_)
{
int r = libusb_init(&usb_context_);
if(r != 0)
{
LOG_ERROR << "failed to create usb context: " << WRITE_LIBUSB_ERROR(r);
return;
}
}
usb_event_loop_.start(usb_context_);
initialized = true;
}
~Freenect2Impl()
{
if (!initialized)
return;
clearDevices();
clearDeviceEnumeration();
usb_event_loop_.stop();
if(managed_usb_context_ && usb_context_ != 0)
{
libusb_exit(usb_context_);
usb_context_ = 0;
}
}
void addDevice(Freenect2DeviceImpl *device)
{
if (!initialized)
return;
devices_.push_back(device);
}
void removeDevice(Freenect2DeviceImpl *device)
{
if (!initialized)
return;
DeviceVector::iterator it = std::find(devices_.begin(), devices_.end(), device);
if(it != devices_.end())
{
devices_.erase(it);
}
else
{
LOG_WARNING << "tried to remove device, which is not in the internal device list!";
}
}
bool tryGetDevice(libusb_device *usb_device, Freenect2DeviceImpl **device)
{
if (!initialized)
return false;
for(DeviceVector::iterator it = devices_.begin(); it != devices_.end(); ++it)
{
if((*it)->isSameUsbDevice(usb_device))
{
*device = *it;
return true;
}
}
return false;
}
void clearDevices()
{
if (!initialized)
return;
DeviceVector devices(devices_.begin(), devices_.end());
for(DeviceVector::iterator it = devices.begin(); it != devices.end(); ++it)
{
delete (*it);
}
if(!devices_.empty())
{
LOG_WARNING << "after deleting all devices the internal device list should be empty!";
}
}
void clearDeviceEnumeration()
{
if (!initialized)
return;
// free enumerated device pointers, this should not affect opened devices
for(UsbDeviceVector::iterator it = enumerated_devices_.begin(); it != enumerated_devices_.end(); ++it)
{
libusb_unref_device(it->dev);
}
enumerated_devices_.clear();
has_device_enumeration_ = false;
}
void enumerateDevices()
{
if (!initialized)
return;
LOG_INFO << "enumerating devices...";
libusb_device **device_list;
int num_devices = libusb_get_device_list(usb_context_, &device_list);
LOG_INFO << num_devices << " usb devices connected";
if(num_devices > 0)
{
for(int idx = 0; idx < num_devices; ++idx)
{
libusb_device *dev = device_list[idx];
libusb_device_descriptor dev_desc;
int r = libusb_get_device_descriptor(dev, &dev_desc); // this is always successful
if(dev_desc.idVendor == Freenect2Device::VendorId && (dev_desc.idProduct == Freenect2Device::ProductId || dev_desc.idProduct == Freenect2Device::ProductIdPreview))
{
Freenect2DeviceImpl *freenect2_dev;
// prevent error if device is already open
if(tryGetDevice(dev, &freenect2_dev))
{
UsbDeviceWithSerial dev_with_serial;
dev_with_serial.dev = dev;
dev_with_serial.serial = freenect2_dev->getSerialNumber();
enumerated_devices_.push_back(dev_with_serial);
continue;
}
else
{
libusb_device_handle *dev_handle;
r = libusb_open(dev, &dev_handle);
if(r == LIBUSB_SUCCESS)
{
unsigned char buffer[1024];
r = libusb_get_string_descriptor_ascii(dev_handle, dev_desc.iSerialNumber, buffer, sizeof(buffer));
if(r > LIBUSB_SUCCESS)
{
UsbDeviceWithSerial dev_with_serial;
dev_with_serial.dev = dev;
dev_with_serial.serial = std::string(reinterpret_cast<char *>(buffer), size_t(r));
LOG_INFO << "found valid Kinect v2 " << PrintBusAndDevice(dev) << " with serial " << dev_with_serial.serial;
// valid Kinect v2
enumerated_devices_.push_back(dev_with_serial);
continue;
}
else
{
LOG_ERROR << "failed to get serial number of Kinect v2: " << PrintBusAndDevice(dev, r);
}
libusb_close(dev_handle);
}
else
{
LOG_ERROR << "failed to open Kinect v2: " << PrintBusAndDevice(dev, r);
}
}
}
libusb_unref_device(dev);
}
}
libusb_free_device_list(device_list, 0);
has_device_enumeration_ = true;
LOG_INFO << "found " << enumerated_devices_.size() << " devices";
}
int getNumDevices()
{
if (!initialized)
return 0;
if(!has_device_enumeration_)
{
enumerateDevices();
}
return enumerated_devices_.size();
}
Freenect2Device *openDevice(int idx, const PacketPipeline *factory, bool attempting_reset);
};
Freenect2Device::~Freenect2Device()
{
}
Freenect2DeviceImpl::Freenect2DeviceImpl(Freenect2Impl *context, const PacketPipeline *pipeline, libusb_device *usb_device, libusb_device_handle *usb_device_handle, const std::string &serial) :
state_(Created),
has_usb_interfaces_(false),
context_(context),
usb_device_(usb_device),
usb_device_handle_(usb_device_handle),
rgb_transfer_pool_(usb_device_handle, 0x83),
ir_transfer_pool_(usb_device_handle, 0x84),
usb_control_(usb_device_handle_),
command_tx_(usb_device_handle_, 0x81, 0x02),
command_seq_(0),
pipeline_(pipeline),
serial_(serial),
firmware_("<unknown>")
{
rgb_transfer_pool_.setCallback(pipeline_->getRgbPacketParser());
ir_transfer_pool_.setCallback(pipeline_->getIrPacketParser());
}
Freenect2DeviceImpl::~Freenect2DeviceImpl()
{
close();
context_->removeDevice(this);
delete pipeline_;
}
int Freenect2DeviceImpl::nextCommandSeq()
{
return command_seq_++;
}
bool Freenect2DeviceImpl::isSameUsbDevice(libusb_device* other)
{
bool result = false;
if(state_ != Closed && usb_device_ != 0)
{
unsigned char bus = libusb_get_bus_number(usb_device_);
unsigned char address = libusb_get_device_address(usb_device_);
unsigned char other_bus = libusb_get_bus_number(other);
unsigned char other_address = libusb_get_device_address(other);
result = (bus == other_bus) && (address == other_address);
}
return result;
}
std::string Freenect2DeviceImpl::getSerialNumber()
{
return serial_;
}
std::string Freenect2DeviceImpl::getFirmwareVersion()
{
return firmware_;
}
Freenect2Device::ColorCameraParams Freenect2DeviceImpl::getColorCameraParams()
{
return rgb_camera_params_;
}
Freenect2Device::IrCameraParams Freenect2DeviceImpl::getIrCameraParams()
{
return ir_camera_params_;
}
void Freenect2DeviceImpl::setColorCameraParams(const Freenect2Device::ColorCameraParams ¶ms)
{
rgb_camera_params_ = params;
}
void Freenect2DeviceImpl::setIrCameraParams(const Freenect2Device::IrCameraParams ¶ms)
{
ir_camera_params_ = params;
DepthPacketProcessor *proc = pipeline_->getDepthPacketProcessor();
if (proc != 0)
{
IrCameraTables tables(params);
proc->loadXZTables(&tables.xtable[0], &tables.ztable[0]);
proc->loadLookupTable(&tables.lut[0]);
}
}
Freenect2Device::Config::Config() :
MinDepth(0.5f),
MaxDepth(4.5f),
EnableBilateralFilter(true),
EnableEdgeAwareFilter(true) {}
void Freenect2DeviceImpl::setConfiguration(const Freenect2Device::Config &config)
{
DepthPacketProcessor *proc = pipeline_->getDepthPacketProcessor();
if (proc != 0)
proc->setConfiguration(config);
}
void Freenect2DeviceImpl::setColorFrameListener(libfreenect2::FrameListener* rgb_frame_listener)
{
// TODO: should only be possible, if not started
if(pipeline_->getRgbPacketProcessor() != 0)
pipeline_->getRgbPacketProcessor()->setFrameListener(rgb_frame_listener);
}
void Freenect2DeviceImpl::setIrAndDepthFrameListener(libfreenect2::FrameListener* ir_frame_listener)
{
// TODO: should only be possible, if not started
if(pipeline_->getDepthPacketProcessor() != 0)
pipeline_->getDepthPacketProcessor()->setFrameListener(ir_frame_listener);
}
bool Freenect2DeviceImpl::open()
{
LOG_INFO << "opening...";
if(state_ != Created) return false;
if(usb_control_.setConfiguration() != UsbControl::Success) return false;
if(!has_usb_interfaces_ && usb_control_.claimInterfaces() != UsbControl::Success) return false;
has_usb_interfaces_ = true;
if(usb_control_.setIsochronousDelay() != UsbControl::Success) return false;
// TODO: always fails right now with error 6 - TRANSFER_OVERFLOW!
//if(usb_control_.setPowerStateLatencies() != UsbControl::Success) return false;
if(usb_control_.setIrInterfaceState(UsbControl::Disabled) != UsbControl::Success) return false;
if(usb_control_.enablePowerStates() != UsbControl::Success) return false;
if(usb_control_.setVideoTransferFunctionState(UsbControl::Disabled) != UsbControl::Success) return false;
int max_iso_packet_size;
if(usb_control_.getIrMaxIsoPacketSize(max_iso_packet_size) != UsbControl::Success) return false;
if(max_iso_packet_size < 0x8400)
{
LOG_ERROR << "max iso packet size for endpoint 0x84 too small! (expected: " << 0x8400 << " got: " << max_iso_packet_size << ")";
return false;
}
rgb_transfer_pool_.allocate(20, 0x4000);
ir_transfer_pool_.allocate(NUM_XFERS, PKTS_PER_XFER, max_iso_packet_size);
state_ = Open;
LOG_INFO << "opened";
return true;
}
void Freenect2DeviceImpl::start()
{
LOG_INFO << "starting...";
if(state_ != Open) return;
CommandTransaction::Result serial_result, firmware_result, result;
usb_control_.setVideoTransferFunctionState(UsbControl::Enabled);
command_tx_.execute(ReadFirmwareVersionsCommand(nextCommandSeq()), firmware_result);
firmware_ = FirmwareVersionResponse(firmware_result.data, firmware_result.length).toString();
command_tx_.execute(ReadData0x14Command(nextCommandSeq()), result);
LOG_DEBUG << "ReadData0x14 response";
LOG_DEBUG << GenericResponse(result.data, result.length).toString();
command_tx_.execute(ReadSerialNumberCommand(nextCommandSeq()), serial_result);
std::string new_serial = SerialNumberResponse(serial_result.data, serial_result.length).toString();
if(serial_ != new_serial)
{
LOG_WARNING << "serial number reported by libusb " << serial_ << " differs from serial number " << new_serial << " in device protocol! ";
}
command_tx_.execute(ReadDepthCameraParametersCommand(nextCommandSeq()), result);
DepthCameraParamsResponse *ir_p = reinterpret_cast<DepthCameraParamsResponse *>(result.data);
IrCameraParams ir_camera_params_;
ir_camera_params_.fx = ir_p->fx;
ir_camera_params_.fy = ir_p->fy;
ir_camera_params_.cx = ir_p->cx;
ir_camera_params_.cy = ir_p->cy;
ir_camera_params_.k1 = ir_p->k1;
ir_camera_params_.k2 = ir_p->k2;
ir_camera_params_.k3 = ir_p->k3;
ir_camera_params_.p1 = ir_p->p1;
ir_camera_params_.p2 = ir_p->p2;
setIrCameraParams(ir_camera_params_);
command_tx_.execute(ReadP0TablesCommand(nextCommandSeq()), result);
if(pipeline_->getDepthPacketProcessor() != 0)
pipeline_->getDepthPacketProcessor()->loadP0TablesFromCommandResponse(result.data, result.length);
command_tx_.execute(ReadRgbCameraParametersCommand(nextCommandSeq()), result);
RgbCameraParamsResponse *rgb_p = reinterpret_cast<RgbCameraParamsResponse *>(result.data);
ColorCameraParams rgb_camera_params_;
rgb_camera_params_.fx = rgb_p->color_f;
rgb_camera_params_.fy = rgb_p->color_f;
rgb_camera_params_.cx = rgb_p->color_cx;
rgb_camera_params_.cy = rgb_p->color_cy;
rgb_camera_params_.shift_d = rgb_p->shift_d;
rgb_camera_params_.shift_m = rgb_p->shift_m;
rgb_camera_params_.mx_x3y0 = rgb_p->mx_x3y0; // xxx
rgb_camera_params_.mx_x0y3 = rgb_p->mx_x0y3; // yyy
rgb_camera_params_.mx_x2y1 = rgb_p->mx_x2y1; // xxy
rgb_camera_params_.mx_x1y2 = rgb_p->mx_x1y2; // yyx
rgb_camera_params_.mx_x2y0 = rgb_p->mx_x2y0; // xx
rgb_camera_params_.mx_x0y2 = rgb_p->mx_x0y2; // yy
rgb_camera_params_.mx_x1y1 = rgb_p->mx_x1y1; // xy
rgb_camera_params_.mx_x1y0 = rgb_p->mx_x1y0; // x
rgb_camera_params_.mx_x0y1 = rgb_p->mx_x0y1; // y
rgb_camera_params_.mx_x0y0 = rgb_p->mx_x0y0; // 1
rgb_camera_params_.my_x3y0 = rgb_p->my_x3y0; // xxx
rgb_camera_params_.my_x0y3 = rgb_p->my_x0y3; // yyy
rgb_camera_params_.my_x2y1 = rgb_p->my_x2y1; // xxy
rgb_camera_params_.my_x1y2 = rgb_p->my_x1y2; // yyx
rgb_camera_params_.my_x2y0 = rgb_p->my_x2y0; // xx
rgb_camera_params_.my_x0y2 = rgb_p->my_x0y2; // yy
rgb_camera_params_.my_x1y1 = rgb_p->my_x1y1; // xy
rgb_camera_params_.my_x1y0 = rgb_p->my_x1y0; // x
rgb_camera_params_.my_x0y1 = rgb_p->my_x0y1; // y
rgb_camera_params_.my_x0y0 = rgb_p->my_x0y0; // 1
setColorCameraParams(rgb_camera_params_);
command_tx_.execute(ReadStatus0x090000Command(nextCommandSeq()), result);
LOG_DEBUG << "ReadStatus0x090000 response";
LOG_DEBUG << GenericResponse(result.data, result.length).toString();
command_tx_.execute(InitStreamsCommand(nextCommandSeq()), result);
usb_control_.setIrInterfaceState(UsbControl::Enabled);
command_tx_.execute(ReadStatus0x090000Command(nextCommandSeq()), result);
LOG_DEBUG << "ReadStatus0x090000 response";
LOG_DEBUG << GenericResponse(result.data, result.length).toString();
command_tx_.execute(SetStreamEnabledCommand(nextCommandSeq()), result);
//command_tx_.execute(Unknown0x47Command(nextCommandSeq()), result);
//command_tx_.execute(Unknown0x46Command(nextCommandSeq()), result);
/*
command_tx_.execute(SetModeEnabledCommand(nextCommandSeq()), result);
command_tx_.execute(SetModeDisabledCommand(nextCommandSeq()), result);
usb_control_.setIrInterfaceState(UsbControl::Enabled);
command_tx_.execute(SetModeEnabledWith0x00640064Command(nextCommandSeq()), result);
command_tx_.execute(ReadData0x26Command(nextCommandSeq()), result);
command_tx_.execute(ReadStatus0x100007Command(nextCommandSeq()), result);
command_tx_.execute(SetModeEnabledWith0x00500050Command(nextCommandSeq()), result);
command_tx_.execute(ReadData0x26Command(nextCommandSeq()), result);
command_tx_.execute(ReadStatus0x100007Command(nextCommandSeq()), result);
command_tx_.execute(ReadData0x26Command(nextCommandSeq()), result);
command_tx_.execute(ReadData0x26Command(nextCommandSeq()), result);
*/
LOG_INFO << "enabling usb transfer submission...";
rgb_transfer_pool_.enableSubmission();
ir_transfer_pool_.enableSubmission();
LOG_INFO << "submitting usb transfers...";
rgb_transfer_pool_.submit(20);
ir_transfer_pool_.submit(NUM_XFERS);
state_ = Streaming;
LOG_INFO << "started";
}
void Freenect2DeviceImpl::stop()
{
LOG_INFO << "stopping...";
if(state_ != Streaming)
{
LOG_INFO << "already stopped, doing nothing";
return;
}
LOG_INFO << "disabling usb transfer submission...";
rgb_transfer_pool_.disableSubmission();
ir_transfer_pool_.disableSubmission();
LOG_INFO << "canceling usb transfers...";
rgb_transfer_pool_.cancel();
ir_transfer_pool_.cancel();
usb_control_.setIrInterfaceState(UsbControl::Disabled);
CommandTransaction::Result result;
command_tx_.execute(Unknown0x0ACommand(nextCommandSeq()), result);
command_tx_.execute(SetStreamDisabledCommand(nextCommandSeq()), result);
usb_control_.setVideoTransferFunctionState(UsbControl::Disabled);
state_ = Open;
LOG_INFO << "stopped";
}
void Freenect2DeviceImpl::close()
{
LOG_INFO << "closing...";
if(state_ == Closed)
{
LOG_INFO << "already closed, doing nothing";
return;
}
if(state_ == Streaming)
{
stop();
}
if(pipeline_->getRgbPacketProcessor() != 0)
pipeline_->getRgbPacketProcessor()->setFrameListener(0);
if(pipeline_->getDepthPacketProcessor() != 0)
pipeline_->getDepthPacketProcessor()->setFrameListener(0);
if(has_usb_interfaces_)
{
LOG_INFO << "releasing usb interfaces...";
usb_control_.releaseInterfaces();
has_usb_interfaces_ = false;
}
LOG_INFO << "deallocating usb transfer pools...";
rgb_transfer_pool_.deallocate();
ir_transfer_pool_.deallocate();
LOG_INFO << "closing usb device...";
libusb_close(usb_device_handle_);
usb_device_handle_ = 0;
usb_device_ = 0;
state_ = Closed;
LOG_INFO << "closed";
}
PacketPipeline *createDefaultPacketPipeline()
{
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
return new OpenGLPacketPipeline();
#else
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
return new OpenCLPacketPipeline();
#else
return new CpuPacketPipeline();
#endif
#endif
}
Freenect2::Freenect2(void *usb_context) :
impl_(new Freenect2Impl(usb_context))
{
}
Freenect2::~Freenect2()
{
delete impl_;
}
int Freenect2::enumerateDevices()
{
impl_->clearDeviceEnumeration();
return impl_->getNumDevices();
}
std::string Freenect2::getDeviceSerialNumber(int idx)
{
if (!impl_->initialized)
return std::string();
if (idx >= impl_->getNumDevices() || idx < 0)
return std::string();
return impl_->enumerated_devices_[idx].serial;
}
std::string Freenect2::getDefaultDeviceSerialNumber()
{
return getDeviceSerialNumber(0);
}
Freenect2Device *Freenect2::openDevice(int idx)
{
return openDevice(idx, createDefaultPacketPipeline());
}
Freenect2Device *Freenect2::openDevice(int idx, const PacketPipeline *pipeline)
{
return impl_->openDevice(idx, pipeline, true);
}
Freenect2Device *Freenect2Impl::openDevice(int idx, const PacketPipeline *pipeline, bool attempting_reset)
{
int num_devices = getNumDevices();
Freenect2DeviceImpl *device = 0;
if(idx >= num_devices)
{
LOG_ERROR << "requested device " << idx << " is not connected!";
delete pipeline;
return device;
}
Freenect2Impl::UsbDeviceWithSerial &dev = enumerated_devices_[idx];
libusb_device_handle *dev_handle;
if(tryGetDevice(dev.dev, &device))
{
LOG_WARNING << "device " << PrintBusAndDevice(dev.dev)
<< " is already be open!";
delete pipeline;
return device;
}
int r = libusb_open(dev.dev, &dev_handle);
if(r != LIBUSB_SUCCESS)
{
LOG_ERROR << "failed to open Kinect v2: " << PrintBusAndDevice(dev.dev, r);
delete pipeline;
return device;
}
if(attempting_reset)
{
r = libusb_reset_device(dev_handle);
if(r == LIBUSB_ERROR_NOT_FOUND)
{
// From libusb documentation:
// "If the reset fails, the descriptors change, or the previous state
// cannot be restored, the device will appear to be disconnected and
// reconnected. This means that the device handle is no longer valid (you
// should close it) and rediscover the device. A return code of
// LIBUSB_ERROR_NOT_FOUND indicates when this is the case."
// be a good citizen
libusb_close(dev_handle);
// HACK: wait for the planets to align... (When the reset fails it may
// take a short while for the device to show up on the bus again. In the
// absence of hotplug support, we just wait a little. If this code path
// is followed there will already be a delay opening the device fully so
// adding a little more is tolerable.)
libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(1000));
// reenumerate devices
LOG_INFO << "re-enumerating devices after reset";
clearDeviceEnumeration();
enumerateDevices();
// re-open without reset
return openDevice(idx, pipeline, false);
}
else if(r != LIBUSB_SUCCESS)
{
LOG_ERROR << "failed to reset Kinect v2: " << PrintBusAndDevice(dev.dev, r);
delete pipeline;
return device;
}
}
device = new Freenect2DeviceImpl(this, pipeline, dev.dev, dev_handle, dev.serial);
addDevice(device);
if(!device->open())