tristan cardot / Pollen3D

Commit b0bb08d1ca7cbe149c272dee6655693ca0ffaf64

Authored by tristan 2020-01-13 10:01:22 +0100

0 parents

Exists in master

init

Showing 41 changed files with 3434 additions and 0 deletions Side-by-side Diff

CMakeLists.txt
README.md
example_datasets/brassica/brassica_01.jpg
example_datasets/brassica/brassica_02.jpg
example_datasets/brassica/brassica_03.jpg
example_datasets/brassica/brassica_04.jpg
example_datasets/grid/grid_01.TIF
example_datasets/grid/grid_02.TIF
example_datasets/grid/grid_03.TIF
example_datasets/grid/grid_04.TIF
example_datasets/grid/grid_05.TIF
example_datasets/piece/piece_01.jpg
example_datasets/piece/piece_02.jpg
example_datasets/piece/piece_03.jpg
example_datasets/vis/vis_01.jpg
example_datasets/vis/vis_02.jpg
example_datasets/vis/vis_03.jpg
gpl-3.0.txt
include/3dReconst.h
include/Autocalib.h
include/FundMatFitting.hpp
include/RectifierAffine.hpp
include/densematcher.h
include/matcell.h
include/optimization.h
include/robust_estim.hpp
include/transformations.h
include/triangulation.h
include/utils.h
lgpl-3.0.txt
src/3dReconst.cpp
src/Autocalib.cpp
src/FundMatFitting.cpp
src/RectifierAffine.cpp
src/densematcher.cpp
src/main.cpp
src/matcell.cpp
src/optimization.cpp
src/transformations.cpp
src/triangulation.cpp
src/utils.cpp

...	...	@@ -0,0 +1,51 @@
	1	+cmake_minimum_required( VERSION 2.8)
	2	+set( CMAKE_CXX_STANDARD 11)
	3	+project( pollen3D)
	4	+find_package( OpenCV REQUIRED)
	5	+set( CMAKE_BUILD_TYPE Release)
	6	+
	7	+find_package( PCL 1.8 REQUIRED)
	8	+include_directories( ${PCL_INCLUDE_DIRS})
	9	+link_directories( ${PCL_LIBRARY_DIRS})
	10	+link_directories( ${PCL_GENERATE_DIRS})
	11	+add_definitions( ${PCL_DEFINITIONS})
	12	+
	13	+find_package( NLopt REQUIRED)
	14	+include_directories( ${NLOPT_INCLUDE_DIRS})
	15	+
	16	+set( PROJECT_SOURCES
	17	+ ${SOURCE}
	18	+ ./src/main.cpp
	19	+ ./src/Autocalib.cpp
	20	+ ./src/FundMatFitting.cpp
	21	+ ./src/optimization.cpp
	22	+ ./src/matcell.cpp
	23	+ ./src/utils.cpp
	24	+ ./src/transformations.cpp
	25	+
	26	+ ./src/3dReconst.cpp
	27	+ ./src/RectifierAffine.cpp
	28	+ ./src/densematcher.cpp
	29	+ ./src/triangulation.cpp
	30	+)
	31	+
	32	+set( PROJET_HEADERS
	33	+ ./include/Autocalib.h
	34	+ ./include/transformations.h
	35	+ ./include/FundMatFitting.hpp
	36	+ ./include/robust_estim.hpp
	37	+ ./include/optimization.h
	38	+ ./include/matcell.h
	39	+ ./include/utils.h
	40	+
	41	+ ./include/3dReconst.h
	42	+ ./include/RectifierAffine.hpp
	43	+ ./include/densematcher.h
	44	+ ./include/triangulation.h
	45	+)
	46	+
	47	+add_executable( pollen3D ${PROJECT_SOURCES} ${PROJECT_HEADER})
	48	+include_directories( ${CMAKE_SOURCE_DIR}/include)
	49	+target_link_libraries( pollen3D ${OpenCV_LIBS})
	50	+target_link_libraries( pollen3D ${PCL_LIBRARIES})
	51	+target_link_libraries( pollen3D ${NLOPT_LIBRARIES})

...	...	@@ -0,0 +1,42 @@
	1	+# Pollen3D
	2	+Pollen3D is a multi-images 3D reconstruction software.
	3	+
	4	+## Dependencies
	5	+- Install cmake:
	6	+ https://cmake.org/download/
	7	+
	8	+- Install libopencv:
	9	+ https://opencv.org/releases/
	10	+
	11	+- Install Point Cloud Library (PCL):
	12	+ http://pointclouds.org/downloads/
	13	+
	14	+- Install NLopt:
	15	+ https://nlopt.readthedocs.io/en/latest/NLopt_Installation/
	16	+
	17	+## Compilation
	18	+Use cmake on the pollen3D folder to generate the binary.
	19	+
	20	+## Usage
	21	+Execute the generated binary and give in arguments the paths to your images.
	22	+
	23	+## usage exemple:
	24	+`./pollen3D /absolute/path/to/images/img_01.png /absolute/path/to/images/img_02.png /absolute/path/to/images/img_03.png`
	25	+`./pollen3D ./relative/path/to/images/img*.jpg`
	26	+You can use the sets of images in `dataset_exemples` for testing.
	27	+
	28	+## Licence
	29	+This program is free software: you can redistribute it and/or modify
	30	+it under the terms of the GNU Limited General Public License as published by
	31	+the Free Software Foundation, either version 3 of the License, or
	32	+(at your option) any later version.
	33	+
	34	+This program is distributed in the hope that it will be useful,
	35	+but WITHOUT ANY WARRANTY; without even the implied warranty of
	36	+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	37	+GNU General Public License for more details.
	38	+
	39	+You should have received a copy of the GNU General Public License
	40	+along with this program. If not, see <https://www.gnu.org/licenses/>
	41	+
	42	+For more details see `lgpl-3.0.txt` or <http://www.gnu.org/licenses/lgpl-3.0.html>

86.3 KB

86.2 KB

85.8 KB

No preview for this file type

1.17 MB

1.05 MB

1.1 MB

1 MB

1000 KB

983 KB

...	...	@@ -0,0 +1,674 @@
	1	+ GNU GENERAL PUBLIC LICENSE
	2	+ Version 3, 29 June 2007
	3	+
	4	+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
	5	+ Everyone is permitted to copy and distribute verbatim copies
	6	+ of this license document, but changing it is not allowed.
	7	+
	8	+ Preamble
	9	+
	10	+ The GNU General Public License is a free, copyleft license for
	11	+software and other kinds of works.
	12	+
	13	+ The licenses for most software and other practical works are designed
	14	+to take away your freedom to share and change the works. By contrast,
	15	+the GNU General Public License is intended to guarantee your freedom to
	16	+share and change all versions of a program--to make sure it remains free
	17	+software for all its users. We, the Free Software Foundation, use the
	18	+GNU General Public License for most of our software; it applies also to
	19	+any other work released this way by its authors. You can apply it to
	20	+your programs, too.
	21	+
	22	+ When we speak of free software, we are referring to freedom, not
	23	+price. Our General Public Licenses are designed to make sure that you
	24	+have the freedom to distribute copies of free software (and charge for
	25	+them if you wish), that you receive source code or can get it if you
	26	+want it, that you can change the software or use pieces of it in new
	27	+free programs, and that you know you can do these things.
	28	+
	29	+ To protect your rights, we need to prevent others from denying you
	30	+these rights or asking you to surrender the rights. Therefore, you have
	31	+certain responsibilities if you distribute copies of the software, or if
	32	+you modify it: responsibilities to respect the freedom of others.
	33	+
	34	+ For example, if you distribute copies of such a program, whether
	35	+gratis or for a fee, you must pass on to the recipients the same
	36	+freedoms that you received. You must make sure that they, too, receive
	37	+or can get the source code. And you must show them these terms so they
	38	+know their rights.
	39	+
	40	+ Developers that use the GNU GPL protect your rights with two steps:
	41	+(1) assert copyright on the software, and (2) offer you this License
	42	+giving you legal permission to copy, distribute and/or modify it.
	43	+
	44	+ For the developers' and authors' protection, the GPL clearly explains
	45	+that there is no warranty for this free software. For both users' and
	46	+authors' sake, the GPL requires that modified versions be marked as
	47	+changed, so that their problems will not be attributed erroneously to
	48	+authors of previous versions.
	49	+
	50	+ Some devices are designed to deny users access to install or run
	51	+modified versions of the software inside them, although the manufacturer
	52	+can do so. This is fundamentally incompatible with the aim of
	53	+protecting users' freedom to change the software. The systematic
	54	+pattern of such abuse occurs in the area of products for individuals to
	55	+use, which is precisely where it is most unacceptable. Therefore, we
	56	+have designed this version of the GPL to prohibit the practice for those
	57	+products. If such problems arise substantially in other domains, we
	58	+stand ready to extend this provision to those domains in future versions
	59	+of the GPL, as needed to protect the freedom of users.
	60	+
	61	+ Finally, every program is threatened constantly by software patents.
	62	+States should not allow patents to restrict development and use of
	63	+software on general-purpose computers, but in those that do, we wish to
	64	+avoid the special danger that patents applied to a free program could
	65	+make it effectively proprietary. To prevent this, the GPL assures that
	66	+patents cannot be used to render the program non-free.
	67	+
	68	+ The precise terms and conditions for copying, distribution and
	69	+modification follow.
	70	+
	71	+ TERMS AND CONDITIONS
	72	+
	73	+ 0. Definitions.
	74	+
	75	+ "This License" refers to version 3 of the GNU General Public License.
	76	+
	77	+ "Copyright" also means copyright-like laws that apply to other kinds of
	78	+works, such as semiconductor masks.
	79	+
	80	+ "The Program" refers to any copyrightable work licensed under this
	81	+License. Each licensee is addressed as "you". "Licensees" and
	82	+"recipients" may be individuals or organizations.
	83	+
	84	+ To "modify" a work means to copy from or adapt all or part of the work
	85	+in a fashion requiring copyright permission, other than the making of an
	86	+exact copy. The resulting work is called a "modified version" of the
	87	+earlier work or a work "based on" the earlier work.
	88	+
	89	+ A "covered work" means either the unmodified Program or a work based
	90	+on the Program.
	91	+
	92	+ To "propagate" a work means to do anything with it that, without
	93	+permission, would make you directly or secondarily liable for
	94	+infringement under applicable copyright law, except executing it on a
	95	+computer or modifying a private copy. Propagation includes copying,
	96	+distribution (with or without modification), making available to the
	97	+public, and in some countries other activities as well.
	98	+
	99	+ To "convey" a work means any kind of propagation that enables other
	100	+parties to make or receive copies. Mere interaction with a user through
	101	+a computer network, with no transfer of a copy, is not conveying.
	102	+
	103	+ An interactive user interface displays "Appropriate Legal Notices"
	104	+to the extent that it includes a convenient and prominently visible
	105	+feature that (1) displays an appropriate copyright notice, and (2)
	106	+tells the user that there is no warranty for the work (except to the
	107	+extent that warranties are provided), that licensees may convey the
	108	+work under this License, and how to view a copy of this License. If
	109	+the interface presents a list of user commands or options, such as a
	110	+menu, a prominent item in the list meets this criterion.
	111	+
	112	+ 1. Source Code.
	113	+
	114	+ The "source code" for a work means the preferred form of the work
	115	+for making modifications to it. "Object code" means any non-source
	116	+form of a work.
	117	+
	118	+ A "Standard Interface" means an interface that either is an official
	119	+standard defined by a recognized standards body, or, in the case of
	120	+interfaces specified for a particular programming language, one that
	121	+is widely used among developers working in that language.
	122	+
	123	+ The "System Libraries" of an executable work include anything, other
	124	+than the work as a whole, that (a) is included in the normal form of
	125	+packaging a Major Component, but which is not part of that Major
	126	+Component, and (b) serves only to enable use of the work with that
	127	+Major Component, or to implement a Standard Interface for which an
	128	+implementation is available to the public in source code form. A
	129	+"Major Component", in this context, means a major essential component
	130	+(kernel, window system, and so on) of the specific operating system
	131	+(if any) on which the executable work runs, or a compiler used to
	132	+produce the work, or an object code interpreter used to run it.
	133	+
	134	+ The "Corresponding Source" for a work in object code form means all
	135	+the source code needed to generate, install, and (for an executable
	136	+work) run the object code and to modify the work, including scripts to
	137	+control those activities. However, it does not include the work's
	138	+System Libraries, or general-purpose tools or generally available free
	139	+programs which are used unmodified in performing those activities but
	140	+which are not part of the work. For example, Corresponding Source
	141	+includes interface definition files associated with source files for
	142	+the work, and the source code for shared libraries and dynamically
	143	+linked subprograms that the work is specifically designed to require,
	144	+such as by intimate data communication or control flow between those
	145	+subprograms and other parts of the work.
	146	+
	147	+ The Corresponding Source need not include anything that users
	148	+can regenerate automatically from other parts of the Corresponding
	149	+Source.
	150	+
	151	+ The Corresponding Source for a work in source code form is that
	152	+same work.
	153	+
	154	+ 2. Basic Permissions.
	155	+
	156	+ All rights granted under this License are granted for the term of
	157	+copyright on the Program, and are irrevocable provided the stated
	158	+conditions are met. This License explicitly affirms your unlimited
	159	+permission to run the unmodified Program. The output from running a
	160	+covered work is covered by this License only if the output, given its
	161	+content, constitutes a covered work. This License acknowledges your
	162	+rights of fair use or other equivalent, as provided by copyright law.
	163	+
	164	+ You may make, run and propagate covered works that you do not
	165	+convey, without conditions so long as your license otherwise remains
	166	+in force. You may convey covered works to others for the sole purpose
	167	+of having them make modifications exclusively for you, or provide you
	168	+with facilities for running those works, provided that you comply with
	169	+the terms of this License in conveying all material for which you do
	170	+not control copyright. Those thus making or running the covered works
	171	+for you must do so exclusively on your behalf, under your direction
	172	+and control, on terms that prohibit them from making any copies of
	173	+your copyrighted material outside their relationship with you.
	174	+
	175	+ Conveying under any other circumstances is permitted solely under
	176	+the conditions stated below. Sublicensing is not allowed; section 10
	177	+makes it unnecessary.
	178	+
	179	+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
	180	+
	181	+ No covered work shall be deemed part of an effective technological
	182	+measure under any applicable law fulfilling obligations under article
	183	+11 of the WIPO copyright treaty adopted on 20 December 1996, or
	184	+similar laws prohibiting or restricting circumvention of such
	185	+measures.
	186	+
	187	+ When you convey a covered work, you waive any legal power to forbid
	188	+circumvention of technological measures to the extent such circumvention
	189	+is effected by exercising rights under this License with respect to
	190	+the covered work, and you disclaim any intention to limit operation or
	191	+modification of the work as a means of enforcing, against the work's
	192	+users, your or third parties' legal rights to forbid circumvention of
	193	+technological measures.
	194	+
	195	+ 4. Conveying Verbatim Copies.
	196	+
	197	+ You may convey verbatim copies of the Program's source code as you
	198	+receive it, in any medium, provided that you conspicuously and
	199	+appropriately publish on each copy an appropriate copyright notice;
	200	+keep intact all notices stating that this License and any
	201	+non-permissive terms added in accord with section 7 apply to the code;
	202	+keep intact all notices of the absence of any warranty; and give all
	203	+recipients a copy of this License along with the Program.
	204	+
	205	+ You may charge any price or no price for each copy that you convey,
	206	+and you may offer support or warranty protection for a fee.
	207	+
	208	+ 5. Conveying Modified Source Versions.
	209	+
	210	+ You may convey a work based on the Program, or the modifications to
	211	+produce it from the Program, in the form of source code under the
	212	+terms of section 4, provided that you also meet all of these conditions:
	213	+
	214	+ a) The work must carry prominent notices stating that you modified
	215	+ it, and giving a relevant date.
	216	+
	217	+ b) The work must carry prominent notices stating that it is
	218	+ released under this License and any conditions added under section
	219	+ 7. This requirement modifies the requirement in section 4 to
	220	+ "keep intact all notices".
	221	+
	222	+ c) You must license the entire work, as a whole, under this
	223	+ License to anyone who comes into possession of a copy. This
	224	+ License will therefore apply, along with any applicable section 7
	225	+ additional terms, to the whole of the work, and all its parts,
	226	+ regardless of how they are packaged. This License gives no
	227	+ permission to license the work in any other way, but it does not
	228	+ invalidate such permission if you have separately received it.
	229	+
	230	+ d) If the work has interactive user interfaces, each must display
	231	+ Appropriate Legal Notices; however, if the Program has interactive
	232	+ interfaces that do not display Appropriate Legal Notices, your
	233	+ work need not make them do so.
	234	+
	235	+ A compilation of a covered work with other separate and independent
	236	+works, which are not by their nature extensions of the covered work,
	237	+and which are not combined with it such as to form a larger program,
	238	+in or on a volume of a storage or distribution medium, is called an
	239	+"aggregate" if the compilation and its resulting copyright are not
	240	+used to limit the access or legal rights of the compilation's users
	241	+beyond what the individual works permit. Inclusion of a covered work
	242	+in an aggregate does not cause this License to apply to the other
	243	+parts of the aggregate.
	244	+
	245	+ 6. Conveying Non-Source Forms.
	246	+
	247	+ You may convey a covered work in object code form under the terms
	248	+of sections 4 and 5, provided that you also convey the
	249	+machine-readable Corresponding Source under the terms of this License,
	250	+in one of these ways:
	251	+
	252	+ a) Convey the object code in, or embodied in, a physical product
	253	+ (including a physical distribution medium), accompanied by the
	254	+ Corresponding Source fixed on a durable physical medium
	255	+ customarily used for software interchange.
	256	+
	257	+ b) Convey the object code in, or embodied in, a physical product
	258	+ (including a physical distribution medium), accompanied by a
	259	+ written offer, valid for at least three years and valid for as
	260	+ long as you offer spare parts or customer support for that product
	261	+ model, to give anyone who possesses the object code either (1) a
	262	+ copy of the Corresponding Source for all the software in the
	263	+ product that is covered by this License, on a durable physical
	264	+ medium customarily used for software interchange, for a price no
	265	+ more than your reasonable cost of physically performing this
	266	+ conveying of source, or (2) access to copy the
	267	+ Corresponding Source from a network server at no charge.
	268	+
	269	+ c) Convey individual copies of the object code with a copy of the
	270	+ written offer to provide the Corresponding Source. This
	271	+ alternative is allowed only occasionally and noncommercially, and
	272	+ only if you received the object code with such an offer, in accord
	273	+ with subsection 6b.
	274	+
	275	+ d) Convey the object code by offering access from a designated
	276	+ place (gratis or for a charge), and offer equivalent access to the
	277	+ Corresponding Source in the same way through the same place at no
	278	+ further charge. You need not require recipients to copy the
	279	+ Corresponding Source along with the object code. If the place to
	280	+ copy the object code is a network server, the Corresponding Source
	281	+ may be on a different server (operated by you or a third party)
	282	+ that supports equivalent copying facilities, provided you maintain
	283	+ clear directions next to the object code saying where to find the
	284	+ Corresponding Source. Regardless of what server hosts the
	285	+ Corresponding Source, you remain obligated to ensure that it is
	286	+ available for as long as needed to satisfy these requirements.
	287	+
	288	+ e) Convey the object code using peer-to-peer transmission, provided
	289	+ you inform other peers where the object code and Corresponding
	290	+ Source of the work are being offered to the general public at no
	291	+ charge under subsection 6d.
	292	+
	293	+ A separable portion of the object code, whose source code is excluded
	294	+from the Corresponding Source as a System Library, need not be
	295	+included in conveying the object code work.
	296	+
	297	+ A "User Product" is either (1) a "consumer product", which means any
	298	+tangible personal property which is normally used for personal, family,
	299	+or household purposes, or (2) anything designed or sold for incorporation
	300	+into a dwelling. In determining whether a product is a consumer product,
	301	+doubtful cases shall be resolved in favor of coverage. For a particular
	302	+product received by a particular user, "normally used" refers to a
	303	+typical or common use of that class of product, regardless of the status
	304	+of the particular user or of the way in which the particular user
	305	+actually uses, or expects or is expected to use, the product. A product
	306	+is a consumer product regardless of whether the product has substantial
	307	+commercial, industrial or non-consumer uses, unless such uses represent
	308	+the only significant mode of use of the product.
	309	+
	310	+ "Installation Information" for a User Product means any methods,
	311	+procedures, authorization keys, or other information required to install
	312	+and execute modified versions of a covered work in that User Product from
	313	+a modified version of its Corresponding Source. The information must
	314	+suffice to ensure that the continued functioning of the modified object
	315	+code is in no case prevented or interfered with solely because
	316	+modification has been made.
	317	+
	318	+ If you convey an object code work under this section in, or with, or
	319	+specifically for use in, a User Product, and the conveying occurs as
	320	+part of a transaction in which the right of possession and use of the
	321	+User Product is transferred to the recipient in perpetuity or for a
	322	+fixed term (regardless of how the transaction is characterized), the
	323	+Corresponding Source conveyed under this section must be accompanied
	324	+by the Installation Information. But this requirement does not apply
	325	+if neither you nor any third party retains the ability to install
	326	+modified object code on the User Product (for example, the work has
	327	+been installed in ROM).
	328	+
	329	+ The requirement to provide Installation Information does not include a
	330	+requirement to continue to provide support service, warranty, or updates
	331	+for a work that has been modified or installed by the recipient, or for
	332	+the User Product in which it has been modified or installed. Access to a
	333	+network may be denied when the modification itself materially and
	334	+adversely affects the operation of the network or violates the rules and
	335	+protocols for communication across the network.
	336	+
	337	+ Corresponding Source conveyed, and Installation Information provided,
	338	+in accord with this section must be in a format that is publicly
	339	+documented (and with an implementation available to the public in
	340	+source code form), and must require no special password or key for
	341	+unpacking, reading or copying.
	342	+
	343	+ 7. Additional Terms.
	344	+
	345	+ "Additional permissions" are terms that supplement the terms of this
	346	+License by making exceptions from one or more of its conditions.
	347	+Additional permissions that are applicable to the entire Program shall
	348	+be treated as though they were included in this License, to the extent
	349	+that they are valid under applicable law. If additional permissions
	350	+apply only to part of the Program, that part may be used separately
	351	+under those permissions, but the entire Program remains governed by
	352	+this License without regard to the additional permissions.
	353	+
	354	+ When you convey a copy of a covered work, you may at your option
	355	+remove any additional permissions from that copy, or from any part of
	356	+it. (Additional permissions may be written to require their own
	357	+removal in certain cases when you modify the work.) You may place
	358	+additional permissions on material, added by you to a covered work,
	359	+for which you have or can give appropriate copyright permission.
	360	+
	361	+ Notwithstanding any other provision of this License, for material you
	362	+add to a covered work, you may (if authorized by the copyright holders of
	363	+that material) supplement the terms of this License with terms:
	364	+
	365	+ a) Disclaiming warranty or limiting liability differently from the
	366	+ terms of sections 15 and 16 of this License; or
	367	+
	368	+ b) Requiring preservation of specified reasonable legal notices or
	369	+ author attributions in that material or in the Appropriate Legal
	370	+ Notices displayed by works containing it; or
	371	+
	372	+ c) Prohibiting misrepresentation of the origin of that material, or
	373	+ requiring that modified versions of such material be marked in
	374	+ reasonable ways as different from the original version; or
	375	+
	376	+ d) Limiting the use for publicity purposes of names of licensors or
	377	+ authors of the material; or
	378	+
	379	+ e) Declining to grant rights under trademark law for use of some
	380	+ trade names, trademarks, or service marks; or
	381	+
	382	+ f) Requiring indemnification of licensors and authors of that
	383	+ material by anyone who conveys the material (or modified versions of
	384	+ it) with contractual assumptions of liability to the recipient, for
	385	+ any liability that these contractual assumptions directly impose on
	386	+ those licensors and authors.
	387	+
	388	+ All other non-permissive additional terms are considered "further
	389	+restrictions" within the meaning of section 10. If the Program as you
	390	+received it, or any part of it, contains a notice stating that it is
	391	+governed by this License along with a term that is a further
	392	+restriction, you may remove that term. If a license document contains
	393	+a further restriction but permits relicensing or conveying under this
	394	+License, you may add to a covered work material governed by the terms
	395	+of that license document, provided that the further restriction does
	396	+not survive such relicensing or conveying.
	397	+
	398	+ If you add terms to a covered work in accord with this section, you
	399	+must place, in the relevant source files, a statement of the
	400	+additional terms that apply to those files, or a notice indicating
	401	+where to find the applicable terms.
	402	+
	403	+ Additional terms, permissive or non-permissive, may be stated in the
	404	+form of a separately written license, or stated as exceptions;
	405	+the above requirements apply either way.
	406	+
	407	+ 8. Termination.
	408	+
	409	+ You may not propagate or modify a covered work except as expressly
	410	+provided under this License. Any attempt otherwise to propagate or
	411	+modify it is void, and will automatically terminate your rights under
	412	+this License (including any patent licenses granted under the third
	413	+paragraph of section 11).
	414	+
	415	+ However, if you cease all violation of this License, then your
	416	+license from a particular copyright holder is reinstated (a)
	417	+provisionally, unless and until the copyright holder explicitly and
	418	+finally terminates your license, and (b) permanently, if the copyright
	419	+holder fails to notify you of the violation by some reasonable means
	420	+prior to 60 days after the cessation.
	421	+
	422	+ Moreover, your license from a particular copyright holder is
	423	+reinstated permanently if the copyright holder notifies you of the
	424	+violation by some reasonable means, this is the first time you have
	425	+received notice of violation of this License (for any work) from that
	426	+copyright holder, and you cure the violation prior to 30 days after
	427	+your receipt of the notice.
	428	+
	429	+ Termination of your rights under this section does not terminate the
	430	+licenses of parties who have received copies or rights from you under
	431	+this License. If your rights have been terminated and not permanently
	432	+reinstated, you do not qualify to receive new licenses for the same
	433	+material under section 10.
	434	+
	435	+ 9. Acceptance Not Required for Having Copies.
	436	+
	437	+ You are not required to accept this License in order to receive or
	438	+run a copy of the Program. Ancillary propagation of a covered work
	439	+occurring solely as a consequence of using peer-to-peer transmission
	440	+to receive a copy likewise does not require acceptance. However,
	441	+nothing other than this License grants you permission to propagate or
	442	+modify any covered work. These actions infringe copyright if you do
	443	+not accept this License. Therefore, by modifying or propagating a
	444	+covered work, you indicate your acceptance of this License to do so.
	445	+
	446	+ 10. Automatic Licensing of Downstream Recipients.
	447	+
	448	+ Each time you convey a covered work, the recipient automatically
	449	+receives a license from the original licensors, to run, modify and
	450	+propagate that work, subject to this License. You are not responsible
	451	+for enforcing compliance by third parties with this License.
	452	+
	453	+ An "entity transaction" is a transaction transferring control of an
	454	+organization, or substantially all assets of one, or subdividing an
	455	+organization, or merging organizations. If propagation of a covered
	456	+work results from an entity transaction, each party to that
	457	+transaction who receives a copy of the work also receives whatever
	458	+licenses to the work the party's predecessor in interest had or could
	459	+give under the previous paragraph, plus a right to possession of the
	460	+Corresponding Source of the work from the predecessor in interest, if
	461	+the predecessor has it or can get it with reasonable efforts.
	462	+
	463	+ You may not impose any further restrictions on the exercise of the
	464	+rights granted or affirmed under this License. For example, you may
	465	+not impose a license fee, royalty, or other charge for exercise of
	466	+rights granted under this License, and you may not initiate litigation
	467	+(including a cross-claim or counterclaim in a lawsuit) alleging that
	468	+any patent claim is infringed by making, using, selling, offering for
	469	+sale, or importing the Program or any portion of it.
	470	+
	471	+ 11. Patents.
	472	+
	473	+ A "contributor" is a copyright holder who authorizes use under this
	474	+License of the Program or a work on which the Program is based. The
	475	+work thus licensed is called the contributor's "contributor version".
	476	+
	477	+ A contributor's "essential patent claims" are all patent claims
	478	+owned or controlled by the contributor, whether already acquired or
	479	+hereafter acquired, that would be infringed by some manner, permitted
	480	+by this License, of making, using, or selling its contributor version,
	481	+but do not include claims that would be infringed only as a
	482	+consequence of further modification of the contributor version. For
	483	+purposes of this definition, "control" includes the right to grant
	484	+patent sublicenses in a manner consistent with the requirements of
	485	+this License.
	486	+
	487	+ Each contributor grants you a non-exclusive, worldwide, royalty-free
	488	+patent license under the contributor's essential patent claims, to
	489	+make, use, sell, offer for sale, import and otherwise run, modify and
	490	+propagate the contents of its contributor version.
	491	+
	492	+ In the following three paragraphs, a "patent license" is any express
	493	+agreement or commitment, however denominated, not to enforce a patent
	494	+(such as an express permission to practice a patent or covenant not to
	495	+sue for patent infringement). To "grant" such a patent license to a
	496	+party means to make such an agreement or commitment not to enforce a
	497	+patent against the party.
	498	+
	499	+ If you convey a covered work, knowingly relying on a patent license,
	500	+and the Corresponding Source of the work is not available for anyone
	501	+to copy, free of charge and under the terms of this License, through a
	502	+publicly available network server or other readily accessible means,
	503	+then you must either (1) cause the Corresponding Source to be so
	504	+available, or (2) arrange to deprive yourself of the benefit of the
	505	+patent license for this particular work, or (3) arrange, in a manner
	506	+consistent with the requirements of this License, to extend the patent
	507	+license to downstream recipients. "Knowingly relying" means you have
	508	+actual knowledge that, but for the patent license, your conveying the
	509	+covered work in a country, or your recipient's use of the covered work
	510	+in a country, would infringe one or more identifiable patents in that
	511	+country that you have reason to believe are valid.
	512	+
	513	+ If, pursuant to or in connection with a single transaction or
	514	+arrangement, you convey, or propagate by procuring conveyance of, a
	515	+covered work, and grant a patent license to some of the parties
	516	+receiving the covered work authorizing them to use, propagate, modify
	517	+or convey a specific copy of the covered work, then the patent license
	518	+you grant is automatically extended to all recipients of the covered
	519	+work and works based on it.
	520	+
	521	+ A patent license is "discriminatory" if it does not include within
	522	+the scope of its coverage, prohibits the exercise of, or is
	523	+conditioned on the non-exercise of one or more of the rights that are
	524	+specifically granted under this License. You may not convey a covered
	525	+work if you are a party to an arrangement with a third party that is
	526	+in the business of distributing software, under which you make payment
	527	+to the third party based on the extent of your activity of conveying
	528	+the work, and under which the third party grants, to any of the
	529	+parties who would receive the covered work from you, a discriminatory
	530	+patent license (a) in connection with copies of the covered work
	531	+conveyed by you (or copies made from those copies), or (b) primarily
	532	+for and in connection with specific products or compilations that
	533	+contain the covered work, unless you entered into that arrangement,
	534	+or that patent license was granted, prior to 28 March 2007.
	535	+
	536	+ Nothing in this License shall be construed as excluding or limiting
	537	+any implied license or other defenses to infringement that may
	538	+otherwise be available to you under applicable patent law.
	539	+
	540	+ 12. No Surrender of Others' Freedom.
	541	+
	542	+ If conditions are imposed on you (whether by court order, agreement or
	543	+otherwise) that contradict the conditions of this License, they do not
	544	+excuse you from the conditions of this License. If you cannot convey a
	545	+covered work so as to satisfy simultaneously your obligations under this
	546	+License and any other pertinent obligations, then as a consequence you may
	547	+not convey it at all. For example, if you agree to terms that obligate you
	548	+to collect a royalty for further conveying from those to whom you convey
	549	+the Program, the only way you could satisfy both those terms and this
	550	+License would be to refrain entirely from conveying the Program.
	551	+
	552	+ 13. Use with the GNU Affero General Public License.
	553	+
	554	+ Notwithstanding any other provision of this License, you have
	555	+permission to link or combine any covered work with a work licensed
	556	+under version 3 of the GNU Affero General Public License into a single
	557	+combined work, and to convey the resulting work. The terms of this
	558	+License will continue to apply to the part which is the covered work,
	559	+but the special requirements of the GNU Affero General Public License,
	560	+section 13, concerning interaction through a network will apply to the
	561	+combination as such.
	562	+
	563	+ 14. Revised Versions of this License.
	564	+
	565	+ The Free Software Foundation may publish revised and/or new versions of
	566	+the GNU General Public License from time to time. Such new versions will
	567	+be similar in spirit to the present version, but may differ in detail to
	568	+address new problems or concerns.
	569	+
	570	+ Each version is given a distinguishing version number. If the
	571	+Program specifies that a certain numbered version of the GNU General
	572	+Public License "or any later version" applies to it, you have the
	573	+option of following the terms and conditions either of that numbered
	574	+version or of any later version published by the Free Software
	575	+Foundation. If the Program does not specify a version number of the
	576	+GNU General Public License, you may choose any version ever published
	577	+by the Free Software Foundation.
	578	+
	579	+ If the Program specifies that a proxy can decide which future
	580	+versions of the GNU General Public License can be used, that proxy's
	581	+public statement of acceptance of a version permanently authorizes you
	582	+to choose that version for the Program.
	583	+
	584	+ Later license versions may give you additional or different
	585	+permissions. However, no additional obligations are imposed on any
	586	+author or copyright holder as a result of your choosing to follow a
	587	+later version.
	588	+
	589	+ 15. Disclaimer of Warranty.
	590	+
	591	+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
	592	+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
	593	+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
	594	+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
	595	+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
	596	+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
	597	+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
	598	+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
	599	+
	600	+ 16. Limitation of Liability.
	601	+
	602	+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
	603	+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
	604	+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
	605	+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
	606	+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
	607	+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
	608	+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
	609	+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
	610	+SUCH DAMAGES.
	611	+
	612	+ 17. Interpretation of Sections 15 and 16.
	613	+
	614	+ If the disclaimer of warranty and limitation of liability provided
	615	+above cannot be given local legal effect according to their terms,
	616	+reviewing courts shall apply local law that most closely approximates
	617	+an absolute waiver of all civil liability in connection with the
	618	+Program, unless a warranty or assumption of liability accompanies a
	619	+copy of the Program in return for a fee.
	620	+
	621	+ END OF TERMS AND CONDITIONS
	622	+
	623	+ How to Apply These Terms to Your New Programs
	624	+
	625	+ If you develop a new program, and you want it to be of the greatest
	626	+possible use to the public, the best way to achieve this is to make it
	627	+free software which everyone can redistribute and change under these terms.
	628	+
	629	+ To do so, attach the following notices to the program. It is safest
	630	+to attach them to the start of each source file to most effectively
	631	+state the exclusion of warranty; and each file should have at least
	632	+the "copyright" line and a pointer to where the full notice is found.
	633	+
	634	+ <one line to give the program's name and a brief idea of what it does.>
	635	+ Copyright (C) <year> <name of author>
	636	+
	637	+ This program is free software: you can redistribute it and/or modify
	638	+ it under the terms of the GNU General Public License as published by
	639	+ the Free Software Foundation, either version 3 of the License, or
	640	+ (at your option) any later version.
	641	+
	642	+ This program is distributed in the hope that it will be useful,
	643	+ but WITHOUT ANY WARRANTY; without even the implied warranty of
	644	+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	645	+ GNU General Public License for more details.
	646	+
	647	+ You should have received a copy of the GNU General Public License
	648	+ along with this program. If not, see <https://www.gnu.org/licenses/>.
	649	+
	650	+Also add information on how to contact you by electronic and paper mail.
	651	+
	652	+ If the program does terminal interaction, make it output a short
	653	+notice like this when it starts in an interactive mode:
	654	+
	655	+ <program> Copyright (C) <year> <name of author>
	656	+ This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
	657	+ This is free software, and you are welcome to redistribute it
	658	+ under certain conditions; type `show c' for details.
	659	+
	660	+The hypothetical commands `show w' and `show c' should show the appropriate
	661	+parts of the General Public License. Of course, your program's commands
	662	+might be different; for a GUI interface, you would use an "about box".
	663	+
	664	+ You should also get your employer (if you work as a programmer) or school,
	665	+if any, to sign a "copyright disclaimer" for the program, if necessary.
	666	+For more information on this, and how to apply and follow the GNU GPL, see
	667	+<https://www.gnu.org/licenses/>.
	668	+
	669	+ The GNU General Public License does not permit incorporating your program
	670	+into proprietary programs. If your program is a subroutine library, you
	671	+may consider it more useful to permit linking proprietary applications with
	672	+the library. If this is what you want to do, use the GNU Lesser General
	673	+Public License instead of this License. But first, please read
	674	+<https://www.gnu.org/licenses/why-not-lgpl.html>.

...	...	@@ -0,0 +1,52 @@
	1	+#include <stdio.h>
	2	+#include <iostream>
	3	+#include <string>
	4	+//#include <vector>
	5	+#include <opencv2/opencv.hpp>
	6	+#include <opencv2/highgui/highgui.hpp>
	7	+#include <opencv2/objdetect/objdetect.hpp>
	8	+#include <opencv2/imgproc/imgproc.hpp>
	9	+
	10	+#include <pcl/point_types.h>
	11	+#include <pcl/io/pcd_io.h>
	12	+#include <pcl/io/io.h>
	13	+#include <boost/thread/thread.hpp>
	14	+#include <pcl/common/common_headers.h>
	15	+#include <pcl/visualization/cloud_viewer.h>
	16	+
	17	+
	18	+#include <pcl/ModelCoefficients.h>
	19	+
	20	+#include <pcl/kdtree/kdtree.h>
	21	+#include <pcl/sample_consensus/method_types.h>
	22	+#include <pcl/sample_consensus/model_types.h>
	23	+#include <pcl/segmentation/sac_segmentation.h>
	24	+#include <pcl/segmentation/extract_clusters.h>
	25	+
	26	+
	27	+#include "RectifierAffine.hpp"
	28	+#include "densematcher.h"
	29	+#include "transformations.h"
	30	+#include "triangulation.h"
	31	+
	32	+
	33	+using namespace std;
	34	+
	35	+
	36	+class Reconst
	37	+{
	38	+public:
	39	+Reconst(){};
	40	+~Reconst(){};
	41	+vector<cv::Mat> RectificaEpipolarLines(vector<cv::Mat> image, vector<vector<cv::Point2d> > MeasureVector, vector<cv::Mat> fundMatrix);
	42	+void disparity(vector<cv::Mat> image);
	43	+void GetPointCloud(vector<cv::Mat> P, cv::Mat image);
	44	+vector<cv::Mat> GetRectifiedMeseureMatrix();
	45	+vector<cv::Mat> GetCameraMatrix(vector<cv::Mat> P);
	46	+void filterPointCloud(int taille);
	47	+
	48	+private:
	49	+vector<vector<cv::Point2d> > _rectifiedMeasureVector;
	50	+vector <cv::Mat> _densePoint;
	51	+
	52	+};

...	...	@@ -0,0 +1,40 @@
	1	+#include <stdio.h>
	2	+#include <iostream>
	3	+#include <vector>
	4	+#include <opencv2/opencv.hpp>
	5	+#include <opencv2/highgui/highgui.hpp>
	6	+#include <opencv2/objdetect/objdetect.hpp>
	7	+#include <opencv2/imgproc/imgproc.hpp>
	8	+
	9	+#include "optimization.h"
	10	+#include "FundMatFitting.hpp"
	11	+
	12	+
	13	+
	14	+using namespace std;
	15	+
	16	+class Autocalib
	17	+{
	18	+
	19	+public:
	20	+
	21	+vector<vector<cv::Point2d> > findFeaturePoint(vector<cv::Mat> img,double qualityLevel ); // paired measurement matrix
	22	+vector<vector<cv::Point2d> > getMeasureVector(vector<vector<cv::Point2d> > MatchVector); // dense measurement matrix i.e. without zeros
	23	+vector<vector<cv::Point2d> > searchAllMatch(vector<vector<cv::Point2d> > Vector); // remove zeros
	24	+cv::Mat vectorPoint2Mat(vector<vector<cv::Point2d> > Vector);
	25	+
	26	+vector<cv::Mat> estimatFundMatVector(vector<vector<cv::Point2d> > MeasureVector);
	27	+
	28	+cv::Mat plotStereoWithEpilines(cv::Mat img1,cv::Mat img2,cv::Mat F,vector<cv::Point2d> pts1,vector<cv::Point2d> pts2);
	29	+
	30	+void showEpipolarLines(vector<cv::Mat> image, vector<vector<cv::Point2d> > measureVector, vector<cv::Mat> fundMatrix);
	31	+//void showRectificaEpipolarLines(vector<Mat> image, vector<vector<Point2d> > MeasureMatrix, vector<Mat> fundMatrix);
	32	+
	33	+vector <cv::Mat> estimatParameter(cv::Mat measureMatrix);
	34	+
	35	+private:
	36	+cv::Mat _W;
	37	+
	38	+};
	39	+
	40	+

...	...	@@ -0,0 +1,58 @@
	1	+/**
	2	+ * @brief Line fitting to a set of 2D points
	3	+ *
	4	+ * @author Andrey Kudryavtsev (avkudr.github.io)
	5	+ * @author Rahima Djahel (github:rahma24000)
	6	+ * @date 27/03/2018
	7	+ * @version 1.0
	8	+ */
	9	+
	10	+#ifndef FUND_MAT_FITTING_H
	11	+#define FUND_MAT_FITTING_H
	12	+
	13	+#include <iostream>
	14	+#include <opencv2/opencv.hpp>
	15	+#include <vector>
	16	+
	17	+#include "robust_estim.hpp"
	18	+
	19	+// ----------------------------------------------------------------------------
	20	+// ------- Class defining the Problem for RobustEstimator (RANSAC, LMedS)
	21	+// ------- for more details visit: /github.com/avkudr/robest
	22	+// ----------------------------------------------------------------------------
	23	+
	24	+class FundMatFitting : public robest::EstimationProblem{
	25	+ typedef std::pair<cv::Point2d,cv::Point2d> Correspondence;
	26	+ typedef std::vector<Correspondence> CorrespondenceVector;
	27	+public:
	28	+ FundMatFitting(){
	29	+ setNbParams(4);//4 parameters a b c d
	30	+ setNbMinSamples(4);//at least 4 corresponndant points
	31	+ }
	32	+
	33	+ void setData(std::vector<cv::Point2d> pts1, std::vector<cv::Point2d> pts2);
	34	+
	35	+ double estimErrorForSample(int i);
	36	+ void estimModelFromSamples(std::vector<int> samplesIdx);
	37	+
	38	+ int getTotalNbSamples() const{
	39	+ return (int) correspondences.size();
	40	+ }
	41	+
	42	+ long double a,b,c,d,e; // Params
	43	+
	44	+ cv::Mat getResult(){
	45	+ cv::Mat F = (cv::Mat_<double>(3,3) << 0,0,a,0,0,b,c,d,1);
	46	+ if ( (!T1.empty()) && (!T2.empty())){
	47	+ F = T2.t() * F * T1;
	48	+ }
	49	+ F = F / cv::norm(F); // replaced with the norm: Peter Sturm suggestion
	50	+ return F;
	51	+ }
	52	+private:
	53	+ bool isDegenerate(std::vector<int> samplesIdx);
	54	+ cv::Mat T1, T2;
	55	+ CorrespondenceVector correspondences; // Data
	56	+};
	57	+
	58	+#endif // FUND_MAT_FITTING_H

...	...	@@ -0,0 +1,60 @@
	1	+/**
	2	+ * @brief Line fitting to a set of 2D points
	3	+ *
	4	+ * @author Andrey Kudryavtsev (avkudr.github.io)
	5	+ * @author Rahima Djahel (github:rahma24000)
	6	+ * @date 27/03/2018
	7	+ * @version 1.0
	8	+ */
	9	+
	10	+#ifndef RECTIFIER_AFFINE_H
	11	+#define RECTIFIER_AFFINE_H
	12	+
	13	+#include <opencv2/opencv.hpp>
	14	+
	15	+class RectifierAffine
	16	+{
	17	+public:
	18	+ RectifierAffine();
	19	+ ~RectifierAffine();
	20	+
	21	+ void init(cv::Mat im1, cv::Mat im2, cv::Mat F, std::vector<cv::Point2d> inliers1, std::vector<cv::Point2d> *inliers2);
	22	+
	23	+ bool isRectificationDone() {return !(imLrect.empty()) && !(imRrect.empty());}
	24	+
	25	+ cv::Mat imLrect;
	26	+ cv::Mat imRrect;
	27	+
	28	+ double angleL = 0.0;
	29	+ double angleR = 0.0;
	30	+ int shift = 0;
	31	+
	32	+ void rectify();
	33	+ bool isReady();
	34	+ cv::Mat get2DRotationMatrixLeft();
	35	+ cv::Mat get2DRotationMatrixRight();
	36	+
	37	+ cv::Mat get2DShiftMatrix();
	38	+
	39	+ cv::Mat get2DTransformationMatrixLeft() { return get2DRotationMatrixLeft() * get2DShiftMatrix(); }
	40	+ cv::Mat get2DTransformationMatrixRight() { return get2DRotationMatrixRight() * get2DShiftMatrix(); }
	41	+
	42	+ void getResult(cv::Mat & resL, cv::Mat & resR,std::vector<cv::Point2d> ptsL,std::vector<cv::Point2d> ptsR){
	43	+
	44	+ resL = this->imLrect;
	45	+ resR = this->imRrect;
	46	+ ptsL=this->_inliers1;
	47	+ ptsR=this->_inliers2;
	48	+ }
	49	+
	50	+private:
	51	+
	52	+ cv::Mat *_imL;
	53	+ cv::Mat *_imR;
	54	+ cv::Mat *_F;
	55	+
	56	+ std::vector<cv::Point2d> *_inliers1;
	57	+ std::vector<cv::Point2d> *_inliers2;
	58	+};
	59	+
	60	+#endif // RECTIFIER_AFFINE_H

...	...	@@ -0,0 +1,94 @@
	1	+/*!
	2	+ * \brief Stereo image dense DenseMatcher
	3	+ * \details This class is used to demonstrate a number of section commands.
	4	+ * \author Andrey Kudryavtsev
	5	+ * \version 0.1
	6	+ * \date 03/06/2016
	7	+ */
	8	+
	9	+#ifndef DenseMatcher_H
	10	+#define DenseMatcher_H
	11	+
	12	+#include <iostream>
	13	+#include <opencv2/core/core.hpp>
	14	+#include <opencv2/calib3d/calib3d.hpp>
	15	+#include <opencv2/highgui/highgui.hpp>
	16	+#include <opencv2/imgproc/imgproc.hpp>
	17	+#include <opencv2/calib3d.hpp>
	18	+
	19	+using namespace std;
	20	+
	21	+
	22	+class DenseMatcher
	23	+{
	24	+
	25	+public:
	26	+ DenseMatcher();
	27	+ DenseMatcher(int method);
	28	+ ~DenseMatcher();
	29	+
	30	+ void init(cv::Mat * lftIm, cv::Mat * rgtIm) { _lftIm = lftIm; _rgtIm = rgtIm;}
	31	+ void setMethod( int method ) { _method = method; }
	32	+ void setBounds( int lowerBound, int upperBound) {_params.lowerBound = lowerBound; _params.upperBound = upperBound;}
	33	+ void setBlockSize( int blockSize) {_params.blockSize = blockSize;}
	34	+
	35	+ void calculateDisparityMap();
	36	+ void plotDisparityMap();
	37	+
	38	+ void filterDisparity(int newVal, int maxSpeckleSize, int maxDiff);
	39	+ void plotDisparityFiltered();
	40	+
	41	+ cv::Mat getDisparityToDisplay() const;
	42	+
	43	+ enum methods{
	44	+ MODE_HH = cv::StereoSGBM::MODE_HH, ///< Perform linear interpolation on the table
	45	+ MODE_SGBM = cv::StereoSGBM::MODE_SGBM, ///< Perform parabolic interpolation on the table
	46	+ MODE_SGBM_3WAY = cv::StereoSGBM::MODE_SGBM_3WAY ///< Perform parabolic interpolation on the table
	47	+ };
	48	+
	49	+ bool isInitialized() { return ! (_lftIm == NULL \|\| _rgtIm == NULL) && ! _lftIm->empty() && ! _rgtIm->empty() ; } // _lftIm and _rgtIm are initialized
	50	+ bool isDisparityEmpty() { return _disp.empty(); }
	51	+ bool isDisparityFiltered() { return _dispFiltered.empty(); }
	52	+
	53	+ cv::Mat _dispFiltered;
	54	+ cv::Mat _disp;
	55	+ cv::Mat _dispValues;
	56	+ cv::Mat getDensePoint();
	57	+
	58	+private:
	59	+
	60	+ int _method = MODE_SGBM; // Default method
	61	+
	62	+ cv::Mat * _lftIm = NULL;
	63	+ cv::Mat * _rgtIm = NULL;
	64	+
	65	+ struct Params{
	66	+ int lowerBound = -1;
	67	+ int upperBound = 2;
	68	+ int blockSize = 9;
	69	+ };
	70	+ Params _params;
	71	+
	72	+ struct ParamsFilter{
	73	+ int newVal = 0; // The disparity value used to paint-off the speckles
	74	+ int maxSpeckleSize = 260; // The maximum speckle size to consider it a speckle. Larger blobs are not affected by the algorithm
	75	+ int maxDiff = 10; // Maximum difference between neighbor disparity pixels to put them into the same blob. Note that since StereoBM,
	76	+ //StereoSGBM and may be other algorithms return a fixed-point disparity map, where disparity values are multiplied by 16,
	77	+ //this scale factor should be taken into account when specifying this parameter value.
	78	+ };
	79	+ ParamsFilter _paramsFilter;
	80	+};
	81	+
	82	+
	83	+
	84	+/**************************
	85	+ *
	86	+ * CONTROL PART
	87	+ *
	88	+ * */
	89	+
	90	+
	91	+
	92	+#endif // DenseMatcher_H
	93	+
	94	+

...	...	@@ -0,0 +1,46 @@
	1	+#ifndef MATCELL_H
	2	+#define MATCELL_H
	3	+
	4	+#include <math.h>
	5	+#include <iostream>
	6	+#include <opencv2/core/core.hpp>
	7	+#include <opencv2/highgui/highgui.hpp>
	8	+
	9	+class MatCell
	10	+{
	11	+public:
	12	+ MatCell();
	13	+ MatCell(int nbElements);
	14	+ ~MatCell();
	15	+
	16	+ operator cv::Mat_<double>() const;
	17	+
	18	+ void eachAssign( const cv::Mat &m);
	19	+ void eachMultiplyRight(const cv::Mat &m);
	20	+ void eachMultiplyLeft(const cv::Mat &m);
	21	+
	22	+ friend std::ostream &operator<<(std::ostream &os, const MatCell &A){
	23	+ MatCell m = A;
	24	+ for (int i = 0; i < m.size(); i++){
	25	+ os << m[i] << "\n";
	26	+ }
	27	+ return os;
	28	+ }
	29	+
	30	+ int size(){ return (int)_elements.size();}
	31	+
	32	+ cv::Mat_<double> &operator[] (const int index);
	33	+ void add(cv::Mat_<double> mat){ _elements.push_back(mat); }
	34	+
	35	+
	36	+private:
	37	+ cv::Mat toMat();
	38	+ std::vector<cv::Mat_<double>> _elements;
	39	+};
	40	+
	41	+//std::ostream &operator<<(std::ostream &os, const MatCell &m)
	42	+//{
	43	+// return os << (cv::Mat) m;
	44	+//}
	45	+
	46	+#endif // MATCELL_H

...	...	@@ -0,0 +1,140 @@
	1	+#ifndef OPTIMIZATION_H
	2	+#define OPTIMIZATION_H
	3	+
	4	+/*!
	5	+ \defgroup optimization Optimization
	6	+ \brief Global optimization for autocalibration
	7	+
	8	+ \bug Problem with the function non-homogenious !!!
	9	+*/
	10	+///@{
	11	+///
	12	+
	13	+#include <math.h>
	14	+#include "matcell.h"
	15	+#include "transformations.h"
	16	+#include "utils.h"
	17	+#include <iostream>
	18	+#include <iomanip>
	19	+
	20	+#include <opencv2/core/core.hpp>
	21	+#include <opencv2/highgui/highgui.hpp>
	22	+#include <nlopt.hpp>
	23	+
	24	+#ifndef PI
	25	+#define PI 3.141592653589793238462643383279502884L
	26	+#endif
	27	+
	28	+using namespace std;
	29	+
	30	+class Optimization
	31	+{
	32	+public:
	33	+ Optimization(){}
	34	+
	35	+ ~Optimization();
	36	+
	37	+ static double wrap(const std::vector<double> &x, std::vector<double> &grad, void *data) {
	38	+ return (reinterpret_cast<Optimization>(data))(x, grad);
	39	+ }
	40	+ double deg2rad(double angDeg);
	41	+ void setMeasurementMatrix(cv::Mat inW);
	42	+ void init();
	43	+ bool isInit() const { return ! W.empty(); }
	44	+
	45	+ void copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat);
	46	+ int _objFuncMethod = OBJFUNC_Rectification;
	47	+
	48	+ enum ObjFuncMethod{
	49	+ OBJFUNC_DistanceSquared,
	50	+ OBJFUNC_Distance,
	51	+ OBJFUNC_PseudoInverse,
	52	+ OBJFUNC_Rectification
	53	+ };
	54	+
	55	+ double _minf;
	56	+ double getMinf() const{ return _minf;}
	57	+
	58	+ int _maxTimeStep1 = 15;
	59	+ int _maxTimeStep2 = 60;
	60	+
	61	+ void setMaxTimeStep1(int maxTime){ _maxTimeStep1 = maxTime; }
	62	+ void setMaxTimeStep2(int maxTime){ _maxTimeStep2 = maxTime; }
	63	+
	64	+ void run();
	65	+
	66	+ double operator() (const std::vector<double> &x, std::vector<double> &grad);
	67	+ double testFunction (const std::vector<double> &x);
	68	+ double distanceSquared (const std::vector<double> &x);
	69	+ double distancePseudoInverse (const std::vector<double> &x);
	70	+ double distanceWminusMMW (const std::vector<double> &x);
	71	+
	72	+ int getParametersNumber();
	73	+ void getInitialConditionsAndBounds(std::vector<double> & _x0, std::vector<double> & _lb, std::vector<double> & _ub);
	74	+ void setObjectiveFunction(int objFuncMethod) { _objFuncMethod = objFuncMethod; _iterCnt = 0;}
	75	+
	76	+ bool isFeaturerScaled = true;
	77	+ void useFeatureScaling(bool b) { isFeaturerScaled = b; }
	78	+
	79	+ double _bestObjValue = HUGE_VAL;
	80	+ int _iterCnt = 0;
	81	+
	82	+ int nbCams, nbPts;
	83	+ cv::Mat W;
	84	+ cv::Mat Win;
	85	+ cv::Mat paramConst; ///< matrix defining if optimization parameter is constant (=0). Size = number of images \f$\times\f$ number of parameters
	86	+ /** matrix defining optimization bounds. Size = number of images \f$\times\f$ number of parameters.
	87	+ * lower bounds = -paramBounds;
	88	+ * upper bounds = +paramBounds;
	89	+ */
	90	+ cv::Mat paramBounds;
	91	+ /** Offset for all parameters. Size = number of images \f$\times\f$ number of parameters.
	92	+ * Actual value is calculated as offset + initial
	93	+ */
	94	+ cv::Mat paramOffset;
	95	+
	96	+ cv::Mat paramInitial; ///< Initial values of parameters. Size = number of images \f$\times\f$ number of parameters
	97	+ cv::Mat paramResult; ///< Resulting values of parameters. Offset + optimization results
	98	+ cv::Mat paramVarIdx; ///< indices of varying parameters in the table of parameters paramConst, paramOffset etc.
	99	+ int nbParams;
	100	+
	101	+
	102	+ ///< indices of each parameter
	103	+ enum{
	104	+ K = 0, ///< scale factor
	105	+ ALPHA, ///< fx/fy, misscaling of axis
	106	+ S, ///< skew
	107	+ RT1, ///< \f$ \theta_1 \f$
	108	+ RR, ///< \f$ \rho \f$
	109	+ RT2, ///< \f$ \theta_2 \f$
	110	+ PARAMS_PER_CAM ///< number of parameters for one camera
	111	+ };
	112	+
	113	+ std::vector<cv::Mat> getCameraMatrices() const;
	114	+ cv::Mat getCalibrationMatrix() const;
	115	+ cv::Mat getRotationAnglesDeg() const;
	116	+ cv::Mat getPoints3D(const std::vector<double> &x) { return getPoints3DfromParams(x);}
	117	+
	118	+
	119	+
	120	+private:
	121	+ std::vector<cv::Mat> tm;
	122	+ void computeWmean(const cv::Mat & Win, cv::Mat & Wmean, std::vector<cv::Mat> & tm);
	123	+ MatCell getMfromParams(const std::vector<double> &x);
	124	+ cv::Mat getPoints3DfromParams(const std::vector<double> &x);
	125	+ cv::Mat getCalibrationMatrixFromParamTable(const cv::Mat &paramTable) const;
	126	+};
	127	+
	128	+
	129	+#endif // OPTIMIZATIONPROBLEM_H
	130	+
	131	+/*nlopt methods
	132	+ * nlopt::opt opt(nlopt::GN_CRS2_LM, optim->getParametersNumber()); // 1
	133	+ //nlopt::opt opt(nlopt::GN_MLSL, paramsNumber); // 2
	134	+ //nlopt::opt opt(nlopt::GN_MLSL_LDS, paramsNumber); // 2
	135	+ //nlopt::opt opt(nlopt::GN_ESCH, paramsNumber); // 3
	136	+ //nlopt::opt opt(nlopt::GN_DIRECT_L, paramsNumber); // 4
	137	+ //nlopt::opt opt(nlopt::GN_DIRECT, paramsNumber); // 4
	138	+ //nlopt::opt opt(nlopt::GN_ORIG_DIRECT, paramsNumber); // 99
	139	+ //nlopt::opt opt(nlopt::GN_ISRES, paramsNumber); // 99
	140	+*/

...	...	@@ -0,0 +1,189 @@
	1	+/**
	2	+ * @brief Robust estimation library
	3	+ *
	4	+ * @author Andrey Kudryavtsev (avkudr.github.io)
	5	+ * @date 01/03/2018
	6	+ * @version 1.0
	7	+ */
	8	+
	9	+#ifndef ROBUST_ESTIMATOR_H
	10	+#define ROBUST_ESTIMATOR_H
	11	+
	12	+#include <vector>
	13	+#include <random>
	14	+#include <numeric>
	15	+#include <functional>
	16	+#include <algorithm>
	17	+#include <iostream>
	18	+#include <iterator>
	19	+
	20	+namespace robest {
	21	+
	22	+class EstimationProblem{
	23	+
	24	+public:
	25	+ // Functions to overload in your class:
	26	+ virtual double estimErrorForSample(int i) = 0;
	27	+ virtual void estimModelFromSamples(std::vector<int> samplesIdx) = 0;
	28	+ virtual int getTotalNbSamples() const = 0;
	29	+
	30	+ int getNbParams() const{return nbParams;}
	31	+ int getNbMinSamples() const{return nbMinSamples;}
	32	+
	33	+protected:
	34	+ void setNbParams(int i) {nbParams = i;}
	35	+ void setNbMinSamples(int i){nbMinSamples = i;}
	36	+
	37	+private:
	38	+ int nbParams = -1;
	39	+ int nbMinSamples = -1;
	40	+};
	41	+
	42	+static std::random_device rd; // random device engine, usually based on /dev/random on UNIX-like systems
	43	+static std::mt19937 rng(rd()); // initialize Mersennes' twister using rd to generate the seed
	44	+//static std::mt19937 rng((unsigned int) - 1);
	45	+
	46	+// Base class for Robust Estimators
	47	+class AbstractEstimator
	48	+{
	49	+public:
	50	+ // Generate X !different! random numbers from 0 to N-1
	51	+ // X - minimal number of samples
	52	+ // N - total number of samples
	53	+ // generated numbers are indices of data points
	54	+
	55	+ std::vector<int> randomSampleIdx(){
	56	+
	57	+ int minNbSamples = problem->getNbMinSamples();
	58	+ int totalNbSamples = problem->getTotalNbSamples();
	59	+
	60	+ std::vector<int> allIdx(totalNbSamples);
	61	+ std::iota (std::begin(allIdx), std::end(allIdx), 0); // Fill with 0, 1, ..., totalNbSamples-1.
	62	+
	63	+ // shuffle the elements of allIdx : Fisher–Yates shuffle
	64	+ for (int i = 0; i < minNbSamples; i++){
	65	+ std::uniform_int_distribution<int> dist(0,totalNbSamples-i-1);
	66	+ int randInt = dist(rng);
	67	+ std::swap(allIdx[totalNbSamples-i-1],allIdx[randInt]);
	68	+ }
	69	+
	70	+ //take last <minNbSamples> elements
	71	+ std::vector<int> idx( allIdx.end() - minNbSamples, allIdx.end());
	72	+// std::cout << "[";
	73	+// for (auto i : idx) std::cout << i << ", ";
	74	+// std::cout << "]\n";
	75	+ return idx;
	76	+ }
	77	+
	78	+ double getInliersFraction() const {return inliersFraction;}
	79	+ std::vector<int> getInliersIndices() const {return inliersIdx;}
	80	+
	81	+protected:
	82	+ void getInliers(double thres){
	83	+ int totalNbSamples = problem->getTotalNbSamples();
	84	+ inliersIdx.clear();
	85	+ for(int j = 0; j < totalNbSamples; j++){
	86	+ double error = problem->estimErrorForSample(j);
	87	+ error = error*error;
	88	+ if (error < thres){
	89	+ inliersIdx.push_back(j);
	90	+ }
	91	+ }
	92	+ this->inliersFraction = (double)(inliersIdx.size()) / (double)(totalNbSamples);
	93	+ }
	94	+
	95	+ EstimationProblem * problem;
	96	+ std::vector<int> bestIdxSet;
	97	+ std::vector<int> inliersIdx;
	98	+ double inliersFraction = -1.0;
	99	+};
	100	+
	101	+class RANSAC : public AbstractEstimator
	102	+{
	103	+public:
	104	+ RANSAC(){
	105	+
	106	+ }
	107	+
	108	+ void solve(EstimationProblem * pb, double thres = 0.1, int nbIter = 10000){
	109	+ problem = pb;
	110	+
	111	+ int totalNbSamples = problem->getTotalNbSamples();
	112	+ for (int i = 0; i < nbIter; i++){
	113	+ std::vector<int> indices = randomSampleIdx();
	114	+
	115	+ problem->estimModelFromSamples(indices);
	116	+
	117	+ //getInliersNb
	118	+ int nbInliers = 0;
	119	+ for(int j = 0; j < totalNbSamples; j++){
	120	+ double error = problem->estimErrorForSample(j);
	121	+ error = error*error;
	122	+ if (error < thres){
	123	+ nbInliers++;
	124	+ }
	125	+ }
	126	+
	127	+ double inliersFraction = (double)(nbInliers) / (double)(problem->getTotalNbSamples());
	128	+ if (inliersFraction > this->inliersFraction){
	129	+ this->inliersFraction = inliersFraction;
	130	+ this->bestIdxSet = indices;
	131	+ }
	132	+ }
	133	+
	134	+ problem->estimModelFromSamples(bestIdxSet);
	135	+ getInliers(thres);
	136	+ problem->estimModelFromSamples(inliersIdx);
	137	+ }
	138	+};
	139	+
	140	+class LMedS : public AbstractEstimator
	141	+{
	142	+public:
	143	+ LMedS(){
	144	+ med = 1000000.0;
	145	+ }
	146	+
	147	+ template <typename T>
	148	+ double median(std::vector<T> & v){
	149	+ std::sort(v.begin(), v.end());
	150	+ if (v.size() % 2 == 0){
	151	+ return (double)(v[v.size()/2-1] + v[v.size()/2]) / 2;
	152	+ }else{
	153	+ return v[v.size()/2];
	154	+ }
	155	+ }
	156	+
	157	+ void solve(EstimationProblem * pb, double thres = 0.1, int nbIter = 1000){
	158	+ problem = pb;
	159	+ int totalNbSamples = problem->getTotalNbSamples();
	160	+
	161	+ for (int i = 0; i < nbIter; i++){
	162	+ std::vector<int> indices = randomSampleIdx();
	163	+
	164	+ problem->estimModelFromSamples(indices);
	165	+
	166	+ std::vector<double> errorsVec(totalNbSamples);
	167	+ for(int j = 0; j < problem->getTotalNbSamples(); j++){
	168	+ double error = problem->estimErrorForSample(j);
	169	+ //errorsVec[j] = error; // error must be squared!
	170	+ errorsVec[j] = error*error; // error must be squared!
	171	+ }
	172	+ double med = median(errorsVec);
	173	+ if (med < this->med){
	174	+ this->med = med;
	175	+ this->bestIdxSet = indices;
	176	+ }
	177	+ }
	178	+
	179	+ problem->estimModelFromSamples(bestIdxSet);
	180	+ getInliers(thres);
	181	+ problem->estimModelFromSamples(inliersIdx);
	182	+ }
	183	+
	184	+private:
	185	+ double med;
	186	+};
	187	+
	188	+}
	189	+#endif // ROBUST_ESTIMATOR_H

...	...	@@ -0,0 +1,32 @@
	1	+#ifndef TRANSFORMATIONS_H
	2	+#define TRANSFORMATIONS_H
	3	+
	4	+/*!
	5	+ \defgroup transformations Transformations
	6	+ \ingroup camera_model Camera Model
	7	+ \brief Different definitions for rotations and translations
	8	+*/
	9	+///@{
	10	+///
	11	+
	12	+
	13	+#include <stdio.h>
	14	+#include <iostream>
	15	+#include <opencv2/opencv.hpp>
	16	+
	17	+#include <cstdlib>
	18	+#include <cmath>
	19	+
	20	+#ifndef PI
	21	+#define PI 3.141592653589793238462643383279502884L
	22	+#endif
	23	+
	24	+//! Construction and decomprosition of rotation matrices
	25	+namespace rotmat{
	26	+ void decomposeEulerZYZ(const cv::Mat & R, double & t1, double & rho, double & t2);
	27	+
	28	+ cv::Mat fromCameraMatrixAffine(const cv::Mat & P);
	29	+ cv::Mat fromEulerZYZ(double t1, double rho, double t2);
	30	+}
	31	+///@}
	32	+#endif

...	...	@@ -0,0 +1,85 @@
	1	+#ifndef TRIANGULATION_H
	2	+#define TRIANGULATION_H
	3	+
	4	+#include <stdio.h>
	5	+#include <iostream>
	6	+#include <opencv2/opencv.hpp>
	7	+#include <opencv2/core/core.hpp>
	8	+#include <opencv2/calib3d/calib3d.hpp>
	9	+#include <opencv2/highgui/highgui.hpp>
	10	+#include <opencv2/imgproc/imgproc.hpp>
	11	+#include <opencv2/calib3d.hpp>
	12	+
	13	+#define NOMINMAX ///< needed for OpenCV functionning
	14	+
	15	+#ifdef _WIN32
	16	+#include <windows.h> // required by 1394camapi.h
	17	+#include <strsafe.h> // required by 1394camapi.h
	18	+#endif
	19	+
	20	+#include <vector>
	21	+#include <string>
	22	+#include <iostream>
	23	+#include <fstream>
	24	+#include <list>
	25	+#include <set>
	26	+
	27	+#include <cstdlib>
	28	+#include <cmath>
	29	+
	30	+//#include <core/core.hpp>
	31	+#include "transformations.h"
	32	+
	33	+#include <pcl/point_types.h>
	34	+#include <pcl/io/pcd_io.h>
	35	+#include <pcl/io/io.h>
	36	+#include <pcl/visualization/pcl_plotter.h>
	37	+
	38	+#undef max
	39	+
	40	+class Triangulator
	41	+{
	42	+
	43	+public:
	44	+
	45	+ Triangulator();
	46	+ Triangulator(int method);
	47	+
	48	+ //! Possible triangulation methods :
	49	+ enum { LINEAR_LS, LINEAR_EIGEN, ITERATIVE_LS, ITERATIVE_EIGEN, DIRECT_AFFINE};
	50	+
	51	+ void setTriangulationMethod(int method);
	52	+
	53	+ double getReprojectionError();
	54	+
	55	+ void triangulate(const cv::Mat & W,
	56	+ const cv::Mat & P,
	57	+ pcl::PointCloud<pcl::PointXYZ> & pointCloud);
	58	+
	59	+ /*void triangulatePoints(const std::vector<cv::KeyPoint>& pt_set1,
	60	+ const std::vector<cv::KeyPoint>& pt_set2,
	61	+ const cv::Mat&Kinv,
	62	+ const cv::Matx34d& P,
	63	+ const cv::Matx34d& P1,
	64	+ std::vector<pcl::PointXYZ>& pointcloud);*/
	65	+
	66	+ pcl::PointCloud<pcl::PointXYZ> triangulatePoints_LinearLS(const cv::Mat & Wf, // Full measurement matrix 2i x p
	67	+ const cv::Mat & Pm // Camera matrices 3i x 4
	68	+ ); // 4 x p
	69	+
	70	+ void triangulateAffine(const cv::Mat & W,
	71	+ const cv::Mat & P,
	72	+ pcl::PointCloud<pcl::PointXYZ> & pointCloud);
	73	+
	74	+private:
	75	+ int triangulationMethod;
	76	+ double reprojectionError;
	77	+
	78	+ //cv::Mat_<double> linearLS( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
	79	+
	80	+ //cv::Mat_<double> linearEigen( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
	81	+
	82	+ //cv::Mat_<double> iterativeLSandEigen( cv::Point3d u, cv::Matx34d P, cv::Point3d u1, cv::Matx34d P1);
	83	+};
	84	+///@}
	85	+#endif

...	...	@@ -0,0 +1,32 @@
	1	+#ifndef UTILS_H
	2	+#define UTILS_H
	3	+
	4	+#include <iostream>
	5	+#include <fstream>
	6	+
	7	+#include <opencv2/opencv.hpp>
	8	+
	9	+#ifndef PI
	10	+#define PI 3.141592653589793238462643383279502884L
	11	+#endif
	12	+
	13	+namespace util{
	14	+
	15	+void saveFileToMatlab(std::string fileName, cv::Mat a, std::string varName);
	16	+
	17	+double rad2deg(double angRad);
	18	+double deg2rad(double angDeg);
	19	+
	20	+void makeNonHomogenious(cv::Mat & m);
	21	+void copyMatElementsToVector(const cv::Mat & mat, const cv::Mat & idx, std::vector<double> & vec);
	22	+void copyVectorToMatElements(const std::vector<double> & vec, const cv::Mat & idx, cv::Mat & mat);
	23	+
	24	+enum{
	25	+ CONCAT_HORIZONTAL,
	26	+ CONCAT_VERTICAL,
	27	+};
	28	+cv::Mat concatenateMat(const std::vector<cv::Mat> & matArray, int method = CONCAT_VERTICAL);
	29	+
	30	+}
	31	+
	32	+#endif // UTILS_H

...	...	@@ -0,0 +1,165 @@
	1	+ GNU LESSER GENERAL PUBLIC LICENSE
	2	+ Version 3, 29 June 2007
	3	+
	4	+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
	5	+ Everyone is permitted to copy and distribute verbatim copies
	6	+ of this license document, but changing it is not allowed.
	7	+
	8	+
	9	+ This version of the GNU Lesser General Public License incorporates
	10	+the terms and conditions of version 3 of the GNU General Public
	11	+License, supplemented by the additional permissions listed below.
	12	+
	13	+ 0. Additional Definitions.
	14	+
	15	+ As used herein, "this License" refers to version 3 of the GNU Lesser
	16	+General Public License, and the "GNU GPL" refers to version 3 of the GNU
	17	+General Public License.
	18	+
	19	+ "The Library" refers to a covered work governed by this License,
	20	+other than an Application or a Combined Work as defined below.
	21	+
	22	+ An "Application" is any work that makes use of an interface provided
	23	+by the Library, but which is not otherwise based on the Library.
	24	+Defining a subclass of a class defined by the Library is deemed a mode
	25	+of using an interface provided by the Library.
	26	+
	27	+ A "Combined Work" is a work produced by combining or linking an
	28	+Application with the Library. The particular version of the Library
	29	+with which the Combined Work was made is also called the "Linked
	30	+Version".
	31	+
	32	+ The "Minimal Corresponding Source" for a Combined Work means the
	33	+Corresponding Source for the Combined Work, excluding any source code
	34	+for portions of the Combined Work that, considered in isolation, are
	35	+based on the Application, and not on the Linked Version.
	36	+
	37	+ The "Corresponding Application Code" for a Combined Work means the
	38	+object code and/or source code for the Application, including any data
	39	+and utility programs needed for reproducing the Combined Work from the
	40	+Application, but excluding the System Libraries of the Combined Work.
	41	+
	42	+ 1. Exception to Section 3 of the GNU GPL.
	43	+
	44	+ You may convey a covered work under sections 3 and 4 of this License
	45	+without being bound by section 3 of the GNU GPL.
	46	+
	47	+ 2. Conveying Modified Versions.
	48	+
	49	+ If you modify a copy of the Library, and, in your modifications, a
	50	+facility refers to a function or data to be supplied by an Application
	51	+that uses the facility (other than as an argument passed when the
	52	+facility is invoked), then you may convey a copy of the modified
	53	+version:
	54	+
	55	+ a) under this License, provided that you make a good faith effort to
	56	+ ensure that, in the event an Application does not supply the
	57	+ function or data, the facility still operates, and performs
	58	+ whatever part of its purpose remains meaningful, or
	59	+
	60	+ b) under the GNU GPL, with none of the additional permissions of
	61	+ this License applicable to that copy.
	62	+
	63	+ 3. Object Code Incorporating Material from Library Header Files.
	64	+
	65	+ The object code form of an Application may incorporate material from
	66	+a header file that is part of the Library. You may convey such object
	67	+code under terms of your choice, provided that, if the incorporated
	68	+material is not limited to numerical parameters, data structure
	69	+layouts and accessors, or small macros, inline functions and templates
	70	+(ten or fewer lines in length), you do both of the following:
	71	+
	72	+ a) Give prominent notice with each copy of the object code that the
	73	+ Library is used in it and that the Library and its use are
	74	+ covered by this License.
	75	+
	76	+ b) Accompany the object code with a copy of the GNU GPL and this license
	77	+ document.
	78	+
	79	+ 4. Combined Works.
	80	+
	81	+ You may convey a Combined Work under terms of your choice that,
	82	+taken together, effectively do not restrict modification of the
	83	+portions of the Library contained in the Combined Work and reverse
	84	+engineering for debugging such modifications, if you also do each of
	85	+the following:
	86	+
	87	+ a) Give prominent notice with each copy of the Combined Work that
	88	+ the Library is used in it and that the Library and its use are
	89	+ covered by this License.
	90	+
	91	+ b) Accompany the Combined Work with a copy of the GNU GPL and this license
	92	+ document.
	93	+
	94	+ c) For a Combined Work that displays copyright notices during
	95	+ execution, include the copyright notice for the Library among
	96	+ these notices, as well as a reference directing the user to the
	97	+ copies of the GNU GPL and this license document.
	98	+
	99	+ d) Do one of the following:
	100	+
	101	+ 0) Convey the Minimal Corresponding Source under the terms of this
	102	+ License, and the Corresponding Application Code in a form
	103	+ suitable for, and under terms that permit, the user to
	104	+ recombine or relink the Application with a modified version of
	105	+ the Linked Version to produce a modified Combined Work, in the
	106	+ manner specified by section 6 of the GNU GPL for conveying
	107	+ Corresponding Source.
	108	+
	109	+ 1) Use a suitable shared library mechanism for linking with the
	110	+ Library. A suitable mechanism is one that (a) uses at run time
	111	+ a copy of the Library already present on the user's computer
	112	+ system, and (b) will operate properly with a modified version
	113	+ of the Library that is interface-compatible with the Linked
	114	+ Version.
	115	+
	116	+ e) Provide Installation Information, but only if you would otherwise
	117	+ be required to provide such information under section 6 of the
	118	+ GNU GPL, and only to the extent that such information is
	119	+ necessary to install and execute a modified version of the
	120	+ Combined Work produced by recombining or relinking the
	121	+ Application with a modified version of the Linked Version. (If
	122	+ you use option 4d0, the Installation Information must accompany
	123	+ the Minimal Corresponding Source and Corresponding Application
	124	+ Code. If you use option 4d1, you must provide the Installation
	125	+ Information in the manner specified by section 6 of the GNU GPL
	126	+ for conveying Corresponding Source.)
	127	+
	128	+ 5. Combined Libraries.
	129	+
	130	+ You may place library facilities that are a work based on the
	131	+Library side by side in a single library together with other library
	132	+facilities that are not Applications and are not covered by this
	133	+License, and convey such a combined library under terms of your
	134	+choice, if you do both of the following:
	135	+
	136	+ a) Accompany the combined library with a copy of the same work based
	137	+ on the Library, uncombined with any other library facilities,
	138	+ conveyed under the terms of this License.
	139	+
	140	+ b) Give prominent notice with the combined library that part of it
	141	+ is a work based on the Library, and explaining where to find the
	142	+ accompanying uncombined form of the same work.
	143	+
	144	+ 6. Revised Versions of the GNU Lesser General Public License.
	145	+
	146	+ The Free Software Foundation may publish revised and/or new versions
	147	+of the GNU Lesser General Public License from time to time. Such new
	148	+versions will be similar in spirit to the present version, but may
	149	+differ in detail to address new problems or concerns.
	150	+
	151	+ Each version is given a distinguishing version number. If the
	152	+Library as you received it specifies that a certain numbered version
	153	+of the GNU Lesser General Public License "or any later version"
	154	+applies to it, you have the option of following the terms and
	155	+conditions either of that published version or of any later version
	156	+published by the Free Software Foundation. If the Library as you
	157	+received it does not specify a version number of the GNU Lesser
	158	+General Public License, you may choose any version of the GNU Lesser
	159	+General Public License ever published by the Free Software Foundation.
	160	+
	161	+ If the Library as you received it specifies that a proxy can decide
	162	+whether future versions of the GNU Lesser General Public License shall
	163	+apply, that proxy's public statement of acceptance of any version is
	164	+permanent authorization for you to choose that version for the
	165	+Library.

...	...	@@ -0,0 +1,167 @@
	1	+#include "3dReconst.h"
	2	+#include "Autocalib.h"
	3	+
	4	+
	5	+//rectifier epipolar Lines and image
	6	+vector<cv::Mat> Reconst::RectificaEpipolarLines(vector<cv::Mat> image, vector<vector<cv::Point2d> > MeasureVector, vector<cv::Mat> fundMatrix)
	7	+{
	8	+ vector<cv::Mat> outImage;
	9	+ vector<cv::Mat> tempImage=image;
	10	+ vector<vector<cv::Point2d> > tempMeasureVector=MeasureVector;
	11	+
	12	+ cv::Mat Frect = (cv::Mat_<double>(3,3) << 0,0,0,0,0,-1.0,0,1.0,0);
	13	+ cout << Frect << endl;
	14	+ Autocalib calib;
	15	+
	16	+ for (int i=0;i<tempImage.size()-1;i++)
	17	+ {
	18	+ RectifierAffine *rec = new RectifierAffine();
	19	+ rec->init(&tempImage[i],&tempImage[i+1],&fundMatrix[i],&MeasureVector[i],&MeasureVector[i+1]);
	20	+ rec->rectify();
	21	+
	22	+ cv::Mat input1, input2;
	23	+ rec->getResult(input1, input2,&MeasureVector[i],&MeasureVector[i+1]);
	24	+ outImage.push_back(input1);
	25	+ outImage.push_back(input2);
	26	+
	27	+ cv::Mat retifImg;
	28	+ retifImg=calib.plotStereoWithEpilines(outImage[2i],outImage[2i+1],Frect,MeasureVector[i],MeasureVector[i+1]);
	29	+ cv::namedWindow( "Epipolar lines: rectified", cv::WINDOW_NORMAL );
	30	+ cv::imshow("Epipolar lines: rectified", retifImg);
	31	+
	32	+ tempMeasureVector[i+1].swap(MeasureVector[i+1]);
	33	+
	34	+ cv::waitKey(0);
	35	+ }
	36	+ return outImage;
	37	+}
	38	+
	39	+//get disparity map and the dense point
	40	+void Reconst::disparity(vector<cv::Mat> image)
	41	+{
	42	+ DenseMatcher DM;
	43	+ for (int i=0;i<image.size()/2;i++)
	44	+ {
	45	+ DM.init(&image[2i],&image[2i+1]);
	46	+ DM.calculateDisparityMap();
	47	+ DM.plotDisparityMap();
	48	+
	49	+ cv::Mat densePt=DM.getDensePoint();
	50	+ _densePoint.push_back(densePt);
	51	+
	52	+ }
	53	+}
	54	+
	55	+
	56	+//do the triangulation and get 3D point cloud
	57	+void Reconst::GetPointCloud(vector<cv::Mat> P, cv::Mat image )
	58	+{
	59	+
	60	+ vector<cv::Mat> rectifiedMeseureMatrice=_densePoint;
	61	+ vector<cv::Mat> cameraMatrix=GetCameraMatrix(P);
	62	+ //cout<<rectifiedMeseureMatrice[1].cols<< endl;
	63	+ pcl::PointCloud<pcl::PointXYZ> allPointCloud;
	64	+ Triangulator triang;
	65	+ for (int i=0;i<cameraMatrix.size();i++)
	66	+ {
	67	+ pcl::PointCloud<pcl::PointXYZ> ptCloud;
	68	+ ptCloud=triang.triangulatePoints_LinearLS(_densePoint[i], cameraMatrix[i]);
	69	+
	70	+ //ptCloud.is_dense = true;
	71	+ int nbr=i;
	72	+ //save 3D point cloud in a file
	73	+ string name = "pointCloud"+to_string(nbr)+".pcd";
	74	+ pcl::io::savePCDFileASCII (name, ptCloud);
	75	+ cerr << "Saved " << ptCloud.points.size () << " data points to pointCloud" <<i<<".pcd." << endl;
	76	+ }
	77	+}
	78	+
	79	+
	80	+vector<cv::Mat> Reconst::GetCameraMatrix(vector<cv::Mat> P)
	81	+{
	82	+ vector<cv::Mat> CameraMatrix;
	83	+ CameraMatrix.resize(P.size()-1);
	84	+ for(int i=0;i<P.size()-1;i++)
	85	+ {
	86	+ CameraMatrix[i].push_back(P[i]);
	87	+ CameraMatrix[i].push_back(P[i+1]);
	88	+ }
	89	+ return CameraMatrix;
	90	+}
	91	+
	92	+
	93	+void Reconst::filterPointCloud(int taille)
	94	+{
	95	+ for(int i=0;i<taille;i++ )
	96	+ {
	97	+ std::stringstream ss1;
	98	+ ss1 << "pointCloud" <<std::to_string(i)<<".pcd" ;
	99	+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	100	+
	101	+ pcl::PCDWriter writer;
	102	+
	103	+ // Fill in the cloud data
	104	+ pcl::PCDReader reader;
	105	+ // Replace the path below with the path where you saved your file
	106	+ reader.read<pcl::PointXYZ> (ss1.str (), *cloud);
	107	+
	108	+ std::cerr << "Cloud before filtering: " << std::endl;
	109	+ std::cerr << *cloud << std::endl;
	110	+
	111	+ // Creating the KdTree object for the search method of the extraction
	112	+ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	113	+ tree->setInputCloud (cloud);
	114	+
	115	+ std::vector<pcl::PointIndices> cluster_indices;
	116	+ pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
	117	+ ec.setClusterTolerance (8); // 2cm
	118	+ ec.setMinClusterSize (30);
	119	+ ec.setMaxClusterSize (1000000);
	120	+ ec.setSearchMethod (tree);
	121	+ ec.setInputCloud (cloud);
	122	+ ec.extract (cluster_indices);
	123	+
	124	+ if(cluster_indices.size()==0){ std::cout<<"no data"<<std::endl; }
	125	+
	126	+
	127	+ int cloudSize=0;
	128	+ for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
	129	+ {
	130	+ pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
	131	+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
	132	+ cloud_cluster->points.push_back (cloud->points[pit]); //
	133	+ cloud_cluster->width = cloud_cluster->points.size ();
	134	+ cloud_cluster->height = 1;
	135	+ cloud_cluster->is_dense = true;
	136	+ if(cloud_cluster->points.size ()>cloudSize)
	137	+ {
	138	+ std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
	139	+ std::stringstream ss2;
	140	+ ss2 << "pointCloud_filtered" <<std::to_string(i)<< ".pcd";
	141	+ writer.write<pcl::PointXYZ> (ss2.str (), *cloud_cluster, false);
	142	+ cloudSize=cloud_cluster->points.size ();
	143	+ } //*
	144	+
	145	+ }
	146	+
	147	+ }
	148	+
	149	+
	150	+}
	151	+
	152	+
	153	+
	154	+
	155	+
	156	+
	157	+
	158	+
	159	+
	160	+
	161	+
	162	+
	163	+
	164	+
	165	+
	166	+
	167	+

...	...	@@ -0,0 +1,321 @@
	1	+#include <Autocalib.h>
	2	+
	3	+vector<vector<cv::Point2d> > Autocalib::findFeaturePoint(vector<cv::Mat> img,double qualityLevel )
	4	+{
	5	+ vector<vector<cv::Point2d> > MatchVector;//measurement matrix
	6	+
	7	+ vector<cv::KeyPoint> kpts1;
	8	+ cv::Mat desc1;
	9	+ cv::Ptr<cv::AKAZE> akaze =cv::AKAZE::create();
	10	+ akaze->setThreshold(qualityLevel);
	11	+ akaze->detectAndCompute(img[0], cv::noArray(), kpts1, desc1);
	12	+
	13	+ //Step 1. Extract features AKAZE
	14	+ for(int i=1;i<img.size();i++)
	15	+ {
	16	+
	17	+ vector<cv::KeyPoint> kpts2;
	18	+ cv::Mat desc2;
	19	+ akaze->detectAndCompute(img[i], cv::noArray(), kpts2, desc2);
	20	+
	21	+ cv::BFMatcher matcher(cv::NORM_HAMMING);
	22	+ vector<vector<cv::DMatch> > nn_matches;
	23	+ matcher.knnMatch(desc1, desc2, nn_matches, 2);
	24	+ vector<cv::KeyPoint> matched1, matched2;
	25	+ vector<cv::DMatch> good_matches;
	26	+ double nnMatchRatio = 0.5;
	27	+ for(int j = 0; j < nn_matches.size(); j++)
	28	+ {
	29	+ cv::DMatch match = nn_matches[j][0];
	30	+ float dist1 = nn_matches[j][0].distance;
	31	+ float dist2 = nn_matches[j][1].distance;
	32	+ if(dist1 < nnMatchRatio * dist2)
	33	+ {
	34	+ good_matches.push_back(match);
	35	+ matched1.push_back(kpts1[match.queryIdx]);
	36	+ matched2.push_back(kpts2[match.trainIdx]);
	37	+ }
	38	+ }
	39	+
	40	+ //step 2.Draw matches and display
	41	+ cv::Mat img_matches;
	42	+ cv::drawMatches( img[i-1], kpts1, img[i], kpts2, good_matches, img_matches );
	43	+ cv::namedWindow( "Matches", cv::WINDOW_NORMAL);
	44	+ cv::imshow("Matches", img_matches );
	45	+ cv::waitKey(0);
	46	+ desc1=desc2.clone();
	47	+ kpts1.swap(kpts2);
	48	+
	49	+ //step 3.Populate data
	50	+ vector<cv::Point2d> imgpts1, imgpts2;
	51	+ for(int k = 0; k < matched1.size(); k++ )
	52	+ {
	53	+ imgpts1.push_back(matched1[k].pt); // queryIdx is the "left" image
	54	+ imgpts2.push_back(matched2[k].pt); // trainIdx is the "right" image
	55	+ //cout << imgpts1[k]<< endl;
	56	+ }
	57	+
	58	+ MatchVector.push_back(imgpts1);
	59	+ MatchVector.push_back(imgpts2);
	60	+ }
	61	+
	62	+
	63	+ return MatchVector;
	64	+}
	65	+
	66	+//search the element which has point correpond with all of others image
	67	+//new function
	68	+vector<vector<cv::Point2d> > Autocalib::getMeasureVector(vector<vector<cv::Point2d> > MatchVector)
	69	+{
	70	+ vector<vector<cv::Point2d> > measureVector;
	71	+ vector<vector<cv::Point2d> > tempMeasureVector;
	72	+ measureVector.push_back(MatchVector[0]);
	73	+ measureVector.push_back(MatchVector[1]);
	74	+
	75	+
	76	+ for (int i=2;i<= MatchVector.size()/2;i++)
	77	+ {
	78	+ vector<cv::Point2d> ::iterator indxMatch;
	79	+ vector<cv::Point2d> tempVector;
	80	+
	81	+
	82	+ for (int j=0;j<measureVector[i-1].size();j++ )
	83	+ {
	84	+ indxMatch = find(MatchVector[2i-2].begin(),MatchVector[2i-2].end(),measureVector[i-1][j] );
	85	+ if (indxMatch!=MatchVector[2*i-2].end())
	86	+ {
	87	+ int dis=distance(MatchVector[2*i-2].begin(),indxMatch);
	88	+ tempVector.push_back(MatchVector[2*i-1][dis]);
	89	+
	90	+ }
	91	+ else
	92	+ {
	93	+ for(int n=i-1;n>=0;n--)
	94	+ {
	95	+ measureVector[n][j].x=0;
	96	+ measureVector[n][j].y=0;
	97	+ }
	98	+ }
	99	+
	100	+ }
	101	+ measureVector.push_back(tempVector);
	102	+
	103	+
	104	+ tempMeasureVector=searchAllMatch(measureVector);
	105	+ measureVector.clear();
	106	+
	107	+ measureVector.swap(tempMeasureVector);
	108	+ tempMeasureVector.clear();
	109	+
	110	+ }
	111	+
	112	+ return measureVector;
	113	+}
	114	+
	115	+//remove the element which contain {0,0}(dosen't have correspond point with all of others image )
	116	+//new function
	117	+vector<vector<cv::Point2d> > Autocalib::searchAllMatch(vector<vector<cv::Point2d> > vector)
	118	+{
	119	+ cv::Point2d pt{0,0};
	120	+
	121	+ for (int i=0; i<vector.size();i++)
	122	+ {
	123	+ std::vector<cv::Point2d> ::iterator indxNotMatch;
	124	+ std::vector<cv::Point2d> ::iterator tempIndx;
	125	+
	126	+ for(indxNotMatch=vector[i].begin();indxNotMatch!=vector[i].end();)
	127	+ {
	128	+ if(*indxNotMatch==pt)
	129	+ {
	130	+ tempIndx=indxNotMatch;
	131	+ indxNotMatch=vector[i].erase(tempIndx);
	132	+ }
	133	+ else { indxNotMatch++; }
	134	+ }
	135	+ }
	136	+
	137	+ return vector;
	138	+}
	139	+
	140	+//transfert vector<vector<Point2d> > to Mat
	141	+//new function
	142	+cv::Mat Autocalib::vectorPoint2Mat(vector<vector<cv::Point2d> > vector)
	143	+{
	144	+ cv::Mat M(vector.size()*2, vector[0].size(),CV_64F);
	145	+
	146	+ for (int i=0;i< vector.size();i++)
	147	+ {
	148	+ for(int j=0; j<vector[i].size();j++)
	149	+ {
	150	+ M.at<double>(2*i,j)=vector[i][j].x;
	151	+ M.at<double>(2*i+1,j)=vector[i][j].y;
	152	+ }
	153	+ }
	154	+
	155	+ return M;
	156	+}
	157	+
	158	+
	159	+vector<cv::Mat> Autocalib::estimatFundMatVector(vector<vector<cv::Point2d> > MeasureVector)
	160	+{
	161	+ vector<cv::Mat> FundMatVector;
	162	+
	163	+ for(int i=1; i< MeasureVector.size();i++)
	164	+ {
	165	+ FundMatFitting * fmestim = new FundMatFitting();
	166	+ fmestim->setData(MeasureVector[i-1],MeasureVector[i]);
	167	+ robest::LMedS * ransacSolver = new robest::LMedS();
	168	+ ransacSolver->solve(fmestim);
	169	+ cv::Mat FM = (cv::Mat_<double>(3,3));
	170	+ FM = fmestim->getResult();
	171	+ FundMatVector.push_back(FM);
	172	+ }
	173	+
	174	+ return FundMatVector;
	175	+
	176	+}
	177	+
	178	+
	179	+
	180	+
	181	+cv::Mat Autocalib::plotStereoWithEpilines(cv::Mat img1,cv::Mat img2,cv::Mat F,vector<cv::Point2d> pts1,vector<cv::Point2d> pts2)
	182	+{
	183	+ cv::Mat epilines1;
	184	+ cv::Mat epilines2 ;
	185	+
	186	+ cv::computeCorrespondEpilines(pts1,1, F,epilines2);
	187	+ cv::computeCorrespondEpilines(pts2,2, F,epilines1);
	188	+
	189	+ CV_Assert(img1.size() == img2.size() && img1.type() == img2.type());
	190	+ cv::Mat outImg(img1.rows, img1.cols*2, CV_8UC3);
	191	+ cv::Rect rect1(0,0, img1.cols, img1.rows);
	192	+ cv::Rect rect2(img1.cols, 0, img2.cols, img2.rows);
	193	+
	194	+ if (img1.type() == CV_8U)
	195	+ {
	196	+ cv::cvtColor(img1, outImg(rect1), cv::COLOR_GRAY2BGR);
	197	+ cv::cvtColor(img2, outImg(rect2), cv::COLOR_GRAY2BGR);
	198	+ }
	199	+ else
	200	+ {
	201	+ img1.copyTo(outImg(rect1));
	202	+ img2.copyTo(outImg(rect2));
	203	+ }
	204	+
	205	+ cv::RNG rng;
	206	+
	207	+ for(int i=0; i<pts1.size(); i++)
	208	+ {
	209	+ cv::Scalar color = cv::Scalar(rng.uniform(0,255), rng.uniform(0, 255), rng.uniform(0, 255));
	210	+
	211	+ std::vector<cv::Point> intesecPoints;
	212	+ intesecPoints.push_back(cv::Point(0,-epilines2.at<double>(i,2)/epilines2.at<double>(i,1)));
	213	+ intesecPoints.push_back(cv::Point(img2.cols,-(epilines2.at<double>(i,2)+epilines2.at<double>(i,0)*img2.cols)/epilines2.at<double>(i,1)));
	214	+ intesecPoints.push_back(cv::Point(-epilines2.at<double>(i,2)/epilines2.at<double>(i,0),0));
	215	+ intesecPoints.push_back(cv::Point(-(epilines2.at<double>(i,2)+epilines2.at<double>(i,1)*img2.rows)/epilines2.at<double>(i,0),img2.rows));
	216	+
	217	+ if ( intesecPoints[3].x < 0 \|\| intesecPoints[3].x > img2.cols )
	218	+ intesecPoints.erase( intesecPoints.begin() + 3);
	219	+ if ( intesecPoints[2].x < 0 \|\| intesecPoints[2].x > img2.cols )
	220	+ intesecPoints.erase( intesecPoints.begin() + 2);
	221	+ if ( intesecPoints[1].y < 0 \|\| intesecPoints[1].y > img2.rows )
	222	+ intesecPoints.erase( intesecPoints.begin() + 1);
	223	+ if ( intesecPoints[0].y < 0 \|\| intesecPoints[0].y > img2.rows )
	224	+ intesecPoints.erase( intesecPoints.begin() + 0);
	225	+
	226	+ cv::line(outImg(rect2),
	227	+ intesecPoints[0],
	228	+ intesecPoints[1],
	229	+ color, 1, cv::LINE_AA);
	230	+ cv::circle(outImg(rect2), pts2[i], 3, color, -1, cv::LINE_AA);
	231	+
	232	+ intesecPoints.clear();
	233	+
	234	+ intesecPoints.push_back(cv::Point(0,-epilines1.at<double>(i,2)/epilines1.at<double>(i,1)));
	235	+ intesecPoints.push_back(cv::Point(img1.cols,-(epilines1.at<double>(i,2)+epilines1.at<double>(i,0)*img1.cols)/epilines1.at<double>(i,1)));
	236	+ intesecPoints.push_back(cv::Point(-epilines1.at<double>(i,2)/epilines1.at<double>(i,0),0));
	237	+ intesecPoints.push_back(cv::Point(-(epilines1.at<double>(i,2)+epilines1.at<double>(i,1)*img1.rows)/epilines1.at<double>(i,0),img1.rows));
	238	+
	239	+ if ( intesecPoints[3].x < 0 \|\| intesecPoints[3].x > img1.cols )
	240	+ intesecPoints.erase( intesecPoints.begin() + 3);
	241	+ if ( intesecPoints[2].x < 0 \|\| intesecPoints[2].x > img1.cols )
	242	+ intesecPoints.erase( intesecPoints.begin() + 2);
	243	+ if ( intesecPoints[1].y < 0 \|\| intesecPoints[1].y > img1.rows )
	244	+ intesecPoints.erase( intesecPoints.begin() + 1);
	245	+ if ( intesecPoints[0].y < 0 \|\| intesecPoints[0].y > img1.rows )
	246	+ intesecPoints.erase( intesecPoints.begin() + 0);
	247	+
	248	+ cv::line(outImg(rect1),
	249	+ intesecPoints[0],
	250	+ intesecPoints[1],
	251	+ color, 1, cv::LINE_AA);
	252	+ cv::circle(outImg(rect1), pts1[i], 3, color, -1, cv::LINE_AA);
	253	+
	254	+ intesecPoints.clear();
	255	+ }
	256	+
	257	+ return outImg;
	258	+}
	259	+
	260	+
	261	+void Autocalib::showEpipolarLines(vector<cv::Mat> image, vector<vector<cv::Point2d> > measureVector, vector<cv::Mat> fundMatrix)
	262	+{
	263	+ for (int i=1;i<image.size();i++)
	264	+ {
	265	+ cv::Mat outImage=plotStereoWithEpilines(image[i-1],image[i],fundMatrix[i-1],measureVector[i-1],measureVector[i]);
	266	+ cv::namedWindow( "Epipolar lines", cv::WINDOW_NORMAL );
	267	+ cv::imshow("Epipolar lines", outImage);
	268	+ cv::waitKey(0);
	269	+ }
	270	+}
	271	+
	272	+
	273	+
	274	+vector <cv::Mat> Autocalib::estimatParameter(cv::Mat measureMatrix)
	275	+{
	276	+ Optimization opt;
	277	+ opt.setMeasurementMatrix(measureMatrix);
	278	+ opt.run();
	279	+ cv::Mat A=opt.getCalibrationMatrix();
	280	+ cout << "calibration matrix " << A << endl;
	281	+ cv::Mat X=opt.getRotationAnglesDeg();
	282	+ cout << "parametres de camera "<<endl << X << endl;
	283	+ return opt.getCameraMatrices();
	284	+}
	285	+
	286	+
	287	+
	288	+
	289	+
	290	+
	291	+
	292	+
	293	+
	294	+
	295	+
	296	+
	297	+
	298	+
	299	+
	300	+
	301	+
	302	+
	303	+
	304	+
	305	+
	306	+
	307	+
	308	+
	309	+
	310	+
	311	+
	312	+
	313	+
	314	+
	315	+
	316	+
	317	+
	318	+
	319	+
	320	+
	321	+

...	...	@@ -0,0 +1,63 @@
	1	+#include "FundMatFitting.hpp"
	2	+
	3	+void FundMatFitting::setData(std::vector<cv::Point2d> pts1, std::vector<cv::Point2d> pts2){
	4	+
	5	+ for (int i = 0; i < pts1.size(); i++){
	6	+ correspondences.push_back(Correspondence(pts1[i],pts2[i]));
	7	+ }
	8	+}
	9	+
	10	+double FundMatFitting::estimErrorForSample(int i)
	11	+{
	12	+ double x2 = correspondences[i].second.x;
	13	+ double y2 = correspondences[i].second.y;
	14	+ double x1 = correspondences[i].first.x;
	15	+ double y1 = correspondences[i].first.y;
	16	+
	17	+ double error = (ax2 + by2 +cx1 + dy1 + 1) / sqrt(aa + bb + cc + dd); //H&Z, p.350
	18	+ return error;
	19	+}
	20	+
	21	+void FundMatFitting::estimModelFromSamples(std::vector<int> samplesIdx)
	22	+{
	23	+ int nbPts = 0;
	24	+ nbPts = (int)samplesIdx.size();
	25	+
	26	+ cv::Mat W = cv::Mat::zeros(4, nbPts, cv::DataType<double>::type);
	27	+
	28	+ for (int i = 0; i < nbPts; i++){
	29	+ W.at<double>(0,i) = correspondences[samplesIdx[i]].second.x;
	30	+ W.at<double>(1,i) = correspondences[samplesIdx[i]].second.y;
	31	+ W.at<double>(2,i) = correspondences[samplesIdx[i]].first.x;
	32	+ W.at<double>(3,i) = correspondences[samplesIdx[i]].first.y;
	33	+ }
	34	+
	35	+ cv::Mat meanW = cv::Mat::zeros(4, 1, cv::DataType<double>::type);
	36	+
	37	+ meanW.at<double>(0) = cv::mean(W.row(0))[0];
	38	+ meanW.at<double>(1) = cv::mean(W.row(1))[0];
	39	+ meanW.at<double>(2) = cv::mean(W.row(2))[0];
	40	+ meanW.at<double>(3) = cv::mean(W.row(3))[0];
	41	+
	42	+ for (int i = 0; i < nbPts; i++){
	43	+ for (int j = 0; j < 4; j++){
	44	+ W.at<double>(j,i) = W.at<double>(j,i) - meanW.at<double>(j);
	45	+ }
	46	+ }
	47	+
	48	+ cv::Mat S, U, Vt;
	49	+ cv::SVD::compute(W.t(), S, U, Vt);
	50	+ cv::Mat V = Vt.t();
	51	+ cv::Mat N = V.col(V.cols - 1);
	52	+
	53	+ double e = cv::Mat(-1N.t()meanW).at<double>(0,0);
	54	+ a = N.at<double>(0) / e;
	55	+ b = N.at<double>(1) / e;
	56	+ c = N.at<double>(2) / e;
	57	+ d = N.at<double>(3) / e;
	58	+}
	59	+
	60	+bool FundMatFitting::isDegenerate(std::vector<int> samplesIdx)
	61	+{
	62	+ return false;
	63	+}

...	...	@@ -0,0 +1,135 @@
	1	+#include "RectifierAffine.hpp"
	2	+
	3	+RectifierAffine::RectifierAffine()
	4	+{
	5	+
	6	+}
	7	+
	8	+RectifierAffine::~RectifierAffine()
	9	+{
	10	+
	11	+}
	12	+
	13	+void RectifierAffine::init(cv::Matim1, cv::Mat im2, cv::Mat F, std::vector<cv::Point2d> inliers1, std::vector<cv::Point2d>* inliers2) {
	14	+ _imL = im1;
	15	+ _imR = im2;
	16	+ _F = F;
	17	+
	18	+ _inliers1 = inliers1;
	19	+ _inliers2 = inliers2;
	20	+}
	21	+
	22	+void RectifierAffine::rectify()
	23	+{
	24	+ std::cout << "Rectification started... " << std::endl;
	25	+ cv::Mat epR;
	26	+ cv::Mat epL ;
	27	+
	28	+ cv::computeCorrespondEpilines(_inliers1,1, _F,epR);
	29	+ cv::computeCorrespondEpilines(_inliers2, 2, _F,epL);
	30	+
	31	+
	32	+ //std::cout << _F.toMat() << "sdsdsds\n\n" << _F.toMat().t() << "\n\n";
	33	+
	34	+ angleL = atan(-epL.at<double>(0)/epL.at<double>(1));
	35	+ angleR = atan(-epR.at<double>(0)/epR.at<double>(1));
	36	+
	37	+ std::cout << "Angles (L,R) : (" << angleL << ", " << angleR << ")" << std::endl;
	38	+
	39	+ cv::Mat Rl = get2DRotationMatrixLeft();
	40	+ cv::Mat Rr = get2DRotationMatrixRight();
	41	+ cv::warpAffine(*_imL, imLrect, Rl, _imL->size());
	42	+ cv::warpAffine(*_imR, imRrect, Rr, _imR->size());
	43	+ std::cout<< "ok"<< std::endl;
	44	+ //cv::cvtColor(imLrect, imLrect, CV_BGR2GRAY);
	45	+ //cv::cvtColor(imRrect, imRrect, CV_BGR2GRAY);
	46	+
	47	+ cv::Mat imLrectShifted;
	48	+ cv::Mat imRrectShifted;
	49	+
	50	+ std::vector<cv::Point2d> inliersLeftTransformed;
	51	+ std::vector<cv::Point2d> inliersRightTransformed;
	52	+
	53	+ for(int i = 0; i < _inliers1->size(); i++) {
	54	+ cv::Mat tempPoint;
	55	+ tempPoint = (cv::Mat_<double>(3,1) << (_inliers1)[i].x, (_inliers1)[i].y, 1);
	56	+ tempPoint = Rl*tempPoint;
	57	+ inliersLeftTransformed.push_back(cv::Point2d(tempPoint.at<double>(0,0),tempPoint.at<double>(1,0)));
	58	+
	59	+ tempPoint = (cv::Mat_<double>(3,1) << (_inliers2)[i].x, (_inliers2)[i].y, 1);
	60	+ tempPoint = Rr*tempPoint;
	61	+ inliersRightTransformed.push_back(cv::Point2d(tempPoint.at<double>(0,0),tempPoint.at<double>(1,0)));
	62	+ }
	63	+
	64	+ cv::Mat rectErrors = cv::Mat::zeros(inliersLeftTransformed.size(), 1, cv::DataType<double>::type);
	65	+ for(int i = 0; i < inliersLeftTransformed.size(); i++) {
	66	+ rectErrors.at<double>(i,0) = inliersLeftTransformed[i].y - inliersRightTransformed[i].y;
	67	+ }
	68	+ cv::Scalar mean, std;
	69	+ cv::meanStdDev ( rectErrors, mean, std );
	70	+
	71	+ shift = round(abs(mean[0]));
	72	+ std::cout<<shift<<std::endl;
	73	+ int rowNb;
	74	+
	75	+ if (mean[0] > 0){ // features of 2nd are higher than the 1st - shift to the second
	76	+ imLrectShifted = imLrect;
	77	+ imRrectShifted = cv::Mat::zeros(shift,imRrect.cols, imRrect.type());
	78	+ imLrectShifted.push_back(imRrectShifted);
	79	+ imRrectShifted.push_back(imRrect);
	80	+ for (int i = 0; i < inliersRightTransformed.size(); i++) {
	81	+ inliersRightTransformed[i].y = inliersRightTransformed[i].y + shift;
	82	+ }
	83	+ }
	84	+ else{
	85	+ imRrectShifted = imRrect;
	86	+ imLrectShifted = cv::Mat::zeros(shift,imLrect.cols, imLrect.type());
	87	+ imRrectShifted.push_back(imLrectShifted);
	88	+ imLrectShifted.push_back(imLrect);
	89	+ for (int i = 0; i < inliersLeftTransformed.size(); i++) {
	90	+ inliersLeftTransformed[i].y = inliersLeftTransformed[i].y + shift;
	91	+ }
	92	+ }
	93	+
	94	+
	95	+ for(int i = 0; i < inliersLeftTransformed.size(); i++) {
	96	+ rectErrors.at<double>(i,0) = inliersLeftTransformed[i].y - inliersRightTransformed[i].y;
	97	+ }
	98	+ cv::meanStdDev ( rectErrors, mean, std );
	99	+
	100	+ std::cout << "Rectification errors:" << std::endl;
	101	+ std::cout << " mean: " << mean << std::endl;
	102	+ std::cout << " std: " << std << std::endl;
	103	+
	104	+ imLrect = imLrectShifted;
	105	+ imRrect = imRrectShifted;
	106	+ *_inliers1=inliersLeftTransformed;
	107	+ *_inliers2= inliersRightTransformed;
	108	+ std::cout << "Done" << std::endl;
	109	+ //cv::imwrite("_R01.jpg", imLrect);
	110	+ //cv::imwrite("_R02.jpg", imRrect);
	111	+}
	112	+
	113	+bool RectifierAffine::isReady()
	114	+{
	115	+ return (! _F->empty()) && (! this->_inliers1->empty()) && (! this->_inliers2->empty());
	116	+}
	117	+
	118	+cv::Mat RectifierAffine::get2DRotationMatrixLeft()
	119	+{
	120	+ return cv::getRotationMatrix2D(cv::Point2d(_imR->cols/2.0,_imR->rows/2.0),angleL*180.0/CV_PI,1);
	121	+}
	122	+
	123	+cv::Mat RectifierAffine::get2DRotationMatrixRight()
	124	+{
	125	+ return cv::getRotationMatrix2D(cv::Point2d(_imR->cols/2.0,_imR->rows/2.0),angleR*180.0/CV_PI,1);
	126	+
	127	+}
	128	+
	129	+cv::Mat RectifierAffine::get2DShiftMatrix()
	130	+{
	131	+ cv::Mat shiftMat = cv::Mat::zeros( 3, 3, cv::DataType<double>::type);
	132	+ shiftMat.at<double>(1,2) = shift;
	133	+ return shiftMat;
	134	+
	135	+}

...	...	@@ -0,0 +1,186 @@
	1	+#include "densematcher.h"
	2	+
	3	+DenseMatcher::DenseMatcher()
	4	+{
	5	+
	6	+}
	7	+
	8	+DenseMatcher::DenseMatcher(int method)
	9	+{
	10	+ _method = method;
	11	+}
	12	+
	13	+DenseMatcher::~DenseMatcher()
	14	+{
	15	+
	16	+}
	17	+
	18	+void DenseMatcher::calculateDisparityMap()
	19	+{
	20	+ std::cout <<"Disparity calculation started... " <<endl;
	21	+ if ( _lftIm == NULL \|\| _rgtIm == NULL ){
	22	+ std::cout<< "DenseMatcher module in not initialized: use denseMantcher->init command\n"<< endl;
	23	+ return;
	24	+ }
	25	+
	26	+ try{
	27	+ cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create( 16*_params.lowerBound,
	28	+ 16*_params.upperBound, //number of disparities
	29	+ _params.blockSize);
	30	+
	31	+
	32	+ sgbm->setMode(_method);
	33	+
	34	+ int cn = _lftIm->channels();
	35	+ sgbm->setP1(8cn_params.blockSize*_params.blockSize);
	36	+ sgbm->setP2(32cn_params.blockSize*_params.blockSize);
	37	+
	38	+ sgbm->compute( _lftIm, _rgtIm, _disp);
	39	+
	40	+ cv::Mat_<float> temp = _disp;
	41	+ temp = temp / 16;
	42	+
	43	+ cv::bilateralFilter(temp,_dispValues,5,30,30); //use bilateral filter ?
	44	+ //_dispValues = temp; // no filter
	45	+
	46	+ cout << "Done"<<endl;
	47	+ }
	48	+ catch(...){
	49	+ cout << "DenseMatcher::Unexpected error\n"<<endl;
	50	+ }
	51	+}
	52	+
	53	+void DenseMatcher::plotDisparityMap()
	54	+{
	55	+ if ( _dispValues.empty() ){
	56	+ std::cout << "Disparity was not calculated yet\n"<< endl;
	57	+ return;
	58	+ }
	59	+ cv::Mat disp8;
	60	+ cv::normalize(_dispValues, disp8, 0, 225, cv::NORM_MINMAX, CV_8U);
	61	+ //cout << _disp<<endl;
	62	+
	63	+
	64	+ cv::imshow("Disparity", disp8);
	65	+ cv::waitKey(0);
	66	+}
	67	+
	68	+cv::Mat DenseMatcher::getDensePoint()
	69	+{
	70	+ int nbRow=_dispValues.rows;
	71	+ int nbCol=_dispValues.cols;
	72	+ //creat meshgrid temp1 et temp2 present reference image
	73	+ cv::Mat temp1=cv::Mat::zeros(nbRow,nbCol,CV_64F);
	74	+ cv::Mat temp2=cv::Mat::zeros(nbRow,nbCol,CV_64F);
	75	+ for(int i=0;i<nbRow;i++)
	76	+ {
	77	+ temp1.row(i)=double(i)*cv::Mat::ones(1,nbCol,temp1.type());
	78	+ }
	79	+
	80	+
	81	+ for(int j=0;j<nbCol;j++)
	82	+ {
	83	+ temp2.col(j)=double(j)*cv::Mat::ones(nbRow,1,temp2.type());
	84	+ }
	85	+
	86	+ temp1=temp1.reshape(0,1);
	87	+ temp2=temp2.reshape(0,1);
	88	+
	89	+ cv::Mat temp3;
	90	+ _disp.convertTo(temp3,CV_64F);
	91	+
	92	+ temp3=temp3.reshape(0,1);
	93	+ temp3=temp3/16;
	94	+ //compute shift from first image to second image
	95	+ cv::Mat temp4=temp1-temp3.reshape(0,1);
	96	+
	97	+ cv::Mat densePoint_temp;
	98	+ cv::Mat densePoint;
	99	+ densePoint_temp.push_back(temp1);
	100	+ densePoint_temp.push_back(temp2);
	101	+ densePoint_temp.push_back(temp4);
	102	+ densePoint_temp.push_back(temp2);
	103	+ //std::cout<<densePoint_temp.colRange(1,20)<<std::endl;
	104	+ double min,max;
	105	+
	106	+ cv::minMaxLoc(temp3, &min, &max,0,0);
	107	+ //cout<<"min"<< min<<endl;
	108	+ temp3=temp3.reshape(0,1);
	109	+ //cout<<temp3.at<double>(6)<<endl;
	110	+
	111	+ densePoint_temp=densePoint_temp.t();
	112	+ //densePoint=densePoint_temp;
	113	+
	114	+ //remove the error colum which affect final result
	115	+ for(int k=0;k<temp3.cols;k++)
	116	+ {
	117	+ if(ceil(temp3.at<double>(k))>min+1&&ceil(temp3.at<double>(k))<max-1)
	118	+ {
	119	+
	120	+ densePoint.push_back(densePoint_temp.row(k));
	121	+
	122	+ }
	123	+
	124	+ }
	125	+
	126	+ densePoint=densePoint.t();
	127	+ //cout<<"2"<<densePoint.size()<<endl;
	128	+ //std::cout<<densePoint.colRange(1,20)<<std::endl;
	129	+
	130	+ return densePoint;
	131	+}
	132	+
	133	+
	134	+
	135	+
	136	+void DenseMatcher::filterDisparity(int newVal, int maxSpeckleSize, int maxDiff)
	137	+{
	138	+ _paramsFilter.newVal = newVal;
	139	+ _paramsFilter.maxSpeckleSize = maxSpeckleSize;
	140	+ _paramsFilter.maxDiff = maxDiff;
	141	+
	142	+ std::cout << "Disparity filtering started... " << endl;
	143	+
	144	+ if ( ! _disp.empty() ){
	145	+ _disp.copyTo(_dispFiltered);
	146	+ cv::filterSpeckles( _dispFiltered, newVal, maxSpeckleSize, maxDiff);
	147	+
	148	+ cv::Mat_<float> temp = _dispFiltered;
	149	+ temp = temp / 16;
	150	+
	151	+ _dispValues = temp;
	152	+
	153	+ }
	154	+ else{
	155	+ std::cout <<"Done\n"<<std::endl;
	156	+ return;
	157	+ }
	158	+ std::cout <<"Done"<< std::endl;
	159	+}
	160	+
	161	+cv::Mat DenseMatcher::getDisparityToDisplay() const
	162	+{
	163	+// if ( _disp.empty() )
	164	+// AR_Printf("Disparity was not calculated yet");
	165	+
	166	+ cv::Mat disp8 = _disp;
	167	+
	168	+ double minVal, maxVal;
	169	+ minMaxLoc(disp8, &minVal, &maxVal); //find minimum and maximum intensities
	170	+ cv::Mat draw;
	171	+ disp8.convertTo(draw, CV_8UC1, 255.0/(maxVal - minVal), -minVal * 255.0/(maxVal - minVal));
	172	+ //cv::imshow("image", draw);
	173	+ return draw;
	174	+}
	175	+
	176	+void DenseMatcher::plotDisparityFiltered()
	177	+{
	178	+ if ( _dispFiltered.empty() ){
	179	+ std::cout << "Disparity was not filtered yet\n" << endl;
	180	+ return;
	181	+ }
	182	+ cv::Mat disp8;
	183	+ cv::normalize(_dispFiltered, disp8, 0, 255, cv::NORM_MINMAX, CV_8U);
	184	+ cv::imshow("Disparity", disp8);
	185	+}
	186	+

...	...	@@ -0,0 +1,69 @@
	1	+#include <stdio.h>
	2	+#include <iostream>
	3	+#include <opencv2/opencv.hpp>
	4	+#include <opencv2/highgui/highgui.hpp>
	5	+#include <opencv2/objdetect/objdetect.hpp>
	6	+#include <opencv2/imgproc/imgproc.hpp>
	7	+
	8	+#include "Autocalib.h"
	9	+#include "3dReconst.h"
	10	+
	11	+using namespace std;
	12	+
	13	+
	14	+int main(int argc, char *argv[])
	15	+{
	16	+ vector<cv::Mat> image;
	17	+
	18	+ for (int index=1; index<argc; index++) {
	19	+ image.push_back( cv::imread( argv[index], cv::IMREAD_GRAYSCALE));
	20	+ }
	21	+
	22	+ cout<<image[0].size()<<endl;
	23	+ /* for(int i=0;i<image.size();i++)
	24	+ {
	25	+ int offset_x = 800;
	26	+ int offset_y = 400;
	27	+ cv::Rect roi;
	28	+ roi.x = offset_x;
	29	+ roi.y = offset_y;
	30	+ roi.width = image[i].size().width - (offset_x*2);
	31	+ cout<<"ok"<<endl;
	32	+ roi.height = image[i].size().height - (offset_y*3);
	33	+ image[i] = image[i](roi);
	34	+ string name = "piece"+to_string(i)+".jpg";
	35	+ cv::imwrite(name,image[i]);
	36	+
	37	+ }*/
	38	+
	39	+ //Extract features AKAZE and estimat measuremental matrix
	40	+ Autocalib clib;
	41	+ vector<vector<cv::Point2d> > matchVector=clib.findFeaturePoint(image,0.001);
	42	+ vector<vector<cv::Point2d> > measureVector;
	43	+ if(image.size()>2){ measureVector=clib.getMeasureVector(matchVector); }
	44	+ else{measureVector=matchVector;}
	45	+ //cout<<measureVector.size()<<endl;
	46	+
	47	+ //Estimate Fundamental matrix and show epipolair lines with images (before rectification)
	48	+ vector<cv::Mat> fundMatVector =clib.estimatFundMatVector(measureVector);
	49	+ clib.showEpipolarLines(image, measureVector, fundMatVector);
	50	+
	51	+ //optimisation
	52	+ cv::Mat measureMatrix = clib.vectorPoint2Mat(measureVector);
	53	+ vector <cv::Mat> P; // P : camera matrix
	54	+ P=clib.estimatParameter(measureMatrix);
	55	+ for(int i=0; i<P.size();i++)
	56	+ {cout <<"matrice de camera"<<endl<< P[i]<<endl;}
	57	+
	58	+
	59	+
	60	+ //reconstruction 3d
	61	+ Reconst rec;
	62	+ vector<cv::Mat> rectifImage;
	63	+ rectifImage=rec.RectificaEpipolarLines(image, measureVector, fundMatVector);
	64	+ rec.disparity(rectifImage);
	65	+ rec.GetPointCloud(P,image[0]);
	66	+ rec.filterPointCloud(rectifImage.size()/2);
	67	+
	68	+ return 0;
	69	+}

...	...	@@ -0,0 +1,58 @@
	1	+#include "matcell.h"
	2	+
	3	+MatCell::MatCell()
	4	+{
	5	+ _elements.clear();
	6	+}
	7	+
	8	+MatCell::MatCell(int nbElements)
	9	+{
	10	+ _elements.clear();
	11	+ _elements = std::vector<cv::Mat_<double>>(nbElements);
	12	+}
	13	+
	14	+MatCell::~MatCell()
	15	+{
	16	+
	17	+}
	18	+
	19	+void MatCell::eachAssign(const cv::Mat &m)
	20	+{
	21	+ for (int i = 0; i < (int)_elements.size(); i++)
	22	+ _elements[i] = m;
	23	+}
	24	+
	25	+void MatCell::eachMultiplyLeft(const cv::Mat &m)
	26	+{
	27	+ for (int i = 0; i < (int)_elements.size(); i++)
	28	+ _elements[i] = m * _elements[i];
	29	+}
	30	+
	31	+void MatCell::eachMultiplyRight(const cv::Mat &m)
	32	+{
	33	+ for (int i = 0; i < (int)_elements.size(); i++)
	34	+ _elements[i] = _elements[i] * m;
	35	+}
	36	+
	37	+cv::Mat_<double> &MatCell::operator[](const int index)
	38	+{
	39	+ return _elements[index];
	40	+}
	41	+
	42	+cv::Mat MatCell::toMat()
	43	+{
	44	+ cv::Mat_<double> a;
	45	+ for (int i = 0; i < (int)_elements.size(); i++){
	46	+ a.push_back(_elements[i]);
	47	+ }
	48	+ return a;
	49	+}
	50	+
	51	+MatCell::operator cv::Mat_<double>() const {
	52	+ cv::Mat_<double> a;
	53	+ for (int i = 0; i < (int)_elements.size(); i++){
	54	+ a.push_back(_elements[i]);
	55	+ }
	56	+ return a;
	57	+}
	58	+

...	...	@@ -0,0 +1,368 @@
	1	+#include "optimization.h"
	2	+
	3	+Optimization::~Optimization()
	4	+{
	5	+
	6	+}
	7	+
	8	+void Optimization::init()
	9	+{
	10	+
	11	+}
	12	+
	13	+
	14	+double Optimization::deg2rad(double angDeg){
	15	+ return angDeg / 180.0 * PI;
	16	+}
	17	+
	18	+void Optimization::setMeasurementMatrix(cv::Mat inW)
	19	+{
	20	+ nbParams = -1;
	21	+
	22	+ Win = inW.clone();
	23	+ //AR_Assert(!(Win->empty()));
	24	+
	25	+ W.release();
	26	+ tm.clear();
	27	+
	28	+ cv::Mat Wmean;
	29	+ computeWmean(Win, Wmean, tm);
	30	+
	31	+ //util::makeNonHomogenious(Wmean);
	32	+ W = Wmean.clone();
	33	+
	34	+ nbCams = W.rows / 2;
	35	+ nbPts = W.cols ;
	36	+
	37	+ paramConst = cv::Mat::ones(nbCams,PARAMS_PER_CAM,CV_8UC1); // 1 for varying parameter
	38	+ paramBounds = PI/2*cv::Mat::ones(nbCams,PARAMS_PER_CAM,cv::DataType<double>::type);
	39	+ paramOffset = cv::Mat::zeros(nbCams,PARAMS_PER_CAM,cv::DataType<double>::type);
	40	+ paramInitial = cv::Mat::zeros(nbCams,PARAMS_PER_CAM,cv::DataType<double>::type);
	41	+
	42	+ // Define default constant parameters:
	43	+ paramConst.col(K) = cv::Mat::zeros(nbCams,1,paramConst.type());
	44	+ paramConst.col(ALPHA) = cv::Mat::zeros(nbCams,1,paramConst.type());
	45	+ paramConst.col(S) = cv::Mat::zeros(nbCams,1,paramConst.type());
	46	+ paramConst.row(0).colRange(RT1,RT2+1) = cv::Mat::zeros(1,3,paramConst.type());
	47	+
	48	+ // Define default offset parameters:
	49	+ paramOffset.col(K) = 1 * cv::Mat::ones(nbCams,1,paramOffset.type());
	50	+ paramOffset.col(ALPHA) = 1 * cv::Mat::ones(nbCams,1,paramOffset.type());
	51	+
	52	+ // Define default bounds (considered symmetric):
	53	+
	54	+ // Define initial values. Actual value of parameter: offset + initial
	55	+ paramInitial.rowRange(1,nbCams).col(RR) = deg2rad(5.0) * cv::Mat::ones(nbCams-1,1,paramInitial.type());
	56	+
	57	+ std::cout << paramInitial;
	58	+}
	59	+
	60	+
	61	+void Optimization::run()
	62	+{
	63	+ _minf = INFINITY;
	64	+ _bestObjValue = INFINITY;
	65	+
	66	+ std::vector<double> x0,lb,ub;
	67	+ this->getInitialConditionsAndBounds(x0, lb, ub);
	68	+ this->setObjectiveFunction(Optimization::OBJFUNC_Rectification);
	69	+ //this->setObjectiveFunction(Optimization::OBJFUNC_Distance);
	70	+
	71	+
	72	+ nlopt::opt optimizer(nlopt::GN_CRS2_LM, this->getParametersNumber()); // 1
	73	+ optimizer.set_lower_bounds(lb);
	74	+ optimizer.set_upper_bounds(ub);
	75	+ optimizer.set_min_objective( *Optimization::wrap, this);
	76	+ optimizer.set_ftol_rel(1e-30);
	77	+ optimizer.set_maxtime(_maxTimeStep1);
	78	+
	79	+ optimizer.optimize(x0, _minf);
	80	+
	81	+ cout << "Found minimum: " << _minf <<endl;
	82	+
	83	+ cv::Mat paramX = 0 * paramOffset.clone();
	84	+ copyVectorToMatElements(x0, paramVarIdx, paramX);
	85	+ paramResult = paramOffset.clone() + paramX;
	86	+}
	87	+
	88	+double Optimization::operator() (const std::vector<double> &x, std::vector<double> &grad)
	89	+{
	90	+ (void)(grad); // remove warning for unused variable
	91	+ switch (_objFuncMethod) {
	92	+ case OBJFUNC_DistanceSquared: return distanceSquared(x); break;
	93	+ case OBJFUNC_Distance : return testFunction(x); break;
	94	+ case OBJFUNC_PseudoInverse : return distancePseudoInverse(x); break;
	95	+ case OBJFUNC_Rectification : return distanceWminusMMW(x); break;
	96	+ default: break;
	97	+ }
	98	+ return 0;
	99	+}
	100	+
	101	+
	102	+double Optimization::testFunction (const std::vector<double> &x){
	103	+ return (x[0]x[0] + x[1]x[1] + 20sin(x[0]) + 20sin(x[1]));
	104	+}
	105	+
	106	+MatCell Optimization::getMfromParams(const std::vector<double> &x){
	107	+ MatCell M(nbCams);
	108	+
	109	+ cv::Mat A = cv::Mat::eye(2, 2, cv::DataType<double>::type);
	110	+ A.at<double>(0,0) = x[0] * x[1]; // k*E
	111	+ A.at<double>(0,1) = x[0] * x[2]; // k*s
	112	+
	113	+ M[0] = A * cv::Mat::eye(2, 3, cv::DataType<double>::type);
	114	+
	115	+ cv::Mat angles = cv::Mat::eye(3, nbCams-1, cv::DataType<double>::type);
	116	+ //angles = params(4 : 4 + 3*(nCams-1)-1);
	117	+ //angles = reshape(angles, [3 length(angles)/3]);
	118	+ for(int i = 0 ; i < angles.cols ; i++){
	119	+ angles.at<double>(0,i) = x[3*i+3];
	120	+ angles.at<double>(1,i) = x[3*i+4];
	121	+ angles.at<double>(2,i) = x[3*i+5];
	122	+ }
	123	+ //std::cout << angles << std::endl;
	124	+
	125	+ for (int i = 1 ; i < nbCams ; i++){
	126	+ double p = angles.at<double>(0,i-1);
	127	+ double t = angles.at<double>(1,i-1);
	128	+ double k = angles.at<double>(2,i-1);
	129	+
	130	+ // R = Rz(k)Ry(t)Rx(p)
	131	+ cv::Mat R = (cv::Mat_<double>(2,3) << cos(k)cos(t), cos(k)sin(p)sin(t) - cos(p)sin(k), sin(k)sin(p) + cos(k)cos(p)*sin(t),
	132	+ cos(t)sin(k), cos(k)cos(p) + sin(k)sin(p)sin(t), cos(p)sin(k)sin(t) - cos(k)*sin(p));
	133	+
	134	+ M[i] = A * R ;
	135	+ }
	136	+ return M;
	137	+}
	138	+
	139	+cv::Mat Optimization::getPoints3DfromParams(const std::vector<double> &x){
	140	+ cv::Mat X = cv::Mat_<double>(3, nbPts);
	141	+ for (int i = 0 ; i < nbPts ; i++){
	142	+ X.at<double>(0,i) = x[3 + 3(nbCams-1) + 3i];
	143	+ X.at<double>(1,i) = x[3 + 3(nbCams-1) + 3i + 1];
	144	+ X.at<double>(2,i) = x[3 + 3(nbCams-1) + 3i + 2];
	145	+ }
	146	+ return X;
	147	+}
	148	+
	149	+
	150	+double Optimization::distanceSquared(const std::vector<double> &x)
	151	+{
	152	+ _iterCnt++;
	153	+
	154	+ MatCell M = getMfromParams(x);
	155	+ cv::Mat X = getPoints3DfromParams(x);
	156	+
	157	+ cv::Mat reprojErrorMat = W - M*X;
	158	+ double reprojError = cv::sum(reprojErrorMat.mul(reprojErrorMat))[0];
	159	+ double s = x[2];
	160	+
	161	+ double objValue = std::abs( reprojError + s );
	162	+
	163	+ if (_iterCnt % nbParams * 500 == 0){
	164	+ if ( objValue < _bestObjValue) _bestObjValue = objValue ;
	165	+ std::stringstream stream;
	166	+ cout << " * " << std::setw(15) << _iterCnt
	167	+ << " * " << std::setw(12) << std::scientific << std::setprecision(5) << _bestObjValue
	168	+ << " * " << std::setw(12) << std::scientific << std::setprecision(5) << objValue << endl;
	169	+ }
	170	+
	171	+ return objValue;
	172	+}
	173	+
	174	+double Optimization::distancePseudoInverse(const std::vector<double> &x)
	175	+{
	176	+ _iterCnt++;
	177	+
	178	+ MatCell M = getMfromParams(x);
	179	+ cv::Mat X = getPoints3DfromParams(x);
	180	+
	181	+ cv::Mat Minv;
	182	+ cv::invert( (cv::Mat) M,Minv,cv::DECOMP_SVD);
	183	+ cv::Mat reprojErrorMat = W - MMinvW;
	184	+ double reprojError = cv::sum(reprojErrorMat.mul(reprojErrorMat))[0];
	185	+ double s = x[2];
	186	+
	187	+ double objValue = std::abs( reprojError + s );
	188	+
	189	+ if (_iterCnt % nbParams * 500 == 0){
	190	+ if ( objValue < _bestObjValue) _bestObjValue = objValue ;
	191	+ std::stringstream stream;
	192	+ cout << " : " << std::setw(15) << _iterCnt
	193	+ << " : " << std::setw(12) << std::scientific << std::setprecision(5) << _bestObjValue
	194	+ << " : " << std::setw(12) << std::scientific << std::setprecision(5) << objValue << endl;
	195	+ }
	196	+
	197	+ return objValue;
	198	+}
	199	+
	200	+double Optimization::distanceWminusMMW(const std::vector<double> &x)
	201	+{
	202	+
	203	+ cv::Mat paramX = 0 * paramOffset.clone();
	204	+ util::copyVectorToMatElements( x, paramVarIdx, paramX);
	205	+ cv::Mat paramTable = paramOffset.clone() + paramX;
	206	+
	207	+ double k = paramTable.at<double>(0,K);
	208	+ double alpha = paramTable.at<double>(0,ALPHA);
	209	+ double s = paramTable.at<double>(0,S);
	210	+ cv::Mat A = k * (cv::Mat_<double>(2,2) << alpha, s, 0, 1.);
	211	+
	212	+ std::vector<cv::Mat> Marray(nbCams);
	213	+ std::vector<cv::Mat> Rarray(nbCams);
	214	+
	215	+ for (auto i = 0; i < nbCams; i++){
	216	+ double t1 = paramTable.at<double>(i,RT1);
	217	+ double rho = paramTable.at<double>(i,RR);
	218	+ double t2 = paramTable.at<double>(i,RT2);
	219	+
	220	+ Rarray[i] = rotmat::fromEulerZYZ(t1,rho,t2);
	221	+ if (i != 0) Rarray[i] = Rarray[i] * Rarray[i-1];
	222	+
	223	+ Marray[i] = A * Rarray[i].rowRange(0,0+2);
	224	+ }
	225	+
	226	+ cv::Mat M = util::concatenateMat(Marray, util::CONCAT_VERTICAL);
	227	+ cv::Mat pinvM;
	228	+ cv::invert(M, pinvM, cv::DECOMP_SVD);
	229	+
	230	+ cv::Mat errorMat = W - MpinvMW;
	231	+ double objValue = cv::sum(errorMat.mul(errorMat))[0];
	232	+
	233	+ _iterCnt++;
	234	+ if ( _iterCnt % 10000 == 0){
	235	+ if ( objValue < _bestObjValue) _bestObjValue = objValue ;
	236	+ std::stringstream stream;
	237	+ cout << " : " << std::setw(15) << _iterCnt
	238	+ << " : " << std::setw(12) << std::scientific << std::setprecision(5) << _bestObjValue
	239	+ << " : " << std::setw(12) << std::scientific << std::setprecision(5) << objValue << endl;
	240	+ }
	241	+
	242	+ return objValue;
	243	+}
	244	+
	245	+
	246	+cv::Mat Optimization::getCalibrationMatrixFromParamTable(const cv::Mat &paramTable ) const{
	247	+ double k = paramTable.at<double>(0,K);
	248	+ double alpha = paramTable.at<double>(0,ALPHA);
	249	+ double s = paramTable.at<double>(0,S);
	250	+ cv::Mat A = k * (cv::Mat_<double>(2,2) << alpha, s, 0, 1.);
	251	+ return A.clone();
	252	+}
	253	+
	254	+
	255	+int Optimization::getParametersNumber()
	256	+{
	257	+ return nbParams;
	258	+}
	259	+
	260	+void Optimization::copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat)
	261	+{
	262	+ int nbIdx = (int) idx.total();
	263	+
	264	+ for (int i = 0; i < nbIdx; i++){
	265	+ int col = (int) idx.at<cv::Point>(i).x;
	266	+ int row = (int) idx.at<cv::Point>(i).y;
	267	+
	268	+ mat.row(row).col(col) = vec[i];
	269	+ }
	270	+}
	271	+
	272	+void Optimization::getInitialConditionsAndBounds(std::vector<double> &_x0, std::vector<double> &_lb, std::vector<double> &_ub)
	273	+{
	274	+ paramVarIdx.release();
	275	+ cv::findNonZero(paramConst, paramVarIdx);
	276	+ nbParams = (int) paramVarIdx.total();
	277	+ cout << "Number of parameters: " << nbParams << endl;
	278	+
	279	+ _x0.clear();
	280	+ _lb.clear();
	281	+ _ub.clear();
	282	+
	283	+ _x0 = std::vector<double>(nbParams);
	284	+ _lb = std::vector<double>(nbParams);
	285	+ _ub = std::vector<double>(nbParams);
	286	+
	287	+ cv::Mat lbMat = -paramBounds.clone();
	288	+ cv::Mat ubMat = paramBounds.clone();
	289	+
	290	+ lbMat.at<double>(1,RR) = 0;
	291	+
	292	+ util::copyMatElementsToVector( paramInitial, paramVarIdx, _x0);
	293	+ util::copyMatElementsToVector( lbMat , paramVarIdx, _lb);
	294	+ util::copyMatElementsToVector( ubMat , paramVarIdx, _ub);
	295	+}
	296	+
	297	+
	298	+std::vector<cv::Mat> Optimization::getCameraMatrices() const
	299	+{
	300	+ std::vector<cv::Mat> Parray(nbCams);
	301	+
	302	+ cv::Mat A = getCalibrationMatrixFromParamTable(paramResult);
	303	+
	304	+ std::vector<cv::Mat> Rarray(nbCams);
	305	+
	306	+ for (auto i = 0; i < nbCams; i++){
	307	+ double t1 = paramResult.at<double>(i,RT1);
	308	+ double rho = paramResult.at<double>(i,RR);
	309	+ double t2 = paramResult.at<double>(i,RT2);
	310	+
	311	+ Rarray[i] = rotmat::fromEulerZYZ(t1,rho,t2);
	312	+ if (i != 0) Rarray[i] = Rarray[i] * Rarray[i-1];
	313	+
	314	+ cv::Mat M = A * Rarray[i].rowRange(0,0+2);
	315	+
	316	+ cv::Matx34d P = cv::Matx34d::zeros();
	317	+
	318	+ P(0,0) = M.at<double>(0,0);
	319	+ P(0,1) = M.at<double>(0,1);
	320	+ P(0,2) = M.at<double>(0,2);
	321	+
	322	+ P(1,0) = M.at<double>(1,0);
	323	+ P(1,1) = M.at<double>(1,1);
	324	+ P(1,2) = M.at<double>(1,2);
	325	+
	326	+ P(0,3) = tm[i].at<double>(0,0);
	327	+ P(1,3) = tm[i].at<double>(1,0);
	328	+ P(2,3) = 1;
	329	+
	330	+ Parray[i] = ((cv::Mat)P).clone();
	331	+ }
	332	+
	333	+ return Parray;
	334	+}
	335	+
	336	+cv::Mat Optimization::getCalibrationMatrix() const
	337	+{
	338	+ cv::Mat A = getCalibrationMatrixFromParamTable(paramResult);
	339	+ return A.clone();
	340	+}
	341	+
	342	+cv::Mat Optimization::getRotationAnglesDeg() const
	343	+{
	344	+ //return cv::Mat(paramResult.colRange(RT1,RT2+1)).clone() / PI * 180.0;
	345	+ return cv::Mat(paramResult.col(RR)).clone() / PI * 180.0;
	346	+}
	347	+
	348	+void Optimization::computeWmean(const cv::Mat & W, cv::Mat & Wmean, std::vector<cv::Mat> & t)
	349	+{
	350	+
	351	+ W.copyTo(Wmean);
	352	+ int nCams = W.rows / 2;
	353	+ t.clear();
	354	+
	355	+ for (int i = 0; i < nCams ; i++)
	356	+ {
	357	+ cv::Mat tC = cv::Mat::zeros(2,1,cv::DataType<double>::type);
	358	+ tC.at<double>(0,0) = cv::mean(W.row( 2*i ))(0);
	359	+ tC.at<double>(1,0) = cv::mean(W.row( 2*i + 1 ))(0);
	360	+ t.push_back(tC);
	361	+ for (int j = 0; j < W.cols ; j++)
	362	+ {
	363	+ Wmean.at<double>(2i ,j) = Wmean.at<double>(2i ,j) - tC.at<double>(0,0);
	364	+ Wmean.at<double>(2i + 1,j) = Wmean.at<double>(2i + 1,j) - tC.at<double>(1,0);
	365	+ }
	366	+ }
	367	+
	368	+}

...	...	@@ -0,0 +1,97 @@
	1	+#include "transformations.h"
	2	+
	3	+/*! Decomposition of rotation matrix in Euler angles ZYZ:
	4	+ * \f[ R = R_z(\theta_1) R_y(\rho) R_z(\theta_2)\f]
	5	+ * \param[in] R rotation matrix
	6	+ * \param[out] t1 \f$\theta_1\f$
	7	+ * \param[out] rho \f$\rho\f$
	8	+ * \param[out] t2 \f$\theta_2\f$
	9	+ */
	10	+void rotmat::decomposeEulerZYZ(const cv::Mat &R, double &t1, double &rho, double &t2){
	11	+ cv::Matx33d rotmat = R;
	12	+
	13	+ std::cout << rotmat;
	14	+ if (rotmat(2,2) < 1){
	15	+ if (rotmat(2,2) > -1){
	16	+ rho = acos(rotmat(2,2));
	17	+ t1 = atan2(rotmat(1,2),rotmat(0,2));
	18	+ t2 = atan2(rotmat(2,1),-rotmat(2,0));
	19	+ }
	20	+ else{
	21	+ rho = PI;
	22	+ t1 = -atan2(rotmat(1,0),rotmat(1,1));
	23	+ t2 = 0;
	24	+ }
	25	+ }
	26	+ else{
	27	+ rho = 0;
	28	+ t1 = atan2(rotmat(1,0),rotmat(1,1));
	29	+ t2 = 0;
	30	+ }
	31	+}
	32	+
	33	+/// Get rotation matrix from affine camera matrix \f$P_{3\times4}\f$
	34	+cv::Mat rotmat::fromCameraMatrixAffine(const cv::Mat &P){
	35	+
	36	+ P.col(1).row(2) = 5;
	37	+
	38	+ if (P.rows != 3 \|\| P.cols != 4)
	39	+ std::runtime_error("assert failed (P.rows == 3 && P.cols == 4)");
	40	+
	41	+ cv::Mat K,R;
	42	+ cv::Mat temp;
	43	+ temp = P.colRange(0,0+3).clone();
	44	+ cv::Point3d p0(temp.row(0).clone());
	45	+ cv::Point3d p1(temp.row(1).clone());
	46	+ temp.row(2) = cv::Mat(p0.cross(p1)).t();
	47	+
	48	+ cv::RQDecomp3x3(temp,K,R);
	49	+ if (K.at<double>(2,2) < 0){
	50	+ K = -K;
	51	+ R = -R;
	52	+ }
	53	+ return R;
	54	+}
	55	+
	56	+/*! Transform a vector \f$(\theta_1,\rho,\theta_2)\f$ reprensenting the euler (Rzyz) angles
	57	+ * into a rotation matrix:
	58	+ * \f[ R = R_z(\theta_1) R_y(\rho) R_z(\theta_2)\f]
	59	+ */
	60	+cv::Mat rotmat::fromEulerZYZ(double t1, double rho, double t2){
	61	+ double c0,c1,c2,s0,s1,s2;
	62	+
	63	+ c0 = cos(t1);
	64	+ c1 = cos(rho);
	65	+ c2 = cos(t2);
	66	+ s0 = sin(t1);
	67	+ s1 = sin(rho);
	68	+ s2 = sin(t2);
	69	+
	70	+ cv::Matx33d R;
	71	+ R(0,0) = c0c1c2 - s0*s2;
	72	+ R(0,1) = -c0c1s2 - s0*c2;
	73	+ R(0,2) = c0*s1;
	74	+ R(1,0) = s0c1c2+c0*s2 ;
	75	+ R(1,1) = -s0c1s2 + c0*c2 ;
	76	+ R(1,2) = s0*s1;
	77	+ R(2,0) = -s1*c2;
	78	+ R(2,1) = s1*s2;
	79	+ R(2,2) = c1;
	80	+ return ((cv::Mat)R).clone();
	81	+}
	82	+
	83	+
	84	+
	85	+
	86	+
	87	+
	88	+
	89	+
	90	+
	91	+
	92	+
	93	+
	94	+
	95	+
	96	+
	97	+

...	...	@@ -0,0 +1,104 @@
	1	+#include "triangulation.h"
	2	+
	3	+Triangulator::Triangulator(){
	4	+ triangulationMethod = 0; //Default method - linearLS
	5	+ reprojectionError = 0;
	6	+}
	7	+
	8	+Triangulator::Triangulator(int method){
	9	+ triangulationMethod = method;
	10	+ reprojectionError = 0;
	11	+}
	12	+
	13	+void Triangulator::setTriangulationMethod (int method){
	14	+ triangulationMethod = method;
	15	+}
	16	+
	17	+double Triangulator::getReprojectionError(){
	18	+ return reprojectionError;
	19	+}
	20	+
	21	+void Triangulator::triangulate(const cv::Mat &W, const cv::Mat &P, pcl::PointCloud<pcl::PointXYZ> &pointCloud)
	22	+{
	23	+ switch(triangulationMethod){
	24	+ case ITERATIVE_LS : break;
	25	+ case ITERATIVE_EIGEN : break;
	26	+ case DIRECT_AFFINE : triangulateAffine(W,P,pointCloud); break;
	27	+ default : break;
	28	+ }
	29	+}
	30	+
	31	+pcl::PointCloud<pcl::PointXYZ> Triangulator::triangulatePoints_LinearLS(const cv::Mat &Wf, const cv::Mat &Pm)
	32	+{
	33	+ //std::cout<<Wf.size()<<std::endl;
	34	+ //std::cout<<Pm<<std::endl;
	35	+ int nPts = (int) Wf.cols ;
	36	+ int nCams = (int) Wf.rows / 2;
	37	+
	38	+ cv::Mat A;
	39	+ cv::Mat B;
	40	+ cv::Mat_<double> pt;
	41	+ cv::Mat temp;
	42	+ pcl::PointCloud<pcl::PointXYZ> cloud;
	43	+ //cloud.is_dense = false;
	44	+
	45	+ for ( int p = 0 ; p < nPts ; p++ ){
	46	+
	47	+ temp = cv::Mat(0,4,cv::DataType<double>::type);
	48	+
	49	+ for ( int i = 0 ; i < nCams ; i++ ){
	50	+
	51	+ temp.push_back( Wf.at<double> ( 2i + 0, p) Pm.row( 3i+2 ) - Pm.row( 3i + 0 ));
	52	+ temp.push_back( Wf.at<double> ( 2i + 1, p) Pm.row( 3i+2 ) - Pm.row( 3i + 1 ));
	53	+
	54	+ }
	55	+
	56	+ A = temp.colRange(0,3);
	57	+ B = -1*temp.col(3);
	58	+
	59	+ cv::solve(A,B,pt,cv::DECOMP_SVD);
	60	+ //cout<< pt<<endl;
	61	+
	62	+ cloud.push_back(pcl::PointXYZ(pt(0),pt(1),pt(2)));
	63	+ //cloud.points[p].x = pt(0);
	64	+ //cloud.points[p].y = pt(1);
	65	+ //cloud.points[p].z = pt(2);
	66	+ //std::cout<<pt(2)<<std::endl;
	67	+ temp.release();
	68	+
	69	+ }
	70	+ return cloud;
	71	+}
	72	+
	73	+void Triangulator::triangulateAffine(const cv::Mat &W, const cv::Mat &P, pcl::PointCloud<pcl::PointXYZ> &pointCloud)
	74	+{
	75	+ if (W.rows%3 == 0){
	76	+
	77	+ }
	78	+
	79	+ cv::Mat q1x = W.row(0);
	80	+ cv::Mat q1y = W.row(1);
	81	+ cv::Mat q2x;
	82	+ if (W.rows%3 == 0) q2x = W.row(3);
	83	+ else q2x = W.row(2);
	84	+
	85	+ double t1,rho,t2;
	86	+ //cv::Mat R = rotmFromCameraMat(P.rowRange(0,0+3));
	87	+ cv::Mat R = rotmat::fromCameraMatrixAffine(P.rowRange(3,3+3).clone());
	88	+ rotmat::decomposeEulerZYZ(R,t1,rho,t2);
	89	+
	90	+ std::cout << rho/PI*180;
	91	+ //rho = 3.0/180.0*PI;
	92	+ double cosRho = cos(rho);
	93	+ double sinRho = sin(rho);
	94	+
	95	+ cv::Mat q1z = (q1x * cosRho - q2x) / sinRho;
	96	+
	97	+ for (auto i = 0; i < q1x.cols; i++){
	98	+ pointCloud.push_back(pcl::PointXYZ(
	99	+ q1x.at<double>(0,i),
	100	+ q1y.at<double>(0,i),
	101	+ q1z.at<double>(0,i)));
	102	+ }
	103	+}
	104	+

...	...	@@ -0,0 +1,106 @@
	1	+#include "utils.h"
	2	+
	3	+void util::saveFileToMatlab(std::string fileName, cv::Mat a, std::string varName){
	4	+ int rows = (int) a.rows;
	5	+ int cols = (int) a.cols;
	6	+
	7	+ std::ofstream myfile;
	8	+ myfile.open (fileName);
	9	+ myfile << "%File generated with Pollen3D \n";
	10	+
	11	+ myfile << varName << " = [";
	12	+
	13	+ for (int row = 0; row < rows; row++){
	14	+ for (int col = 0; col < cols; col++){
	15	+ myfile << a.at<double>(row,col) << " ";
	16	+ }
	17	+ if ( row != rows - 1) myfile << ";\n";
	18	+ else myfile << " ];\n";
	19	+ }
	20	+
	21	+ myfile.close();
	22	+}
	23	+
	24	+
	25	+double util::rad2deg(double angRad){
	26	+ return angRad / PI * 180.0;
	27	+}
	28	+
	29	+double util::deg2rad(double angDeg){
	30	+ return angDeg / 180.0 * PI;
	31	+}
	32	+
	33	+/*! Make matrix non-homogenious. It is mainly used for measurement matrix \f$W\f$.
	34	+ * If ((W mod 3) == 0) it will supress every third row
	35	+ */
	36	+void util::makeNonHomogenious(cv::Mat &m)
	37	+{
	38	+ // Make W non-homogenious
	39	+ bool isNonHomogenious = !( (m.rows % 3) == 0 && (m.at<double>(2,0) == 1));
	40	+ if (isNonHomogenious)
	41	+ return;
	42	+
	43	+ cv::Mat newM;
	44	+
	45	+ for (int i = 0 ; i < m.rows / 3 ; i++){
	46	+ newM.push_back(m.row(3*i));
	47	+ newM.push_back(m.row(3*i + 1));
	48	+ }
	49	+
	50	+ m.release();
	51	+ m = newM.clone();
	52	+}
	53	+
	54	+/*! Copy elements of matrix with indices idx to vector
	55	+ * Equivalent to MATLAB: v = M(idx);
	56	+ */
	57	+void util::copyMatElementsToVector(const cv::Mat &mat, const cv::Mat &idx, std::vector<double> &vec)
	58	+{
	59	+ int nbIdx = (int) idx.total();
	60	+
	61	+ for (int i = 0; i < nbIdx; i++){
	62	+ int col = (int) idx.at<cv::Point>(i).x;
	63	+ int row = (int) idx.at<cv::Point>(i).y;
	64	+
	65	+ vec[i] = mat.at<double>(row,col);
	66	+ }
	67	+}
	68	+
	69	+/*! Copy vector to matrix elements with indices idx
	70	+ * Equivalent to MATLAB: M(idx) = v;
	71	+ */
	72	+void util::copyVectorToMatElements(const std::vector<double> &vec, const cv::Mat &idx, cv::Mat &mat)
	73	+{
	74	+ int nbIdx = (int) idx.total();
	75	+
	76	+ for (int i = 0; i < nbIdx; i++){
	77	+ int col = (int) idx.at<cv::Point>(i).x;
	78	+ int row = (int) idx.at<cv::Point>(i).y;
	79	+
	80	+ mat.row(row).col(col) = vec[i];
	81	+ }
	82	+}
	83	+
	84	+/*! Concatenate vector of matrices
	85	+ * \param matArray vector of matrices
	86	+ * \param method concatenation method
	87	+ * - CONCAT_HORIZONTAL (Horizontally) : M = [M1 M2 M3 M4]
	88	+ * - CONCAT_VERTICAL (Vertically) : M = [M1;M2;M3;M4]
	89	+ */
	90	+cv::Mat util::concatenateMat(const std::vector<cv::Mat> &matArray, int method)
	91	+{
	92	+ cv::Mat outMat;
	93	+ if (method == CONCAT_VERTICAL){
	94	+ for(auto& m:matArray){
	95	+ outMat.push_back(m.clone());
	96	+ }
	97	+ }else{
	98	+ for(auto& m:matArray){
	99	+ cv::Mat mt = m.t();
	100	+ outMat.push_back(mt.clone());
	101	+ }
	102	+ outMat = outMat.t();
	103	+ }
	104	+ return outMat.clone();
	105	+}
	106	+