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

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

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