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 Inline 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

File was created	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

File was created	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

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

File was created	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

File was created	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(){};

File was created	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

File was created	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;

File was created	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;

File was created	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;

File was created	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

File was created	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

File was created	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	}

File was created	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{

File was created	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,

File was created	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);

File was created	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.

File was created	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);

File was created	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],

File was created	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();

File was created	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;

File was created	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;

File was created	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++)

File was created	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	}

File was created	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);

File was created	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;

File was created	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);

File was created	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