├── .gitignore ├── .gitmodules ├── LICENSE_INTEL ├── README.md ├── dclassify ├── README.md ├── dclassify.cpp ├── dclassify.s2e ├── dclassify.vcxproj ├── load_trained_cnn.vcxproj ├── science_rulz.sb2 ├── scratchtest.sb2 └── simple_load_trained_cnn.cpp ├── dpca ├── BreakoutGame.html ├── dpca.cpp └── dpca.vcxproj ├── dphyspush ├── dphyspush.cpp └── dphyspush.vcxproj ├── dsamples_vs2013.sln ├── dsamples_vs2015.sln └── include ├── dcam.h ├── gl_gui.h └── misc_image.h /.gitignore: -------------------------------------------------------------------------------- 1 | Debug/ 2 | */*_Debug*.exe 3 | */*_Debug*.ilk 4 | */*_Debug*.pdb 5 | 6 | Release/ 7 | */*_Release*.exe 8 | */*_Release*.ilk 9 | */*_Release*.pdb 10 | 11 | /ipch/ 12 | /packages/ 13 | /*.opensdf 14 | /*.sdf 15 | /*.suo 16 | obj/* 17 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/sandbox"] 2 | path = third_party/sandbox 3 | url = https://github.com/melax/sandbox.git 4 | [submodule "third_party/librealsense"] 5 | path = third_party/librealsense 6 | url = https://github.com/IntelRealSense/librealsense.git 7 | -------------------------------------------------------------------------------- /LICENSE_INTEL: -------------------------------------------------------------------------------- 1 | Code Samples License Agreement (Version December 2015) 2 | 3 | IMPORTANT - READ BEFORE COPYING, INSTALLING OR USING. Do not copy, install or 4 | use the Materials (as defined below) provided under this license agreement 5 | ("Agreement") from Intel Corporation ("Intel"), until you ("You") have carefully 6 | read the following terms and conditions. By copying, installing or otherwise 7 | using the Materials, You agree to be bound by the terms of this Agreement. If 8 | You do not agree to the terms of this Agreement, do not copy, install or use the 9 | Materials. 10 | 11 | If You are agreeing to the terms and conditions of this Agreement on behalf of a 12 | company or other legal entity ("Legal Entity"), You represent and warrant that 13 | You have the legal authority to bind that Legal Entity to the Agreement, in 14 | which case, "You" or "Your" will mean such Legal Entity. 15 | 16 | By agreeing to this Agreement, You affirm that You are of legal age (18 years 17 | old or older) to enter into this Agreement. If You are not of legal age You may 18 | not enter into this Agreement, and either Your parent, legal guardian or Legal 19 | Entity must agree to the terms and conditions of this Agreement and enter into 20 | this Agreement, in which case, "You" or "Your" will mean such parent, legal 21 | guardian, or Legal Entity. 22 | 23 | Third Party Programs (as defined below), even if included with the distribution 24 | of the Materials, are governed by separate third party license terms, including 25 | without limitation, open source software license terms. Such third party license 26 | terms (and not this Agreement) govern Your use of the Third Party Programs, and 27 | Intel is not liable for the Third Party Programs. 28 | 29 | 1. LICENSE DEFINITIONS: 30 | 31 | "Licensed Patent Claims" means the claims of Intel's patents that are 32 | necessarily and directly infringed by the reproduction and distribution of the 33 | Materials that is authorized in Section 2 below, when the Materials are in its 34 | unmodified form as delivered by Intel to You and not modified or combined with 35 | anything else. Licensed Patent Claims are only those claims that Intel can 36 | license without paying, or getting the consent of, a third party. 37 | 38 | "Materials" means Sample Source Code, Redistributables, and End-User 39 | Documentation but do not include Third Party Programs. 40 | 41 | "Sample Source Code" means Source Code files that are identified as sample code 42 | and which may include example interface or application source code, and any 43 | updates, provided under this Agreement. 44 | 45 | "Source Code" is defined as the software (and not documentation or text) portion 46 | of the Materials provided in human readable format, and includes modifications 47 | that You make or are made on Your behalf as expressly permitted under the terms 48 | of this Agreement. 49 | 50 | "Redistributables" means header, library, and dynamically linkable library 51 | files, and any updates, provided under this Agreement. 52 | 53 | "Third Party Programs" (if any) are the third party software files that may be 54 | included with the Materials for the applicable software that include a separate 55 | third party license agreement in an attached text file. 56 | 57 | "End-User Documentation" means textual materials intended for end users relating 58 | to the Materials. 59 | 60 | 2. LICENSE GRANT: 61 | 62 | Subject to the terms and conditions of this Agreement, Intel grants You a 63 | non-exclusive, worldwide, non-assignable, royalty-free limited right and 64 | license: 65 | A. under its copyrights, to: 66 | 1) Copy, modify, and compile the Sample Source Code and distribute it solely 67 | in Your products in executable and source code form; 68 | 2) Copy and distribute the Redistributables solely with Your products; 69 | 3) Copy, modify, and distribute the End User Documentation solely with Your 70 | products. 71 | 72 | B. Under its patents, to: 73 | 1) make copies of the Materials internally only; 74 | 2) use the Materials internally only; and 3) offer to distribute, and 75 | distribute, but not sell, the Materials only as part of or with Your 76 | products, under Intel's copyright license granted in Section 2(A) but only 77 | under the terms of that copyright license and not as a sale (but this 78 | right does not include the right to sub-license); 79 | 4) provided, further, that the license under the Licensed Patent Claims does 80 | not and will not apply to any modifications to, or derivative works of, 81 | the Materials, whether made by You, Your end user (which, for all purposes 82 | under this Agreement, will mean either an end user, a customer, reseller, 83 | distributor or other channel partner), or any third party even if the 84 | modification and creation of derivative works are permitted under 2(A). 85 | 86 | 3. LICENSE RESTRICTIONS: 87 | 88 | Except as expressly provided in this Agreement, You may not: 89 | i. use, copy, distribute or publicly display the Materials; 90 | ii. reverse-assemble, reverse-compile, or otherwise reverse-engineer any 91 | software provided solely in binary form, 92 | iii. rent or lease the Materials to any third party; 93 | iv. assign this Agreement or display the Materials; 94 | v. assign this Agreement or transfer the Materials; 95 | vi. modify, adapt or translate the Materials in whole or in part; 96 | vii. distribute, sublicense or transfer the source code form of the Materials 97 | or derivatives thereof to any third party; 98 | viii. distribute the Materials except as part of Your products; 99 | ix. include the Materials in malicious, deceptive, or unlawful programs or 100 | products; 101 | x. modify, create a derivative work, link or distribute the Materials so that 102 | any part of it becomes subject to an Excluded License. 103 | 104 | Upon Intel's release of an update, upgrade, or new version of the Materials, you 105 | will make reasonable efforts to discontinue distribution of the enclosed 106 | Materials and you will make reasonable efforts to distribute such updates, 107 | upgrades, or new versions to your customers who have received the Materials 108 | herein. 109 | 110 | Distribution of the Materials is also subject to the following limitations. 111 | You: 112 | i. will be solely responsible to your customers for any update or support 113 | obligation or other liability which may arise from the distribution; 114 | ii. will not make any statement that your product is "certified", or that its 115 | performance is guaranteed, by Intel; 116 | iii. will not use Intel's name or trademarks to market your product without 117 | written permission; 118 | iv. will prohibit disassembly and reverse engineering of the Materials 119 | provided in executable form; 120 | v. will not publish reviews of Materials without written permission by Intel, 121 | and 122 | vi. will indemnify, hold harmless, and defend Intel and its suppliers from and 123 | against any claims or lawsuits, including attorney's fees, that arise or 124 | result from your distribution of any product. 125 | 126 | 4. OWNERSHIP: 127 | 128 | Title to the Materials and all copies thereof remain with Intel or its 129 | suppliers. The Materials are copyrighted and are protected by United States 130 | copyright laws and international treaty provisions. You will not remove any 131 | copyright notice from the Materials. You agree to prevent unauthorized copying 132 | of the Materials. Except as expressly provided herein, Intel does not grant any 133 | express or implied right to you under Intel patents, copyrights, trademarks, or 134 | trade secret information. 135 | 136 | 5. NO WARRANTY AND NO SUPPORT: 137 | 138 | Disclaimer. Intel disclaims all warranties of any kind and the terms and 139 | remedies provided in this Agreement are instead of any other warranty or 140 | condition, express, implied or statutory, including those regarding 141 | merchantability, fitness for any particular purpose, non-infringement or any 142 | warranty arising out of any course of dealing, usage of trade, proposal, 143 | specification or sample. Intel does not assume (and does not authorize any 144 | person to assume on its behalf) any other liability. 145 | 146 | Intel may make changes to the Materials, or to items referenced therein, at any 147 | time without notice, but is not obligated to support, update or provide training 148 | for the Materials. Intel may in its sole discretion offer such support, update 149 | or training services under separate terms at Intel's then-current rates. You 150 | may request additional information on Intel's service offerings from an Intel 151 | sales representative. 152 | 153 | 6. USER SUBMISSIONS: 154 | 155 | You agree that any material, information, or other communication, including all 156 | data, images, sounds, text, and other things embodied therein, you transmit or 157 | post to an Intel website will be considered non-confidential ("Communications"). 158 | Intel will have no confidentiality obligations with respect to the 159 | Communications. You agree that Intel and its designees will be free to copy, 160 | modify, create derivative works, publicly display, disclose, distribute, license 161 | and sublicense through multiple tiers of distribution and licensees, 162 | incorporate, and otherwise use the Communications, including derivative works 163 | thereto, for any and all commercial or non-commercial purposes. 164 | 165 | 7.LIMITATION OF LIABILITY: 166 | 167 | Neither Intel nor its suppliers shall be liable for any damages whatsoever 168 | (including, without limitation, damages for loss of business profits, business 169 | interruption, loss of business information, or other loss) arising out of the 170 | use of or inability to use the Materials, even if Intel has been advised of the 171 | possibility of such damages. Because some jurisdictions prohibit the exclusion 172 | or limitation of liability for consequential or incidental damages, the above 173 | limitation may not apply to You. 174 | 175 | 8. TERM AND TERMINATION: 176 | 177 | This Agreement commences upon Your copying, installing or using the Materials 178 | and continues until terminated. Either You or Intel may terminate this 179 | Agreement at any time upon 30 days prior written notice to the other party. 180 | Intel may terminate this license at any time if you are in breach of any of its 181 | terms and conditions. Upon termination, You will immediately destroy the 182 | Materials or return all copies of the Materials to Intel along with any copies 183 | You have made. After termination, the license grant to any Materials or 184 | Redistributables distributed by You in accordance with the terms and conditions 185 | of this Agreement, prior to the effective date of such termination, will survive 186 | any such termination of this Agreement. 187 | 188 | 9. U.S. GOVERNMENT RESTRICTED RIGHTS: 189 | 190 | The technical data and computer software covered by this license is a 191 | "Commercial Item", as such term is defined by the FAR 2.101 (48 C.F.R. 2.101) 192 | and is "commercial computer software" and "commercial computer software 193 | documentation" as specified under FAR 12.212 (48 C.F.R. 12.212) or DFARS 194 | 227.7202 (48 C.F.R. 227.7202), as applicable. This commercial computer software 195 | and related documentation is provided to end users for use by and on behalf of 196 | the U.S. Government, with only those rights as are granted to all other end 197 | users pursuant to the terms and conditions herein. Use for or on behalf of the 198 | U.S. Government is permitted only if the party acquiring or using this software 199 | is properly authorized by an appropriate U.S. Government official. This use by 200 | or for the U.S. Government clause is in lieu of, and supersedes, any other FAR, 201 | DFARS, or other provision that addresses Government rights in the computer 202 | software or documentation covered by this license. All copyright licenses 203 | granted to the U.S. Government are coextensive with the technical data and 204 | computer software licenses granted herein. The U.S. Government will only have 205 | the right to reproduce, distribute, perform, display, and prepare derivative 206 | works as needed to implement those rights. 207 | 208 | 10. APPLICABLE LAWS: 209 | 210 | All disputes arising out of or related to this Agreement, whether based on 211 | contract, tort, or any other legal or equitable theory, will in all respects be 212 | governed by, and construed and interpreted under, the laws of the United States 213 | of America and the State of Delaware, without reference to conflict of laws 214 | principles. The parties agree that the United Nations Convention on Contracts 215 | for the International Sale of Goods (1980) is specifically excluded from and 216 | will not apply to this Agreement. All disputes arising out of or related to this 217 | Agreement, whether based on contract, tort, or any other legal or equitable 218 | theory, will be subject to the exclusive jurisdiction of the courts of the State 219 | of Delaware or of the Federal courts sitting in that State. Each party submits 220 | to the personal jurisdiction of those courts and waives all objections to that 221 | jurisdiction and venue for those disputes. 222 | 223 | 10. SEVERABILITY: 224 | 225 | The parties intend that if a court holds that any provision or part of this 226 | Agreement is invalid or unenforceable under applicable law, the court will 227 | modify the provision to the minimum extent necessary to make it valid and 228 | enforceable, or if it cannot be made valid and enforceable, the parties intend 229 | that the court will sever and delete the provision or part from this Agreement. 230 | Any change to or deletion of a provision or part of this Agreement under this 231 | Section will not affect the validity or enforceability of the remainder of this 232 | Agreement, which will continue in full force and effect. 233 | 234 | 11. EXPORT: 235 | 236 | You must comply with all laws and regulations of the United States and other 237 | countries governing the export, re-export, import, transfer, distribution, use, 238 | and servicing of Software. In particular, You must not: (a) sell or transfer 239 | Software to a country subject to sanctions, or to any entity listed on a denial 240 | order published by the United States government or any other relevant 241 | government; or (b) use, sell, or transfer Software for the development, design, 242 | manufacture, or production of nuclear, missile, chemical or biological weapons, 243 | or for any other purpose prohibited by the United States government or other 244 | applicable government; without first obtaining all authorizations required by 245 | all applicable laws. For more details on Your export obligations, please visit 246 | http://www.intel.com/content/www/us/en/legal/export-compliance.html?wapkw=export. 247 | 248 | 12. ENTIRE AGREEMENT: 249 | 250 | This Agreement contains the complete and exclusive agreement and understanding 251 | between the parties concerning the subject matter of this Agreement, and 252 | supersedes all prior and contemporaneous proposals, agreements, understanding, 253 | negotiations, representations, warranties, conditions, and communications, oral 254 | or written, between the parties relating to the same subject matter. No 255 | modification or amendment to this Agreement will be effective unless in writing 256 | and signed by authorized representatives of each party, and must specifically 257 | identify this Agreement. 258 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | ## DSamples 3 | Samples that use depth camera data. 4 | 5 | | Sample | Description | 6 | | ------------- | -------------- | 7 | | dpca | point cloud analysis methods that provide useful and fairly consistent input for a number of use cases | 8 | | dclassify | simple CNN based machine learning app with on-the-fly data collection, labelling, and training. | 9 | | dphyspush | shows direct manipulation of physically simulated rigidbodies using point cloud data | 10 | 11 | #### Release Info 12 | | Software Version | Actual State | 13 | | ----------------- | ------------- | 14 | | sqrt(-0.01) | code should run and be useful, but this is just an "imaginary unofficial test release" of code samples that are in the process of being made available in an alpha state. | 15 | 16 | In other words, put here on github for early testing just to make sure project files, submodules, and all source files are included and configured correctly. 17 | The point cloud or principal component analysis is a librealsense reimplementation of one of the samples from a IDF2013 tutorial session about depth cameras. 18 | Feel free to submit any feedback or suggestions via github issues. 19 | 20 | ### Details 21 | - samples use librealsense api/library to get depth and ir data from realsense camera. 22 | - samples developed and tested on r200 (aka ds4) realsense camera. f200 and sr300 should work, let me know if not. 23 | - samples only tested under windows OS. the opengl-window initialization wrapper is windows specific at the moment. 24 | - requires visual studio 2015. A 2013 solution file is provided but it isn't regularly tested after each code commit, and thus might require minor fixes. 25 | 26 | #### git submodules 27 | This repo depends on librealsense and sandbox repos on github. 28 | So if you are using tortoise git, be sure the recursive checkbox is checked to automatically fetch these dependencies. 29 | Alternatively, if you prefer to use the command line, then open a git bash shell and enter commands: 30 | ```sh 31 | $ git submodule init 32 | $ git submodule update 33 | ``` 34 | With the repo and submodules cloned, open up the solution ( dsamples_vs2015.sln ) and everything should sucessfully build and run (assuming a depth camera is plugged in) on the first try. 35 | 36 | 37 | -------------------------------------------------------------------------------- /dclassify/README.md: -------------------------------------------------------------------------------- 1 | # dclassify 2 | cnn classification with depth and ir data 3 | 4 | Yo, when you first clone from github, be sure to click the 'recursive' box if using tortoisegit so it will automatically grab librealsense into the third_party subfolder. 5 | 6 | If you already cloned without getting the submodule, you can either try again, or just fetch the submodule after the fact by going to the git bash commandline and entering: 7 | * git submodule init 8 | * git submodule update 9 | 10 | Note that the solution and project files are for visual studio 2015 11 | 12 | A video showing how to use the app can be seen at: 13 | https://www.youtube.com/watch?v=0umLZPGkbt4 14 | 15 | You can save trained CNNs, either without (ctrl-s) or with (ctrl-t) the training data. 16 | Look for generated .derp files exported in the same directory. 17 | Load a previously trained CNN on program startup by dragging trained_netXX.derp file onto the .exe 18 | or specify file as first parameter argv[1] when running from the commandline. 19 | 20 | If you dont want the GUI, an additional project, load_trained_cnn, also shows how to just load the CNN and use it in code. 21 | With just ascii output, this latter project is only meant to be minimal reference code. 22 | 23 | See source code (its pretty short) for some additional information and instructions. 24 | 25 | The program also can act as a http web server to allow it to work with a browser or even 26 | the scratch offline editor: https://scratch.mit.edu/scratch2download/ 27 | There's a button in dclassify's gui to turn on the http server. 28 | To use this in Scratch, Hold SHIFT while you left click to select the FILE drop down menu so the additional options show up. 29 | Select "Import experimental HTTP extension", then browse to the dclassify subfolder and open the extension file: DClassify.s2e 30 | The extension will show up in the "More Blocks" section. 31 | 32 | ## FAQ 33 | 34 | 35 | > How do I know when the training is completed? 36 | 37 | You can usually see the error rate go down. 38 | Note that the program doesn't separate a test test from a training set, so its only the MSE error (average) on the most recent iterations. 39 | You can just let it continue to train while you are using it to test your results by classifying the live input from camera. 40 | If the CNN error rate is low and not gettting any better, then you can probably shut off the training. 41 | 42 | 43 | > What's the scoop on the slider widget for training? 44 | 45 | That sets the number of backprop (training) iterations to do per frame. this number increases exponentially. 46 | So increase this, but not too much - no reason to push it beyond the point that it slows down your framerate. 47 | Be sure to compile/run in release mode. Debug mode for this program is quite slow for actual usage. 48 | 49 | 50 | > Can I add samples during training or should I stop the training, add samples and restart the training? 51 | 52 | No need to stop the training, just add samples as necessary - especially if you see the CNN output is not what it should be. 53 | Just mouseclick (or corresponding keypress) on the correct category while the live feed is on the object/thing you want to be classified. 54 | 55 | 56 | > My trained CNN continues to output one of my categories/labels even when there's none of my objects in front of the camera. 57 | 58 | Add a category for 'everything else'. pick a free category slot and collect samples while pointing the 59 | camera at background and other random non-objects that you want to rule out. 60 | 61 | 62 | > What about RGB? 63 | 64 | The CNN only uses ir and depth inputs. 65 | One thing that is nice about this is that both the depth and the IR lighting are fairly consistent. 66 | This means that a trained net should continue to work as the ambient lighting in your environment changes (time of day, lights turned on/off etc). 67 | So its not necessary to collect input samples under every possible lighting condition. 68 | That said, color could be easily added - just more input channels to the first convolution layer. 69 | 70 | 71 | > When/why would I change the size of the CNN? 72 | 73 | This can only be changed not only before any training has happened, but also before any samples are collected. 74 | The default smallest CNN only uses 16x16 input pixels, subsampling from the input area as necessary. 75 | This makes for fast training which may be sufficient in many cases. 76 | In some situations, you may really need to have a bigger net and use more (perhaps 32x32 or more) input image. 77 | Note that this will typically require more input samples to train properly. 78 | 79 | 80 | > How best to set the samping area and depth range? 81 | 82 | In case anyone missed it, you can change the size of the cropped subwindow that is used as input to the CNN. 83 | Use the left mouse in either the depth or ir view of the data to change this value. 84 | Note this doesn't change the size of the input to the CNN. 85 | This subwindow will be subsampled corresponding to the CNN input size. 86 | Pick a reasonable size that fits around the objects you want to classify. 87 | Also set a depth range that extends as much as needed and crops things beyond a distance you dont care about. 88 | 89 | 90 | > What about changing these values after samples have been collected? 91 | 92 | The CNN doesn't know the scale, absolute depth, or subsampling rate. 93 | It essentially is getting the grey pixels shown in the IR and Depth subsample subwindows. 94 | You'll have to provide enough samples to ensure coverage for all the situations that you want to train for. 95 | 96 | 97 | > Importance of data preprocessing? 98 | 99 | Not really. The header file for the depth camera does currently do a tiny bit of preprocessing on the depth data. 100 | It was just a small pass to remove "flying pixels". Probably doesn't have a big impact on this app. 101 | Probably not necessary since we see time and time again that machine learning systems 102 | deal with noisy data better than well intentioned (but actually information removing) cv code. 103 | 104 | 105 | -------------------------------------------------------------------------------- /dclassify/dclassify.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include // std::tolower 4 | #include 5 | #include // std::max 6 | #include // rdtsc 7 | #include "dcam.h" 8 | #include "misc_image.h" 9 | #include "misc.h" // for freefilename() 10 | #include "geometric.h" 11 | #include "glwin.h" 12 | #include "misc_gl.h" 13 | #include "gl_gui.h" 14 | #include "cnn.h" 15 | #include "tcpsocket.h" 16 | 17 | 18 | CNN baby_cnn() 19 | { 20 | CNN cnn({}); 21 | cnn.layers.push_back(new CNN::LConv({ 16, 16, 2 }, { 5, 5, 2, 16 }, { 12, 12, 16 })); 22 | cnn.layers.push_back(new CNN::LActivation(12 * 12 * 16)); 23 | cnn.layers.push_back(new CNN::LMaxPool(int3(12, 12, 16))); 24 | cnn.layers.push_back(new CNN::LFull(6 * 6 * 16, 32)); 25 | cnn.layers.push_back(new CNN::LActivation(32)); 26 | cnn.layers.push_back(new CNN::LFull(32, 5)); 27 | cnn.layers.push_back(new CNN::LSoftMax(5)); 28 | cnn.Init(); // initializes weights 29 | return cnn; 30 | } 31 | CNN small_cnn() 32 | { 33 | CNN cnn({}); 34 | cnn.layers.push_back(new CNN::LConv({ 32, 32, 2 }, { 5, 5, 2, 16 }, { 28, 28, 16 })); 35 | cnn.layers.push_back(new CNN::LActivation(28 * 28 * 16)); 36 | cnn.layers.push_back(new CNN::LMaxPool(int3(28, 28, 16))); 37 | cnn.layers.push_back(new CNN::LMaxPool(int3(14, 14, 16))); 38 | cnn.layers.push_back(new CNN::LFull(7 * 7 * 16, 32)); 39 | cnn.layers.push_back(new CNN::LActivation(32)); 40 | cnn.layers.push_back(new CNN::LFull(32, 5)); 41 | cnn.layers.push_back(new CNN::LSoftMax(5)); 42 | cnn.Init(); 43 | return cnn; 44 | } 45 | CNN reg_cnn() // probably too big to learn quickly with small ground truth sample size 46 | { 47 | CNN cnn({}); 48 | cnn.layers.push_back(new CNN::LConv({ 64, 64, 2 }, { 5, 5, 2, 16 }, { 60, 60, 16 })); 49 | cnn.layers.push_back(new CNN::LActivation(60 * 60 * 16)); 50 | cnn.layers.push_back(new CNN::LMaxPool(int3(60, 60, 16))); 51 | cnn.layers.push_back(new CNN::LMaxPool(int3(30, 30, 16))); 52 | cnn.layers.push_back(new CNN::LConv({ 15, 15, 16 }, { 8, 8, 16, 256 }, { 8, 8, 64 })); 53 | cnn.layers.push_back(new CNN::LActivation(8 * 8 * 64)); 54 | cnn.layers.push_back(new CNN::LMaxPool(int3(8, 8, 64))); 55 | cnn.layers.push_back(new CNN::LFull(4 * 4 * 64, 64)); 56 | cnn.layers.push_back(new CNN::LActivation(64)); 57 | cnn.layers.push_back(new CNN::LFull(64, 5)); 58 | cnn.layers.push_back(new CNN::LSoftMax(5)); 59 | cnn.Init(); 60 | return cnn; 61 | } 62 | 63 | CNN(*cnntypes[])() = { baby_cnn,small_cnn,reg_cnn }; 64 | char *cnndescriptions[] = { "baby_cnn","small_cnn","reg_cnn" }; 65 | 66 | int main(int argc, char *argv[]) try 67 | { 68 | //hack(); 69 | std::cout << "creating network\n"; 70 | 71 | 72 | int cnn_type=0; 73 | auto cnn = cnntypes[cnn_type](); 74 | int2 inputsize = ((CNN::LConv*)cnn.layers[0])->indims.xy(); // depending which is created, inputsize will be either 16x16 or 32x32 75 | 76 | GLWin glwin((argc==2)? (std::string("using previously trained cnn: ")+argv[1]).c_str() : "librealsense simple CNN classifier util - alpha version", 1240, 660); 77 | 78 | RSCam dcam; 79 | //dcam.enable_filter_depth = false; 80 | dcam.Init(); 81 | float depth_scale = (dcam.dev) ? dcam.dev->get_depth_scale() : 0.001f; // to put into meters // if file assume file is mm 82 | 83 | //glwin.ViewAngle = dcam.fov().y; 84 | float viewdist = 0.75f; 85 | 86 | int skip = 10; // selection subregion will be center of input and of size 16*skip squared, with 128x128 max. will subsample as necessary to fit cnn input size 87 | float2 drange = { 0.80f,0.30f }; // range of depth we care about. note the .x > .y, this is so that closer things are more white and further things are more dark. 88 | 89 | std::default_random_engine rng; 90 | int training = false; 91 | int samplereview = 0; 92 | int currentsample = 0; 93 | bool trainstarted = false; 94 | bool sinusoidal = false; 95 | float time = 0.0f; 96 | 97 | std::vector> samples; 98 | std::vector> labels; 99 | std::vector> snapshots; 100 | 101 | Image sample_dp(inputsize); 102 | Image sample_ir(inputsize); 103 | auto sample_cl = Image({ 1,1 }, std::vector(1*1, { 127,127,127 })); 104 | std::vector sample_in; 105 | std::vector categories(5,0); 106 | float3 catcolors[] = { {0,0,1},{0,1,0},{1,0,0},{1,1,0},{1,0,1} }; 107 | std::vector> icons; 108 | for (auto c : categories) 109 | icons.push_back(sample_cl); 110 | auto addsample = [&](int c) 111 | { 112 | if (samplereview) {samplereview = 0; return; } 113 | if (icons[c].raster.size()<=1)icons[c] = sample_cl; 114 | snapshots.push_back(sample_cl); 115 | categories[c]++; 116 | samples.push_back(sample_in); 117 | labels.push_back(std::vector(categories.size(), 0.0f)); 118 | labels.back()[c] = 1.0f; 119 | }; 120 | 121 | if (argc == 2) // load everything 122 | { 123 | std::ifstream cfile(argv[1], std::ios_base::binary | std::ios_base::in); 124 | if (!cfile.is_open()) 125 | throw("unable to open file"); 126 | cfile.read((char*)&cnn_type, sizeof(cnn_type)); 127 | cfile.read((char*)&skip , sizeof(skip)); 128 | cfile.read((char*)&drange , sizeof(drange)); 129 | for (auto &icon : icons) 130 | { 131 | cfile.read((char*)&icon.cam, sizeof(icon.cam)); 132 | icon.raster.resize(icon.cam.dim().x*icon.cam.dim().y); 133 | cfile.read((char*)icon.raster.data(), icon.raster.size()*sizeof(*icon.raster.data())); 134 | } 135 | cnn = cnntypes[cnn_type](); 136 | inputsize = ((CNN::LConv*)cnn.layers[0])->indims.xy(); 137 | cnn.loadb(cfile); 138 | trainstarted = true; 139 | while (!cfile.eof()) 140 | { 141 | DCamera cam; 142 | cfile.read((char*)&cam, sizeof(cam)); 143 | Image cl(cam,std::vector(cam.dim().x*cam.dim().y)); 144 | cfile.read((char*)cl.raster.data(), cl.raster.size()*sizeof(*cl.raster.data())); 145 | std::vector sample(2*inputsize.x*inputsize.y); 146 | std::vector label(5, 0.0f); 147 | cfile.read((char*)sample.data(), sample.size()*sizeof(float)); 148 | cfile.read((char*)label.data() , label.size()*sizeof(float)); 149 | samples.push_back(sample); 150 | labels.push_back(label); 151 | snapshots.push_back(sample_cl); 152 | categories[std::max_element(label.begin(), label.end()) - label.begin()]++; 153 | } 154 | } 155 | auto save_everything = [&](bool append_training_set) 156 | { 157 | std::ofstream cfile(freefilename("trained_net",".derp"), std::ios_base::binary); 158 | if (!cfile.is_open()) 159 | throw("unable to open file"); 160 | cfile.write((char*)&cnn_type, sizeof(cnn_type)); 161 | cfile.write((char*)&skip , sizeof(skip)); 162 | cfile.write((char*)&drange , sizeof(drange)); 163 | for (auto &icon : icons) 164 | { 165 | cfile.write((char*)&icon.cam, sizeof(icon.cam)); 166 | cfile.write((char*)icon.raster.data(), icon.raster.size()*sizeof(*icon.raster.data())); 167 | } 168 | cnn.saveb(cfile); 169 | for (unsigned int i = 0; append_training_set && i < samples.size(); i++) 170 | { 171 | cfile.write((char*)&snapshots[i].cam, sizeof(snapshots[i].cam)); 172 | cfile.write((char*)snapshots[i].raster.data(), snapshots[i].raster.size()*sizeof(*snapshots[i].raster.data())); 173 | cfile.write((char*)samples[i].data(), samples[i].size()*sizeof(float)); 174 | cfile.write((char*) labels[i].data(), labels[i].size()*sizeof(float)); 175 | } 176 | }; 177 | float sint = 0.0f; 178 | glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void 179 | { 180 | switch (std::tolower(key)) 181 | { 182 | case 'q': case 27: exit(0); break; // 27 is ESC 183 | case '=': case '+': skip++; break; 184 | case '-': case '_': skip--; break; 185 | case 'r': samplereview = !samplereview; break; 186 | case '\b': if (samplereview) { categories[std::max_element(labels[currentsample].begin(), labels[currentsample].end()) - labels[currentsample].begin()]--; snapshots.erase(snapshots.begin() + currentsample); labels.erase(labels.begin() + currentsample); samples.erase(samples.begin() + currentsample); } // no break here, fall through to clamp currentsample 187 | case '[': case ']': currentsample = clamp(currentsample + (key == ']') - (key == '['), 0, (int)samples.size() - 1); break; 188 | case 's': if (sinusoidal) { addsample(clamp((int)(sint * 5), 0, 4)); for (int i = 0;i < 5;i++)labels.back()[i] = clamp(1.0f - pow(((i + 0.5f) / 5.0f) - sint, 2.0f)*4.0f); } sinusoidal = true; break; // sinusoidal motion; 189 | case 'n': addsample(0); break; 190 | case 'p': addsample(1); break; 191 | case '0': case '1':case '2':case '3':case '4': addsample(key - '0'); break; 192 | case ')': icons[0] = sample_cl; break; // shift-0 is ')' on my keyboard 193 | case '!': icons[1] = sample_cl; break; 194 | case '@': icons[2] = sample_cl; break; 195 | case '#': icons[3] = sample_cl; break; 196 | case '$': icons[4] = sample_cl; break; 197 | case 't': if (key == 'T') training++; else training = (!training)*5; break; // t toggles while shift-T will continue to increas amount of backprop iterations per frame 198 | case '\x13': case '\x14': save_everything(key=='\x14'); break; // ctrl-s or crtl-t (crtl-t causes all the training samples to be output as well) 199 | case '\x18': cnn = cnntypes[cnn_type](); std::cout << "recreating CNN\n"; break; 200 | default: std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n"; break; 201 | } 202 | 203 | skip = clamp(skip, 1, 10); 204 | }; 205 | try { 206 | if (dcam.dev->supports_option(rs::option::r200_lr_auto_exposure_enabled)) 207 | dcam.dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1); 208 | } 209 | catch (...) {} 210 | void* current_selection = NULL; 211 | Pose camera; // note this is the opengl camera, not the depth camera 212 | camera.orientation = normalize(float4(3, 0, 1,0)); 213 | int gv = 240*3/2; int gh = 320*3/2; 214 | int frame = 0; 215 | std::vector errorhistory(128,1.0f); 216 | SOCKET server = INVALID_SOCKET; 217 | int enable_server=1; 218 | 219 | 220 | while (glwin.WindowUp()) 221 | { 222 | 223 | viewdist *= powf(1.1f, (float)glwin.mousewheel); 224 | if (!samples.size()) 225 | samplereview = 0; // cant review samples if there aren't any, stay in live camera mode 226 | 227 | // CNN training if it happens to be enabled 228 | __int64 timestart = __rdtsc(); 229 | float mse = 0.0f; 230 | int traincount = training ? (1 << training) : 0; 231 | for (int i = 0; i < traincount && samples.size() != 0; i++) 232 | { 233 | trainstarted = true; 234 | auto s = std::uniform_int(0, (int)samples.size() - 1)(rng); 235 | mse += cnn.Train(samples[s], labels[s]); 236 | } 237 | mse = traincount ? mse / (float)traincount : 0; 238 | errorhistory[frame % (errorhistory.size() / 2)] = errorhistory[frame % (errorhistory.size() / 2) + (errorhistory.size() / 2)] = sqrt(mse); 239 | __int64 cycles_bprop = __rdtsc() - timestart; 240 | 241 | 242 | 243 | Image dpimage = dcam.GetDepth(); 244 | auto dleft = (const uint8_t *)dcam.dev->get_frame_data(rs::stream::infrared); 245 | Image irimage(dcam.dcamera() , std::vector(dleft, dleft + product(dcam.dim()))); 246 | auto dcolor = (Image({ dcam.dim() }, (const byte3*)dcam.dev->get_frame_data(rs::stream::color_aligned_to_depth))); 247 | if (dpimage.dim().x>320) // since sr300 might be 640x480 and we wanted 320x240 248 | { 249 | dpimage = DownSampleFst(dpimage); 250 | irimage = DownSampleFst(irimage); 251 | dcolor = DownSampleFst(dcolor ); 252 | } 253 | auto dp_image_g = Transform(dpimage, [&drange, depth_scale](unsigned short s) {return (unsigned char)clamp((int)(256.0f * ((s*depth_scale) - drange.x) / (drange.y - drange.x)), 0, 255); }); 254 | auto dp_image_c = torgb(dp_image_g); // Transform(dpimage, [](unsigned short s) {return byte3((unsigned char)clamp(255 - s / 4, 0, 255)); }); 255 | auto ir_image_c = torgb(irimage); //, [](unsigned char c) {return byte3(c);} ); 256 | 257 | std::vector pts; 258 | std::vector outliers; 259 | 260 | 261 | 262 | int2 swsize = int2(16,16) *skip, swoffset = irimage.dim() / 2 - swsize / 2; // use 16,16 instead of input size here 263 | auto sample_raw = Crop(dpimage, swoffset, swsize); 264 | for (auto p : rect_iteration(sample_raw.dim())) 265 | { 266 | float3 v = sample_raw.cam.deprojectz(p, sample_raw.pixel(p))*depth_scale; 267 | ((v.z > drange.y && v.z < drange.x) ? &pts : &outliers)->push_back(v); 268 | } 269 | sample_dp = Crop(dp_image_g, swoffset, swsize); 270 | sample_ir = Crop(irimage, swoffset, swsize); 271 | sample_cl = Crop(dcolor, swoffset, swsize); 272 | 273 | if (sample_dp.cam.dim() != inputsize) 274 | { 275 | float2 scaleby = (asfloat2(inputsize) / asfloat2(sample_dp.cam.dim())); 276 | sample_dp = Sample(std::move(sample_dp), DCamera(inputsize, sample_dp.cam.focal()* scaleby, sample_dp.cam.principal()* scaleby,depth_scale)); 277 | sample_ir = Sample(std::move(sample_ir), DCamera(inputsize, sample_ir.cam.focal()* scaleby, sample_ir.cam.principal()* scaleby,depth_scale)); 278 | } 279 | sample_in = Transform(sample_dp, greyscaletofloat).raster; 280 | Append(sample_in, Transform(sample_ir, greyscaletofloat).raster); 281 | 282 | if (samplereview) 283 | { 284 | currentsample = clamp(currentsample, 0, samples.size()); 285 | sample_in = samples[currentsample]; 286 | sample_cl = snapshots[currentsample]; 287 | auto sd = Transform(samples[currentsample], [](float a) {return togreyscale(a); }); 288 | sample_dp.raster = std::vector(sd.begin(), sd.begin() + sd.size() / 2); 289 | sample_ir.raster = std::vector(sd.begin() + sd.size() / 2,sd.end()); 290 | } 291 | 292 | 293 | timestart = __rdtsc(); 294 | auto cnn_out = trainstarted ? cnn.Eval(sample_in) : std::vector(categories.size(), 0.0f); 295 | __int64 cycles_fprop = __rdtsc() - timestart; 296 | int best = std::max_element(cnn_out.begin(),cnn_out.end())-cnn_out.begin(); 297 | if(enable_server) 298 | do_server_thing(server,std::string("category ")+std::to_string(best)); 299 | 300 | 301 | auto samplec = torgb(UpSample(UpSample(sample_dp))); 302 | 303 | for (auto im : { &dp_image_c,&ir_image_c ,&dcolor }) for (int i = 0; i < 7;i++) // draw red-blue box around focus subwindow 304 | { 305 | for (int x = 0; x < swsize.x; x++) for (auto y : { -1-i,swsize.y+i }) 306 | im->pixel(swoffset + int2(x, y)) = byte3(255, 0, 0); 307 | for (int y = 0; y < swsize.y; y++) for (auto x : { -1-i,swsize.x+i }) 308 | im->pixel(swoffset + int2(x, y)) = byte3(0, 0, 255); 309 | } 310 | 311 | 312 | GUI gui(glwin, current_selection); 313 | 314 | static int debugpanelheight = 1; 315 | gui.splity(debugpanelheight,3); 316 | if(gui.dims().y>10) 317 | { 318 | glwin.PrintString({ 1,-1 }, " training %s %d er %5.3f", training ? "ON" : "OFF", training, sqrt(mse)); 319 | glwin.PrintString({ 1,-4 }, "rdtsc fprop %9d bprop %9d ", (int)cycles_fprop, (int)cycles_bprop); 320 | glwin.PrintString({ 1,-10 }, "x,y,b,d %d %d %d %d", glwin.mousepos.x, glwin.mousepos.y, glwin.MouseState, glwin.downevent); 321 | } 322 | gui.pop(); 323 | gv = std::max(gv, std::min(gui.dims().y,100)); 324 | gui.splityn(gv); // gui section for live camera input feeds and setting depth range and sample window size 325 | { 326 | gh = gv * 4 / 3; 327 | gui.splitxc(gh+gh/2); 328 | if (samplereview) 329 | { 330 | gui.splitync(gui.dims().y-30); 331 | gui.splitxc(gui.dims().y); 332 | gui.drawimagem(snapshots[currentsample]); 333 | glwin.PrintString({ 0,-1 }, "Captured Sample Review Mode"); 334 | gui.pop(); 335 | auto cat = std::max_element(labels[currentsample].begin(), labels[currentsample].end()) - labels[currentsample].begin(); 336 | gui.drawbar(0.5f, catcolors[cat], catcolors[cat]); 337 | glwin.PrintString({ 0,-1 }, (cat == best) ? "Success, CNN output matches this category" : "Fail, sample miscategorized"); 338 | gui.pop(); 339 | gui.slider(currentsample, { 0,(int)snapshots.size() - 1 }, "sample #"); // note the potential side effect of changing this value here 340 | } 341 | else 342 | { 343 | gui.splitx(gh); 344 | gv = gh * 3 / 4; 345 | gui.drawimagem(dcolor); 346 | gui.pop(); 347 | gui.splityc(gv / 2, 1); 348 | gui.drawimagem(dp_image_c); 349 | if (gui.inview()) 350 | glwin.PrintString({ 0,-1 }, "depth stream"); 351 | if (gui.focus(&dp_image_c)) 352 | { 353 | auto c = (gui.mouse()-gui.offset()) * dp_image_c.dim() / gui.dims() - dp_image_c.dim() / 2; 354 | skip = clamp(std::max(abs(c.x), abs(c.y)) / (16 / 2), 1, 10); 355 | } 356 | gui.pop(); 357 | gui.drawimagem(ir_image_c); 358 | if (gui.inview()) 359 | glwin.PrintString({ 0,-1 }, "IR stream"); 360 | if (gui.focus(&ir_image_c)) 361 | { 362 | auto c = (gui.mouse() - gui.offset())* ir_image_c.dim() / gui.dims() - ir_image_c.dim() / 2; 363 | skip = clamp(std::max(abs(c.x), abs(c.y)) / (16 / 2), 1, 10); 364 | } 365 | } 366 | gui.pop(); 367 | gui.splitxc( gv / 2); 368 | { // draw the two 16x16 actual inputs zoomed in. 369 | gui.splityc(gv / 2, 1); 370 | gui.drawimagem(samplec); 371 | if (gui.inview()) 372 | glwin.PrintString({ 0,-1 }, "CNN input depth"); 373 | gui.pop(); 374 | gui.drawimagem(torgb(UpSample(UpSample(sample_ir)))); 375 | if (gui.inview()) 376 | glwin.PrintString({ 0,-1 }, "CNN input IR"); 377 | } 378 | gui.pop(); // now the last bit on the right 379 | { 380 | static int slider_set = 92; 381 | slider_set = std::max(std::min(gui.dims().y,45), slider_set); 382 | gui.splity(slider_set); 383 | { 384 | // gui.splityc(slider_set / 3 - 1); 385 | auto regions = gui.partition_y(3); 386 | gui.set(regions[0]); 387 | { 388 | gui.slider(skip, { 1,10 }, "samplearea "); 389 | skip = clamp(skip, 1, 10); 390 | } 391 | //gui.pop(); 392 | //gui.splityc(slider_set/3-1); 393 | gui.set(regions[1]); 394 | { 395 | gui.slider(drange.x, { 0.25f,2.0f }, "depthmax "); 396 | drange.x = clamp(drange.x, 0.25f, 2.0f); 397 | drange.y = std::min(drange.y, drange.x - 0.001f); 398 | } 399 | //gui.pop(); 400 | gui.set(regions[2]); 401 | { 402 | gui.slider(drange.y, { 0.25f,2.0f }, "depthmin "); 403 | drange.y = clamp(drange.y, 0.25f, 2.0f); 404 | drange.x = std::max(drange.x, drange.y + 0.001f); // drange note x>y so brighter is closer 405 | } 406 | } 407 | gui.pop(); 408 | { 409 | camera.orientation = qconj(camera.orientation); // since trackball is normally used for adjusting another object's orientation, but here we're updating a the viewcamera looking at the object 410 | gui.trackball(camera.orientation); // may update camera.orientation only if user mouse drags within subwindow area 411 | camera.orientation = qconj(camera.orientation); // and back 412 | gui.perspective(); 413 | glPushMatrix(); 414 | camera.position = float3(0, 0, (drange.x + drange.y)*0.5f) + qzdir(camera.orientation) * viewdist; 415 | glMultMatrixf(camera.inverse().matrix()); 416 | 417 | glColor3f(1, 1, 1); 418 | glwirefrustumz(dcam.deprojectextents(), { 0.1f,1.0f }); // draw the depth camera frustum volume 419 | glColor3f(0, 1, 0.5f); 420 | glwirefrustumz(sample_dp.cam.deprojectextents(), drange); // draw the sample camera frustum volume 421 | drawpoints(pts, { 0 ,1,0 }); 422 | drawpoints(outliers, { 0.5f,0,0 }); 423 | glPopMatrix(); 424 | } 425 | } 426 | } 427 | gui.pop(); // end of the camera feeds 428 | 429 | static int cdata = 60; 430 | int midy = gui.dims().y; 431 | gui.splitxc(gui.dims().x - midy*2); // leaves a couple square sections on the right in the middle 432 | { 433 | auto cat_regions = gui.partition_y((int)categories.size()); 434 | for (int i = 0; i < (int)categories.size(); i++) 435 | { 436 | //gui.splitync(midy / categories.size() - 1, 1); 437 | gui.set(cat_regions[i]); 438 | gui.splitxc(midy / categories.size()); 439 | gui.drawimagem(icons[i]); 440 | glwin.PrintString({ 0,-1 }, "%d", i); 441 | if (gui.focus(&icons[i])) 442 | addsample(i); 443 | gui.pop(); 444 | static int ncdata; 445 | auto bwidth = gui.dims().x; 446 | ncdata = bwidth - cdata; 447 | gui.splitx(ncdata); 448 | cdata = bwidth - ncdata; 449 | gui.drawbar(cnn_out[i], catcolors[i], catcolors[i] * 0.25f); 450 | glwin.PrintString({ 0,-1 }, "%5.2f", cnn_out[i]); 451 | gui.pop(); 452 | {// the little text block at the end of each category row 453 | glwin.PrintString({ 0,-1 }, " %d ", categories[i]); // puts the number of samples of this type as text output 454 | } 455 | //gui.pop(); 456 | } 457 | } 458 | gui.pop(); // done left side of mid section 459 | gui.splitxc(midy); 460 | { // middle of middle section 461 | static int slider_height = 30; 462 | slider_height = std::max(slider_height, 10); 463 | gui.splity(slider_height); 464 | gui.slider(training, { 0,6 }, training? " Training: 2^":" click here to crank up training"); 465 | gui.pop(); 466 | gui.splitxc(gui.dims().y-slider_height); // just to make it square 467 | if (training) 468 | { 469 | gui.ortho(); 470 | glColor3f(1, 0, 0); 471 | glBegin(GL_QUAD_STRIP); 472 | for (unsigned int i = 0; i < errorhistory.size() / 2; i++) 473 | { 474 | glVertex2f((float)i / (errorhistory.size()/2), errorhistory[1 + i + frame % (errorhistory.size() / 2)]); 475 | glVertex2f((float)i / (errorhistory.size()/2), 0); 476 | } 477 | glEnd(); 478 | glColor3f(1, 1, 1); 479 | glwin.PrintString({ 0,-1 }, "error: %5.2f", sqrt(mse)); 480 | } 481 | else if (sinusoidal) 482 | { 483 | float sint = 0.5f + sin(2.0f * time/2*3.1415f) / 2; 484 | gui.ortho(); 485 | glColor3f(1, 0, 0); 486 | glBegin(GL_QUAD_STRIP); 487 | for (float x : {0.0f, 1.0f}) for (float dy : {-0.05f, 0.05f}) 488 | { 489 | glVertex2f(x,1.0f-sint+dy); 490 | } 491 | glEnd(); 492 | glColor3f(1, 1, 1); 493 | glwin.PrintString({ 0,-1 }, "normalized sin(k*time): %5.2f", sint); 494 | } 495 | else 496 | { // sample status 497 | gui.ortho(); 498 | auto sizes = Transform(categories, [&](int c) {return (float)c / samples.size(); }); 499 | float b = 0; 500 | float r = log2(samples.size() + 1.0f)/10.0f * 0.5f; 501 | for (unsigned int i = 0; i < sizes.size(); i++) 502 | { 503 | glColor3fv(catcolors[i]); 504 | glBegin(GL_TRIANGLE_FAN); 505 | glVertex2f(0.5f, 0.5f); 506 | for (float t = 0; t <= 1.0f; t += 1 / 32.0f) 507 | glVertex2f(0.5f + r* cos(2 * 3.14159f*(b + sizes[i] * t)), 0.5f + r* sin(2 * 3.14159f*(b + sizes[i] * t))); 508 | glEnd(); 509 | b += sizes[i]; 510 | } 511 | glColor3f(1, 1, 1); 512 | glwin.PrintString({ 0,-1 }, "samples collected: %d",samples.size()); 513 | } 514 | gui.offset().y += gui.dims().y - 40; 515 | gui.dims().y = 37; 516 | gui.offset().x += 5; 517 | gui.dims().x -= 10; 518 | gui.set(); 519 | gui.splitxc(gui.dims().x-90, 5); 520 | gui.dims().x = std::min(gui.dims().x, 180); 521 | gui.set(); 522 | static float rslide = 0.0f; 523 | gui.slider(rslide, { 0.0f,1.0f }, samplereview? "Review Samples ON " : "Review Samples OFF " ); 524 | if (gui.selection == &rslide) 525 | samplereview = (rslide>0.5f); 526 | else 527 | rslide += samplereview ? 0.01f : -0.01f; 528 | rslide = clamp(rslide); 529 | gui.pop(); 530 | gui.slider(enable_server, { 0,1 }, "HTTP "); 531 | if(enable_server && server==INVALID_SOCKET) 532 | server = start_server(12345); 533 | if (server == INVALID_SOCKET) 534 | enable_server = 0; 535 | gui.pop(); 536 | // a vertical strip thats the same width as the slider for the training 537 | } 538 | gui.pop(); 539 | if(trainstarted) 540 | { // winner of cnn evaluation 541 | gui.drawbar(1.0f, catcolors[best], catcolors[best]); 542 | gui.offset() += int2(5, 5); 543 | gui.dims() -= int2(10, 10); 544 | gui.set(); 545 | gui.drawimagem(icons[best]); 546 | glwin.PrintString({ 0,-1 }, "%d eval: %5.3f", best, cnn_out[best]); 547 | } 548 | else if (!samples.size()) 549 | { 550 | // if we haven't done anything yet, this part of the GUI allows us to change the size of the CNN 551 | auto lg2 = [](int a)->int { int x = 0; while (a) { a >>= 1; x++; }return x; }; 552 | gui.splitync(40); 553 | auto w_old = cnn_type; 554 | gui.slider(cnn_type, { 0,2 }, "Size of CNN, (16,32,64) squared samples "); 555 | if (cnn_type != w_old) 556 | cnn = cnntypes[cnn_type](); 557 | inputsize = ((CNN::LConv*)cnn.layers[0])->indims.xy(); 558 | gui.pop(); 559 | gui.splitync(40); 560 | glwin.PrintString({ 0,-1 }, "Current CNN %s config uses %dx%d input", cnndescriptions[cnn_type], inputsize.x, inputsize.y); 561 | gui.pop(); 562 | 563 | } 564 | gui.pop(); // done mid section 565 | 566 | frame++; 567 | time += 0.016f; // yo, which sys call should we use here? 568 | sint = 0.5f + sin(2.0f * time / 2.0f * 3.1415f) / 2; 569 | } 570 | return 0; 571 | } 572 | catch (const char *c) 573 | { 574 | MessageBox(GetActiveWindow(), c, "FAIL", 0); 575 | } 576 | catch (std::exception e) 577 | { 578 | MessageBox(GetActiveWindow(), e.what(), "FAIL", 0); 579 | } 580 | -------------------------------------------------------------------------------- /dclassify/dclassify.s2e: -------------------------------------------------------------------------------- 1 | { "extensionName": "DClassify", 2 | "extensionPort": 12345, 3 | "blockSpecs": [ 4 | ["r", "category", "category"], 5 | ] 6 | } 7 | -------------------------------------------------------------------------------- /dclassify/dclassify.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {78461E47-3F4A-4AA3-8455-7D2E5366618C} 23 | Win32Proj 24 | dclassify 25 | 8.1 26 | 27 | 28 | 29 | Application 30 | true 31 | $(DefaultPlatformToolset) 32 | NotSet 33 | 34 | 35 | Application 36 | false 37 | $(DefaultPlatformToolset) 38 | true 39 | NotSet 40 | 41 | 42 | Application 43 | true 44 | $(DefaultPlatformToolset) 45 | NotSet 46 | 47 | 48 | Application 49 | false 50 | $(DefaultPlatformToolset) 51 | true 52 | NotSet 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | true 74 | $(ProjectDir) 75 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 76 | $(ProjectName)_$(Configuration)_$(Platform) 77 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 78 | 79 | 80 | true 81 | $(ProjectDir) 82 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 83 | $(ProjectName)_$(Configuration)_$(Platform) 84 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 85 | 86 | 87 | false 88 | $(ProjectDir) 89 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 90 | $(ProjectName)_$(Configuration)_$(Platform) 91 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 92 | 93 | 94 | false 95 | $(ProjectDir) 96 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 97 | $(ProjectName)_$(Configuration)_$(Platform) 98 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 99 | 100 | 101 | 102 | 103 | 104 | Level3 105 | Disabled 106 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 107 | 108 | 109 | Console 110 | true 111 | 112 | 113 | 114 | 115 | 116 | 117 | Level3 118 | Disabled 119 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 120 | 121 | 122 | Console 123 | true 124 | 125 | 126 | 127 | 128 | Level3 129 | 130 | 131 | MaxSpeed 132 | true 133 | true 134 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 135 | 136 | 137 | Console 138 | true 139 | true 140 | true 141 | 142 | 143 | 144 | 145 | Level3 146 | 147 | 148 | MaxSpeed 149 | true 150 | true 151 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 152 | 153 | 154 | Console 155 | true 156 | true 157 | true 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | -------------------------------------------------------------------------------- /dclassify/load_trained_cnn.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | 23 | 24 | 25 | {DE74729B-1644-4955-8C7B-1A2CAC271B75} 26 | Win32Proj 27 | test_cnn 28 | 8.1 29 | load_trained_cnn 30 | 31 | 32 | 33 | Application 34 | true 35 | $(DefaultPlatformToolset) 36 | NotSet 37 | 38 | 39 | Application 40 | false 41 | $(DefaultPlatformToolset) 42 | true 43 | NotSet 44 | 45 | 46 | Application 47 | true 48 | $(DefaultPlatformToolset) 49 | NotSet 50 | 51 | 52 | Application 53 | false 54 | $(DefaultPlatformToolset) 55 | true 56 | NotSet 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | true 78 | $(ProjectDir) 79 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 80 | $(ProjectName)_$(Configuration)_$(Platform) 81 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 82 | 83 | 84 | true 85 | $(ProjectDir) 86 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 87 | $(ProjectName)_$(Configuration)_$(Platform) 88 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 89 | 90 | 91 | false 92 | $(ProjectDir) 93 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 94 | $(ProjectName)_$(Configuration)_$(Platform) 95 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 96 | 97 | 98 | false 99 | $(ProjectDir) 100 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 101 | $(ProjectName)_$(Configuration)_$(Platform) 102 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 103 | 104 | 105 | 106 | 107 | 108 | Level3 109 | Disabled 110 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 111 | 112 | 113 | Console 114 | true 115 | 116 | 117 | 118 | 119 | 120 | 121 | Level3 122 | Disabled 123 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 124 | 125 | 126 | Console 127 | true 128 | 129 | 130 | 131 | 132 | Level3 133 | 134 | 135 | MaxSpeed 136 | true 137 | true 138 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 139 | 140 | 141 | Console 142 | true 143 | true 144 | true 145 | 146 | 147 | 148 | 149 | Level3 150 | 151 | 152 | MaxSpeed 153 | true 154 | true 155 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 156 | 157 | 158 | Console 159 | true 160 | true 161 | true 162 | 163 | 164 | 165 | 166 | 167 | -------------------------------------------------------------------------------- /dclassify/science_rulz.sb2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/melax/dsamples/a075a93608f1460d46b3be4c64561cbe55fc5c0f/dclassify/science_rulz.sb2 -------------------------------------------------------------------------------- /dclassify/scratchtest.sb2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/melax/dsamples/a075a93608f1460d46b3be4c64561cbe55fc5c0f/dclassify/scratchtest.sb2 -------------------------------------------------------------------------------- /dclassify/simple_load_trained_cnn.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // minimal program with reference code to show how to load and use a CNN trained using dclassified application 3 | // for more information please first see dclassified project. 4 | // 5 | // in windows desktop gui you can drag a .derp file onto the .exe, 6 | // or, if using the command line, you can specify trained cnn file as first argument to .exe 7 | // 8 | // its just text output 0-4 that should change depending on what the camera sees and what you trained the net for. 9 | // 10 | 11 | #include 12 | #include // std::max 13 | #include "dcam.h" 14 | #include "misc_image.h" 15 | //#include "misc.h" // for freefilename() 16 | #include "geometric.h" 17 | #include "cnn.h" 18 | 19 | #define NOMINMAX 20 | #include // only used for messagebox 21 | 22 | 23 | // note theres some duplicate code that could probably be moved into a common header... 24 | 25 | CNN baby_cnn() 26 | { 27 | CNN cnn({}); 28 | cnn.layers.push_back(new CNN::LConv({ 16,16,2 }, { 5,5,2,16 }, { 12,12,16 })); 29 | cnn.layers.push_back(new CNN::LActivation(12 * 12 * 16)); 30 | cnn.layers.push_back(new CNN::LMaxPool(int3(12, 12, 16))); 31 | cnn.layers.push_back(new CNN::LFull(6 * 6 * 16, 32)); 32 | cnn.layers.push_back(new CNN::LActivation(32)); 33 | cnn.layers.push_back(new CNN::LFull(32, 5)); 34 | cnn.layers.push_back(new CNN::LSoftMax(5)); 35 | cnn.Init(); // initializes weights 36 | return cnn; 37 | } 38 | CNN small_cnn() 39 | { 40 | CNN cnn({}); 41 | cnn.layers.push_back(new CNN::LConv({ 32, 32, 2 }, { 5, 5, 2, 16 }, { 28, 28, 16 })); 42 | cnn.layers.push_back(new CNN::LActivation(28 * 28 * 16)); 43 | cnn.layers.push_back(new CNN::LMaxPool(int3(28, 28, 16))); 44 | cnn.layers.push_back(new CNN::LMaxPool(int3(14, 14, 16))); 45 | cnn.layers.push_back(new CNN::LFull(7 * 7 * 16, 32)); 46 | cnn.layers.push_back(new CNN::LActivation(32)); 47 | cnn.layers.push_back(new CNN::LFull(32, 5)); 48 | cnn.layers.push_back(new CNN::LSoftMax(5)); 49 | cnn.Init(); 50 | return cnn; 51 | } 52 | CNN reg_cnn() // probably too big to learn quickly with small ground truth sample size 53 | { 54 | CNN cnn({}); 55 | cnn.layers.push_back(new CNN::LConv({ 64, 64, 2 }, { 5, 5, 2, 16 }, { 60, 60, 16 })); 56 | cnn.layers.push_back(new CNN::LActivation(60 * 60 * 16)); 57 | cnn.layers.push_back(new CNN::LMaxPool(int3(60, 60, 16))); 58 | cnn.layers.push_back(new CNN::LMaxPool(int3(30, 30, 16))); 59 | cnn.layers.push_back(new CNN::LConv({ 15, 15, 16 }, { 8, 8, 16, 256 }, { 8, 8, 64 })); 60 | cnn.layers.push_back(new CNN::LActivation(8 * 8 * 64)); 61 | cnn.layers.push_back(new CNN::LMaxPool(int3(8, 8, 64))); 62 | cnn.layers.push_back(new CNN::LFull(4 * 4 * 64, 64)); 63 | cnn.layers.push_back(new CNN::LActivation(64)); 64 | cnn.layers.push_back(new CNN::LFull(64, 5)); 65 | cnn.layers.push_back(new CNN::LSoftMax(5)); 66 | cnn.Init(); 67 | return cnn; 68 | } 69 | 70 | CNN(*cnntypes[])() = { baby_cnn,small_cnn,reg_cnn }; 71 | char *cnndescriptions[] = { "baby_cnn","small_cnn","reg_cnn" }; 72 | 73 | int main(int argc, char *argv[]) try 74 | { 75 | if (argc != 2) 76 | throw("usage test_cnn.exe trained_cnn_file.derp"); 77 | 78 | std::cout << "creating network\n"; 79 | 80 | 81 | int cnn_type=0; 82 | auto cnn = cnntypes[cnn_type](); 83 | int2 inputsize = ((CNN::LConv*)cnn.layers[0])->indims.xy(); // depending which is created, inputsize will be either 16x16 or 32x32 84 | 85 | 86 | int skip = 4; // selection subregion will be center of input and of size 16*skip squared, with 128x128 max. will subsample as necessary to fit cnn input size 87 | float2 drange = { 0.80f,0.30f }; // range of depth we care about. note the .x > .y, this is so that closer things are more white and further things are more dark. 88 | 89 | 90 | Image sample_dp(inputsize); 91 | Image sample_ir(inputsize); 92 | std::vector sample_in; 93 | std::vector categories(5, 0); 94 | float3 catcolors[] = { { 0,0,1 },{ 0,1,0 },{ 1,0,0 },{ 1,1,0 },{ 1,0,1 } }; 95 | std::vector> icons; 96 | auto sample_cl = Image({ 1,1 }, std::vector(1 * 1, { 127,127,127 })); 97 | for (auto c : categories) 98 | icons.push_back(sample_cl); 99 | 100 | 101 | std::ifstream cfile(argv[1], std::ios_base::binary | std::ios_base::in); 102 | if (!cfile.is_open()) 103 | throw("unable to open file"); 104 | cfile.read((char*)&cnn_type, sizeof(cnn_type)); 105 | cfile.read((char*)&skip , sizeof(skip)); 106 | cfile.read((char*)&drange , sizeof(drange)); 107 | for (auto &icon : icons) 108 | { 109 | cfile.read((char*)&icon.cam, sizeof(icon.cam)); 110 | icon.raster.resize(icon.cam.dim().x*icon.cam.dim().y); 111 | cfile.read((char*)icon.raster.data(), icon.raster.size()*sizeof(*icon.raster.data())); 112 | } 113 | cnn = cnntypes[cnn_type](); 114 | inputsize = ((CNN::LConv*)cnn.layers[0])->indims.xy(); 115 | cnn.loadb(cfile); 116 | 117 | 118 | RSCam dcam; 119 | //dcam.enable_filter_depth = false; 120 | dcam.Init(); 121 | float depth_scale = (dcam.dev) ? dcam.dev->get_depth_scale() : 0.001f; // to put into meters // if file assume file is mm 122 | if (dcam.dev->supports_option(rs::option::r200_lr_auto_exposure_enabled)) 123 | dcam.dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1); 124 | 125 | void* current_selection = NULL; 126 | Pose camera; // note this is the opengl camera, not the depth camera 127 | camera.orientation = normalize(float4(0, -3, 0, 1)); 128 | int gv = 240*3/2; int gh = 320*3/2; 129 | int frame = 0; 130 | std::vector errorhistory(128,1.0f); 131 | while (1) 132 | { 133 | auto dpimage = dcam.GetDepth(); 134 | auto dleft = (const uint8_t *)dcam.dev->get_frame_data(rs::stream::infrared); 135 | Image irimage(dcam.dcamera(), std::vector(dleft, dleft + dcam.dim().x*dcam.dim().y)); 136 | auto dp_image_g = Transform(dpimage, [&drange, depth_scale](unsigned short s) {return (unsigned char)clamp((int)(256.0f * ((s*depth_scale) - drange.x) / (drange.y - drange.x)), 0, 255); }); 137 | auto dp_image_c = torgb(dp_image_g); 138 | auto ir_image_c = torgb(irimage); 139 | auto dcolor = (Image({ 320, 240 }, (const byte3*)dcam.dev->get_frame_data(rs::stream::color_aligned_to_depth))); 140 | 141 | 142 | int2 swsize = int2(16,16) *skip, swoffset = irimage.dim() / 2 - swsize / 2; // use 16,16 instead of input size here 143 | auto sample_raw = Crop(dpimage, swoffset, swsize); 144 | sample_dp = Crop(dp_image_g, swoffset, swsize); 145 | sample_ir = Crop(irimage, swoffset, swsize); 146 | sample_cl = Crop(dcolor, swoffset, swsize); 147 | 148 | if (sample_dp.cam.dim() != inputsize) 149 | { 150 | float2 scaleby = (asfloat2(inputsize) / asfloat2(sample_dp.cam.dim())); 151 | sample_dp = Sample(std::move(sample_dp), DCamera(inputsize, sample_dp.cam.focal()* scaleby, sample_dp.cam.principal()* scaleby,depth_scale)); 152 | sample_ir = Sample(std::move(sample_ir), DCamera(inputsize, sample_ir.cam.focal()* scaleby, sample_ir.cam.principal()* scaleby,depth_scale)); 153 | } 154 | sample_in = Transform(sample_dp, greyscaletofloat).raster; 155 | Append(sample_in, Transform(sample_ir, greyscaletofloat).raster); 156 | 157 | 158 | 159 | 160 | auto cnn_out = cnn.Eval(sample_in) ; 161 | int best = std::max_element(cnn_out.begin(),cnn_out.end())-cnn_out.begin(); 162 | 163 | std::cout << "Category: " << best << '\r'; 164 | std::cout.flush(); 165 | 166 | } 167 | return 0; 168 | } 169 | catch (const char *c) 170 | { 171 | std::cerr << "Error: " << c << '\n'; 172 | MessageBox(GetActiveWindow(), c, "FAIL", 0); 173 | } 174 | catch (std::exception e) 175 | { 176 | std::cerr << "Error: " << e.what() << '\n'; 177 | MessageBox(GetActiveWindow(), e.what(), "FAIL", 0); 178 | } 179 | -------------------------------------------------------------------------------- /dpca/BreakoutGame.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | HTML and JavaScript Depth Camera Sample 6 | 220 | 221 | 222 |

Depth Camera with HTML and JavaScript Sample

223 |

224 | Depth camera blob Tracking added to (Bill Mill's) JavaScript Canvas Tutorial (a Breakout clone). 225 |

226 | 227 |

228 | Breakout style game with depth camera tracking to move the paddle.

229 |

230 | Please note, this integration of depth data into a browser and this canvas demo is in an experimental state. 231 | This demo may or may not work. It might depend on the combination of web browser and os and whatever settings are in use. 232 | I had success on my home desktop using the Chrome browser for windows. Microsoft Edge, on my windows10 desktop didn't seem to work. 233 | Microsoft's Internet Explorer (IE), which is apparantly different than Edge, does work. 234 | On my windows8 desktop at the office, Chrome didn't work (perhaps due to proxy settings), but Microsoft's Internet Explorer still worked ok. 235 |

236 |

How to get the demo running:

237 |
    238 |
  • Compile C++ project dpca with visual studio 2015. Its best to use RELEASE mode.
    • 239 |
  • Run the program dpca_Release_Win32.exe .
    • 240 |
  • 241 | Hit 'h'. The http server part of the principal component analysis sample is not on by default. 242 | With that program in focus (alt-tab if necessary) hit the 'h' key so that it will also be a mini web http server to feed blob pose (position) information into this web page. 243 |
    • 244 |
  • You might get a popup "Windows Firewall has blocked Access...". This is the operating system asking if you wish to allow this .exe to accept tcp/ip connections and serve http requests. Given that you just compiled and ran this .exe yourself, you can see the code for yourself and determine that it should be safe.
    • 245 |
  • 246 | Next click this button: To start/stop the game in the canvas above. 247 |
    • 248 |
  • 249 | by waving your hand or something else in the near view volume of the depth camera you will control the x position of the paddle above. 250 |
    • 251 |
  • 252 | If the framerate is slow or paddle movement updates are infrequent (feels "choppy" or slow and responsive), 253 | then it could be because the c++ dpca exe closes the socket after each http request. 254 | Look for comments in the code (in dcpa.cpp) for a one line code change that will attempt to reuse the socket. 255 | This made things in Chrome run much faster. 256 | I didn't leave that in by default since for some reason the "keep-alive" version didn't work at all for internet explorer on work PC at the office, 257 | whereas closing the socket each time worked fine and fast too. 258 |
    • 259 |
  • 260 | For some versions of Internet Explorer, when this html page is first loaded into the IE browser, you might see "Internet Explorer restricted this webpage from running scripts or ActiveX controls." beside a button to "Allow Blocked Content". 261 | You can scan the source html and javascript to this page (all contained in this .html file rather than external links) and detemine if you want to allow it to run. 262 |
    • 263 |
  • 264 | If the demo doesn't work at first, try refreshing this web page, and restarting the dpca program and its httpd server. 265 | If it still doesn't work, and you've tried a couple of different browsers, uhm, i dont know. 266 |
    • 267 |
268 | 269 |
270 | 271 | 272 | 273 |

274 | This sample is meant to show how one might route information originally derived from depth data into the web browser environment. 275 | If javascript is working and tracking server is up and running, then the updates should appear in the sections below: 276 |

277 |
278 |

279 | the javascript status output section. various function output is written here 280 |

281 |
282 |

283 | another javascript response paragraph used to trace the http response. 284 |

285 |
286 | 287 |

288 | i just used this paragraph to trace the blob or hand 'x' position 289 |

290 |
291 |

See the source of this .html file and the C++ source code handtrackserver.cpp and others for more information about how this all works.

292 |
293 |

special thanks to Bill Mill for making the original canvas tutorial, and allowing its use in this depth-camera-sample here.

294 | 295 | 296 | 297 | -------------------------------------------------------------------------------- /dpca/dpca.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // DPCA - Principal Component Analysis on point cloud depth data. 3 | // uses Intel RealSense depth camera 4 | // 5 | // Many basic use cases involving depth cameras can be implemented robustly with some very simple point cloud analysis. 6 | // This sample is meant to show how to extract the center of mass and covariance (or orientation) of a point cloud. 7 | // One mode shows how to also extract the curvatures on point cloud (or surface of object in view). 8 | // There is no segmentation, so experiment with this putting only one object or hand within the view frustum of the depth camera. 9 | // The range is limited, and can be adjusted dynamically, to avoid picking up unwanted things in the background. 10 | // 11 | // this program is a re-implementation of one of the examples from our "programming with depth data" developer lab session at IDF2013. 12 | // the video https://www.youtube.com/watch?v=a9vgHR8qyHg shows what was covered in that course 13 | // 14 | 15 | #include // std::tolower 16 | #include 17 | #include // std::max 18 | 19 | #include 20 | #include 21 | #include 22 | #include "dcam.h" 23 | #include "mesh.h" 24 | #include "misc_gl.h" 25 | #include "misc.h" 26 | #include "tcpsocket.h" 27 | 28 | // util drawing routines 29 | // most of this code is duplicated from testcov and ploidfit from the sandbox repo 30 | 31 | void glAxis(float len=1.0f) 32 | { 33 | glPushAttrib(GL_ALL_ATTRIB_BITS); 34 | glLineWidth(3.0f); 35 | glBegin(GL_LINES); 36 | for (int i : {0, 1, 2}) 37 | { 38 | float3 v(0, 0, 0); 39 | v[i] = 1.0f; 40 | glColor3fv(v); 41 | glVertex3fv({ 0, 0, 0 }); 42 | glVertex3fv(v*len); 43 | } 44 | glEnd(); 45 | glPopAttrib(); 46 | } 47 | void glAxis(const Pose &pose, float len = 1.0f) 48 | { 49 | glPushMatrix(); 50 | glMultMatrixf(pose.matrix()); 51 | glAxis(len); 52 | glPopMatrix(); 53 | } 54 | 55 | std::vector gridtriangulation(int2 tess) 56 | { 57 | std::vector tris; 58 | for (int y = 0; y < tess.y - 1; y++) for (int x = 0; x < tess.x - 1; x++) 59 | { 60 | tris.push_back({ (y + 1)* tess.x + x + 0, (y + 0)* tess.x + x + 0, (y + 0)* tess.x + x + 1 }); 61 | tris.push_back({ (y + 0)* tess.x + x + 1, (y + 1)* tess.x + x + 1, (y + 1)* tess.x + x + 0 }); // note the {2,0} edge is the one that cuts across the quad 62 | } 63 | return tris; 64 | } 65 | Mesh sphere(int2 tess) 66 | { 67 | std::vector verts; 68 | for (int y = 0; y < tess.y; y++) for (int x = 0; x < tess.x; x++) 69 | { 70 | float lat = 3.14159f * (y / (tess.y - 1.0f) - 0.5f); 71 | float lng = 3.14159f * 2.0f * x / (tess.x - 1.0f); 72 | float3 p(cos(lat)*cos(lng), cos(lat)*sin(lng), sin(lat)); 73 | float3 u(-sin(lng), cos(lng), 0); 74 | verts.push_back({ p, quatfrommat({ u, cross(p, u), p }),{ x / (tess.x - 1.0f), y / (tess.y - 1.0f) } }); 75 | } 76 | return{ verts, gridtriangulation(tess), Pose(), "",{ 1, 1, 1, 1 } }; 77 | } 78 | 79 | Mesh scale(Mesh m, float3 r) 80 | { 81 | auto stretch = [&r](Vertex v)->Vertex 82 | { 83 | float3 n = qzdir(v.orientation) / r; 84 | float3 u = qxdir(v.orientation) / r; 85 | return{ v.position* r, quatfrommat({ u, cross(n, u), n }), v.texcoord }; 86 | }; 87 | std::transform(m.verts.begin(), m.verts.end(), m.verts.begin(), stretch); 88 | return m; 89 | } 90 | Mesh ellipse(float3 r) 91 | { 92 | return scale(sphere({ 23, 17 }), r); 93 | } 94 | 95 | bool show_ellipsoid_normals = false; // just to make sure my mesh scale worked ok 96 | inline void glellipsoid(const float3 &r) // wire mesh version 97 | { 98 | auto e = ellipse(r); 99 | glBegin(GL_LINES); 100 | glColor3f(0.5f, 0.5f, 0.5f); 101 | for (auto t : e.tris) for (auto i : { 0, 1, 1, 2 }) // just draw first 2 edges since {0,2} is the diagonal that cuts across the quad 102 | glVertex3fv(e.verts[t[i]].position); 103 | glColor3f(0.0f, 0.5f, 0.5f); 104 | for (auto p : e.verts) for (auto n : { 0.0f, 0.02f }) if (show_ellipsoid_normals) // just for debugging 105 | glVertex3fv(p.position + qzdir(p.orientation)*n); 106 | glEnd(); 107 | } 108 | inline void glellipsoid(const float3 &r, const Pose &pose) { glPushMatrix(); glMultMatrixf(pose.matrix()); glellipsoid(r); glPopMatrix(); } 109 | 110 | 111 | void glWirePatch(std::function m) 112 | { 113 | auto f = [](std::function m) 114 | { 115 | for (float x = 0; x <= 1.0f; x += 1.0f / 16) 116 | { 117 | glBegin(GL_LINE_STRIP); 118 | for (float y = 0; y <= 1.0f; y += 1.0f / 16) 119 | glVertex3fv(m({ x,y })); 120 | glEnd(); 121 | } 122 | }; 123 | f(m); 124 | f([m](float2 c) {return m({ c.y,c.x }); }); 125 | } 126 | 127 | void glHeightField(float2 bmin, float2 bmax, std::function h, float3 c = { 1,1,1 }) 128 | { 129 | glColor3fv(c); 130 | glWirePatch([&h, &bmin, &bmax](float2 c)->float3 {auto p = bmin + (bmax - bmin)*c; return{ p.x,p.y,h(p) }; }); 131 | } 132 | 133 | 134 | // math support routine copied from ploidfit 135 | float4 ParabloidFit(const std::vector &pts) // least squares fitting quadratic patch centered at x,y==0,0 136 | { 137 | float4x4 m; 138 | float4 b; 139 | for (auto &p : pts) 140 | { 141 | float4 v(p.x*p.x, p.y*p.y, p.x*p.y, 1.0f); 142 | m += outerprod(v, v); 143 | b += v*p.z; 144 | } 145 | return mul(inverse(m), b); // returns 'h' best fits z = h.x * x*x + h.y * y*y + h.z * x*y + h.w hessian and zoffset 146 | } 147 | 148 | 149 | int main(int argc, char *argv[]) try 150 | { 151 | GLWin glwin("dpca - center of mass and covariance of depth data"); 152 | RSCam dcam; 153 | dcam.Init((argc == 2) ? argv[1] : NULL); 154 | glwin.ViewAngle = dcam.fov().y; 155 | Image dimage(dcam.dcamera()); // will hold the depth data that comes from the depth camera 156 | Pose camera = { { 0, 0, 0 },linalg::normalize(float4{ 1, 0, -0.3f, 0 }) }; 157 | float viewdist = 1.0f; 158 | float2 wrange = { 0.125f,0.625f }; 159 | bool pause = false; 160 | bool recording = false; 161 | bool hold = false; 162 | int mode = 2; 163 | SOCKET server = INVALID_SOCKET; 164 | int tcp_port = 12346; 165 | 166 | glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void 167 | { 168 | switch (std::tolower(key)) 169 | { 170 | case 'q': case 27: exit(0) ; break; // 27 is ESC 171 | case ' ': pause = !pause ; break; 172 | case '0': case '1': case '2': case '3': case '4': mode = (key - '0')%4; break; // select current viewing mode 173 | case '[': case'-': case '_': wrange.y /= 1.125f ; break; 174 | case ']': case'=': case '+': wrange.y *= 1.125f ; break; 175 | case 'b': dcam.addbackground(dimage.raster.data()) ; break; 176 | case 'h': server = start_server(tcp_port) ; break; 177 | default: std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n"; break; 178 | } 179 | }; 180 | 181 | if(dcam.dev && dcam.dev->supports_option(rs::option::r200_lr_auto_exposure_enabled)) 182 | dcam.dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1); 183 | float depth_scale = (dcam.dev) ? dcam.dev->get_depth_scale() : 0.001f; // to put into meters // if file assume file is mm 184 | 185 | while (glwin.WindowUp()) 186 | { 187 | if (glwin.MouseState) 188 | { 189 | camera.orientation = qmul(camera.orientation, qconj(VirtualTrackBall(float3(0, 0, 1), float3(0, 0, 0), glwin.OldMouseVector, glwin.MouseVector))); // equation is non-typical we are orbiting the camera, not rotating the object 190 | } 191 | viewdist *= powf(1.1f, (float) glwin.mousewheel); 192 | camera.position = float3(0, 0, sum(wrange) / 2.0f) + qzdir(camera.orientation) * viewdist; 193 | 194 | if (!pause) 195 | dimage = dcam.GetDepth(); 196 | 197 | std::vector pointcloud,outliers; 198 | for (auto p : rect_iteration(dcam.dim())) // p is int2 from 0,0 to w,h of dcam 199 | { 200 | float d = dimage.pixel(p) * depth_scale; // d is in meters, whereas depth[i] is in camera units mm for R200, .125mm for SR300 ivycam 201 | if (d > wrange.y*1.5f) // way outside sample range 202 | continue; 203 | // stuff near edges our outside sample range put into outlier points 204 | (d wrange.y || p.x<5 || p.x> dcam.dim().x - 5 || p.y<5 || p.y>dcam.dim().y - 5 ?outliers:pointcloud).push_back(dcam.deprojectz(asfloat2(p), d)); 205 | } 206 | Pose pa; 207 | float3 va; 208 | std::tie(pa, va) = PrincipalAxes(pointcloud); 209 | auto sd = sqrt(va); // standard deviation (often use 2*sd for that 90%interval) 210 | 211 | if (server != INVALID_SOCKET) 212 | { 213 | do_server_thing(server, ToString() << "pose " << pa); 214 | // do_server_thing_persist(server, ToString() << "pose " << pa); // if the frame rate is slow in a web page then you can try this call which attempts to re-use the same connection. sometimes this fails though. 215 | } 216 | 217 | glPushAttrib(GL_ALL_ATTRIB_BITS); 218 | glViewport(0, 0, glwin.res.x, glwin.res.y); 219 | glClearColor(0.1f, 0.1f, 0.15f, 1); 220 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 221 | 222 | glMatrixMode(GL_PROJECTION); 223 | glPushMatrix(); 224 | glLoadIdentity(); 225 | gluPerspective(glwin.ViewAngle, (double)glwin.aspect_ratio(), 0.01f, 50.0f); 226 | 227 | glMatrixMode(GL_MODELVIEW); 228 | glPushMatrix(); 229 | glEnable(GL_DEPTH_TEST); 230 | glMultMatrixf(camera.inverse().matrix()); 231 | 232 | auto dp_image_c = Transform(dimage, [wrange,depth_scale](unsigned short s) {return byte3((unsigned char)clamp((int)((s*depth_scale-wrange.y)/(wrange.x-wrange.y)*255), 0, 255)); }); 233 | //if(pointcloud.size()) 234 | // dp_image_c.pixel(int2(dp_image_c.cam.projectz(pa.position))) = { 0,255,0 }; 235 | drawimage(dp_image_c, { 0.68f,0.22f }, { 0.3f,-0.2f },3); 236 | 237 | if (mode == 0) 238 | glcolorbox({ 0.2f,0.1f,0.02f },pa); 239 | else if(mode==1) 240 | glGridxy(sd.x*2.0f, { 0.75f,1.0f,1.0f }, pa); 241 | else if (mode == 2) 242 | glellipsoid(sd*2.0f, pa); 243 | else if (mode == 3 && pointcloud.size() > 10) 244 | { 245 | auto localcloud = Transform(pointcloud, [pa](const float3 &v) {return pa.inverse()*v; }); 246 | auto h = ParabloidFit(localcloud); 247 | localcloud = Transform(localcloud, [&h](const float3 &p) { return p - float3(0.0f, 0.0f, h.w); }); // for (auto &p : points) p.z -= h.w; 248 | float2x2 hess = { { h.x,h.z / 2.0f },{ h.z / 2.0f,h.y } }; 249 | float ha = Diagonalizer(hess); 250 | float2x2 hr = { { cosf(ha),sinf(ha) },{ -sinf(ha),cosf(ha) } }; // axes of curvature 251 | auto hd = mul(mul(transpose(hr), hess), hr); // diagonal entries are the maximum and minimum curvatures 252 | glPushMatrix(); 253 | glMultMatrixf(pa.matrix()); 254 | for (int i : {0, 1}) // showing two principal directions of curvature (different than principal axes of the point cloud) 255 | { 256 | glPushAttrib(GL_ALL_ATTRIB_BITS); 257 | glLineWidth(2.0f); 258 | glBegin(GL_LINE_STRIP); 259 | glColor3fv([i]() {float3 c(0.5f); c[i] = 1.0f; return c; }()); 260 | for (float d = -sd.x*2.5f; d <= sd.x*2.5f; d += sd.x / 32) 261 | glVertex3fv(float3(hr[i] * d, hd[i][i] * d*d)); 262 | glEnd(); 263 | glPopAttrib(); 264 | } 265 | glHeightField(-sd.xy()*2.0f, sd.xy()*2.0f, [&hess](float2 p)->float {return dot(p, mul(hess, p)); }); // draws a quad patch using the hessian matrix 266 | glPopMatrix(); 267 | } 268 | glAxis(pa,0.1f); 269 | 270 | glColor3f(1, 1, 1); 271 | glwirefrustumz(dcam.deprojectextents(), wrange ); // draw the camera frustum volume 272 | 273 | glPushAttrib(GL_ALL_ATTRIB_BITS); 274 | glPointSize(1); 275 | glBegin(GL_POINTS); 276 | glColor3f(0, 1, 0.5f); 277 | for (auto p : pointcloud) 278 | glVertex3fv(p); 279 | glColor3f(0.3f,0,0); 280 | for (auto p : outliers) 281 | glVertex3fv(p); 282 | glEnd(); 283 | glPopAttrib(); 284 | glColor3f(1, 1, 1); 285 | 286 | 287 | // Restore state 288 | glMatrixMode(GL_PROJECTION); 289 | glPopMatrix(); 290 | glMatrixMode(GL_MODELVIEW); 291 | glPopMatrix(); 292 | glPopAttrib(); 293 | 294 | glColor3f(1, 1, 1); 295 | const char *mode_description[] = { "Oriented box (fixed size)" , "Plane fit" , "Covariance" , "Hessian (principal curvatures)" }; 296 | glwin.PrintString({ 0,0 }, "Camera %s ", pause ? "paused" : "running"); 297 | glwin.PrintString({ 0,1 }, "Mode %d %s ", mode, mode_description[mode]); 298 | glwin.PrintString({ 0,2 }, "keys 0-3 to change draw mode, -/+ move backplane, space to pause"); 299 | if (server!=INVALID_SOCKET) 300 | glwin.PrintString({ 0,3 }, "HTTP server enabled localhost:%d",tcp_port); 301 | glwin.SwapBuffers(); 302 | 303 | } 304 | return 0; 305 | } 306 | catch (const char *c) 307 | { 308 | MessageBox(GetActiveWindow(),c, "FAIL", 0); 309 | } 310 | catch (std::exception e) 311 | { 312 | MessageBox(GetActiveWindow(), e.what(), "FAIL", 0); 313 | } 314 | -------------------------------------------------------------------------------- /dpca/dpca.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54} 23 | Win32Proj 24 | dpca 25 | 8.1 26 | 27 | 28 | 29 | Application 30 | true 31 | $(DefaultPlatformToolset) 32 | NotSet 33 | 34 | 35 | Application 36 | false 37 | $(DefaultPlatformToolset) 38 | true 39 | NotSet 40 | 41 | 42 | Application 43 | true 44 | $(DefaultPlatformToolset) 45 | NotSet 46 | 47 | 48 | Application 49 | false 50 | $(DefaultPlatformToolset) 51 | true 52 | NotSet 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | true 74 | $(ProjectDir) 75 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 76 | $(ProjectName)_$(Configuration)_$(Platform) 77 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 78 | 79 | 80 | true 81 | $(ProjectDir) 82 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 83 | $(ProjectName)_$(Configuration)_$(Platform) 84 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 85 | 86 | 87 | false 88 | $(ProjectDir) 89 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 90 | $(ProjectName)_$(Configuration)_$(Platform) 91 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 92 | 93 | 94 | false 95 | $(ProjectDir) 96 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 97 | $(ProjectName)_$(Configuration)_$(Platform) 98 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 99 | 100 | 101 | 102 | 103 | 104 | Level3 105 | Disabled 106 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 107 | 108 | 109 | Console 110 | true 111 | 112 | 113 | 114 | 115 | 116 | 117 | Level3 118 | Disabled 119 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 120 | 121 | 122 | Console 123 | true 124 | 125 | 126 | 127 | 128 | Level3 129 | 130 | 131 | MaxSpeed 132 | true 133 | true 134 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 135 | 136 | 137 | Console 138 | true 139 | true 140 | true 141 | 142 | 143 | 144 | 145 | Level3 146 | 147 | 148 | MaxSpeed 149 | true 150 | true 151 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 152 | 153 | 154 | Console 155 | true 156 | true 157 | true 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /dphyspush/dphyspush.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // uses librealsense and depth camera 3 | // 4 | // experimenting with direct point cloud direct interaction 5 | // 6 | 7 | 8 | #include // std::tolower 9 | #include 10 | #include // std::max 11 | 12 | #include 13 | #include 14 | #include 15 | #include "dcam.h" 16 | #include "physics.h" 17 | #include "wingmesh.h" // to easily whip up some content 18 | 19 | #include "misc_gl.h" 20 | #include "misc_image.h" 21 | 22 | 23 | 24 | inline std::vector Planes(const std::vector &verts, const std::vector &tris) { std::vector planes; for (auto &t : tris) planes.push_back(PolyPlane({ verts[t[0]], verts[t[1]], verts[t[2]] })); return planes; } 25 | 26 | inline float4 mostabove(std::vector &planes, const float3 &v) // returns plane which v is above by the largest amount 27 | { 28 | assert(planes.size()); 29 | return *std::max_element(planes.begin(), planes.end(), [&v](const float4 &a, const float4 &b) {return (dot(a, float4(v, 1)) < dot(b, float4(v, 1))); }); 30 | } 31 | 32 | Shape AsShape(const WingMesh &m) { return Shape(m.verts, m.GenerateTris()); } 33 | 34 | 35 | void draw(const Mesh &mesh) 36 | { 37 | glBegin(GL_TRIANGLES); 38 | for (auto t : mesh.tris) for (int j = 0; j < 3; j++) 39 | { 40 | auto &v = mesh.verts[t[j]]; 41 | glNormal3fv(qzdir(v.orientation)); 42 | glTexCoord2fv(v.texcoord); 43 | glVertex3fv(v.position); 44 | } 45 | glEnd(); 46 | } 47 | 48 | 49 | 50 | std::vector ObtainVoxelPointCloud(std::vector & dpts, float voxelSize, int minOccupants) 51 | { 52 | std::vector points; 53 | // Structure for storing per-voxel data 54 | enum { HASH_SIZE = 1024, HASH_MASK = HASH_SIZE - 1 }; // Only works if HASH_SIZE is power of two 55 | struct Voxel { int3 coord; float3 point; int count; }; 56 | Voxel voxelHash[HASH_SIZE]; 57 | memset(voxelHash, 0, sizeof(voxelHash)); 58 | 59 | const float inverseVoxelSize = 1.0f / voxelSize; 60 | static const int3 hashCoeff(7171, 3079, 4231); 61 | 62 | // For the pixels in our image 63 | for (auto &p : dpts) 64 | { 65 | // Obtain corresponding voxel 66 | auto fcoord = floor(p * inverseVoxelSize); 67 | auto vcoord = int3(static_cast(fcoord.x), static_cast(fcoord.y), static_cast(fcoord.z)); 68 | auto hash = dot(vcoord, hashCoeff) & HASH_MASK; 69 | auto & voxel = voxelHash[hash]; 70 | 71 | // If we collide, flush existing voxel contents 72 | if (voxel.count && voxel.coord != vcoord) 73 | { 74 | if (voxel.count > minOccupants) points.push_back(voxel.point / (float)voxel.count); 75 | voxel.count = 0; 76 | } 77 | 78 | // If voxel is empty, store the point 79 | if (voxel.count == 0) 80 | { 81 | voxel.coord = vcoord; 82 | voxel.point = p; 83 | voxel.count = 1; 84 | } 85 | else // Otherwise just add position contribution 86 | { 87 | voxel.point += p; 88 | ++voxel.count; 89 | } 90 | 91 | } 92 | 93 | // Flush remaining voxels 94 | for (auto it = std::begin(voxelHash); it != std::end(voxelHash); ++it) 95 | { 96 | if (it->count > minOccupants) 97 | { 98 | points.push_back(it->point / (float)it->count); 99 | } 100 | } 101 | return points; 102 | } 103 | 104 | 105 | void rbdraw(const RigidBody *rb) 106 | { 107 | glPushMatrix(); 108 | glMultMatrixf(rb->pose().matrix()); 109 | for (const auto &s : rb->shapes) 110 | gldraw(s.verts, s.tris); 111 | glPopMatrix(); 112 | } 113 | 114 | 115 | 116 | int main(int argc, char *argv[]) try 117 | { 118 | 119 | physics_driftmax = 0.0025f; 120 | 121 | GLWin glwin("point cloud push interaction"); 122 | RSCam dcam; 123 | dcam.Init((argc == 2) ? argv[1] : NULL); 124 | Image dimage(dcam.dcamera()); 125 | 126 | glwin.ViewAngle = dcam.fov().y; 127 | float viewdist = 2.0f; 128 | float yaw = 120; 129 | int mousexold = 0; 130 | Mesh mesh; 131 | 132 | bool pause = false; 133 | bool debuglines=false; 134 | int center = 0; 135 | bool chains = true; 136 | bool usehull = false; 137 | std::vector rigidbodies; 138 | std::vector < std::pair> links; 139 | for (float x = -0.2f; x < 0.2f; x+= 0.07f) 140 | for(float z: {0.350f}) 141 | for (float y = -0.2f; y <= 0.2f; y += 0.07f) 142 | { 143 | rigidbodies.push_back(new RigidBody({ AsShape(WingMeshDual(WingMeshCube(0.025f),0.028f)) }, { x,y,z })); 144 | //rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.025f) ) }, { x,y,z })); 145 | links.push_back({(y > -0.2f)?rigidbodies[rigidbodies.size() - 2]:NULL , rigidbodies.back()}); 146 | } 147 | 148 | //rigidbodies.push_back(new RigidBody({ AsShape(WingMeshCube(0.05f)) }, { 0,0,0.50f })); 149 | 150 | auto seesaw = new RigidBody({ AsShape(WingMeshBox({ 0.20f, 0.015f, 0.05f })) }, { 0,0,0.45f }); 151 | rigidbodies.push_back(seesaw); 152 | 153 | glwin.keyboardfunc = [&](unsigned char key, int x, int y)->void 154 | { 155 | switch (std::tolower(key)) 156 | { 157 | case 'q': case 27: exit(0); break; // 27 is ESC 158 | case ' ': pause = !pause; break; 159 | case 'c': chains = !chains; break; 160 | case 'd': debuglines = !debuglines; break; 161 | case 'h': usehull = !usehull; break; 162 | case 'r': for (auto &rb : rigidbodies) { rb->angular_momentum = rb->linear_momentum = float3(0.0f);rb->pose() = { rb->position_start,rb->orientation_start }; } break; 163 | default: std::cout << "unassigned key (" << (int)key << "): '" << key << "'\n"; break; 164 | } 165 | }; 166 | 167 | if (dcam.dev->supports_option(rs::option::r200_lr_auto_exposure_enabled)) 168 | dcam.dev->set_option(rs::option::r200_lr_auto_exposure_enabled, 1); 169 | 170 | while (glwin.WindowUp()) 171 | { 172 | if (glwin.MouseState) 173 | { 174 | yaw += glwin.mousepos.x - mousexold; 175 | } 176 | mousexold = glwin.mousepos.x; 177 | viewdist *= powf(1.1f, (float)glwin.mousewheel); 178 | 179 | if (!pause) 180 | dimage = dcam.GetDepth(); 181 | 182 | glPushAttrib(GL_ALL_ATTRIB_BITS); 183 | glViewport(0, 0, glwin.res.x, glwin.res.y); 184 | glClearColor(0.1f, 0.1f, 0.15f, 1); 185 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 186 | 187 | glMatrixMode(GL_PROJECTION); 188 | glPushMatrix(); 189 | glLoadIdentity(); 190 | gluPerspective(glwin.ViewAngle, (double)glwin.aspect_ratio(), 0.01f, 50.0f); 191 | 192 | glMatrixMode(GL_MODELVIEW); 193 | glPushMatrix(); 194 | gluLookAt(0, 0, viewdist, 0, 0, 0, 0, -1, 0); 195 | glEnable(GL_DEPTH_TEST); 196 | 197 | glTranslatef(0, 0, 0.35f); 198 | glRotatef(yaw, 0, 1, 0); 199 | glTranslatef(0, 0, -0.35f); 200 | 201 | std::vector pts; 202 | std::vector outliers; 203 | std::vector vpts; 204 | glDisable(GL_BLEND); 205 | 206 | float2 wrange = { 0.20f,0.60f }; 207 | 208 | 209 | auto dp_image_c = Transform(dimage, [](unsigned short s) {return byte3((unsigned char)clamp(255 - s / 4, 0, 255)); }); 210 | drawimage(dp_image_c, { 0.78f,0.22f }, { 0.2f,-0.2f }, 3); 211 | 212 | float depth_scale = (dcam.dev) ? dcam.dev->get_depth_scale() : 0.001f; // to put into meters // if file assume file is mm 213 | 214 | 215 | for (auto p : rect_iteration(dimage.dim())) // p is int2 from 0,0 to w,h of dcam 216 | { 217 | float d = dimage.pixel(p) * depth_scale; // d is in meters, whereas depth[i] is in camera units mm for R200, .125mm for SR300 ivycam 218 | if (p.x<5 || p.x> dimage.dim().x - 5 || p.y<5 || p.y>dimage.dim().y - 5) continue; // crop, seems to be lots of noise at the edges 219 | if (d > 1.0f) // just too far 220 | continue; 221 | float3 v = dimage.cam.deprojectz(asfloat2(p), d); 222 | if (d>wrange.x && d < wrange.y) 223 | pts.push_back(v); 224 | else 225 | outliers.push_back(v); 226 | } 227 | 228 | vpts = ObtainVoxelPointCloud(pts, 0.0082f, 8); 229 | 230 | 231 | std::vector> lines; 232 | std::vector> glines; 233 | 234 | if (1)// && pts.size()) 235 | { 236 | std::vector linears; 237 | std::vector angulars; 238 | physics_gravity = { 0, (float) chains,0 }; // ugg y is down 239 | 240 | if(!usehull) for(auto rb:rigidbodies) 241 | { 242 | if (!rb->shapes[0].planes.size()) 243 | rb->shapes[0].planes = Planes(rb->shapes[0].verts, rb->shapes[0].tris); 244 | auto planes = Transform(rb->shapes[0].planes, [&](float4 p) { return rb->pose().TransformPlane(p);}); 245 | rb->gravscale = (float)chains; 246 | float separation = FLT_MAX; 247 | float3 pushpoint = float3(0, 0, 0); // 248 | float4 pushplane; 249 | for (auto p : vpts) 250 | { 251 | auto plane = mostabove(planes, p); 252 | float sep; 253 | if ((sep = dot(plane, float4(p, 1))) < separation) 254 | { 255 | pushpoint = p; 256 | pushplane = plane; 257 | separation = sep; 258 | } 259 | } 260 | if (separation > 0.1f) 261 | continue; 262 | float3 closestpoint = ProjectOntoPlane(pushplane, pushpoint); 263 | pushplane = float4({ -pushplane.xyz(), -dot(-pushplane.xyz(),pushpoint) }); 264 | linears.push_back(ConstrainAlongDirection(NULL, pushpoint, rb, rb->pose().inverse()*closestpoint, pushplane.xyz(), 0, 100.0f)); // FLT_MAX)); 265 | lines.push_back({ closestpoint,pushpoint }); 266 | auto cp=Separated(rb->shapes[0].verts, rb->position, rb->orientation, { pushpoint }, { 0,0,0 }, { 0,0,0,1 }, 1); 267 | glines.push_back({ cp.p0w, cp.p1w }); 268 | } 269 | Append(linears, ConstrainPositionNailed(NULL, seesaw->position_start, seesaw, { 0, 0, 0 })); 270 | Append(angulars, ConstrainAngularRange(NULL, seesaw, { 0, 0, 0, 1 }, { 0, 0,-20 }, { 0, 0,20 })); 271 | 272 | if (chains) for (auto link : links) 273 | Append(linears, ConstrainPositionNailed(link.first,link.first? float3(0, 0.035f, 0) : link.second->position_start-float3(0, -0.035f, 0) , link.second, { 0,-0.035f,0 })); 274 | if(!pause) 275 | 276 | if(usehull && vpts.size()>5) 277 | PhysicsUpdate(rigidbodies, linears, angulars, { &vpts }); 278 | else 279 | PhysicsUpdate(rigidbodies, linears, angulars, std::vector*>()); 280 | } 281 | 282 | 283 | glColor3f(1, 1, 1); 284 | glwirefrustumz(dcam.deprojectextents(), { 0.1f,1.0f }); // draw the camera frustum volume 285 | 286 | glPushAttrib(GL_ALL_ATTRIB_BITS); 287 | glPointSize(1); 288 | glBegin(GL_POINTS); 289 | glColor3f(0, 1, 0.5f); 290 | for (auto p : pts) 291 | glVertex3fv(p); 292 | glColor3f(1, 0.15f, 0.15f); 293 | for (auto p : outliers) 294 | glVertex3fv(p); 295 | glEnd(); 296 | 297 | glPointSize(3); 298 | glBegin(GL_POINTS); 299 | glColor3f(1, 1, 1); 300 | for (auto p : vpts) // was: spts 301 | glVertex3fv(p); 302 | glEnd(); 303 | 304 | glPopAttrib(); 305 | 306 | if (debuglines) 307 | { 308 | glBegin(GL_LINES); 309 | glColor3f(0, 1, 1); 310 | if (0)for (auto line : lines) 311 | glVertex3fv(line.first), glVertex3fv(line.second); 312 | glColor3f(1, 1, 0); 313 | for (auto line : glines) 314 | glVertex3fv(line.first), glVertex3fv(line.second); 315 | glEnd(); 316 | } 317 | if (usehull && vpts.size() > 5) 318 | { 319 | auto tris = calchull(vpts, 0); 320 | glBegin(GL_LINES); 321 | glColor3f(1, 1, 1); 322 | for (auto t : tris) for( int i : {0,1,1,2,2,0}) 323 | glVertex3fv(vpts[t[i]]); 324 | glEnd(); 325 | 326 | } 327 | if (chains) 328 | { 329 | glBegin(GL_LINES); 330 | glColor3f(1, 0, 1); 331 | for (auto link : links) 332 | { 333 | if(link.first) 334 | glVertex3fv(link.first->pose()* float3(0, 0, 0)), glVertex3fv(link.first->pose()* float3(0, 0.035f, 0)); 335 | glVertex3fv(link.second->pose()* float3(0, 0, 0)) , glVertex3fv(link.second->pose()* float3(0, -0.035f, 0)); 336 | } 337 | glEnd(); 338 | } 339 | glPushAttrib(GL_ALL_ATTRIB_BITS); 340 | glEnable(GL_LIGHTING); 341 | glEnable(GL_LIGHT0); 342 | glEnable(GL_TEXTURE_2D); 343 | glColor3f(0.5f, 0.5f, 0.5f); 344 | for (auto &rb : rigidbodies) 345 | rbdraw(rb); 346 | glPopAttrib(); // Restore state 347 | 348 | // Restore state 349 | glPopMatrix(); //should be currently in modelview mode 350 | glMatrixMode(GL_PROJECTION); 351 | glPopMatrix(); 352 | glPopAttrib(); 353 | glMatrixMode(GL_MODELVIEW); 354 | 355 | glwin.PrintString({ 0,0 }, "esc to quit."); 356 | glwin.PrintString({ 0,1 }, "[h] collision %s ",(usehull)?"hull":"points"); 357 | glwin.SwapBuffers(); 358 | 359 | } 360 | return 0; 361 | } 362 | catch (const char *c) 363 | { 364 | MessageBox(GetActiveWindow(), c, "FAIL", 0); 365 | } 366 | catch (std::exception e) 367 | { 368 | MessageBox(GetActiveWindow(), e.what(), "FAIL", 0); 369 | } 370 | -------------------------------------------------------------------------------- /dphyspush/dphyspush.vcxproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | Debug 14 | x64 15 | 16 | 17 | Release 18 | x64 19 | 20 | 21 | 22 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3} 23 | Win32Proj 24 | dphyspush 25 | 8.1 26 | 27 | 28 | 29 | Application 30 | true 31 | $(DefaultPlatformToolset) 32 | NotSet 33 | 34 | 35 | Application 36 | false 37 | $(DefaultPlatformToolset) 38 | true 39 | NotSet 40 | 41 | 42 | Application 43 | true 44 | $(DefaultPlatformToolset) 45 | NotSet 46 | 47 | 48 | Application 49 | false 50 | $(DefaultPlatformToolset) 51 | true 52 | NotSet 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | true 74 | $(ProjectDir) 75 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 76 | $(ProjectName)_$(Configuration)_$(Platform) 77 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 78 | 79 | 80 | true 81 | $(ProjectDir) 82 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 83 | $(ProjectName)_$(Configuration)_$(Platform) 84 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 85 | 86 | 87 | false 88 | $(ProjectDir) 89 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 90 | $(ProjectName)_$(Configuration)_$(Platform) 91 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 92 | 93 | 94 | false 95 | $(ProjectDir) 96 | $(SolutionDir)\obj\$(ProjectName)\$(Configuration)_$(Platform)\ 97 | $(ProjectName)_$(Configuration)_$(Platform) 98 | ../include/;../third_party/sandbox/include/;$(VC_IncludePath);$(WindowsSDK_IncludePath); 99 | 100 | 101 | 102 | 103 | 104 | Level3 105 | Disabled 106 | WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) 107 | 108 | 109 | Console 110 | true 111 | 112 | 113 | 114 | 115 | 116 | 117 | Level3 118 | Disabled 119 | _DEBUG;_CONSOLE;%(PreprocessorDefinitions) 120 | 121 | 122 | Console 123 | true 124 | 125 | 126 | 127 | 128 | Level3 129 | 130 | 131 | MaxSpeed 132 | true 133 | true 134 | WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 135 | 136 | 137 | Console 138 | true 139 | true 140 | true 141 | 142 | 143 | 144 | 145 | Level3 146 | 147 | 148 | MaxSpeed 149 | true 150 | true 151 | NDEBUG;_CONSOLE;%(PreprocessorDefinitions) 152 | 153 | 154 | Console 155 | true 156 | true 157 | true 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /dsamples_vs2013.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2013 4 | VisualStudioVersion = 12.0.40629.0 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "dpca", "dpca\dpca.vcxproj", "{C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}" 7 | ProjectSection(ProjectDependencies) = postProject 8 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 9 | EndProjectSection 10 | EndProject 11 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "realsense-s", "third_party\librealsense\librealsense.vc12\realsense-s\realsense-s.vcxproj", "{622186C4-6D7D-4288-B8FD-8F08419181C2}" 12 | EndProject 13 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "dclassify", "dclassify\dclassify.vcxproj", "{78461E47-3F4A-4AA3-8455-7D2E5366618C}" 14 | ProjectSection(ProjectDependencies) = postProject 15 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 16 | EndProjectSection 17 | EndProject 18 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "load_trained_cnn", "dclassify\load_trained_cnn.vcxproj", "{DE74729B-1644-4955-8C7B-1A2CAC271B75}" 19 | ProjectSection(ProjectDependencies) = postProject 20 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 21 | EndProjectSection 22 | EndProject 23 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "dphyspush", "dphyspush\dphyspush.vcxproj", "{E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}" 24 | ProjectSection(ProjectDependencies) = postProject 25 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 26 | EndProjectSection 27 | EndProject 28 | Global 29 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 30 | Debug|Win32 = Debug|Win32 31 | Debug|x64 = Debug|x64 32 | Release|Win32 = Release|Win32 33 | Release|x64 = Release|x64 34 | EndGlobalSection 35 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 36 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|Win32.ActiveCfg = Debug|Win32 37 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|Win32.Build.0 = Debug|Win32 38 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|x64.ActiveCfg = Debug|x64 39 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|x64.Build.0 = Debug|x64 40 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|Win32.ActiveCfg = Release|Win32 41 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|Win32.Build.0 = Release|Win32 42 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|x64.ActiveCfg = Release|x64 43 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|x64.Build.0 = Release|x64 44 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|Win32.ActiveCfg = Debug|Win32 45 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|Win32.Build.0 = Debug|Win32 46 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|x64.ActiveCfg = Debug|x64 47 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|x64.Build.0 = Debug|x64 48 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|Win32.ActiveCfg = Release|Win32 49 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|Win32.Build.0 = Release|Win32 50 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|x64.ActiveCfg = Release|x64 51 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|x64.Build.0 = Release|x64 52 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|Win32.ActiveCfg = Debug|Win32 53 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|Win32.Build.0 = Debug|Win32 54 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|x64.ActiveCfg = Debug|x64 55 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|x64.Build.0 = Debug|x64 56 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|Win32.ActiveCfg = Release|Win32 57 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|Win32.Build.0 = Release|Win32 58 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|x64.ActiveCfg = Release|x64 59 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|x64.Build.0 = Release|x64 60 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|Win32.ActiveCfg = Debug|Win32 61 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|Win32.Build.0 = Debug|Win32 62 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|x64.ActiveCfg = Debug|x64 63 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|x64.Build.0 = Debug|x64 64 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|Win32.ActiveCfg = Release|Win32 65 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|Win32.Build.0 = Release|Win32 66 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|x64.ActiveCfg = Release|x64 67 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|x64.Build.0 = Release|x64 68 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|Win32.ActiveCfg = Debug|Win32 69 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|Win32.Build.0 = Debug|Win32 70 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|x64.ActiveCfg = Debug|x64 71 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|x64.Build.0 = Debug|x64 72 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|Win32.ActiveCfg = Release|Win32 73 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|Win32.Build.0 = Release|Win32 74 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|x64.ActiveCfg = Release|x64 75 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|x64.Build.0 = Release|x64 76 | EndGlobalSection 77 | GlobalSection(SolutionProperties) = preSolution 78 | HideSolutionNode = FALSE 79 | EndGlobalSection 80 | EndGlobal 81 | -------------------------------------------------------------------------------- /dsamples_vs2015.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 14 4 | VisualStudioVersion = 14.0.24720.0 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "dpca", "dpca\dpca.vcxproj", "{C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}" 7 | ProjectSection(ProjectDependencies) = postProject 8 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 9 | EndProjectSection 10 | EndProject 11 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "realsense-s", "third_party\librealsense\librealsense.vc14\realsense-s\realsense-s.vcxproj", "{622186C4-6D7D-4288-B8FD-8F08419181C2}" 12 | EndProject 13 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "dclassify", "dclassify\dclassify.vcxproj", "{78461E47-3F4A-4AA3-8455-7D2E5366618C}" 14 | ProjectSection(ProjectDependencies) = postProject 15 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 16 | EndProjectSection 17 | EndProject 18 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "load_trained_cnn", "dclassify\load_trained_cnn.vcxproj", "{DE74729B-1644-4955-8C7B-1A2CAC271B75}" 19 | ProjectSection(ProjectDependencies) = postProject 20 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 21 | EndProjectSection 22 | EndProject 23 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "dphyspush", "dphyspush\dphyspush.vcxproj", "{E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}" 24 | ProjectSection(ProjectDependencies) = postProject 25 | {622186C4-6D7D-4288-B8FD-8F08419181C2} = {622186C4-6D7D-4288-B8FD-8F08419181C2} 26 | EndProjectSection 27 | EndProject 28 | Global 29 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 30 | Debug|Win32 = Debug|Win32 31 | Debug|x64 = Debug|x64 32 | Release|Win32 = Release|Win32 33 | Release|x64 = Release|x64 34 | EndGlobalSection 35 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 36 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|Win32.ActiveCfg = Debug|Win32 37 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|Win32.Build.0 = Debug|Win32 38 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|x64.ActiveCfg = Debug|x64 39 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Debug|x64.Build.0 = Debug|x64 40 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|Win32.ActiveCfg = Release|Win32 41 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|Win32.Build.0 = Release|Win32 42 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|x64.ActiveCfg = Release|x64 43 | {622186C4-6D7D-4288-B8FD-8F08419181C2}.Release|x64.Build.0 = Release|x64 44 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|Win32.ActiveCfg = Debug|Win32 45 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|Win32.Build.0 = Debug|Win32 46 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|x64.ActiveCfg = Debug|x64 47 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Debug|x64.Build.0 = Debug|x64 48 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|Win32.ActiveCfg = Release|Win32 49 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|Win32.Build.0 = Release|Win32 50 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|x64.ActiveCfg = Release|x64 51 | {78461E47-3F4A-4AA3-8455-7D2E5366618C}.Release|x64.Build.0 = Release|x64 52 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|Win32.ActiveCfg = Debug|Win32 53 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|Win32.Build.0 = Debug|Win32 54 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|x64.ActiveCfg = Debug|x64 55 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Debug|x64.Build.0 = Debug|x64 56 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|Win32.ActiveCfg = Release|Win32 57 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|Win32.Build.0 = Release|Win32 58 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|x64.ActiveCfg = Release|x64 59 | {DE74729B-1644-4955-8C7B-1A2CAC271B75}.Release|x64.Build.0 = Release|x64 60 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|Win32.ActiveCfg = Debug|Win32 61 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|Win32.Build.0 = Debug|Win32 62 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|x64.ActiveCfg = Debug|x64 63 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Debug|x64.Build.0 = Debug|x64 64 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|Win32.ActiveCfg = Release|Win32 65 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|Win32.Build.0 = Release|Win32 66 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|x64.ActiveCfg = Release|x64 67 | {E59B16B0-3F9C-4526-A7C3-77395C8FDBF3}.Release|x64.Build.0 = Release|x64 68 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|Win32.ActiveCfg = Debug|Win32 69 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|Win32.Build.0 = Debug|Win32 70 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|x64.ActiveCfg = Debug|x64 71 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Debug|x64.Build.0 = Debug|x64 72 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|Win32.ActiveCfg = Release|Win32 73 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|Win32.Build.0 = Release|Win32 74 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|x64.ActiveCfg = Release|x64 75 | {C3D0AEE8-AD80-4F00-BB79-DF47E61DCC54}.Release|x64.Build.0 = Release|x64 76 | EndGlobalSection 77 | GlobalSection(SolutionProperties) = preSolution 78 | HideSolutionNode = FALSE 79 | EndGlobalSection 80 | EndGlobal 81 | -------------------------------------------------------------------------------- /include/dcam.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef RS_CAM_H 4 | #define RS_CAM_H 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include "geometric.h" 14 | #include "misc.h" 15 | #include "misc_image.h" 16 | 17 | #include "../third_party/librealsense/include/librealsense/rs.hpp" 18 | #include "../third_party/librealsense/include/librealsense/rsutil.h" 19 | 20 | #if (_MSC_VER >= 1900) 21 | #ifdef _WIN64 22 | #ifdef _DEBUG 23 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc14/realsense-s/obj/Debug-x64/realsense-sd.lib") 24 | #else // release x64 25 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc14/realsense-s/obj/Release-x64/realsense-s.lib") 26 | #endif // _DEBUG vs release for x64 27 | #else // we have _WIN32 28 | #ifdef _DEBUG 29 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc14/realsense-s/obj/Debug-Win32/realsense-sd.lib") 30 | #else // release 32 31 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc14/realsense-s/obj/Release-Win32/realsense-s.lib") // pragma easier than using settings within project properties 32 | #endif // _DEBUG vs release for Win32 33 | #endif // _WIN64 vs _WIN32 34 | #else 35 | #ifdef _WIN64 36 | #ifdef _DEBUG 37 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc12/realsense-s/obj/Debug-x64/realsense-sd.lib") 38 | #else // release x64 39 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc12/realsense-s/obj/Release-x64/realsense-s.lib") 40 | #endif // _DEBUG vs release for x64 41 | #else // we have _WIN32 42 | #ifdef _DEBUG 43 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc12/realsense-s/obj/Debug-Win32/realsense-sd.lib") 44 | #else // release 32 45 | #pragma comment(lib, "../third_party/librealsense/librealsense.vc12/realsense-s/obj/Release-Win32/realsense-s.lib") // pragma easier than using settings within project properties 46 | #endif // _DEBUG vs release for Win32 47 | #endif // _WIN64 vs _WIN32 48 | #endif 49 | 50 | class DCam // generic intrinsics depth camera interface 51 | { 52 | public: 53 | rs::intrinsics zintrin; 54 | float depth_scale = 0.001f; 55 | float get_depth_scale(){ return depth_scale; } 56 | const int2& dim() const { return *(reinterpret_cast(&zintrin.width)); } 57 | int2& dim() { return *(reinterpret_cast< int2* >(&zintrin.width)); } 58 | const float2& focal() const { return *(reinterpret_cast(&zintrin.fx)); } 59 | float2& focal() { return *(reinterpret_cast< float2*>(&zintrin.fx)); } 60 | const float2& principal()const { return *(reinterpret_cast(&zintrin.ppx)); } 61 | float2& principal() { return *(reinterpret_cast< float2*>(&zintrin.ppx)); } 62 | 63 | float2 fov() const { return{ zintrin.hfov(), zintrin.vfov() }; } // in degrees 64 | inline float3 deprojectz(float2 p, float d) const { return (const float3 &)zintrin.deproject({p.x, p.y}, d); } 65 | inline float3 deprojectz(int2 p, unsigned short d) const { return deprojectz(float2((float)p.x, (float)p.y), d); } 66 | inline float3 deprojectz(int x, int y, unsigned short d)const { return deprojectz(float2((float)x , (float)y ), d); } 67 | inline float3 deprojectz(int x, int y, float d) const { return deprojectz(float2((float)x , (float)y ), d); } 68 | inline float2 project(const float3 &v) const { return (float2&) zintrin.project((const rs::float3&) v); } 69 | inline float2x2 deprojectextents() const { return float2x2(deprojectz(float2(0, 0), 1.0f).xy(), deprojectz(asfloat2(dim()), 1.0f).xy()); } 70 | std::istream& intrinsicsimport(std::istream &is) { return is >> dim() >> focal() >> principal() >> depth_scale; } 71 | std::ostream& intrinsicsexport(std::ostream &os) const { return os << dim() << " " << focal() << " " << principal() << " " << depth_scale; } 72 | friend std::istream& operator>>(std::istream &is, DCam &dcam) { return dcam.intrinsicsimport(is); } 73 | friend std::ostream& operator<<(std::ostream &os, const DCam &dcam) { return dcam.intrinsicsexport(os); } 74 | void intrinsicsimport(std::string filename) 75 | { 76 | if (filename=="") 77 | return; 78 | std::ifstream fs(filename.c_str()); 79 | if (!fs.is_open()) 80 | return; 81 | fs >> *this; 82 | } 83 | void intrinsicsexport(std::string filename) const 84 | { 85 | std::ofstream camfile(filename.c_str(), std::ios_base::trunc | std::ios_base::out); 86 | if (!camfile.is_open()) 87 | throw filename + " unable to open intrinsics output file"; 88 | camfile << *this << " " << fov() << " // w h fx fy px py depth_scale fovx fovy (degrees)\n"; 89 | } 90 | DCamera dcamera() { return { dim(), focal(), principal() ,get_depth_scale() }; } 91 | }; 92 | 93 | class RSCam : public DCam // wrapper allows for easy interchange between device and file stream 94 | { 95 | public: 96 | rs::context ctx; 97 | rs::device * dev; 98 | 99 | std::ifstream filein; 100 | std::vector fbuffer; 101 | bool enable_filter_depth = true; 102 | 103 | std::vector filtered_depth; 104 | std::vector background; 105 | void addbackground(const unsigned short *dp,unsigned short fudge=3) 106 | { 107 | background.resize(product(dim()),4096); 108 | for (int i = 0; i < dim().x*dim().y; i++) 109 | background[i] = std::min(background[i], (unsigned short) (dp[i]-fudge)); 110 | } 111 | 112 | Image FilterDS4(unsigned short *dp) // modified in-place very ds4 specific 113 | { 114 | Image filtered_depth(dcamera(),std::vector(dp, dp + product(dim()))); 115 | dp = filtered_depth.raster.data(); 116 | auto dleft = (const uint8_t *)dev->get_frame_data(rs::stream::infrared); 117 | // depth_scale = dev->get_depth_scale() ; <- to do the math in meters use this term and dont assume native is mm 118 | auto hasneighbor = [](unsigned short *p, int stride) ->bool { return (std::abs(p[stride] - p[0]) < 10 || std::abs(p[-stride] - p[0]) < 10); }; 119 | for (int i = 0; i < dim().x*dim().y; i++) 120 | { 121 | if(dp[i]<30 || dleft[i] <8) // ignore dark pixels - these could be background, but stereo match put them in foreground 122 | { 123 | dp[i] = 4096; 124 | } 125 | } 126 | for (int i = 0; i < dim().x*dim().y; i++) // for (int2 p: rect_iteration(dim())) 127 | { 128 | int x = i%dim().x, y = i / dim().x; 129 | if (x < 2 || x >= dim().x - 2 || y < 2 || y >= dim().y - 2) 130 | continue; 131 | if ( !hasneighbor(dp + i, 1) || !hasneighbor(dp + i, dim().x) || !hasneighbor(dp + i, 2) || !hasneighbor(dp + i, dim().x * 2)) // ignore flying pixels 132 | { 133 | dp[i] = 4096; 134 | } 135 | } 136 | for (int i = 0; i < dim().x*dim().y; i++) 137 | { 138 | if (background.size() == dim().x*dim().y) 139 | if (dp[i]>background[i]) 140 | dp[i] = 4096; 141 | } 142 | return filtered_depth; 143 | } 144 | Image FilterIvy(unsigned short *dp) // modified in-place very ds4 specific 145 | { 146 | Image filtered_depth(dcamera(), std::vector(dp, dp + product(dim()))); 147 | dp = filtered_depth.raster.data(); 148 | for (int i = 0; i < product(dim()); i++) 149 | { 150 | if (dp[i] == 0) 151 | { 152 | dp[i] = (unsigned short) (4.0f / dev->get_depth_scale()); 153 | } 154 | } 155 | return filtered_depth; 156 | } 157 | Image GetFileDepth() 158 | { 159 | Image image(dcamera(), std::vector(product(dim()))); 160 | filein.read((char*)image.raster.data(), sizeof(unsigned short)*image.raster.size()); 161 | return image; 162 | } 163 | Image ToImage(unsigned short *p) 164 | { 165 | Image image(dcamera() , std::vector(p, p + product(dim()))); 166 | return image; 167 | } 168 | Image GetDepth() 169 | { 170 | if (filein.is_open()) return GetFileDepth(); 171 | dev->wait_for_frames(); 172 | auto rawdepth = (uint16_t *)dev->get_frame_data(rs::stream::depth); 173 | return (enable_filter_depth)? ((dev->get_stream_mode_count(rs::stream::infrared2))?FilterDS4(rawdepth):FilterIvy(rawdepth)): ToImage(rawdepth); // filter is r200 specific 174 | } 175 | std::string CamName() { return (dev) ? ((dev->get_stream_mode_count(rs::stream::infrared2)) ? "dscam" : "ivycam") : "filecam"; } 176 | 177 | inline bool Init() 178 | { 179 | if (ctx.get_device_count() == 0) throw std::runtime_error("No device found"); 180 | dev = ctx.get_device(0); 181 | std::cout << "Found Device: '" << dev->get_name() << "'" << std::endl; 182 | std::cout << "Firmware version: " << dev->get_firmware_version() << std::endl; 183 | std::cout << "Serial number: " << dev->get_serial() << std::endl; 184 | try 185 | { 186 | dev->enable_stream(rs::stream::depth, 320, 240, rs::format::z16, 60); 187 | dev->enable_stream(rs::stream::infrared, 0, 0, rs::format::y8, 0); 188 | //dev->enable_stream(rs::stream::infrared2, 0, 0, rs::format::y8, 0); 189 | dev->enable_stream(rs::stream::color, 640, 480, rs::format::rgb8, 30); 190 | dev->start(); 191 | } 192 | catch (...) 193 | { 194 | dev->enable_stream(rs::stream::depth, 640, 480, rs::format::z16, 60); 195 | dev->enable_stream(rs::stream::infrared, 0, 0, rs::format::y8, 0); 196 | dev->start(); 197 | } 198 | //rs_apply_ivcam_preset((rs_device*)dev, 6); // from leo: more raw IVCAM data (always 60Hz!) 199 | 200 | zintrin = dev->get_stream_intrinsics(rs::stream::depth); 201 | depth_scale = dev->get_depth_scale(); 202 | // leo suggested: rs_apply_depth_control_preset((rs_device*)dev, 5); 203 | std::cout << "ds4 dims: " << *this << " " << fov() << " // w h fx fy px py depth_scale fovx fovy (degrees)\n"; 204 | 205 | 206 | return true; 207 | } 208 | inline bool Init(const char *filename_) 209 | { 210 | if (!filename_ || !*filename_) 211 | return Init(); 212 | filein.open(filename_, std::ios_base::binary | std::ios_base::in); 213 | if (!filein.is_open()) 214 | throw(std::exception((std::string("unable to open depth camera file: ")+filename_).c_str())); 215 | intrinsicsimport(std::string(filename_) +"i"); // look for intrinsics file 216 | fbuffer.resize(dim().x*dim().y); 217 | return true; 218 | } 219 | RSCam() :dev(nullptr) 220 | { 221 | } 222 | }; 223 | 224 | #endif 225 | -------------------------------------------------------------------------------- /include/gl_gui.h: -------------------------------------------------------------------------------- 1 | // 2 | // minimal gui support 3 | // 4 | // enough of an immediate mode gui to break up opengl window into resizable panels. 5 | // this is gl only and depends on glwin, opengl wrapper. 6 | // first quick attempt, the design/api needs some rethinking, sorry bout that. 7 | // 8 | 9 | #pragma once 10 | #ifndef GL_GUI_H 11 | #define GL_GUI_H 12 | 13 | #include 14 | 15 | #include "geometric.h" 16 | #include "glwin.h" 17 | #include "misc_gl.h" // for drawimage() 18 | 19 | //---------- mesh draw gl -------------- 20 | 21 | 22 | class GUI // simple immediate mode gui 23 | { 24 | struct Partitioning : public std::vector 25 | { 26 | GUI &gui; 27 | Partitioning() = delete; 28 | Partitioning(const Partitioning& rhs):std::vector(rhs),gui(rhs.gui) { gui.bounds.push_back(gui.bounds.back()); } 29 | Partitioning(GUI &gui,std::vector regions) :std::vector(regions),gui(gui) { gui.bounds.push_back(gui.bounds.back()); } 30 | ~Partitioning() { gui.pop(); } 31 | }; 32 | public: 33 | Partitioning partition_y(int n) 34 | { 35 | int2 subdims = this->dims() / int2(1, n) - int2(0, 1); 36 | std::vector regions; 37 | for (int i = 0; i < n;i++) 38 | regions.push_back({ {offset().x,offset().y + (subdims.y + 1)*i},subdims }); 39 | return Partitioning(*this, regions); 40 | } 41 | GLWin &glwin; 42 | void * &selection; 43 | int2 mouse() { return{ glwin.mousepos.x, glwin.res.y - glwin.mousepos.y }; } 44 | int2 mouse_prev() { return{ glwin.mousepos_previous.x, glwin.res.y - glwin.mousepos_previous.y }; } 45 | std::vector bounds; // offset, size; 46 | int2 &dims() { return bounds.back()[1]; } 47 | int2 &offset() { return bounds.back()[0]; } 48 | float2 tolocal(const int2& v) { return (asfloat2(v - offset())/ asfloat2(dims()));} 49 | float2 mousef() { return tolocal(mouse()); } 50 | float3 deproject(float2 p) { p = (p - float2(0.5f, 0.5f))* float2(2.0f*dims().x / dims().y, 2.0f); float fl = tan(glwin.ViewAngle / 2.0f*3.14f / 180.0f); return normalize(float3(p* fl, -1.0f)); } 51 | bool inview(void *c = NULL) { return (selection) ? (selection == c) : within_range(mouse(), offset(), offset() + dims() - int2(1, 1)); } 52 | bool focus(void *c = NULL) { if (inview(c) && (glwin.downevent||glwin.MouseState))selection = c; return selection == c; } 53 | void set(int2x2 &b) 54 | { 55 | bounds.back() = b; 56 | set(); 57 | } 58 | void set() 59 | { 60 | if (bounds.size() == 0) 61 | bounds.push_back({ { 0, 0}, glwin.res }); 62 | bounds.back()[1] = max(int2(0,0), bounds.back()[1]); 63 | glViewport(bounds.back()[0].x, bounds.back()[0].y, bounds.back()[1].x, bounds.back()[1].y); 64 | glScissor (bounds.back()[0].x, bounds.back()[0].y, bounds.back()[1].x, bounds.back()[1].y); 65 | } 66 | void pop() 67 | { 68 | bounds.pop_back(); 69 | set(); 70 | } 71 | GUI(GLWin &glwin, void *&s, float4 c = { 0,0,0,0 }) :glwin(glwin), selection(s) 72 | { 73 | if (!glwin.MouseState) s = NULL; // not sure 74 | bounds.push_back({ { 0, 0}, glwin.res }); 75 | set(); 76 | glClearColor(c.x,c.y,c.z,c.w); 77 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 78 | glPushAttrib(GL_ALL_ATTRIB_BITS); 79 | glEnable(GL_SCISSOR_TEST); 80 | 81 | // Set up the viewport 82 | 83 | // Set up matrices 84 | glMatrixMode(GL_PROJECTION); 85 | glPushMatrix(); 86 | glLoadIdentity(); 87 | perspective(); 88 | 89 | glMatrixMode(GL_MODELVIEW); 90 | glPushMatrix(); 91 | 92 | } 93 | ~GUI() 94 | { 95 | glMatrixMode(GL_PROJECTION); 96 | glPopMatrix(); 97 | glMatrixMode(GL_MODELVIEW); 98 | glPopMatrix(); //leave things in modelview mode 99 | glPopAttrib(); 100 | glwin.PrintString({ 0, 0 }, "esc to quit."); 101 | glwin.SwapBuffers(); 102 | } 103 | void start() 104 | { 105 | 106 | } 107 | std::vector stack; 108 | int2x2 splityc(int y,int r=1) 109 | { 110 | auto b = bounds.back(); 111 | bounds.pop_back(); // wont need to ever go back to this. 112 | bounds.push_back({ { b[0].x,b[0].y + y + r},{b[1].x,b[1].y - y - r } }); 113 | bounds.push_back({ { b[0].x,b[0].y },{b[1].x,y - r } }); 114 | set(); 115 | return b; 116 | } 117 | void splity(int &y, int r = 2) 118 | { 119 | auto b = splityc(y, r); 120 | bounds.push_back({ { b[0].x,b[0].y + y - r},{b[1].x,r * 2 } }); 121 | if (focus(&y)) 122 | y = mouse().y - b[0].y; // mouse in local 123 | y = clamp(y, r, b[1].y - 1 - r); 124 | if (r) 125 | { 126 | set(); 127 | glClearColor((float)(focus(&y)), (float)(inview(&y)), 0.5f, 1); 128 | glClear(GL_COLOR_BUFFER_BIT); 129 | } 130 | pop(); 131 | } 132 | int2x2 splitync(int y, int r = 1) 133 | { 134 | auto b = bounds.back(); 135 | bounds.pop_back(); // wont need to ever go back to this. 136 | bounds.push_back({ { b[0].x,b[0].y },{b[1].x,b[1].y - y - r } }); 137 | bounds.push_back({ { b[0].x,b[0].y + b[1].y - y + r},{b[1].x,y - r } }); 138 | set(); 139 | return b; 140 | } 141 | void splityn(int &y, int r = 2) // find a better way to combine with prev functino 142 | { 143 | auto b = splitync(y,r); 144 | bounds.push_back({ { b[0].x,b[0].y + b[1].y - y - r},{b[1].x,r * 2 }}); 145 | if (focus(&y)) 146 | y = b[1].y-(mouse().y - b[0].y); // mouse in local 147 | y = clamp(y, r, b[1].y - 1 - r); 148 | if (r) 149 | { 150 | set(); 151 | glClearColor((float)(focus(&y)), (float)(inview(&y)), 0.5f, 1); 152 | glClear(GL_COLOR_BUFFER_BIT); 153 | } 154 | pop(); 155 | } 156 | int2x2 splitxc(int x, int r = 1) 157 | { 158 | auto b = bounds.back(); 159 | bounds.pop_back(); // wont need to ever go back to this. 160 | bounds.push_back({ { b[0].x + x + r,b[0].y} ,{b[1].x - x - r,b[1].y }}); 161 | bounds.push_back({ { b[0].x,b[0].y},{x - r,b[1].y } }); 162 | set(); 163 | return b; 164 | } 165 | void splitx(int &x, int r = 2) 166 | { 167 | auto b = splitxc(x, r); 168 | bounds.push_back({ { b[0].x + x - r,b[0].y},{r * 2,b[1].y } }); 169 | if (focus(&x)) 170 | x = mouse().x - b[0].x; // mouse in local 171 | x = clamp(x, r, b[1].x - 1 - r); 172 | if (r) 173 | { 174 | set(); 175 | glClearColor((float)(focus(&x)), (float)(inview(&x)), 0.5f, 1); 176 | glClear(GL_COLOR_BUFFER_BIT); 177 | } 178 | pop(); 179 | } 180 | void ortho() 181 | { 182 | glMatrixMode(GL_PROJECTION); 183 | glLoadIdentity(); 184 | glOrtho(0.0, 1.0, 0.0, 1.0, -1.0, 1.0); 185 | glMatrixMode(GL_MODELVIEW); 186 | glLoadIdentity(); 187 | } 188 | void perspective() 189 | { 190 | glMatrixMode(GL_PROJECTION); 191 | glLoadIdentity(); 192 | gluPerspective(glwin.ViewAngle, (double)dims().x / dims().y, 0.01, 10); 193 | glMatrixMode(GL_MODELVIEW); 194 | glLoadIdentity(); 195 | } 196 | void spinview(Pose &camera) 197 | { 198 | if (focus(&camera)) 199 | camera.orientation = normalize(qmul(camera.orientation, quat_from_to(deproject(mousef()), deproject(tolocal(mouse_prev()))))); 200 | } 201 | void trackball(float4 &q) 202 | { 203 | if (focus(&q)) 204 | q = normalize(qmul( quat_from_to(deproject(tolocal(mouse_prev()))+float3(0,0,2), deproject(mousef()) + float3(0, 0, 2)),q)); 205 | } 206 | void drawbar(float t,float3 c0,float3 c1 ) 207 | { 208 | ortho(); 209 | glBegin(GL_QUADS); 210 | glColor3fv(c0); 211 | glVertex3f(0, 1, -0.5f); 212 | glVertex3f(0, 0, -0.5f); 213 | glVertex3f(t, 0, -0.5f); 214 | glVertex3f(t, 1, -0.5f); 215 | glColor3fv(c1); 216 | glVertex3f(t, 1, -0.5f); 217 | glVertex3f(t, 0, -0.5f); 218 | glVertex3f(1, 0, -0.5f); 219 | glVertex3f(1, 1, -0.5f); 220 | glColor3f(1, 1, 1); 221 | glEnd(); 222 | } 223 | void slider(int &i, const int2 range, std::string p = "") 224 | { 225 | if (range.y<=range.x) 226 | return; 227 | if (focus(&i)) 228 | { 229 | i = clamp((int)(mousef().x * (range.y-range.x+1))+range.x, range.x, range.y); 230 | } 231 | drawbar((float)(i-range.x) / (range.y-range.x), float3(1,0,inview(&i)*0.5f), float3(0, 1, inview(&i)*0.5f)); 232 | glwin.PrintString({ 0,-1 }, "%s%d/%d", p.c_str(), i, range.y); 233 | } 234 | void slider(float &t, const float2 range = { 0.0f,1.0f }, std::string p = "") 235 | { 236 | if (focus(&t)) 237 | t = clamp(mousef().x*(range.y-range.x)+range.x,range.x,range.y); 238 | drawbar((t-range.x) / (range.y-range.x), float3(1, 0, inview(&t)*0.5f), float3(0, 1, inview(&t)*0.5f)); 239 | glwin.PrintString({ 0,-1 }, "%s%5.3f", p.c_str(), t); 240 | } 241 | void drawimage(const std::vector &raster, const int2& dims) { ortho(); ::drawimage(raster, dims, { 0,0 }, { 1.0f,1.0f }); } 242 | void drawimage(std::pair &, int2> im) { drawimage(im.first, im.second); } 243 | void drawimagem(const std::vector &raster, const int2& dims) { ortho(); ::drawimage(raster, dims, { 0,1.0f }, { 1.0f,-1.0f }); } 244 | void drawimagem(std::pair &, int2> im) { drawimagem(im.first, im.second); } 245 | }; 246 | 247 | 248 | #endif // MISC_GL_H 249 | -------------------------------------------------------------------------------- /include/misc_image.h: -------------------------------------------------------------------------------- 1 | // 2 | // typical generic image utility routines 3 | // 4 | 5 | #pragma once 6 | #ifndef MISCIMAGE_H 7 | #define MISCIMAGE_H 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "geometric.h" 15 | 16 | class DCamera // generic depth camera interface since other version depends on external library 17 | { 18 | int2 dim_ = { 320, 240 }; 19 | float2 focal_ = { 241.811768f, 241.811768f }; // focal length 1 pixel = 1 unit 20 | float2 principal_ = { 162.830505f, 118.740089f }; 21 | public: 22 | float depth_scale = 0.001f; 23 | float get_depth_scale() { return depth_scale; } 24 | Pose pose; 25 | int2& dim() { return dim_; } 26 | const int2& dim() const { return dim_; } 27 | float2& focal() { return focal_; } // focal length 1 pixel = 1 unit 28 | const float2& focal() const { return focal_; } 29 | float2& principal() { return principal_; } 30 | const float2& principal() const { return principal_; } 31 | DCamera() {} 32 | DCamera(int2 dim, float2 focal, float2 principal,float depth_scale,Pose pose=Pose()) :dim_(dim), focal_(focal), principal_(principal),depth_scale(depth_scale),pose(pose) {} 33 | DCamera(int2 dim) :dim_(dim), focal_(asfloat2(dim)), principal_(asfloat2(dim)/2.0f) {} 34 | float3 deprojectz(const float2 &p, float d) const { return float3((p.x - principal().x) / focal().x, (p.y - principal().y) / focal().y, 1.0f) * d; } 35 | float3 deprojectz(const int2 &p,unsigned short d) const { return deprojectz(asfloat2(p), (float)d); } 36 | float2 projectz(const float3 &v) const { return v.xy() / v.z * focal() + principal(); } 37 | std::vector projectz(const std::vector &pts) const { return Transform(pts, [&](const float3 &v) {return this->projectz(v); }); } // array version of projectz 38 | float2x2 deprojectextents() const { return float2x2(deprojectz(float2(0, 0), 1.0f).xy(), deprojectz(asfloat2(dim()), 1.0f).xy()); } // upper left and lower right xy corners at plane z==1 39 | float2 fov() const { return{ atan2(principal().x + 0.5f, focal().x) + atan2(dim().x - principal().x - 0.5f, focal().x), atan2(principal().y + 0.5f, focal().y) + atan2(dim().y - principal().y - 0.5f, focal().y) }; } // using ds4 conventions with that 0.5 offset 40 | bool intrinsicsimport(std::string filename) 41 | { 42 | std::ifstream camfile(filename); 43 | if (!camfile.is_open()) 44 | return false; 45 | camfile >> dim() >> focal() >> principal() >> depth_scale; 46 | return true; 47 | } 48 | void intrinsicsexport(std::string filename) 49 | { 50 | std::ofstream camfile(filename, std::ios_base::trunc | std::ios_base::out); 51 | if (!camfile.is_open()) 52 | throw "unable to open intrinsics output file"; 53 | camfile << dim() << " " << focal() << " " << principal() << " " << depth_scale <<" " << fov()*180.0f / 3.14159f << " // w h fx fy px py depth_scale fovx fovy (degrees)\n"; 54 | } 55 | template friend void visit_fields(DCamera & cam, F f); 56 | }; 57 | inline std::ostream & operator << (std::ostream & out, const DCamera& dcam) { return out << dcam.dim() << " " << dcam.focal() << " " << dcam.principal() << " " << dcam.depth_scale; } 58 | template void visit_fields(DCamera & cam, F f) { f("dims", cam.dim_); f("focal", cam.focal_); f("principal", cam.principal_); f("depth_scale", cam.depth_scale); } 59 | 60 | inline DCamera camcrop (const DCamera &c, int2 offset, int2 dim) { return{ dim, c.focal(), c.principal() - float2((float)offset.x, (float)offset.y) ,c.depth_scale, c.pose}; } 61 | inline DCamera camsub (const DCamera &c, int s) { return{ c.dim() / s, c.focal() / (float)s, c.principal() / (float)s ,c.depth_scale , c.pose}; } 62 | inline DCamera operator*(const DCamera &c, int s) { return{ c.dim() * s, c.focal() * (float)s, c.principal() * (float)s ,c.depth_scale , c.pose}; } 63 | inline DCamera operator/(const DCamera &c, int s) { return{ c.dim() / s, c.focal() / (float)s, c.principal() / (float)s ,c.depth_scale , c.pose}; } 64 | 65 | //----------------- 66 | // using this 'greyscale' for double/float to/from byte representations with range 0 to 1 for float and 0 to 255 for byte representations 67 | inline unsigned char togreyscale(float x) { return (unsigned char)clamp(x*255.0f, 0.0f, 255.0f); } 68 | inline unsigned char togreyscale(double x) { return togreyscale((float)x); } 69 | inline unsigned char togreyscale(unsigned char x) { return x; } 70 | inline float greyscaletofloat(unsigned char c) { return c / 255.0f; } 71 | inline double greyscaletodouble(unsigned char c) { return c / 255.0; } 72 | 73 | 74 | 75 | 76 | template std::vector subimage(const T *src, int stride, int2 dim) 77 | { 78 | std::vector dst(dim.x*dim.y); 79 | for (int y = 0; y < dim.y; y++) for (int x = 0; x < dim.x; x++) 80 | dst[y*dim.x + x] = src[y*stride + x]; 81 | return dst; 82 | } 83 | template std::vector subimage(const std::vector &src, int2 srcdim, int2 dim, int2 offset) { return subimage(src.data() + offset.x + offset.y*srcdim.x, srcdim.x, dim); } 84 | template std::vector subimage(const std::vector &src, const DCamera &sc, const DCamera &dc) { return subimage(src, sc.dim(), dc.dim(), asint2(sc.principal() - dc.principal())); } 85 | 86 | 87 | template 88 | std::vector DownSample(F f, const std::vector &src, int2 sdim) // max,min,plus 2x2 subsampling 89 | { 90 | int2 ddim = sdim / 2; 91 | std::vector dst(ddim.x*ddim.y); 92 | auto s = [&src, &sdim](int2 p) { return src[p.y*sdim.x + p.x]; }; 93 | for (int2 p(0, 0); p.y < sdim.y; p.y += 2) for (p.x = 0; p.x < sdim.x; p.x += 2) 94 | dst[p.y / 2 * ddim.x + p.x / 2] = f(f(s(p + int2(0, 0)), s(p + int2(1, 0))), f(s(p + int2(0, 1)), s(p + int2(1, 1)))); 95 | return dst; 96 | } 97 | 98 | template std::vector DownSampleMin(const std::vector &src, int2 dim) { return DownSample([](T a, T b) {return std::min(a, b); }, src, dim); } 99 | template std::vector DownSampleFst(const std::vector &src, int2 dim) { return DownSample([](T a, T) {return a; }, src, dim); } 100 | 101 | template std::vector UpSample(const std::vector &src, int2 sdim) 102 | { 103 | std::vector dst(src.size() * 4); 104 | for (int2 p(0, 0); p.y < sdim.y * 2; p.y++) for (p.x = 0; p.x < sdim.x * 2; p.x++) 105 | dst[p.y*sdim.x * 2 + p.x] = src[p.y / 2 * sdim.x + p.x / 2]; 106 | return dst; 107 | } 108 | 109 | 110 | template struct Image 111 | { 112 | DCamera cam; // camera intrinsics including width,height in pixels of image 113 | std::vector raster; 114 | 115 | int2& dim() { return cam.dim(); } 116 | const int2 dim() const { return cam.dim(); } 117 | T& pixel(int2 p) { return raster[p.y*dim().x + p.x]; } 118 | const T& pixel(int2 p) const { return raster[p.y*dim().x + p.x]; } 119 | Image(int2 dim ) :cam(dim), raster(product(dim)) {} 120 | Image(int2 dim,T t) :cam(dim), raster(product(dim),t) {} 121 | Image(DCamera cam) :cam(cam), raster(cam.dim().x*cam.dim().y,T(0)) {} 122 | Image(DCamera cam, std::vector data) :cam(cam), raster(data) {} 123 | Image(int2 dim, std::vector data) :cam(dim), raster(data) {} 124 | Image(int2 dim, const T *data) :cam(dim), raster(data,data+dim.x*dim.y) {} 125 | operator std::pair&, int2>() const { return { raster,dim() }; } 126 | }; 127 | template Image Crop(const Image &src, int2 offset, int2 dim) 128 | { 129 | return {DCamera(dim, src.cam.focal(), src.cam.principal() - asfloat2(offset),src.cam.depth_scale), subimage(src.raster, src.dim(), dim, offset) }; 130 | } 131 | 132 | template Image UpSample(const Image &image) 133 | { 134 | return { image.cam * 2, UpSample(image.raster,image.dim()) }; 135 | } 136 | template Image DownSampleFst(const Image &src) 137 | { 138 | return{ src.cam / 2 , DownSampleFst(src.raster,src.dim()) }; 139 | } 140 | template Image DownSampleMin(const Image &src) 141 | { 142 | return{ src.cam / 2 , DownSampleMin(src.raster,src.dim()) }; 143 | } 144 | 145 | 146 | 147 | template Image Sample(const Image &src, const DCamera &dstcam, T background = 0) // uses point sampling 148 | { 149 | Image dst(dstcam); 150 | int2 pp; 151 | for (auto p : rect_iteration(dst.dim())) // for (int y = 0; y < dst.dim().y; y++) for (int x = 0; x < dst.dim().x; x++) 152 | dst.pixel(p) = within_range(pp = asint2(src.cam.projectz(dst.cam.pose*dst.cam.deprojectz(asfloat2(p), 1.f))), int2(0, 0), src.dim() - int2(1, 1)) ? src.pixel(pp) : background; 153 | return dst; 154 | } 155 | 156 | // Note that for depth image, the pixel values are distance from the image plane. 157 | // because the sampling may include a rotated camera, we here adjust the distance (sampled value) to be distance from the destination camera's image plane 158 | template Image SampleD(const Image &src, const DCamera &dstcam, T background = 0) // uses point sampling 159 | { 160 | Image dst(dstcam); 161 | float3 ppdir = dstcam.pose * dstcam.deprojectz(dstcam.principal(), 1.0f); 162 | int2 pp; 163 | for (auto p : rect_iteration(dst.dim())) // for (int y = 0; y < dst.dim().y; y++) for (int x = 0; x < dst.dim().x; x++) 164 | dst.pixel(p) = within_range(pp = asint2(src.cam.projectz(dst.cam.pose*dst.cam.deprojectz(asfloat2(p), 1.f))), int2(0, 0), src.dim() - int2(1, 1)) ? (T)dot(ppdir, src.cam.deprojectz(pp, src.pixel(pp))) : background; 165 | return dst; 166 | } 167 | 168 | 169 | template auto Transform(const Image &src, F f) -> Image { std::vector dst(src.raster.size()); std::transform(src.raster.begin(), src.raster.end(), dst.begin(), f); return Image(src.cam, dst); } 170 | 171 | template Image togreyscale(const Image &src) 172 | { 173 | return Transform(src,[](T x) {return togreyscale(x); }); 174 | } 175 | 176 | Image torgb(const Image &src) // grey to rgb for drawing 177 | { 178 | return Transform(src, [](unsigned char p) {return byte3(p, p, p); }); 179 | } 180 | Image torgb(const Image &src) { return torgb(togreyscale(src)); } 181 | Image torgb(const Image &src) { return torgb(togreyscale(src)); } 182 | 183 | 184 | template int2 imagefindmax(const T *image, int2 dim, std::function f = [](const T& a, const T&b) {return a > b; }) 185 | { 186 | int2 best(0, 0); 187 | for (int2 p(0, 0); p.y < dim.y; p.y++) for (p.x = 0; p.x < dim.x; p.x++) 188 | if (f(image[dot(p, { 1, dim.x })], image[dot(best, { 1,dim.x })])) 189 | best = p; 190 | return best; 191 | } 192 | template int2 imagefindmax(const Image &image, std::function f = [](const T& a, const T&b) {return a > b; }) 193 | { 194 | return imagefindmax(image.raster.data(), image.dim(), f); 195 | } 196 | 197 | 198 | 199 | inline Image Threshold(const Image &depthdata, std::function f) 200 | { 201 | return Transform(depthdata, [&](unsigned short d)->unsigned char {return (f(d)) ? 255 : 0; }); 202 | } 203 | inline Image DistanceTransform(Image image) // just manhattan 204 | { 205 | int2 dim = image.dim(); 206 | auto cm = [dim](int2 p) {return clamp(p, int2(0,0), dim - int2(1, 1)); }; 207 | for(auto p: rect_iteration(dim)) 208 | image.pixel(p) = (unsigned char)std::min(255, std::min(std::min(image.pixel(cm(p - int2(1,0))) + 1, image.pixel(cm(p-int2(0,1))) + 1), image.pixel(p) + 0)); 209 | for (auto r : rect_iteration(dim)) 210 | { 211 | int2 p = dim - int2(1, 1) - r ; 212 | image.pixel(p) = (unsigned char)std::min(255, std::min(std::min(image.pixel(cm(p + int2(1, 0))) + 1, image.pixel(cm(p + int2(0, 1))) + 1), image.pixel(p) + 0)); 213 | } 214 | return image; 215 | } 216 | template Image SetBorder(Image image, T v = T(0)) 217 | { 218 | int2 dim = image.dim(); 219 | for (int y = 0; y < dim.y; y++) 220 | image.pixel({ 0,y }) = image.pixel({ dim.x - 1, y }) = v; 221 | for (int x = 0; x < dim.x; x++) 222 | image.pixel({ x,0 }) = image.pixel({ x, dim.y - 1 }) = v; 223 | return image; 224 | } 225 | 226 | 227 | Image Clip(Image depth, float4 plane,unsigned short val) 228 | { 229 | for (int2 p(0, 0);p.y < depth.dim().y;p.y++) for (p.x = 0;p.x < depth.dim().x;p.x++) 230 | if (dot(depth.cam.deprojectz(p, depth.pixel(p)), plane.xyz()) + plane.w < 0) 231 | depth.pixel(p) = val; 232 | return depth; 233 | } 234 | inline bool within_range(float t, float2 r) { return t >= r.x && t < r.y; } 235 | template std::vector PointCloud(const Image &dimage, float2 filter_range, float depth_scale = 0.001f) 236 | { 237 | std::vector pointcloud; 238 | float d; 239 | for (int2 p : rect_iteration(dimage.dim())) // p.y and p.x iterate over dimensions of image 240 | if(within_range(d= (dimage.pixel(p)* depth_scale),filter_range)) 241 | pointcloud.push_back( dimage.cam.deprojectz(float2(p),d ) ); 242 | return pointcloud; 243 | } 244 | 245 | 246 | #if _MSC_VER > 1800 247 | 248 | struct VertexPT { float3 position; float2 texcoord; }; 249 | template std::vector PointCloudT(const Image &dimage, float2 filter_range, float depth_scale = 0.001f) 250 | { 251 | std::vector pointcloud; 252 | float d; 253 | for (int2 p : rect_iteration(dimage.dim())) // p.y and p.x iterate over dimensions of image 254 | if (within_range(d = (dimage.pixel(p)* depth_scale), filter_range)) 255 | pointcloud.push_back({ dimage.cam.deprojectz(float2(p), d), float2(p) / float2(dimage.dim()) }); 256 | return pointcloud; 257 | } 258 | 259 | 260 | 261 | 262 | auto PlaneSplit(const std::vector &points,float4 plane, float epsilon=0.02f) 263 | { 264 | struct result { std::vector under, coplanar, over; }; 265 | std::vector b[3]; // under, coplanar, over; 266 | for (auto p : points) 267 | { 268 | float pd = dot(float4(p, 1), plane); 269 | b[(pd > -epsilon) + (pd > epsilon)].push_back(p); 270 | } 271 | return result{b[0], b[1], b[2]}; 272 | //return make_tuple(move(b[0]), move(b[1]), move(b[2])); // { under, coplanar, over }; 273 | } 274 | std::vector Mirror(std::vector points, float4 plane) 275 | { 276 | for (auto &p : points) 277 | p += plane.xyz()* (dot(float4(p, 1), plane)*-2.0f); 278 | return points; 279 | } 280 | auto MirrorPlaneSplit(const std::vector &points, float4 plane, float epsilon = 0.02f) 281 | { 282 | auto r = PlaneSplit(points, plane, epsilon); 283 | r.under = Mirror(std::move(r.under), plane); 284 | return r; 285 | } 286 | #endif 287 | 288 | 289 | 290 | // Segment() - not a generic routine... 291 | // The following is specific to the depth camera tracking application 292 | // the implementation is a bit ad hoc. its a first pass to get the plumbing all working. 293 | static float2 g_com(0,0); // for debugging only 294 | static int2 g_entry(0, 0); // for debugging only 295 | static int2 g_extreme(0, 0); // for debugging only 296 | 297 | Image Segment(const Image &depth, int entry_options = 0xF, float2 wrange = { 0.1f,0.65f } , float depth_scale = 0.001f) 298 | { 299 | auto dim = depth.dim(); 300 | Image depthsmall = DownSampleMin(DownSampleMin(depth)); 301 | ushort2 wranged = ushort2(wrange / depth_scale); 302 | auto dt = DistanceTransform(Threshold(depthsmall, [wranged](unsigned short d) {return /*d >= wranged.x && */d < wranged.y; })); 303 | 304 | int2 entry = (entry_options == 1) ? int2(dt.dim().x/2, dt.dim().y -1) : (entry_options==4)? int2(dt.dim().x-1,dt.dim().y/2): (entry_options == 8) ? int2(0, dt.dim().y / 2) : int2(0,0); 305 | if (entry_options & 1) for (int2 p(0, dt.dim().y-1); p.x < dt.dim().x;p.x++) if (dt.pixel(p) > dt.pixel(entry)) entry = p; 306 | if (entry_options & 2) for (int2 p(0, 0 ); p.x < dt.dim().x;p.x++) if (dt.pixel(p) > dt.pixel(entry)) entry = p; 307 | if (entry_options & 4) for (int2 p(dt.dim().x-1, 0); p.y < dt.dim().y;p.y++) if (dt.pixel(p) > dt.pixel(entry)) entry = p; 308 | if (entry_options & 8) for (int2 p(0 , 0); p.y < dt.dim().y;p.y++) if (dt.pixel(p) > dt.pixel(entry)) entry = p; 309 | 310 | float avgdepth = 0; 311 | float2 com(0, 0); 312 | float wtotal = 0.0f; 313 | int count = 0; 314 | const int min_blob_radius = 2; // note that this will be after downsampled for distance transform had a camera that was having edge noise issues 315 | 316 | for(auto p: rect_iteration(dt.dim())) 317 | { 318 | if (dt.pixel(p) >= min_blob_radius) // && p.y < firsty + 128 / 4) 319 | { 320 | float w = length(float2(p - entry)); 321 | wtotal += w; 322 | com += asfloat2(p)*w; 323 | avgdepth += depthsmall.pixel(p)*w; // although 32 bit float, avgdepth sum happens in unsigned short raster units at its depth_scale 324 | count++; 325 | } 326 | } 327 | if (count) 328 | { 329 | avgdepth *= depth_scale/wtotal; // avgdepth convert to meters and divide by total weighting 330 | com /= wtotal; 331 | } 332 | float2 extreme = float2(entry); 333 | for (auto p : rect_iteration(dt.dim())) 334 | { 335 | if (dt.pixel(p) >= min_blob_radius) // && p.y < firsty + 128 / 4) 336 | if (dot(float2(p) - float2(entry), com - float2(entry)) > dot(float2(extreme) - float2(entry), com - float2(entry))) 337 | extreme = float2(p); 338 | } 339 | float scale = 2.0f; 340 | float angle = 0.0f; 341 | const float diam = 0.17f; // diameter_of_interest = 342 | if (count) 343 | { 344 | angle = atan2((float)com.x - entry.x, (float)entry.y - com.y); 345 | scale = 2.0f / clamp(avgdepth / 0.450f, 0.55f, 2.0f); // magic number alert, the 45cm comes from using it previously as a target distance before i had adaptive scale 346 | float exrad = dot(extreme - com, normalize(com - float2(entry))); 347 | com += normalize(com - float2(entry)) * (exrad - diam/2.0f/avgdepth * dt.cam.focal().x) ; // shift com to be half of diam ~20cm (ie 10cm) away from extreme point 348 | } 349 | 350 | DCamera dstcam({ 64,64 }, float2( avgdepth*64.0f/diam) , { 32,32 },depth.cam.depth_scale); 351 | 352 | float3 zd = linalg::normalize(dstcam.deprojectz(asfloat2(dstcam.dim()/2), 1.0f)); 353 | float3 yd = linalg::normalize(ProjectOntoPlane({ zd,0.0f }, dt.cam.deprojectz(float2(entry), 1.0f) - dstcam.deprojectz(asfloat2(dstcam.dim() / 2), 1.0f) )); 354 | dstcam.pose.orientation = qmul(quat_from_to(dt.cam.deprojectz(dt.cam.principal(), 1.0f), dt.cam.deprojectz(com, 1.0f)), QuatFromAxisAngle(float3(0, 0, 1), angle)); //quatfrommat({ cross(yd, zd), yd, zd }); 355 | dstcam.principal() = asfloat2(dstcam.dim())*0.5f; 356 | 357 | g_com = linalg::clamp(com, float2(0, 0), float2(dt.cam.dim() - int2(1, 1))); 358 | g_entry=entry; 359 | g_extreme=int2(extreme); 360 | 361 | return SampleD(depth, dstcam, (unsigned short)(4.0f/depth_scale)); // 4m away for anything outside of src depth image 362 | } 363 | 364 | 365 | 366 | #endif // MISCIMAGE_H 367 | 368 | --------------------------------------------------------------------------------