diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..c40dc14
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,288 @@
+cmake_minimum_required(VERSION 3.6)
+project(VINS_Fusion)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+set(SOURCE_FILES
+ camera_models/camera_calib_example/calibrationdata/left-0000.png
+ camera_models/camera_calib_example/calibrationdata/left-0001.png
+ camera_models/camera_calib_example/calibrationdata/left-0002.png
+ camera_models/camera_calib_example/calibrationdata/left-0003.png
+ camera_models/camera_calib_example/calibrationdata/left-0004.png
+ camera_models/camera_calib_example/calibrationdata/left-0005.png
+ camera_models/camera_calib_example/calibrationdata/left-0006.png
+ camera_models/camera_calib_example/calibrationdata/left-0007.png
+ camera_models/camera_calib_example/calibrationdata/left-0008.png
+ camera_models/camera_calib_example/calibrationdata/left-0009.png
+ camera_models/camera_calib_example/calibrationdata/left-0010.png
+ camera_models/camera_calib_example/calibrationdata/left-0011.png
+ camera_models/camera_calib_example/calibrationdata/left-0012.png
+ camera_models/camera_calib_example/calibrationdata/left-0013.png
+ camera_models/camera_calib_example/calibrationdata/left-0014.png
+ camera_models/camera_calib_example/calibrationdata/left-0015.png
+ camera_models/camera_calib_example/calibrationdata/left-0016.png
+ camera_models/camera_calib_example/calibrationdata/left-0017.png
+ camera_models/camera_calib_example/calibrationdata/left-0018.png
+ camera_models/camera_calib_example/calibrationdata/left-0019.png
+ camera_models/camera_calib_example/calibrationdata/left-0020.png
+ camera_models/camera_calib_example/calibrationdata/left-0021.png
+ camera_models/camera_calib_example/calibrationdata/left-0022.png
+ camera_models/camera_calib_example/calibrationdata/left-0023.png
+ camera_models/camera_calib_example/calibrationdata/left-0024.png
+ camera_models/camera_calib_example/calibrationdata/left-0025.png
+ camera_models/camera_calib_example/calibrationdata/left-0026.png
+ camera_models/camera_calib_example/calibrationdata/left-0027.png
+ camera_models/camera_calib_example/calibrationdata/left-0028.png
+ camera_models/camera_calib_example/calibrationdata/left-0029.png
+ camera_models/camera_calib_example/calibrationdata/left-0030.png
+ camera_models/camera_calib_example/calibrationdata/left-0031.png
+ camera_models/camera_calib_example/calibrationdata/left-0032.png
+ camera_models/camera_calib_example/calibrationdata/left-0033.png
+ camera_models/camera_calib_example/calibrationdata/left-0034.png
+ camera_models/camera_calib_example/calibrationdata/left-0035.png
+ camera_models/camera_calib_example/calibrationdata/left-0036.png
+ camera_models/camera_calib_example/calibrationdata/left-0037.png
+ camera_models/camera_calib_example/calibrationdata/left-0038.png
+ camera_models/camera_calib_example/calibrationdata/left-0039.png
+ camera_models/camera_calib_example/calibrationdata/left-0040.png
+ camera_models/camera_calib_example/calibrationdata/left-0041.png
+ camera_models/camera_calib_example/calibrationdata/left-0042.png
+ camera_models/camera_calib_example/calibrationdata/left-0043.png
+ camera_models/camera_calib_example/calibrationdata/left-0044.png
+ camera_models/camera_calib_example/calibrationdata/left-0045.png
+ camera_models/camera_calib_example/calibrationdata/left-0046.png
+ camera_models/camera_calib_example/calibrationdata/left-0047.png
+ camera_models/camera_calib_example/calibrationdata/left-0048.png
+ camera_models/camera_calib_example/calibrationdata/left-0049.png
+ camera_models/camera_calib_example/calibrationdata/left-0050.png
+ camera_models/camera_calib_example/calibrationdata/left-0051.png
+ camera_models/camera_calib_example/calibrationdata/left-0052.png
+ camera_models/camera_calib_example/calibrationdata/left-0053.png
+ camera_models/camera_calib_example/calibrationdata/left-0054.png
+ camera_models/camera_calib_example/calibrationdata/left-0055.png
+ camera_models/camera_calib_example/calibrationdata/left-0056.png
+ camera_models/camera_calib_example/calibrationdata/left-0057.png
+ camera_models/camera_calib_example/calibrationdata/left-0058.png
+ camera_models/camera_calib_example/calibrationdata/left-0059.png
+ camera_models/camera_calib_example/calibrationdata/left-0060.png
+ camera_models/camera_calib_example/calibrationdata/left-0061.png
+ camera_models/camera_calib_example/calibrationdata/left-0062.png
+ camera_models/camera_calib_example/calibrationdata/left-0063.png
+ camera_models/camera_calib_example/calibrationdata/left-0064.png
+ camera_models/camera_calib_example/calibrationdata/left-0065.png
+ camera_models/camera_calib_example/calibrationdata/left-0066.png
+ camera_models/camera_calib_example/calibrationdata/left-0067.png
+ camera_models/camera_calib_example/calibrationdata/left-0068.png
+ camera_models/camera_calib_example/calibrationdata/left-0069.png
+ camera_models/camera_calib_example/calibrationdata/left-0070.png
+ camera_models/camera_calib_example/calibrationdata/left-0071.png
+ camera_models/camera_calib_example/calibrationdata/left-0072.png
+ camera_models/camera_calib_example/calibrationdata/left-0073.png
+ camera_models/camera_calib_example/calibrationdata/left-0074.png
+ camera_models/camera_calib_example/calibrationdata/left-0075.png
+ camera_models/camera_calib_example/calibrationdata/left-0076.png
+ camera_models/camera_calib_example/calibrationdata/left-0077.png
+ camera_models/camera_calib_example/calibrationdata/left-0078.png
+ camera_models/camera_calib_example/calibrationdata/left-0079.png
+ camera_models/camera_calib_example/calibrationdata/left-0080.png
+ camera_models/camera_calib_example/calibrationdata/left-0081.png
+ camera_models/camera_calib_example/calibrationdata/left-0082.png
+ camera_models/camera_calib_example/calibrationdata/left-0083.png
+ camera_models/camera_calib_example/calibrationdata/left-0084.png
+ camera_models/camera_calib_example/readme.txt
+ camera_models/cmake/FindEigen.cmake
+ camera_models/include/camodocal/calib/CameraCalibration.h
+ camera_models/include/camodocal/camera_models/Camera.h
+ camera_models/include/camodocal/camera_models/CameraFactory.h
+ camera_models/include/camodocal/camera_models/CataCamera.h
+ camera_models/include/camodocal/camera_models/CostFunctionFactory.h
+ camera_models/include/camodocal/camera_models/EquidistantCamera.h
+ camera_models/include/camodocal/camera_models/PinholeCamera.h
+ camera_models/include/camodocal/camera_models/PinholeFullCamera.h
+ camera_models/include/camodocal/camera_models/ScaramuzzaCamera.h
+ camera_models/include/camodocal/chessboard/Chessboard.h
+ camera_models/include/camodocal/chessboard/ChessboardCorner.h
+ camera_models/include/camodocal/chessboard/ChessboardQuad.h
+ camera_models/include/camodocal/chessboard/Spline.h
+ camera_models/include/camodocal/gpl/EigenQuaternionParameterization.h
+ camera_models/include/camodocal/gpl/EigenUtils.h
+ camera_models/include/camodocal/gpl/gpl.h
+ camera_models/include/camodocal/sparse_graph/Transform.h
+ camera_models/src/calib/CameraCalibration.cc
+ camera_models/src/camera_models/Camera.cc
+ camera_models/src/camera_models/CameraFactory.cc
+ camera_models/src/camera_models/CataCamera.cc
+ camera_models/src/camera_models/CostFunctionFactory.cc
+ camera_models/src/camera_models/EquidistantCamera.cc
+ camera_models/src/camera_models/PinholeCamera.cc
+ camera_models/src/camera_models/PinholeFullCamera.cc
+ camera_models/src/camera_models/ScaramuzzaCamera.cc
+ camera_models/src/chessboard/Chessboard.cc
+ camera_models/src/gpl/EigenQuaternionParameterization.cc
+ camera_models/src/gpl/gpl.cc
+ camera_models/src/sparse_graph/Transform.cc
+ camera_models/src/intrinsic_calib.cc
+ camera_models/CMakeLists.txt
+ camera_models/package.xml
+ camera_models/readme.md
+ config/A3_ptgrey/a3_ptgrey_mono_imu_config.yaml
+ config/A3_ptgrey/a3_ptgrey_stereo_config.yaml
+ config/A3_ptgrey/a3_ptgrey_stereo_imu_config.yaml
+ config/A3_ptgrey/left.yaml
+ config/A3_ptgrey/right.yaml
+ config/euroc/cam0_mei.yaml
+ config/euroc/cam0_pinhole.yaml
+ config/euroc/cam1_mei.yaml
+ config/euroc/cam1_pinhole.yaml
+ config/euroc/euroc_mono_imu_config.yaml
+ config/euroc/euroc_stereo_config.yaml
+ config/euroc/euroc_stereo_imu_config.yaml
+ config/kitti_odom/cam00-02.yaml
+ config/kitti_odom/cam03.yaml
+ config/kitti_odom/cam04-12.yaml
+ config/kitti_odom/cam13-21.yaml
+ config/kitti_odom/kitti_config00-02.yaml
+ config/kitti_odom/kitti_config03.yaml
+ config/kitti_odom/kitti_config04-12.yaml
+ config/kitti_odom/kitti_config13-21.yaml
+ config/kitti_raw/cam_09_30.yaml
+ config/kitti_raw/cam_10_03.yaml
+ config/kitti_raw/kitti_09_30_config.yaml
+ config/kitti_raw/kitti_10_03_config.yaml
+ config/mynteye/left_mei.yaml
+ config/mynteye/mynteye_mono_imu_config.yaml
+ config/mynteye/mynteye_stereo_config.yaml
+ config/mynteye/mynteye_stereo_imu_config.yaml
+ config/mynteye/right_mei.yaml
+ config/simulation/cam0_mei.yaml
+ config/simulation/cam1_mei.yaml
+ config/simulation/simulation_config.yaml
+ config/vi_car/cam0_mei.yaml
+ config/vi_car/cam0_pinhole.yaml
+ config/vi_car/cam1_mei.yaml
+ config/vi_car/cam1_pinhole.yaml
+ config/vi_car/vi_car.yaml
+ config/extrinsic_parameter_example.pdf
+ config/fisheye_mask.jpg
+ config/fisheye_mask_752x480.jpg
+ config/vins_rviz_config.rviz
+ global_fusion/models/car.dae
+ global_fusion/models/hummingbird.mesh
+ global_fusion/src/Factors.h
+ global_fusion/src/globalOpt.cpp
+ global_fusion/src/globalOpt.h
+ global_fusion/src/globalOptNode.cpp
+ global_fusion/src/tic_toc.h
+ global_fusion/ThirdParty/GeographicLib/include/Config.h
+ global_fusion/ThirdParty/GeographicLib/include/Constants.hpp
+ global_fusion/ThirdParty/GeographicLib/include/Geocentric.hpp
+ global_fusion/ThirdParty/GeographicLib/include/LocalCartesian.hpp
+ global_fusion/ThirdParty/GeographicLib/include/Math.hpp
+ global_fusion/ThirdParty/GeographicLib/src/Geocentric.cpp
+ global_fusion/ThirdParty/GeographicLib/src/LocalCartesian.cpp
+ global_fusion/ThirdParty/GeographicLib/src/Math.cpp
+ global_fusion/ThirdParty/GeographicLib/CMakeLists.txt
+ global_fusion/CMakeLists.txt
+ global_fusion/package.xml
+ loop_fusion/cmake/FindEigen.cmake
+ loop_fusion/src/ThirdParty/DBoW/BowVector.cpp
+ loop_fusion/src/ThirdParty/DBoW/BowVector.h
+ loop_fusion/src/ThirdParty/DBoW/DBoW2.h
+ loop_fusion/src/ThirdParty/DBoW/FBrief.cpp
+ loop_fusion/src/ThirdParty/DBoW/FBrief.h
+ loop_fusion/src/ThirdParty/DBoW/FClass.h
+ loop_fusion/src/ThirdParty/DBoW/FeatureVector.cpp
+ loop_fusion/src/ThirdParty/DBoW/FeatureVector.h
+ loop_fusion/src/ThirdParty/DBoW/QueryResults.cpp
+ loop_fusion/src/ThirdParty/DBoW/QueryResults.h
+ loop_fusion/src/ThirdParty/DBoW/ScoringObject.cpp
+ loop_fusion/src/ThirdParty/DBoW/ScoringObject.h
+ loop_fusion/src/ThirdParty/DBoW/TemplatedDatabase.h
+ loop_fusion/src/ThirdParty/DBoW/TemplatedVocabulary.h
+ loop_fusion/src/ThirdParty/DUtils/DException.h
+ loop_fusion/src/ThirdParty/DUtils/DUtils.h
+ loop_fusion/src/ThirdParty/DUtils/Random.cpp
+ loop_fusion/src/ThirdParty/DUtils/Random.h
+ loop_fusion/src/ThirdParty/DUtils/Timestamp.cpp
+ loop_fusion/src/ThirdParty/DUtils/Timestamp.h
+ loop_fusion/src/ThirdParty/DVision/BRIEF.cpp
+ loop_fusion/src/ThirdParty/DVision/BRIEF.h
+ loop_fusion/src/ThirdParty/DVision/DVision.h
+ loop_fusion/src/ThirdParty/VocabularyBinary.cpp
+ loop_fusion/src/ThirdParty/VocabularyBinary.hpp
+ loop_fusion/src/utility/CameraPoseVisualization.cpp
+ loop_fusion/src/utility/CameraPoseVisualization.h
+ loop_fusion/src/utility/tic_toc.h
+ loop_fusion/src/utility/utility.cpp
+ loop_fusion/src/utility/utility.h
+ loop_fusion/src/keyframe.cpp
+ loop_fusion/src/keyframe.h
+ loop_fusion/src/parameters.h
+ loop_fusion/src/pose_graph.cpp
+ loop_fusion/src/pose_graph.h
+ loop_fusion/src/pose_graph_node.cpp
+ loop_fusion/CMakeLists.txt
+ loop_fusion/package.xml
+ support_files/image/car.png
+ support_files/image/car_gif.gif
+ support_files/image/euroc.gif
+ support_files/image/kitti.gif
+ support_files/image/kitti.png
+ support_files/image/kitti_rank.png
+ support_files/image/vins.png
+ support_files/image/vins_black.png
+ support_files/image/vins_logo.png
+ "support_files/paper/VINS-Mono: A Robust and Versatile Monocular\nVisual-Inertial State Estimator.pdf"
+ support_files/brief_k10L6.bin
+ support_files/brief_pattern.yml
+ support_files/paper_bib.txt
+ vins_estimator/cmake/FindEigen.cmake
+ vins_estimator/launch/vins_rviz.launch
+ vins_estimator/src/estimator/estimator.cpp
+ vins_estimator/src/estimator/estimator.h
+ vins_estimator/src/estimator/feature_manager.cpp
+ vins_estimator/src/estimator/feature_manager.h
+ vins_estimator/src/estimator/parameters.cpp
+ vins_estimator/src/estimator/parameters.h
+ vins_estimator/src/factor/imu_factor.h
+ vins_estimator/src/factor/initial_bias_factor.h
+ vins_estimator/src/factor/initial_pose_factor.h
+ vins_estimator/src/factor/integration_base.h
+ vins_estimator/src/factor/marginalization_factor.cpp
+ vins_estimator/src/factor/marginalization_factor.h
+ vins_estimator/src/factor/pose_local_parameterization.cpp
+ vins_estimator/src/factor/pose_local_parameterization.h
+ vins_estimator/src/factor/projection_factor.cpp
+ vins_estimator/src/factor/projection_factor.h
+ vins_estimator/src/factor/projectionOneFrameTwoCamFactor.cpp
+ vins_estimator/src/factor/projectionOneFrameTwoCamFactor.h
+ vins_estimator/src/factor/projectionTwoFrameOneCamFactor.cpp
+ vins_estimator/src/factor/projectionTwoFrameOneCamFactor.h
+ vins_estimator/src/factor/projectionTwoFrameTwoCamFactor.cpp
+ vins_estimator/src/factor/projectionTwoFrameTwoCamFactor.h
+ vins_estimator/src/featureTracker/feature_tracker.cpp
+ vins_estimator/src/featureTracker/feature_tracker.h
+ vins_estimator/src/initial/initial_aligment.cpp
+ vins_estimator/src/initial/initial_alignment.h
+ vins_estimator/src/initial/initial_ex_rotation.cpp
+ vins_estimator/src/initial/initial_ex_rotation.h
+ vins_estimator/src/initial/initial_sfm.cpp
+ vins_estimator/src/initial/initial_sfm.h
+ vins_estimator/src/initial/solve_5pts.cpp
+ vins_estimator/src/initial/solve_5pts.h
+ vins_estimator/src/utility/CameraPoseVisualization.cpp
+ vins_estimator/src/utility/CameraPoseVisualization.h
+ vins_estimator/src/utility/tic_toc.h
+ vins_estimator/src/utility/utility.cpp
+ vins_estimator/src/utility/utility.h
+ vins_estimator/src/utility/visualization.cpp
+ vins_estimator/src/utility/visualization.h
+ vins_estimator/src/KITTIGPSTest.cpp
+ vins_estimator/src/KITTIOdomTest.cpp
+ vins_estimator/src/rosNodeTest.cpp
+ vins_estimator/CMakeLists.txt
+ vins_estimator/package.xml
+ LICENCE
+ README.md)
+
+add_executable(VINS_Fusion ${SOURCE_FILES})
\ No newline at end of file
diff --git a/LICENCE b/LICENCE
new file mode 100644
index 0000000..9cecc1d
--- /dev/null
+++ b/LICENCE
@@ -0,0 +1,674 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc.
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+ The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users. We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors. You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+ To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights. Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received. You must make sure that they, too, receive
+or can get the source code. And you must show them these terms so they
+know their rights.
+
+ Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+ For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software. For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+ Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so. This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software. The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable. Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products. If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+ Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary. To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+ 13. Use with the GNU Affero General Public License.
+
+ Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+ 14. Revised Versions of this License.
+
+ The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+ If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Programs
+
+ If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+ To do so, attach the following notices to the program. It is safest
+to attach them to the start of each source file to most effectively
+state the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+ {one line to give the program's name and a brief idea of what it does.}
+ Copyright (C) {year} {name of author}
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+
+Also add information on how to contact you by electronic and paper mail.
+
+ If the program does terminal interaction, make it output a short
+notice like this when it starts in an interactive mode:
+
+ {project} Copyright (C) {year} {fullname}
+ This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+ This is free software, and you are welcome to redistribute it
+ under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License. Of course, your program's commands
+might be different; for a GUI interface, you would use an "about box".
+
+ You should also get your employer (if you work as a programmer) or school,
+if any, to sign a "copyright disclaimer" for the program, if necessary.
+For more information on this, and how to apply and follow the GNU GPL, see
+.
+
+ The GNU General Public License does not permit incorporating your program
+into proprietary programs. If your program is a subroutine library, you
+may consider it more useful to permit linking proprietary applications with
+the library. If this is what you want to do, use the GNU Lesser General
+Public License instead of this License. But first, please read
+.
diff --git a/camera_models/CMakeLists.txt b/camera_models/CMakeLists.txt
new file mode 100644
index 0000000..3b61190
--- /dev/null
+++ b/camera_models/CMakeLists.txt
@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(camera_models)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++11")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC")
+
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ std_msgs
+ )
+
+find_package(Boost REQUIRED COMPONENTS filesystem program_options system)
+include_directories(${Boost_INCLUDE_DIRS})
+
+find_package(OpenCV REQUIRED)
+
+# set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3")
+find_package(Ceres REQUIRED)
+include_directories(${CERES_INCLUDE_DIRS})
+
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES camera_models
+ CATKIN_DEPENDS roscpp std_msgs
+# DEPENDS system_lib
+ )
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+ )
+
+include_directories("include")
+
+add_executable(Calibrations
+ src/intrinsic_calib.cc
+ src/chessboard/Chessboard.cc
+ src/calib/CameraCalibration.cc
+ src/camera_models/Camera.cc
+ src/camera_models/CameraFactory.cc
+ src/camera_models/CostFunctionFactory.cc
+ src/camera_models/PinholeCamera.cc
+ src/camera_models/PinholeFullCamera.cc
+ src/camera_models/CataCamera.cc
+ src/camera_models/EquidistantCamera.cc
+ src/camera_models/ScaramuzzaCamera.cc
+ src/sparse_graph/Transform.cc
+ src/gpl/gpl.cc
+ src/gpl/EigenQuaternionParameterization.cc)
+
+add_library(camera_models
+ src/chessboard/Chessboard.cc
+ src/calib/CameraCalibration.cc
+ src/camera_models/Camera.cc
+ src/camera_models/CameraFactory.cc
+ src/camera_models/CostFunctionFactory.cc
+ src/camera_models/PinholeCamera.cc
+ src/camera_models/PinholeFullCamera.cc
+ src/camera_models/CataCamera.cc
+ src/camera_models/EquidistantCamera.cc
+ src/camera_models/ScaramuzzaCamera.cc
+ src/sparse_graph/Transform.cc
+ src/gpl/gpl.cc
+ src/gpl/EigenQuaternionParameterization.cc)
+
+target_link_libraries(Calibrations ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
+target_link_libraries(camera_models ${Boost_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES})
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0000.png b/camera_models/camera_calib_example/calibrationdata/left-0000.png
new file mode 100644
index 0000000..2fc0de4
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0000.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0001.png b/camera_models/camera_calib_example/calibrationdata/left-0001.png
new file mode 100644
index 0000000..89a5d7e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0001.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0002.png b/camera_models/camera_calib_example/calibrationdata/left-0002.png
new file mode 100644
index 0000000..12dae33
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0002.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0003.png b/camera_models/camera_calib_example/calibrationdata/left-0003.png
new file mode 100644
index 0000000..bc33f0e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0003.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0004.png b/camera_models/camera_calib_example/calibrationdata/left-0004.png
new file mode 100644
index 0000000..eb6d826
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0004.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0005.png b/camera_models/camera_calib_example/calibrationdata/left-0005.png
new file mode 100644
index 0000000..771b9d6
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0005.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0006.png b/camera_models/camera_calib_example/calibrationdata/left-0006.png
new file mode 100644
index 0000000..f74523f
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0006.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0007.png b/camera_models/camera_calib_example/calibrationdata/left-0007.png
new file mode 100644
index 0000000..4f8825f
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0007.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0008.png b/camera_models/camera_calib_example/calibrationdata/left-0008.png
new file mode 100644
index 0000000..06e435c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0008.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0009.png b/camera_models/camera_calib_example/calibrationdata/left-0009.png
new file mode 100644
index 0000000..d06b395
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0009.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0010.png b/camera_models/camera_calib_example/calibrationdata/left-0010.png
new file mode 100644
index 0000000..b6e62b3
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0010.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0011.png b/camera_models/camera_calib_example/calibrationdata/left-0011.png
new file mode 100644
index 0000000..13a531b
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0011.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0012.png b/camera_models/camera_calib_example/calibrationdata/left-0012.png
new file mode 100644
index 0000000..610a28f
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0012.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0013.png b/camera_models/camera_calib_example/calibrationdata/left-0013.png
new file mode 100644
index 0000000..ba9df7c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0013.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0014.png b/camera_models/camera_calib_example/calibrationdata/left-0014.png
new file mode 100644
index 0000000..247bf5e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0014.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0015.png b/camera_models/camera_calib_example/calibrationdata/left-0015.png
new file mode 100644
index 0000000..222b3b7
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0015.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0016.png b/camera_models/camera_calib_example/calibrationdata/left-0016.png
new file mode 100644
index 0000000..864abca
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0016.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0017.png b/camera_models/camera_calib_example/calibrationdata/left-0017.png
new file mode 100644
index 0000000..6292866
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0017.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0018.png b/camera_models/camera_calib_example/calibrationdata/left-0018.png
new file mode 100644
index 0000000..c9fd031
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0018.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0019.png b/camera_models/camera_calib_example/calibrationdata/left-0019.png
new file mode 100644
index 0000000..3802175
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0019.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0020.png b/camera_models/camera_calib_example/calibrationdata/left-0020.png
new file mode 100644
index 0000000..eab6d24
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0020.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0021.png b/camera_models/camera_calib_example/calibrationdata/left-0021.png
new file mode 100644
index 0000000..d8b9a90
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0021.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0022.png b/camera_models/camera_calib_example/calibrationdata/left-0022.png
new file mode 100644
index 0000000..8254218
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0022.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0023.png b/camera_models/camera_calib_example/calibrationdata/left-0023.png
new file mode 100644
index 0000000..999cc54
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0023.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0024.png b/camera_models/camera_calib_example/calibrationdata/left-0024.png
new file mode 100644
index 0000000..becd591
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0024.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0025.png b/camera_models/camera_calib_example/calibrationdata/left-0025.png
new file mode 100644
index 0000000..614156c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0025.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0026.png b/camera_models/camera_calib_example/calibrationdata/left-0026.png
new file mode 100644
index 0000000..9648ea8
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0026.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0027.png b/camera_models/camera_calib_example/calibrationdata/left-0027.png
new file mode 100644
index 0000000..9b0ccb9
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0027.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0028.png b/camera_models/camera_calib_example/calibrationdata/left-0028.png
new file mode 100644
index 0000000..1dec5f3
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0028.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0029.png b/camera_models/camera_calib_example/calibrationdata/left-0029.png
new file mode 100644
index 0000000..43f830f
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0029.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0030.png b/camera_models/camera_calib_example/calibrationdata/left-0030.png
new file mode 100644
index 0000000..d04595c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0030.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0031.png b/camera_models/camera_calib_example/calibrationdata/left-0031.png
new file mode 100644
index 0000000..c880b6b
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0031.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0032.png b/camera_models/camera_calib_example/calibrationdata/left-0032.png
new file mode 100644
index 0000000..9bb78d2
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0032.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0033.png b/camera_models/camera_calib_example/calibrationdata/left-0033.png
new file mode 100644
index 0000000..9914ff0
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0033.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0034.png b/camera_models/camera_calib_example/calibrationdata/left-0034.png
new file mode 100644
index 0000000..938b0df
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0034.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0035.png b/camera_models/camera_calib_example/calibrationdata/left-0035.png
new file mode 100644
index 0000000..8037ed8
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0035.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0036.png b/camera_models/camera_calib_example/calibrationdata/left-0036.png
new file mode 100644
index 0000000..5bc149c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0036.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0037.png b/camera_models/camera_calib_example/calibrationdata/left-0037.png
new file mode 100644
index 0000000..5777c86
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0037.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0038.png b/camera_models/camera_calib_example/calibrationdata/left-0038.png
new file mode 100644
index 0000000..7f0d88a
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0038.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0039.png b/camera_models/camera_calib_example/calibrationdata/left-0039.png
new file mode 100644
index 0000000..82039af
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0039.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0040.png b/camera_models/camera_calib_example/calibrationdata/left-0040.png
new file mode 100644
index 0000000..3cc8804
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0040.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0041.png b/camera_models/camera_calib_example/calibrationdata/left-0041.png
new file mode 100644
index 0000000..ef31425
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0041.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0042.png b/camera_models/camera_calib_example/calibrationdata/left-0042.png
new file mode 100644
index 0000000..823daa8
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0042.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0043.png b/camera_models/camera_calib_example/calibrationdata/left-0043.png
new file mode 100644
index 0000000..f14b803
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0043.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0044.png b/camera_models/camera_calib_example/calibrationdata/left-0044.png
new file mode 100644
index 0000000..b109a8b
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0044.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0045.png b/camera_models/camera_calib_example/calibrationdata/left-0045.png
new file mode 100644
index 0000000..affcc5a
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0045.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0046.png b/camera_models/camera_calib_example/calibrationdata/left-0046.png
new file mode 100644
index 0000000..11e6afe
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0046.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0047.png b/camera_models/camera_calib_example/calibrationdata/left-0047.png
new file mode 100644
index 0000000..dd19626
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0047.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0048.png b/camera_models/camera_calib_example/calibrationdata/left-0048.png
new file mode 100644
index 0000000..3470997
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0048.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0049.png b/camera_models/camera_calib_example/calibrationdata/left-0049.png
new file mode 100644
index 0000000..3e19935
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0049.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0050.png b/camera_models/camera_calib_example/calibrationdata/left-0050.png
new file mode 100644
index 0000000..f4865f8
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0050.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0051.png b/camera_models/camera_calib_example/calibrationdata/left-0051.png
new file mode 100644
index 0000000..a72ebf9
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0051.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0052.png b/camera_models/camera_calib_example/calibrationdata/left-0052.png
new file mode 100644
index 0000000..6cb8205
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0052.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0053.png b/camera_models/camera_calib_example/calibrationdata/left-0053.png
new file mode 100644
index 0000000..dd034b3
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0053.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0054.png b/camera_models/camera_calib_example/calibrationdata/left-0054.png
new file mode 100644
index 0000000..de6739a
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0054.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0055.png b/camera_models/camera_calib_example/calibrationdata/left-0055.png
new file mode 100644
index 0000000..f54492e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0055.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0056.png b/camera_models/camera_calib_example/calibrationdata/left-0056.png
new file mode 100644
index 0000000..8ebf5c2
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0056.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0057.png b/camera_models/camera_calib_example/calibrationdata/left-0057.png
new file mode 100644
index 0000000..ea3cad9
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0057.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0058.png b/camera_models/camera_calib_example/calibrationdata/left-0058.png
new file mode 100644
index 0000000..473f3b7
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0058.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0059.png b/camera_models/camera_calib_example/calibrationdata/left-0059.png
new file mode 100644
index 0000000..99d5114
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0059.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0060.png b/camera_models/camera_calib_example/calibrationdata/left-0060.png
new file mode 100644
index 0000000..5253a4e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0060.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0061.png b/camera_models/camera_calib_example/calibrationdata/left-0061.png
new file mode 100644
index 0000000..da303fe
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0061.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0062.png b/camera_models/camera_calib_example/calibrationdata/left-0062.png
new file mode 100644
index 0000000..e112114
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0062.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0063.png b/camera_models/camera_calib_example/calibrationdata/left-0063.png
new file mode 100644
index 0000000..5914621
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0063.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0064.png b/camera_models/camera_calib_example/calibrationdata/left-0064.png
new file mode 100644
index 0000000..bbd0439
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0064.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0065.png b/camera_models/camera_calib_example/calibrationdata/left-0065.png
new file mode 100644
index 0000000..709f15b
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0065.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0066.png b/camera_models/camera_calib_example/calibrationdata/left-0066.png
new file mode 100644
index 0000000..943c5ac
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0066.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0067.png b/camera_models/camera_calib_example/calibrationdata/left-0067.png
new file mode 100644
index 0000000..d56b91e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0067.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0068.png b/camera_models/camera_calib_example/calibrationdata/left-0068.png
new file mode 100644
index 0000000..33bcbff
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0068.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0069.png b/camera_models/camera_calib_example/calibrationdata/left-0069.png
new file mode 100644
index 0000000..07d9e14
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0069.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0070.png b/camera_models/camera_calib_example/calibrationdata/left-0070.png
new file mode 100644
index 0000000..cf1926e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0070.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0071.png b/camera_models/camera_calib_example/calibrationdata/left-0071.png
new file mode 100644
index 0000000..0d739b2
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0071.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0072.png b/camera_models/camera_calib_example/calibrationdata/left-0072.png
new file mode 100644
index 0000000..3bba0a8
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0072.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0073.png b/camera_models/camera_calib_example/calibrationdata/left-0073.png
new file mode 100644
index 0000000..e8bd938
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0073.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0074.png b/camera_models/camera_calib_example/calibrationdata/left-0074.png
new file mode 100644
index 0000000..9aa586e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0074.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0075.png b/camera_models/camera_calib_example/calibrationdata/left-0075.png
new file mode 100644
index 0000000..36923a5
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0075.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0076.png b/camera_models/camera_calib_example/calibrationdata/left-0076.png
new file mode 100644
index 0000000..3a6c4f4
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0076.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0077.png b/camera_models/camera_calib_example/calibrationdata/left-0077.png
new file mode 100644
index 0000000..7e75ec2
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0077.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0078.png b/camera_models/camera_calib_example/calibrationdata/left-0078.png
new file mode 100644
index 0000000..da9ab4c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0078.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0079.png b/camera_models/camera_calib_example/calibrationdata/left-0079.png
new file mode 100644
index 0000000..0b47d1c
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0079.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0080.png b/camera_models/camera_calib_example/calibrationdata/left-0080.png
new file mode 100644
index 0000000..c7d9376
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0080.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0081.png b/camera_models/camera_calib_example/calibrationdata/left-0081.png
new file mode 100644
index 0000000..a06ada4
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0081.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0082.png b/camera_models/camera_calib_example/calibrationdata/left-0082.png
new file mode 100644
index 0000000..d77971e
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0082.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0083.png b/camera_models/camera_calib_example/calibrationdata/left-0083.png
new file mode 100644
index 0000000..b931338
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0083.png differ
diff --git a/camera_models/camera_calib_example/calibrationdata/left-0084.png b/camera_models/camera_calib_example/calibrationdata/left-0084.png
new file mode 100644
index 0000000..a01d4cd
Binary files /dev/null and b/camera_models/camera_calib_example/calibrationdata/left-0084.png differ
diff --git a/camera_models/camera_calib_example/kannala-brandt_result/camera_camera_calib.yaml b/camera_models/camera_calib_example/kannala-brandt_result/camera_camera_calib.yaml
new file mode 100644
index 0000000..8a5787c
--- /dev/null
+++ b/camera_models/camera_calib_example/kannala-brandt_result/camera_camera_calib.yaml
@@ -0,0 +1,15 @@
+%YAML:1.0
+---
+model_type: KANNALA_BRANDT
+camera_name: camera
+image_width: 752
+image_height: 480
+projection_parameters:
+ k2: 3.0514929333970435e-03
+ k3: 1.1408907470112554e-02
+ k4: -1.8120028556338826e-02
+ k5: 1.3690329192990333e-02
+ mu: 4.6936828350396729e+02
+ mv: 4.7009150903673395e+02
+ u0: 3.5252230051016619e+02
+ v0: 2.2951671053317494e+02
diff --git a/camera_models/camera_calib_example/kannala-brandt_result/camera_chessboard_data.dat b/camera_models/camera_calib_example/kannala-brandt_result/camera_chessboard_data.dat
new file mode 100644
index 0000000..97ff434
Binary files /dev/null and b/camera_models/camera_calib_example/kannala-brandt_result/camera_chessboard_data.dat differ
diff --git a/camera_models/camera_calib_example/mei_result/camera_camera_calib.yaml b/camera_models/camera_calib_example/mei_result/camera_camera_calib.yaml
new file mode 100644
index 0000000..4937b7b
--- /dev/null
+++ b/camera_models/camera_calib_example/mei_result/camera_camera_calib.yaml
@@ -0,0 +1,18 @@
+%YAML:1.0
+---
+model_type: MEI
+camera_name: camera
+image_width: 752
+image_height: 480
+mirror_parameters:
+ xi: 1.8087217573513699e+00
+distortion_parameters:
+ k1: -5.5722149398730970e-02
+ k2: 5.2830313708627064e-01
+ p1: -3.5759477628336721e-04
+ p2: -2.5507955155386390e-04
+projection_parameters:
+ gamma1: 1.3182310349674190e+03
+ gamma2: 1.3201698352651651e+03
+ u0: 3.5263370550000445e+02
+ v0: 2.2968719113611769e+02
diff --git a/camera_models/camera_calib_example/mei_result/camera_chessboard_data.dat b/camera_models/camera_calib_example/mei_result/camera_chessboard_data.dat
new file mode 100644
index 0000000..30f9ac7
Binary files /dev/null and b/camera_models/camera_calib_example/mei_result/camera_chessboard_data.dat differ
diff --git a/camera_models/camera_calib_example/pinhole_result/camera_camera_calib.yaml b/camera_models/camera_calib_example/pinhole_result/camera_camera_calib.yaml
new file mode 100644
index 0000000..23db253
--- /dev/null
+++ b/camera_models/camera_calib_example/pinhole_result/camera_camera_calib.yaml
@@ -0,0 +1,16 @@
+%YAML:1.0
+---
+model_type: PINHOLE
+camera_name: camera
+image_width: 752
+image_height: 480
+distortion_parameters:
+ k1: -2.9839715720368004e-01
+ k2: 9.2224519780383304e-02
+ p1: -1.1990340691398964e-04
+ p2: -7.4597868881593333e-05
+projection_parameters:
+ fx: 4.6894439430045270e+02
+ fy: 4.6967991177317379e+02
+ cx: 3.5308385782265555e+02
+ cy: 2.2974410036894824e+02
diff --git a/camera_models/camera_calib_example/pinhole_result/camera_chessboard_data.dat b/camera_models/camera_calib_example/pinhole_result/camera_chessboard_data.dat
new file mode 100644
index 0000000..915eddf
Binary files /dev/null and b/camera_models/camera_calib_example/pinhole_result/camera_chessboard_data.dat differ
diff --git a/camera_models/camera_calib_example/readme.txt b/camera_models/camera_calib_example/readme.txt
new file mode 100644
index 0000000..f249647
--- /dev/null
+++ b/camera_models/camera_calib_example/readme.txt
@@ -0,0 +1,8 @@
+# help for checking input parameters.
+rosrun camera_models Calibrations --help
+
+# example pinhole model.
+rosrun camera_models Calibrations -w 12 -h 8 -s 80 -i calibrationdata --camera-model pinhole
+
+# example mei model.
+rosrun camera_models Calibrations -w 12 -h 8 -s 80 -i calibrationdata --camera-model mei
diff --git a/camera_models/cmake/FindEigen.cmake b/camera_models/cmake/FindEigen.cmake
new file mode 100644
index 0000000..cdeb305
--- /dev/null
+++ b/camera_models/cmake/FindEigen.cmake
@@ -0,0 +1,172 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+# used to endorse or promote products derived from this software without
+# specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: alexs.mac@gmail.com (Alex Stewart)
+#
+
+# FindEigen.cmake - Find Eigen library, version >= 3.
+#
+# This module defines the following variables:
+#
+# EIGEN_FOUND: TRUE iff Eigen is found.
+# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
+#
+# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
+# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
+# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
+# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
+#
+# The following variables control the behaviour of this module:
+#
+# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
+# search for eigen includes, e.g: /timbuktu/eigen3.
+#
+# The following variables are also defined by this module, but in line with
+# CMake recommended FindPackage() module style should NOT be referenced directly
+# by callers (use the plural variables detailed above instead). These variables
+# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
+# are NOT re-called (i.e. search for library is not repeated) if these variables
+# are set with valid values _in the CMake cache_. This means that if these
+# variables are set directly in the cache, either by the user in the CMake GUI,
+# or by the user passing -DVAR=VALUE directives to CMake when called (which
+# explicitly defines a cache variable), then they will be used verbatim,
+# bypassing the HINTS variables and other hard-coded search locations.
+#
+# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
+# include directory of any dependencies.
+
+# Called if we failed to find Eigen or any of it's required dependencies,
+# unsets all public (designed to be used externally) variables and reports
+# error message at priority depending upon [REQUIRED/QUIET/] argument.
+macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
+ unset(EIGEN_FOUND)
+ unset(EIGEN_INCLUDE_DIRS)
+ # Make results of search visible in the CMake GUI if Eigen has not
+ # been found so that user does not have to toggle to advanced view.
+ mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
+ # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
+ # use the camelcase library name, not uppercase.
+ if (Eigen_FIND_QUIETLY)
+ message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+ elseif (Eigen_FIND_REQUIRED)
+ message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+ else()
+ # Neither QUIETLY nor REQUIRED, use no priority which emits a message
+ # but continues configuration and allows generation.
+ message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+ endif ()
+ return()
+endmacro(EIGEN_REPORT_NOT_FOUND)
+
+# Protect against any alternative find_package scripts for this library having
+# been called previously (in a client project) which set EIGEN_FOUND, but not
+# the other variables we require / set here which could cause the search logic
+# here to fail.
+unset(EIGEN_FOUND)
+
+# Search user-installed locations first, so that we prefer user installs
+# to system installs where both exist.
+list(APPEND EIGEN_CHECK_INCLUDE_DIRS
+ /usr/local/include
+ /usr/local/homebrew/include # Mac OS X
+ /opt/local/var/macports/software # Mac OS X.
+ /opt/local/include
+ /usr/include)
+# Additional suffixes to try appending to each search path.
+list(APPEND EIGEN_CHECK_PATH_SUFFIXES
+ eigen3 # Default root directory for Eigen.
+ Eigen/include/eigen3 # Windows (for C:/Program Files prefix) < 3.3
+ Eigen3/include/eigen3 ) # Windows (for C:/Program Files prefix) >= 3.3
+
+# Search supplied hint directories first if supplied.
+find_path(EIGEN_INCLUDE_DIR
+ NAMES Eigen/Core
+ PATHS ${EIGEN_INCLUDE_DIR_HINTS}
+ ${EIGEN_CHECK_INCLUDE_DIRS}
+ PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
+
+if (NOT EIGEN_INCLUDE_DIR OR
+ NOT EXISTS ${EIGEN_INCLUDE_DIR})
+ eigen_report_not_found(
+ "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
+ "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
+endif (NOT EIGEN_INCLUDE_DIR OR
+ NOT EXISTS ${EIGEN_INCLUDE_DIR})
+
+# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
+# if called.
+set(EIGEN_FOUND TRUE)
+
+# Extract Eigen version from Eigen/src/Core/util/Macros.h
+if (EIGEN_INCLUDE_DIR)
+ set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
+ if (NOT EXISTS ${EIGEN_VERSION_FILE})
+ eigen_report_not_found(
+ "Could not find file: ${EIGEN_VERSION_FILE} "
+ "containing version information in Eigen install located at: "
+ "${EIGEN_INCLUDE_DIR}.")
+ else (NOT EXISTS ${EIGEN_VERSION_FILE})
+ file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
+
+ string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
+ EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+ string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
+ EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
+
+ string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
+ EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+ string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
+ EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
+
+ string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
+ EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+ string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
+ EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
+
+ # This is on a single line s/t CMake does not interpret it as a list of
+ # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
+ set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
+ endif (NOT EXISTS ${EIGEN_VERSION_FILE})
+endif (EIGEN_INCLUDE_DIR)
+
+# Set standard CMake FindPackage variables if found.
+if (EIGEN_FOUND)
+ set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
+endif (EIGEN_FOUND)
+
+# Handle REQUIRED / QUIET optional arguments and version.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen
+ REQUIRED_VARS EIGEN_INCLUDE_DIRS
+ VERSION_VAR EIGEN_VERSION)
+
+# Only mark internal variables as advanced if we found Eigen, otherwise
+# leave it visible in the standard GUI for the user to set manually.
+if (EIGEN_FOUND)
+ mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
+endif (EIGEN_FOUND)
diff --git a/camera_models/include/camodocal/calib/CameraCalibration.h b/camera_models/include/camodocal/calib/CameraCalibration.h
new file mode 100644
index 0000000..ac80f69
--- /dev/null
+++ b/camera_models/include/camodocal/calib/CameraCalibration.h
@@ -0,0 +1,81 @@
+#ifndef CAMERACALIBRATION_H
+#define CAMERACALIBRATION_H
+
+#include
+
+#include "camodocal/camera_models/Camera.h"
+
+namespace camodocal
+{
+
+class CameraCalibration
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ CameraCalibration();
+
+ CameraCalibration(Camera::ModelType modelType,
+ const std::string& cameraName,
+ const cv::Size& imageSize,
+ const cv::Size& boardSize,
+ float squareSize);
+
+ void clear(void);
+
+ void addChessboardData(const std::vector& corners);
+
+ bool calibrate(void);
+
+ int sampleCount(void) const;
+ std::vector >& imagePoints(void);
+ const std::vector >& imagePoints(void) const;
+ std::vector >& scenePoints(void);
+ const std::vector >& scenePoints(void) const;
+ CameraPtr& camera(void);
+ const CameraConstPtr camera(void) const;
+
+ Eigen::Matrix2d& measurementCovariance(void);
+ const Eigen::Matrix2d& measurementCovariance(void) const;
+
+ cv::Mat& cameraPoses(void);
+ const cv::Mat& cameraPoses(void) const;
+
+ void drawResults(std::vector& images) const;
+
+ void writeParams(const std::string& filename) const;
+
+ bool writeChessboardData(const std::string& filename) const;
+ bool readChessboardData(const std::string& filename);
+
+ void setVerbose(bool verbose);
+
+private:
+ bool calibrateHelper(CameraPtr& camera,
+ std::vector& rvecs, std::vector& tvecs) const;
+
+ void optimize(CameraPtr& camera,
+ std::vector& rvecs, std::vector& tvecs) const;
+
+ template
+ void readData(std::ifstream& ifs, T& data) const;
+
+ template
+ void writeData(std::ofstream& ofs, T data) const;
+
+ cv::Size m_boardSize;
+ float m_squareSize;
+
+ CameraPtr m_camera;
+ cv::Mat m_cameraPoses;
+
+ std::vector > m_imagePoints;
+ std::vector > m_scenePoints;
+
+ Eigen::Matrix2d m_measurementCovariance;
+
+ bool m_verbose;
+};
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/Camera.h b/camera_models/include/camodocal/camera_models/Camera.h
new file mode 100644
index 0000000..68e2bd7
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/Camera.h
@@ -0,0 +1,148 @@
+#ifndef CAMERA_H
+#define CAMERA_H
+
+#include
+#include
+#include
+#include
+
+namespace camodocal
+{
+
+class Camera
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ enum ModelType
+ {
+ KANNALA_BRANDT,
+ MEI,
+ PINHOLE,
+ PINHOLE_FULL,
+ SCARAMUZZA
+ };
+
+ class Parameters
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ Parameters( ModelType modelType );
+
+ Parameters( ModelType modelType, const std::string& cameraName, int w, int h );
+
+ ModelType& modelType( void );
+ std::string& cameraName( void );
+ int& imageWidth( void );
+ int& imageHeight( void );
+
+ ModelType modelType( void ) const;
+ const std::string& cameraName( void ) const;
+ int imageWidth( void ) const;
+ int imageHeight( void ) const;
+
+ int nIntrinsics( void ) const;
+
+ virtual bool readFromYamlFile( const std::string& filename ) = 0;
+ virtual void writeToYamlFile( const std::string& filename ) const = 0;
+
+ protected:
+ ModelType m_modelType;
+ int m_nIntrinsics;
+ std::string m_cameraName;
+ int m_imageWidth;
+ int m_imageHeight;
+ };
+
+ virtual ModelType modelType( void ) const = 0;
+ virtual const std::string& cameraName( void ) const = 0;
+ virtual int imageWidth( void ) const = 0;
+ virtual int imageHeight( void ) const = 0;
+
+ virtual cv::Mat& mask( void );
+ virtual const cv::Mat& mask( void ) const;
+
+ virtual void estimateIntrinsics( const cv::Size& boardSize,
+ const std::vector< std::vector< cv::Point3f > >& objectPoints,
+ const std::vector< std::vector< cv::Point2f > >& imagePoints )
+ = 0;
+ virtual void estimateExtrinsics( const std::vector< cv::Point3f >& objectPoints,
+ const std::vector< cv::Point2f >& imagePoints,
+ cv::Mat& rvec,
+ cv::Mat& tvec ) const;
+
+ // Lift points from the image plane to the sphere
+ virtual void liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const = 0;
+ //%output P
+
+ // Lift points from the image plane to the projective space
+ virtual void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const = 0;
+ //%output P
+
+ // Projects 3D points to the image plane (Pi function)
+ virtual void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const = 0;
+ //%output p
+
+ // Projects 3D points to the image plane (Pi function)
+ // and calculates jacobian
+ // virtual void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+ // Eigen::Matrix& J) const = 0;
+ //%output p
+ //%output J
+
+ virtual void undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const = 0;
+ //%output p
+
+ // virtual void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0)
+ // const = 0;
+ virtual cv::Mat initUndistortRectifyMap( cv::Mat& map1,
+ cv::Mat& map2,
+ float fx = -1.0f,
+ float fy = -1.0f,
+ cv::Size imageSize = cv::Size( 0, 0 ),
+ float cx = -1.0f,
+ float cy = -1.0f,
+ cv::Mat rmat = cv::Mat::eye( 3, 3, CV_32F ) ) const = 0;
+
+ virtual int parameterCount( void ) const = 0;
+
+ virtual void readParameters( const std::vector< double >& parameters ) = 0;
+ virtual void writeParameters( std::vector< double >& parameters ) const = 0;
+
+ virtual void writeParametersToYamlFile( const std::string& filename ) const = 0;
+
+ virtual std::string parametersToString( void ) const = 0;
+
+ /**
+ * \brief Calculates the reprojection distance between points
+ *
+ * \param P1 first 3D point coordinates
+ * \param P2 second 3D point coordinates
+ * \return euclidean distance in the plane
+ */
+ double reprojectionDist( const Eigen::Vector3d& P1, const Eigen::Vector3d& P2 ) const;
+
+ double reprojectionError( const std::vector< std::vector< cv::Point3f > >& objectPoints,
+ const std::vector< std::vector< cv::Point2f > >& imagePoints,
+ const std::vector< cv::Mat >& rvecs,
+ const std::vector< cv::Mat >& tvecs,
+ cv::OutputArray perViewErrors = cv::noArray( ) ) const;
+
+ double reprojectionError( const Eigen::Vector3d& P,
+ const Eigen::Quaterniond& camera_q,
+ const Eigen::Vector3d& camera_t,
+ const Eigen::Vector2d& observed_p ) const;
+
+ void projectPoints( const std::vector< cv::Point3f >& objectPoints,
+ const cv::Mat& rvec,
+ const cv::Mat& tvec,
+ std::vector< cv::Point2f >& imagePoints ) const;
+
+ protected:
+ cv::Mat m_mask;
+};
+
+typedef boost::shared_ptr< Camera > CameraPtr;
+typedef boost::shared_ptr< const Camera > CameraConstPtr;
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/CameraFactory.h b/camera_models/include/camodocal/camera_models/CameraFactory.h
new file mode 100644
index 0000000..e3c2f28
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/CameraFactory.h
@@ -0,0 +1,32 @@
+#ifndef CAMERAFACTORY_H
+#define CAMERAFACTORY_H
+
+#include
+#include
+
+#include "camodocal/camera_models/Camera.h"
+
+namespace camodocal
+{
+
+class CameraFactory
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ CameraFactory();
+
+ static boost::shared_ptr instance(void);
+
+ CameraPtr generateCamera(Camera::ModelType modelType,
+ const std::string& cameraName,
+ cv::Size imageSize) const;
+
+ CameraPtr generateCameraFromYamlFile(const std::string& filename);
+
+private:
+ static boost::shared_ptr m_instance;
+};
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/CataCamera.h b/camera_models/include/camodocal/camera_models/CataCamera.h
new file mode 100644
index 0000000..14384a5
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/CataCamera.h
@@ -0,0 +1,210 @@
+#ifndef CATACAMERA_H
+#define CATACAMERA_H
+
+#include
+#include
+
+#include "ceres/rotation.h"
+#include "Camera.h"
+
+namespace camodocal
+{
+
+/**
+ * C. Mei, and P. Rives, Single View Point Omnidirectional Camera Calibration
+ * from Planar Grids, ICRA 2007
+ */
+
+class CataCamera: public Camera
+{
+public:
+ class Parameters: public Camera::Parameters
+ {
+ public:
+ Parameters();
+ Parameters(const std::string& cameraName,
+ int w, int h,
+ double xi,
+ double k1, double k2, double p1, double p2,
+ double gamma1, double gamma2, double u0, double v0);
+
+ double& xi(void);
+ double& k1(void);
+ double& k2(void);
+ double& p1(void);
+ double& p2(void);
+ double& gamma1(void);
+ double& gamma2(void);
+ double& u0(void);
+ double& v0(void);
+
+ double xi(void) const;
+ double k1(void) const;
+ double k2(void) const;
+ double p1(void) const;
+ double p2(void) const;
+ double gamma1(void) const;
+ double gamma2(void) const;
+ double u0(void) const;
+ double v0(void) const;
+
+ bool readFromYamlFile(const std::string& filename);
+ void writeToYamlFile(const std::string& filename) const;
+
+ Parameters& operator=(const Parameters& other);
+ friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
+
+ private:
+ double m_xi;
+ double m_k1;
+ double m_k2;
+ double m_p1;
+ double m_p2;
+ double m_gamma1;
+ double m_gamma2;
+ double m_u0;
+ double m_v0;
+ };
+
+ CataCamera();
+
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ CataCamera(const std::string& cameraName,
+ int imageWidth, int imageHeight,
+ double xi, double k1, double k2, double p1, double p2,
+ double gamma1, double gamma2, double u0, double v0);
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ CataCamera(const Parameters& params);
+
+ Camera::ModelType modelType(void) const;
+ const std::string& cameraName(void) const;
+ int imageWidth(void) const;
+ int imageHeight(void) const;
+
+ void estimateIntrinsics(const cv::Size& boardSize,
+ const std::vector< std::vector >& objectPoints,
+ const std::vector< std::vector >& imagePoints);
+
+ // Lift points from the image plane to the sphere
+ void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Lift points from the image plane to the projective space
+ void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Projects 3D points to the image plane (Pi function)
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
+ //%output p
+
+ // Projects 3D points to the image plane (Pi function)
+ // and calculates jacobian
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+ Eigen::Matrix& J) const;
+ //%output p
+ //%output J
+
+ void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
+ //%output p
+
+ template
+ static void spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p);
+
+ void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;
+ void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,
+ Eigen::Matrix2d& J) const;
+
+ void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
+ cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
+ float fx = -1.0f, float fy = -1.0f,
+ cv::Size imageSize = cv::Size(0, 0),
+ float cx = -1.0f, float cy = -1.0f,
+ cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
+
+ int parameterCount(void) const;
+
+ const Parameters& getParameters(void) const;
+ void setParameters(const Parameters& parameters);
+
+ void readParameters(const std::vector& parameterVec);
+ void writeParameters(std::vector& parameterVec) const;
+
+ void writeParametersToYamlFile(const std::string& filename) const;
+
+ std::string parametersToString(void) const;
+
+private:
+ Parameters mParameters;
+
+ double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+ bool m_noDistortion;
+};
+
+typedef boost::shared_ptr CataCameraPtr;
+typedef boost::shared_ptr CataCameraConstPtr;
+
+template
+void
+CataCamera::spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p)
+{
+ T P_w[3];
+ P_w[0] = T(P(0));
+ P_w[1] = T(P(1));
+ P_w[2] = T(P(2));
+
+ // Convert quaternion from Eigen convention (x, y, z, w)
+ // to Ceres convention (w, x, y, z)
+ T q_ceres[4] = {q[3], q[0], q[1], q[2]};
+
+ T P_c[3];
+ ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+ P_c[0] += t[0];
+ P_c[1] += t[1];
+ P_c[2] += t[2];
+
+ // project 3D object point to the image plane
+ T xi = params[0];
+ T k1 = params[1];
+ T k2 = params[2];
+ T p1 = params[3];
+ T p2 = params[4];
+ T gamma1 = params[5];
+ T gamma2 = params[6];
+ T alpha = T(0); //cameraParams.alpha();
+ T u0 = params[7];
+ T v0 = params[8];
+
+ // Transform to model plane
+ T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
+ P_c[0] /= len;
+ P_c[1] /= len;
+ P_c[2] /= len;
+
+ T u = P_c[0] / (P_c[2] + xi);
+ T v = P_c[1] / (P_c[2] + xi);
+
+ T rho_sqr = u * u + v * v;
+ T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
+ T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
+ T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;
+
+ u = L * u + du;
+ v = L * v + dv;
+ p(0) = gamma1 * (u + alpha * v) + u0;
+ p(1) = gamma2 * v + v0;
+}
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/CostFunctionFactory.h b/camera_models/include/camodocal/camera_models/CostFunctionFactory.h
new file mode 100644
index 0000000..40260da
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/CostFunctionFactory.h
@@ -0,0 +1,82 @@
+#ifndef COSTFUNCTIONFACTORY_H
+#define COSTFUNCTIONFACTORY_H
+
+#include
+#include
+
+#include "camodocal/camera_models/Camera.h"
+
+namespace ceres
+{
+ class CostFunction;
+}
+
+namespace camodocal
+{
+
+enum
+{
+ CAMERA_INTRINSICS = 1 << 0,
+ CAMERA_POSE = 1 << 1,
+ POINT_3D = 1 << 2,
+ ODOMETRY_INTRINSICS = 1 << 3,
+ ODOMETRY_3D_POSE = 1 << 4,
+ ODOMETRY_6D_POSE = 1 << 5,
+ CAMERA_ODOMETRY_TRANSFORM = 1 << 6
+};
+
+class CostFunctionFactory
+{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ CostFunctionFactory();
+
+ static boost::shared_ptr instance(void);
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
+ const Eigen::Vector3d& observed_P,
+ const Eigen::Vector2d& observed_p,
+ int flags) const;
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
+ const Eigen::Vector3d& observed_P,
+ const Eigen::Vector2d& observed_p,
+ const Eigen::Matrix2d& sqrtPrecisionMat,
+ int flags) const;
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
+ const Eigen::Vector2d& observed_p,
+ int flags, bool optimize_cam_odo_z = true) const;
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
+ const Eigen::Vector2d& observed_p,
+ const Eigen::Matrix2d& sqrtPrecisionMat,
+ int flags, bool optimize_cam_odo_z = true) const;
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
+ const Eigen::Vector3d& odo_pos,
+ const Eigen::Vector3d& odo_att,
+ const Eigen::Vector2d& observed_p,
+ int flags, bool optimize_cam_odo_z = true) const;
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera,
+ const Eigen::Quaterniond& cam_odo_q,
+ const Eigen::Vector3d& cam_odo_t,
+ const Eigen::Vector3d& odo_pos,
+ const Eigen::Vector3d& odo_att,
+ const Eigen::Vector2d& observed_p,
+ int flags) const;
+
+ ceres::CostFunction* generateCostFunction(const CameraConstPtr& cameraLeft,
+ const CameraConstPtr& cameraRight,
+ const Eigen::Vector3d& observed_P,
+ const Eigen::Vector2d& observed_p_left,
+ const Eigen::Vector2d& observed_p_right) const;
+
+private:
+ static boost::shared_ptr m_instance;
+};
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/EquidistantCamera.h b/camera_models/include/camodocal/camera_models/EquidistantCamera.h
new file mode 100644
index 0000000..4e172d3
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/EquidistantCamera.h
@@ -0,0 +1,215 @@
+#ifndef EQUIDISTANTCAMERA_H
+#define EQUIDISTANTCAMERA_H
+
+#include
+#include
+
+#include "ceres/rotation.h"
+#include "Camera.h"
+
+namespace camodocal
+{
+
+/**
+ * J. Kannala, and S. Brandt, A Generic Camera Model and Calibration Method
+ * for Conventional, Wide-Angle, and Fish-Eye Lenses, PAMI 2006
+ */
+
+class EquidistantCamera: public Camera
+{
+public:
+ class Parameters: public Camera::Parameters
+ {
+ public:
+ Parameters();
+ Parameters(const std::string& cameraName,
+ int w, int h,
+ double k2, double k3, double k4, double k5,
+ double mu, double mv,
+ double u0, double v0);
+
+ double& k2(void);
+ double& k3(void);
+ double& k4(void);
+ double& k5(void);
+ double& mu(void);
+ double& mv(void);
+ double& u0(void);
+ double& v0(void);
+
+ double k2(void) const;
+ double k3(void) const;
+ double k4(void) const;
+ double k5(void) const;
+ double mu(void) const;
+ double mv(void) const;
+ double u0(void) const;
+ double v0(void) const;
+
+ bool readFromYamlFile(const std::string& filename);
+ void writeToYamlFile(const std::string& filename) const;
+
+ Parameters& operator=(const Parameters& other);
+ friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
+
+ private:
+ // projection
+ double m_k2;
+ double m_k3;
+ double m_k4;
+ double m_k5;
+
+ double m_mu;
+ double m_mv;
+ double m_u0;
+ double m_v0;
+ };
+
+ EquidistantCamera();
+
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ EquidistantCamera(const std::string& cameraName,
+ int imageWidth, int imageHeight,
+ double k2, double k3, double k4, double k5,
+ double mu, double mv,
+ double u0, double v0);
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ EquidistantCamera(const Parameters& params);
+
+ Camera::ModelType modelType(void) const;
+ const std::string& cameraName(void) const;
+ int imageWidth(void) const;
+ int imageHeight(void) const;
+
+ void estimateIntrinsics(const cv::Size& boardSize,
+ const std::vector< std::vector >& objectPoints,
+ const std::vector< std::vector >& imagePoints);
+
+ // Lift points from the image plane to the sphere
+ virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Lift points from the image plane to the projective space
+ void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Projects 3D points to the image plane (Pi function)
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
+ //%output p
+
+ // Projects 3D points to the image plane (Pi function)
+ // and calculates jacobian
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+ Eigen::Matrix& J) const;
+ //%output p
+ //%output J
+
+ void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
+ //%output p
+
+ template
+ static void spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p);
+
+ void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
+ cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
+ float fx = -1.0f, float fy = -1.0f,
+ cv::Size imageSize = cv::Size(0, 0),
+ float cx = -1.0f, float cy = -1.0f,
+ cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
+
+ int parameterCount(void) const;
+
+ const Parameters& getParameters(void) const;
+ void setParameters(const Parameters& parameters);
+
+ void readParameters(const std::vector& parameterVec);
+ void writeParameters(std::vector& parameterVec) const;
+
+ void writeParametersToYamlFile(const std::string& filename) const;
+
+ std::string parametersToString(void) const;
+
+private:
+ template
+ static T r(T k2, T k3, T k4, T k5, T theta);
+
+
+ void fitOddPoly(const std::vector& x, const std::vector& y,
+ int n, std::vector& coeffs) const;
+
+ void backprojectSymmetric(const Eigen::Vector2d& p_u,
+ double& theta, double& phi) const;
+
+ Parameters mParameters;
+
+ double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+};
+
+typedef boost::shared_ptr EquidistantCameraPtr;
+typedef boost::shared_ptr EquidistantCameraConstPtr;
+
+template
+T
+EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta)
+{
+ // k1 = 1
+ return theta +
+ k2 * theta * theta * theta +
+ k3 * theta * theta * theta * theta * theta +
+ k4 * theta * theta * theta * theta * theta * theta * theta +
+ k5 * theta * theta * theta * theta * theta * theta * theta * theta * theta;
+}
+
+template
+void
+EquidistantCamera::spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p)
+{
+ T P_w[3];
+ P_w[0] = T(P(0));
+ P_w[1] = T(P(1));
+ P_w[2] = T(P(2));
+
+ // Convert quaternion from Eigen convention (x, y, z, w)
+ // to Ceres convention (w, x, y, z)
+ T q_ceres[4] = {q[3], q[0], q[1], q[2]};
+
+ T P_c[3];
+ ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+ P_c[0] += t[0];
+ P_c[1] += t[1];
+ P_c[2] += t[2];
+
+ // project 3D object point to the image plane;
+ T k2 = params[0];
+ T k3 = params[1];
+ T k4 = params[2];
+ T k5 = params[3];
+ T mu = params[4];
+ T mv = params[5];
+ T u0 = params[6];
+ T v0 = params[7];
+
+ T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]);
+ T theta = acos(P_c[2] / len);
+ T phi = atan2(P_c[1], P_c[0]);
+
+ Eigen::Matrix p_u = r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi));
+
+ p(0) = mu * p_u(0) + u0;
+ p(1) = mv * p_u(1) + v0;
+}
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/PinholeCamera.h b/camera_models/include/camodocal/camera_models/PinholeCamera.h
new file mode 100644
index 0000000..f368a0b
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/PinholeCamera.h
@@ -0,0 +1,196 @@
+#ifndef PINHOLECAMERA_H
+#define PINHOLECAMERA_H
+
+#include
+#include
+
+#include "ceres/rotation.h"
+#include "Camera.h"
+
+namespace camodocal
+{
+
+class PinholeCamera: public Camera
+{
+public:
+ class Parameters: public Camera::Parameters
+ {
+ public:
+ Parameters();
+ Parameters(const std::string& cameraName,
+ int w, int h,
+ double k1, double k2, double p1, double p2,
+ double fx, double fy, double cx, double cy);
+
+ double& k1(void);
+ double& k2(void);
+ double& p1(void);
+ double& p2(void);
+ double& fx(void);
+ double& fy(void);
+ double& cx(void);
+ double& cy(void);
+
+ double xi(void) const;
+ double k1(void) const;
+ double k2(void) const;
+ double p1(void) const;
+ double p2(void) const;
+ double fx(void) const;
+ double fy(void) const;
+ double cx(void) const;
+ double cy(void) const;
+
+ bool readFromYamlFile(const std::string& filename);
+ void writeToYamlFile(const std::string& filename) const;
+
+ Parameters& operator=(const Parameters& other);
+ friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
+
+ private:
+ double m_k1;
+ double m_k2;
+ double m_p1;
+ double m_p2;
+ double m_fx;
+ double m_fy;
+ double m_cx;
+ double m_cy;
+ };
+
+ PinholeCamera();
+
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ PinholeCamera(const std::string& cameraName,
+ int imageWidth, int imageHeight,
+ double k1, double k2, double p1, double p2,
+ double fx, double fy, double cx, double cy);
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ PinholeCamera(const Parameters& params);
+
+ Camera::ModelType modelType(void) const;
+ const std::string& cameraName(void) const;
+ int imageWidth(void) const;
+ int imageHeight(void) const;
+
+ void estimateIntrinsics(const cv::Size& boardSize,
+ const std::vector< std::vector >& objectPoints,
+ const std::vector< std::vector >& imagePoints);
+
+ // Lift points from the image plane to the sphere
+ virtual void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Lift points from the image plane to the projective space
+ void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Projects 3D points to the image plane (Pi function)
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
+ //%output p
+
+ // Projects 3D points to the image plane (Pi function)
+ // and calculates jacobian
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+ Eigen::Matrix& J) const;
+ //%output p
+ //%output J
+
+ void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
+ //%output p
+
+ template
+ static void spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p);
+
+ void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const;
+ void distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u,
+ Eigen::Matrix2d& J) const;
+
+ void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
+ cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
+ float fx = -1.0f, float fy = -1.0f,
+ cv::Size imageSize = cv::Size(0, 0),
+ float cx = -1.0f, float cy = -1.0f,
+ cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
+
+ int parameterCount(void) const;
+
+ const Parameters& getParameters(void) const;
+ void setParameters(const Parameters& parameters);
+
+ void readParameters(const std::vector& parameterVec);
+ void writeParameters(std::vector& parameterVec) const;
+
+ void writeParametersToYamlFile(const std::string& filename) const;
+
+ std::string parametersToString(void) const;
+
+private:
+ Parameters mParameters;
+
+ double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+ bool m_noDistortion;
+};
+
+typedef boost::shared_ptr PinholeCameraPtr;
+typedef boost::shared_ptr PinholeCameraConstPtr;
+
+template
+void
+PinholeCamera::spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p)
+{
+ T P_w[3];
+ P_w[0] = T(P(0));
+ P_w[1] = T(P(1));
+ P_w[2] = T(P(2));
+
+ // Convert quaternion from Eigen convention (x, y, z, w)
+ // to Ceres convention (w, x, y, z)
+ T q_ceres[4] = {q[3], q[0], q[1], q[2]};
+
+ T P_c[3];
+ ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+ P_c[0] += t[0];
+ P_c[1] += t[1];
+ P_c[2] += t[2];
+
+ // project 3D object point to the image plane
+ T k1 = params[0];
+ T k2 = params[1];
+ T p1 = params[2];
+ T p2 = params[3];
+ T fx = params[4];
+ T fy = params[5];
+ T alpha = T(0); //cameraParams.alpha();
+ T cx = params[6];
+ T cy = params[7];
+
+ // Transform to model plane
+ T u = P_c[0] / P_c[2];
+ T v = P_c[1] / P_c[2];
+
+ T rho_sqr = u * u + v * v;
+ T L = T(1.0) + k1 * rho_sqr + k2 * rho_sqr * rho_sqr;
+ T du = T(2.0) * p1 * u * v + p2 * (rho_sqr + T(2.0) * u * u);
+ T dv = p1 * (rho_sqr + T(2.0) * v * v) + T(2.0) * p2 * u * v;
+
+ u = L * u + du;
+ v = L * v + dv;
+ p(0) = fx * (u + alpha * v) + cx;
+ p(1) = fy * v + cy;
+}
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/PinholeFullCamera.h b/camera_models/include/camodocal/camera_models/PinholeFullCamera.h
new file mode 100644
index 0000000..b391c8a
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/PinholeFullCamera.h
@@ -0,0 +1,274 @@
+#ifndef PinholeFullCAMERA_H
+#define PinholeFullCAMERA_H
+
+#include
+#include
+
+#include "Camera.h"
+#include "ceres/rotation.h"
+
+namespace camodocal
+{
+
+class PinholeFullCamera : public Camera
+{
+ public:
+ class Parameters : public Camera::Parameters
+ {
+ public:
+ Parameters( );
+ Parameters( const std::string& cameraName,
+ int w,
+ int h,
+ double k1,
+ double k2,
+ double k3,
+ double k4,
+ double k5,
+ double k6,
+ double p1,
+ double p2,
+ double fx,
+ double fy,
+ double cx,
+ double cy );
+
+ double& k1( void );
+ double& k2( void );
+ double& k3( void );
+ double& k4( void );
+ double& k5( void );
+ double& k6( void );
+ double& p1( void );
+ double& p2( void );
+ double& fx( void );
+ double& fy( void );
+ double& cx( void );
+ double& cy( void );
+
+ double xi( void ) const;
+ double k1( void ) const;
+ double k2( void ) const;
+ double k3( void ) const;
+ double k4( void ) const;
+ double k5( void ) const;
+ double k6( void ) const;
+ double p1( void ) const;
+ double p2( void ) const;
+ double fx( void ) const;
+ double fy( void ) const;
+ double cx( void ) const;
+ double cy( void ) const;
+
+ bool readFromYamlFile( const std::string& filename );
+ void writeToYamlFile( const std::string& filename ) const;
+
+ Parameters& operator=( const Parameters& other );
+ friend std::ostream& operator<<( std::ostream& out, const Parameters& params );
+
+ private:
+ double m_k1;
+ double m_k2;
+ double m_p1;
+ double m_p2;
+ double m_fx;
+ double m_fy;
+ double m_cx;
+ double m_cy;
+ double m_k3;
+ double m_k4;
+ double m_k5;
+ double m_k6;
+ };
+
+ PinholeFullCamera( );
+
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ PinholeFullCamera( const std::string& cameraName,
+ int imageWidth,
+ int imageHeight,
+ double k1,
+ double k2,
+ double k3,
+ double k4,
+ double k5,
+ double k6,
+ double p1,
+ double p2,
+ double fx,
+ double fy,
+ double cx,
+ double cy );
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ PinholeFullCamera( const Parameters& params );
+
+ Camera::ModelType modelType( void ) const;
+ const std::string& cameraName( void ) const;
+ int imageWidth( void ) const;
+ int imageHeight( void ) const;
+ cv::Size imageSize( ) const { return cv::Size( imageWidth( ), imageHeight( ) ); }
+ cv::Point2f getPrinciple( ) const
+ {
+ return cv::Point2f( mParameters.cx( ), mParameters.cy( ) );
+ }
+
+ void estimateIntrinsics( const cv::Size& boardSize,
+ const std::vector< std::vector< cv::Point3f > >& objectPoints,
+ const std::vector< std::vector< cv::Point2f > >& imagePoints );
+
+ void setInitIntrinsics( const std::vector< std::vector< cv::Point3f > >& objectPoints,
+ const std::vector< std::vector< cv::Point2f > >& imagePoints )
+ {
+ Parameters params = getParameters( );
+
+ params.k1( ) = 0.0;
+ params.k2( ) = 0.0;
+ params.k3( ) = 0.0;
+ params.k4( ) = 0.0;
+ params.k5( ) = 0.0;
+ params.k6( ) = 0.0;
+ params.p1( ) = 0.0;
+ params.p2( ) = 0.0;
+
+ double cx = params.imageWidth( ) / 2.0;
+ double cy = params.imageHeight( ) / 2.0;
+ params.cx( ) = cx;
+ params.cy( ) = cy;
+ params.fx( ) = 1200;
+ params.fy( ) = 1200;
+
+ setParameters( params );
+ }
+
+ // Lift points from the image plane to the sphere
+ virtual void liftSphere( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const;
+ //%output P
+
+ // Lift points from the image plane to the projective space
+ void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P ) const;
+ //%output P
+
+ void liftProjective( const Eigen::Vector2d& p, Eigen::Vector3d& P, float image_scale ) const;
+
+ // Projects 3D points to the image plane (Pi function)
+ void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p ) const;
+ //%output p
+
+ void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, float image_scalse ) const;
+
+ // Projects 3D points to the image plane (Pi function)
+ // and calculates jacobian
+ void spaceToPlane( const Eigen::Vector3d& P, Eigen::Vector2d& p, Eigen::Matrix< double, 2, 3 >& J ) const;
+ //%output p
+ //%output J
+
+ void undistToPlane( const Eigen::Vector2d& p_u, Eigen::Vector2d& p ) const;
+ //%output p
+
+ template< typename T >
+ static void spaceToPlane( const T* const params,
+ const T* const q,
+ const T* const t,
+ const Eigen::Matrix< T, 3, 1 >& P,
+ Eigen::Matrix< T, 2, 1 >& p );
+
+ void distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u ) const;
+ void distortion( const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u, Eigen::Matrix2d& J ) const;
+
+ void initUndistortMap( cv::Mat& map1, cv::Mat& map2, double fScale = 1.0 ) const;
+ cv::Mat initUndistortRectifyMap( cv::Mat& map1,
+ cv::Mat& map2,
+ float fx = -1.0f,
+ float fy = -1.0f,
+ cv::Size imageSize = cv::Size( 0, 0 ),
+ float cx = -1.0f,
+ float cy = -1.0f,
+ cv::Mat rmat = cv::Mat::eye( 3, 3, CV_32F ) ) const;
+
+ int parameterCount( void ) const;
+
+ const Parameters& getParameters( void ) const;
+ void setParameters( const Parameters& parameters );
+
+ void readParameters( const std::vector< double >& parameterVec );
+ void writeParameters( std::vector< double >& parameterVec ) const;
+
+ void writeParametersToYamlFile( const std::string& filename ) const;
+
+ std::string parametersToString( void ) const;
+
+ private:
+ Parameters mParameters;
+
+ double m_inv_K11, m_inv_K13, m_inv_K22, m_inv_K23;
+ bool m_noDistortion;
+};
+
+typedef boost::shared_ptr< PinholeFullCamera > PinholeFullCameraPtr;
+typedef boost::shared_ptr< const PinholeFullCamera > PinholeFullCameraConstPtr;
+
+template< typename T >
+void
+PinholeFullCamera::spaceToPlane( const T* const params,
+ const T* const q,
+ const T* const t,
+ const Eigen::Matrix< T, 3, 1 >& P,
+ Eigen::Matrix< T, 2, 1 >& p )
+{
+ T P_w[3];
+ P_w[0] = T( P( 0 ) );
+ P_w[1] = T( P( 1 ) );
+ P_w[2] = T( P( 2 ) );
+
+ // Convert quaternion from Eigen convention (x, y, z, w)
+ // to Ceres convention (w, x, y, z)
+ T q_ceres[4] = { q[3], q[0], q[1], q[2] };
+
+ T P_c[3];
+ ceres::QuaternionRotatePoint( q_ceres, P_w, P_c );
+
+ P_c[0] += t[0];
+ P_c[1] += t[1];
+ P_c[2] += t[2];
+
+ // project 3D object point to the image plane
+ T k1 = params[0];
+ T k2 = params[1];
+ T k3 = params[2];
+ T k4 = params[3];
+ T k5 = params[4];
+ T k6 = params[5];
+ T p1 = params[6];
+ T p2 = params[7];
+ T fx = params[8];
+ T fy = params[9];
+ T alpha = T( 0 ); // cameraParams.alpha();
+ T cx = params[10];
+ T cy = params[11];
+
+ // Transform to model plane
+ T x = P_c[0] / P_c[2];
+ T y = P_c[1] / P_c[2];
+ // T z = P_c[2] / P_c[2];
+
+ T r2 = x * x + y * y;
+ T r4 = r2 * r2;
+ T r6 = r4 * r2;
+ T a1 = T( 2 ) * x * y;
+ T a2 = r2 + T( 2 ) * x * x;
+ T a3 = r2 + T( 2 ) * y * y;
+ T cdist = T( 1 ) + k1 * r2 + k2 * r4 + k3 * r6;
+ T icdist2 = T( 1. ) / ( T( 1 ) + k4 * r2 + k5 * r4 + k6 * r6 );
+ T xd0 = x * cdist * icdist2 + p1 * a1 + p2 * a2; // + k[8] * r2 + k[9] * r4;
+ T yd0 = y * cdist * icdist2 + p1 * a3 + p2 * a1; // + k[10] * r2 + k[11] * r4;
+
+ p( 0 ) = xd0 * fx + cx;
+ p( 1 ) = yd0 * fy + cy;
+}
+}
+
+#endif
diff --git a/camera_models/include/camodocal/camera_models/ScaramuzzaCamera.h b/camera_models/include/camodocal/camera_models/ScaramuzzaCamera.h
new file mode 100644
index 0000000..3d54aca
--- /dev/null
+++ b/camera_models/include/camodocal/camera_models/ScaramuzzaCamera.h
@@ -0,0 +1,348 @@
+#ifndef SCARAMUZZACAMERA_H
+#define SCARAMUZZACAMERA_H
+
+#include
+#include
+
+#include "ceres/rotation.h"
+#include "Camera.h"
+
+namespace camodocal
+{
+
+#define SCARAMUZZA_POLY_SIZE 5
+#define SCARAMUZZA_INV_POLY_SIZE 20
+
+#define SCARAMUZZA_CAMERA_NUM_PARAMS (SCARAMUZZA_POLY_SIZE + SCARAMUZZA_INV_POLY_SIZE + 2 /*center*/ + 3 /*affine*/)
+
+/**
+ * Scaramuzza Camera (Omnidirectional)
+ * https://sites.google.com/site/scarabotix/ocamcalib-toolbox
+ */
+
+class OCAMCamera: public Camera
+{
+public:
+ class Parameters: public Camera::Parameters
+ {
+ public:
+ Parameters();
+
+ double& C(void) { return m_C; }
+ double& D(void) { return m_D; }
+ double& E(void) { return m_E; }
+
+ double& center_x(void) { return m_center_x; }
+ double& center_y(void) { return m_center_y; }
+
+ double& poly(int idx) { return m_poly[idx]; }
+ double& inv_poly(int idx) { return m_inv_poly[idx]; }
+
+ double C(void) const { return m_C; }
+ double D(void) const { return m_D; }
+ double E(void) const { return m_E; }
+
+ double center_x(void) const { return m_center_x; }
+ double center_y(void) const { return m_center_y; }
+
+ double poly(int idx) const { return m_poly[idx]; }
+ double inv_poly(int idx) const { return m_inv_poly[idx]; }
+
+ bool readFromYamlFile(const std::string& filename);
+ void writeToYamlFile(const std::string& filename) const;
+
+ Parameters& operator=(const Parameters& other);
+ friend std::ostream& operator<< (std::ostream& out, const Parameters& params);
+
+ private:
+ double m_poly[SCARAMUZZA_POLY_SIZE];
+ double m_inv_poly[SCARAMUZZA_INV_POLY_SIZE];
+ double m_C;
+ double m_D;
+ double m_E;
+ double m_center_x;
+ double m_center_y;
+ };
+
+ OCAMCamera();
+
+ /**
+ * \brief Constructor from the projection model parameters
+ */
+ OCAMCamera(const Parameters& params);
+
+ Camera::ModelType modelType(void) const;
+ const std::string& cameraName(void) const;
+ int imageWidth(void) const;
+ int imageHeight(void) const;
+
+ void estimateIntrinsics(const cv::Size& boardSize,
+ const std::vector< std::vector >& objectPoints,
+ const std::vector< std::vector >& imagePoints);
+
+ // Lift points from the image plane to the sphere
+ void liftSphere(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Lift points from the image plane to the projective space
+ void liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const;
+ //%output P
+
+ // Projects 3D points to the image plane (Pi function)
+ void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p) const;
+ //%output p
+
+ // Projects 3D points to the image plane (Pi function)
+ // and calculates jacobian
+ //void spaceToPlane(const Eigen::Vector3d& P, Eigen::Vector2d& p,
+ // Eigen::Matrix& J) const;
+ //%output p
+ //%output J
+
+ void undistToPlane(const Eigen::Vector2d& p_u, Eigen::Vector2d& p) const;
+ //%output p
+
+ template
+ static void spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p);
+ template
+ static void spaceToSphere(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& P_s);
+ template
+ static void LiftToSphere(const T* const params,
+ const Eigen::Matrix& p,
+ Eigen::Matrix& P);
+
+ template
+ static void SphereToPlane(const T* const params, const Eigen::Matrix& P,
+ Eigen::Matrix& p);
+
+
+ void initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale = 1.0) const;
+ cv::Mat initUndistortRectifyMap(cv::Mat& map1, cv::Mat& map2,
+ float fx = -1.0f, float fy = -1.0f,
+ cv::Size imageSize = cv::Size(0, 0),
+ float cx = -1.0f, float cy = -1.0f,
+ cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const;
+
+ int parameterCount(void) const;
+
+ const Parameters& getParameters(void) const;
+ void setParameters(const Parameters& parameters);
+
+ void readParameters(const std::vector& parameterVec);
+ void writeParameters(std::vector& parameterVec) const;
+
+ void writeParametersToYamlFile(const std::string& filename) const;
+
+ std::string parametersToString(void) const;
+
+private:
+ Parameters mParameters;
+
+ double m_inv_scale;
+};
+
+typedef boost::shared_ptr OCAMCameraPtr;
+typedef boost::shared_ptr OCAMCameraConstPtr;
+
+template
+void
+OCAMCamera::spaceToPlane(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& p)
+{
+ T P_c[3];
+ {
+ T P_w[3];
+ P_w[0] = T(P(0));
+ P_w[1] = T(P(1));
+ P_w[2] = T(P(2));
+
+ // Convert quaternion from Eigen convention (x, y, z, w)
+ // to Ceres convention (w, x, y, z)
+ T q_ceres[4] = {q[3], q[0], q[1], q[2]};
+
+ ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+ P_c[0] += t[0];
+ P_c[1] += t[1];
+ P_c[2] += t[2];
+ }
+
+ T c = params[0];
+ T d = params[1];
+ T e = params[2];
+ T xc[2] = { params[3], params[4] };
+
+ //T poly[SCARAMUZZA_POLY_SIZE];
+ //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
+ // poly[i] = params[5+i];
+
+ T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
+ for (int i=0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+ inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];
+
+ T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
+ T norm = T(0.0);
+ if (norm_sqr > T(0.0))
+ norm = sqrt(norm_sqr);
+
+ T theta = atan2(-P_c[2], norm);
+ T rho = T(0.0);
+ T theta_i = T(1.0);
+
+ for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+ {
+ rho += theta_i * inv_poly[i];
+ theta_i *= theta;
+ }
+
+ T invNorm = T(1.0) / norm;
+ T xn[2] = {
+ P_c[0] * invNorm * rho,
+ P_c[1] * invNorm * rho
+ };
+
+ p(0) = xn[0] * c + xn[1] * d + xc[0];
+ p(1) = xn[0] * e + xn[1] + xc[1];
+}
+
+template
+void
+OCAMCamera::spaceToSphere(const T* const params,
+ const T* const q, const T* const t,
+ const Eigen::Matrix& P,
+ Eigen::Matrix& P_s)
+{
+ T P_c[3];
+ {
+ T P_w[3];
+ P_w[0] = T(P(0));
+ P_w[1] = T(P(1));
+ P_w[2] = T(P(2));
+
+ // Convert quaternion from Eigen convention (x, y, z, w)
+ // to Ceres convention (w, x, y, z)
+ T q_ceres[4] = {q[3], q[0], q[1], q[2]};
+
+ ceres::QuaternionRotatePoint(q_ceres, P_w, P_c);
+
+ P_c[0] += t[0];
+ P_c[1] += t[1];
+ P_c[2] += t[2];
+ }
+
+ //T poly[SCARAMUZZA_POLY_SIZE];
+ //for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
+ // poly[i] = params[5+i];
+
+ T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2];
+ T norm = T(0.0);
+ if (norm_sqr > T(0.0))
+ norm = sqrt(norm_sqr);
+
+ P_s(0) = P_c[0] / norm;
+ P_s(1) = P_c[1] / norm;
+ P_s(2) = P_c[2] / norm;
+}
+
+template
+void
+OCAMCamera::LiftToSphere(const T* const params,
+ const Eigen::Matrix& p,
+ Eigen::Matrix& P)
+{
+ T c = params[0];
+ T d = params[1];
+ T e = params[2];
+ T cc[2] = { params[3], params[4] };
+ T poly[SCARAMUZZA_POLY_SIZE];
+ for (int i=0; i < SCARAMUZZA_POLY_SIZE; i++)
+ poly[i] = params[5+i];
+
+ // Relative to Center
+ T p_2d[2];
+ p_2d[0] = T(p(0));
+ p_2d[1] = T(p(1));
+
+ T xc[2] = { p_2d[0] - cc[0], p_2d[1] - cc[1]};
+
+ T inv_scale = T(1.0) / (c - d * e);
+
+ // Affine Transformation
+ T xc_a[2];
+
+ xc_a[0] = inv_scale * (xc[0] - d * xc[1]);
+ xc_a[1] = inv_scale * (-e * xc[0] + c * xc[1]);
+
+ T norm_sqr = xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1];
+ T phi = sqrt(norm_sqr);
+ T phi_i = T(1.0);
+ T z = T(0.0);
+
+ for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
+ {
+ if (i!=1) {
+ z += phi_i * poly[i];
+ }
+ phi_i *= phi;
+ }
+
+ T p_3d[3];
+ p_3d[0] = xc[0];
+ p_3d[1] = xc[1];
+ p_3d[2] = -z;
+
+ T p_3d_norm_sqr = p_3d[0] * p_3d[0] + p_3d[1] * p_3d[1] + p_3d[2] * p_3d[2];
+ T p_3d_norm = sqrt(p_3d_norm_sqr);
+
+ P << p_3d[0] / p_3d_norm, p_3d[1] / p_3d_norm, p_3d[2] / p_3d_norm;
+}
+
+template
+void OCAMCamera::SphereToPlane(const T* const params, const Eigen::Matrix& P,
+ Eigen::Matrix& p) {
+ T P_c[3];
+ {
+ P_c[0] = T(P(0));
+ P_c[1] = T(P(1));
+ P_c[2] = T(P(2));
+ }
+
+ T c = params[0];
+ T d = params[1];
+ T e = params[2];
+ T xc[2] = {params[3], params[4]};
+
+ T inv_poly[SCARAMUZZA_INV_POLY_SIZE];
+ for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
+ inv_poly[i] = params[5 + SCARAMUZZA_POLY_SIZE + i];
+
+ T norm_sqr = P_c[0] * P_c[0] + P_c[1] * P_c[1];
+ T norm = T(0.0);
+ if (norm_sqr > T(0.0)) norm = sqrt(norm_sqr);
+
+ T theta = atan2(-P_c[2], norm);
+ T rho = T(0.0);
+ T theta_i = T(1.0);
+
+ for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++) {
+ rho += theta_i * inv_poly[i];
+ theta_i *= theta;
+ }
+
+ T invNorm = T(1.0) / norm;
+ T xn[2] = {P_c[0] * invNorm * rho, P_c[1] * invNorm * rho};
+
+ p(0) = xn[0] * c + xn[1] * d + xc[0];
+ p(1) = xn[0] * e + xn[1] + xc[1];
+}
+}
+
+#endif
diff --git a/camera_models/include/camodocal/chessboard/Chessboard.h b/camera_models/include/camodocal/chessboard/Chessboard.h
new file mode 100644
index 0000000..fb42198
--- /dev/null
+++ b/camera_models/include/camodocal/chessboard/Chessboard.h
@@ -0,0 +1,86 @@
+#ifndef CHESSBOARD_H
+#define CHESSBOARD_H
+
+#include
+#include
+
+namespace camodocal
+{
+
+// forward declarations
+class ChessboardCorner;
+typedef boost::shared_ptr ChessboardCornerPtr;
+class ChessboardQuad;
+typedef boost::shared_ptr ChessboardQuadPtr;
+
+class Chessboard
+{
+public:
+ Chessboard(cv::Size boardSize, cv::Mat& image);
+
+ void findCorners(bool useOpenCV = false);
+ const std::vector& getCorners(void) const;
+ bool cornersFound(void) const;
+
+ const cv::Mat& getImage(void) const;
+ const cv::Mat& getSketch(void) const;
+
+private:
+ bool findChessboardCorners(const cv::Mat& image,
+ const cv::Size& patternSize,
+ std::vector& corners,
+ int flags, bool useOpenCV);
+
+ bool findChessboardCornersImproved(const cv::Mat& image,
+ const cv::Size& patternSize,
+ std::vector& corners,
+ int flags);
+
+ void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize);
+
+ void findConnectedQuads(std::vector& quads,
+ std::vector& group,
+ int group_idx, int dilation);
+
+// int checkQuadGroup(std::vector& quadGroup,
+// std::vector& outCorners,
+// cv::Size patternSize);
+
+ void labelQuadGroup(std::vector& quad_group,
+ cv::Size patternSize, bool firstRun);
+
+ void findQuadNeighbors(std::vector& quads, int dilation);
+
+ int augmentBestRun(std::vector& candidateQuads, int candidateDilation,
+ std::vector& existingQuads, int existingDilation);
+
+ void generateQuads(std::vector& quads,
+ cv::Mat& image, int flags,
+ int dilation, bool firstRun);
+
+ bool checkQuadGroup(std::vector& quads,
+ std::vector& corners,
+ cv::Size patternSize);
+
+ void getQuadrangleHypotheses(const std::vector< std::vector >& contours,
+ std::vector< std::pair >& quads,
+ int classId) const;
+
+ bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const;
+
+ bool checkBoardMonotony(std::vector& corners,
+ cv::Size patternSize);
+
+ bool matchCorners(ChessboardQuadPtr& quad1, int corner1,
+ ChessboardQuadPtr& quad2, int corner2) const;
+
+ cv::Mat mImage;
+ cv::Mat mSketch;
+ std::vector mCorners;
+ cv::Size mBoardSize;
+ bool mCornersFound;
+};
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/chessboard/ChessboardCorner.h b/camera_models/include/camodocal/chessboard/ChessboardCorner.h
new file mode 100644
index 0000000..8b0f2bb
--- /dev/null
+++ b/camera_models/include/camodocal/chessboard/ChessboardCorner.h
@@ -0,0 +1,45 @@
+#ifndef CHESSBOARDCORNER_H
+#define CHESSBOARDCORNER_H
+
+#include
+#include
+
+namespace camodocal
+{
+
+class ChessboardCorner;
+typedef boost::shared_ptr ChessboardCornerPtr;
+
+class ChessboardCorner
+{
+public:
+ ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {}
+
+ float meanDist(int &n) const
+ {
+ float sum = 0;
+ n = 0;
+ for (int i = 0; i < 4; ++i)
+ {
+ if (neighbors[i].get())
+ {
+ float dx = neighbors[i]->pt.x - pt.x;
+ float dy = neighbors[i]->pt.y - pt.y;
+ sum += sqrt(dx*dx + dy*dy);
+ n++;
+ }
+ }
+ return sum / std::max(n, 1);
+ }
+
+ cv::Point2f pt; // X and y coordinates
+ int row; // Row and column of the corner
+ int column; // in the found pattern
+ bool needsNeighbor; // Does the corner require a neighbor?
+ int count; // number of corner neighbors
+ ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors
+};
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/chessboard/ChessboardQuad.h b/camera_models/include/camodocal/chessboard/ChessboardQuad.h
new file mode 100644
index 0000000..9bb7784
--- /dev/null
+++ b/camera_models/include/camodocal/chessboard/ChessboardQuad.h
@@ -0,0 +1,29 @@
+#ifndef CHESSBOARDQUAD_H
+#define CHESSBOARDQUAD_H
+
+#include
+
+#include "camodocal/chessboard/ChessboardCorner.h"
+
+namespace camodocal
+{
+
+class ChessboardQuad;
+typedef boost::shared_ptr ChessboardQuadPtr;
+
+class ChessboardQuad
+{
+public:
+ ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {}
+
+ int count; // Number of quad neighbors
+ int group_idx; // Quad group ID
+ float edge_len; // Smallest side length^2
+ ChessboardCornerPtr corners[4]; // Coordinates of quad corners
+ ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors
+ bool labeled; // Has this corner been labeled?
+};
+
+}
+
+#endif
diff --git a/camera_models/include/camodocal/chessboard/Spline.h b/camera_models/include/camodocal/chessboard/Spline.h
new file mode 100644
index 0000000..5ac5809
--- /dev/null
+++ b/camera_models/include/camodocal/chessboard/Spline.h
@@ -0,0 +1,319 @@
+/* dynamo:- Event driven molecular dynamics simulator
+ http://www.marcusbannerman.co.uk/dynamo
+ Copyright (C) 2011 Marcus N Campbell Bannerman
+
+ This program is free software: you can redistribute it and/or
+ modify it under the terms of the GNU General Public License
+ version 3 as published by the Free Software Foundation.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+*/
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+namespace ublas = boost::numeric::ublas;
+
+class Spline : private std::vector >
+{
+public:
+ //The boundary conditions available
+ enum BC_type {
+ FIXED_1ST_DERIV_BC,
+ FIXED_2ND_DERIV_BC,
+ PARABOLIC_RUNOUT_BC
+ };
+
+ enum Spline_type {
+ LINEAR,
+ CUBIC
+ };
+
+ //Constructor takes the boundary conditions as arguments, this
+ //sets the first derivative (gradient) at the lower and upper
+ //end points
+ Spline():
+ _valid(false),
+ _BCLow(FIXED_2ND_DERIV_BC), _BCHigh(FIXED_2ND_DERIV_BC),
+ _BCLowVal(0), _BCHighVal(0),
+ _type(CUBIC)
+ {}
+
+ typedef std::vector > base;
+ typedef base::const_iterator const_iterator;
+
+ //Standard STL read-only container stuff
+ const_iterator begin() const { return base::begin(); }
+ const_iterator end() const { return base::end(); }
+ void clear() { _valid = false; base::clear(); _data.clear(); }
+ size_t size() const { return base::size(); }
+ size_t max_size() const { return base::max_size(); }
+ size_t capacity() const { return base::capacity(); }
+ bool empty() const { return base::empty(); }
+
+ //Add a point to the spline, and invalidate it so its
+ //recalculated on the next access
+ inline void addPoint(double x, double y)
+ {
+ _valid = false;
+ base::push_back(std::pair(x,y));
+ }
+
+ //Reset the boundary conditions
+ inline void setLowBC(BC_type BC, double val = 0)
+ { _BCLow = BC; _BCLowVal = val; _valid = false; }
+
+ inline void setHighBC(BC_type BC, double val = 0)
+ { _BCHigh = BC; _BCHighVal = val; _valid = false; }
+
+ void setType(Spline_type type) { _type = type; _valid = false; }
+
+ //Check if the spline has been calculated, then generate the
+ //spline interpolated value
+ double operator()(double xval)
+ {
+ if (!_valid) generate();
+
+ //Special cases when we're outside the range of the spline points
+ if (xval <= x(0)) return lowCalc(xval);
+ if (xval >= x(size()-1)) return highCalc(xval);
+
+ //Check all intervals except the last one
+ for (std::vector::const_iterator iPtr = _data.begin();
+ iPtr != _data.end()-1; ++iPtr)
+ if ((xval >= iPtr->x) && (xval <= (iPtr+1)->x))
+ return splineCalc(iPtr, xval);
+
+ return splineCalc(_data.end() - 1, xval);
+ }
+
+private:
+
+ ///////PRIVATE DATA MEMBERS
+ struct SplineData { double x,a,b,c,d; };
+ //vector of calculated spline data
+ std::vector _data;
+ //Second derivative at each point
+ ublas::vector _ddy;
+ //Tracks whether the spline parameters have been calculated for
+ //the current set of points
+ bool _valid;
+ //The boundary conditions
+ BC_type _BCLow, _BCHigh;
+ //The values of the boundary conditions
+ double _BCLowVal, _BCHighVal;
+
+ Spline_type _type;
+
+ ///////PRIVATE FUNCTIONS
+ //Function to calculate the value of a given spline at a point xval
+ inline double splineCalc(std::vector::const_iterator i, double xval)
+ {
+ const double lx = xval - i->x;
+ return ((i->a * lx + i->b) * lx + i->c) * lx + i->d;
+ }
+
+ inline double lowCalc(double xval)
+ {
+ const double lx = xval - x(0);
+
+ if (_type == LINEAR)
+ return lx * _BCHighVal + y(0);
+
+ const double firstDeriv = (y(1) - y(0)) / h(0) - 2 * h(0) * (_data[0].b + 2 * _data[1].b) / 6;
+
+ switch(_BCLow)
+ {
+ case FIXED_1ST_DERIV_BC:
+ return lx * _BCLowVal + y(0);
+ case FIXED_2ND_DERIV_BC:
+ return lx * lx * _BCLowVal + firstDeriv * lx + y(0);
+ case PARABOLIC_RUNOUT_BC:
+ return lx * lx * _ddy[0] + lx * firstDeriv + y(0);
+ }
+ throw std::runtime_error("Unknown BC");
+ }
+
+ inline double highCalc(double xval)
+ {
+ const double lx = xval - x(size() - 1);
+
+ if (_type == LINEAR)
+ return lx * _BCHighVal + y(size() - 1);
+
+ const double firstDeriv = 2 * h(size() - 2) * (_ddy[size() - 2] + 2 * _ddy[size() - 1]) / 6 + (y(size() - 1) - y(size() - 2)) / h(size() - 2);
+
+ switch(_BCHigh)
+ {
+ case FIXED_1ST_DERIV_BC:
+ return lx * _BCHighVal + y(size() - 1);
+ case FIXED_2ND_DERIV_BC:
+ return lx * lx * _BCHighVal + firstDeriv * lx + y(size() - 1);
+ case PARABOLIC_RUNOUT_BC:
+ return lx * lx * _ddy[size()-1] + lx * firstDeriv + y(size() - 1);
+ }
+ throw std::runtime_error("Unknown BC");
+ }
+
+ //These just provide access to the point data in a clean way
+ inline double x(size_t i) const { return operator[](i).first; }
+ inline double y(size_t i) const { return operator[](i).second; }
+ inline double h(size_t i) const { return x(i+1) - x(i); }
+
+ //Invert a arbitrary matrix using the boost ublas library
+ template
+ bool InvertMatrix(ublas::matrix A,
+ ublas::matrix& inverse)
+ {
+ using namespace ublas;
+
+ // create a permutation matrix for the LU-factorization
+ permutation_matrix pm(A.size1());
+
+ // perform LU-factorization
+ int res = lu_factorize(A,pm);
+ if( res != 0 ) return false;
+
+ // create identity matrix of "inverse"
+ inverse.assign(ublas::identity_matrix(A.size1()));
+
+ // backsubstitute to get the inverse
+ lu_substitute(A, pm, inverse);
+
+ return true;
+ }
+
+ //This function will recalculate the spline parameters and store
+ //them in _data, ready for spline interpolation
+ void generate()
+ {
+ if (size() < 2)
+ throw std::runtime_error("Spline requires at least 2 points");
+
+ //If any spline points are at the same x location, we have to
+ //just slightly seperate them
+ {
+ bool testPassed(false);
+ while (!testPassed)
+ {
+ testPassed = true;
+ std::sort(base::begin(), base::end());
+
+ for (base::iterator iPtr = base::begin(); iPtr != base::end() - 1; ++iPtr)
+ if (iPtr->first == (iPtr+1)->first)
+ {
+ if ((iPtr+1)->first != 0)
+ (iPtr+1)->first += (iPtr+1)->first
+ * std::numeric_limits::epsilon() * 10;
+ else
+ (iPtr+1)->first = std::numeric_limits::epsilon() * 10;
+ testPassed = false;
+ break;
+ }
+ }
+ }
+
+ const size_t e = size() - 1;
+
+ switch (_type)
+ {
+ case LINEAR:
+ {
+ _data.resize(e);
+ for (size_t i(0); i < e; ++i)
+ {
+ _data[i].x = x(i);
+ _data[i].a = 0;
+ _data[i].b = 0;
+ _data[i].c = (y(i+1) - y(i)) / (x(i+1) - x(i));
+ _data[i].d = y(i);
+ }
+ break;
+ }
+ case CUBIC:
+ {
+ ublas::matrix A(size(), size());
+ for (size_t yv(0); yv <= e; ++yv)
+ for (size_t xv(0); xv <= e; ++xv)
+ A(xv,yv) = 0;
+
+ for (size_t i(1); i < e; ++i)
+ {
+ A(i-1,i) = h(i-1);
+ A(i,i) = 2 * (h(i-1) + h(i));
+ A(i+1,i) = h(i);
+ }
+
+ ublas::vector C(size());
+ for (size_t xv(0); xv <= e; ++xv)
+ C(xv) = 0;
+
+ for (size_t i(1); i < e; ++i)
+ C(i) = 6 *
+ ((y(i+1) - y(i)) / h(i)
+ - (y(i) - y(i-1)) / h(i-1));
+
+ //Boundary conditions
+ switch(_BCLow)
+ {
+ case FIXED_1ST_DERIV_BC:
+ C(0) = 6 * ((y(1) - y(0)) / h(0) - _BCLowVal);
+ A(0,0) = 2 * h(0);
+ A(1,0) = h(0);
+ break;
+ case FIXED_2ND_DERIV_BC:
+ C(0) = _BCLowVal;
+ A(0,0) = 1;
+ break;
+ case PARABOLIC_RUNOUT_BC:
+ C(0) = 0; A(0,0) = 1; A(1,0) = -1;
+ break;
+ }
+
+ switch(_BCHigh)
+ {
+ case FIXED_1ST_DERIV_BC:
+ C(e) = 6 * (_BCHighVal - (y(e) - y(e-1)) / h(e-1));
+ A(e,e) = 2 * h(e - 1);
+ A(e-1,e) = h(e - 1);
+ break;
+ case FIXED_2ND_DERIV_BC:
+ C(e) = _BCHighVal;
+ A(e,e) = 1;
+ break;
+ case PARABOLIC_RUNOUT_BC:
+ C(e) = 0; A(e,e) = 1; A(e-1,e) = -1;
+ break;
+ }
+
+ ublas::matrix AInv(size(), size());
+ InvertMatrix(A,AInv);
+
+ _ddy = ublas::prod(C, AInv);
+
+ _data.resize(size()-1);
+ for (size_t i(0); i < e; ++i)
+ {
+ _data[i].x = x(i);
+ _data[i].a = (_ddy(i+1) - _ddy(i)) / (6 * h(i));
+ _data[i].b = _ddy(i) / 2;
+ _data[i].c = (y(i+1) - y(i)) / h(i) - _ddy(i+1) * h(i) / 6 - _ddy(i) * h(i) / 3;
+ _data[i].d = y(i);
+ }
+ }
+ }
+ _valid = true;
+ }
+};
diff --git a/camera_models/include/camodocal/gpl/EigenQuaternionParameterization.h b/camera_models/include/camodocal/gpl/EigenQuaternionParameterization.h
new file mode 100644
index 0000000..3d08f0e
--- /dev/null
+++ b/camera_models/include/camodocal/gpl/EigenQuaternionParameterization.h
@@ -0,0 +1,40 @@
+#ifndef EIGENQUATERNIONPARAMETERIZATION_H
+#define EIGENQUATERNIONPARAMETERIZATION_H
+
+#include "ceres/local_parameterization.h"
+
+namespace camodocal
+{
+
+class EigenQuaternionParameterization : public ceres::LocalParameterization
+{
+public:
+ virtual ~EigenQuaternionParameterization() {}
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual int GlobalSize() const { return 4; }
+ virtual int LocalSize() const { return 3; }
+
+private:
+ template
+ void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const;
+};
+
+
+template
+void
+EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const
+{
+ zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1];
+ zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0];
+ zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3];
+ zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2];
+}
+
+}
+
+#endif
+
diff --git a/camera_models/include/camodocal/gpl/EigenUtils.h b/camera_models/include/camodocal/gpl/EigenUtils.h
new file mode 100644
index 0000000..72b46c7
--- /dev/null
+++ b/camera_models/include/camodocal/gpl/EigenUtils.h
@@ -0,0 +1,418 @@
+#ifndef EIGENUTILS_H
+#define EIGENUTILS_H
+
+#include
+
+#include "ceres/rotation.h"
+#include "camodocal/gpl/gpl.h"
+
+namespace camodocal
+{
+
+// Returns the 3D cross product skew symmetric matrix of a given 3D vector
+template
+Eigen::Matrix skew(const Eigen::Matrix& vec)
+{
+ return (Eigen::Matrix() << T(0), -vec(2), vec(1),
+ vec(2), T(0), -vec(0),
+ -vec(1), vec(0), T(0)).finished();
+}
+
+template
+typename Eigen::MatrixBase::PlainObject sqrtm(const Eigen::MatrixBase& A)
+{
+ Eigen::SelfAdjointEigenSolver es(A);
+
+ return es.operatorSqrt();
+}
+
+template
+Eigen::Matrix AngleAxisToRotationMatrix(const Eigen::Matrix& rvec)
+{
+ T angle = rvec.norm();
+ if (angle == T(0))
+ {
+ return Eigen::Matrix::Identity();
+ }
+
+ Eigen::Matrix axis;
+ axis = rvec.normalized();
+
+ Eigen::Matrix rmat;
+ rmat = Eigen::AngleAxis(angle, axis);
+
+ return rmat;
+}
+
+template
+Eigen::Quaternion AngleAxisToQuaternion(const Eigen::Matrix& rvec)
+{
+ Eigen::Matrix rmat = AngleAxisToRotationMatrix(rvec);
+
+ return Eigen::Quaternion(rmat);
+}
+
+template
+void AngleAxisToQuaternion(const Eigen::Matrix& rvec, T* q)
+{
+ Eigen::Quaternion quat = AngleAxisToQuaternion(rvec);
+
+ q[0] = quat.x();
+ q[1] = quat.y();
+ q[2] = quat.z();
+ q[3] = quat.w();
+}
+
+template
+Eigen::Matrix RotationToAngleAxis(const Eigen::Matrix & rmat)
+{
+ Eigen::AngleAxis angleaxis;
+ angleaxis.fromRotationMatrix(rmat);
+ return angleaxis.angle() * angleaxis.axis();
+
+}
+
+template
+void QuaternionToAngleAxis(const T* const q, Eigen::Matrix& rvec)
+{
+ Eigen::Quaternion quat(q[3], q[0], q[1], q[2]);
+
+ Eigen::Matrix rmat = quat.toRotationMatrix();
+
+ Eigen::AngleAxis angleaxis;
+ angleaxis.fromRotationMatrix(rmat);
+
+ rvec = angleaxis.angle() * angleaxis.axis();
+}
+
+template
+Eigen::Matrix QuaternionToRotation(const T* const q)
+{
+ T R[9];
+ ceres::QuaternionToRotation(q, R);
+
+ Eigen::Matrix rmat;
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ rmat(i,j) = R[i * 3 + j];
+ }
+ }
+
+ return rmat;
+}
+
+template
+void QuaternionToRotation(const T* const q, T* rot)
+{
+ ceres::QuaternionToRotation(q, rot);
+}
+
+template
+Eigen::Matrix QuaternionMultMatLeft(const Eigen::Quaternion& q)
+{
+ return (Eigen::Matrix() << q.w(), -q.z(), q.y(), q.x(),
+ q.z(), q.w(), -q.x(), q.y(),
+ -q.y(), q.x(), q.w(), q.z(),
+ -q.x(), -q.y(), -q.z(), q.w()).finished();
+}
+
+template
+Eigen::Matrix QuaternionMultMatRight(const Eigen::Quaternion& q)
+{
+ return (Eigen::Matrix() << q.w(), q.z(), -q.y(), q.x(),
+ -q.z(), q.w(), q.x(), q.y(),
+ q.y(), -q.x(), q.w(), q.z(),
+ -q.x(), -q.y(), -q.z(), q.w()).finished();
+}
+
+/// @param theta - rotation about screw axis
+/// @param d - projection of tvec on the rotation axis
+/// @param l - screw axis direction
+/// @param m - screw axis moment
+template
+void AngleAxisAndTranslationToScrew(const Eigen::Matrix& rvec,
+ const Eigen::Matrix& tvec,
+ T& theta, T& d,
+ Eigen::Matrix& l,
+ Eigen::Matrix& m)
+{
+
+ theta = rvec.norm();
+ if (theta == 0)
+ {
+ l.setZero();
+ m.setZero();
+ std::cout << "Warning: Undefined screw! Returned 0. " << std::endl;
+ }
+
+ l = rvec.normalized();
+
+ Eigen::Matrix t = tvec;
+
+ d = t.transpose() * l;
+
+ // point on screw axis - projection of origin on screw axis
+ Eigen::Matrix c;
+ c = 0.5 * (t - d * l + (1.0 / tan(theta / 2.0) * l).cross(t));
+
+ // c and hence the screw axis is not defined if theta is either 0 or M_PI
+ m = c.cross(l);
+}
+
+template
+Eigen::Matrix RPY2mat(T roll, T pitch, T yaw)
+{
+ Eigen::Matrix m;
+
+ T cr = cos(roll);
+ T sr = sin(roll);
+ T cp = cos(pitch);
+ T sp = sin(pitch);
+ T cy = cos(yaw);
+ T sy = sin(yaw);
+
+ m(0,0) = cy * cp;
+ m(0,1) = cy * sp * sr - sy * cr;
+ m(0,2) = cy * sp * cr + sy * sr;
+ m(1,0) = sy * cp;
+ m(1,1) = sy * sp * sr + cy * cr;
+ m(1,2) = sy * sp * cr - cy * sr;
+ m(2,0) = - sp;
+ m(2,1) = cp * sr;
+ m(2,2) = cp * cr;
+ return m;
+}
+
+template
+void mat2RPY(const Eigen::Matrix& m, T& roll, T& pitch, T& yaw)
+{
+ roll = atan2(m(2,1), m(2,2));
+ pitch = atan2(-m(2,0), sqrt(m(2,1) * m(2,1) + m(2,2) * m(2,2)));
+ yaw = atan2(m(1,0), m(0,0));
+}
+
+template
+Eigen::Matrix homogeneousTransform(const Eigen::Matrix& R, const Eigen::Matrix& t)
+{
+ Eigen::Matrix H;
+ H.setIdentity();
+
+ H.block(0,0,3,3) = R;
+ H.block(0,3,3,1) = t;
+
+ return H;
+}
+
+template
+Eigen::Matrix poseWithCartesianTranslation(const T* const q, const T* const p)
+{
+ Eigen::Matrix pose = Eigen::Matrix::Identity();
+
+ T rotation[9];
+ ceres::QuaternionToRotation(q, rotation);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ pose(i,j) = rotation[i * 3 + j];
+ }
+ }
+
+ pose(0,3) = p[0];
+ pose(1,3) = p[1];
+ pose(2,3) = p[2];
+
+ return pose;
+}
+
+template
+Eigen::Matrix poseWithSphericalTranslation(const T* const q, const T* const p, const T scale = T(1.0))
+{
+ Eigen::Matrix pose = Eigen::Matrix::Identity();
+
+ T rotation[9];
+ ceres::QuaternionToRotation(q, rotation);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ pose(i,j) = rotation[i * 3 + j];
+ }
+ }
+
+ T theta = p[0];
+ T phi = p[1];
+ pose(0,3) = sin(theta) * cos(phi) * scale;
+ pose(1,3) = sin(theta) * sin(phi) * scale;
+ pose(2,3) = cos(theta) * scale;
+
+ return pose;
+}
+
+// Returns the Sampson error of a given essential matrix and 2 image points
+template
+T sampsonError(const Eigen::Matrix& E,
+ const Eigen::Matrix& p1,
+ const Eigen::Matrix& p2)
+{
+ Eigen::Matrix Ex1 = E * p1;
+ Eigen::Matrix Etx2 = E.transpose() * p2;
+
+ T x2tEx1 = p2.dot(Ex1);
+
+ // compute Sampson error
+ T err = square(x2tEx1) / (square(Ex1(0,0)) + square(Ex1(1,0)) + square(Etx2(0,0)) + square(Etx2(1,0)));
+
+ return err;
+}
+
+// Returns the Sampson error of a given rotation/translation and 2 image points
+template
+T sampsonError(const Eigen::Matrix& R,
+ const Eigen::Matrix& t,
+ const Eigen::Matrix& p1,
+ const Eigen::Matrix