[ { "path": ".gitignore", "content": ".DS_Store\n" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 2.8.3)\nproject(mono-slam)\nset(CMAKE_CXX_FLAGS \"-O2 -msse4 -DEIGEN_NO_DEBUG -DNDEBUG\")\n## Find catkin macros and libraries\n## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)\n## is used, also find other catkin packages\n\nfind_package(catkin REQUIRED COMPONENTS cv_bridge tf sensor_msgs cv_bridge roscpp std_msgs image_transport)\nfind_package(OpenCV REQUIRED)\n## System dependencies are found with CMake's conventions\n# find_package(Boost REQUIRED COMPONENTS system)\n\n\n## Uncomment this if the package has a setup.py. This macro ensures\n## modules and global scripts declared therein get installed\n## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html\n# catkin_python_setup()\n\n#######################################\n## Declare ROS messages and services ##\n#######################################\n\n## Generate messages in the 'msg' folder\n# add_message_files(\n# FILES\n# Message1.msg\n# Message2.msg\n# )\n\n## Generate services in the 'srv' folder\n# add_service_files(\n# FILES\n# Service1.srv\n# Service2.srv\n# )\n\n## Generate added messages and services with any dependencies listed here\n# generate_messages(\n# DEPENDENCIES\n# std_msgs # Or other packages containing msgs\n# )\n\n###################################\n## catkin specific configuration ##\n###################################\n## The catkin_package macro generates cmake config files for your package\n## Declare things to be passed to dependent projects\n## LIBRARIES: libraries you create in this project that dependent projects also need\n## CATKIN_DEPENDS: catkin_packages dependent projects also need\n## DEPENDS: system dependencies of this project that dependent projects also need\ncatkin_package(\n# INCLUDE_DIRS include\n# LIBRARIES mono_slam\n# CATKIN_DEPENDS cv_bridge\n# DEPENDS system_lib\n)\n\n###########\n## Build ##\n###########\n\n## Specify additional locations of header files\n## Your package locations should be listed before other locations\n# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})\n\n## Declare a cpp library\n# add_library(mono_slam\n# src/${PROJECT_NAME}/mono_slam.cpp\n# )\n\n## Declare a cpp executable\nadd_executable(mono-slam \n\t\tsrc/monoslam_ransac.cpp \n\t\t#src/camOCV.cpp \n\t\tsrc/vslamRansac.cpp \n\t\tsrc/RosVSLAMRansac.cpp \n\t\tsrc/Patch.cpp \n\t\tsrc/utils.cpp\n\t\t#src/camOCV.hpp \n\t\tsrc/vslamRansac.hpp \n\t\tsrc/RosVSLAMRansac.hpp \n\t\tsrc/Patch.hpp \n\t\tsrc/utils.hpp\n\t\tsrc/libblur.h\n\t\tsrc/libblur.cpp\n\t\tsrc/ConfigVSLAM.cpp\n\t \tsrc/ConfigVSLAM.h\n\t \tsrc/camModel.hpp\n\t \tsrc/camModel.cpp\n\t\t)\n\n\n## Add cmake target dependencies of the executable/library\n## as an example, message headers may need to be generated before nodes\n# add_dependencies(mono_slam_node mono_slam_generate_messages_cpp)\n\n## Specify libraries to link a library or executable target against\n #target_link_libraries(ludoslam ${catkin_LIBRARIES})\n \n target_link_libraries(mono-slam ${catkin_LIBRARIES} config++)\n\n#############\n## Install ##\n#############\n\n# all install targets should use catkin DESTINATION variables\n# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html\n\n## Mark executable scripts (Python etc.) for installation\n## in contrast to setup.py, you can choose the destination\n# install(PROGRAMS\n# scripts/my_python_script\n# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark executables and/or libraries for installation\n# install(TARGETS mono_slam mono_slam_node\n# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}\n# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}\n# )\n\n## Mark cpp header files for installation\n# install(DIRECTORY include/${PROJECT_NAME}/\n# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}\n# FILES_MATCHING PATTERN \"*.h\"\n# PATTERN \".svn\" EXCLUDE\n# )\n\n## Mark other files for installation (e.g. launch and bag files, etc.)\n# install(FILES\n# # myfile1\n# # myfile2\n# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}\n# )\n\n#############\n## Testing ##\n#############\n\n## Add gtest based cpp test target and link libraries\n# catkin_add_gtest(${PROJECT_NAME}-test test/test_mono_slam.cpp)\n# if(TARGET ${PROJECT_NAME}-test)\n# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})\n# endif()\n\n## Add folders to be run by python nosetests\n# catkin_add_nosetests(test)\n" }, { "path": "LICENSE.md", "content": "Mozilla Public License Version 2.0\n==================================\n\n1. Definitions\n--------------\n\n1.1. \"Contributor\"\n means each individual or legal entity that creates, contributes to\n the creation of, or owns Covered Software.\n\n1.2. \"Contributor Version\"\n means the combination of the Contributions of others (if any) used\n by a Contributor and that particular Contributor's Contribution.\n\n1.3. \"Contribution\"\n means Covered Software of a particular Contributor.\n\n1.4. \"Covered Software\"\n means Source Code Form to which the initial Contributor has attached\n the notice in Exhibit A, the Executable Form of such Source Code\n Form, and Modifications of such Source Code Form, in each case\n including portions thereof.\n\n1.5. \"Incompatible With Secondary Licenses\"\n means\n\n (a) that the initial Contributor has attached the notice described\n in Exhibit B to the Covered Software; or\n\n (b) that the Covered Software was made available under the terms of\n version 1.1 or earlier of the License, but not also under the\n terms of a Secondary License.\n\n1.6. \"Executable Form\"\n means any form of the work other than Source Code Form.\n\n1.7. \"Larger Work\"\n means a work that combines Covered Software with other material, in\n a separate file or files, that is not Covered Software.\n\n1.8. \"License\"\n means this document.\n\n1.9. \"Licensable\"\n means having the right to grant, to the maximum extent possible,\n whether at the time of the initial grant or subsequently, any and\n all of the rights conveyed by this License.\n\n1.10. \"Modifications\"\n means any of the following:\n\n (a) any file in Source Code Form that results from an addition to,\n deletion from, or modification of the contents of Covered\n Software; or\n\n (b) any new file in Source Code Form that contains any Covered\n Software.\n\n1.11. \"Patent Claims\" of a Contributor\n means any patent claim(s), including without limitation, method,\n process, and apparatus claims, in any patent Licensable by such\n Contributor that would be infringed, but for the grant of the\n License, by the making, using, selling, offering for sale, having\n made, import, or transfer of either its Contributions or its\n Contributor Version.\n\n1.12. \"Secondary License\"\n means either the GNU General Public License, Version 2.0, the GNU\n Lesser General Public License, Version 2.1, the GNU Affero General\n Public License, Version 3.0, or any later versions of those\n licenses.\n\n1.13. \"Source Code Form\"\n means the form of the work preferred for making modifications.\n\n1.14. \"You\" (or \"Your\")\n means an individual or a legal entity exercising rights under this\n License. For legal entities, \"You\" includes any entity that\n controls, is controlled by, or is under common control with You. For\n purposes of this definition, \"control\" means (a) the power, direct\n or indirect, to cause the direction or management of such entity,\n whether by contract or otherwise, or (b) ownership of more than\n fifty percent (50%) of the outstanding shares or beneficial\n ownership of such entity.\n\n2. License Grants and Conditions\n--------------------------------\n\n2.1. Grants\n\nEach Contributor hereby grants You a world-wide, royalty-free,\nnon-exclusive license:\n\n(a) under intellectual property rights (other than patent or trademark)\n Licensable by such Contributor to use, reproduce, make available,\n modify, display, perform, distribute, and otherwise exploit its\n Contributions, either on an unmodified basis, with Modifications, or\n as part of a Larger Work; and\n\n(b) under Patent Claims of such Contributor to make, use, sell, offer\n for sale, have made, import, and otherwise transfer either its\n Contributions or its Contributor Version.\n\n2.2. Effective Date\n\nThe licenses granted in Section 2.1 with respect to any Contribution\nbecome effective for each Contribution on the date the Contributor first\ndistributes such Contribution.\n\n2.3. Limitations on Grant Scope\n\nThe licenses granted in this Section 2 are the only rights granted under\nthis License. No additional rights or licenses will be implied from the\ndistribution or licensing of Covered Software under this License.\nNotwithstanding Section 2.1(b) above, no patent license is granted by a\nContributor:\n\n(a) for any code that a Contributor has removed from Covered Software;\n or\n\n(b) for infringements caused by: (i) Your and any other third party's\n modifications of Covered Software, or (ii) the combination of its\n Contributions with other software (except as part of its Contributor\n Version); or\n\n(c) under Patent Claims infringed by Covered Software in the absence of\n its Contributions.\n\nThis License does not grant any rights in the trademarks, service marks,\nor logos of any Contributor (except as may be necessary to comply with\nthe notice requirements in Section 3.4).\n\n2.4. Subsequent Licenses\n\nNo Contributor makes additional grants as a result of Your choice to\ndistribute the Covered Software under a subsequent version of this\nLicense (see Section 10.2) or under the terms of a Secondary License (if\npermitted under the terms of Section 3.3).\n\n2.5. Representation\n\nEach Contributor represents that the Contributor believes its\nContributions are its original creation(s) or it has sufficient rights\nto grant the rights to its Contributions conveyed by this License.\n\n2.6. Fair Use\n\nThis License is not intended to limit any rights You have under\napplicable copyright doctrines of fair use, fair dealing, or other\nequivalents.\n\n2.7. Conditions\n\nSections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted\nin Section 2.1.\n\n3. Responsibilities\n-------------------\n\n3.1. Distribution of Source Form\n\nAll distribution of Covered Software in Source Code Form, including any\nModifications that You create or to which You contribute, must be under\nthe terms of this License. You must inform recipients that the Source\nCode Form of the Covered Software is governed by the terms of this\nLicense, and how they can obtain a copy of this License. You may not\nattempt to alter or restrict the recipients' rights in the Source Code\nForm.\n\n3.2. Distribution of Executable Form\n\nIf You distribute Covered Software in Executable Form then:\n\n(a) such Covered Software must also be made available in Source Code\n Form, as described in Section 3.1, and You must inform recipients of\n the Executable Form how they can obtain a copy of such Source Code\n Form by reasonable means in a timely manner, at a charge no more\n than the cost of distribution to the recipient; and\n\n(b) You may distribute such Executable Form under the terms of this\n License, or sublicense it under different terms, provided that the\n license for the Executable Form does not attempt to limit or alter\n the recipients' rights in the Source Code Form under this License.\n\n3.3. Distribution of a Larger Work\n\nYou may create and distribute a Larger Work under terms of Your choice,\nprovided that You also comply with the requirements of this License for\nthe Covered Software. If the Larger Work is a combination of Covered\nSoftware with a work governed by one or more Secondary Licenses, and the\nCovered Software is not Incompatible With Secondary Licenses, this\nLicense permits You to additionally distribute such Covered Software\nunder the terms of such Secondary License(s), so that the recipient of\nthe Larger Work may, at their option, further distribute the Covered\nSoftware under the terms of either this License or such Secondary\nLicense(s).\n\n3.4. Notices\n\nYou may not remove or alter the substance of any license notices\n(including copyright notices, patent notices, disclaimers of warranty,\nor limitations of liability) contained within the Source Code Form of\nthe Covered Software, except that You may alter any license notices to\nthe extent required to remedy known factual inaccuracies.\n\n3.5. Application of Additional Terms\n\nYou may choose to offer, and to charge a fee for, warranty, support,\nindemnity or liability obligations to one or more recipients of Covered\nSoftware. However, You may do so only on Your own behalf, and not on\nbehalf of any Contributor. You must make it absolutely clear that any\nsuch warranty, support, indemnity, or liability obligation is offered by\nYou alone, and You hereby agree to indemnify every Contributor for any\nliability incurred by such Contributor as a result of warranty, support,\nindemnity or liability terms You offer. You may include additional\ndisclaimers of warranty and limitations of liability specific to any\njurisdiction.\n\n4. Inability to Comply Due to Statute or Regulation\n---------------------------------------------------\n\nIf it is impossible for You to comply with any of the terms of this\nLicense with respect to some or all of the Covered Software due to\nstatute, judicial order, or regulation then You must: (a) comply with\nthe terms of this License to the maximum extent possible; and (b)\ndescribe the limitations and the code they affect. Such description must\nbe placed in a text file included with all distributions of the Covered\nSoftware under this License. Except to the extent prohibited by statute\nor regulation, such description must be sufficiently detailed for a\nrecipient of ordinary skill to be able to understand it.\n\n5. Termination\n--------------\n\n5.1. The rights granted under this License will terminate automatically\nif You fail to comply with any of its terms. However, if You become\ncompliant, then the rights granted under this License from a particular\nContributor are reinstated (a) provisionally, unless and until such\nContributor explicitly and finally terminates Your grants, and (b) on an\nongoing basis, if such Contributor fails to notify You of the\nnon-compliance by some reasonable means prior to 60 days after You have\ncome back into compliance. Moreover, Your grants from a particular\nContributor are reinstated on an ongoing basis if such Contributor\nnotifies You of the non-compliance by some reasonable means, this is the\nfirst time You have received notice of non-compliance with this License\nfrom such Contributor, and You become compliant prior to 30 days after\nYour receipt of the notice.\n\n5.2. If You initiate litigation against any entity by asserting a patent\ninfringement claim (excluding declaratory judgment actions,\ncounter-claims, and cross-claims) alleging that a Contributor Version\ndirectly or indirectly infringes any patent, then the rights granted to\nYou by any and all Contributors for the Covered Software under Section\n2.1 of this License shall terminate.\n\n5.3. In the event of termination under Sections 5.1 or 5.2 above, all\nend user license agreements (excluding distributors and resellers) which\nhave been validly granted by You or Your distributors under this License\nprior to termination shall survive termination.\n\n************************************************************************\n* *\n* 6. Disclaimer of Warranty *\n* ------------------------- *\n* *\n* Covered Software is provided under this License on an \"as is\" *\n* basis, without warranty of any kind, either expressed, implied, or *\n* statutory, including, without limitation, warranties that the *\n* Covered Software is free of defects, merchantable, fit for a *\n* particular purpose or non-infringing. The entire risk as to the *\n* quality and performance of the Covered Software is with You. *\n* Should any Covered Software prove defective in any respect, You *\n* (not any Contributor) assume the cost of any necessary servicing, *\n* repair, or correction. This disclaimer of warranty constitutes an *\n* essential part of this License. No use of any Covered Software is *\n* authorized under this License except under this disclaimer. *\n* *\n************************************************************************\n\n************************************************************************\n* *\n* 7. Limitation of Liability *\n* -------------------------- *\n* *\n* Under no circumstances and under no legal theory, whether tort *\n* (including negligence), contract, or otherwise, shall any *\n* Contributor, or anyone who distributes Covered Software as *\n* permitted above, be liable to You for any direct, indirect, *\n* special, incidental, or consequential damages of any character *\n* including, without limitation, damages for lost profits, loss of *\n* goodwill, work stoppage, computer failure or malfunction, or any *\n* and all other commercial damages or losses, even if such party *\n* shall have been informed of the possibility of such damages. This *\n* limitation of liability shall not apply to liability for death or *\n* personal injury resulting from such party's negligence to the *\n* extent applicable law prohibits such limitation. Some *\n* jurisdictions do not allow the exclusion or limitation of *\n* incidental or consequential damages, so this exclusion and *\n* limitation may not apply to You. *\n* *\n************************************************************************\n\n8. Litigation\n-------------\n\nAny litigation relating to this License may be brought only in the\ncourts of a jurisdiction where the defendant maintains its principal\nplace of business and such litigation shall be governed by laws of that\njurisdiction, without reference to its conflict-of-law provisions.\nNothing in this Section shall prevent a party's ability to bring\ncross-claims or counter-claims.\n\n9. Miscellaneous\n----------------\n\nThis License represents the complete agreement concerning the subject\nmatter hereof. If any provision of this License is held to be\nunenforceable, such provision shall be reformed only to the extent\nnecessary to make it enforceable. Any law or regulation which provides\nthat the language of a contract shall be construed against the drafter\nshall not be used to construe this License against a Contributor.\n\n10. Versions of the License\n---------------------------\n\n10.1. New Versions\n\nMozilla Foundation is the license steward. Except as provided in Section\n10.3, no one other than the license steward has the right to modify or\npublish new versions of this License. Each version will be given a\ndistinguishing version number.\n\n10.2. Effect of New Versions\n\nYou may distribute the Covered Software under the terms of the version\nof the License under which You originally received the Covered Software,\nor under the terms of any subsequent version published by the license\nsteward.\n\n10.3. Modified Versions\n\nIf you create software not governed by this License, and you want to\ncreate a new license for such software, you may create and use a\nmodified version of this License if you rename the license and remove\nany references to the name of the license steward (except to note that\nsuch modified license differs from this License).\n\n10.4. Distributing Source Code Form that is Incompatible With Secondary\nLicenses\n\nIf You choose to distribute Source Code Form that is Incompatible With\nSecondary Licenses under the terms of this version of the License, the\nnotice described in Exhibit B of this License must be attached.\n\nExhibit A - Source Code Form License Notice\n-------------------------------------------\n\n This Source Code Form is subject to the terms of the Mozilla Public\n License, v. 2.0. If a copy of the MPL was not distributed with this\n file, You can obtain one at http://mozilla.org/MPL/2.0/.\n\nIf it is not possible or desirable to put the notice in a particular\nfile, then You may include the notice in a location (such as a LICENSE\nfile in a relevant directory) where a recipient would be likely to look\nfor such a notice.\n\nYou may add additional accurate notices of copyright ownership.\n\nExhibit B - \"Incompatible With Secondary Licenses\" Notice\n---------------------------------------------------------\n\n This Source Code Form is \"Incompatible With Secondary Licenses\", as\n defined by the Mozilla Public License, v. 2.0.\n" }, { "path": "README.md", "content": "# Mono-Slam Implementation in ROS\n\nThis repository implements the mono-slam algorithm first introduced by [Andrew Davison](https://www.doc.ic.ac.uk/~ajd/Publications/davison_etal_pami2007.pdf),\nand is the final outcome of the M.Sc. Thesis of [Ludovico O. Russo](https://github.com/ludusrusso)\nin the 2013.\n\n[Reference: Russo L.O., Rosa S., Bona B., Matteucci M., \"A ROS implementation of the mono-slam algorithm\",\nInternational Journal of Computer Science & Information Technology, Vol. 6 Issue 1, p339](https://www.researchgate.net/publication/269200654_A_ROS_Implementation_of_the_Mono-Slam_Algorithm)\n\n\n## Usage:\n\n```bash\n$ rosrun mono-slam mono-slam configuration.cfg /image:=/your_image_topic\n```\n\n> configuration file contains parameters for the mono-slam algorithm\n and camera calibration.\n Sample configuration files are provided in the \"conf\" folder\n\n## Example videos\n\n - https://www.youtube.com/watch?v=Cf0iKfhnyu4\n - https://www.youtube.com/watch?v=Jjmm9yZY3kA\n - https://www.youtube.com/watch?v=ANNwb4NqlIM\n" }, { "path": "conf/conf.cfg", "content": "#configuration file for MonoSLAM\n\nsigma_v = 0.01;\nsigma_w = 0.005;\n\n\nrho_0 = 0.1;\nsigma_rho_0 = 0.25;\n\nsigma_pixel = 2;\nwindow_size = 21;\n\nkernel_size = 1000;\n\nscale = 2;\n\nT_camera = 0.1;\n\nmin_features = 30;\nmax_features = 100;\n\n\n\ncamera = {\n # fx = 1436.8218;\n # fy = 1412.0250;\n # u0 = 630.9579;\n # v0 = 432.8925;\n\n fx = 718.4109;\n fy = 706.0125;\n u0 = 315.47895;\n v0 = 216.44625;\n k1 = -0.0182844;\n k2 = 0.04502865;\n p1 = 0.0;\n p2 = 0.0;\n k3 = 0.0;\n};\n\n\n" }, { "path": "conf/conf_firewire.cfg", "content": "#configuration file for MonoSLAM\n\nsigma_vx = 0.01;\nsigma_vy = 0.01;\nsigma_vz = 0.01;\nsigma_wx = 0.005; #metà delle v\nsigma_wy = 0.005;\nsigma_wz = 0.005;\n\n# non cambia\nrho_0 = 0.1;\nsigma_rho_0 = 0.25;\n\n# pixel error\nsigma_pixel = 2;\n# patch size (11)\nwindow_size = 21;\n\nkernel_size = 1000000000;\n\nscale = 1;\n\nT_camera = 0.5;\n\nnInitFeatures = 10;\nmin_features = 20;\nmax_features = 60;\n\ncamera = {\n fx = 563.21765;\n fy = 558.45293;\n u0 = 347.75115;\n v0 = 246.19144;\n k1 = -0.45720;\n k2 = 0.30980;\n p1 = -0.00265;\n p2 = 0.00078;\n k3 = -0.13950;\n};\n\n\n#Focal Length: fc = [ 563.21765 558.45293 ] � [ 1.61456 1.51463 ]\n#Principal point: cc = [ 347.75115 246.19144 ] � [ 2.20868 1.89131 ]\n#Skew: alpha_c = [ 0.00000 ] � [ 0.00000 ] => angle of pixel axes = 90.00000 � 0.00000 degrees\n#Distortion: kc = [ -0.45720 0.30980 -0.00265 0.00078 -0.13950 ] � [ 0.00697 0.02570 0.00071 0.00064 0.02797 ]\n#Pixel error: err = [ 0.31093 0.24720 ]\n\n" }, { "path": "conf/conf_kinect.cfg", "content": "#configuration file for MonoSLAM\n\nsigma_vx = 0.03;\nsigma_vy = 0.03;\nsigma_vz = 0.03;\nsigma_wx = 0.015; #metà delle v\nsigma_wy = 0.015;\nsigma_wz = 0.015;\n\n# non cambia\nrho_0 = 0.2;\nsigma_rho_0 = 0.25;\n\n# pixel error\nsigma_pixel = 2;\n# patch size (11)\nwindow_size = 15;\n\n#kernel blurring\nkernel_size = 1000;\n\n# image scaling\nscale = 2;\n\n# camera aperture rispetto a framerate\nT_camera = 0.0; #0.5;\n\n# parametri features\nnInitFeatures = 50;\nmin_features = 100;\nmax_features = 1000;\n\ncamera = {\n fx = 537.673722507338;\n fy = 534.380205679756;\n u0 = 321.226061527052;\n v0 = 249.773992466202;\n k1 = 0.0395956005042652;\n k2 = -0.111310452999064;\n p1 = 0.00211988964071199;\n p2 = 0.00070924348636878;\n k3 = 0.0;\n};\n\n\n#Focal Length: fc = [ 563.21765 558.45293 ] � [ 1.61456 1.51463 ]\n#Principal point: cc = [ 347.75115 246.19144 ] � [ 2.20868 1.89131 ]\n#Skew: alpha_c = [ 0.00000 ] � [ 0.00000 ] => angle of pixel axes = 90.00000 � 0.00000 degrees\n#Distortion: kc = [ -0.45720 0.30980 -0.00265 0.00078 -0.13950 ] � [ 0.00697 0.02570 0.00071 0.00064 0.02797 ]\n#Pixel error: err = [ 0.31093 0.24720 ]\n\n" }, { "path": "conf/confing.cfg", "content": "#configuration file for MonoSLAM\n\nsigma_v = 0.004;\nsigma_w = 0.004;\n\n\nrho_0 = 0.1;\nsigma_rho_0 = 0.01;\n\nsigma_pixel = 2;\nwindow_size = 51;\n\nkernel_size = 3;\n\ncamera = {\n fx = 611.3357;\n fy = 600.4211;\n u0 = 299.5985;\n v0 = 288.7289;\n k1 = -0.2677;\n k2 = -0.1539;\n p1 = -0.0179;\n p2 = 0.0056;\n k3 = 0.0;\n};\n" }, { "path": "data/camera.ply", "content": "ply\nformat ascii 1.0\ncomment Created by Blender 2.62 (sub 0) - www.blender.org, source file: 'camera.blend'\nelement vertex 153\nproperty float x\nproperty float y\nproperty float z\nproperty float nx\nproperty float ny\nproperty float nz\nelement face 70\nproperty list uchar uint vertex_indices\nend_header\n0.243863 1.225982 1.972152 -0.083231 -0.845056 0.528160\n0.000000 1.250000 1.972152 -0.083231 -0.845056 0.528160\n0.000000 0.000000 -0.027848 -0.083231 -0.845056 0.528160\n0.000000 0.000000 -0.027848 -0.246494 -0.812581 0.528160\n0.478354 1.154849 1.972152 -0.246494 -0.812581 0.528160\n0.243863 1.225982 1.972152 -0.246494 -0.812581 0.528160\n0.000000 0.000000 -0.027848 -0.400284 -0.748879 0.528160\n0.694463 1.039337 1.972152 -0.400284 -0.748879 0.528160\n0.478354 1.154849 1.972152 -0.400284 -0.748879 0.528160\n0.000000 0.000000 -0.027848 -0.538692 -0.656398 0.528160\n0.883883 0.883883 1.972152 -0.538692 -0.656398 0.528160\n0.694463 1.039337 1.972152 -0.538692 -0.656398 0.528160\n0.000000 0.000000 -0.027848 -0.656398 -0.538692 0.528160\n1.039337 0.694463 1.972152 -0.656398 -0.538692 0.528160\n0.883883 0.883883 1.972152 -0.656398 -0.538692 0.528160\n0.000000 0.000000 -0.027848 -0.748879 -0.400284 0.528160\n1.154849 0.478354 1.972152 -0.748879 -0.400284 0.528160\n1.039337 0.694463 1.972152 -0.748879 -0.400284 0.528160\n0.000000 0.000000 -0.027848 -0.812581 -0.246494 0.528160\n1.225982 0.243863 1.972152 -0.812581 -0.246494 0.528160\n1.154849 0.478354 1.972152 -0.812581 -0.246494 0.528160\n0.000000 0.000000 -0.027848 -0.845056 -0.083231 0.528160\n1.250000 0.000000 1.972152 -0.845056 -0.083231 0.528160\n1.225982 0.243863 1.972152 -0.845056 -0.083231 0.528160\n0.000000 0.000000 -0.027848 -0.845056 0.083231 0.528160\n1.225982 -0.243863 1.972152 -0.845056 0.083231 0.528160\n1.250000 0.000000 1.972152 -0.845056 0.083231 0.528160\n0.000000 0.000000 -0.027848 -0.812581 0.246494 0.528160\n1.154849 -0.478354 1.972152 -0.812581 0.246494 0.528160\n1.225982 -0.243863 1.972152 -0.812581 0.246494 0.528160\n0.000000 0.000000 -0.027848 -0.748879 0.400284 0.528160\n1.039337 -0.694463 1.972152 -0.748879 0.400284 0.528160\n1.154849 -0.478354 1.972152 -0.748879 0.400284 0.528160\n0.000000 0.000000 -0.027848 -0.656398 0.538692 0.528160\n0.883883 -0.883883 1.972152 -0.656398 0.538692 0.528160\n1.039337 -0.694463 1.972152 -0.656398 0.538692 0.528160\n0.000000 0.000000 -0.027848 -0.538692 0.656398 0.528160\n0.694463 -1.039337 1.972152 -0.538692 0.656398 0.528160\n0.883883 -0.883883 1.972152 -0.538692 0.656398 0.528160\n0.000000 0.000000 -0.027848 -0.400284 0.748879 0.528160\n0.478354 -1.154850 1.972152 -0.400284 0.748879 0.528160\n0.694463 -1.039337 1.972152 -0.400284 0.748879 0.528160\n0.000000 0.000000 -0.027848 -0.246493 0.812581 0.528160\n0.243863 -1.225982 1.972152 -0.246493 0.812581 0.528160\n0.478354 -1.154850 1.972152 -0.246493 0.812581 0.528160\n0.000000 0.000000 -0.027848 -0.083231 0.845056 0.528160\n-0.000000 -1.250000 1.972152 -0.083231 0.845056 0.528160\n0.243863 -1.225982 1.972152 -0.083231 0.845056 0.528160\n0.000000 0.000000 -0.027848 0.083231 0.845056 0.528160\n-0.243863 -1.225981 1.972152 0.083231 0.845056 0.528160\n-0.000000 -1.250000 1.972152 0.083231 0.845056 0.528160\n0.000000 0.000000 -0.027848 0.246494 0.812581 0.528160\n-0.478355 -1.154849 1.972152 0.246494 0.812581 0.528160\n-0.243863 -1.225981 1.972152 0.246494 0.812581 0.528160\n0.000000 0.000000 -0.027848 0.400284 0.748879 0.528160\n-0.694463 -1.039337 1.972152 0.400284 0.748879 0.528160\n-0.478355 -1.154849 1.972152 0.400284 0.748879 0.528160\n0.000000 0.000000 -0.027848 0.538692 0.656398 0.528160\n-0.883884 -0.883883 1.972152 0.538692 0.656398 0.528160\n-0.694463 -1.039337 1.972152 0.538692 0.656398 0.528160\n0.000000 0.000000 -0.027848 0.656398 0.538691 0.528160\n-1.039338 -0.694462 1.972152 0.656398 0.538691 0.528160\n-0.883884 -0.883883 1.972152 0.656398 0.538691 0.528160\n0.000000 0.000000 -0.027848 0.748879 0.400283 0.528160\n-1.154850 -0.478353 1.972152 0.748879 0.400283 0.528160\n-1.039338 -0.694462 1.972152 0.748879 0.400283 0.528160\n0.000000 0.000000 -0.027848 0.812581 0.246493 0.528160\n-1.225982 -0.243862 1.972152 0.812581 0.246493 0.528160\n-1.154850 -0.478353 1.972152 0.812581 0.246493 0.528160\n0.000000 0.000000 -0.027848 0.845056 0.083230 0.528160\n-1.250000 0.000001 1.972152 0.845056 0.083230 0.528160\n-1.225982 -0.243862 1.972152 0.845056 0.083230 0.528160\n0.000000 0.000000 -0.027848 0.845056 -0.083232 0.528160\n-1.225981 0.243864 1.972152 0.845056 -0.083232 0.528160\n-1.250000 0.000001 1.972152 0.845056 -0.083232 0.528160\n0.000000 0.000000 -0.027848 0.812581 -0.246495 0.528160\n-1.154849 0.478356 1.972152 0.812581 -0.246495 0.528160\n-1.225981 0.243864 1.972152 0.812581 -0.246495 0.528160\n0.000000 0.000000 -0.027848 0.748879 -0.400285 0.528160\n-1.039336 0.694464 1.972152 0.748879 -0.400285 0.528160\n-1.154849 0.478356 1.972152 0.748879 -0.400285 0.528160\n0.000000 0.000000 -0.027848 0.656397 -0.538693 0.528160\n-0.883882 0.883885 1.972152 0.656397 -0.538693 0.528160\n-1.039336 0.694464 1.972152 0.656397 -0.538693 0.528160\n0.000000 0.000000 -0.027848 0.538691 -0.656398 0.528160\n-0.694461 1.039338 1.972152 0.538691 -0.656398 0.528160\n-0.883882 0.883885 1.972152 0.538691 -0.656398 0.528160\n0.000000 0.000000 -0.027848 0.400283 -0.748880 0.528160\n-0.478353 1.154850 1.972152 0.400283 -0.748880 0.528160\n-0.694461 1.039338 1.972152 0.400283 -0.748880 0.528160\n0.000000 0.000000 -0.027848 0.246492 -0.812581 0.528160\n-0.243861 1.225982 1.972152 0.246492 -0.812581 0.528160\n-0.478353 1.154850 1.972152 0.246492 -0.812581 0.528160\n0.000000 0.000000 -0.027848 0.083230 -0.845056 0.528160\n0.000000 1.250000 1.972152 0.083230 -0.845056 0.528160\n-0.243861 1.225982 1.972152 0.083230 -0.845056 0.528160\n0.000000 0.000000 1.972152 -0.000000 -0.000000 -1.000000\n0.000000 1.250000 1.972152 -0.000000 -0.000000 -1.000000\n0.243863 1.225982 1.972152 -0.000000 -0.000000 -1.000000\n0.478354 1.154849 1.972152 -0.000000 0.000000 -1.000000\n0.694463 1.039337 1.972152 -0.000000 0.000000 -1.000000\n0.883883 0.883883 1.972152 -0.000000 0.000000 -1.000000\n1.039337 0.694463 1.972152 -0.000000 0.000000 -1.000000\n1.154849 0.478354 1.972152 -0.000000 0.000000 -1.000000\n1.225982 0.243863 1.972152 -0.000000 0.000000 -1.000000\n1.250000 0.000000 1.972152 -0.000000 0.000000 -1.000000\n1.225982 -0.243863 1.972152 -0.000000 0.000000 -1.000000\n1.154849 -0.478354 1.972152 0.000000 0.000000 -1.000000\n1.039337 -0.694463 1.972152 0.000000 0.000000 -1.000000\n0.883883 -0.883883 1.972152 0.000000 0.000000 -1.000000\n0.694463 -1.039337 1.972152 0.000000 0.000000 -1.000000\n0.478354 -1.154850 1.972152 0.000000 0.000000 -1.000000\n0.243863 -1.225982 1.972152 0.000000 0.000000 -1.000000\n-0.000000 -1.250000 1.972152 0.000000 0.000000 -1.000000\n-0.243863 -1.225981 1.972152 0.000000 0.000000 -1.000000\n-0.478355 -1.154849 1.972152 0.000000 0.000000 -1.000000\n-0.694463 -1.039337 1.972152 0.000000 0.000000 -1.000000\n-0.883884 -0.883883 1.972152 0.000000 0.000000 -1.000000\n-1.039338 -0.694462 1.972152 0.000000 0.000000 -1.000000\n-1.154850 -0.478353 1.972152 0.000000 0.000000 -1.000000\n-1.225982 -0.243862 1.972152 0.000000 0.000000 -1.000000\n-1.250000 0.000001 1.972152 0.000000 0.000000 -1.000000\n-1.225981 0.243864 1.972152 0.000000 -0.000000 -1.000000\n-1.154849 0.478356 1.972152 0.000000 -0.000000 -1.000000\n-1.039336 0.694464 1.972152 0.000000 -0.000000 -1.000000\n-0.883882 0.883885 1.972152 0.000000 -0.000000 -1.000000\n-0.694461 1.039338 1.972152 0.000000 -0.000000 -1.000000\n-0.478353 1.154850 1.972152 0.000000 -0.000000 -1.000000\n-0.243861 1.225982 1.972152 0.000000 -0.000000 -1.000000\n1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000\n1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000\n-1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000\n-1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000\n1.000000 0.999999 1.000000 0.000000 -0.000000 1.000000\n-1.000000 1.000000 1.000000 0.000000 -0.000000 1.000000\n-1.000000 -1.000000 1.000000 0.000000 -0.000000 1.000000\n0.999999 -1.000001 1.000000 0.000000 -0.000000 1.000000\n1.000000 1.000000 -1.000000 1.000000 -0.000000 0.000000\n1.000000 0.999999 1.000000 1.000000 -0.000000 0.000000\n0.999999 -1.000001 1.000000 1.000000 -0.000000 0.000000\n1.000000 -1.000000 -1.000000 1.000000 -0.000000 0.000000\n1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000\n0.999999 -1.000001 1.000000 -0.000000 -1.000000 -0.000000\n-1.000000 -1.000000 1.000000 -0.000000 -1.000000 -0.000000\n-1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000\n-1.000000 -1.000000 -1.000000 -1.000000 0.000000 -0.000000\n-1.000000 -1.000000 1.000000 -1.000000 0.000000 -0.000000\n-1.000000 1.000000 1.000000 -1.000000 0.000000 -0.000000\n-1.000000 1.000000 -1.000000 -1.000000 0.000000 -0.000000\n1.000000 0.999999 1.000000 0.000000 1.000000 0.000000\n1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000\n-1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000\n-1.000000 1.000000 1.000000 0.000000 1.000000 0.000000\n3 0 1 2\n3 3 4 5\n3 6 7 8\n3 9 10 11\n3 12 13 14\n3 15 16 17\n3 18 19 20\n3 21 22 23\n3 24 25 26\n3 27 28 29\n3 30 31 32\n3 33 34 35\n3 36 37 38\n3 39 40 41\n3 42 43 44\n3 45 46 47\n3 48 49 50\n3 51 52 53\n3 54 55 56\n3 57 58 59\n3 60 61 62\n3 63 64 65\n3 66 67 68\n3 69 70 71\n3 72 73 74\n3 75 76 77\n3 78 79 80\n3 81 82 83\n3 84 85 86\n3 87 88 89\n3 90 91 92\n3 93 94 95\n3 96 97 98\n3 96 98 99\n3 96 99 100\n3 96 100 101\n3 96 101 102\n3 96 102 103\n3 96 103 104\n3 96 104 105\n3 96 105 106\n3 96 106 107\n3 96 107 108\n3 96 108 109\n3 96 109 110\n3 96 110 111\n3 96 111 112\n3 96 112 113\n3 96 113 114\n3 96 114 115\n3 96 115 116\n3 96 116 117\n3 96 117 118\n3 96 118 119\n3 96 119 120\n3 96 120 121\n3 96 121 122\n3 96 122 123\n3 96 123 124\n3 96 124 125\n3 96 125 126\n3 96 126 127\n3 96 127 128\n3 128 97 96\n4 129 130 131 132\n4 133 134 135 136\n4 137 138 139 140\n4 141 142 143 144\n4 145 146 147 148\n4 149 150 151 152\n" }, { "path": "data/test.dae", "content": "\n\n \n \n VCGLab\n VCGLib | MeshLab\n \n Y_UP\n Thu Sep 12 09:33:03 2013\n Thu Sep 12 09:33:03 2013\n \n \n \n \n \n \n \n \n 0.243863 1.22598 1.97215 0 1.25 1.97215 0 0 -0.027848 0 0 -0.027848 0.478354 1.15485 1.97215 0.243863 1.22598 1.97215 0 0 -0.027848 0.694463 1.03934 1.97215 0.478354 1.15485 1.97215 0 0 -0.027848 0.883883 0.883883 1.97215 0.694463 1.03934 1.97215 0 0 -0.027848 1.03934 0.694463 1.97215 0.883883 0.883883 1.97215 0 0 -0.027848 1.15485 0.478354 1.97215 1.03934 0.694463 1.97215 0 0 -0.027848 1.22598 0.243863 1.97215 1.15485 0.478354 1.97215 0 0 -0.027848 1.25 0 1.97215 1.22598 0.243863 1.97215 0 0 -0.027848 1.22598 -0.243863 1.97215 1.25 0 1.97215 0 0 -0.027848 1.15485 -0.478354 1.97215 1.22598 -0.243863 1.97215 0 0 -0.027848 1.03934 -0.694463 1.97215 1.15485 -0.478354 1.97215 0 0 -0.027848 0.883883 -0.883883 1.97215 1.03934 -0.694463 1.97215 0 0 -0.027848 0.694463 -1.03934 1.97215 0.883883 -0.883883 1.97215 0 0 -0.027848 0.478354 -1.15485 1.97215 0.694463 -1.03934 1.97215 0 0 -0.027848 0.243863 -1.22598 1.97215 0.478354 -1.15485 1.97215 0 0 -0.027848 0 -1.25 1.97215 0.243863 -1.22598 1.97215 0 0 -0.027848 -0.243863 -1.22598 1.97215 0 -1.25 1.97215 0 0 -0.027848 -0.478355 -1.15485 1.97215 -0.243863 -1.22598 1.97215 0 0 -0.027848 -0.694463 -1.03934 1.97215 -0.478355 -1.15485 1.97215 0 0 -0.027848 -0.883884 -0.883883 1.97215 -0.694463 -1.03934 1.97215 0 0 -0.027848 -1.03934 -0.694462 1.97215 -0.883884 -0.883883 1.97215 0 0 -0.027848 -1.15485 -0.478353 1.97215 -1.03934 -0.694462 1.97215 0 0 -0.027848 -1.22598 -0.243862 1.97215 -1.15485 -0.478353 1.97215 0 0 -0.027848 -1.25 1e-06 1.97215 -1.22598 -0.243862 1.97215 0 0 -0.027848 -1.22598 0.243864 1.97215 -1.25 1e-06 1.97215 0 0 -0.027848 -1.15485 0.478356 1.97215 -1.22598 0.243864 1.97215 0 0 -0.027848 -1.03934 0.694464 1.97215 -1.15485 0.478356 1.97215 0 0 -0.027848 -0.883882 0.883885 1.97215 -1.03934 0.694464 1.97215 0 0 -0.027848 -0.694461 1.03934 1.97215 -0.883882 0.883885 1.97215 0 0 -0.027848 -0.478353 1.15485 1.97215 -0.694461 1.03934 1.97215 0 0 -0.027848 -0.243861 1.22598 1.97215 -0.478353 1.15485 1.97215 0 0 -0.027848 0 1.25 1.97215 -0.243861 1.22598 1.97215 0 0 1.97215 0 1.25 1.97215 0.243863 1.22598 1.97215 0.478354 1.15485 1.97215 0.694463 1.03934 1.97215 0.883883 0.883883 1.97215 1.03934 0.694463 1.97215 1.15485 0.478354 1.97215 1.22598 0.243863 1.97215 1.25 0 1.97215 1.22598 -0.243863 1.97215 1.15485 -0.478354 1.97215 1.03934 -0.694463 1.97215 0.883883 -0.883883 1.97215 0.694463 -1.03934 1.97215 0.478354 -1.15485 1.97215 0.243863 -1.22598 1.97215 0 -1.25 1.97215 -0.243863 -1.22598 1.97215 -0.478355 -1.15485 1.97215 -0.694463 -1.03934 1.97215 -0.883884 -0.883883 1.97215 -1.03934 -0.694462 1.97215 -1.15485 -0.478353 1.97215 -1.22598 -0.243862 1.97215 -1.25 1e-06 1.97215 -1.22598 0.243864 1.97215 -1.15485 0.478356 1.97215 -1.03934 0.694464 1.97215 -0.883882 0.883885 1.97215 -0.694461 1.03934 1.97215 -0.478353 1.15485 1.97215 -0.243861 1.22598 1.97215 1 1 -1 1 -1 -1 -1 -1 -1 -1 1 -1 1 0.999999 1 -1 1 1 -1 -1 1 0.999999 -1 1 1 1 -1 1 0.999999 1 0.999999 -1 1 1 -1 -1 1 -1 -1 0.999999 -1 1 -1 -1 1 -1 -1 -1 -1 -1 -1 -1 -1 1 -1 1 1 -1 1 -1 1 0.999999 1 1 1 -1 -1 1 -1 -1 1 1\n \n \n \n \n \n \n \n \n \n -0.0832295 -0.845056 0.52816 -0.246496 -0.81258 0.52816 -0.400282 -0.74888 0.52816 -0.538694 -0.656396 0.52816 -0.656396 -0.538694 0.52816 -0.74888 -0.400282 0.52816 -0.81258 -0.246496 0.52816 -0.845056 -0.0832295 0.52816 -0.845056 0.0832295 0.52816 -0.81258 0.246496 0.52816 -0.74888 0.400282 0.52816 -0.656396 0.538694 0.52816 -0.538694 0.656396 0.52816 -0.400285 0.748878 0.52816 -0.246493 0.812581 0.52816 -0.0832295 0.845056 0.52816 0.0832328 0.845056 0.52816 0.246492 0.812581 0.52816 0.400284 0.748879 0.52816 0.538692 0.656397 0.52816 0.656398 0.538692 0.52816 0.74888 0.400282 0.52816 0.812581 0.246493 0.52816 0.845056 0.0832295 0.52816 0.845056 -0.0832328 0.52816 0.812581 -0.246492 0.52816 0.748878 -0.400287 0.52816 0.656398 -0.538692 0.52816 0.53869 -0.656399 0.52816 0.400284 -0.748879 0.52816 0.246492 -0.812581 0.52816 0.0832302 -0.845056 0.52816 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 0 0 1 1 -2.5332e-07 2.5332e-07 1 -2.5332e-07 2.5332e-07 -2.38419e-07 -1 -2.38419e-07 -2.38419e-07 -1 -2.38419e-07 -1 0 0 -1 0 0 2.5332e-07 1 2.5332e-07 2.5332e-07 1 2.5332e-07\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n

0 0 1 0 2 0 3 1 4 1 5 1 6 2 7 2 8 2 9 3 10 3 11 3 12 4 13 4 14 4 15 5 16 5 17 5 18 6 19 6 20 6 21 7 22 7 23 7 24 8 25 8 26 8 27 9 28 9 29 9 30 10 31 10 32 10 33 11 34 11 35 11 36 12 37 12 38 12 39 13 40 13 41 13 42 14 43 14 44 14 45 15 46 15 47 15 48 16 49 16 50 16 51 17 52 17 53 17 54 18 55 18 56 18 57 19 58 19 59 19 60 20 61 20 62 20 63 21 64 21 65 21 66 22 67 22 68 22 69 23 70 23 71 23 72 24 73 24 74 24 75 25 76 25 77 25 78 26 79 26 80 26 81 27 82 27 83 27 84 28 85 28 86 28 87 29 88 29 89 29 90 30 91 30 92 30 93 31 94 31 95 31 96 32 97 32 98 32 96 33 98 33 99 33 96 34 99 34 100 34 96 35 100 35 101 35 96 36 101 36 102 36 96 37 102 37 103 37 96 38 103 38 104 38 96 39 104 39 105 39 96 40 105 40 106 40 96 41 106 41 107 41 96 42 107 42 108 42 96 43 108 43 109 43 96 44 109 44 110 44 96 45 110 45 111 45 96 46 111 46 112 46 96 47 112 47 113 47 96 48 113 48 114 48 96 49 114 49 115 49 96 50 115 50 116 50 96 51 116 51 117 51 96 52 117 52 118 52 96 53 118 53 119 53 96 54 119 54 120 54 96 55 120 55 121 55 96 56 121 56 122 56 96 57 122 57 123 57 96 58 123 58 124 58 96 59 124 59 125 59 96 60 125 60 126 60 96 61 126 61 127 61 96 62 127 62 128 62 128 63 97 63 96 63 129 64 130 64 131 64 129 65 131 65 132 65 133 66 134 66 135 66 133 67 135 67 136 67 137 68 138 68 139 68 137 69 139 69 140 69 141 70 142 70 143 70 141 71 143 71 144 71 145 72 146 72 147 72 145 73 147 73 148 73 149 74 150 74 151 74 149 75 151 75 152 75

\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "data/test.ply", "content": "ply\nformat ascii 1.0\ncomment Created by Blender 2.62 (sub 0) - www.blender.org, source file: ''\nelement vertex 178\nproperty float x\nproperty float y\nproperty float z\nproperty float nx\nproperty float ny\nproperty float nz\nelement face 70\nproperty list uchar uint vertex_indices\nend_header\n0.162214 -1.005273 2.545801 0.094857 -0.960119 -0.263009\n-0.032876 -1.024490 2.545591 0.094857 -0.960119 -0.263009\n-0.032876 -0.009762 -1.158691 0.094857 -0.960119 -0.263009\n-0.032876 -0.009762 -1.158691 0.280905 -0.923042 -0.262843\n0.349807 -0.948362 2.546426 0.280905 -0.923042 -0.262843\n0.162214 -1.005273 2.545801 0.280905 -0.923042 -0.262843\n-0.032876 -0.009762 -1.158691 0.456096 -0.850330 -0.262518\n0.522694 -0.855943 2.547440 0.456096 -0.850330 -0.262518\n0.349807 -0.948362 2.546426 0.456096 -0.850330 -0.262518\n-0.032876 -0.009762 -1.158691 0.613670 -0.744809 -0.262047\n0.674230 -0.731567 2.548804 0.613670 -0.744809 -0.262047\n0.522694 -0.855943 2.547440 0.613670 -0.744809 -0.262047\n-0.032876 -0.009762 -1.158691 0.747558 -0.610575 -0.261449\n0.798593 -0.580015 2.550467 0.747558 -0.610575 -0.261449\n0.674230 -0.731567 2.548804 0.747558 -0.610575 -0.261449\n-0.032876 -0.009762 -1.158691 0.852619 -0.452827 -0.260747\n0.891003 -0.407111 2.552364 0.852619 -0.452827 -0.260747\n0.798593 -0.580015 2.550467 0.852619 -0.452827 -0.260747\n-0.032876 -0.009762 -1.158691 0.924834 -0.277665 -0.259971\n0.947909 -0.219499 2.554422 0.924834 -0.277665 -0.259971\n0.891003 -0.407111 2.552364 0.924834 -0.277665 -0.259971\n-0.032876 -0.009762 -1.158691 0.961461 -0.091843 -0.259149\n0.967124 -0.024389 2.556563 0.961461 -0.091843 -0.259149\n0.947909 -0.219499 2.554422 0.961461 -0.091843 -0.259149\n-0.032876 -0.009762 -1.158691 0.961129 0.097487 -0.258314\n0.947909 0.170721 2.558703 0.961129 0.097487 -0.258314\n0.967124 -0.024389 2.556563 0.961129 0.097487 -0.258314\n-0.032876 -0.009762 -1.158691 0.923891 0.283056 -0.257499\n0.891003 0.358333 2.560761 0.923891 0.283056 -0.257499\n0.947909 0.170721 2.558703 0.923891 0.283056 -0.257499\n-0.032876 -0.009762 -1.158691 0.851207 0.457750 -0.256733\n0.798593 0.531237 2.562659 0.851207 0.457750 -0.256733\n0.891003 0.358333 2.560761 0.851207 0.457750 -0.256733\n-0.032876 -0.009762 -1.158691 0.745893 0.614885 -0.256047\n0.674230 0.682789 2.564321 0.745893 0.614885 -0.256047\n0.798593 0.531237 2.562659 0.745893 0.614885 -0.256047\n-0.032876 -0.009762 -1.158691 0.612005 0.748457 -0.255464\n0.522694 0.807164 2.565686 0.612005 0.748457 -0.255464\n0.674230 0.682789 2.564321 0.612005 0.748457 -0.255464\n-0.032876 -0.009762 -1.158691 0.454684 0.853366 -0.255008\n0.349807 0.899584 2.566700 0.454684 0.853366 -0.255008\n0.522694 0.807164 2.565686 0.454684 0.853366 -0.255008\n-0.032876 -0.009762 -1.158691 0.279961 0.925609 -0.254694\n0.162214 0.956495 2.567324 0.279961 0.925609 -0.254694\n0.349807 0.899584 2.566700 0.279961 0.925609 -0.254694\n-0.032876 -0.009762 -1.158691 0.094526 0.962433 -0.254534\n-0.032877 0.975712 2.567535 0.094526 0.962433 -0.254534\n0.162214 0.956495 2.567324 0.094526 0.962433 -0.254534\n-0.032876 -0.009762 -1.158691 -0.094526 0.962433 -0.254534\n-0.227967 0.956495 2.567324 -0.094526 0.962433 -0.254534\n-0.032877 0.975712 2.567535 -0.094526 0.962433 -0.254534\n-0.032876 -0.009762 -1.158691 -0.279961 0.925609 -0.254694\n-0.415560 0.899583 2.566700 -0.279961 0.925609 -0.254694\n-0.227967 0.956495 2.567324 -0.279961 0.925609 -0.254694\n-0.032876 -0.009762 -1.158691 -0.454684 0.853366 -0.255008\n-0.588447 0.807164 2.565686 -0.454684 0.853366 -0.255008\n-0.415560 0.899583 2.566700 -0.454684 0.853366 -0.255008\n-0.032876 -0.009762 -1.158691 -0.612005 0.748457 -0.255464\n-0.739984 0.682789 2.564321 -0.612005 0.748457 -0.255464\n-0.588447 0.807164 2.565686 -0.612005 0.748457 -0.255464\n-0.032876 -0.009762 -1.158691 -0.745893 0.614885 -0.256047\n-0.864346 0.531237 2.562659 -0.745893 0.614885 -0.256047\n-0.739984 0.682789 2.564321 -0.745893 0.614885 -0.256047\n-0.032876 -0.009762 -1.158691 -0.851207 0.457750 -0.256733\n-0.956756 0.358332 2.560761 -0.851207 0.457750 -0.256733\n-0.864346 0.531237 2.562659 -0.851207 0.457750 -0.256733\n-0.032876 -0.009762 -1.158691 -0.923891 0.283055 -0.257499\n-1.013662 0.170720 2.558703 -0.923891 0.283055 -0.257499\n-0.956756 0.358332 2.560761 -0.923891 0.283055 -0.257499\n-0.032876 -0.009762 -1.158691 -0.961130 0.097486 -0.258314\n-1.032876 -0.024390 2.556563 -0.961130 0.097486 -0.258314\n-1.013662 0.170720 2.558703 -0.961130 0.097486 -0.258314\n-0.032876 -0.009762 -1.158691 -0.961461 -0.091844 -0.259149\n-1.013661 -0.219500 2.554422 -0.961461 -0.091844 -0.259149\n-1.032876 -0.024390 2.556563 -0.961461 -0.091844 -0.259149\n-0.032876 -0.009762 -1.158691 -0.924833 -0.277666 -0.259971\n-0.956755 -0.407112 2.552364 -0.924833 -0.277666 -0.259971\n-1.013661 -0.219500 2.554422 -0.924833 -0.277666 -0.259971\n-0.032876 -0.009762 -1.158691 -0.852618 -0.452828 -0.260747\n-0.864345 -0.580016 2.550467 -0.852618 -0.452828 -0.260747\n-0.956755 -0.407112 2.552364 -0.852618 -0.452828 -0.260747\n-0.032876 -0.009762 -1.158691 -0.747557 -0.610576 -0.261449\n-0.739982 -0.731568 2.548804 -0.747557 -0.610576 -0.261449\n-0.864345 -0.580016 2.550467 -0.747557 -0.610576 -0.261449\n-0.032876 -0.009762 -1.158691 -0.613669 -0.744810 -0.262047\n-0.588445 -0.855943 2.547440 -0.613669 -0.744810 -0.262047\n-0.739982 -0.731568 2.548804 -0.613669 -0.744810 -0.262047\n-0.032876 -0.009762 -1.158691 -0.456095 -0.850330 -0.262518\n-0.415558 -0.948363 2.546426 -0.456095 -0.850330 -0.262518\n-0.588445 -0.855943 2.547440 -0.456095 -0.850330 -0.262518\n-0.032876 -0.009762 -1.158691 -0.280903 -0.923042 -0.262843\n-0.227965 -1.005274 2.545801 -0.280903 -0.923042 -0.262843\n-0.415558 -0.948363 2.546426 -0.280903 -0.923042 -0.262843\n-0.032876 -0.009762 -1.158691 -0.094857 -0.960119 -0.263009\n-0.032876 -1.024490 2.545591 -0.094857 -0.960119 -0.263009\n-0.227965 -1.005274 2.545801 -0.094857 -0.960119 -0.263009\n-0.032876 -0.024389 2.556563 0.000000 -0.010970 0.999940\n-0.032876 -1.024490 2.545591 0.000000 -0.010970 0.999940\n0.162214 -1.005273 2.545801 0.000000 -0.010970 0.999940\n0.349807 -0.948362 2.546426 -0.000000 -0.010970 0.999940\n-0.032876 -0.024389 2.556563 0.000001 -0.010970 0.999940\n0.349807 -0.948362 2.546426 0.000001 -0.010970 0.999940\n0.522694 -0.855943 2.547440 0.000001 -0.010970 0.999940\n-0.032876 -0.024389 2.556563 -0.000001 -0.010971 0.999940\n0.522694 -0.855943 2.547440 -0.000001 -0.010971 0.999940\n0.674230 -0.731567 2.548804 -0.000001 -0.010971 0.999940\n0.674230 -0.731567 2.548804 0.000001 -0.010970 0.999940\n0.798593 -0.580015 2.550467 0.000001 -0.010970 0.999940\n-0.032876 -0.024389 2.556563 -0.000000 -0.010971 0.999940\n0.798593 -0.580015 2.550467 -0.000000 -0.010971 0.999940\n0.891003 -0.407111 2.552364 -0.000000 -0.010971 0.999940\n0.891003 -0.407111 2.552364 -0.000000 -0.010970 0.999940\n0.947909 -0.219499 2.554422 -0.000000 -0.010970 0.999940\n0.967124 -0.024389 2.556563 -0.000000 -0.010970 0.999940\n0.947909 0.170721 2.558703 -0.000000 -0.010970 0.999940\n0.891003 0.358333 2.560761 0.000000 -0.010970 0.999940\n0.891003 0.358333 2.560761 0.000000 -0.010971 0.999940\n0.798593 0.531237 2.562659 0.000000 -0.010971 0.999940\n-0.032876 -0.024389 2.556563 -0.000001 -0.010970 0.999940\n0.798593 0.531237 2.562659 -0.000001 -0.010970 0.999940\n0.674230 0.682789 2.564321 -0.000001 -0.010970 0.999940\n-0.032876 -0.024389 2.556563 0.000001 -0.010971 0.999940\n0.674230 0.682789 2.564321 0.000001 -0.010971 0.999940\n0.522694 0.807164 2.565686 0.000001 -0.010971 0.999940\n0.522694 0.807164 2.565686 -0.000001 -0.010970 0.999940\n0.349807 0.899584 2.566700 -0.000001 -0.010970 0.999940\n0.349807 0.899584 2.566700 0.000000 -0.010970 0.999940\n0.162214 0.956495 2.567324 0.000000 -0.010970 0.999940\n-0.032877 0.975712 2.567535 -0.000000 -0.010970 0.999940\n-0.227967 0.956495 2.567324 0.000000 -0.010970 0.999940\n-0.415560 0.899583 2.566700 -0.000000 -0.010970 0.999940\n-0.415560 0.899583 2.566700 0.000001 -0.010970 0.999940\n-0.588447 0.807164 2.565686 0.000001 -0.010970 0.999940\n-0.588447 0.807164 2.565686 -0.000001 -0.010971 0.999940\n-0.739984 0.682789 2.564321 -0.000001 -0.010971 0.999940\n-0.739984 0.682789 2.564321 0.000001 -0.010970 0.999940\n-0.864346 0.531237 2.562659 0.000001 -0.010970 0.999940\n-0.864346 0.531237 2.562659 -0.000000 -0.010971 0.999940\n-0.956756 0.358332 2.560761 -0.000000 -0.010971 0.999940\n-0.956756 0.358332 2.560761 0.000000 -0.010970 0.999940\n-1.013662 0.170720 2.558703 0.000000 -0.010970 0.999940\n-1.032876 -0.024390 2.556563 0.000000 -0.010970 0.999940\n-1.013661 -0.219500 2.554422 0.000000 -0.010970 0.999940\n-0.956755 -0.407112 2.552364 0.000000 -0.010970 0.999940\n-0.956755 -0.407112 2.552364 0.000000 -0.010971 0.999940\n-0.864345 -0.580016 2.550467 0.000000 -0.010971 0.999940\n-0.864345 -0.580016 2.550467 -0.000001 -0.010970 0.999940\n-0.739982 -0.731568 2.548804 -0.000001 -0.010970 0.999940\n-0.739982 -0.731568 2.548804 0.000001 -0.010971 0.999940\n-0.588445 -0.855943 2.547440 0.000001 -0.010971 0.999940\n-0.588445 -0.855943 2.547440 -0.000001 -0.010970 0.999940\n-0.415558 -0.948363 2.546426 -0.000001 -0.010970 0.999940\n-0.415558 -0.948363 2.546426 0.000000 -0.010970 0.999940\n-0.227965 -1.005274 2.545801 0.000000 -0.010970 0.999940\n1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000\n1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000\n-1.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000\n-1.000000 1.000000 -1.000000 0.000000 0.000000 -1.000000\n1.000000 0.999999 1.000000 0.000000 -0.000000 1.000000\n-1.000000 1.000000 1.000000 0.000000 -0.000000 1.000000\n-1.000000 -1.000000 1.000000 0.000000 -0.000000 1.000000\n0.999999 -1.000001 1.000000 0.000000 -0.000000 1.000000\n1.000000 1.000000 -1.000000 1.000000 -0.000000 0.000000\n1.000000 0.999999 1.000000 1.000000 -0.000000 0.000000\n0.999999 -1.000001 1.000000 1.000000 -0.000000 0.000000\n1.000000 -1.000000 -1.000000 1.000000 -0.000000 0.000000\n1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000\n0.999999 -1.000001 1.000000 -0.000000 -1.000000 -0.000000\n-1.000000 -1.000000 1.000000 -0.000000 -1.000000 -0.000000\n-1.000000 -1.000000 -1.000000 -0.000000 -1.000000 -0.000000\n-1.000000 -1.000000 -1.000000 -1.000000 0.000000 -0.000000\n-1.000000 -1.000000 1.000000 -1.000000 0.000000 -0.000000\n-1.000000 1.000000 1.000000 -1.000000 0.000000 -0.000000\n-1.000000 1.000000 -1.000000 -1.000000 0.000000 -0.000000\n1.000000 0.999999 1.000000 0.000000 1.000000 0.000000\n1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000\n-1.000000 1.000000 -1.000000 0.000000 1.000000 0.000000\n-1.000000 1.000000 1.000000 0.000000 1.000000 0.000000\n3 0 1 2\n3 3 4 5\n3 6 7 8\n3 9 10 11\n3 12 13 14\n3 15 16 17\n3 18 19 20\n3 21 22 23\n3 24 25 26\n3 27 28 29\n3 30 31 32\n3 33 34 35\n3 36 37 38\n3 39 40 41\n3 42 43 44\n3 45 46 47\n3 48 49 50\n3 51 52 53\n3 54 55 56\n3 57 58 59\n3 60 61 62\n3 63 64 65\n3 66 67 68\n3 69 70 71\n3 72 73 74\n3 75 76 77\n3 78 79 80\n3 81 82 83\n3 84 85 86\n3 87 88 89\n3 90 91 92\n3 93 94 95\n3 96 97 98\n3 96 98 99\n3 100 101 102\n3 103 104 105\n3 100 106 107\n3 108 109 110\n3 96 111 112\n3 96 112 113\n3 96 113 114\n3 96 114 115\n3 108 116 117\n3 118 119 120\n3 121 122 123\n3 118 124 125\n3 96 126 127\n3 96 127 128\n3 96 128 129\n3 96 129 130\n3 100 131 132\n3 103 133 134\n3 100 135 136\n3 108 137 138\n3 96 139 140\n3 96 140 141\n3 96 141 142\n3 96 142 143\n3 108 144 145\n3 118 146 147\n3 121 148 149\n3 118 150 151\n3 96 152 153\n3 153 97 96\n4 154 155 156 157\n4 158 159 160 161\n4 162 163 164 165\n4 166 167 168 169\n4 170 171 172 173\n4 174 175 176 177\n" }, { "path": "data/untitled.ply", "content": "ply\nformat ascii 1.0\ncomment Created by Blender 2.62 (sub 0) - www.blender.org, source file: ''\nelement vertex 129\nproperty float x\nproperty float y\nproperty float z\nproperty float nx\nproperty float ny\nproperty float nz\nelement face 64\nproperty list uchar uint vertex_indices\nend_header\n0.195090 0.980785 -1.000000 0.087754 0.890977 0.445488\n0.000000 1.000000 -1.000000 0.087754 0.890977 0.445488\n0.000000 0.000000 1.000000 0.087754 0.890977 0.445488\n0.000000 0.000000 1.000000 0.259888 0.856737 0.445488\n0.382683 0.923880 -1.000000 0.259888 0.856737 0.445488\n0.195090 0.980785 -1.000000 0.259888 0.856737 0.445488\n0.000000 0.000000 1.000000 0.422036 0.789573 0.445488\n0.555570 0.831470 -1.000000 0.422036 0.789573 0.445488\n0.382683 0.923880 -1.000000 0.422036 0.789573 0.445488\n0.000000 0.000000 1.000000 0.567965 0.692067 0.445488\n0.707107 0.707107 -1.000000 0.567965 0.692067 0.445488\n0.555570 0.831470 -1.000000 0.567965 0.692067 0.445488\n0.000000 0.000000 1.000000 0.692067 0.567965 0.445488\n0.831470 0.555570 -1.000000 0.692067 0.567965 0.445488\n0.707107 0.707107 -1.000000 0.692067 0.567965 0.445488\n0.000000 0.000000 1.000000 0.789573 0.422036 0.445488\n0.923880 0.382683 -1.000000 0.789573 0.422036 0.445488\n0.831470 0.555570 -1.000000 0.789573 0.422036 0.445488\n0.000000 0.000000 1.000000 0.856737 0.259888 0.445488\n0.980785 0.195090 -1.000000 0.856737 0.259888 0.445488\n0.923880 0.382683 -1.000000 0.856737 0.259888 0.445488\n0.000000 0.000000 1.000000 0.890977 0.087754 0.445488\n1.000000 0.000000 -1.000000 0.890977 0.087754 0.445488\n0.980785 0.195090 -1.000000 0.890977 0.087754 0.445488\n0.000000 0.000000 1.000000 0.890977 -0.087753 0.445488\n0.980785 -0.195090 -1.000000 0.890977 -0.087753 0.445488\n1.000000 0.000000 -1.000000 0.890977 -0.087753 0.445488\n0.000000 0.000000 1.000000 0.856737 -0.259888 0.445488\n0.923880 -0.382683 -1.000000 0.856737 -0.259888 0.445488\n0.980785 -0.195090 -1.000000 0.856737 -0.259888 0.445488\n0.000000 0.000000 1.000000 0.789573 -0.422035 0.445488\n0.831470 -0.555570 -1.000000 0.789573 -0.422035 0.445488\n0.923880 -0.382683 -1.000000 0.789573 -0.422035 0.445488\n0.000000 0.000000 1.000000 0.692067 -0.567965 0.445488\n0.707107 -0.707107 -1.000000 0.692067 -0.567965 0.445488\n0.831470 -0.555570 -1.000000 0.692067 -0.567965 0.445488\n0.000000 0.000000 1.000000 0.567965 -0.692067 0.445488\n0.555570 -0.831470 -1.000000 0.567965 -0.692067 0.445488\n0.707107 -0.707107 -1.000000 0.567965 -0.692067 0.445488\n0.000000 0.000000 1.000000 0.422036 -0.789573 0.445488\n0.382683 -0.923880 -1.000000 0.422036 -0.789573 0.445488\n0.555570 -0.831470 -1.000000 0.422036 -0.789573 0.445488\n0.000000 0.000000 1.000000 0.259888 -0.856737 0.445488\n0.195090 -0.980785 -1.000000 0.259888 -0.856737 0.445488\n0.382683 -0.923880 -1.000000 0.259888 -0.856737 0.445488\n0.000000 0.000000 1.000000 0.087753 -0.890977 0.445488\n-0.000000 -1.000000 -1.000000 0.087753 -0.890977 0.445488\n0.195090 -0.980785 -1.000000 0.087753 -0.890977 0.445488\n0.000000 0.000000 1.000000 -0.087754 -0.890977 0.445488\n-0.195091 -0.980785 -1.000000 -0.087754 -0.890977 0.445488\n-0.000000 -1.000000 -1.000000 -0.087754 -0.890977 0.445488\n0.000000 0.000000 1.000000 -0.259889 -0.856737 0.445488\n-0.382684 -0.923879 -1.000000 -0.259889 -0.856737 0.445488\n-0.195091 -0.980785 -1.000000 -0.259889 -0.856737 0.445488\n0.000000 0.000000 1.000000 -0.422036 -0.789573 0.445488\n-0.555571 -0.831469 -1.000000 -0.422036 -0.789573 0.445488\n-0.382684 -0.923879 -1.000000 -0.422036 -0.789573 0.445488\n0.000000 0.000000 1.000000 -0.567965 -0.692066 0.445488\n-0.707107 -0.707106 -1.000000 -0.567965 -0.692066 0.445488\n-0.555571 -0.831469 -1.000000 -0.567965 -0.692066 0.445488\n0.000000 0.000000 1.000000 -0.692067 -0.567964 0.445488\n-0.831470 -0.555570 -1.000000 -0.692067 -0.567964 0.445488\n-0.707107 -0.707106 -1.000000 -0.692067 -0.567964 0.445488\n0.000000 0.000000 1.000000 -0.789574 -0.422035 0.445488\n-0.923880 -0.382683 -1.000000 -0.789574 -0.422035 0.445488\n-0.831470 -0.555570 -1.000000 -0.789574 -0.422035 0.445488\n0.000000 0.000000 1.000000 -0.856737 -0.259887 0.445488\n-0.980785 -0.195089 -1.000000 -0.856737 -0.259887 0.445488\n-0.923880 -0.382683 -1.000000 -0.856737 -0.259887 0.445488\n0.000000 0.000000 1.000000 -0.890977 -0.087753 0.445488\n-1.000000 0.000001 -1.000000 -0.890977 -0.087753 0.445488\n-0.980785 -0.195089 -1.000000 -0.890977 -0.087753 0.445488\n0.000000 0.000000 1.000000 -0.890977 0.087754 0.445488\n-0.980785 0.195091 -1.000000 -0.890977 0.087754 0.445488\n-1.000000 0.000001 -1.000000 -0.890977 0.087754 0.445488\n0.000000 0.000000 1.000000 -0.856737 0.259889 0.445488\n-0.923879 0.382684 -1.000000 -0.856737 0.259889 0.445488\n-0.980785 0.195091 -1.000000 -0.856737 0.259889 0.445488\n0.000000 0.000000 1.000000 -0.789573 0.422037 0.445488\n-0.831469 0.555571 -1.000000 -0.789573 0.422037 0.445488\n-0.923879 0.382684 -1.000000 -0.789573 0.422037 0.445488\n0.000000 0.000000 1.000000 -0.692066 0.567966 0.445488\n-0.707106 0.707108 -1.000000 -0.692066 0.567966 0.445488\n-0.831469 0.555571 -1.000000 -0.692066 0.567966 0.445488\n0.000000 0.000000 1.000000 -0.567964 0.692067 0.445488\n-0.555569 0.831470 -1.000000 -0.567964 0.692067 0.445488\n-0.707106 0.707108 -1.000000 -0.567964 0.692067 0.445488\n0.000000 0.000000 1.000000 -0.422035 0.789574 0.445488\n-0.382682 0.923880 -1.000000 -0.422035 0.789574 0.445488\n-0.555569 0.831470 -1.000000 -0.422035 0.789574 0.445488\n0.000000 0.000000 1.000000 -0.259887 0.856737 0.445488\n-0.195089 0.980786 -1.000000 -0.259887 0.856737 0.445488\n-0.382682 0.923880 -1.000000 -0.259887 0.856737 0.445488\n0.000000 0.000000 1.000000 -0.087753 0.890977 0.445488\n0.000000 1.000000 -1.000000 -0.087753 0.890977 0.445488\n-0.195089 0.980786 -1.000000 -0.087753 0.890977 0.445488\n0.000000 0.000000 -1.000000 -0.000000 -0.000000 -1.000000\n0.000000 1.000000 -1.000000 -0.000000 -0.000000 -1.000000\n0.195090 0.980785 -1.000000 -0.000000 -0.000000 -1.000000\n0.382683 0.923880 -1.000000 -0.000000 0.000000 -1.000000\n0.555570 0.831470 -1.000000 -0.000000 0.000000 -1.000000\n0.707107 0.707107 -1.000000 -0.000000 0.000000 -1.000000\n0.831470 0.555570 -1.000000 -0.000000 0.000000 -1.000000\n0.923880 0.382683 -1.000000 -0.000000 0.000000 -1.000000\n0.980785 0.195090 -1.000000 -0.000000 0.000000 -1.000000\n1.000000 0.000000 -1.000000 -0.000000 0.000000 -1.000000\n0.980785 -0.195090 -1.000000 -0.000000 0.000000 -1.000000\n0.923880 -0.382683 -1.000000 0.000000 0.000000 -1.000000\n0.831470 -0.555570 -1.000000 0.000000 0.000000 -1.000000\n0.707107 -0.707107 -1.000000 0.000000 0.000000 -1.000000\n0.555570 -0.831470 -1.000000 0.000000 0.000000 -1.000000\n0.382683 -0.923880 -1.000000 0.000000 0.000000 -1.000000\n0.195090 -0.980785 -1.000000 0.000000 0.000000 -1.000000\n-0.000000 -1.000000 -1.000000 0.000000 0.000000 -1.000000\n-0.195091 -0.980785 -1.000000 0.000000 0.000000 -1.000000\n-0.382684 -0.923879 -1.000000 0.000000 0.000000 -1.000000\n-0.555571 -0.831469 -1.000000 0.000000 0.000000 -1.000000\n-0.707107 -0.707106 -1.000000 0.000000 0.000000 -1.000000\n-0.831470 -0.555570 -1.000000 0.000000 0.000000 -1.000000\n-0.923880 -0.382683 -1.000000 0.000000 0.000000 -1.000000\n-0.980785 -0.195089 -1.000000 0.000000 0.000000 -1.000000\n-1.000000 0.000001 -1.000000 0.000000 0.000000 -1.000000\n-0.980785 0.195091 -1.000000 0.000000 -0.000000 -1.000000\n-0.923879 0.382684 -1.000000 0.000000 -0.000000 -1.000000\n-0.831469 0.555571 -1.000000 0.000000 -0.000000 -1.000000\n-0.707106 0.707108 -1.000000 0.000000 -0.000000 -1.000000\n-0.555569 0.831470 -1.000000 0.000000 -0.000000 -1.000000\n-0.382682 0.923880 -1.000000 0.000000 -0.000000 -1.000000\n-0.195089 0.980786 -1.000000 0.000000 -0.000000 -1.000000\n3 0 1 2\n3 3 4 5\n3 6 7 8\n3 9 10 11\n3 12 13 14\n3 15 16 17\n3 18 19 20\n3 21 22 23\n3 24 25 26\n3 27 28 29\n3 30 31 32\n3 33 34 35\n3 36 37 38\n3 39 40 41\n3 42 43 44\n3 45 46 47\n3 48 49 50\n3 51 52 53\n3 54 55 56\n3 57 58 59\n3 60 61 62\n3 63 64 65\n3 66 67 68\n3 69 70 71\n3 72 73 74\n3 75 76 77\n3 78 79 80\n3 81 82 83\n3 84 85 86\n3 87 88 89\n3 90 91 92\n3 93 94 95\n3 96 97 98\n3 96 98 99\n3 96 99 100\n3 96 100 101\n3 96 101 102\n3 96 102 103\n3 96 103 104\n3 96 104 105\n3 96 105 106\n3 96 106 107\n3 96 107 108\n3 96 108 109\n3 96 109 110\n3 96 110 111\n3 96 111 112\n3 96 112 113\n3 96 113 114\n3 96 114 115\n3 96 115 116\n3 96 116 117\n3 96 117 118\n3 96 118 119\n3 96 119 120\n3 96 120 121\n3 96 121 122\n3 96 122 123\n3 96 123 124\n3 96 124 125\n3 96 125 126\n3 96 126 127\n3 96 127 128\n3 128 97 96\n" }, { "path": "package.xml", "content": "\n\n mono-slam\n 0.0.1\n Mono-SLAM implementation for ROS\n\n \n \n \n Ludovico O. Russo\n\n\n \n \n \n BSDv2.0\n\n\n \n \n \n \n\n\n \n \n \n \n\n\n \n \n \n \n \n \n \n \n \n \n \n catkin\n\n \n cv_bridge\n \tOpenCV\n \tsensor_msgs\n \topencv2\n \tcv_bridge\n \troscpp\n \tstd_msgs\n \timage_transport\n \n \n cv_bridge\n \tOpenCV\n \tsensor_msgs\n \topencv2\n \tcv_bridge\n \troscpp\n \tstd_msgs\n \timage_transport\n \n\n \n \n\n" }, { "path": "src/ConfigVSLAM.cpp", "content": "/*\n * ConfigVSLAM.cpp\n *\n * Created on: Jul 10, 2013\n * Author: ludovico\n */\n\n#include \"ConfigVSLAM.h\"\n\n#include \n#include \n#include \n#include \n\n\nusing namespace libconfig;\nusing namespace std;\n\ntemplate\nvoid lookupAndPrintValue(Config & cfg, string name_value, T & value) {\n\tbool flag = cfg.lookupValue(name_value, value);\n\tcout << setw(20) << name_value << setw(30) << value << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n}\n\n\n\n\n\nConfigVSLAM::ConfigVSLAM(char *file) {\n\tsigma_vx = sigma_vy = sigma_vz = 0.01f;\n\tsigma_wx = sigma_wy = sigma_wz = 0.01f;\n\n\twindow_size = 21;\n\tsigma_pixel = 2;\n\n\trho_0 = 0.1f;\n\tsigma_rho_0 = 0.25f;\n\t\n\tscale = 1;\n\n\tT_camera = 0.5f;\n\n\tsigma_size = 2;\n\n\tnInitFeatures = 5;\n\n\tmin_features = 30;\n\tmax_features = 100;\n\n\tforsePlane = 0;\n\n\n\n\tcout << \"-------------------------------------------------------------------------------------------------\" << endl;\n\n\tif (file) {\n\n\t\tConfig cfg;\n\t\tbool flag;\n\t\tcout << \"Opening configuration File \" << file << endl;\n\n\t\tcfg.readFile(file);\n\t\tcout << \"Reading configuration File \" << file << endl;\n\n\t\tcout << \"-------------------------------------------------------------------------------------------------\" << endl;\n\t\tlookupAndPrintValue(cfg, \"sigma_vx\", sigma_vx);\n\t\tlookupAndPrintValue(cfg, \"sigma_wx\", sigma_wx);\n\t\tlookupAndPrintValue(cfg, \"sigma_vy\", sigma_vy);\n\t\tlookupAndPrintValue(cfg, \"sigma_wy\", sigma_wy);\n\t\tlookupAndPrintValue(cfg, \"sigma_vz\", sigma_vz);\n\t\tlookupAndPrintValue(cfg, \"sigma_wz\", sigma_wz);\n\n\n\t\tlookupAndPrintValue(cfg, \"rho_0\", rho_0);\n\t\tlookupAndPrintValue(cfg, \"sigma_rho_0\", sigma_rho_0);\n\n\t\tlookupAndPrintValue(cfg, \"window_size\", window_size);\n\t\tlookupAndPrintValue(cfg, \"sigma_pixel\", sigma_pixel);\n\t\t\n\t\tlookupAndPrintValue(cfg, \"kernel_size\", kernel_size);\n\t\t\n\t\tlookupAndPrintValue(cfg, \"scale\", scale);\n\t\tlookupAndPrintValue(cfg, \"T_camera\", T_camera);\n\t\tlookupAndPrintValue(cfg, \"sigma_size\", sigma_size);\n\t\t\n\t\tlookupAndPrintValue(cfg, \"nInitFeatures\", nInitFeatures);\n\t\tlookupAndPrintValue(cfg, \"min_features\", min_features);\n\t\tlookupAndPrintValue(cfg, \"max_features\", max_features);\n\n\t\tlookupAndPrintValue(cfg, \"forsePlane\", forsePlane);\n\n\n\t\tconst Setting &root = cfg.getRoot();\n\t\tconst Setting &camera = root[\"camera\"];\n\t\t\n\t\tflag = camera.lookupValue(\"fx\", camParams.fx);\n\t\tcamParams.fx = camParams.fx/scale;\n\t\tcout << setw(20) << \"camera.fx\" << setw(30) << camParams.fx << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"fy\", camParams.fy);\n\t\tcamParams.fy = camParams.fy/scale;\n\t\tcout << setw(20) << \"camera.fy\" << setw(30) << camParams.fy << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"u0\", camParams.u0);\n\t\tcamParams.u0 = camParams.u0/scale;\n\t\tcout << setw(20) << \"camera.u0\" << setw(30) << camParams.u0 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"v0\", camParams.v0);\n\t\tcamParams.v0 = camParams.v0/scale;\n\n\n\n\t\tcout << setw(20) << \"camera.v0\" << setw(30) << camParams.v0 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"k1\", camParams.k1);\n\t\tcout << setw(20) << \"camera.k1\" << setw(30) << camParams.k1 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"k2\", camParams.k2);\n\t\tcout << setw(20) << \"camera.k2\" << setw(30) << camParams.k2 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"k3\", camParams.k3);\n\t\tcout << setw(20) << \"camera.k3\" << setw(30) << camParams.k3 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"p1\", camParams.p1);\n\t\tcout << setw(20) << \"camera.p1\" << setw(30) << camParams.p1 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\tflag = camera.lookupValue(\"p2\", camParams.p2);\n\t\tcout << setw(20) << \"camera.p2\" << setw(30) << camParams.p2 << setw(30) << (flag ? \"Read\" : \"Default\") << endl;\n\t\t\n\n\n\n\n\t} else {\n\t\tcout << \"No configuration File\" << endl;\n\t\tcout << \"-------------------------------------------------------------------------------------------------\" << endl;\n\n\t\tconst bool flag = false;\n\t\tcout << setw(20) << \"sigma_vx\" << setw(30) << sigma_vx << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_vy\" << setw(30) << sigma_vy << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_vz\" << setw(30) << sigma_vz << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_wx\" << setw(30) << sigma_wx << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_wy\" << setw(30) << sigma_wy << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_wz\" << setw(30) << sigma_wz << setw(30) << \"Default\" << endl;\n\n\t\tcout << setw(20) << \"window_size\"<< setw(30) << window_size << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_pixel\"<< setw(30) << sigma_pixel << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"rho_0\"<< setw(30) << rho_0 << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_rho_0\"<< setw(30) << sigma_rho_0 << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"sigma_size\"<< setw(30) << sigma_size << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"nInitFeatures\"<< setw(30) << nInitFeatures << setw(30) << \"Default\" << endl;\n\t\tcout << setw(20) << \"forsePlane\"<< setw(30) << forsePlane << setw(30) << \"Default\" << endl;\n\n\t}\n\n\tcout << \"-------------------------------------------------------------------------------------------------\" << endl;\n\n\n}\n\n" }, { "path": "src/ConfigVSLAM.h", "content": "/*\n * ConfigVSLAM.h\n *\n * Created on: Jul 10, 2013\n * Author: ludovico\n */\n\n#ifndef CONFIGVSLAM_H_\n#define CONFIGVSLAM_H_\n\n#include \n#include \"camModel.hpp\"\n\n\n\nclass ConfigVSLAM {\npublic:\n\tConfigVSLAM(char *file = NULL);\n\n\tfloat sigma_vx, sigma_vy, sigma_vz;\n\tfloat sigma_wx, sigma_wy, sigma_wz;\n\n\tfloat rho_0;\n\tfloat sigma_rho_0;\n\n\tint window_size;\n\tint sigma_pixel;\n\t\n\tint kernel_size;\n\tint sigma_size;\n\t\n\tcamConfig camParams;\n\tint scale;\n\n\tfloat T_camera;\n\n\tint nInitFeatures;\n\tint min_features;\n\tint max_features;\n\n\tint forsePlane;\n\n};\n\n#endif /* CONFIGVSLAM_H_ */\n" }, { "path": "src/FeatureModel.cpp", "content": "/*\n * FeatureModel.cpp\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#include \"FeatureModel.h\"\n\nFeatureModel::FeatureModel(\tconst CamModel & _camera,\n\t\t\t\t\t\t\t\tconst MotionModel & _motion,\n\t\t\t\t\t\t\t\tconst FeaturesState & _featuresState,\n\t\t\t\t\t\t\t\tconst Mat & frame,\n\t\t\t\t\t\t\t\tconst Point2f & pf,\n\t\t\t\t\t\t\t\tint patchSize,\n\t\t\t\t\t\t\t\tfloat rho_0,\n\t\t\t\t\t\t\t\tfloat sigma_rho):\n\t\t\t\t\t\t\t\tcamera(_camera),\n\t\t\t\t\t\t\t\tmotion(_motion),\n\t\t\t\t\t\t\t\tfeatures(_featuresState)\n\t{\n Mat newPatch(cv::Mat(frame, cv::Rect(pf.x-patchSize/2, pf.y-patchSize/2, patchSize,patchSize)));\n this->imageLocation << pf.x, pf.y;\n\n this->Patch = newPatch.clone();\n\n\n Vector2f hd = (Vector2f << (float)pf.x, (float)pf.y);\n\n // TODO: convert using motionmodel\n Vector3f r = motion->get_r();\n Quatenionf q = motion->get_q();\n\n\n Vector3f hC = this->camera.unproject(hd, true);\n Matrix3f Jac_hCHd = this->camera.getUnprojectionJacobian();\n\n Matrix3f Rot = quat2rot(q);\n\n Vector3f hW = Rot*hC;\n\n float hx = hW(0);\n float hy = hW(1);\n float hz = hW(2);\n\n\n\n float theta = atan2(hx,hz);\n float phi = atan2(-hy, sqrt(hx*hx+hz*hz));\n\n\t// Updating state and Sigma\n MatrixXf Jx = this->computeJx();\n\tMatrixXf Jm = this->computeJm();\n\tMatrix2f Sigma_mm = this->computeSigma_mm(sigma_pixel, sigma_pixel);\n\n\tthis->f = (VectorXf(6) << r, theta, phi, rho_0);\n\tthis->Sigma_ii = Jm*Sigma_mm*Jm.transpose() + Jx*motion->getSigma_xx()*Jx.transpose();\n\n\tthis->motion->updateVariaceBlocks(Jx);\n\tthis->features->updateVariaceBlocks(Jx);\n\tthis->features.push_back((*this));\n return 1;\n\n}\n\nFeatureModel::~FeatureModel() {\n\t// TODO Auto-generated destructor stub\n}\n\nMatrix3f FeatureModel::computeSigma_mm(float sigma_pixel, float sigma_rho) {\n\tMatrix3f Sigma_mm = pow(sigma_pixel,2)*Matrix3f::Identity();\n\tSigma_mm(2,2) = pow(sigma_rho,2);\n\treturn Sigma_mm;\n}\n\nMatrix FeatureModel::computeJx() {\n\t// TODO\n\tMatrix Jx = Matrix::Zero();\n\tJx.block<3,3>(0,0) = Matrix3f::Identity();\n\n\tMatrixXf Jf_hW = d_f_d_hW(hW);\n MatrixXf J_hW_q = dRq_times_d_dq(q,hC);\n Jx.block<6,4>(0,3) = Jf_hW*J_hW_q;\n return Jx;\n}\n\nMatrix FeatureModel::computeJm() {\n\n\t// TODO\n\tMatrix Jm = Matrix::Zero();\n MatrixXf Jf_hW = d_f_d_hW(hW);\n Jm.block<6,2>(0,0) = Jf_hW*Rot*Jac_hCHd;\n Js.block<6,1>(0, 2) << 0,0,0,0,0,1;\n return Jm;\n}\n\n" }, { "path": "src/FeatureModel.h", "content": "/*\n * FeatureModel.h\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#ifndef FEATUREMODEL_H_\n#define FEATUREMODEL_H_\n\n#include \n#include \n\n#include \"MotionModel.h\"\n#include \"cameraModel.hpp\"\n#include \"FeaturesState.h\"\n\n\nusing namespace Eigen;\nusing namespace std;\nusing namespace cv;\n\n\nclass FeatureModel {\nprivate:\n\tMatrixXf H_matrix;\n\tMat Patch;\n\n\tMatrix computeJx();\n\tMatrix computeJm();\n\tMatrix3f computeSigma_mm(float sigma_pixel = 1, float sigma_rho = 0.5);\n\n\tconst CameraModel & camera;\n\tconst MotionModel & motion;\n\tconst FeaturesState & features;\n\n\npublic:\n\tFeatureModel(const CameraModel & _camera);\n\tvirtual ~FeatureModel();\n\n\tbool isInInverseDepth;\n\n\tVector2f imageLocation;\n\n\tVectorXf f;\n\n\tVector2f z;\n\tVector2f h;\n\tMatrixXf Sigma_ii;\n\tvector Sigma_ij;\n\n\tMatrix3f get3DCovariance();\n\tMatrix2f get2DCovariance();\n\n\tVector2f get2DPosition();\n\tVector3f get3DPosition();\n\n\tVector2f predict();\n};\n\n#endif /* FEATUREMODEL_H_ */\n" }, { "path": "src/FeaturesState.cpp", "content": "/*\n * FeaturesState.cpp\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#include \"FeaturesState.h\"\n\nFeaturesState::FeaturesState() {\n\t// TODO Auto-generated constructor stub\n\n}\n\nFeaturesState::~FeaturesState() {\n\t// TODO Auto-generated destructor stub\n}\n" }, { "path": "src/FeaturesState.h", "content": "/*\n * FeaturesState.h\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#ifndef FEATURESSTATE_H_\n#define FEATURESSTATE_H_\n\n#include \n#include \"FeatureModel.h\"\n\nusing namespace Eigen;\nusing namespace std;\n\n\nclass FeaturesState: public vector{\npublic:\n\tFeaturesState();\n\tvirtual ~FeaturesState();\n\tvoid updateVariaceBlocks(MatrixXf Jx);\n};\n\n#endif /* FEATURESSTATE_H_ */\n" }, { "path": "src/Patch.cpp", "content": "#include \"Patch.hpp\"\n#include \n#include \n#include \"libblur.h\"\n\n#include \n\nfloat computeCorrelation(cv::Mat f1, cv::Mat f2);\n\n\nvoid Patch::blurTest(const Vector2f &p1, const Vector2f &p2, int kernel_min_size, int iName, int deltaT) {\n\n\n\tcv::Mat blurredPatch;\n\tif ((p1-p2).norm() > kernel_min_size) {\n\t\tblurredPatch = blurPatch(patch, cv::Point2f(p1(0), p1(1)), cv::Point2f(p2(0), p2(1))).clone();\n\t} else {\n\t\tblurredPatch = patch.clone();\n\t}\n\n\tchar name[100];\n\tsprintf(name, \"%04d-%02d-%03d.jpg\", imgCounter++, iName, deltaT);\n\tstd::cout << name << std::endl;\n\n\tcv::imwrite(name, blurredPatch);\n\tcv::imshow(\"cioa\", blurredPatch);\n\tcv::waitKey(1);\n\n}\n\n\nvoid Patch::blur(const Vector2f &p1, const Vector2f &p2, int kernel_min_size) {\n\n\tif ((p1-p2).norm() > kernel_min_size) {\n\t\tmatching_patch = blurPatch(patch, cv::Point2f(p1(0), p1(1)), cv::Point2f(p2(0), p2(1))).clone();\n\t} else {\n\t\tmatching_patch = patch.clone();\n\t}\n}\n\nvoid Patch::deblur(const Vector2f &p1, const Vector2f &p2) {\n\tpatch = deblurPatch(patch, cv::Point2f(p1(0), p1(1)), cv::Point2f(p2(0), p2(1)));\n}\n\n\nvoid Patch::change_position(int dp) {\n\tposition_in_state += dp;\n}\n\n\nbool Patch::isXYZ() {\n\treturn coding;\n}\n\nvoid Patch::convertInXYZ() {\n\tcoding = XYZ;\n}\n\nPatch::Patch(const cv::Mat &p, cv::Point2f c, int position) {\n\tnumMatch = 1;\n\tnumMismatch = 0;\n\tn_tot = 1;\n\tn_find = 1;\n\tisInInnovation = false;\n\tisInLi = false;\n\tisInHi = false;\n\tremoveFlag = false;\n\n\tpatch = p.clone();\n\tcenter = c;\n\transacFlag = true;\n\n\n\tquality_index = 0.0f;\n\tcoding = INV;\n\tposition_in_state = position;\n\n\t imgCounter = 0;\n\n}\n\nvoid Patch::setRansac() {\n\transacFlag = false;\n}\n\n\nbool Patch::ransacFound() {\n\treturn ransacFlag;\n}\n\n\nvoid Patch::setRemove() {\n\tremoveFlag = true;\n}\nbool Patch::mustBeRemove() {\n\treturn removeFlag;\n}\n\n\nvoid Patch::setIsInInnovation(bool flag) {\n\tisInInnovation = flag;\n\tisInLi = isInHi = false;\n\transacFlag = flag;\n\tisInLiDef = false;\n#ifndef USE_RANSAC\n\t//\tn_find++;\n#endif\n\n}\n\nvoid Patch::setIsInLi(bool flag){\n\tisInLi = flag;\n}\n\nvoid Patch::ConfirmIsInLi(){\n\tisInLiDef = isInLi;\n}\n\nvoid Patch::update_quality_index() {\n\t\n\tfloat lambda = 0.95;\n\tfloat mu = 0.5;\n\tif (this->patchIsInHi() || this->patchIsInLi()) n_find++;\n\n\tquality_index = (float)(n_tot - n_find)/((float)n_find);\n\n\tif (quality_index > 0.2) this->setRemove();\n}\n\n\nfloat Patch::get_quality_index() {\n\treturn quality_index;\n\n}\n\n\n\n\nvoid Patch::setIsInHi(bool flag) {\n\tisInHi = flag;\n\n\t/*\n\tnumMatch = 0.9*numMatch;\n\tnumMismatch = 0.9*numMismatch;\n\t*/\n\t\n\t\n\tif(!isInHi) ransac_index = 0.5;\n\telse {\n\t\transac_index = 0;\n#ifdef USE_RANSAC\n\t\t//n_find++;\n#endif\n\t}\n\n}\n\nbool Patch::patchIsInInnovation() {\n\treturn isInInnovation;\n}\n\nbool Patch::patchIsInLi(){\n\treturn isInLi;\n}\n\nbool Patch::patchIsInHi(){\n\treturn isInHi;\n}\n\n\n\nvoid Patch::drawUpdate(cv::Mat &img, int index) {\n\n\t//std::cout << \"feature \" << index << \"is in innovation = \" << this->patchIsInInnovation() << std::endl;\n\n\tif (!this->patchIsInInnovation()) return;\n\tint windowSize = patch.cols;\n cv::rectangle(img, cv::Point(z(0) - windowSize/2, z(1) - windowSize/2), cv::Point(z(0) + windowSize/2, z(1) + windowSize/2), CV_RGB(0,0,255), 1);\n //cv::Rect roi = cv::Rect(z(0) - windowSize/2, z(1) - windowSize/2, windowSize, windowSize);\n //this->patch.copyTo(img(roi));\n if (isInHi) cv::circle(img, cv::Point(z(0),z(1)), 3, CV_RGB(0,255,0) ,2);\n else if (isInLi) cv::circle(img, cv::Point(z(0),z(1)), 3, CV_RGB(0,0,255) ,2);\n else if (isInInnovation) cv::circle(img, cv::Point(z(0),z(1)), 3, CV_RGB(255,0,0) ,2);\n\n // char text[10];\n\n // sprintf(text,\"%d\",index);\n // cv::putText(img, text, cv::Point(z(0) - windowSize/2, z(1) - windowSize/2), cv::FONT_HERSHEY_SIMPLEX, 0.5, CV_RGB(255,0,0));\n}\n\nbool Patch::findMatch(cv::Mat frame, MatrixXf covarianceMatrix, float sigma_size, bool showimg_flag) {\n\n\tstatic int imgn = 0;\n\tn_tot++;\n int windowSize = matching_patch.cols;\n \n int uc = h(0);\n int vc = h(1);\n MatrixXf invSMatrix = covarianceMatrix.inverse();\n\tcv::Mat newPatch;\n\tcv::Mat newPatch_no_blur;\n\n \n float x_2_coeff = invSMatrix(0,0);\n float y_2_coeff = invSMatrix(1,1);\n float yx_coeff = 2*invSMatrix(1,0);\n\n float max = -1;\n \n float max_no_blurr = -1;\n\n float sigma_2 = sigma_size*sigma_size;\n \n float delta_u = sigma_size*sqrt((double)covarianceMatrix(0,0));\n float delta_v = sigma_size*sqrt((double)covarianceMatrix(1,1));\n if (delta_u > 20) delta_u = 20;\n if (delta_v > 20) delta_v = 20;\n\n float newmax, new_max_no_blurr;\n for (int i = uc - delta_u; i <= uc + delta_u; i++) {\n for (int j = vc - delta_v; j <= vc + delta_v; j++) {\n if (i > windowSize/2 && j > windowSize/2 && i < frame.size().width - windowSize/2 && j < frame.size().height - windowSize/2) {\n if (x_2_coeff*(i - uc)*(i - uc) + y_2_coeff*(j - vc)*(j - vc) + yx_coeff*(i - uc)*(j - vc) <= sigma_2) {\n cv::Mat subImg = cv::Mat(frame, cv::Rect( i - windowSize/2, j-windowSize/2, windowSize,windowSize));\n newmax = computeCorrelation(this->matching_patch, subImg);\n // new_max_no_blurr = computeCorrelation(this->patch, subImg);\n\n if ( newmax > max) {\n this->center = cv::Point2f(i,j);\n max = newmax;\n newPatch = subImg.clone();\n }\n }\n }\n }\n }\n \n this->matched_patch_blur = newPatch;\n\n if (showimg_flag) {\n\t\tcv::Mat img;\n\t\thconcat(this->patch, this->matching_patch, img);\n\t\thconcat(img, this->matched_patch_blur, img);\n\t\timshow(\"Blurring\", img);\n\t\tchar name[100];\n\t\tsprintf(name, \"blur%03d.jpg\",imgn++);\n\t\tcv::imwrite(name, img);\n\t\tcv::waitKey(1);\n }\n\n\n\n\n if (max < 0.8) {\n this->center = cv::Point2f(-1,-1);\n this->founded = false;\n this->setIsInInnovation(false);\n return false;\n } else {\n\t\tthis->founded = true;\n\t// this->patch = 0.95*this->patch.clone() + 0.05*newPatch.clone();\n\t\tthis->z(0) = this->center.x;\n\t\tthis->z(1) = this->center.y;\n // this->setIsInInnovation(true);\n\t\treturn true;\n }\n}\n\nfloat computeCorrelation(cv::Mat f1, cv::Mat f2) {\n\n\tint step = 1;\n\n double m1 = 0, m2 = 0, n1 = 0, n2 = 0;\n double corr = 0;\n\n int n = f1.rows*f1.cols;\n\n for (int i = 0; i < f1.rows; i+=step) {\n for (int j = 0; j < f1.cols; j+=step) {\n m1 += f1.at(i,j);\n m2 += f2.at(i,j);\n }\n }\n\n m1 = m1/n;\n m2 = m2/n;\n\n float s1 = 0, s2 = 0;\n for (int i = 0; i < f1.rows; i+=step) {\n for (int j = 0; j < f1.cols; j+=step) {\n s1 = f1.at(i,j);\n n1 += (s1-m1)*(s1-m1);\n\n s2 = f2.at(i,j);\n n2 += (s2-m2)*(s2-m2);\n\n corr += (s1-m1)*(s2-m2);\n }\n }\n return corr/sqrt(n2*n1);\n\n}\n\n" }, { "path": "src/Patch.hpp", "content": "#ifndef __PATCH_CLASS__\n#define __PATCH_CLASS__\n\n#include \n#include \n\n\n\n\n#define XYZ true\n#define INV false\n\n#define USE_RANSAC\n\n\nusing namespace Eigen;\n\nclass Patch {\nprotected:\n\tint nMatch;\n int nMisMatch;\n bool founded;\n\tbool isInInnovation;\n\tbool isInLi;\n\tbool isInLiDef;\n\tbool isInHi;\n\tbool removeFlag;\n\tbool ransacFlag;\n\t\n\tfloat quality_index;\n\tfloat ransac_index;\n\t\n\npublic:\n\tint position_in_z;\n\tint n_tot, n_find;\n\n\n\tvoid update_quality_index();\n\tfloat get_quality_index();\n\n\tvoid setIsInInnovation(bool flag);\n\tvoid setIsInLi(bool flag);\n\tvoid setIsInHi(bool flag);\n\tvoid ConfirmIsInLi();\n\n\tbool patchIsInInnovation();\n\tbool patchIsInLi();\n\tbool patchIsInHi();\n\n\tvoid setRemove();\n\tbool mustBeRemove();\n\n\tbool ransacFound();\n\tvoid setRansac();\n\n\tvoid blur(const Vector2f &p1, const Vector2f &p2, int kernel_min_size);\n\tvoid deblur(const Vector2f &p1, const Vector2f &p2);\n\n\tvoid blurTest(const Vector2f &p1, const Vector2f &p2, int kernel_min_size, int iName, int deltaT);\n\n\n\t\n\tint position_in_state;\n\tbool coding;\n\n MatrixXd mPatch;\n\n cv::Mat patch;\n cv::Mat matching_patch;\n\n cv::Mat matched_patch;\n cv::Mat matched_patch_blur;\n\n\n\n cv::Point2f center;\n\t\n\n\tfloat numMatch;\n\tfloat numMismatch;\n\n\tbool findMatch(cv::Mat frame, MatrixXf covarianceMatrix, float sigma_size = 3.0f, bool showimg_flag = false);\n\tPatch(const cv::Mat &p, cv::Point2f c, int position = 0);\n\t\n\tVector2f z;\n\tVector2f h;\n\t\n\tMatrixXf H; \n\t\n\tVector3f XYZ_pos;\n\n\t\n\tvoid change_position(int dp);\n\n\n\n\tvoid drawUpdate(cv::Mat &img, int index);\n\t\n\tbool isXYZ();\n\tvoid convertInXYZ();\n\n\t int imgCounter;\n\n};\n\n#endif\n" }, { "path": "src/Patches.cpp", "content": "" }, { "path": "src/Patches.hpp", "content": "#include \"Patch.hpp\"\n\n#ifndef __PATCHES_CLASS__\n#define __PATCHES_CLASS__\n\n#include \n#include \n\n\nusing namespace Eigen;\n\nclass Patch: public std::vector {\nprotected:\n\t\n\t\n\t\n\t\npublic:\n\n};\n\n#endif\n" }, { "path": "src/RosVSLAMRansac.cpp", "content": "#include \"RosVSLAMRansac.hpp\"\n#include \n#include \n#include \n\n\n // #define DRAW_COV\n\n\nRosVSLAM::RosVSLAM(char *file): VSlamFilter(file) {\n\tthis->cameraPath.header.frame_id = \"/world\";\n\tthis->cameraPath.header.stamp = ros::Time::now();\n}\n\ngeometry_msgs::Pose RosVSLAM::getCameraPose() {\n\tgeometry_msgs::Pose p;\n\n\tVectorXf state = getState();\n\n\tp.position.x = state(0)*map_scale;\n\tp.position.y = state(1)*map_scale;\n\tp.position.z = state(2)*map_scale;\n\tp.orientation.x = state(4);\n\tp.orientation.y = state(5);\n\tp.orientation.z = state(6);\n\tp.orientation.w = state(3);\n\treturn p;\n}\n\nvoid RosVSLAM::updatePath() {\n\tgeometry_msgs::PoseStamped pose;\n\n\tVectorXf state = getState();\n\n\tpose.pose.position.x = state(0)*map_scale;\n\tpose.pose.position.y = state(1)*map_scale;\n\tpose.pose.position.z = state(2)*map_scale;\n\tpose.pose.orientation.x = state(4);\n\tpose.pose.orientation.y = state(5);\n\tpose.pose.orientation.z = state(6);\n\tpose.pose.orientation.w = state(3);\n\tthis->cameraPath.poses.push_back(pose);\n}\n\nvoid RosVSLAM::predict(float v_x, float w_z) {\n\tthis->VSlamFilter::predict();\n\tthis->updatePath();\n}\n\n\nvoid RosVSLAM::update(float v_speed_, float w_speed_) {\n\tthis->VSlamFilter::update(v_speed_,w_speed_);\n\tthis->updatePath();\n}\n\nnav_msgs::Odometry RosVSLAM::getVisualOdometry(ros::Time stamp){\n\tVectorXf state = getState();\n\tnav_msgs::Odometry odom;\n\todom.header.stamp = stamp;\n\todom.twist.twist.linear.x = state(7);\n\todom.twist.twist.linear.y = state(8);\n\todom.twist.twist.linear.z = state(9);\n\n\todom.twist.twist.angular.x = state(10);\n\todom.twist.twist.angular.y = state(11);\n\todom.twist.twist.angular.z = state(12);\n\n\tMatrixXf S = Sigma.block<6,6>(7,7);\n\n\tfor(int i = 0; i < 6; i++) {\n\t\tfor(int j = 0; j < 6; j++){\n\t\t\todom.twist.covariance[i+6*j] = S(i,j);\n\t\t}\n\t}\n\n\treturn odom;\n\n}\n\n\n\nvisualization_msgs::MarkerArray RosVSLAM::ActualCameraRepr() {\n\n\tvisualization_msgs::MarkerArray camera;\n\n\tvisualization_msgs::Marker camera_marker;\n\tcamera_marker.header.frame_id = \"/world\";\n\tcamera_marker.header.stamp = ros::Time::now();\n\tcamera_marker.ns = \"camera_marker\";\n\tcamera_marker.type = visualization_msgs::Marker::MESH_RESOURCE;\n\tcamera_marker.mesh_resource = \"package://vslam/mesh/test.dae\";\n camera_marker.action = visualization_msgs::Marker::ADD;\n camera_marker.pose.orientation.w = 1.0;\n\tcamera_marker.id = 0;\n camera_marker.scale.x = 0.1;\n camera_marker.scale.y = 0.1;\n\tcamera_marker.scale.z = 0.1;\n\tcamera_marker.color.b = 1.0f;\n camera_marker.color.a = 1.0;\n\n\tVectorXf state = getState();\n\n\tstd::cout << map_scale << std::endl;\n\tcamera_marker.pose.position.x = state(0)*map_scale;\n\tcamera_marker.pose.position.y = state(1)*map_scale;\n\tcamera_marker.pose.position.z = state(2)*map_scale;\n\tcamera_marker.pose.orientation.x = state(4);\n\tcamera_marker.pose.orientation.y = state(5);\n\tcamera_marker.pose.orientation.z = state(6);\n\tcamera_marker.pose.orientation.w = state(3);\n\n\n\tMatrix3f Cov = Sigma.block<3,3>(0,0);\n SelfAdjointEigenSolver eigenSolver(Cov);\n Vector3f eigs = eigenSolver.eigenvalues();\n Matrix3f vecs = eigenSolver.eigenvectors();\n Quaternionf q(vecs);\n\n\tvisualization_msgs::Marker CameraCov;\n\tCameraCov.header.frame_id = \"/world\";\n\tCameraCov.header.stamp = ros::Time::now();\n\n\tCameraCov.ns = \"CameraCov\";\n\tCameraCov.action = visualization_msgs::Marker::ADD;\n // pointsXYZ.pose.orientation.w = 1.0;\n\tCameraCov.id = 0;\n\tCameraCov.type = visualization_msgs::Marker::SPHERE;\n\tCameraCov.scale.x = 9*eigs(0)*map_scale;\n\tCameraCov.scale.y = 9*eigs(1)*map_scale;\n\tCameraCov.scale.z = 9*eigs(2)*map_scale;\n\tCameraCov.pose.orientation.w = q.w();\n\tCameraCov.pose.orientation.x = q.x();\n\tCameraCov.pose.orientation.y = q.y();\n\tCameraCov.pose.orientation.z = q.z();\n\tCameraCov.pose.position.x = state(0)*map_scale;\n\tCameraCov.pose.position.y = state(1)*map_scale;\n\tCameraCov.pose.position.z = state(2)*map_scale;\n\tCameraCov.color.b = 1.0f;\n\tCameraCov.color.a = 0.3;\n\tCameraCov.lifetime.sec = 1;\n\n\tcamera.markers.push_back(CameraCov);\n\tcamera.markers.push_back(camera_marker);\n\n\n\n\treturn camera;\n}\n\n\n\nvisualization_msgs::MarkerArray RosVSLAM::getFeatures() {\n\tvisualization_msgs::Marker pointsINV;\n\tpointsINV.header.frame_id = \"/world\";\n\tpointsINV.header.stamp = ros::Time::now();\n\tpointsINV.ns = \"points_and_lines\";\n\tpointsINV.action = visualization_msgs::Marker::ADD;\n\tpointsINV.pose.orientation.w = 1.0;\n\tpointsINV.id = 0;\n\tpointsINV.type = visualization_msgs::Marker::SPHERE_LIST;\n\tpointsINV.scale.x = 0.1;\n\tpointsINV.scale.y = 0.1;\n\tpointsINV.scale.z = 0.1;\n\tpointsINV.color.r = 0.0f;\n\tpointsINV.color.g = 0.0f;\n\tpointsINV.color.b = 0.0f;\n\tpointsINV.color.a = 1.0;\n\n\n\tvisualization_msgs::MarkerArray points;\n\n\tfor (int i = 0; i < this->patches.size(); ++i) {\n\t\tint pos = this->patches[i].position_in_state;\n\t\tVector3f d;\n\t\tMatrix3f Cov;\n\t\tif (!patches[i].isXYZ()) {\n\t\t\tMatrixXf Jf;\n\t\t\td = depth2XYZ(mu.segment<6>(pos), Jf);\n\t\t\tgeometry_msgs::Point p;\n\t\t\tp.x = d(0)*map_scale;\n\t\t\tp.y = d(1)*map_scale;\n\t\t\tp.z = d(2)*map_scale;\n\t\t\tpointsINV.points.push_back(p);\n\t\t\tCov = Jf*Sigma.block<6,6>(pos,pos)*Jf.transpose();\n\n\t SelfAdjointEigenSolver eigenSolver(Cov);\n\t Vector3f eigs = eigenSolver.eigenvalues();\n\t Matrix3f vecs = eigenSolver.eigenvectors();\n\t Quaternionf q(vecs);\n\n\t \tvisualization_msgs::Marker pointsINV;\n\t \tpointsINV.header.frame_id = \"/world\";\n\t \tpointsINV.header.stamp = ros::Time::now();\n\t \tchar name[20];\n\t \tsprintf(name, \"F%d\",i);\n\t \tstd::stringstream ss;\n\t \tss << i;\n\n\t \tpointsINV.ns = name;\n\t \tpointsINV.action = visualization_msgs::Marker::ADD;\n\t // pointsXYZ.pose.orientation.w = 1.0;\n\t \tpointsINV.id = i;\n\t pointsINV.type = visualization_msgs::Marker::SPHERE;\n\t pointsINV.scale.x = eigs(0)*map_scale;\n\t pointsINV.scale.y = eigs(1)*map_scale;\n\t pointsINV.scale.z = eigs(2)*map_scale;\n\t pointsINV.pose.orientation.w = q.w();\n\t pointsINV.pose.orientation.x = q.x();\n\t pointsINV.pose.orientation.y = q.y();\n\t pointsINV.pose.orientation.z = q.z();\n\t pointsINV.pose.position.x = d(0)*map_scale;\n\t pointsINV.pose.position.y = d(1)*map_scale;\n\t pointsINV.pose.position.z = d(2)*map_scale;\n\t pointsINV.color.g = 1.0f;\n\t pointsINV.color.a = 0.5;\n\t pointsINV.lifetime.sec = 1;\n\n\t \t// points.markers.push_back(pointsINV);\n\n\t\t} else {\n\t\t\td = mu.segment<3>(pos);\n\t\t\tCov = Sigma.block<3,3>(pos,pos);\n\t\t\tCov.eigenvalues();\n\t SelfAdjointEigenSolver eigenSolver(Cov);\n\t Vector3f eigs = eigenSolver.eigenvalues();\n\t Matrix3f vecs = eigenSolver.eigenvectors();\n\t Quaternion q(vecs);\n\n\t visualization_msgs::Marker pointsXYZ;\n\t \tpointsXYZ.header.frame_id = \"/world\";\n\t \tpointsXYZ.header.stamp = ros::Time::now();\n\t \tchar name[20];\n\t \tsprintf(name, \"F%d\",i);\n\t \tstd::stringstream ss;\n\t \tss << i;\n\t \tpointsXYZ.ns = name;\n\t \tpointsXYZ.action = visualization_msgs::Marker::ADD;\n\t pointsXYZ.id = i;\n\t pointsXYZ.type = visualization_msgs::Marker::SPHERE;\n\t pointsXYZ.scale.x = eigs(0)*map_scale;\n\t pointsXYZ.scale.y = eigs(1)*map_scale;\n\t pointsXYZ.scale.z = eigs(2)*map_scale;\n\t pointsXYZ.pose.orientation.w = q.w();\n\t pointsXYZ.pose.orientation.x = q.x();\n\t pointsXYZ.pose.orientation.y = q.y();\n\t pointsXYZ.pose.orientation.z = q.z();\n\t pointsXYZ.pose.position.x = d(0)*map_scale;\n\t pointsXYZ.pose.position.y = d(1)*map_scale;\n\t pointsXYZ.pose.position.z = d(2)*map_scale;\n\t pointsXYZ.color.r = 1.0f;\n\t pointsXYZ.color.a = 0.5;\n\t pointsXYZ.lifetime.sec = 1;\n\t \t//points.markers.push_back(pointsXYZ);\n\n\t visualization_msgs::Marker textpointsXYZ;\n\t textpointsXYZ.header.frame_id = \"/world\";\n\t textpointsXYZ.header.stamp = ros::Time::now();\n\t \tsprintf(name, \"name%d\",i);\n\t \ttextpointsXYZ.ns = name;\n\t \ttextpointsXYZ.text = ss.str();\n\t \ttextpointsXYZ.action = visualization_msgs::Marker::ADD;\n\t // pointsXYZ.pose.orientation.w = 1.0;\n\t \ttextpointsXYZ.id = 0;\n\t \ttextpointsXYZ.type = visualization_msgs::Marker::TEXT_VIEW_FACING;\n\t \ttextpointsXYZ.scale.x = 1;\n\t textpointsXYZ.scale.y = 1;\n\t textpointsXYZ.scale.z = 1;\n\t textpointsXYZ.pose.position.x = d(0)*map_scale;\n\t textpointsXYZ.pose.position.y = d(1)*map_scale;\n\t textpointsXYZ.pose.position.z = d(2)*map_scale;\n\t textpointsXYZ.color.r = 0.0f;\n\t textpointsXYZ.color.g = 0.0f;\n\t textpointsXYZ.color.b = 0.0f;\n\t textpointsXYZ.color.a = 1;\n\t textpointsXYZ.lifetime.sec = 1;\n\t \t// points.markers.push_back(textpointsXYZ);\n\n\n\n\n\n\n\n\t\t\tgeometry_msgs::Point p;\n\t\t\tp.x = d(0)*map_scale;\n\t\t\tp.y = d(1)*map_scale;\n\t\t\tp.z = d(2)*map_scale;\n\t\t\tpointsINV.points.push_back(p);\n\n\n\t\t}\n\n }\n\n\tpoints.markers.push_back(pointsINV);\n\n\n\n\n\tstatic int count_OLD = 0;\n\tchar name[100];\n\tvisualization_msgs::Marker pointsOLD;\n\tpointsOLD.header.frame_id = \"/world\";\n\tpointsOLD.header.stamp = ros::Time::now();\n\tpointsOLD.action = visualization_msgs::Marker::ADD;\n\tpointsOLD.pose.orientation.w = 1.0;\n\tpointsOLD.id = 0;\n\tpointsOLD.type = visualization_msgs::Marker::SPHERE_LIST;\n\tpointsOLD.scale.x = 0.1;\n\tpointsOLD.scale.y = 0.1;\n\tpointsOLD.scale.z = 0.1;\n\tpointsOLD.color.r = 0.0f;\n\tpointsOLD.color.g = 0.0f;\n\tpointsOLD.color.b = 0.0f;\n\tpointsOLD.color.a = 1.0;\n\n\n\n\tstd::cout << \"size = \" < 1000) {\n\t\tfor (int i = 0; i < deleted_patches.size(); i++) {\n\t\t\tsprintf(name, \"OLD_points%d\", count_OLD++);\n\t\t\tpointsOLD.ns = name;\n\t\t\tgeometry_msgs::Point p;\n\t\t\tp.x = deleted_patches[i].XYZ_pos(0)*map_scale;\n\t\t\tp.y = deleted_patches[i].XYZ_pos(1)*map_scale;\n\t\t\tp.z = deleted_patches[i].XYZ_pos(2)*map_scale;\n\t\t\tpointsOLD.points.push_back(p);\n\t\t}\n\t\tdeleted_patches.clear();\n\t} else {\n\t\tfor (int i = 0; i < deleted_patches.size(); i++) {\n\t\t\tpointsOLD.ns = \"OLD_points\";\n\t\t\tgeometry_msgs::Point p;\n\t\t\tp.x = deleted_patches[i].XYZ_pos(0)*map_scale;\n\t\t\tp.y = deleted_patches[i].XYZ_pos(1)*map_scale;\n\t\t\tp.z = deleted_patches[i].XYZ_pos(2)*map_scale;\n\t\t\tpointsOLD.points.push_back(p);\n\t\t}\n\t}\n\n\tpoints.markers.push_back(pointsOLD);\n\n\n return points;\n}\n\nnav_msgs::Path RosVSLAM::getCameraPath() {\n\treturn this->cameraPath;\n}\n" }, { "path": "src/RosVSLAMRansac.hpp", "content": "#include \"vslamRansac.hpp\"\n\n#include \n\n#include \n#include \n#include \n\n\n#include \n#include \n\n#include \n\n#include \nusing namespace Eigen;\n\n\nclass RosVSLAM : public VSlamFilter\n{\nprivate:\n\tnav_msgs::Path cameraPath;\n\tvoid updatePath();\n\tstd::vector vQuat;\n\tstd::vector vCov;\n\npublic:\n\tRosVSLAM(char *file = NULL);\n\tgeometry_msgs::Pose getCameraPose();\n\tvisualization_msgs::MarkerArray getFeatures();\n\tvisualization_msgs::MarkerArray ActualCameraRepr();\n\n\tvoid predict(float v_x = 0, float w_z = 0);\n\tvoid update(float v_x = 0, float w_z = 0);\n\tnav_msgs::Path getCameraPath();\n\tnav_msgs::Odometry getVisualOdometry(ros::Time stamp);\n};\n\n\n" }, { "path": "src/camModel.cpp", "content": "#include \"camModel.hpp\"\n\n\nvoid CamModel::setParam(camConfig camParams) {\n\tfx = camParams.fx;\n\tfy = camParams.fy;\n\tu0 = camParams.u0;\n\tv0 = camParams.v0;\n\tk1 = camParams.k1;\n\tk2 = camParams.k2;\n\tk3 = camParams.k3;\n\tp1 = camParams.p1;\n\tp2 = camParams.p2;\n\t\n}\n\n\nMatrix2f CamModel::diff_distort_undistort(Vector2f hn) {\n\n\tconst float u = hn(0);\n\tconst float v = hn(1);\n\n\tconst float r_2 = hn(0)*hn(0) + hn(1)*hn(1);\n\tconst float l = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n\n\tVector2f hn_contr;\n hn_contr << hn(1),hn(0);\n\n Vector2f pv;\n pv << p1,p2;\n\n Vector2f pv_contr;\n pv_contr << p2,p1;\n\n Matrix2f j_matrix;\n j_matrix << p2*hn(0), 0, 0, p1*hn(1);\n\n\n const float f = k1 + 2*k2*r_2 + 3*k3*r_2*r_2;\n\n Matrix2f Jacobian_Distort_Undistort = l*Matrix2f::Identity() + 2*f*hn*hn.transpose() + 2*pv*hn_contr.transpose() + 2*pv_contr*hn.transpose() + 4*j_matrix;\n return Jacobian_Distort_Undistort;\n\n}\n\n\n\nCamModel::CamModel(float _fx, float _fy, float _u0, float _v0, float _k1, float _k2, float _k3, float _p1, float _p2):\n fx(_fx),\n fy(_fy),\n u0(_u0),\n v0(_v0),\n k1(_k1),\n k2(_k2),\n k3(_k3),\n p1(_p1),\n p2(_p2)\n {\n \n\n }\n \n\n\nVector2f CamModel::projectAndDistort(Vector3f h, MatrixXf &J) {\n \n float x = h(0);\n float y = h(1);\n float z = h(2);\n \n \n // Compute point retina undistor\n float inv_z = 1/z;\n\n float x1 = x/z;\n float y1 = y/z;\n \n \n \n MatrixXf Jacobian_RetinaUnd_hC(2,3);\n Jacobian_RetinaUnd_hC << 1/z, 0, -x/z/z, 0, 1/z, -y/z/z;\n \n // Compute point retina distort\n\n float r_2 = x1*x1+y1*y1;\n float l = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n\n \n float x2 = x1*l + 2*p1*x1*y1 + p2*(r_2 + 2*x1*x1);\n float y2 = y1*l + 2*p2*x1*y1 + p1*(r_2 + 2*y1*y1);\n\n\n Vector2f hn;\n hn << x1,y1;\n\n \n Vector2f hd;\n hd << fx*x2 + u0, fy*y2 + v0; \n\n MatrixXf Jacobian_Pixel_Retina(2,2);\n Jacobian_Pixel_Retina << fx, 0, 0, fy;\n \n \n // ************************************ //\n \n J = Jacobian_Pixel_Retina*diff_distort_undistort(hn)*Jacobian_RetinaUnd_hC;\n \n\n return hd;\n}\n\nVector2f CamModel::projectAndDistort(Vector3f h) {\n \n\n float x = h(0);\n float y = h(1);\n float z = h(2);\n \n \n // Compute point retina undistor\n float inv_z = 1/z;\n\n float x1 = x/z;\n float y1 = y/z;\n\n\n\n \n // Compute point retina distort\n\n float r_2 = x1*x1+y1*y1;\n float l = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n\n \n float x2 = x1*l + 2*p1*x1*y1 + p2*(r_2 + 2*x1*x1);\n float y2 = y1*l + 2*p2*x1*y1 + p1*(r_2 + 2*y1*y1);\n\n Vector2f hd;\n hd << fx*x2 + u0, fy*y2 + v0;\n\n return hd;\n}\n\nVector3f CamModel::UndistortAndDeproject(Vector2f hd, MatrixXf &J){\n \n // Compute retina distort\n float x2 = (hd(0) - u0)/fx;\n float y2 = (hd(1) - v0)/fy;\n\n MatrixXf Jacobian_Retina_Pixel(2,2);\n Jacobian_Retina_Pixel << 1/fx, 0, 0, 1/fy;\n\n // Compute retina undistort\n \n \n \n float x1 = x2;\n float y1 = y2;\n\n float r_2;\n float l;\n\n float dx,dy;\n\n\n int n_iter = 50;\n for (int i = 0; i < n_iter; ++i) {\n \tr_2 = x1*x1 + y1*y1;\n \tl = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n\n dx = 2*p1*x1*y1 + p2*(r_2 + 2*x1*x1);\n dy = 2*p2*x1*y1 + p1*(r_2 + 2*y1*y1);\n\n x1 = (x2 - dx)/l;\n y1 = (y2 - dy)/l;\n }\n\n\n\t\n Vector2f hn;\n hn << x1,y1;\n \n Vector3f hC;\n hC << x1, y1, 1;\n \n MatrixXf Jacobian_hC_RetinaUnd(3,2);\n Jacobian_hC_RetinaUnd << 1, 0, 0, 1, 0, 0;\n \n \n J = Jacobian_hC_RetinaUnd*diff_distort_undistort(hn).inverse()*Jacobian_Retina_Pixel;\n\n return hC;\n}\n\n\n\n" }, { "path": "src/camModel.hpp", "content": "#ifndef __CAM_MODEL__\n#define __CAM_MODEL__\n\n#include \n#include \n\n\n\ntypedef struct _camConfig_ {\n\tfloat fx, fy, u0,v0,k1,k2,k3,p1,p2;\n} camConfig;\n\n\nusing namespace Eigen;\n\nclass CamModel {\n float fx, fy;\n float u0, v0;\n float k1, k2, k3, p1, p2;\n \npublic:\n \n // Camera fatta a mano wide angle\n CamModel(\n float _fx = 592.2860,\n float _fy = 584.9968,\n float _u0 = 362.1059,\n float _v0 = 275.9642,\n float _k1 = -0.3954,\n float _k2 = 0.5521,\n float _k3 = 0,\n float _p1 = -0.0075,\n float _p2 = 0.0140);\n\n\n\tvoid setParam(camConfig camParams);\n Vector3f UndistortAndDeproject(Vector2f hd, MatrixXf &J);\n Vector2f projectAndDistort(Vector3f h, MatrixXf &J);\n\tVector2f projectAndDistort(Vector3f h);\n\tMatrix2f diff_distort_undistort(Vector2f hn);\n};\n\n#endif\n\n/*\n 592.2860\n 584.9968\n \n \n 362.1059\n 275.9642\n \n -0.3954\n 0.5521\n -0.0075\n 0.0140\n 0\n\n\n*/\n" }, { "path": "src/camOCV.cpp", "content": "#include \"camOCV.hpp\"\n\nCamModel::CamModel(float _fx, float _fy, float _u0, float _v0, float _k1, float _k2):\n fx(_fx),\n fy(_fy),\n u0(_u0),\n v0(_v0),\n k1(_k1),\n k2(_k2)\n {}\n \n \n \n \n \nCamModel::CamModel(cv::Mat parameters):\n fx(parameters.at(0)),\n fy(parameters.at(1)),\n u0(parameters.at(2)),\n v0(parameters.at(3)),\n k1(parameters.at(4)),\n k2(parameters.at(5))\n{\n std::cout << \"Cam Model \" << fx << \" \" << fy << \" \" << u0 << \" \"<< v0 << \" \" << k1 << \" \" << k2 << std::endl;\n}\n\n\n\nVector2f CamModel::projectAndDistort(Vector3f h, MatrixXf &J) {\n \n float x = h(0);\n float y = h(1);\n float z = h(2);\n \n \n // Compute point retina undistor\n float inv_z = 1/z;\n\n float x1 = x/z;\n float y1 = y/z;\n \n \n \n MatrixXf Jacobian_RetinaUnd_hC(2,3);\n Jacobian_RetinaUnd_hC << 1/z, 0, -x/z/z, 0, 1/z, -y/z/z;\n \n // Compute point retina distort\n\n float ru = sqrt(x1*x1 + y1*y1);\n float ru_2 = ru*ru;\n float r = ru/(1+k1*ru_2+k2*ru_2*ru_2);\n \n for (int i = 0; i < 100; i++) {\n r = r - (r + k1*pow(r,3.0) + k2*pow(r,5.0) - ru)/(1 + 3*k1*pow(r,2.0)+ 5*k2*pow(r,4.0));\n }\n \n float r_2 = r*r;\n\n float l = 1 + k1*r_2 + k2*r_2*r_2;\n float x2 = x1/l;\n float y2 = y1/l;\n \n \n float m = 2*k1 + 4*k2*r_2;\n MatrixXf Jacobian_Undistort_Distort(2,2);\n Jacobian_Undistort_Distort(0,0) = l + m*x2*x2;\n Jacobian_Undistort_Distort(1,1) = l + m*y2*y2;\n Jacobian_Undistort_Distort(0,1) = Jacobian_Undistort_Distort(1,0) = m*x2*y2;\n\n \n \n // Compute point pixel distort\n \n Vector2f hd;\n hd << fx*x2 + u0, fy*y2 + v0;\n\n MatrixXf Jacobian_Pixel_Retina(2,2);\n Jacobian_Pixel_Retina << fx, 0, 0, fy;\n \n \n // ************************************ //\n \n J = Jacobian_Pixel_Retina*Jacobian_Undistort_Distort.inverse()*Jacobian_RetinaUnd_hC;\n \n\n return hd;\n}\n\nVector2f CamModel::projectAndDistort(Vector3f h) {\n \n float x = h(0);\n float y = h(1);\n float z = h(2);\n \n \n // Compute point retina undistor\n float inv_z = 1/z;\n\n float x1 = x*inv_z;\n float y1 = y*inv_z;\n\n \n // Compute point retina distort\n\n float ru = sqrt(x1*x1 + y1*y1);\n float ru_2 = ru*ru;\n float r = ru/(1+k1*ru_2+k2*ru_2*ru_2);\n \n for (int i = 0; i < 100; i++) {\n r = r - (r + k1*pow(r,3.0) + k2*pow(r,5.0) - ru)/(1 + 3*k1*pow(r,2.0)+ 5*k2*pow(r,4.0));\n }\n \n float r_2 = r*r;\n\n float l = 1 + k1*r_2 + k2*r_2*r_2;\n float x2 = x1/l;\n float y2 = y1/l;\n \n \n float m = 2*k1 + 4*k2*r_2;\n\n \n // Compute point pixel distort\n \n Vector2f hd;\n hd << fx*x2 + u0, fy*y2 + v0;\n\n return hd;\n}\n\nVector3f CamModel::UndistortAndDeproject(Vector2f hd, MatrixXf &J){\n \n // Compute retina distort\n float x2 = (hd(0) - u0)/fx;\n float y2 = (hd(1) - v0)/fy;\n\n MatrixXf Jacobian_Retina_Pixel(2,2);\n Jacobian_Retina_Pixel << 1/fx, 0, 0, 1/fy;\n\n // Compute retina undistort\n \n float r_2 = x2*x2 + y2*y2;\n float l = 1+k1*r_2+k2*r_2*r_2;\n float x1 = x2*l;\n float y1 = y2*l;\n \n float m = 2*k1 + 4*k2*r_2;\n MatrixXf Jacobian_Undistort_Distort(2,2);\n Jacobian_Undistort_Distort(0,0) = l + m*x2*x2;\n Jacobian_Undistort_Distort(1,1) = l + m*y2*y2;\n Jacobian_Undistort_Distort(0,1) = Jacobian_Undistort_Distort(1,0) = m*x2*y2;\n \n // Compute 3D Line\n \n Vector3f hC;\n hC << x1, y1, 1;\n \n MatrixXf Jacobian_hC_RetinaUnd(3,2);\n Jacobian_hC_RetinaUnd << 1, 0, 0, 1, 0, 0;\n \n J = Jacobian_hC_RetinaUnd*Jacobian_Undistort_Distort*Jacobian_Retina_Pixel;\n\n return hC;\n}\n\n\n\n" }, { "path": "src/camOCV.hpp", "content": "#include \n#include \n\nusing namespace Eigen;\n\nclass CamModel {\n float fx, fy, k1, k2;\n float u0, v0;\n \npublic:\n \n // Camera fatta a mano wide angle\n CamModel(\n float _fx = 558.665908,\n float _fy = 554.953227,\n float _u0 = 346.781365,\n float _v0 = 241.238318,\n float _k1 = -0.433618,\n float _k2 = 0.220758);\n\n /*\n *\n *\n * Quelli della camera rossa stereo\n CamModel(\n float _fx = 884.805305,\n float _fy = 880.495678,\n float _u0 = 320.125098,\n float _v0 = 234.720010,\n float _k1 = -0.105199,\n float _k2 = -0.028570);\n*/\n /*\n CamModel(\n float _fx = 862.350016,\n float _fy = 856.474667,\n float _u0 = 312.041077 -0.5,\n float _v0 = 212.607881 -0.5,\n float _k1 = -0.098650,\n float _k2 = 0.009240);*/\n\n\n\n\n\n/*\nCamModel(\n\n float _fx = 1443.6375/4,\n float _fy = 1418.4542/4,\n float _u0 = 617.56/4,\n float _v0 = 474.686/4,\n float _k1 = 0.013002,\n float _k2 = -0.024809);*/\n CamModel(cv::Mat parameters);\n\n Vector3f UndistortAndDeproject(Vector2f hd, MatrixXf &J);\n Vector2f projectAndDistort(Vector3f h, MatrixXf &J);\n\tVector2f projectAndDistort(Vector3f h);\n};\n" }, { "path": "src/cameraModel.cpp", "content": "#include \"cameraModel.hpp\"\n\n\nvoid CameraModel::setParam(camConfig camParams) {\n\tfx = camParams.fx;\n\tfy = camParams.fy;\n\tu0 = camParams.u0;\n\tv0 = camParams.v0;\n\tk1 = camParams.k1;\n\tk2 = camParams.k2;\n\tk3 = camParams.k3;\n\tp1 = camParams.p1;\n\tp2 = camParams.p2;\n\t\n}\n\n\nMatrix2f CameraModel::diff_distort_undistort(Vector2f hn) {\n\n\tfloat r_2 = hn(0)*hn(0) + hn(1)*hn(1);\n\tfloat l = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n\n\tVector2f hn_contr;\n hn_contr << hn(1),hn(0);\n\n Vector2f pv;\n pv << p1,p2;\n\n Vector2f pv_contr;\n pv_contr << p2,p1;\n\n Matrix2f j_matrix;\n j_matrix << p2*hn(0), 0, 0, p1*hn(1);\n\n\n float f = k1 + 2*k2*r_2 + 3*k3*r_2*r_2;\n\n Matrix2f Jacobian_Distort_Undistort = l*Matrix2f::Identity() + 2*f*hn*hn.transpose() + 2*pv*hn_contr.transpose() + 2*pv_contr*hn.transpose() + 4*j_matrix;\n return Jacobian_Distort_Undistort;\n\n}\n\n\n\nCameraModel::CameraModel(float _fx, float _fy, float _u0, float _v0, float _k1, float _k2, float _k3, float _p1, float _p2):\n fx(_fx), fy(_fy),\n u0(_u0), v0(_v0),\n k1(_k1), k2(_k2), k3(_k3),\n p1(_p1), p2(_p2)\n { }\n \n\n\nVector2f CameraModel::project(Vector3f h1, bool computeJacobian) {\n\n\tconst float x = h1(0);\n const float y = h1(1);\n const float z = h1(2);\n\n // compute retina projection\n const float x1 = x/z;\n const float y1 = y/z;\n\n\n\n // Distort retina point\n const float r_2 = x1*x1+y1*y1;\n const float l = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n const float x2 = x1*l + 2*p1*x1*y1 + p2*(r_2 + 2*x1*x1);\n const float y2 = y1*l + 2*p2*x1*y1 + p1*(r_2 + 2*y1*y1);\n\n // project in image RF\n Vector2f hd = (Vector2f << fx*x2 + u0, fy*y2 + v0);\n\n\n if (computeJacobian) {\n Vector2f hn = (Vector2f << x1,y1);\n \tMatrix Jacobian_RetinaUnd_hC = (Matrix << 1/z, 0, -x/z/z, 0, 1/z, -y/z/z);\n \tMatrix2f Jacobian_Pixel_Retina = (Matrix2f << fx, 0, 0, fy);\n \tthis->projectionJacobian = Jacobian_Pixel_Retina*diff_distort_undistort(hn)*Jacobian_RetinaUnd_hC;\n }\n\n return hd;\n}\n\n\nMatrix2f CameraModel::getProjectionJacobian() {\n\treturn this->projectionJacobian;\n}\n\n\n\n\nVector3f CameraModel::unproject(Vector2f hd, bool computeJacobian){\n \n // Compute retina distort\n const float x2 = (hd(0) - u0)/fx;\n const float y2 = (hd(1) - v0)/fy;\n \n // Compute retina undistort using iterative solution\n float x1 = x2;\n float y1 = y2;\n\n float r_2;\n float l;\n\n float dx,dy;\n\n\n const int n_iter = 20;\n for (int i = 0; i < n_iter; ++i) {\n \tr_2 = x1*x1 + y1*y1;\n \tl = 1 + k1*r_2 + k2*r_2*r_2 + k3*r_2*r_2*r_2;\n\n dx = 2*p1*x1*y1 + p2*(r_2 + 2*x1*x1);\n dy = 2*p2*x1*y1 + p1*(r_2 + 2*y1*y1);\n\n x1 = (x2 - dx)/l;\n y1 = (y2 - dy)/l;\n }\n\n // Epipolar line normal vector\n Vector3f hC = (Vector3f << x1, y1, 1);\n\n if (computeJacobian) {\n Vector2f hn = (Vector2f << x1,y1);\n Matrix2f Jacobian_Retina_Pixel = (Matrix2f << 1/fx, 0, 0, 1/fy);\n Matrix Jacobian_hC_RetinaUnd = (Matrix << 1, 0, 0, 1, 0, 0);\n this->unprojectionJacobian = Jacobian_hC_RetinaUnd*diff_distort_undistort(hn).inverse()*Jacobian_Retina_Pixel;\n }\n\n return hC;\n}\n\nMatrix3f CameraModel::getUnprojectionJacobian() {\n\treturn this->unprojectionJacobian;\n}\n\n\n\n\n" }, { "path": "src/cameraModel.hpp", "content": "#ifndef __CAMERA_MODEL__\n#define __CAMERA_MODEL__\n\n#include \n#include \n\n\n\ntypedef struct _camConfig_ {\n\tfloat fx, fy, u0,v0,k1,k2,k3,p1,p2;\n} camConfig;\n\n\nusing namespace Eigen;\n\nclass CameraModel {\n float fx, fy;\n float u0, v0;\n float k1, k2, k3, p1, p2;\n \nprivate:\n\tMatrix2f diff_distort_undistort(Vector2f hn);\n\tMatrix2f projectionJacobian;\n\tMatrix3f unprojectionJacobian;\n\npublic:\n \n // Camera fatta a mano wide angle\n CameraModel(\n float _fx = 592.2860,\n float _fy = 584.9968,\n float _u0 = 362.1059,\n float _v0 = 275.9642,\n float _k1 = -0.3954,\n float _k2 = 0.5521,\n float _k3 = 0,\n float _p1 = -0.0075,\n float _p2 = 0.0140);\n\n\n\n\tvoid setParam(camConfig camParams);\n Vector3f unproject(Vector2f hd, bool computeJacobian = false);\n Matrix3f getUnprojectionJacobian();\n\n Vector2f project(Vector3f h, bool computeJacobian = false);\n Matrix2f getProjectionJacobian();\n\n};\n\n#endif\n\n" }, { "path": "src/libblur.cpp", "content": "/*\n * libblur.cpp\n *\n * Created on: Jul 9, 2013\n * Author: ludovico\n */\n#include \"libblur.h\"\n#include \n\n// Input: parametri del kernel (parametri predetti da kalman)\n// Output: kernel stimato\n\n// This function is to evaluate the kernel\n// (the smallest one possible inferable from EKF)\n// knowing initial and final point after camera shake\n\ncv::Mat evaluateKernel(cv::Point2f one, cv::Point2f two) {\n\tint width, height;\n\n height = abs(one.x - two.x ) + 1;\n\twidth = abs(one.y - two.y) + 1;\n\n\tcv::Mat kernel = cv::Mat::zeros(width, height, cv::DataType::type);\n\tdouble length, theta, value;\n\ttheta = atan2((one.y - two.y),(one.x - two.x));\n\n\tlength = norm(one - two);\n double c = cos(theta);\n double s = sin(theta);\n\n int x0 = (s < 0 ? -s*length: 0);\n int y0 = (c < 0 ? -c*length: 0);\n\n\n\tfor(int i=0; i(x,y) = 1;\n }\n //std::cout << \"Kernel Built\" << std::endl;\n kernel = kernel/sum(kernel).val[0];\n\n\n // //std::cout << kernel << std::endl;\n\n\treturn kernel.clone();\n}\n\n\n// Input: patch non blurrata + parametri del kernel (parametri predetti da kalman)\n// Output: patch blurrata\n\n// This function is to blur the patch with the predicted kernel\n// (the smallest one possible inferable from EKF)\n\ncv::Mat blurPatch(const cv::Mat &_patch, cv::Point2f one, cv::Point2f two) {\n cv::Mat blurredWindow = _patch.clone();\n //std::cout << \"Start Blurring\" << std::endl;\n\n cv::Mat patch = _patch.clone();\n patch.convertTo(patch, cv::DataType::type);\n\n\tcv::Mat kernel = evaluateKernel(one,two);\n\n\tcv::Point2f anchor = cv::Point( -1, -1 );\n\n\tint delta = 0, ddepth = -1;\n\tfilter2D(patch, patch, ddepth, kernel, anchor, delta, cv::BORDER_DEFAULT);\n\n patch.convertTo(patch, CV_8U);\n //std::cout << \"Stop Blurring\" << std::endl;\n\n // hconcat(blurredWindow, patch, blurredWindow);\n // imshow(\"Blurring\", blurredWindow);\n // cv::waitKey(1);\n\n\treturn patch;\n}\n\n\n\n// Input: nuova patch blurrata + parametri del kernel (parametri stimati)\n// Output: patch deblurrata\n\n// This function is to deblur the patch with the estimated kernel\n// LR deconvolution is used in the spatial domain - Improved algorithm\n// (see Fish et al. - Blind deconvolution by means of the RichardsonñLucy algorithm )\n\n// Input kernel is supposed to be normalised (sum of its element equal to 1)\n\n// If the deblurring process is not giving high quality outcomes, try to\n// augment the number of iterations or to pre-process the kernel taping its edges\n// (using http://read.pudn.com/downloads112/sourcecode/graph/texture_mapping/464953/images/edgetaper.m__.htm )\n\ncv::Mat deblurPatch(const cv::Mat &_patch, cv::Point2f one, cv::Point2f two) {\n ////std::cout << \"Start Deblurring\" << one << std::endl << two << std::endl;\n cv::Mat blurredWindow = _patch.clone();\n cv::Mat patch = _patch.clone();\n patch.convertTo(patch, cv::DataType::type);\n cv::Mat kernel = evaluateKernel(one,two);\n// //std::cout << \" kernel\" << kernel << std::endl;\n\tcv::Mat kernel_hat = kernel.clone(), result = patch.clone();\n\tcv::Mat est_conv = result.clone(), relative_blur = patch.clone(), error_est = patch.clone();\n\tcv::Mat patch_hat = patch.clone(), k_est_conv = kernel.clone(), k_relative_blur = kernel.clone(), k_error_est = kernel.clone();\n\n\tint rows = kernel.rows, cols = kernel.cols, maxIter = 3;\n\tint pRows = patch.rows, pCols = patch.cols;\n\tcv::Point2f anchor = cv::Point( -1, -1 );\n\tint delta = 0, ddepth = -1;\n\n double eps = DBL_MIN;\n\tfor(int l=0; l(i,j) = patch.at(pRows-i-1,pCols-j-1);\n\t\tfilter2D(kernel, k_est_conv, ddepth, patch, anchor, delta, cv::BORDER_DEFAULT);\n\t\tfor(int i=0; i(i,j) < eps)\n\t\t\t\t//\test_conv.at(i,j) = 10*eps;\n\t\t\t\tk_relative_blur.at(i,j) = kernel.at(i,j) / k_est_conv.at(i,j);\n }\n\t\tfilter2D(k_relative_blur, k_error_est, ddepth, patch_hat);\n\t\tfor(int i=0; i(i,j) = kernel.at(i,j) * k_error_est.at(i,j);\n }\n\n\t\t// Classical LR\n\t\tfor(int i=0; i(i,j) = kernel.at(rows-i-1,cols-j-1);\n\t\tfilter2D(result, est_conv, ddepth, kernel);\n\t\tfor(int i=0; i(i,j) < eps)\n\t\t\t\t//\test_conv.at(i,j) = 10*eps;\n\t\t\t\trelative_blur.at(i,j) = patch.at(i,j) / est_conv.at(i,j);\n\n }\n\t\tfilter2D(relative_blur, error_est, ddepth, kernel_hat, anchor, delta, cv::BORDER_DEFAULT);\n\t\tfor(int i=0; i(i,j) = result.at(i,j) * error_est.at(i,j);\n }\n\t}\n\n\n\n result.convertTo(result, CV_8U);\n\n hconcat(blurredWindow, result, blurredWindow);\n imshow(\"Deblurring\", blurredWindow);\n\n\n //std::cout << \"Stop Deblurring\" << std::endl;\n\n\treturn result;\n}\n\n\n\n/*\n\n// Input: nuova patch blurrata + parametri del kernel (parametri stimati)\n// Output: patch deblurrata\n\n// This function is to deblur the patch with the estimated kernel\n// LR deconvolution is used in the spatial domain\n// (for a quick reference see http://en.wikipedia.org/wiki/RichardsonñLucy_deconvolution )\n\n// Input kernel is supposed to be normalised (sum of its element equal to 1)\n\n// If the deblurring process is not giving high quality outcomes, try to\n// augment the number of iterations or to pre-process the kernel taping its edges\n// (using http://read.pudn.com/downloads112/sourcecode/graph/texture_mapping/464953/images/edgetaper.m__.htm )\n\ncv::Mat deblurPatch(cv::Mat patch, cv::Point2f one, cv::Point2f two) {\n //std::cout << \"Start Deblurring\" << one << std::endl << two << std::endl;\n\n imshow(\"predublurring\", patch);\n\n patch.convertTo(patch, cv::DataType::type);\n patch = patch/255;//(patch.rows*patch.cols*cv::mean(patch).val[0]);\n\n cv::Mat kernel = evaluateKernel(one,two);\n //std::cout << \" kernel\" << kernel << std::endl;\n\tcv::Mat kernel_hat = kernel.clone(), result = patch.clone();\n\tcv::Mat est_conv = result.clone(), relative_blur = patch.clone(), error_est = patch.clone();\n\n\tint rows = kernel.rows, cols = kernel.cols, maxIter = 30;\n\tint pRows = patch.rows, pCols = patch.cols;\n\tcv::Point2f anchor = cv::Point( -1, -1 );\n\tint delta = 0, ddepth = -1;\n\n\tfor(int i=0; i(i,j) = kernel.at(rows-i-1,cols-j-1);\n\n double eps = DBL_MIN;\n\tfor(int l=0; l(i,j) < eps) est_conv.at(i,j) = 10*eps;\n\t\t\t\trelative_blur.at(i,j) = patch.at(i,j) / est_conv.at(i,j);\n ////std::cout << patch.at(i,j) << \" \" << est_conv.at(i,j) << \" \"<< relative_blur.at(i,j) << std::endl;\n\n }\n\t\tfilter2D(relative_blur, error_est, ddepth, kernel_hat, anchor, delta, cv::BORDER_DEFAULT);\n\t\tfor(int i=0; i(i,j) = result.at(i,j) * error_est.at(i,j);\n ////std::cout << \"iters = \" << l << \" \" << i << \" \" << j << std::endl;\n }\n\n\t}\n\n\n\n result = result*255;\n result.convertTo(result, CV_8U);\n imshow(\"postdublurring\", result);\n\n //std::cout << \"Stop Deblurring\" << std::endl;\n\n\treturn result;\n}\n\n*/\n" }, { "path": "src/libblur.h", "content": "/*\n * libblur.h\n *\n * Created on: Jul 9, 2013\n * Author: Ludovico\n */\n\n#ifndef LIBBLUR_H_\n#define LIBBLUR_H_\n\n\n#include \n\n// Input: parametri del kernel (parametri predetti da kalman)\n// Output: kernel stimato\n\n// This function is to evaluate the kernel\n// (the smallest one possible inferable from EKF)\n// knowing initial and final point after camera shake\n\ncv::Mat evaluateKernel(cv::Point2f one, cv::Point2f two);\n\n\n\n\n// Input: patch non blurrata + parametri del kernel (parametri predetti da kalman)\n// Output: patch blurrata\n\n// This function is to blur the patch with the predicted kernel\n// (the smallest one possible inferable from EKF)\n\ncv::Mat blurPatch(const cv::Mat &patch, cv::Point2f one, cv::Point2f two);\n\n\n\n\n\n// Input: nuova patch blurrata + parametri del kernel (parametri stimati)\n// Output: patch deblurrata\n\n\n// This function is to deblur the patch with the estimated kernel\n// LR deconvolution is used in the spatial domain\n// (for a quick reference see http://en.wikipedia.org/wiki/RichardsonñLucy_deconvolution )\n\n\n// Input kernel is supposed to be normalised (sum of its element equal to 1)\n\n\n// If the deblurring process is not giving high quality outcomes, try to\n// augment the number of iterations or to pre-process the kernel taping its edges\n// (using http://read.pudn.com/downloads112/sourcecode/graph/texture_mapping/464953/images/edgetaper.m__.htm )\n\n\ncv::Mat deblurPatch(const cv::Mat &patch, cv::Point2f one, cv::Point2f two);\n\n#endif /* LIBBLUR_H_ */\n" }, { "path": "src/monoslam_ransac.cpp", "content": "#include \n#include \n#include \n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \n\n#include \"RosVSLAMRansac.hpp\"\n#include \"utils.hpp\"\n\n#include \n#include \n\n#include \n\n\nusing namespace cv;\nusing namespace std;\nusing namespace geometry_msgs;\n\n\n\nnamespace enc = sensor_msgs::image_encodings;\n\nstatic const char WINDOW[] = \"Image window\";\n\n\n\n\nclass ImageConverter\n{\n\n ros::NodeHandle nh_;\n ros::Publisher camera_poses_pub;\n ros::Publisher camera_pose_pub;\n\n\n\n\n ros::Publisher features_pub;\n ros::Publisher camera_pub;\n\n ros::Publisher odometry_pub;\n\n\n image_transport::ImageTransport it_;\n image_transport::Subscriber image_sub_;\n image_transport::Publisher image_pub_;\n \n ros::Publisher quality_index_pub;\n\n\n\nprivate:\n\tRosVSLAM slam;\n\tint first;\n\tPoseArray poses;\n\tint numFeatures;\n\ttf::TransformListener tf_listener;\n\n\tros::Subscriber odom_subscriber;\n\n\tnav_msgs::Odometry latest_odom_;\n\tnav_msgs::Odometry last_odom_;\n\tbool received_odom_;\n\n\tfloat v_speed_, w_speed_;\n\tfstream fs;\n\npublic:\n ImageConverter(char *file = NULL)\n : it_(nh_), first(1), slam(file)\n {\n\t v_speed_ = w_speed_ = 100;\n \tposes.header.frame_id = \"/world\"; \n image_pub_ = it_.advertise(\"/monoslam/imgproc\", 1);\n image_sub_ = it_.subscribe(\"/camera/image_raw\", 1, &ImageConverter::imageCb, this);\n\n odom_subscriber = nh_.subscribe(\"/pose\", 1, &ImageConverter::odom_callback, this);\n\n\n cv::namedWindow(WINDOW);\n\tcamera_poses_pub = nh_.advertise(\"/monoslam/camera_poses\", 1000);\n\tcamera_pose_pub = nh_.advertise(\"/monoslam/camera_pose\", 1000);\n\t\n\todometry_pub = nh_.advertise(\"/monoslam/visual_odometry\", 1000);\n\n\n\tcamera_pub = nh_.advertise(\"/monoslam/camera\", 1000);\n\n\tfeatures_pub = nh_.advertise(\"/monoslam/features\", 1000);\n\n\n\n\tquality_index_pub = nh_.advertise(\"/monoslam/quality_index\",1000);\n\n }\n\n\n\n\n\n ~ImageConverter()\n {\n cv::destroyWindow(WINDOW);\n fs.close();\n }\n\n void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)\n {\n\t v_speed_ = odom_msg->twist.twist.linear.x;\n\t w_speed_ = odom_msg->twist.twist.angular.z;\n }\n\n void imageCb(const sensor_msgs::ImageConstPtr& msg)\n {\n\t//std::cout << \"New Frame Caputerd\" << endl;\n cv_bridge::CvImagePtr cv_ptr;\n try\n {\n cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);\n }\n catch (cv_bridge::Exception& e)\n {\n ROS_ERROR(\"cv_bridge exception: %s\", e.what());\n return;\n }\n \n\tMat frame;\n\tint scale = 1;\n\tcv::resize(cv_ptr->image,cv_ptr->image,cv::Size(cv_ptr->image.size().width/scale, cv_ptr->image.size().height/scale));\n\t\n\tif (first) {\n\n\t\tfirst = 0;\n\n\t\tslam.captureNewFrame(cv_ptr->image);\n\n\n \tslam.findNewFeatures();\n\n\t\tframe = slam.returnImageDrawed();\n\t} else {\n\t\tslam.captureNewFrame(cv_ptr->image, msg->header.stamp.toSec());\n\n\n\n\t\tslam.predict();\n\t\tslam.update();\n\t\t\n\t\tposes.poses.push_back(slam.getCameraPose());\n\t\tcamera_poses_pub.publish(slam.getCameraPath());\n\n\t\tgeometry_msgs::Pose camPose = slam.getCameraPose();\n\n\t\tcamera_pose_pub.publish(camPose);\n\n\t\tfeatures_pub.publish(slam.getFeatures());\n\t\tcamera_pub.publish(slam.ActualCameraRepr());\n\t\t\n\n\t\todometry_pub.publish(slam.getVisualOdometry(msg->header.stamp));\n\t\tframe = slam.returnImageDrawed();\n\t}\n\t\n\t\t\n\n\tcv_ptr->image = frame;\n image_pub_.publish(cv_ptr->toImageMsg());\n\n char name[100];\n static int num = 0;\n\n /*\n \tsprintf(name, \"frame%03d.jpg\", num++);\n \tcv::imwrite(name, frame);\n */\n }\n};\n\nint main(int argc, char** argv)\n{\n\n\tros::init(argc, argv, \"Mono_SLAM\");\n\tchar *file = NULL;\n\tif (argc > 1) file = argv[1];\n\n\n\tstd::cout << \"Starting monoslam_exec\" << endl;\n\t\n \tImageConverter ic(file);\n ros::spin();\n return 0;\n}\n\n\n" }, { "path": "src/utils.cpp", "content": "#include \"utils.hpp\"\n\n\n\nvoid plotFeatures(cv::Mat img, std::vector features) {\n int fontFace = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;\n double fontScale = 0.5;\n int thickness = 3;\n char buffer [33];\n \n for (int i = 0; i < features.size(); i++ ) {\n sprintf(buffer,\"%d\",i);\n std::string s(buffer);\n cv::circle(img, features[i], 10, CV_RGB(255, 32, 32));\n cv::putText(img, s, features[i], fontFace, fontScale, CV_RGB(32,255, 32), thickness, 8);\n }\n}\n\nMatrixXf vConcat(MatrixXf m1, MatrixXf m2) {\n\tif (m1.rows() == 0) return m2;\n\telse {\n\t\tMatrixXf concatMatrix(m1.rows()+m2.rows(), m1.cols());\n\t\tconcatMatrix << m1,m2;\n\t\treturn concatMatrix;\n\t}\n\n}\n\nVectorXf Concat(VectorXf m1, VectorXf m2) {\n\tif (m1.rows() == 0) return m2;\n\telse {\n\t\tVectorXf concatMatrix(m1.rows()+m2.rows());\n\t\tconcatMatrix << m1,m2;\n\t\treturn concatMatrix;\n\t}\n\n}\n\nMatrixXf hConcat(MatrixXf m1, MatrixXf m2) {\n\tif (m1.cols() == 0) return m2;\n\telse {\n\t\tMatrixXf concatMatrix(m1.rows(), m1.cols() + m2.cols());\n\t\tconcatMatrix << m1,m2;\n\t\treturn concatMatrix;\n\t}\n\n}\n" }, { "path": "src/utils.hpp", "content": "#include \n#include \n#include \n#include \n\n#include \n\n\nusing namespace Eigen;\n\nvoid plotFeatures(cv::Mat img, std::vector features);\n\nMatrixXf vConcat(MatrixXf m1, MatrixXf m2);\nMatrixXf hConcat(MatrixXf m1, MatrixXf m2);\nVectorXf Concat(VectorXf m1, VectorXf m2);\n\n\n\n\n" }, { "path": "src/vslamRansac.cpp", "content": "//#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(2,1)\n\n#include \n\n#include \"vslamRansac.hpp\"\n#include \"utils.hpp\"\n\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\n#define STATE_DIM 14\n\n/**\n * Prototipes of the used funcitons\n *\n */\n\n\n\n\n\n/**\n *\n * @param St\n * @param sigma_size\n * @return\n */\n/* Compute the ellispoides parameters from features prediction covariance and store it\n* in a matrix of which each row represent a features ellipoide in form (a,b,theta), where\n* a and b are the semiaxis and theta are the rotation angle\n*/\nMatrixXi computeEllipsoidParameters(MatrixXf St, int sigma_size);\n\n\n/**\n * Converts a Vector3f to a Quaternion\n * @param vec Vector to convert\n * @return Quaternion\n */\nVector4f vec2quat(Vector3f vec);\n\n/**\n * Computes the rotation matrix associated to a quaternion\n * @param quat The quaternion\n * @return The rotation matrix\n */\nMatrix3f quat2rot(Vector4f quat);\n\n/**\n * Computes the quaternion complement associated to a given quaternion\n * @param quat The quaternion\n * @return The complement of quat\n */\nVector4f quatComplement(Vector4f quat);\n\nMatrix3f diffQuat2rot(Vector4f quat, int index); // compute de partial derivate matrix of q2r(q) for q_index\n\nVector4f quatZero();\nVector4f quatCrossProduct(Vector4f h, Vector4f g);\n\n\n// return Jacobian d(g(x))/dx\nMatrixXf dg_x_dx(VectorXf X_old, float dT);\n\n\n// compute the Jacobian between q(t) and q(t-1) knowing the rotation h = quat(w*T)\nMatrix4f Jacobian_qt_qt1(Vector4f h);\n\n// Computer the Jacobian df/dhW\nMatrixXf d_f_d_hW(Vector3f hW);\n\n// compute the Jacobian between q(t) and w(t-1) knowing the actual rotation q, w and dT\nMatrixXf Jacobian_qt_w(Vector4f q, Vector3f w, float dT);\n\n// return the Jacobian d(q^c)/d(q)\nMatrix4f d_qbar_q();\n\n// return Jacobian d(R(q)d)/dq\nMatrixXf dRq_times_d_dq(Vector4f q, Vector3f d);\n\nvoid normalizeQuaternion(VectorXf &mu, MatrixXf &Sigma);\n\n/* state camera Prediction\n\tThis function predicts the State of the camera using kwnoledge on Motion \n\tand the time distance from the last update.\n\t\n\tXv: last state of the camera\n\tdT: time distance\n\t\n\treturn: Predicted state of the camera\n*/\nVectorXf fv(VectorXf Xv, double dT);\n\n\nbool isInsideImage(Vector2f hi, cv::Size size, int windowsSize);\n\n\n\n// VSLAM Class Implementation\n\n\nint VSlamFilter::numOfFeatures() {\n\treturn this->patches.size();\n}\n\nVectorXf VSlamFilter::getState() {\n\tVectorXf state(STATE_DIM);\n\tVector4f qr;\n\tMatrix3f Rot;\n\t//Rot << 0, -1, 0, 0,0,-1,1,0,0;\n\t//qr << 0.5, 0.5, -0.5, 0.5;\n\tstate = mu.segment(0);\n\n return state;\n}\n\nVSlamFilter::VSlamFilter(char *file) : config(file) {\n\n\told_ts = -1;\n\tfloat sigma_vv = 0.0004, sigma_ww = 0.0004;\n\t\n\tkernel_min_size = config.kernel_size;\n\tsigma_pixel = config.sigma_pixel;\n\tsigma_pixel_2 = sigma_pixel*sigma_pixel;\n\tsigma_size = config.sigma_size;\n\twindowsSize = config.window_size;\n\n \tdT = 1;\n\tT_camera = config.T_camera;\n\tscale = config.scale;\n\tcam.setParam(config.camParams);\n\n\n mu = VectorXf::Zero(STATE_DIM); mu(3) = 1; mu(13) = 1; // initialization mu\n\n Vmax = MatrixXf::Identity(6,6);\n\tVmax(0,0) = config.sigma_vx*config.sigma_vx;\n\tVmax(1,1) = config.sigma_vy*config.sigma_vy;\n\tVmax(2,2) = config.sigma_vz*config.sigma_vz;\n\tVmax(3,3) = config.sigma_wx*config.sigma_wx;\n\tVmax(4,4) = config.sigma_wy*config.sigma_wy;\n\tVmax(5,5) = config.sigma_wz*config.sigma_wz;\n\n\n\tSigma = 0.0000000004*MatrixXf::Identity(STATE_DIM,STATE_DIM);\n\tSigma(13,13) = 0.09;\n\n\tSigma.block<3,3>(7,7) = sigma_vv*sigma_vv*MatrixXf::Identity(3,3);\n\tSigma.block<3,3>(10,10) = sigma_ww*sigma_ww*MatrixXf::Identity(3,3);\n\n nInitFeatures = config.nInitFeatures;\n\n}\n\n\n\nvoid VSlamFilter::captureNewFrame(cv::Mat newFrame, double time_stamp) {\n\tif (old_ts > 0) {\n\t\tdT = (time_stamp - old_ts);\n\t\tstd::cout << \"my Dt = \" << dT*1000 << std::endl;\n\t}\n\t\n\told_ts = time_stamp;\n\tcaptureNewFrame(newFrame);\n}\n\nvoid VSlamFilter::captureNewFrame(cv::Mat newFrame) {\n\tcv::resize(newFrame,newFrame,cv::Size(newFrame.size().width/scale, newFrame.size().height/scale));\n\told_frame = frame.clone();\n originalImg = newFrame.clone();\n drawedImg = newFrame.clone();\n cvtColor( newFrame, frame, CV_BGR2GRAY );\n\n}\n\n\nint VSlamFilter::addFeature(cv::Point2f pf) {\n if (pf.x > windowsSize/2 && pf.y > windowsSize/2 && pf.x < frame.size().width - windowsSize/2 && pf.y < frame.size().height - windowsSize/2) {\n\t\n\n \tint pos = mu.size();\n Patch newPat(cv::Mat(frame, cv::Rect(pf.x-windowsSize/2, pf.y-windowsSize/2, windowsSize,windowsSize)), pf, pos);\n patches.push_back(newPat);\n\n\t\t\n Vector2f hd;\n hd << (float)pf.x, (float)pf.y;\n \n Vector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n \n MatrixXf Jac_hCHd;\n Vector3f hC = cam.UndistortAndDeproject(hd, Jac_hCHd);\n \n Matrix3f Rot = quat2rot(q);\n\n Vector3f hW = Rot*hC;\n\n float hx = hW(0);\n float hy = hW(1);\n float hz = hW(2);\n\n float ro = config.rho_0;\n\n\n float theta = atan2(hx,hz);\n float phi = atan2(-hy, sqrt(hx*hx+hz*hz));\t\t\n\n\t\t// Updating state and Sigma\n\n\t\tVectorXf f(6);\n\t\tf << r, theta, phi, ro;\n\t\tmu = Concat(mu,f);\n\n\n int nOld = Sigma.rows();\n MatrixXf Js = MatrixXf::Zero(nOld+6, nOld+3);\n Js.block(0,0,nOld,nOld) = MatrixXf::Identity(nOld,nOld);\n Js.block<3,3>(nOld,0) = MatrixXf::Identity(3,3);\n\t\t\n\t\t\n\n\n MatrixXf Jf_hW = d_f_d_hW(hW);\n MatrixXf J_hW_q = dRq_times_d_dq(q,hC);\n\n Js.block<6,4>(nOld,3) = Jf_hW*J_hW_q;\n Js.block<6,2>(nOld,nOld) = Jf_hW*Rot*Jac_hCHd;\n Js.block<6,1>(nOld, nOld+2) << 0,0,0,0,0,1;\n\n MatrixXf S = sigma_pixel_2*MatrixXf::Identity(nOld+3, nOld+3);\n S.block(0,0, nOld, nOld) = Sigma;\n\n S(nOld + 2, nOld + 2) = config.sigma_rho_0;\n\n\t\tSigma = Js*S*Js.transpose();\n return 1;\n }\n return 0;\n}\n\nvoid VSlamFilter::removeFeature(int index) {\n int pos = patches[index].position_in_state;\n \n Patch p = patches[index];\n\n\n int first = pos;\n \n int size = (p.isXYZ()?3:6);\n\n\n if (p.n_find > 5) {\n\t\tif (p.isXYZ()) {\n\t\t\tp.XYZ_pos = mu.segment<3>(pos);\n\t\t} else {\n\t\t\tMatrixXf J;\n\t\t\tp.XYZ_pos = depth2XYZ(mu.segment<6>(pos), J);\n\t\t}\n\t deleted_patches.push_back(p);\n }\n\n\n int dim = Sigma.cols();\n\n int last = mu.rows() - pos - size;\n\n if (index != patches.size() - 1) {\n mu = Concat(mu.head(first), mu.tail(last));\n Sigma = vConcat(Sigma.topRows(first), Sigma.bottomRows(last));\n Sigma = hConcat(Sigma.leftCols(first), Sigma.rightCols(last));\n } else {\n mu = mu.head(pos).eval();\n Sigma = Sigma.block(0,0,pos,pos).eval();\n }\n\n for(int i = index + 1; i < patches.size(); i++) {\n \tpatches[i].change_position(-size);\n }\n\n patches.erase(patches.begin()+index);\n}\n\n\nvoid VSlamFilter::predict(float v_x, float w_z) {\n\tFt = dg_x_dx(mu.segment<13>(0), dT);\n const int n = Sigma.cols();\n \n MatrixXf Ft_complete = MatrixXf::Identity(n,n);\n Ft_complete.block<13,13>(0,0) = Ft;\n MatrixXf Q;\n Q = Ft.middleCols<6>(7)*(Vmax/dT/dT)*Ft.middleCols<6>(7).transpose();\n\n\n MatrixXf Qtot = MatrixXf::Zero(n,n);\n Qtot.block<13,13>(0,0) = Q;\n Sigma = (Ft_complete*Sigma*Ft_complete.transpose() + Qtot).eval();\n\n mu.segment<13>(0) = fv(mu.segment<13>(0), dT);\n Vector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n Vector3f v = mu.segment<3>(7);\n Vector3f w = mu.segment<3>(10);\n Vector4f h = vec2quat(w);\n\n map_scale = mu(13);\n\n\n\n Vector4f qc = quatComplement(q); // quaternion Complement\n Matrix3f RotCW = quat2rot(qc);\n\n // blurring components\n\n\n Matrix3f RotCW_blurring = quat2rot(quatComplement(quatCrossProduct(q,vec2quat(w*T_camera*dT))));\n Vector3f r_blurring = r + v*T_camera*dT;\n Vector2f hi_out_blurred;\n\n\n VectorXf hi_out;\n MatrixXf Hit;\n \n int featurer_counter = 0;\n \n for (int i = 0; i < patches.size(); i++) {\n\t\tint pos = patches[i].position_in_state;\n if (!patches[i].isXYZ()) { // Feature in Inverse Depth Form\n Hit = MatrixXf::Zero(2, mu.rows());\n VectorXf f = mu.segment<6>(pos);\n\n if (f(5) <= 0) {\n \tROS_ERROR(\"Feature %d ad infinity: %f\", i, f(5));\n \tpatches[i].setRemove();\n \tpatches[i].setIsInInnovation(false);\n \tcontinue;\n }\n MatrixXf J_hW_f;\n Vector3f d = inverse2XYZ(f, r, J_hW_f);\n Vector3f hC = RotCW*d;\n MatrixXf J_h_hC;\n hi_out = cam.projectAndDistort(hC, J_h_hC);\n bool flag = isInsideImage(hi_out, frame.size(), windowsSize) && hC(2) >= 0 && f(5) > 0;\n\n\t\t\tpatches[i].setIsInInnovation(flag);\n if (!flag) continue;\n\t\t\t\n\t\t\tfeaturer_counter++;\n MatrixXf d_h_q = dRq_times_d_dq(qc,d)*d_qbar_q();\n \n Hit.middleCols<3>(0) = -f(5)*J_h_hC*RotCW;\n Hit.middleCols<4>(3) = J_h_hC*d_h_q;\n Hit.middleCols<6>(pos) = J_h_hC*RotCW*J_hW_f;\n patches[i].h = hi_out;\n\t\t\tpatches[i].H = Hit;\n\n\n\t\t\t// Blurring\n\t\t\thi_out_blurred = cam.projectAndDistort(RotCW_blurring*inverse2XYZ(f,r_blurring, J_hW_f), J_h_hC);\n\t\t\tpatches[i].blur(hi_out, hi_out_blurred,kernel_min_size );\n\n\n//#define TEST_BLURR\n#ifdef TEST_BLURR\n\t\t\tfor (float delta_T_camera = 0; delta_T_camera <= 1; delta_T_camera +=0.1) {\n\t\t\t Matrix3f RotCW_blurring = quat2rot(quatComplement(quatCrossProduct(q,vec2quat(w*delta_T_camera))));\n\t\t\t Vector3f r_blurring = r + v*delta_T_camera;\n\t\t\t\tMatrixXf df_blur = cam.projectAndDistort(RotCW_blurring*inverse2XYZ(f,r_blurring, J_hW_f), J_h_hC);\n\t\t\t\tpatches[i].blurTest(hi_out, df_blur ,kernel_min_size, i, 10*delta_T_camera);\n\t\t\t}\n#endif\n\n\n\n } else { // Feature in Inverse Depth Form\n Hit = MatrixXf::Zero(2, mu.rows());\n Vector3f y = mu.segment<3>(pos);\n\n Vector3f d = y-r;\n Vector3f hC = RotCW*d;\n MatrixXf J_h_hC;\n hi_out = cam.projectAndDistort(hC, J_h_hC);\n bool flag = isInsideImage(hi_out, frame.size(), windowsSize) && hC(2) >= 0;\n\n\t\t\tpatches[i].setIsInInnovation(flag);\n if (!flag) continue;\n\n\t\t\tfeaturer_counter++;\n MatrixXf d_h_q = dRq_times_d_dq(qc,d)*d_qbar_q();\n\n Hit.middleCols<3>(0) = -J_h_hC*RotCW;\n Hit.middleCols<4>(3) = J_h_hC*d_h_q;\n Hit.middleCols<3>(pos) = J_h_hC*RotCW;\n patches[i].h = hi_out;\n\t\t\tpatches[i].H = Hit;\n\n\n\t\t\t// Blurring\n\t\t\thi_out_blurred = cam.projectAndDistort(RotCW_blurring*(y-r_blurring), J_h_hC);\n\t\t\tpatches[i].blur(hi_out, hi_out_blurred,kernel_min_size );\n\n#ifdef TEST_BLURR\n\t\t\tfor (float delta_T_camera = 0; delta_T_camera <= 1; delta_T_camera +=0.1) {\n\t\t\t Matrix3f RotCW_blurring = quat2rot(quatComplement(quatCrossProduct(q,vec2quat(w*delta_T_camera))));\n\t\t\t Vector3f r_blurring = r + v*delta_T_camera;\n\t\t\t\tMatrixXf df_blur = cam.projectAndDistort(RotCW_blurring*(y-r_blurring), J_h_hC);\n\t\t\t\tpatches[i].blurTest(hi_out, hi_out_blurred,kernel_min_size, i, 10*delta_T_camera);\n\t\t\t}\n#endif\n\n\n }\n }\n\n VectorXf temp_h_out;\n MatrixXf temp_Ht;\n\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInInnovation()) {\n\t\t\ttemp_h_out = Concat(temp_h_out,patches[i].h);\n\t\t\ttemp_Ht = vConcat(temp_Ht,patches[i].H);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t} \n\t\n h_out = temp_h_out;\n Ht = temp_Ht;\n \n\n if (h_out.rows() > 0) {\n St = (Ht*Sigma*Ht.transpose() + sigma_pixel_2*MatrixXf::Identity(Ht.rows(), Ht.rows())).eval();\n this->drawPrediction();\n }\n}\n\n\nVector3f VSlamFilter::depth2XYZ(VectorXf f, MatrixXf &J) {\n\n\tconst float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n\n Vector3f y = f.segment<3>(0) + m/ro;\n\n\n\n J = MatrixXf::Zero(3,6);\n J.middleCols<3>(0) = Matrix3f::Identity();\n\n J(0,3) = cos(theta)*cos(phi)/ro;\n J(1,3) = 0;\n J(2,3) = -sin(theta)*cos(phi)/ro;\n\n J(0,4) = -sin(theta)*sin(phi)/ro;\n J(1,4) = -cos(phi)/ro;\n J(2,4) = -cos(theta)*sin(phi)/ro;\n\n J.col(5) = -m/(ro*ro);\n return y;\n\n}\n\n\nvoid VSlamFilter::convert2XYZ(int index) {\n\tint pos = patches[index].position_in_state;\n\tint size = 6;\n\tint dx = 3;\n\tVectorXf f = mu.segment<6>(pos);\n\tconst float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n\n Vector3f y = f.segment<3>(0) + m/ro;\n\n Vector3f d = y - mu.segment<3>(0);\n\n // controllo sul L_i\n\n float sigma = Sigma(pos+5,pos+5);\n\n float t = d.transpose()*m;\n\n float L = 4*sigma*abs(t)/(ro*ro*d.squaredNorm());\n if (L <= 0) {\n \tpatches[index].setRemove();\n }\n if (L < 0.01) {\n\n\n MatrixXf J_hp_f = MatrixXf::Zero(3,6);\n J_hp_f.middleCols<3>(0) = Matrix3f::Identity();\n\n\n J_hp_f(0,3) = cos(theta)*cos(phi)/ro;\n J_hp_f(1,3) = 0;\n J_hp_f(2,3) = -sin(theta)*cos(phi)/ro;\n\n J_hp_f(0,4) = -sin(theta)*sin(phi)/ro;\n J_hp_f(1,4) = -cos(phi)/ro;\n J_hp_f(2,4) = -cos(theta)*sin(phi)/ro;\n\n J_hp_f.col(5) = -m/(ro*ro);\n\n\n int first = pos;\n int last = mu.rows() - pos - 3;\n\n\n if (index != patches.size() - 1) {\n mu = Concat(mu.head(first), mu.tail(last));\n mu.segment<3>(pos) = y;\n } else {\n mu = Concat(mu.head(first), y);\n }\n\n int n = Sigma.cols();\n MatrixXf J = MatrixXf::Zero(n-3,n);\n J.topLeftCorner(pos,pos) = MatrixXf::Identity(pos,pos);\n J.block<3,6>(pos,pos) = J_hp_f;\n if (index != patches.size() - 1) {\n \tJ.bottomRightCorner(n-pos-6,n-pos-6) = MatrixXf::Identity(n-pos-6,n-pos-6);\n }\n\n Sigma = J*Sigma*J.transpose();\n\n patches[index].convertInXYZ();\n\n for(int i = index + 1; i < patches.size(); i++) {\n \tpatches[i].change_position(-3);\n }\n }\n}\n\n\nvoid VSlamFilter::findFeaturesConvertible2XYZ() {\n\tfor (int i = 0; i < patches.size(); i++) {\n\t\tif (!patches[i].isXYZ()) convert2XYZ(i);\n\t}\n\n}\n\n\n\nvoid VSlamFilter::findNewFeatures(int num) {\n\tROS_INFO(\"Finding new Feature\");\n\n\tif (num <= 0) num = nInitFeatures;\n\n int winSize = 2*windowsSize+1;\n cv::Mat mask = cv::Mat(frame.size(),CV_8UC1 );\n mask.setTo(cv::Scalar(0));\n\tint estrem = windowsSize;\n cv::Mat(mask, cv::Rect(estrem, estrem,mask.size().width-2*estrem, mask.size().height - 2*estrem)).setTo(cv::Scalar(255));\n\n\n\n for (int i = 0; i < patches.size(); i++) {\n \tif (patches[i].center.x > windowsSize && \n \t\tpatches[i].center.y > windowsSize && \n \t\tpatches[i].center.x < mask.size().width - windowsSize && \n \t\tpatches[i].center.y < mask.size().height - windowsSize) \n \t\t{\n \t\tint x = (patches[i].center.x-winSize/2 > 0 ? \n \t\t\t\tpatches[i].center.x-winSize/2 : 0);\n \t\tint y = (patches[i].center.y-winSize/2 > 0 ? \n \t\t\t\tpatches[i].center.y-winSize/2 : 0);\n \t\tint width = winSize - \n \t\t\t(patches[i].center.x+winSize/2 <= frame.size().width ? \n \t\t\t0 : frame.size().width - patches[i].center.x-winSize/2);\n \t\t\n\t\t\tint height = winSize - \n \t\t\t(patches[i].center.y+winSize/2 <= frame.size().height ? \n \t\t\t0 : frame.size().height - patches[i].center.y-winSize/2);\n \t\t\t\n \t\tcv::Rect roi = cv::Rect(x,y, width, height);\n \t\tcv::Mat(mask, roi).setTo(cv::Scalar(0));\n\n \t}\n }\n std::vector features;\n\t\n\t\n goodFeaturesToTrack(frame,features,num,0.01f,5,mask);\n\n for (int i = 0; i < features.size(); i++) {\n\tcv::Point2f newFeature = cv::Point2f(features[i].x, features[i].y);\n this->addFeature(newFeature);\n }\n \n\tROS_DEBUG(\"Found %d Feature[s]\", features.size());\n\n}\n\nvoid VSlamFilter::update(float v_x, float w_z) {\n\n cv::Point2f locF;\n int matchedFeatures = 0;\n int searchedFeatures = 0;\n\n\t\n for (int i = 0; i < patches.size(); i++) {\n if (patches[i].patchIsInInnovation()) {\n patches[i].findMatch(frame, St.block<2,2>(patches[i].position_in_z,patches[i].position_in_z),sigma_size, false);\n searchedFeatures++;\n }\n }\n\n#ifdef USE_RANSAC\n\n\tint nhyp = 10000;\n\tfloat p = 0.99;\n\tfloat th = 2*sigma_pixel;\n\t\n\tsrand(time(NULL));\n\t\n\tint num_zli = 0;\n\t\n\tstd::vector ransacindex;\n\n\n\tfor (int i = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInInnovation())\n\t\t\transacindex.push_back(i);\n\t}\n\n\n\tfor (int i = 0; i < nhyp && ransacindex.size() > 0; i++) {\n\t\tint actual_num_zli = 0;\n\n\t\tint posRansac = rand()%ransacindex.size();\n\t\tint selectPatch = ransacindex[posRansac];\n\t\transacindex.erase(ransacindex.begin() + posRansac);\n\n\t\tMatrixXf S_i = patches[selectPatch].H*Sigma*patches[selectPatch].H.transpose() + sigma_pixel_2*MatrixXf::Identity(patches[selectPatch].H.rows(), patches[selectPatch].H.rows());\n MatrixXf K_i = Sigma*patches[selectPatch].H.transpose()*S_i.inverse();\n\t\tVectorXf mu_i = mu + K_i*(patches[selectPatch].z - patches[selectPatch].h);\n\t\t\n\t\tVector3f r = mu_i.segment<3>(0);\n\t\tVector4f q = mu_i.segment<4>(3)/mu_i.segment<4>(3).norm();\n \tMatrix3f RotCW = quat2rot(quatComplement(q));\t\n\t\t\n \tint searched_features = 0;\n\t\tfor (int i = 0; i < patches.size(); i++) {\n\t\t\tif (!patches[i].patchIsInInnovation()) continue;\n\t\t\tVector2f hi_i;\n\t\t\tint pos = patches[i].position_in_state;\n\n\t\t\tif (!patches[i].isXYZ()) {\n\t\t\t\tVectorXf f = mu_i.segment<6>(pos);\n\t\t\t\tVector3f d = inverse2XYZ(f, r);\n\t\t\t\thi_i = cam.projectAndDistort(RotCW*d);\n\t\t\t} else {\n\t Vector3f y = mu.segment<3>(pos);\n\t Vector3f d = y-r;\n\t Vector3f hC = RotCW*d;\n\t MatrixXf J_hf_hC;\n\t hi_i = cam.projectAndDistort(hC, J_hf_hC);\n\t\t\t}\n\n\t\t\tpatches[i].setIsInLi((patches[i].z - hi_i).norm() <= th);\n\t\t\tif (patches[i].patchIsInLi()) actual_num_zli++;\n\t\t\tsearched_features++;\n\n \t}\n \t\n \tif (actual_num_zli > num_zli) {\n \t\tnum_zli = actual_num_zli;\n \t\tnhyp = log(1-p)/(log(num_zli/(matchedFeatures+0.0f)));\n \t\tfor (int i = 0; i < patches.size(); i++) {\n \t\t\tif (patches[i].patchIsInInnovation()) patches[i].ConfirmIsInLi();\n \t\t}\n \t}\n\t}\n\t\n\n\tVectorXf z_li;\n MatrixXf H_li;\n VectorXf h_li;\n\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInLi()) {\n\t\t\th_li = Concat(h_li,patches[i].h);\n\t\t\tH_li = vConcat(H_li,patches[i].H);\n\t\t\tz_li = Concat(z_li, patches[i].z);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t} \n\n\n\tMatrixXf Sigma_tmp = Sigma;\n\tVectorXf mu_tmp = mu;\n\n if (z_li.rows() > 0) {\n \tint p = H_li.rows();\n \tSt = H_li*Sigma_tmp*H_li.transpose() + sigma_pixel_2*MatrixXf::Identity(p,p);\n \t//Kt = Sigma*H_li.transpose()*St.inverse();\n \t//MatrixXf Stinv = MatrixXf::Identity(p,p);\n \t//St.llt().solveInPlace(Stinv);\n \tKt = Sigma_tmp*H_li.transpose()*St.inverse();//\n\n \t//\tMatrixXf Stinv = St.lu().solve(Matrix::Identity(p,p));\n \t//\tKt = Sigma*H_li.transpose()*Stinv;\n\n \tmu_tmp = mu + Kt*(z_li - h_li);\n \tSigma_tmp = ((MatrixXf::Identity(Sigma_tmp.rows(),Sigma_tmp.rows()) - Kt*H_li)*Sigma_tmp);\n \tnormalizeQuaternion(mu_tmp,Sigma_tmp);\n } else {\n ROS_ERROR(\"No Matching li\");\n }\n\n \n //////////////////////////////////////////////////////////\n\tth = 1;\n if (1) {\n\t\tVector3f r = mu.segment<3>(0);\n\t\tVector4f q = mu.segment<4>(3);\n\t\tVector4f qc = quatComplement(q);\n\t\tMatrix3f RotCW = quat2rot(qc);\n\t\tMatrix2f S_hi;\n\t\tVector2f hi_hi;\n\n\n\n\t\tint counter = 0;\n\t\tfor (int i = 0; i < patches.size(); i++) {\n\t\t\tif (!patches[i].patchIsInLi() && patches[i].patchIsInInnovation()) {\n\t\t\t\tint pos = patches[i].position_in_state;\n\t\t\t\tif (!patches[i].isXYZ()) {\n\t\t\t\t\tMatrixXf Hi_hi = MatrixXf::Zero(2, mu.rows());\n\t\t\t\t\tVectorXf f = mu.segment<6>(pos);\n\t\t\t\t\tMatrixXf J_hp_f;\n\t\t\t\t\tVector3f d = inverse2XYZ(f, r, J_hp_f);\n\t\t\t\t\tMatrixXf J_hf_hi;\n\t\t\t\t\tpatches[i].h = cam.projectAndDistort(RotCW*d, J_hf_hi);\n\n\t\t MatrixXf d_h_q = dRq_times_d_dq(qc,d)*d_qbar_q();\n\t\t Hi_hi.middleCols<3>(0) = -f(5)*J_hf_hi*RotCW;\n\t\t Hi_hi.middleCols<4>(3) = J_hf_hi*d_h_q;\n\t\t Hi_hi.middleCols<6>(pos) = J_hf_hi*RotCW*J_hp_f;\n\t\t\t\t\tpatches[i].H = Hi_hi;\n\n\n\t\t\t\t} else {\n\t\t\t\t\tMatrixXf Hi_hi = MatrixXf::Zero(2, mu.rows());\n\t\t Vector3f y = mu.segment<3>(pos);\n\t\t Vector3f d = y-r;\n\t\t Vector3f hC = RotCW*d;\n\t\t MatrixXf J_hf_hC;\n\t\t patches[i].h = cam.projectAndDistort(hC, J_hf_hC);\n\n\t\t MatrixXf d_h_q = dRq_times_d_dq(qc,d)*d_qbar_q();\n\n\t\t Hi_hi.middleCols<3>(0) = -J_hf_hC*RotCW;\n\t\t Hi_hi.middleCols<4>(3) = J_hf_hC*d_h_q;\n\t\t Hi_hi.middleCols<3>(pos) = J_hf_hC*RotCW;\n\t\t\t\t\tpatches[i].H = Hi_hi;\n\n\t\t\t\t}\n\n\t\t\t\tS_hi = patches[i].H*Sigma_tmp*patches[i].H.transpose();\n\t\t\t\tpatches[i].setIsInHi((patches[i].h - patches[i].z).transpose()*S_hi.inverse()*(patches[i].h - patches[i].z) <= th);\n\t\t\t\tcounter++;\n\t\t\t}\n\t\t}\n }\n\n\n VectorXf z_hi;\n MatrixXf H_hi;\n VectorXf h_hi;\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInHi()) {\n\t\t\th_hi = Concat(h_hi, patches[i].h);\n\t\t\tH_hi = vConcat(H_hi, patches[i].H);\n\t\t\tz_hi = Concat(z_hi, patches[i].z);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t} \n\n#endif\n\n\n#ifndef USE_RANSAC\n VectorXf z_hi;\n MatrixXf H_hi;\n VectorXf h_hi;\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInInnovation()) {\n\t\t\th_hi = Concat(h_hi, patches[i].h);\n\t\t\tH_hi = vConcat(H_hi, patches[i].H);\n\t\t\tz_hi = Concat(z_hi, patches[i].z);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t}\n#endif\n\n\n\n\tbool flag_correction = false;\n\tMatrix3f corr_ass_sigma = 0.00001*Matrix3f::Identity();\n\n\tif (config.forsePlane) {\n\t\tVector3f corr_ass = Vector3f::Zero();\n\t\tMatrixXf corr_ass_H = MatrixXf::Zero(3,mu_tmp.size());\n\t\tcorr_ass_H(0,1) = 1;\n\t\tcorr_ass_H(1,4) = 1;\n\t\tcorr_ass_H(2,6) = 1;\n\n\t\tVector3f h_corr_ass;\n\t\th_corr_ass << mu_tmp(1), mu_tmp(4), mu_tmp(6);\n\t\th_hi = Concat(h_hi, h_corr_ass);\n\t\tz_hi = Concat(z_hi, corr_ass);\n\t\tH_hi = vConcat(H_hi, corr_ass_H);\n\t\tflag_correction = true;\n\t}\n\n\n\n\n if (z_hi.rows() > 0) {\n \tint p = H_hi.rows();\n \tSt = H_hi*Sigma_tmp*H_hi.transpose();\n\n \tMatrixXf St_error = sigma_pixel_2*MatrixXf::Identity(p,p);\n \t//if (flag_odom) St_error(p-1,p-1) = 0.2;\n \tif (flag_correction) St_error.block<3,3>(p-3,p-3) = corr_ass_sigma;\n\n \tSt += St_error;\n\n \tKt = Sigma_tmp*H_hi.transpose()*St.inverse();//lu().solve(Matrix::Identity(p,p));\n \tmu_tmp = mu_tmp + Kt*(z_hi - h_hi);\n Sigma_tmp = ((MatrixXf::Identity(Sigma_tmp.rows(),Sigma_tmp.rows()) - Kt*H_hi)*Sigma_tmp).eval();\n \tnormalizeQuaternion(mu_tmp,Sigma_tmp);\n }\n \n if (1) {\n \tmu = mu_tmp;\n \tSigma = Sigma_tmp;\n }\n\n\n\n for (int i = 0; i < patches.size(); i++) {\n \tpatches[i].drawUpdate(drawedImg, i);\n }\n\n\n\tfor (int i = patches.size()-1; i >= 0; --i){\n\t\tpatches[i].update_quality_index();\n\t\tif (patches[i].mustBeRemove()) this->removeFeature(i);\n\t}\n\n int nVisibleFeature = 0;\n for (int i = 0; i < patches.size(); i++) if (patches[i].patchIsInInnovation()) nVisibleFeature++;\n\n if (nVisibleFeature < config.min_features) {\n \tif (patches.size() > config.max_features) this->removeFeature(0);\n \tthis->findNewFeatures(5);\n }\n\n\tfindFeaturesConvertible2XYZ();\n}\n\n\nvoid VSlamFilter::drawUpdate(cv::Point f) {\n cv::rectangle(drawedImg, cv::Point(f.x - windowsSize/2, f.y - windowsSize/2), cv::Point(f.x + windowsSize/2, f.y + windowsSize/2), CV_RGB(0,0,255), 1);\n cv::circle(drawedImg, f, 2, CV_RGB(0,0,255),2);\n \n\t/*std::stringstream text;\n\ttext << 1;\n\tcv::putText(drawedImg,text.str(), f,cv::FONT_HERSHEY_SIMPLEX, 2, CV_RGB(0,0,255));\n\t*/\n}\n\n\nvoid VSlamFilter::drawPrediction() {\n \n MatrixXi sigma_mis = computeEllipsoidParameters(St, sigma_size);\n for (int i = 0; i < St.rows()/2; i++) {\n cv::Point2i predictFeature(h_out(2*i), h_out(2*i+1));\n cv::ellipse(drawedImg, predictFeature, cv::Size(sigma_mis(i,0),sigma_mis(i,1)), (double)sigma_mis(i,2), 0, 360, CV_RGB(32, 32, 255));\n }\n}\n\ncv::Mat VSlamFilter::returnImageDrawed() {\n return drawedImg.clone();\n}\nMatrixXi computeEllipsoidParameters(MatrixXf St, int sigma_size) {\n int nFeatures = St.cols()/2;\n MatrixXi sigma_mis(nFeatures,3);\n for (int i = 0; i < nFeatures; i++) {\n SelfAdjointEigenSolver eigenSolver(St.block<2,2>(2*i,2*i));\n Vector2f eigs = eigenSolver.eigenvalues();\n Matrix2f vecs = eigenSolver.eigenvectors();\n \n sigma_mis(i,0) = (eigs(0) > 0 ? (int)sigma_size*sqrt((double)eigs(0)) : 1);\n sigma_mis(i,1) = (eigs(1) > 0 ? (int)sigma_size*sqrt((double)eigs(1)) : 1);\n sigma_mis(i,2) = (int)(180/3.14*atan2((double)vecs(1,0), (double)vecs(0,0)));\n }\n return sigma_mis;\n}\n\n\n\n\n/*************************/\n\nVector4f vec2quat(Vector3f vec) {\n float alpha = vec.norm();\n \n if (alpha != 0) {\n Vector4f quat;\n quat(0) = cos(alpha/2);\n quat.segment<3>(1) = vec*sin(alpha/2)/alpha;\n return quat;\n }\n else {\n return quatZero();\n }\n}\n\nVector4f quatZero() {\n Vector4f quat;\n quat << 1,0,0,0;\n return quat;\n}\n\nMatrix3f quat2rot(Vector4f quat) {\n float qr = quat(0);\n float qi = quat(1);\n float qj = quat(2);\n float qk = quat(3);\n \n const float w = quat(0);\n const float x = quat(1);\n const float y = quat(2);\n const float z = quat(3);\n\n Matrix3f rot;\n\n\trot <<\t qr*qr + qi*qi - qj*qj - qk*qk, \t\t-2*qr*qk+2*qi*qj,\t\t\t\t\t 2*qr*qj+2*qi*qk,\n\t\t 2*qr*qk+2*qi*qj,\t\t\t\t qr*qr - qi*qi + qj*qj - qk*qk,\t\t\t\t-2*qr*qi+2*qj*qk,\n\t\t-2*qr*qj+2*qi*qk,\t\t\t\t 2*qr*qi+2*qj*qk,\t\t\t\t\t qr*qr - qi*qi - qj*qj + qk*qk;\n /*\n\trot <<\t qr*qr + qi*qi - qj*qj - qk*qk, \t\t-2*qr*qk+2*qi*qj,\t\t\t\t\t 2*qr*qj+2*qi*qk,\n\t\t 2*qr*qk+2*qi*qj,\t\t\t\t qr*qr - qi*qi + qj*qj - qk*qk,\t\t\t\t-2*qr*qi+2*qj*qk,\n\t\t-2*qr*qj+2*qi*qk,\t\t\t\t 2*qr*qi+2*qj*qk,\t\t\t\t\t qr*qr - qi*qi - qj*qj + qk*qk;\n\n\n\tconst float norm = quat.segment<3>(1).norm();\n\tVector3f v = quat.segment<3>(1);\n\tMatrix3f skew;\n\tskew << 0, -z, y, z, 0, -x, -y, x, 0;\n\trot = Matrix3f::Identity()*(1 - 2*norm*norm) + 2*(v*v.transpose() + w*skew);\n*/\n return rot;\n}\n\nMatrix4f YupsilonMatric(Vector4f q1) {\n\tMatrix4f res;\n \tconst float r1=q1(0);\n \tconst float x1=q1(1);\n \tconst float y1=q1(2);\n \tconst float z1=q1(3);\n\tres << r1, -x1, -y1, -z1,\n\t\tx1, r1, -z1, y1,\n\t\ty1, z1, r1, -x1,\n\t\tz1, -y1, x1, r1;\n\t\n\treturn res;\n\t\t\n}\n\n\nMatrix4f YupsilonMatricComplementar(Vector4f q1) {\n\tMatrix4f res;\n \tconst float r1=q1(0);\n \tconst float x1=q1(1);\n \tconst float y1=q1(2);\n \tconst float z1=q1(3);\n\tres << r1, -x1, -y1, -z1,\n\t\tx1, r1, z1, -y1,\n\t\ty1, -z1, r1, x1,\n\t\tz1, y1, -x1, r1;\n\n\treturn res;\n\t\t\n}\n\nVector4f quatCrossProduct(Vector4f q1, Vector4f q2) {\n Vector4f q;\n // First quaternion q1 (x1 y1 z1 r1)\n const float r1=q1(0);\n const float x1=q1(1);\n const float y1=q1(2);\n const float z1=q1(3);\n \n \n // Second quaternion q2 (x2 y2 z2 r2)\n const float r2=q2(0);\n const float x2=q2(1);\n const float y2=q2(2);\n const float z2=q2(3);\n \n q(0) = r1*r2 - x1*x2 - y1*y2 - z1*z2; // r component\n q(1) = x1*r2 + r1*x2 + y1*z2 - z1*y2; // x component\n q(2) = r1*y2 - x1*z2 + y1*r2 + z1*x2; // y component\n q(3) = r1*z2 + x1*y2 - y1*x2 + z1*r2; // z component\n\n\n \n return YupsilonMatric(q1)*q2;\n}\n\n\n\nVector3f inverse2XYZ(VectorXf f, Vector3f r, MatrixXf &J_hp_f, Matrix3f R) {\n const float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n \n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n \n J_hp_f = MatrixXf(3,6);\n J_hp_f.middleCols<3>(0) = ro*Matrix3f::Identity();\n\n J_hp_f(0,3) = cos(theta)*cos(phi);\n J_hp_f(1,3) = 0;\n J_hp_f(2,3) = -sin(theta)*cos(phi);\n \n J_hp_f(0,4) = -sin(theta)*sin(phi);\n J_hp_f(1,4) = -cos(phi);\n J_hp_f(2,4) = -cos(theta)*sin(phi);\n \n J_hp_f.col(5) = f.segment<3>(0)-r;\n \n \n //J_hp_f= R*J_hp_f;\n\n \n MatrixXf ret;\n //ret = R*(ro*(f.segment<3>(0)-r) + m);\n ret = (ro*(f.segment<3>(0)-r) + m);\n return ret;\n}\n\nVector3f inverse2XYZ(VectorXf f, Vector3f r) {\n const float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n \n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n \n return ro*(f.segment<3>(0)-r) + m;\n}\n\n// return Jacobian d(g(x))/dx\nMatrixXf dg_x_dx(VectorXf mu, float dT) {\n\tVector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n Vector3f v = mu.segment<3>(7);\n Vector3f w = mu.segment<3>(10);\n Vector4f h = vec2quat(dT*w); // h = quat(w)\n\n\tMatrixXf Ft;\n Ft = MatrixXf::Identity(13,13);\n Ft.block<4,4>(3,3) = Jacobian_qt_qt1(h);\n Ft.block<4,3>(3,10) = Jacobian_qt_w(q,w,dT);\n\tFt.block<3,3>(0,7) = dT*MatrixXf::Identity(3,3);\n\n\treturn Ft;\n}\n\n\n\n// compute the Jacobian between q(t) and q(t-1) knowing the rotation h = quat(w*T)\nMatrix4f Jacobian_qt_qt1(Vector4f h) {\n const float hr=h(0);\n const float hx=h(1);\n const float hy=h(2);\n const float hz=h(3);\n \n Matrix4f ret;\n ret << hr, -hx, -hy, -hz,\n hx, hr, hz, -hy,\n hy, -hz, hr, hx,\n hz, hy, -hx, hr;\n\n return YupsilonMatricComplementar(h); \n}\n\n// compute the Jacobian between q(t) and w(t-1) knowing the actual rotation q, w and dT\nMatrixXf Jacobian_qt_w(Vector4f q, Vector3f w, float dT) {\n const float qr=q(0);\n const float qx=q(1);\n const float qy=q(2);\n const float qz=q(3);\n \n const float n = w.norm();\n const float s = sin(dT*n/2);\n const float c = cos(dT*n/2);\n const float Sinc = (n == 0 ? 1: 2*sin(dT*n/2)/(dT*n));\n \n Vector3f n_w;\n if (n > 0) n_w = w/n;\n \n Matrix4f t1;\n\tt1 << \tqr, -qx, -qy, -qz,\n \tqx, qr, -qz, qy,\n \tqy, qz, qr, -qx,\n qz, -qy, qx, qr;\n\n\tt1 = YupsilonMatric(q);\n \n MatrixXf t2 = MatrixXf::Zero(4,3);\n t2.row(0) = -dT*0.5*s*n_w.transpose();\n t2.middleRows<3>(1) = dT*0.5*(Sinc*Matrix3f::Identity() + (c-Sinc)*n_w*n_w.transpose());\n\n\tMatrixXf ret;\n\tret = t1*t2;\n return ret;\n}\n\nMatrix3f diffQuat2rot(Vector4f quat, int index) {\n Matrix3f dR;\n const float q0_2 = 2*quat(0);\n const float qx_2 = 2*quat(1);\n const float qy_2 = 2*quat(2);\n const float qz_2 = 2*quat(3);\n\n const float w = quat(0);\n const float x = quat(1);\n const float y = quat(2);\n const float z = quat(3);\n \n if (index == 0) {\n dR << \t q0_2, \t-qz_2,\t qy_2,\n \t\t qz_2,\t q0_2,\t-qx_2,\n \t\t-qy_2,\t qx_2,\t q0_2;\n\n \t\t/*dR << \t 0, \t-qz_2,\t qy_2,\n \t\t qz_2,\t 0,\t-qx_2,\n \t\t-qy_2,\t qx_2,\t 0;*/\n \n } else if (index == 1) {\n dR << \tqx_2, \t qy_2, \t qz_2,\n \t\tqy_2, \t-qx_2, \t-q0_2,\n \t\tqz_2,\t q0_2,\t-qx_2;\n\n\t\t/*dR << \t0, \t qy_2, \t qz_2,\n \t\tqy_2, \t-2*qx_2, -q0_2,\n \t\tqz_2,\t q0_2,\t-2*qx_2;*/\n \n } else if (index == 2) {\n dR <<\t-qy_2, qx_2, q0_2,\n \t\t qx_2, qy_2, qz_2,\n \t\t-q0_2, qz_2, -qy_2;\n\n \t\t/*dR <<\t-2*qy_2, qx_2, q0_2,\n \t\t qx_2, 0, \tqz_2,\n \t\t-q0_2, qz_2, -2*qy_2;*/\n \n } else if (index == 3) {\n dR <<\t-qz_2,\t\t-q0_2, \tqx_2,\n \t\t q0_2, -qz_2, qy_2,\n \t\t qx_2, qy_2, qz_2;\n\n /*dR <<\t-2*qz_2,\t\t-q0_2, \tqx_2,\n \t\t q0_2, -2*qz_2, qy_2,\n \t\t qx_2, qy_2, 0;*/\n }\n return dR;\n}\n\nVector4f quatComplement(Vector4f quat) {\n Vector4f qC;\n qC << quat(0), -quat(1), -quat(2), -quat(3);\n return qC;\n}\n\n// state camera Update\nVectorXf fv(VectorXf Xv, double dT) {\n\n\n\tVector3f v = Xv.segment<3>(7);\n\tVector3f w = Xv.segment<3>(10);\n\n Xv.segment<3>(0) += dT*v;\n Xv.segment<4>(3) = quatCrossProduct( Xv.segment<4>(3),vec2quat(dT*w));\n\n Xv.segment<3>(7) = v;\n\tXv.segment<3>(10) = w;\n return Xv;\n}\n\nMatrix4f d_qbar_q() {\n Matrix4f J = -Matrix4f::Identity();\n J(0,0) = 1;\n return J;\n}\n\n\n// Compute the Jacobian df/dhW\nMatrixXf d_f_d_hW(Vector3f hW) {\n const float hx = hW(0);\n const float hy = hW(1);\n const float hz = hW(2);\n \n // d_Theta_hW\n MatrixXf J_Theta_hW(1,3);\n float normal = hx*hx+hz*hz;\n J_Theta_hW(0,0) = hz/normal;\n J_Theta_hW(0,1) = 0;\n J_Theta_hW(0,2) = -hx/normal;\n\n \n // d_Phi_hW\n float normal2 = hx*hx+hy*hy+hz*hz;\n MatrixXf J_Phi_hW(1,3);\n J_Phi_hW(0,0) = hx*hy/sqrt(normal)/normal2;\n J_Phi_hW(0,1) = -sqrt(normal)/normal2;\n J_Phi_hW(0,2) = hz*hy/sqrt(normal)/normal2;\n \n \n MatrixXf J_f_hW = MatrixXf::Zero(6,3);\n J_f_hW.row(3) = J_Theta_hW;\n J_f_hW.row(4) = J_Phi_hW;\n\n return J_f_hW;\n}\n\n\nvoid normalizeQuaternion(VectorXf &mu, MatrixXf &Sigma) {\n Vector4f q = mu.segment<4>(3);\n \n const float norma = q.norm();\n mu.segment<4>(3) = q/norma;\n\n MatrixXf Q;\n Q = norma*norma*Matrix4f::Identity() - q*q.transpose();\n \n Q = (Q*(1/(norma*norma*norma))).eval();\n\n MatrixXf Qc = MatrixXf::Identity(Sigma.rows(), Sigma.cols());\n Qc.block<4,4>(3,3) = Q;\n\n\tSigma = (Qc*Sigma*Qc.transpose()).eval();\n\n}\n\n\n\nbool isInsideImage(Vector2f hi, cv::Size size, int windowsSize) {\n float i = hi(0);\n float j = hi(1);\n \n if (i > windowsSize/2 && j > windowsSize/2 && i < size.width - windowsSize/2 && j < size.height - windowsSize/2) {\n return true;\n }\n return false;\n\n}\n\nMatrixXf dRq_times_d_dq(Vector4f q, Vector3f d) {\n MatrixXf diff_Rq_times_dq(3,4);\n for (int j = 0; j < 4; j++) {\n \tdiff_Rq_times_dq.col(j) = diffQuat2rot(q,j)*d;\n }\n return diff_Rq_times_dq;\n}\n\n" }, { "path": "src/vslamRansac.hpp", "content": "#define DEBUG\n\n#include \n#include \"camModel.hpp\"\n\n#include \"Patch.hpp\"\n\n#include \n\n#include \"ConfigVSLAM.h\"\n\n\n\nclass VSlamFilter {\n CamModel cam;\n \n\tfloat dT;\n\n MatrixXf Vmax; //Covariance of speed (linear and rotation)\n\n MatrixXf Kt;\n MatrixXf Ft;\n MatrixXf Fnt;\n MatrixXf Ht; // measures prediction\n VectorXf h_out;\n VectorXf tot_h_out;\n\n MatrixXf St; // innovation covariance matrix\n \n \n \n cv::Mat frame, old_frame;\n cv::Mat originalImg;\n cv::Mat drawedImg;\n \n \n void drawPrediction();\n void drawUpdate(cv::Point f);\n \nprotected:\n float map_scale; // scale of the map\n\n std::vector patches;\n std::vector deleted_patches;\n\n VectorXf mu;\n MatrixXf Sigma;\n\tint nFeatures;\n float T_camera;\n ConfigVSLAM config;\n\t\n\n int windowsSize;\n int sigma_pixel;\n int sigma_pixel_2;\n\n\tint kernel_min_size;\n\tint scale;\n\tint sigma_size;\n\n\tint nInitFeatures;\n\n\tint camera_state_dim;\n\n\tfloat vz_odom;\n\tMatrixXf Hvz_odom;\n\t\n\t\n\tdouble old_ts;\n\n\t\npublic:\n\n\n\n\tfloat feature_index;\n\n VSlamFilter(char *file = NULL);\n int addFeature(cv::Point2f pf);\n\n void removeFeature(int index);\n\n void predict(float v_x = 0, float w_z = 0);\n void update(float v_x = 0, float w_z = 0);\n \n \n void captureNewFrame(cv::Mat newFrame);\n void captureNewFrame(cv::Mat newFrame, double time_stamp);\n \n cv::Mat returnImageDrawed();\n \n VectorXf getState();\n \n void findNewFeatures(int num = -1);\n\n void convert2XYZ(int index);\n void findFeaturesConvertible2XYZ();\n Vector3f depth2XYZ(VectorXf f, MatrixXf &J);\n\n int numOfFeatures();\n\n\n};\n\nVector3f inverse2XYZ(VectorXf f, Vector3f r, MatrixXf &J_hp_f , Matrix3f R = Matrix3f::Identity());\nVector3f inverse2XYZ(VectorXf f, Vector3f r);\n" }, { "path": "src/vslamRansac_OLD.cpp", "content": "//#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(2,1)\n\n#include \n\n#include \"vslamRansac.hpp\"\n#include \"utils.hpp\"\n\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n\n\n\n// Compute the ellispoides parameters from features prediction covariance and store it\n// in a matrix of which each row represent a features ellipoide in form (a,b,theta), where\n// a and b are the semiaxis and theta are the rotation angle\nMatrixXi computeEllipsoidParameters(MatrixXf St, int sigma_size);\n\n\nVector4f vec2quat(Vector3f vec);\nMatrix3f quat2rot(Vector4f quat);\nVector4f quatComplement(Vector4f quat);\n\nMatrix3f diffQuat2rot(Vector4f quat, int index); // compute de partial derivate matrix of q2r(q) for q_index\n\nVector4f quatZero();\nVector4f quatCrossProduct(Vector4f h, Vector4f g);\n\n\n// return Jacobian d(g(x))/dx\nMatrixXf dg_x_dx(VectorXf X_old);\n\n\n// compute the Jacobian between q(t) and q(t-1) knowing the rotation h = quat(w*T)\nMatrix4f Jacobian_qt_qt1(Vector4f h);\n\n// Computer the Jacobian df/dhW\nMatrixXf d_f_d_hW(Vector3f hW);\n\n// compute the Jacobian between q(t) and w(t-1) knowing the actual rotation q, w and dT\nMatrixXf Jacobian_qt_w(Vector4f q, Vector3f w);\n\n// return the Jacobian d(q^c)/d(q)\nMatrix4f d_qbar_q();\n\n// return Jacobian d(R(q)d)/dq\nMatrixXf dRq_times_d_dq(Vector4f q, Vector3f d);\n\nvoid normalizeQuaternion(VectorXf &mu, MatrixXf &Sigma);\n\n// state camera Update\nVectorXf fv(VectorXf Xv_1, float v_x, float w_z);\n\n\nbool isInsideImage(Vector2f hi, cv::Size size, int windowsSize);\n\n\n\n// VSLAM Class Implementation\n\nVectorXf VSlamFilter::getState() {\n\tVectorXf state(13);\n\tVector4f qr;\n\tMatrix3f Rot;\n\t//Rot << 0, -1, 0, 0,0,-1,1,0,0;\n\t//qr << 0.5, 0.5, -0.5, 0.5;\n\tstate = mu.segment<13>(0);\n\n return state;\n}\n\nVSlamFilter::VSlamFilter(char *file) : config(file) {\n\n\tfloat s_v = config.sigma_v, s_w = config.sigma_v;\n\tfloat sigma_vv = 0.0004, sigma_ww = 0.0004;\n\t\n\tkernel_min_size = config.kernel_size;\n\n\tsigma_pixel = config.sigma_pixel;\n\tsigma_pixel_2 = sigma_pixel*sigma_pixel;\n\n\tsigma_size = config.sigma_size;\n\n\twindowsSize = config.window_size;\n\n \tnFeatures = 0;\n \tdT = 1;\n \tT_camera = config.T_camera;\n\tscale = config.scale;\n\tcam.setParam(config.camParams);\n\n\n mu = VectorXf::Zero(13); mu(3) = 1; // initializa mu\n\n\n VectorXf Pn_diagonal(6);\n\n\tVmax = s_w*s_w*MatrixXf::Identity(6,6);\n\tVmax.block<3,3>(0,0) = s_v*s_v*Matrix3f::Identity();\n\n\n\n\n\tSigma = 0.0000000004*MatrixXf::Identity(13,13);\n\n Sigma.block<3,3>(7,7) = sigma_vv*sigma_vv*MatrixXf::Identity(3,3);\n Sigma.block<3,3>(10,10) = sigma_ww*sigma_ww*MatrixXf::Identity(3,3);\n\nstd::cout << Sigma < windowsSize/2 && pf.y > windowsSize/2 && pf.x < frame.size().width - windowsSize/2 && pf.y < frame.size().height - windowsSize/2) {\n\t\n\n \tint pos = mu.size();\n Patch newPat(cv::Mat(frame, cv::Rect(pf.x-windowsSize/2, pf.y-windowsSize/2, windowsSize,windowsSize)), pf, pos);\n patches.push_back(newPat);\n\n\t\t\n Vector2f hd;\n hd << (float)pf.x, (float)pf.y;\n \n Vector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n \n MatrixXf Jac_hCHd;\n Vector3f hC = cam.UndistortAndDeproject(hd, Jac_hCHd);\n \n Matrix3f Rot = quat2rot(q);\n\n Vector3f hW = Rot*hC;\n\n float hx = hW(0);\n float hy = hW(1);\n float hz = hW(2);\n\n float ro = config.rho_0;\n\n\n float theta = atan2(hx,hz);\n float phi = atan2(-hy, sqrt(hx*hx+hz*hz));\t\t\n\n\t\t// Updating state and Sigma\n\n\t\tVectorXf f(6);\n\t\tf << r, theta, phi, ro;\n\t\tmu = Concat(mu,f);\n\n\n int nOld = Sigma.rows();\n MatrixXf Js = MatrixXf::Zero(nOld+6, nOld+3);\n Js.block(0,0,nOld,nOld) = MatrixXf::Identity(nOld,nOld);\n Js.block<3,3>(nOld,0) = MatrixXf::Identity(3,3);\n\t\t\n\t\t\n\n\n MatrixXf Jf_hW = d_f_d_hW(hW);\n MatrixXf J_hW_q = dRq_times_d_dq(q,hC);\n\n Js.block<6,4>(nOld,3) = Jf_hW*J_hW_q;\n Js.block<6,2>(nOld,nOld) = Jf_hW*Rot*Jac_hCHd;\n Js.block<6,1>(nOld, nOld+2) << 0,0,0,0,0,1;\n\n MatrixXf S = sigma_pixel_2*MatrixXf::Identity(nOld+3, nOld+3);\n S.block(0,0, nOld, nOld) = Sigma;\n\n S(nOld + 2, nOld + 2) = config.sigma_rho_0;\n\n\t\tSigma = Js*S*Js.transpose();\n\t\tnFeatures++;\n return 1;\n }\n return 0;\n}\n\nvoid VSlamFilter::removeFeature(int index) {\n int pos = patches[index].position_in_state;\n \n int first = pos;\n \n int size = (patches[index].isXYZ()?3:6);\n\n int last = mu.rows() - pos - size;\n\n if (index != nFeatures - 1) {\n mu = Concat(mu.head(first), mu.tail(last));\n Sigma = vConcat(Sigma.topRows(first), Sigma.bottomRows(last));\n Sigma = hConcat(Sigma.leftCols(first), Sigma.rightCols(last));\n } else {\n mu = mu.head(pos);\n Sigma = Sigma.topLeftCorner(pos,pos);\n }\n\n for(int i = index + 1; i < patches.size(); i++) {\n \tpatches[i].change_position(-size);\n }\n\n patches.erase(patches.begin()+index);\n nFeatures--;\n}\n\n\nvoid VSlamFilter::predict(float v_x, float w_z) {\n\tFt = dg_x_dx(mu.segment<13>(0));\n const int n = Sigma.cols();\n \n MatrixXf Ft_complete = MatrixXf::Identity(n,n);\n Ft_complete.block<13,13>(0,0) = Ft;\n MatrixXf Q;\n Q = Ft.middleCols<6>(7)*Vmax*Ft.middleCols<6>(7).transpose();\n\n\n MatrixXf Qtot = MatrixXf::Zero(n,n);\n Qtot.block<13,13>(0,0) = Q;\n Sigma = (Ft_complete*Sigma*Ft_complete.transpose() + Qtot).eval();\n\n mu.segment<13>(0) = fv(mu.segment<13>(0), v_x, w_z);\n Vector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n Vector3f v = mu.segment<3>(7);\n Vector3f w = mu.segment<3>(10);\n Vector4f h = vec2quat(w);\n\n // state features prediction\n \n Vector4f qc = quatComplement(q); // quaternion Complement\n Matrix3f RotCW = quat2rot(qc);\n\n // blurring components\n\n\n Matrix3f RotCW_blurring = quat2rot(quatComplement(quatCrossProduct(q,vec2quat(w*T_camera))));\n Vector3f r_blurring = r + v*T_camera;\n Vector2f hi_out_blurred;\n\n\n VectorXf hi_out;\n MatrixXf Hit;\n \n int featurer_counter = 0;\n \n for (int i = 0; i < patches.size(); i++) {\n\t\tint pos = patches[i].position_in_state;\n if (!patches[i].isXYZ()) { // Feature in Inverse Depth Form\n Hit = MatrixXf::Zero(2, mu.rows());\n VectorXf f = mu.segment<6>(pos);\n\n if (f(5) <= 0) {\n \tROS_ERROR(\"Feature %d ad infinity: %f\", i, f(5));\n \tpatches[i].setRemove();\n \tpatches[i].setIsInInnovation(false);\n \tcontinue;\n \t/*f(5) = 0;\n \tmu(pos+5) = 0;\n \tMatrixXf J = MatrixXf::Identity(Sigma.cols(),Sigma.cols());\n \tJ(pos+5,pos+5) = 0;\n \tSigma = J*Sigma*J.transpose();\n \tSigma(pos+5,pos+5) = config.sigma_rho_0;*/\n }\n MatrixXf J_hW_f;\n Vector3f d = inverse2XYZ(f, r, J_hW_f);\n Vector3f hC = RotCW*d;\n MatrixXf J_h_hC;\n hi_out = cam.projectAndDistort(hC, J_h_hC);\n bool flag = isInsideImage(hi_out, frame.size(), windowsSize) && hC(2) >= 0 && f(5) > 0;\n\n\t\t\tpatches[i].setIsInInnovation(flag);\n if (!flag) continue;\n\t\t\t\n\t\t\tfeaturer_counter++;\n MatrixXf d_h_q = dRq_times_d_dq(qc,d)*d_qbar_q();\n \n Hit.middleCols<3>(0) = -f(5)*J_h_hC*RotCW;\n Hit.middleCols<4>(3) = J_h_hC*d_h_q;\n Hit.middleCols<6>(pos) = J_h_hC*RotCW*J_hW_f;\n patches[i].h = hi_out;\n\t\t\tpatches[i].H = Hit;\n\n\n\t\t\t// Blurring\n\t\t\thi_out_blurred = cam.projectAndDistort(RotCW_blurring*inverse2XYZ(f,r_blurring, J_hW_f), J_h_hC);\n\t\t\tpatches[i].blur(hi_out, hi_out_blurred,kernel_min_size );\n\n\t\t\t/*\n\t\t\tif ((hi_out - hi_out_blurred).norm() > kernel_min_size && (hi_out - hi_out_blurred).norm() < 10*kernel_min_size) {\n\t\t\t\tstd::cout << \"Blurred patch estimated\" << std::endl;\n\t\t\t\tstd::cout << \"Blurred patch predicted\" << std::endl;\n\t\t\t}*/\n\n } else { // Feature in Inverse Depth Form\n Hit = MatrixXf::Zero(2, mu.rows());\n Vector3f y = mu.segment<3>(pos);\n\n Vector3f d = y-r;\n Vector3f hC = RotCW*d;\n MatrixXf J_h_hC;\n hi_out = cam.projectAndDistort(hC, J_h_hC);\n bool flag = isInsideImage(hi_out, frame.size(), windowsSize) && hC(2) >= 0;\n\n\t\t\tpatches[i].setIsInInnovation(flag);\n if (!flag) continue;\n\n\t\t\tfeaturer_counter++;\n MatrixXf d_h_q = dRq_times_d_dq(qc,d)*d_qbar_q();\n\n Hit.middleCols<3>(0) = -J_h_hC*RotCW;\n Hit.middleCols<4>(3) = J_h_hC*d_h_q;\n Hit.middleCols<3>(pos) = J_h_hC*RotCW;\n patches[i].h = hi_out;\n\t\t\tpatches[i].H = Hit;\n\n\n\t\t\t// Blurring\n\t\t\thi_out_blurred = cam.projectAndDistort(RotCW_blurring*(y-r_blurring), J_h_hC);\n\t\t\tpatches[i].blur(hi_out, hi_out_blurred,kernel_min_size );\n\n\t\t\t/*\n\t\t\tif ((hi_out - hi_out_blurred).norm() > kernel_min_size && (hi_out - hi_out_blurred).norm() < 10*kernel_min_size) {\n\t\t\t\tstd::cout << \"Blurred patch estimated\" << std::endl;\n\t\t\t\tstd::cout << \"Blurred patch predicted\" << std::endl;\n\t\t\t}*/\n }\n }\n\n VectorXf temp_h_out;\n MatrixXf temp_Ht;\n\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInInnovation()) {\n\t\t\ttemp_h_out = Concat(temp_h_out,patches[i].h);\n\t\t\ttemp_Ht = vConcat(temp_Ht,patches[i].H);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t} \n\t\n h_out = temp_h_out;\n Ht = temp_Ht;\n \n\n if (h_out.rows() > 0) {\n St = (Ht*Sigma*Ht.transpose() + sigma_pixel_2*MatrixXf::Identity(Ht.rows(), Ht.rows())).eval();\n this->drawPrediction();\n }\n/*\n for (int i = 0; i < patches.size(); i++) {\n \tMatrix2f Sii = St.block<2,2>(2*i,2*i);\n \tif (Sii.determinant() > 250000) {\n \t\tpatches[i].setRemove();\n \t}\n }\n */\n}\n\n\nVector3f VSlamFilter::depth2XYZ(VectorXf f, MatrixXf &J) {\n\n\tconst float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n\n Vector3f y = f.segment<3>(0) + m/ro;\n\n\n\n J = MatrixXf::Zero(3,6);\n J.middleCols<3>(0) = Matrix3f::Identity();\n\n J(0,3) = cos(theta)*cos(phi)/ro;\n J(1,3) = 0;\n J(2,3) = -sin(theta)*cos(phi)/ro;\n\n J(0,4) = -sin(theta)*sin(phi)/ro;\n J(1,4) = -cos(phi)/ro;\n J(2,4) = -cos(theta)*sin(phi)/ro;\n\n J.col(5) = -m/(ro*ro);\n return y;\n\n}\n\n\nvoid VSlamFilter::convert2XYZ(int index) {\n\tint pos = patches[index].position_in_state;\n\tint size = 6;\n\tint dx = 3;\n\tVectorXf f = mu.segment<6>(pos);\n\tconst float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n\n Vector3f y = f.segment<3>(0) + m/ro;\n\n Vector3f d = y - mu.segment<3>(0);\n\n // controllo sul L_i\n\n float sigma = Sigma(pos+5,pos+5);\n\n float t = d.transpose()*m;\n\n float L = 4*sigma*abs(t)/(ro*ro*d.squaredNorm());\n if (L <= 0) {\n \tpatches[index].setRemove();\n \tstd::cout << \"L = \" << L << \" sigma = \" << sigma << std::cout;\n }\n if (L < 0.01) {\n\n\n MatrixXf J_hp_f = MatrixXf::Zero(3,6);\n J_hp_f.middleCols<3>(0) = Matrix3f::Identity();\n\n J_hp_f(0,3) = cos(theta)*cos(phi)/ro;\n J_hp_f(1,3) = 0;\n J_hp_f(2,3) = -sin(theta)*cos(phi)/ro;\n\n J_hp_f(0,4) = -sin(theta)*sin(phi)/ro;\n J_hp_f(1,4) = -cos(phi)/ro;\n J_hp_f(2,4) = -cos(theta)*sin(phi)/ro;\n\n J_hp_f.col(5) = -m/(ro*ro);\n\n\n int first = pos;\n int last = mu.rows() - pos - 3;\n\n\n if (index != patches.size() - 1) {\n mu = Concat(mu.head(first), mu.tail(last));\n mu.segment<3>(pos) = y;\n } else {\n mu = Concat(mu.head(first), y);\n }\n\n\n\n int n = Sigma.cols();\n MatrixXf J = MatrixXf::Zero(n-3,n);\n J.topLeftCorner(pos,pos) = MatrixXf::Identity(pos,pos);\n J.block<3,6>(pos,pos) = J_hp_f;\n if (index != patches.size() - 1) {\n \tJ.bottomRightCorner(n-pos-6,n-pos-6) = MatrixXf::Identity(n-pos-6,n-pos-6);\n }\n\n\n Sigma = J*Sigma*J.transpose();\n\n std::cout << \"Sigma is positive ? \" << Sigma.ldlt().isPositive() << std::endl;\n\n patches[index].convertInXYZ();\n\n for(int i = index + 1; i < patches.size(); i++) {\n \tpatches[i].change_position(-3);\n }\n }\n}\n\n\nvoid VSlamFilter::findFeaturesConvertible2XYZ() {\n\tfor (int i = 0; i < patches.size(); i++) {\n\t\tif (!patches[i].isXYZ()) convert2XYZ(i);\n\t}\n\n}\n\n\n\nvoid VSlamFilter::findNewFeatures(int num) {\n\tROS_INFO(\"Finding new Feature\");\n\n int winSize = 2*windowsSize+1;\n cv::Mat mask = cv::Mat(frame.size(),CV_8UC1 );\n mask.setTo(cv::Scalar(0));\n\tint estrem = windowsSize;\n cv::Mat(mask, cv::Rect(estrem, estrem,mask.size().width-2*estrem, mask.size().height - 2*estrem)).setTo(cv::Scalar(255));\n\n\n\n for (int i = 0; i < patches.size(); i++) {\n \tif (patches[i].center.x > windowsSize && \n \t\tpatches[i].center.y > windowsSize && \n \t\tpatches[i].center.x < mask.size().width - windowsSize && \n \t\tpatches[i].center.y < mask.size().height - windowsSize) \n \t\t{\n \t\tint x = (patches[i].center.x-winSize/2 > 0 ? \n \t\t\t\tpatches[i].center.x-winSize/2 : 0);\n \t\tint y = (patches[i].center.y-winSize/2 > 0 ? \n \t\t\t\tpatches[i].center.y-winSize/2 : 0);\n \t\tint width = winSize - \n \t\t\t(patches[i].center.x+winSize/2 <= frame.size().width ? \n \t\t\t0 : frame.size().width - patches[i].center.x-winSize/2);\n \t\t\n\t\t\tint height = winSize - \n \t\t\t(patches[i].center.y+winSize/2 <= frame.size().height ? \n \t\t\t0 : frame.size().height - patches[i].center.y-winSize/2);\n \t\t\t\n \t\tcv::Rect roi = cv::Rect(x,y, width, height);\n \t\tcv::Mat(mask, roi).setTo(cv::Scalar(0));\n\n \t}\n }\n std::vector features;\n\t\n\t\n goodFeaturesToTrack(frame,features,num,0.2f,20,mask);\n\n for (int i = 0; i < features.size(); i++) {\n\tcv::Point2f newFeature = cv::Point2f(features[i].x+0.5, features[i].y+0.5);\n this->addFeature(newFeature);\n }\n \n\tROS_DEBUG(\"Found %d Feature[s]\", features.size());\n\n}\n\nvoid VSlamFilter::update() {\n\n cv::Point2f locF;\n int matchedFeatures = 0;\n int searchedFeatures = 0;\n\n\t\n for (int i = 0; i < nFeatures; i++) {\n if (patches[i].patchIsInInnovation()) {\n patches[i].findMatch(frame, St.block<2,2>(patches[i].position_in_z,patches[i].position_in_z),sigma_size, false);\n searchedFeatures++;\n }\n }\n\n#ifdef USE_RANSAC\n\n\tint nhyp = 1000;\n\tfloat p = 0.9;\n\tfloat th = 1;\n\t\n\tsrand(time(NULL));\n\t\n\tint num_zli = 0;\n\t\n\tstd::vector ransacindex;\n\n\n\tfor (int i = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInInnovation())\n\t\t\transacindex.push_back(i);\n\t}\n\n\n\tfor (int i = 0; i < nhyp && ransacindex.size() > 0; i++) {\n\t\tint actual_num_zli = 0;\n\n\t\tint posRansac = rand()%ransacindex.size();\n\t\tint selectPatch = ransacindex[posRansac];\n\n\t\transacindex.erase(ransacindex.begin() + posRansac);\n\n\t\tMatrixXf S_i = patches[selectPatch].H*Sigma*patches[selectPatch].H.transpose() + sigma_pixel_2*MatrixXf::Identity(patches[selectPatch].H.rows(), patches[selectPatch].H.rows());\n MatrixXf K_i = Sigma*patches[selectPatch].H.transpose()*S_i.inverse();\n\t\tVectorXf mu_i = mu + K_i*(patches[selectPatch].z - patches[selectPatch].h);\n\t\t\n\t\tVector3f r = mu_i.segment<3>(0);\n\t\tVector4f q = mu_i.segment<4>(3)/mu_i.segment<4>(3).norm();\n \tMatrix3f RotCW = quat2rot(quatComplement(q));\t\n\t\t\n \tint searched_features = 0;\n\t\tfor (int i = 0; i < nFeatures; i++) {\n\t\t\tif (!patches[i].patchIsInInnovation()) continue;\n\t\t\tVector2f hi_i;\n\t\t\tint pos = patches[i].position_in_state;\n\n\t\t\tif (!patches[i].isXYZ()) {\n\t\t\t\tVectorXf f = mu_i.segment<6>(pos);\n\t\t\t\tVector3f d = inverse2XYZ(f, r);\n\t\t\t\thi_i = cam.projectAndDistort(RotCW*d);\n\t\t\t} else {\n\t Vector3f y = mu.segment<3>(pos);\n\t Vector3f d = y-r;\n\t Vector3f hC = RotCW*d;\n\t MatrixXf J_hf_hC;\n\t hi_i = cam.projectAndDistort(hC, J_hf_hC);\n\t\t\t}\n\n\t\t\tpatches[i].setIsInLi((patches[i].z - hi_i).transpose()*St.block<2,2>(patches[i].position_in_z,patches[i].position_in_z).inverse()*(patches[i].z - hi_i) <= th);\n\t\t\tif (patches[i].patchIsInLi()) actual_num_zli++;\n\t\t\tsearched_features++;\n\n \t}\n \t\n \tif (actual_num_zli > num_zli) {\n \t\tnum_zli = actual_num_zli;\n \t\tnhyp = log(1-p)/(log(num_zli/(matchedFeatures+0.0f)));\n \t\tfor (int i = 0; i < patches.size(); i++) {\n \t\t\tif (patches[i].patchIsInInnovation()) patches[i].ConfirmIsInLi();\n \t\t}\n \t}\n\t}\n\t\n\n\tVectorXf z_li;\n MatrixXf H_li;\n VectorXf h_li;\n\tfor (int i = 0, j = 0; i < nFeatures; i++) {\n\t\tif (patches[i].patchIsInLi()) {\n\t\t\th_li = Concat(h_li,patches[i].h);\n\t\t\tH_li = vConcat(H_li,patches[i].H);\n\t\t\tz_li = Concat(z_li, patches[i].z);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t} \n\n\tVectorXf mu_li;\n\tMatrixXf Sigma_li;\n if (z_li.rows() > 0) {\n \tSt = H_li*Sigma*H_li.transpose() + sigma_pixel_2*MatrixXf::Identity(H_li.rows(), H_li.rows());\n \tKt = Sigma*H_li.transpose()*St.inverse();\n\n \t//\tMatrixXf Stinv = St.lu().solve(Matrix::Identity(p,p));\n \t//\tKt = Sigma*H_li.transpose()*Stinv;\n\n \tmu_li = mu + Kt*(z_li - h_li);\n Sigma_li = ((MatrixXf::Identity(Sigma.rows(),Sigma.rows()) - Kt*H_li)*Sigma);\n \tnormalizeQuaternion(mu_li,Sigma_li);\n } else {\n ROS_ERROR(\"No Matching li\");\n }\n\n \n //////////////////////////////////////////////////////////\n\tth = 0.5;\n if (mu_li.rows() > 0) {\n\t\tVector3f r = mu_li.segment<3>(0);\n\t\tVector4f q = mu_li.segment<4>(3);\n\t\tVector4f qc = quatComplement(q);\n\t\tMatrix3f RotCW = quat2rot(qc);\n\t\tMatrix2f S_hi;\n\n\t\tint counter = 0;\n\t\tfor (int i = 0; i < patches.size(); i++) {\n\t\t\tif (patches[i].patchIsInLi()) {\n\t\t\t\tint pos = patches[i].position_in_state;\n\t\t\t\tVector2f hi_hi;\n\t\t\t\tif (!patches[i].isXYZ()) {\n\t\t\t\t\tMatrixXf Hi_hi = MatrixXf::Zero(2, mu_li.rows());\n\t\t\t\t\tVectorXf f = mu_li.segment<6>(pos);\n\t\t\t\t\tMatrixXf J_hp_f;\n\t\t\t\t\tVector3f d = inverse2XYZ(f, r, J_hp_f);\n\t\t\t\t\tMatrixXf J_hf_hi;\n\t\t\t\t\thi_hi = cam.projectAndDistort(RotCW*d, J_hf_hi);\n\t\t\t\t} else {\n\t\t Vector3f y = mu.segment<3>(pos);\n\t\t Vector3f d = y-r;\n\t\t Vector3f hC = RotCW*d;\n\t\t MatrixXf J_hf_hC;\n\t\t hi_hi = cam.projectAndDistort(hC, J_hf_hC);\n\t\t\t\t}\n\n\t\t\t\tS_hi = St.block<2,2>(patches[i].position_in_z ,\tpatches[i].position_in_z);\n\n\t\t\t\tpatches[i].setIsInHi((hi_hi - patches[i].z).transpose()*S_hi.inverse()*(hi_hi - patches[i].z) <= th);\n\n\t\t\t\tcounter++;\n\n\t\t\t}\n\t\t}\n }\n\n \n \n VectorXf z_hi;\n MatrixXf H_hi;\n VectorXf h_hi;\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInHi()) {\n\t\t\th_hi = Concat(h_hi, patches[i].h);\n\t\t\tH_hi = vConcat(H_hi, patches[i].H);\n\t\t\tz_hi = Concat(z_hi, patches[i].z);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t} \n\n#endif\n\n\n\n#ifndef USE_RANSAC\n VectorXf z_hi;\n MatrixXf H_hi;\n VectorXf h_hi;\n\tfor (int i = 0, j = 0; i < patches.size(); i++) {\n\t\tif (patches[i].patchIsInInnovation()) {\n\t\t\th_hi = Concat(h_hi, patches[i].h);\n\t\t\tH_hi = vConcat(H_hi, patches[i].H);\n\t\t\tz_hi = Concat(z_hi, patches[i].z);\n\t\t\tpatches[i].position_in_z = 2*j;\n\t\t\tj++;\n\t\t}\n\t}\n\n#endif\n\n if (z_hi.rows() > 0) {\n \tSt = H_hi*Sigma*H_hi.transpose() + sigma_pixel_2*MatrixXf::Identity(H_hi.rows(), H_hi.rows());\n \t// int p = St.cols();\n \t// MatrixXf Stinv;\n \t//\tStinv = St.lu().solve(Matrix::Identity(p,p));\n \t// \tKt = Sigma*H_hi.transpose()*Stinv;\n\n \tKt = Sigma*H_hi.transpose()*St.inverse();\n\n mu = mu + Kt*(z_hi - h_hi);\n Sigma = ((MatrixXf::Identity(Sigma.rows(),Sigma.rows()) - Kt*H_hi)*Sigma).eval();\n\n \tnormalizeQuaternion(mu,Sigma);\n } else {\n ROS_ERROR(\"No Matching hi\");\n }\n \n \n for (int i = 0; i < patches.size(); i++) {\n \tpatches[i].drawUpdate(drawedImg, i);\n }\n\n int nVisibleFeature = 0;\n for (int i = 0; i < nFeatures; i++) {\n if (patches[i].patchIsInInnovation()) nVisibleFeature++;\n }\n \n if (patches.size() < 5) this->findNewFeatures(1);\n if (nVisibleFeature < 5) this->findNewFeatures(1);\n\n for (int i = patches.size()-1; i >= 0; --i) {\n \tif (patches[i].mustBeRemove()) this->removeFeature(i);\n }\n\n /*\n\n Vector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n Vector3f v = mu.segment<3>(7);\n Vector3f w = mu.segment<3>(10);\n\n Matrix3f RotCW = quat2rot(quatComplement(q));\n Matrix3f RotCW_blurring = quat2rot(quatComplement(quatCrossProduct(q,vec2quat(w*T_camera))));\n Vector3f r_blurring = r + T_camera*v;\n Vector2f hi_out_blurred;\n\n for (int i = 0; i < patches.size(); i++) {\n if (patches[i].patchIsInHi()) { // Feature in Inverse Depth Form\n int pos = 13 + i*6;\n VectorXf f = mu.segment<6>(pos);\n MatrixXf J_hp_f;\n MatrixXf J_hf_hi;\n\n // Compute blurred patches\n\n\n Vector2f hi_out = cam.projectAndDistort(RotCW*inverse2XYZ(f,r, J_hp_f), J_hf_hi);\n hi_out_blurred = cam.projectAndDistort(RotCW_blurring*inverse2XYZ(f,r_blurring, J_hp_f), J_hf_hi);\n if ((hi_out - hi_out_blurred).norm() > kernel_min_size && (hi_out - hi_out_blurred).norm() < 10*kernel_min_size) {\n std::cout << \"Deblurred patch found\" << std::endl;\n patches[i].deblur(hi_out, hi_out_blurred);\n std::cout << \"Deblurred patch updated\" << std::endl;\n\n }\n\n }\n\n }\n \n */\n\n // feature_index = 0;\n\n\n\n for (int i = 0; i < patches.size(); i++) {\n \tpatches[i].update_quality_index();\n //\tfeature_index+=patches[i].get_quality_index();\n }\n \n\tfeature_index = feature_index/patches.size();\n\t\n\tfor (int i = patches.size()-1; i >= 0; --i){\n\t\tif (patches[i].mustBeRemove()) removeFeature(i);\n\t}\n\n\tfindFeaturesConvertible2XYZ();\n\n std::cout << \"Sigma is positive ? \" << Sigma.ldlt().isPositive() << std::endl;\n\n}\n\n\nvoid VSlamFilter::drawUpdate(cv::Point f) {\n cv::rectangle(drawedImg, cv::Point(f.x - windowsSize/2, f.y - windowsSize/2), cv::Point(f.x + windowsSize/2, f.y + windowsSize/2), CV_RGB(0,0,255), 1);\n cv::circle(drawedImg, f, 2, CV_RGB(0,0,255),2);\n \n\tstd::stringstream text;\n\ttext << 1;\n\tcv::putText(drawedImg,text.str(), f,cv::FONT_HERSHEY_SIMPLEX, 2, CV_RGB(0,0,255));\n\n}\n\n\nvoid VSlamFilter::drawPrediction() {\n \n MatrixXi sigma_mis = computeEllipsoidParameters(St, sigma_size);\n for (int i = 0; i < St.rows()/2; i++) {\n cv::Point2i predictFeature(h_out(2*i), h_out(2*i+1));\n cv::ellipse(drawedImg, predictFeature, cv::Size(sigma_mis(i,0),sigma_mis(i,1)), (double)sigma_mis(i,2), 0, 360, CV_RGB(32, 32, 255));\n }\n}\n\ncv::Mat VSlamFilter::returnImageDrawed() {\n return drawedImg.clone();\n}\nMatrixXi computeEllipsoidParameters(MatrixXf St, int sigma_size) {\n int nFeatures = St.rows()/2;\n MatrixXi sigma_mis(nFeatures,3);\n for (int i = 0; i < nFeatures; i++) {\n SelfAdjointEigenSolver eigenSolver(St.block<2,2>(2*i,2*i));\n Vector2f eigs = eigenSolver.eigenvalues();\n Matrix2f vecs = eigenSolver.eigenvectors();\n \n sigma_mis(i,0) = (eigs(0) > 0 ? (int)sigma_size*sqrt((double)eigs(0)) : 1);\n sigma_mis(i,1) = (eigs(1) > 0 ? (int)sigma_size*sqrt((double)eigs(1)) : 1);\n sigma_mis(i,2) = (int)(180/3.14*atan2((double)vecs(1,0), (double)vecs(0,0)));\n }\n return sigma_mis;\n}\n\n\n\n\n/*************************/\n\nVector4f vec2quat(Vector3f vec) {\n float alpha = vec.norm();\n \n if (alpha != 0) {\n Vector4f quat;\n quat(0) = cos(alpha/2);\n quat.segment<3>(1) = vec*sin(alpha/2)/alpha;\n return quat;\n }\n else {\n return quatZero();\n }\n}\n\nVector4f quatZero() {\n Vector4f quat;\n quat << 1,0,0,0;\n return quat;\n}\n\nMatrix3f quat2rot(Vector4f quat) {\n float qr = quat(0);\n float qi = quat(1);\n float qj = quat(2);\n float qk = quat(3);\n \n const float w = quat(0);\n const float x = quat(1);\n const float y = quat(2);\n const float z = quat(3);\n\n Matrix3f rot;\n\n\trot <<\t qr*qr + qi*qi - qj*qj - qk*qk, \t\t-2*qr*qk+2*qi*qj,\t\t\t\t\t 2*qr*qj+2*qi*qk,\n\t\t 2*qr*qk+2*qi*qj,\t\t\t\t qr*qr - qi*qi + qj*qj - qk*qk,\t\t\t\t-2*qr*qi+2*qj*qk,\n\t\t-2*qr*qj+2*qi*qk,\t\t\t\t 2*qr*qi+2*qj*qk,\t\t\t\t\t qr*qr - qi*qi - qj*qj + qk*qk;\n /*\n\trot <<\t qr*qr + qi*qi - qj*qj - qk*qk, \t\t-2*qr*qk+2*qi*qj,\t\t\t\t\t 2*qr*qj+2*qi*qk,\n\t\t 2*qr*qk+2*qi*qj,\t\t\t\t qr*qr - qi*qi + qj*qj - qk*qk,\t\t\t\t-2*qr*qi+2*qj*qk,\n\t\t-2*qr*qj+2*qi*qk,\t\t\t\t 2*qr*qi+2*qj*qk,\t\t\t\t\t qr*qr - qi*qi - qj*qj + qk*qk;\n\n\n\tconst float norm = quat.segment<3>(1).norm();\n\tVector3f v = quat.segment<3>(1);\n\tMatrix3f skew;\n\tskew << 0, -z, y, z, 0, -x, -y, x, 0;\n\trot = Matrix3f::Identity()*(1 - 2*norm*norm) + 2*(v*v.transpose() + w*skew);\n*/\n return rot;\n}\n\nMatrix4f YupsilonMatric(Vector4f q1) {\n\tMatrix4f res;\n \tconst float r1=q1(0);\n \tconst float x1=q1(1);\n \tconst float y1=q1(2);\n \tconst float z1=q1(3);\n\tres << r1, -x1, -y1, -z1,\n\t\tx1, r1, -z1, y1,\n\t\ty1, z1, r1, -x1,\n\t\tz1, -y1, x1, r1;\n\t\n\treturn res;\n\t\t\n}\n\n\nMatrix4f YupsilonMatricComplementar(Vector4f q1) {\n\tMatrix4f res;\n \tconst float r1=q1(0);\n \tconst float x1=q1(1);\n \tconst float y1=q1(2);\n \tconst float z1=q1(3);\n\tres << r1, -x1, -y1, -z1,\n\t\tx1, r1, z1, -y1,\n\t\ty1, -z1, r1, x1,\n\t\tz1, y1, -x1, r1;\n\n\treturn res;\n\t\t\n}\n\nVector4f quatCrossProduct(Vector4f q1, Vector4f q2) {\n Vector4f q;\n // First quaternion q1 (x1 y1 z1 r1)\n const float r1=q1(0);\n const float x1=q1(1);\n const float y1=q1(2);\n const float z1=q1(3);\n \n \n // Second quaternion q2 (x2 y2 z2 r2)\n const float r2=q2(0);\n const float x2=q2(1);\n const float y2=q2(2);\n const float z2=q2(3);\n \n q(0) = r1*r2 - x1*x2 - y1*y2 - z1*z2; // r component\n q(1) = x1*r2 + r1*x2 + y1*z2 - z1*y2; // x component\n q(2) = r1*y2 - x1*z2 + y1*r2 + z1*x2; // y component\n q(3) = r1*z2 + x1*y2 - y1*x2 + z1*r2; // z component\n\n\t\n \n return YupsilonMatricComplementar(q2)*q1;\n}\n\n\n\nVector3f inverse2XYZ(VectorXf f, Vector3f r, MatrixXf &J_hp_f, Matrix3f R) {\n const float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n \n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n \n J_hp_f = MatrixXf(3,6);\n J_hp_f.middleCols<3>(0) = ro*Matrix3f::Identity();\n\n J_hp_f(0,3) = cos(theta)*cos(phi);\n J_hp_f(1,3) = 0;\n J_hp_f(2,3) = -sin(theta)*cos(phi);\n \n J_hp_f(0,4) = -sin(theta)*sin(phi);\n J_hp_f(1,4) = -cos(phi);\n J_hp_f(2,4) = -cos(theta)*sin(phi);\n \n J_hp_f.col(5) = f.segment<3>(0)-r;\n \n \n //J_hp_f= R*J_hp_f;\n\n \n MatrixXf ret;\n //ret = R*(ro*(f.segment<3>(0)-r) + m);\n ret = (ro*(f.segment<3>(0)-r) + m);\n return ret;\n}\n\nVector3f inverse2XYZ(VectorXf f, Vector3f r) {\n const float theta = f(3);\n const float phi = f(4);\n const float ro = f(5);\n \n Vector3f m;\n m(0) = sin(theta)*cos(phi);\n m(1) = -sin(phi);\n m(2) = cos(theta)*cos(phi);\n \n return ro*(f.segment<3>(0)-r) + m;\n}\n\n// return Jacobian d(g(x))/dx\nMatrixXf dg_x_dx(VectorXf mu) {\n\tVector3f r = mu.segment<3>(0);\n Vector4f q = mu.segment<4>(3);\n Vector3f v = mu.segment<3>(7);\n Vector3f w = mu.segment<3>(10);\n Vector4f h = vec2quat(w); // h = quat(w)\n\n\tMatrixXf Ft;\n Ft = MatrixXf::Identity(13,13);\n Ft.block<4,4>(3,3) = Jacobian_qt_qt1(h);\n Ft.block<4,3>(3,10) = Jacobian_qt_w(q,w);\n\tFt.block<3,3>(0,7) = MatrixXf::Identity(3,3);\n\n\treturn Ft;\n}\n\n\n\n// compute the Jacobian between q(t) and q(t-1) knowing the rotation h = quat(w*T)\nMatrix4f Jacobian_qt_qt1(Vector4f h) {\n const float hr=h(0);\n const float hx=h(1);\n const float hy=h(2);\n const float hz=h(3);\n \n Matrix4f ret;\n ret << hr, -hx, -hy, -hz,\n hx, hr, hz, -hy,\n hy, -hz, hr, hx,\n hz, hy, -hx, hr;\n\n return YupsilonMatricComplementar(h); \n}\n\n// compute the Jacobian between q(t) and w(t-1) knowing the actual rotation q, w and dT\nMatrixXf Jacobian_qt_w(Vector4f q, Vector3f w) {\n const float qr=q(0);\n const float qx=q(1);\n const float qy=q(2);\n const float qz=q(3);\n \n const float n = w.norm();\n const float s = sin(n/2);\n const float c = cos(n/2);\n const float Sinc = (n == 0 ? 1: 2*sin(n/2)/n);\n \n Vector3f n_w;\n if (n > 0) n_w = w/n;\n \n Matrix4f t1;\n\tt1 << \tqr, -qx, -qy, -qz,\n \tqx, qr, -qz, qy,\n \tqy, qz, qr, -qx,\n qz, -qy, qx, qr;\n\n\tt1 = YupsilonMatric(q);\n \n MatrixXf t2 = MatrixXf::Zero(4,3);\n t2.row(0) = -0.5*s*n_w.transpose();\n t2.middleRows<3>(1) = 0.5*(Sinc*Matrix3f::Identity() + (c-Sinc)*n_w*n_w.transpose());\n\n\tMatrixXf ret;\n\tret = t1*t2;\n return ret;\n}\n\nMatrix3f diffQuat2rot(Vector4f quat, int index) {\n Matrix3f dR;\n const float q0_2 = 2*quat(0);\n const float qx_2 = 2*quat(1);\n const float qy_2 = 2*quat(2);\n const float qz_2 = 2*quat(3);\n\n const float w = quat(0);\n const float x = quat(1);\n const float y = quat(2);\n const float z = quat(3);\n \n if (index == 0) {\n dR << \t q0_2, \t-qz_2,\t qy_2,\n \t\t qz_2,\t q0_2,\t-qx_2,\n \t\t-qy_2,\t qx_2,\t q0_2;\n\n \t\t/*dR << \t 0, \t-qz_2,\t qy_2,\n \t\t qz_2,\t 0,\t-qx_2,\n \t\t-qy_2,\t qx_2,\t 0;*/\n \n } else if (index == 1) {\n dR << \tqx_2, \t qy_2, \t qz_2,\n \t\tqy_2, \t-qx_2, \t-q0_2,\n \t\tqz_2,\t q0_2,\t-qx_2;\n\n\t\t/*dR << \t0, \t qy_2, \t qz_2,\n \t\tqy_2, \t-2*qx_2, -q0_2,\n \t\tqz_2,\t q0_2,\t-2*qx_2;*/\n \n } else if (index == 2) {\n dR <<\t-qy_2, qx_2, q0_2,\n \t\t qx_2, qy_2, qz_2,\n \t\t-q0_2, qz_2, -qy_2;\n\n \t\t/*dR <<\t-2*qy_2, qx_2, q0_2,\n \t\t qx_2, 0, \tqz_2,\n \t\t-q0_2, qz_2, -2*qy_2;*/\n \n } else if (index == 3) {\n dR <<\t-qz_2,\t\t-q0_2, \tqx_2,\n \t\t q0_2, -qz_2, qy_2,\n \t\t qx_2, qy_2, qz_2;\n\n /*dR <<\t-2*qz_2,\t\t-q0_2, \tqx_2,\n \t\t q0_2, -2*qz_2, qy_2,\n \t\t qx_2, qy_2, 0;*/\n }\n return dR;\n}\n\nVector4f quatComplement(Vector4f quat) {\n Vector4f qC;\n qC << quat(0), -quat(1), -quat(2), -quat(3);\n return qC;\n}\n\n// state camera Update\nVectorXf fv(VectorXf Xv, float v_x, float w_z) {\n\n\n Xv.segment<3>(0) += Xv.segment<3>(7);\n Xv.segment<4>(3) = quatCrossProduct( Xv.segment<4>(3),vec2quat(Xv.segment<3>(10)));\n\n return Xv;\n}\n\nMatrix4f d_qbar_q() {\n Matrix4f J = -Matrix4f::Identity();\n J(0,0) = 1;\n return J;\n}\n\n\n// Compute the Jacobian df/dhW\nMatrixXf d_f_d_hW(Vector3f hW) {\n const float hx = hW(0);\n const float hy = hW(1);\n const float hz = hW(2);\n \n // d_Theta_hW\n MatrixXf J_Theta_hW(1,3);\n float normal = hx*hx+hz*hz;\n J_Theta_hW(0,0) = hz/normal;\n J_Theta_hW(0,1) = 0;\n J_Theta_hW(0,2) = -hx/normal;\n\n \n // d_Phi_hW\n float normal2 = hx*hx+hy*hy+hz*hz;\n MatrixXf J_Phi_hW(1,3);\n J_Phi_hW(0,0) = hx*hy/sqrt(normal)/normal2;\n J_Phi_hW(0,1) = -sqrt(normal)/normal2;\n J_Phi_hW(0,2) = hz*hy/sqrt(normal)/normal2;\n \n \n MatrixXf J_f_hW = MatrixXf::Zero(6,3);\n J_f_hW.row(3) = J_Theta_hW;\n J_f_hW.row(4) = J_Phi_hW;\n\n return J_f_hW;\n}\n\n\nvoid normalizeQuaternion(VectorXf &mu, MatrixXf &Sigma) {\n Vector4f q = mu.segment<4>(3);\n \n const float norma = q.norm();\n mu.segment<4>(3) = q/norma;\n\n MatrixXf Q;\n Q = norma*norma*Matrix4f::Identity() - q*q.transpose();\n \n Q = Q*(1/(norma*norma*norma));\n\n MatrixXf Qc = MatrixXf::Identity(Sigma.rows(), Sigma.cols());\n Qc.block<4,4>(3,3) = Q;\n\n\tSigma = (Qc*Sigma*Qc.transpose()).eval();\n\n}\n\n\n\nbool isInsideImage(Vector2f hi, cv::Size size, int windowsSize) {\n float i = hi(0);\n float j = hi(1);\n \n if (i > windowsSize/2 && j > windowsSize/2 && i < size.width - windowsSize/2 && j < size.height - windowsSize/2) {\n return true;\n }\n return false;\n\n}\n\nMatrixXf dRq_times_d_dq(Vector4f q, Vector3f d) {\n MatrixXf diff_Rq_times_dq(3,4);\n for (int j = 0; j < 4; j++) {\n \tdiff_Rq_times_dq.col(j) = diffQuat2rot(q,j)*d;\n }\n return diff_Rq_times_dq;\n}\n\n" } ]