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
================================================
<?xml version="1.0" encoding="UTF-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author>VCGLab</author>
<authoring_tool>VCGLib | MeshLab</authoring_tool>
</contributor>
<up_axis>Y_UP</up_axis>
<created>Thu Sep 12 09:33:03 2013</created>
<modified>Thu Sep 12 09:33:03 2013</modified>
</asset>
<library_images/>
<library_materials/>
<library_effects/>
<library_geometries>
<geometry id="shape0-lib" name="shape0">
<mesh>
<source id="shape0-lib-positions" name="position">
<float_array id="shape0-lib-positions-array" count="459">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</float_array>
<technique_common>
<accessor count="153" source="#shape0-lib-positions-array" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="shape0-lib-normals" name="normal">
<float_array id="shape0-lib-normals-array" count="228">-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</float_array>
<technique_common>
<accessor count="76" source="#shape0-lib-normals-array" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="shape0-lib-vertices">
<input semantic="POSITION" source="#shape0-lib-positions"/>
</vertices>
<triangles count="76">
<input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
<input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
<p>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</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="VisualSceneNode" name="VisualScene">
<node id="node" name="node">
<instance_geometry url="#shape0-lib">
<bind_material>
<technique_common/>
</bind_material>
</instance_geometry>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#VisualSceneNode"/>
</scene>
</COLLADA>
================================================
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
================================================
<?xml version="1.0"?>
<package>
<name>mono-slam</name>
<version>0.0.1</version>
<description>Mono-SLAM implementation for ROS</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="ludus.russo@gmal.com">Ludovico O. Russo</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSDv2.0</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://ros.org/wiki/mono_slam</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>
cv_bridge
OpenCV
sensor_msgs
opencv2
cv_bridge
roscpp
std_msgs
image_transport
</build_depend>
<run_depend>
cv_bridge
OpenCV
sensor_msgs
opencv2
cv_bridge
roscpp
std_msgs
image_transport
</run_depend>
<export>
</export>
</package>
================================================
FILE: src/ConfigVSLAM.cpp
================================================
/*
* ConfigVSLAM.cpp
*
* Created on: Jul 10, 2013
* Author: ludovico
*/
#include "ConfigVSLAM.h"
#include <libconfig.h++>
#include <iostream>
#include <fstream>
#include <iomanip>
using namespace libconfig;
using namespace std;
template<typename T>
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 <cstddef>
#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<float, 6, 13> FeatureModel::computeJx() {
// TODO
Matrix<float, 6, 13> Jx = Matrix<float, 6, 13>::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<float, 6, 3> FeatureModel::computeJm() {
// TODO
Matrix<float, 6, 3> Jm = Matrix<float, 6, 6>::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 <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>
#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<float, 6, 13> computeJx();
Matrix<float, 6, 6> 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<MatrixXf> 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 <eigen3/Eigen/Dense>
#include "FeatureModel.h"
using namespace Eigen;
using namespace std;
class FeaturesState: public vector<FeatureModel>{
public:
FeaturesState();
virtual ~FeaturesState();
void updateVariaceBlocks(MatrixXf Jx);
};
#endif /* FEATURESSTATE_H_ */
================================================
FILE: src/Patch.cpp
================================================
#include "Patch.hpp"
#include <stdlib.h>
#include <stdio.h>
#include "libblur.h"
#include <fstream>
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<uchar>(i,j);
m2 += f2.at<uchar>(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<uchar>(i,j);
n1 += (s1-m1)*(s1-m1);
s2 = f2.at<uchar>(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 <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
#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 <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
using namespace Eigen;
class Patch: public std::vector<Patch> {
protected:
public:
};
#endif
================================================
FILE: src/RosVSLAMRansac.cpp
================================================
#include "RosVSLAMRansac.hpp"
#include <sstream>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
// #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<MatrixXf> 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<MatrixXf> 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<MatrixXf> eigenSolver(Cov);
Vector3f eigs = eigenSolver.eigenvalues();
Matrix3f vecs = eigenSolver.eigenvectors();
Quaternion<float> 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 = " <<deleted_patches.size() << std::endl;
if (deleted_patches.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 <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <nav_msgs/Path.h>
#include <eigen3/Eigen/Dense>
using namespace Eigen;
class RosVSLAM : public VSlamFilter
{
private:
nav_msgs::Path cameraPath;
void updatePath();
std::vector<Quaternionf> vQuat;
std::vector<Matrix3f> 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 <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
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<float>(0)),
fy(parameters.at<float>(1)),
u0(parameters.at<float>(2)),
v0(parameters.at<float>(3)),
k1(parameters.at<float>(4)),
k2(parameters.at<float>(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 <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
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<float, 2, 3> Jacobian_RetinaUnd_hC = (Matrix<float, 2, 3> << 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<float, 3, 2> Jacobian_hC_RetinaUnd = (Matrix<float, 3, 2> << 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 <opencv2/opencv.hpp>
#include <eigen3/Eigen/Dense>
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 <math.h>
// 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<double>::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<length; ++i) {
int x = i*s + x0;
int y = i*c + y0;
kernel.at<double>(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<double>::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<double>::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<maxIter; l++) {
//Improvement
for(int i=0; i<pRows; i++)
for(int j=0; j<pCols; j++)
patch_hat.at<double>(i,j) = patch.at<double>(pRows-i-1,pCols-j-1);
filter2D(kernel, k_est_conv, ddepth, patch, anchor, delta, cv::BORDER_DEFAULT);
for(int i=0; i<rows; i++)
for(int j=0; j<cols; j++) {
//if(est_conv.at<double>(i,j) < eps)
// est_conv.at<double>(i,j) = 10*eps;
k_relative_blur.at<double>(i,j) = kernel.at<double>(i,j) / k_est_conv.at<double>(i,j);
}
filter2D(k_relative_blur, k_error_est, ddepth, patch_hat);
for(int i=0; i<rows; i++)
for(int j=0; j<cols; j++) {
kernel.at<double>(i,j) = kernel.at<double>(i,j) * k_error_est.at<double>(i,j);
}
// Classical LR
for(int i=0; i<rows; i++)
for(int j=0; j<cols; j++)
kernel_hat.at<double>(i,j) = kernel.at<double>(rows-i-1,cols-j-1);
filter2D(result, est_conv, ddepth, kernel);
for(int i=0; i<pRows; i++)
for(int j=0; j<pCols; j++) {
//if(est_conv.at<double>(i,j) < eps)
// est_conv.at<double>(i,j) = 10*eps;
relative_blur.at<double>(i,j) = patch.at<double>(i,j) / est_conv.at<double>(i,j);
}
filter2D(relative_blur, error_est, ddepth, kernel_hat, anchor, delta, cv::BORDER_DEFAULT);
for(int i=0; i<pRows; i++)
for(int j=0; j<pCols; j++) {
result.at<double>(i,j) = result.at<double>(i,j) * error_est.at<double>(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<double>::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<rows; i++)
for(int j=0; j<cols; j++)
kernel_hat.at<double>(i,j) = kernel.at<double>(rows-i-1,cols-j-1);
double eps = DBL_MIN;
for(int l=0; l<maxIter; l++) {
filter2D(result, est_conv, ddepth, kernel, anchor, delta, cv::BORDER_DEFAULT);
for(int i=0; i<pRows; i++)
for(int j=0; j<pCols; j++) {
if(est_conv.at<double>(i,j) < eps) est_conv.at<double>(i,j) = 10*eps;
relative_blur.at<double>(i,j) = patch.at<double>(i,j) / est_conv.at<double>(i,j);
////std::cout << patch.at<double>(i,j) << " " << est_conv.at<double>(i,j) << " "<< relative_blur.at<double>(i,j) << std::endl;
}
filter2D(relative_blur, error_est, ddepth, kernel_hat, anchor, delta, cv::BORDER_DEFAULT);
for(int i=0; i<pRows; i++)
for(int j=0; j<pCols; j++) {
result.at<double>(i,j) = result.at<double>(i,j) * error_est.at<double>(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 <opencv2/opencv.hpp>
// 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 <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float32.h>
#include "RosVSLAMRansac.hpp"
#include "utils.hpp"
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <fstream>
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<nav_msgs::Path>("/monoslam/camera_poses", 1000);
camera_pose_pub = nh_.advertise<geometry_msgs::Pose>("/monoslam/camera_pose", 1000);
odometry_pub = nh_.advertise<nav_msgs::Odometry>("/monoslam/visual_odometry", 1000);
camera_pub = nh_.advertise<visualization_msgs::MarkerArray>("/monoslam/camera", 1000);
features_pub = nh_.advertise<visualization_msgs::MarkerArray>("/monoslam/features", 1000);
quality_index_pub = nh_.advertise<std_msgs::Float32>("/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<cv::Point2f> 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 <opencv2/opencv.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <eigen3/Eigen/Dense>
using namespace Eigen;
void plotFeatures(cv::Mat img, std::vector<cv::Point2f> 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 <ros/ros.h>
#include "vslamRansac.hpp"
#include "utils.hpp"
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <fstream>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Cholesky>
#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<STATE_DIM>(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<cv::Point2f> 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<int> 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<float,Dynamic,Dynamic>::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<float,Dynamic,Dynamic>::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<MatrixXf> 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 <opencv2/opencv.hpp>
#include "camModel.hpp"
#include "Patch.hpp"
#include <eigen3/Eigen/Dense>
#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<Patch> patches;
std::vector<Patch> 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 <ros/ros.h>
#include "vslamRansac.hpp"
#include "utils.hpp"
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <fstream>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Cholesky>
// 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 <<std::endl;
}
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();
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<cv::Point2f> 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<int> 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<float,Dynamic,Dynamic>::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<float,Dynamic,Dynamic>::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<MatrixXf> 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;
}
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
SYMBOL INDEX (83 symbols across 21 files)
FILE: src/ConfigVSLAM.cpp
function lookupAndPrintValue (line 20) | void lookupAndPrintValue(Config & cfg, string name_value, T & value) {
FILE: src/ConfigVSLAM.h
function class (line 16) | class ConfigVSLAM {
FILE: src/FeatureModel.cpp
function Matrix3f (line 70) | Matrix3f FeatureModel::computeSigma_mm(float sigma_pixel, float sigma_rh...
FILE: src/FeatureModel.h
function class (line 24) | class FeatureModel {
FILE: src/FeaturesState.h
function class (line 18) | class FeaturesState: public vector<FeatureModel>{
FILE: src/Patch.cpp
function computeCorrelation (line 269) | float computeCorrelation(cv::Mat f1, cv::Mat f2) {
FILE: src/Patch.hpp
class Patch (line 18) | class Patch {
FILE: src/Patches.hpp
class Patch (line 12) | class Patch: public std::vector<Patch> {
FILE: src/RosVSLAMRansac.hpp
class RosVSLAM (line 19) | class RosVSLAM : public VSlamFilter
FILE: src/camModel.cpp
function Matrix2f (line 18) | Matrix2f CamModel::diff_distort_undistort(Vector2f hn) {
function Vector2f (line 65) | Vector2f CamModel::projectAndDistort(Vector3f h, MatrixXf &J) {
function Vector2f (line 112) | Vector2f CamModel::projectAndDistort(Vector3f h) {
function Vector3f (line 144) | Vector3f CamModel::UndistortAndDeproject(Vector2f hd, MatrixXf &J){
FILE: src/camModel.hpp
type _camConfig_ (line 9) | struct _camConfig_ {
class CamModel (line 16) | class CamModel {
FILE: src/camOCV.cpp
function Vector2f (line 29) | Vector2f CamModel::projectAndDistort(Vector3f h, MatrixXf &J) {
function Vector2f (line 89) | Vector2f CamModel::projectAndDistort(Vector3f h) {
function Vector3f (line 131) | Vector3f CamModel::UndistortAndDeproject(Vector2f hd, MatrixXf &J){
FILE: src/camOCV.hpp
class CamModel (line 6) | class CamModel {
FILE: src/cameraModel.cpp
function Matrix2f (line 18) | Matrix2f CameraModel::diff_distort_undistort(Vector2f hn) {
function Vector2f (line 54) | Vector2f CameraModel::project(Vector3f h1, bool computeJacobian) {
function Matrix2f (line 87) | Matrix2f CameraModel::getProjectionJacobian() {
function Vector3f (line 94) | Vector3f CameraModel::unproject(Vector2f hd, bool computeJacobian){
function Matrix3f (line 135) | Matrix3f CameraModel::getUnprojectionJacobian() {
FILE: src/cameraModel.hpp
type _camConfig_ (line 9) | struct _camConfig_ {
class CameraModel (line 16) | class CameraModel {
FILE: src/libblur.cpp
function evaluateKernel (line 17) | cv::Mat evaluateKernel(cv::Point2f one, cv::Point2f two) {
function blurPatch (line 56) | cv::Mat blurPatch(const cv::Mat &_patch, cv::Point2f one, cv::Point2f tw...
function deblurPatch (line 95) | cv::Mat deblurPatch(const cv::Mat &_patch, cv::Point2f one, cv::Point2f ...
FILE: src/monoslam_ransac.cpp
class ImageConverter (line 38) | class ImageConverter
method ImageConverter (line 79) | ImageConverter(char *file = NULL)
method odom_callback (line 117) | void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg)
method imageCb (line 123) | void imageCb(const sensor_msgs::ImageConstPtr& msg)
function main (line 189) | int main(int argc, char** argv)
FILE: src/utils.cpp
function plotFeatures (line 5) | void plotFeatures(cv::Mat img, std::vector<cv::Point2f> features) {
function MatrixXf (line 19) | MatrixXf vConcat(MatrixXf m1, MatrixXf m2) {
function VectorXf (line 29) | VectorXf Concat(VectorXf m1, VectorXf m2) {
function MatrixXf (line 39) | MatrixXf hConcat(MatrixXf m1, MatrixXf m2) {
FILE: src/vslamRansac.cpp
function VectorXf (line 114) | VectorXf VSlamFilter::getState() {
function Vector3f (line 446) | Vector3f VSlamFilter::depth2XYZ(VectorXf f, MatrixXf &J) {
function MatrixXi (line 908) | MatrixXi computeEllipsoidParameters(MatrixXf St, int sigma_size) {
function Vector4f (line 928) | Vector4f vec2quat(Vector3f vec) {
function Vector4f (line 942) | Vector4f quatZero() {
function Matrix3f (line 948) | Matrix3f quat2rot(Vector4f quat) {
function Matrix4f (line 979) | Matrix4f YupsilonMatric(Vector4f q1) {
function Matrix4f (line 995) | Matrix4f YupsilonMatricComplementar(Vector4f q1) {
function Vector4f (line 1010) | Vector4f quatCrossProduct(Vector4f q1, Vector4f q2) {
function Vector3f (line 1037) | Vector3f inverse2XYZ(VectorXf f, Vector3f r, MatrixXf &J_hp_f, Matrix3f ...
function Vector3f (line 1070) | Vector3f inverse2XYZ(VectorXf f, Vector3f r) {
function MatrixXf (line 1084) | MatrixXf dg_x_dx(VectorXf mu, float dT) {
function Matrix4f (line 1103) | Matrix4f Jacobian_qt_qt1(Vector4f h) {
function MatrixXf (line 1119) | MatrixXf Jacobian_qt_w(Vector4f q, Vector3f w, float dT) {
function Matrix3f (line 1150) | Matrix3f diffQuat2rot(Vector4f quat, int index) {
function Vector4f (line 1201) | Vector4f quatComplement(Vector4f quat) {
function VectorXf (line 1208) | VectorXf fv(VectorXf Xv, double dT) {
function Matrix4f (line 1222) | Matrix4f d_qbar_q() {
function MatrixXf (line 1230) | MatrixXf d_f_d_hW(Vector3f hW) {
function normalizeQuaternion (line 1259) | void normalizeQuaternion(VectorXf &mu, MatrixXf &Sigma) {
function isInsideImage (line 1279) | bool isInsideImage(Vector2f hi, cv::Size size, int windowsSize) {
function MatrixXf (line 1290) | MatrixXf dRq_times_d_dq(Vector4f q, Vector3f d) {
FILE: src/vslamRansac.hpp
class VSlamFilter (line 14) | class VSlamFilter {
FILE: src/vslamRansac_OLD.cpp
function VectorXf (line 68) | VectorXf VSlamFilter::getState() {
function Vector3f (line 377) | Vector3f VSlamFilter::depth2XYZ(VectorXf f, MatrixXf &J) {
function MatrixXi (line 848) | MatrixXi computeEllipsoidParameters(MatrixXf St, int sigma_size) {
function Vector4f (line 868) | Vector4f vec2quat(Vector3f vec) {
function Vector4f (line 882) | Vector4f quatZero() {
function Matrix3f (line 888) | Matrix3f quat2rot(Vector4f quat) {
function Matrix4f (line 919) | Matrix4f YupsilonMatric(Vector4f q1) {
function Matrix4f (line 935) | Matrix4f YupsilonMatricComplementar(Vector4f q1) {
function Vector4f (line 950) | Vector4f quatCrossProduct(Vector4f q1, Vector4f q2) {
function Vector3f (line 977) | Vector3f inverse2XYZ(VectorXf f, Vector3f r, MatrixXf &J_hp_f, Matrix3f ...
function Vector3f (line 1010) | Vector3f inverse2XYZ(VectorXf f, Vector3f r) {
function MatrixXf (line 1024) | MatrixXf dg_x_dx(VectorXf mu) {
function Matrix4f (line 1043) | Matrix4f Jacobian_qt_qt1(Vector4f h) {
function MatrixXf (line 1059) | MatrixXf Jacobian_qt_w(Vector4f q, Vector3f w) {
function Matrix3f (line 1090) | Matrix3f diffQuat2rot(Vector4f quat, int index) {
function Vector4f (line 1141) | Vector4f quatComplement(Vector4f quat) {
function VectorXf (line 1148) | VectorXf fv(VectorXf Xv, float v_x, float w_z) {
function Matrix4f (line 1157) | Matrix4f d_qbar_q() {
function MatrixXf (line 1165) | MatrixXf d_f_d_hW(Vector3f hW) {
function normalizeQuaternion (line 1194) | void normalizeQuaternion(VectorXf &mu, MatrixXf &Sigma) {
function isInsideImage (line 1214) | bool isInsideImage(Vector2f hi, cv::Size size, int windowsSize) {
function MatrixXf (line 1225) | MatrixXf dRq_times_d_dq(Vector4f q, Vector3f d) {
Condensed preview — 41 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (205K chars).
[
{
"path": ".gitignore",
"chars": 10,
"preview": ".DS_Store\n"
},
{
"path": "CMakeLists.txt",
"chars": 4357,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(mono-slam)\nset(CMAKE_CXX_FLAGS \"-O2 -msse4 -DEIGEN_NO_DEBUG -DNDEBUG\")\n## "
},
{
"path": "LICENSE.md",
"chars": 16725,
"preview": "Mozilla Public License Version 2.0\n==================================\n\n1. Definitions\n--------------\n\n1.1. \"Contributor\""
},
{
"path": "README.md",
"chars": 1022,
"preview": "# Mono-Slam Implementation in ROS\n\nThis repository implements the mono-slam algorithm first introduced by [Andrew Daviso"
},
{
"path": "conf/conf.cfg",
"chars": 470,
"preview": "#configuration file for MonoSLAM\n\nsigma_v = 0.01;\nsigma_w = 0.005;\n\n\nrho_0 = 0.1;\nsigma_rho_0 = 0.25;\n\nsigma_pixel = 2;\n"
},
{
"path": "conf/conf_firewire.cfg",
"chars": 1012,
"preview": "#configuration file for MonoSLAM\n\nsigma_vx = 0.01;\nsigma_vy = 0.01;\nsigma_vz = 0.01;\nsigma_wx = 0.005; #metà delle v\nsi"
},
{
"path": "conf/conf_kinect.cfg",
"chars": 1175,
"preview": "#configuration file for MonoSLAM\n\nsigma_vx = 0.03;\nsigma_vy = 0.03;\nsigma_vz = 0.03;\nsigma_wx = 0.015; #metà delle v\nsi"
},
{
"path": "conf/confing.cfg",
"chars": 315,
"preview": "#configuration file for MonoSLAM\n\nsigma_v = 0.004;\nsigma_w = 0.004;\n\n\nrho_0 = 0.1;\nsigma_rho_0 = 0.01;\n\nsigma_pixel = 2;"
},
{
"path": "data/camera.ply",
"chars": 9764,
"preview": "ply\nformat ascii 1.0\ncomment Created by Blender 2.62 (sub 0) - www.blender.org, source file: 'camera.blend'\nelement vert"
},
{
"path": "data/test.dae",
"chars": 8503,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "data/test.ply",
"chars": 11350,
"preview": "ply\nformat ascii 1.0\ncomment Created by Blender 2.62 (sub 0) - www.blender.org, source file: ''\nelement vertex 178\nprope"
},
{
"path": "data/untitled.ply",
"chars": 8345,
"preview": "ply\nformat ascii 1.0\ncomment Created by Blender 2.62 (sub 0) - www.blender.org, source file: ''\nelement vertex 129\nprope"
},
{
"path": "package.xml",
"chars": 2058,
"preview": "<?xml version=\"1.0\"?>\n<package>\n <name>mono-slam</name>\n <version>0.0.1</version>\n <description>Mono-SLAM implementat"
},
{
"path": "src/ConfigVSLAM.cpp",
"chars": 5464,
"preview": "/*\n * ConfigVSLAM.cpp\n *\n * Created on: Jul 10, 2013\n * Author: ludovico\n */\n\n#include \"ConfigVSLAM.h\"\n\n#include <"
},
{
"path": "src/ConfigVSLAM.h",
"chars": 584,
"preview": "/*\n * ConfigVSLAM.h\n *\n * Created on: Jul 10, 2013\n * Author: ludovico\n */\n\n#ifndef CONFIGVSLAM_H_\n#define CONFIGV"
},
{
"path": "src/FeatureModel.cpp",
"chars": 2400,
"preview": "/*\n * FeatureModel.cpp\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#include \"FeatureModel.h\"\n\nFea"
},
{
"path": "src/FeatureModel.h",
"chars": 1032,
"preview": "/*\n * FeatureModel.h\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#ifndef FEATUREMODEL_H_\n#define "
},
{
"path": "src/FeaturesState.cpp",
"chars": 273,
"preview": "/*\n * FeaturesState.cpp\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#include \"FeaturesState.h\"\n\nF"
},
{
"path": "src/FeaturesState.h",
"chars": 420,
"preview": "/*\n * FeaturesState.h\n *\n * Created on: Jul 22, 2013\n * Author: ludovicorusso\n */\n\n#ifndef FEATURESSTATE_H_\n#defin"
},
{
"path": "src/Patch.cpp",
"chars": 6897,
"preview": "#include \"Patch.hpp\"\n#include <stdlib.h>\n#include <stdio.h>\n#include \"libblur.h\"\n\n#include <fstream>\n\nfloat computeCorre"
},
{
"path": "src/Patch.hpp",
"chars": 1656,
"preview": "#ifndef __PATCH_CLASS__\n#define __PATCH_CLASS__\n\n#include <opencv2/opencv.hpp>\n#include <eigen3/Eigen/Dense>\n\n\n\n\n#define"
},
{
"path": "src/Patches.cpp",
"chars": 0,
"preview": ""
},
{
"path": "src/Patches.hpp",
"chars": 241,
"preview": "#include \"Patch.hpp\"\n\n#ifndef __PATCHES_CLASS__\n#define __PATCHES_CLASS__\n\n#include <opencv2/opencv.hpp>\n#include <eigen"
},
{
"path": "src/RosVSLAMRansac.cpp",
"chars": 10395,
"preview": "#include \"RosVSLAMRansac.hpp\"\n#include <sstream>\n#include <geometry_msgs/PoseStamped.h>\n#include <nav_msgs/Odometry.h>\n\n"
},
{
"path": "src/RosVSLAMRansac.hpp",
"chars": 838,
"preview": "#include \"vslamRansac.hpp\"\n\n#include <ros/ros.h>\n\n#include <geometry_msgs/PoseArray.h>\n#include <geometry_msgs/Pose.h>\n#"
},
{
"path": "src/camModel.cpp",
"chars": 3688,
"preview": "#include \"camModel.hpp\"\n\n\nvoid CamModel::setParam(camConfig camParams) {\n\tfx = camParams.fx;\n\tfy = camParams.fy;\n\tu0 = c"
},
{
"path": "src/camModel.hpp",
"chars": 1047,
"preview": "#ifndef __CAM_MODEL__\n#define __CAM_MODEL__\n\n#include <opencv2/opencv.hpp>\n#include <eigen3/Eigen/Dense>\n\n\n\ntypedef stru"
},
{
"path": "src/camOCV.cpp",
"chars": 3721,
"preview": "#include \"camOCV.hpp\"\n\nCamModel::CamModel(float _fx, float _fy, float _u0, float _v0, float _k1, float _k2):\n fx("
},
{
"path": "src/camOCV.hpp",
"chars": 1449,
"preview": "#include <opencv2/opencv.hpp>\n#include <eigen3/Eigen/Dense>\n\nusing namespace Eigen;\n\nclass CamModel {\n float fx, fy, "
},
{
"path": "src/cameraModel.cpp",
"chars": 3326,
"preview": "#include \"cameraModel.hpp\"\n\n\nvoid CameraModel::setParam(camConfig camParams) {\n\tfx = camParams.fx;\n\tfy = camParams.fy;\n\t"
},
{
"path": "src/cameraModel.hpp",
"chars": 1065,
"preview": "#ifndef __CAMERA_MODEL__\n#define __CAMERA_MODEL__\n\n#include <opencv2/opencv.hpp>\n#include <eigen3/Eigen/Dense>\n\n\n\ntypede"
},
{
"path": "src/libblur.cpp",
"chars": 7798,
"preview": "/*\n * libblur.cpp\n *\n * Created on: Jul 9, 2013\n * Author: ludovico\n */\n#include \"libblur.h\"\n#include <math.h>\n\n//"
},
{
"path": "src/libblur.h",
"chars": 1514,
"preview": "/*\n * libblur.h\n *\n * Created on: Jul 9, 2013\n * Author: Ludovico\n */\n\n#ifndef LIBBLUR_H_\n#define LIBBLUR_H_\n\n\n#in"
},
{
"path": "src/monoslam_ransac.cpp",
"chars": 4131,
"preview": "#include <ros/ros.h>\n#include <image_transport/image_transport.h>\n#include <cv_bridge/cv_bridge.h>\n\n#include <sensor_msg"
},
{
"path": "src/utils.cpp",
"chars": 1086,
"preview": "#include \"utils.hpp\"\n\n\n\nvoid plotFeatures(cv::Mat img, std::vector<cv::Point2f> features) {\n int fontFace = cv::FONT_"
},
{
"path": "src/utils.hpp",
"chars": 349,
"preview": "#include <opencv2/opencv.hpp>\n#include <iostream>\n#include <stdio.h>\n#include <stdlib.h>\n\n#include <eigen3/Eigen/Dense>\n"
},
{
"path": "src/vslamRansac.cpp",
"chars": 33917,
"preview": "//#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(2,1)\n\n#include <ros/ros.h>\n\n#include \"vslamRansac.hpp\"\n#include \"utils"
},
{
"path": "src/vslamRansac.hpp",
"chars": 1842,
"preview": "#define DEBUG\n\n#include <opencv2/opencv.hpp>\n#include \"camModel.hpp\"\n\n#include \"Patch.hpp\"\n\n#include <eigen3/Eigen/Dense"
},
{
"path": "src/vslamRansac_OLD.cpp",
"chars": 32789,
"preview": "//#define EIGEN_DEFAULT_IO_FORMAT Eigen::IOFormat(2,1)\n\n#include <ros/ros.h>\n\n#include \"vslamRansac.hpp\"\n#include \"utils"
}
]
// ... and 2 more files (download for full content)
About this extraction
This page contains the full source code of the rrg-polito/mono-slam GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 41 files (188.5 KB), approximately 70.4k tokens, and a symbol index with 83 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.