├── AprilTag3.tar.gz ├── LICENSE ├── MVTB-4.3.mltbx ├── Machine Vision Toolbox for MATLAB ├── info │ └── html │ │ └── apriltags.html └── vision │ ├── apriltags.m │ └── mex │ ├── apriltag │ ├── apriltag_pose.c │ └── apriltag_pose.c.old │ ├── apriltags.c │ ├── apriltags.mexa64 │ └── make.m ├── PROCEDURE.md ├── README.md └── tag36h11.tar.gz /AprilTag3.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alddiaz/MATLAB_AprilTag3/852a575f0a411ff400c7d39c27dd431309ef8be0/AprilTag3.tar.gz -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | 341 | -------------------------------------------------------------------------------- /MVTB-4.3.mltbx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alddiaz/MATLAB_AprilTag3/852a575f0a411ff400c7d39c27dd431309ef8be0/MVTB-4.3.mltbx -------------------------------------------------------------------------------- /Machine Vision Toolbox for MATLAB/info/html/apriltags.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | M-File Help: apriltags 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 |
M-File Help: apriltagsView code for apriltags
14 |

apriltags

Read April tags from image

15 | tags = apriltags(im) is a vector of structures that describe each of the 16 | April tags found within the image IM. 17 | 18 |

19 |

20 | Elements of the structure are: 21 | 22 |

23 | 24 | 25 | 26 | 27 | 30 | 31 | 32 | 33 | 34 |
.id decoded id of the tag in the range 1-255
.hamming number of corrected id bits, 0 is best
.margin decision margin, high is better
.H homography matrix (3x3) describing the projection from an 28 | "ideal" tag (with corners at (-1,-1), (1,-1), (1,1), and (-1,1)) to 29 | pixels in the image
.center centre of the tag in the image (2x1)
.corners corners of the tag in the image (2x4)
.p position of the tag center in camera coordinates (3x1)
.R rotation of the tag center with respect to camera coordinates (3x3)
35 |

Notes

36 | 41 |

Author

42 | 48 |
49 | 50 | 51 |
 
52 |

© 1990-2014 Peter Corke.

53 | 54 | -------------------------------------------------------------------------------- /Machine Vision Toolbox for MATLAB/vision/apriltags.m: -------------------------------------------------------------------------------- 1 | %APRILTAGS Read April tags from image 2 | % 3 | % tags = apriltags(IM, TAGSIZE, K) is a vector of structures that describe 4 | % each of the AprilTags found within the image IM. 5 | % 6 | % TAGSIZE specifies tag size in meters. 7 | % 8 | % K specifies camera calibration as a 3x3 matrix 9 | % defined as K = [fx 0 u0; 0 fy v0; 0 0 1], with parameters 10 | % (fx, fy) camera focal length 11 | % (u0, v0) principal point (optical center of camera) 12 | % 13 | % Elements of the structure are: 14 | % .id decoded id of the tag in the range 1-255 15 | % .hamming number of corrected id bits, 0 is best 16 | % .margin decision margin, high is better 17 | % .H homography matrix (3x3) describing the projection from an 18 | % "ideal" tag (with corners at (-1,-1), (1,-1), (1,1), and (-1,1)) to 19 | % pixels in the image 20 | % .center center of the tag in the image (2x1) 21 | % .corners corners of the tag in the image (2x4) 22 | % .p 3-D position of the tag center in camera coordinates (3x1) 23 | % .R 3-D rotation of the tag center with respect to camera coordinates (3x3) 24 | % 25 | % Notes:: 26 | % - implementation is a MEX file. 27 | % - the image must be uint8 or double (grey level range 0 to 1). 28 | % - only tag family 'tag36h11' is supported. 29 | % 30 | % Author:: 31 | % 32 | % - AprilTags is open-source software from University of Michigan 33 | % - details at https://april.eecs.umich.edu/software/apriltag.html 34 | % - This wrapper by Peter Corke 35 | % - Updated by Aldo Diaz, Linkoping, 2019 36 | 37 | % Copyright (C) 1993-2011, by Peter I. Corke 38 | % 39 | % This file is part of The Machine Vision Toolbox for Matlab (MVTB). 40 | % 41 | % MVTB is free software: you can redistribute it and/or modify 42 | % it under the terms of the GNU Lesser General Public License as published by 43 | % the Free Software Foundation, either version 3 of the License, or 44 | % (at your option) any later version. 45 | % 46 | % MVTB is distributed in the hope that it will be useful, 47 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 48 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 49 | % GNU Lesser General Public License for more details. 50 | % 51 | % You should have received a copy of the GNU Leser General Public License 52 | % along with MVTB. If not, see . 53 | 54 | if ~exist('apriltags', 'file') 55 | error('you need to build the MEX version of apriltags, see vision/mex/README'); 56 | end 57 | -------------------------------------------------------------------------------- /Machine Vision Toolbox for MATLAB/vision/mex/apriltag/apriltag_pose.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "apriltag_pose.h" 5 | #include "apriltag_math.h" 6 | #include "common/homography.h" 7 | #include "common/image_u8x3.h" 8 | 9 | 10 | /** 11 | * Calculate projection operator from image points. 12 | */ 13 | matd_t* calculate_F(matd_t* v) { 14 | matd_t* outer_product = matd_op("MM'", v, v, v, v); 15 | matd_t* inner_product = matd_op("M'M", v, v); 16 | matd_scale_inplace(outer_product, 1.0/inner_product->data[0]); 17 | matd_destroy(inner_product); 18 | return outer_product; 19 | } 20 | 21 | /** 22 | * Returns the value of the supplied scalar matrix 'a' and destroys the matrix. 23 | */ 24 | double matd_to_double(matd_t *a) 25 | { 26 | assert(matd_is_scalar(a)); 27 | double d = a->data[0]; 28 | matd_destroy(a); 29 | return d; 30 | } 31 | 32 | /** 33 | * @param v Image points on the image plane. 34 | * @param p Object points in object space. 35 | * @outparam t Optimal translation. 36 | * @param R In/Outparam. Should be set to initial guess at R. Will be modified to be the optimal translation. 37 | * @param n_points Number of points. 38 | * @param n_steps Number of iterations. 39 | * 40 | * @return Object-space error after iteration. 41 | * 42 | * Implementation of Orthogonal Iteration from Lu, 2000. 43 | */ 44 | double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int n_points, int n_steps) { 45 | matd_t* p_mean = matd_create(3, 1); 46 | for (int i = 0; i < n_points; i++) { 47 | matd_add_inplace(p_mean, p[i]); 48 | } 49 | matd_scale_inplace(p_mean, 1.0/n_points); 50 | 51 | matd_t** p_res = malloc(sizeof(matd_t *)*n_points); 52 | for (int i = 0; i < n_points; i++) { 53 | p_res[i] = matd_op("M-M", p[i], p_mean); 54 | } 55 | 56 | // Compute M1_inv. 57 | matd_t** F = malloc(sizeof(matd_t *)*n_points); 58 | matd_t *avg_F = matd_create(3, 3); 59 | for (int i = 0; i < n_points; i++) { 60 | F[i] = calculate_F(v[i]); 61 | matd_add_inplace(avg_F, F[i]); 62 | } 63 | matd_scale_inplace(avg_F, 1.0/n_points); 64 | matd_t *I3 = matd_identity(3); 65 | matd_t *M1 = matd_subtract(I3, avg_F); 66 | matd_t *M1_inv = matd_inverse(M1); 67 | matd_destroy(avg_F); 68 | matd_destroy(M1); 69 | 70 | double prev_error = HUGE_VAL; 71 | // Iterate. 72 | for (int i = 0; i < n_steps; i++) { 73 | // Calculate translation. 74 | matd_t *M2 = matd_create(3, 1); 75 | for (int j = 0; j < n_points; j++) { 76 | matd_t* M2_update = matd_op("(M - M)*M*M", F[j], I3, *R, p[j]); 77 | matd_add_inplace(M2, M2_update); 78 | matd_destroy(M2_update); 79 | } 80 | matd_scale_inplace(M2, 1.0/n_points); 81 | matd_destroy(*t); 82 | *t = matd_multiply(M1_inv, M2); 83 | matd_destroy(M2); 84 | 85 | // Calculate rotation. 86 | matd_t** q = malloc(sizeof(matd_t *)*n_points); 87 | matd_t* q_mean = matd_create(3, 1); 88 | for (int j = 0; j < n_points; j++) { 89 | q[j] = matd_op("M*(M*M+M)", F[j], *R, p[j], *t); 90 | matd_add_inplace(q_mean, q[j]); 91 | } 92 | matd_scale_inplace(q_mean, 1.0/n_points); 93 | 94 | matd_t* M3 = matd_create(3, 3); 95 | for (int j = 0; j < n_points; j++) { 96 | matd_t *M3_update = matd_op("(M-M)*M'", q[j], q_mean, p_res[j]); 97 | matd_add_inplace(M3, M3_update); 98 | matd_destroy(M3_update); 99 | } 100 | matd_svd_t M3_svd = matd_svd(M3); 101 | matd_destroy(M3); 102 | matd_destroy(*R); 103 | *R = matd_op("M*M'", M3_svd.U, M3_svd.V); 104 | matd_destroy(M3_svd.U); 105 | matd_destroy(M3_svd.S); 106 | matd_destroy(M3_svd.V); 107 | matd_destroy(q_mean); 108 | for (int j = 0; j < n_points; j++) { 109 | matd_destroy(q[j]); 110 | } 111 | 112 | double error = 0; 113 | for (int j = 0; j < 4; j++) { 114 | matd_t* err_vec = matd_op("(M-M)(MM+M)", I3, F[j], *R, p[j], *t); 115 | error += matd_to_double(matd_op("M'M", err_vec, err_vec)); 116 | matd_destroy(err_vec); 117 | } 118 | prev_error = error; 119 | 120 | free(q); 121 | } 122 | 123 | matd_destroy(I3); 124 | matd_destroy(M1_inv); 125 | for (int i = 0; i < n_points; i++) { 126 | matd_destroy(p_res[i]); 127 | matd_destroy(F[i]); 128 | } 129 | free(p_res); 130 | free(F); 131 | matd_destroy(p_mean); 132 | return prev_error; 133 | } 134 | 135 | /** 136 | * Evaluates polynomial p at x. 137 | */ 138 | double polyval(double* p, int degree, double x) { 139 | double ret = 0; 140 | for (int i = 0; i <= degree; i++) { 141 | ret += p[i]*pow(x, i); 142 | } 143 | return ret; 144 | } 145 | 146 | /** 147 | * Numerically solve small degree polynomials. This is a customized method. It 148 | * ignores roots larger than 1000 and only gives small roots approximately. 149 | * 150 | * @param p Array of parameters s.t. p(x) = p[0] + p[1]*x + ... 151 | * @param degree The degree of p(x). 152 | * @outparam roots 153 | * @outparam n_roots 154 | */ 155 | void solve_poly_approx(double* p, int degree, double* roots, int* n_roots) { 156 | static const int MAX_ROOT = 1000; 157 | if (degree == 1) { 158 | if (fabs(p[0]) > MAX_ROOT*fabs(p[1])) { 159 | *n_roots = 0; 160 | } else { 161 | roots[0] = -p[0]/p[1]; 162 | *n_roots = 1; 163 | } 164 | return; 165 | } 166 | 167 | // Calculate roots of derivative. 168 | double *p_der = malloc(sizeof(double)*degree); 169 | for (int i = 0; i < degree; i++) { 170 | p_der[i] = (i + 1) * p[i+1]; 171 | } 172 | 173 | double *der_roots = malloc(sizeof(double)*(degree - 1)); 174 | int n_der_roots; 175 | solve_poly_approx(p_der, degree - 1, der_roots, &n_der_roots); 176 | 177 | 178 | // Go through all possibilities for roots of the polynomial. 179 | *n_roots = 0; 180 | for (int i = 0; i <= n_der_roots; i++) { 181 | double min; 182 | if (i == 0) { 183 | min = -MAX_ROOT; 184 | } else { 185 | min = der_roots[i - 1]; 186 | } 187 | 188 | double max; 189 | if (i == n_der_roots) { 190 | max = MAX_ROOT; 191 | } else { 192 | max = der_roots[i]; 193 | } 194 | 195 | if (polyval(p, degree, min)*polyval(p, degree, max) < 0) { 196 | // We have a zero-crossing in this interval, use a combination of Newton' and bisection. 197 | // Some thanks to Numerical Recipes in C. 198 | 199 | double lower; 200 | double upper; 201 | if (polyval(p, degree, min) < polyval(p, degree, max)) { 202 | lower = min; 203 | upper = max; 204 | } else { 205 | lower = max; 206 | upper = min; 207 | } 208 | double root = 0.5*(lower + upper); 209 | double dx_old = upper - lower; 210 | double dx = dx_old; 211 | double f = polyval(p, degree, root); 212 | double df = polyval(p_der, degree - 1, root); 213 | 214 | for (int j = 0; j < 100; j++) { 215 | if (((f + df*(upper - root))*(f + df*(lower - root)) > 0) 216 | || (fabs(2*f) > fabs(dx_old*df))) { 217 | dx_old = dx; 218 | dx = 0.5*(upper - lower); 219 | root = lower + dx; 220 | } else { 221 | dx_old = dx; 222 | dx = -f/df; 223 | root += dx; 224 | } 225 | 226 | if (root == upper || root == lower) { 227 | break; 228 | } 229 | 230 | f = polyval(p, degree, root); 231 | df = polyval(p_der, degree - 1, root); 232 | 233 | if (f > 0) { 234 | upper = root; 235 | } else { 236 | lower = root; 237 | } 238 | } 239 | 240 | roots[(*n_roots)++] = root; 241 | } else if(polyval(p, degree, max) == 0) { 242 | // Double/triple root. 243 | roots[(*n_roots)++] = max; 244 | } 245 | } 246 | 247 | free(der_roots); 248 | free(p_der); 249 | } 250 | 251 | /** 252 | * Given a local minima of the pose error tries to find the other minima. 253 | */ 254 | matd_t* fix_pose_ambiguities(matd_t** v, matd_t** p, matd_t* t, matd_t* R, int n_points) { 255 | matd_t* I3 = matd_identity(3); 256 | 257 | // 1. Find R_t 258 | matd_t* R_t_3 = matd_vec_normalize(t); 259 | 260 | matd_t* e_x = matd_create(3, 1); 261 | MATD_EL(e_x, 0, 0) = 1; 262 | matd_t* R_t_1_tmp = matd_op("M-(M'*M)*M", e_x, e_x, R_t_3, R_t_3); 263 | matd_t* R_t_1 = matd_vec_normalize(R_t_1_tmp); 264 | matd_destroy(e_x); 265 | matd_destroy(R_t_1_tmp); 266 | 267 | matd_t* R_t_2 = matd_crossproduct(R_t_3, R_t_1); 268 | 269 | matd_t* R_t = matd_create_data(3, 3, (double[]) { 270 | MATD_EL(R_t_1, 0, 0), MATD_EL(R_t_1, 0, 1), MATD_EL(R_t_1, 0, 2), 271 | MATD_EL(R_t_2, 0, 0), MATD_EL(R_t_2, 0, 1), MATD_EL(R_t_2, 0, 2), 272 | MATD_EL(R_t_3, 0, 0), MATD_EL(R_t_3, 0, 1), MATD_EL(R_t_3, 0, 2)}); 273 | matd_destroy(R_t_1); 274 | matd_destroy(R_t_2); 275 | matd_destroy(R_t_3); 276 | 277 | // 2. Find R_z 278 | matd_t* R_1_prime = matd_multiply(R_t, R); 279 | double r31 = MATD_EL(R_1_prime, 2, 0); 280 | double r32 = MATD_EL(R_1_prime, 2, 1); 281 | double hypotenuse = sqrt(r31*r31 + r32*r32); 282 | if (hypotenuse < 1e-100) { 283 | r31 = 1; 284 | r32 = 0; 285 | hypotenuse = 1; 286 | } 287 | matd_t* R_z = matd_create_data(3, 3, (double[]) { 288 | r31/hypotenuse, -r32/hypotenuse, 0, 289 | r32/hypotenuse, r31/hypotenuse, 0, 290 | 0, 0, 1}); 291 | 292 | // 3. Calculate parameters of Eos 293 | matd_t* R_trans = matd_multiply(R_1_prime, R_z); 294 | double sin_gamma = -MATD_EL(R_trans, 0, 1); 295 | double cos_gamma = MATD_EL(R_trans, 1, 1); 296 | matd_t* R_gamma = matd_create_data(3, 3, (double[]) { 297 | cos_gamma, -sin_gamma, 0, 298 | sin_gamma, cos_gamma, 0, 299 | 0, 0, 1}); 300 | 301 | double sin_beta = -MATD_EL(R_trans, 2, 0); 302 | double cos_beta = MATD_EL(R_trans, 2, 2); 303 | double t_initial = atan2(sin_beta, cos_beta); 304 | matd_destroy(R_trans); 305 | 306 | matd_t** v_trans = malloc(sizeof(matd_t *)*n_points); 307 | matd_t** p_trans = malloc(sizeof(matd_t *)*n_points); 308 | matd_t** F_trans = malloc(sizeof(matd_t *)*n_points); 309 | matd_t* avg_F_trans = matd_create(3, 3); 310 | for (int i = 0; i < n_points; i++) { 311 | p_trans[i] = matd_op("M'*M", R_z, p[i]); 312 | v_trans[i] = matd_op("M*M", R_t, v[i]); 313 | F_trans[i] = calculate_F(v_trans[i]); 314 | matd_add_inplace(avg_F_trans, F_trans[i]); 315 | } 316 | matd_scale_inplace(avg_F_trans, 1.0/n_points); 317 | 318 | matd_t* G = matd_op("(M-M)^-1", I3, avg_F_trans); 319 | matd_scale_inplace(G, 1.0/n_points); 320 | 321 | matd_t* M1 = matd_create_data(3, 3, (double[]) { 322 | 0, 0, 2, 323 | 0, 0, 0, 324 | -2, 0, 0}); 325 | matd_t* M2 = matd_create_data(3, 3, (double[]) { 326 | -1, 0, 0, 327 | 0, 1, 0, 328 | 0, 0, -1}); 329 | 330 | matd_t* b0 = matd_create(3, 1); 331 | matd_t* b1 = matd_create(3, 1); 332 | matd_t* b2 = matd_create(3, 1); 333 | for (int i = 0; i < n_points; i++) { 334 | matd_t* op_tmp1 = matd_op("(M-M)MM", F_trans[i], I3, R_gamma, p_trans[i]); 335 | matd_t* op_tmp2 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M1, p_trans[i]); 336 | matd_t* op_tmp3 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M2, p_trans[i]); 337 | 338 | matd_add_inplace(b0, op_tmp1); 339 | matd_add_inplace(b1, op_tmp2); 340 | matd_add_inplace(b2, op_tmp3); 341 | 342 | matd_destroy(op_tmp1); 343 | matd_destroy(op_tmp2); 344 | matd_destroy(op_tmp3); 345 | } 346 | matd_t* b0_ = matd_multiply(G, b0); 347 | matd_t* b1_ = matd_multiply(G, b1); 348 | matd_t* b2_ = matd_multiply(G, b2); 349 | 350 | double a0 = 0; 351 | double a1 = 0; 352 | double a2 = 0; 353 | double a3 = 0; 354 | double a4 = 0; 355 | for (int i = 0; i < n_points; i++) { 356 | matd_t* c0 = matd_op("(M-M)(MM+M)", I3, F_trans[i], R_gamma, p_trans[i], b0_); 357 | matd_t* c1 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M1, p_trans[i], b1_); 358 | matd_t* c2 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M2, p_trans[i], b2_); 359 | 360 | a0 += matd_to_double(matd_op("M'M", c0, c0)); 361 | a1 += matd_to_double(matd_op("2M'M", c0, c1)); 362 | a2 += matd_to_double(matd_op("M'M+2M'M", c1, c1, c0, c2)); 363 | a3 += matd_to_double(matd_op("2M'M", c1, c2)); 364 | a4 += matd_to_double(matd_op("M'M", c2, c2)); 365 | 366 | matd_destroy(c0); 367 | matd_destroy(c1); 368 | matd_destroy(c2); 369 | } 370 | 371 | matd_destroy(b0); 372 | matd_destroy(b1); 373 | matd_destroy(b2); 374 | matd_destroy(b0_); 375 | matd_destroy(b1_); 376 | matd_destroy(b2_); 377 | 378 | for (int i = 0; i < n_points; i++) { 379 | matd_destroy(p_trans[i]); 380 | matd_destroy(v_trans[i]); 381 | matd_destroy(F_trans[i]); 382 | } 383 | free(p_trans); 384 | free(v_trans); 385 | free(F_trans); 386 | matd_destroy(avg_F_trans); 387 | matd_destroy(G); 388 | 389 | 390 | // 4. Solve for minima of Eos. 391 | double p0 = a1; 392 | double p1 = 2*a2 - 4*a0; 393 | double p2 = 3*a3 - 3*a1; 394 | double p3 = 4*a4 - 2*a2; 395 | double p4 = -a3; 396 | 397 | double roots[4]; 398 | int n_roots; 399 | solve_poly_approx((double []) {p0, p1, p2, p3, p4}, 4, roots, &n_roots); 400 | 401 | double minima[4]; 402 | int n_minima = 0; 403 | for (int i = 0; i < n_roots; i++) { 404 | double t1 = roots[i]; 405 | double t2 = t1*t1; 406 | double t3 = t1*t2; 407 | double t4 = t1*t3; 408 | double t5 = t1*t4; 409 | // Check extrema is a minima. 410 | if (a2 - 2*a0 + (3*a3 - 6*a1)*t1 + (6*a4 - 8*a2 + 10*a0)*t2 + (-8*a3 + 6*a1)*t3 + (-6*a4 + 3*a2)*t4 + a3*t5 >= 0) { 411 | // And that it corresponds to an angle different than the known minimum. 412 | double t_cur = 2*atan(roots[i]); 413 | // We only care about finding a second local minima which is qualitatively 414 | // different than the first. 415 | if (fabs(t_cur - t_initial) > 0.1) { 416 | minima[n_minima++] = roots[i]; 417 | } 418 | } 419 | } 420 | 421 | // 5. Get poses for minima. 422 | matd_t* ret = NULL; 423 | if (n_minima == 1) { 424 | double t_cur = minima[0]; 425 | matd_t* R_beta = matd_copy(M2); 426 | matd_scale_inplace(R_beta, t_cur); 427 | matd_add_inplace(R_beta, M1); 428 | matd_scale_inplace(R_beta, t_cur); 429 | matd_add_inplace(R_beta, I3); 430 | matd_scale_inplace(R_beta, 1/(1 + t_cur*t_cur)); 431 | ret = matd_op("M'MMM'", R_t, R_gamma, R_beta, R_z); 432 | matd_destroy(R_beta); 433 | } else if (n_minima > 1) { 434 | // This can happen if our prior pose estimate was not very good. 435 | fprintf(stderr, "Error, more than one new minimum found.\n"); 436 | } 437 | matd_destroy(I3); 438 | matd_destroy(M1); 439 | matd_destroy(M2); 440 | matd_destroy(R_t); 441 | matd_destroy(R_gamma); 442 | matd_destroy(R_z); 443 | matd_destroy(R_1_prime); 444 | return ret; 445 | } 446 | 447 | /** 448 | * Estimate pose of the tag using the homography method. 449 | */ 450 | void estimate_pose_for_tag_homography(apriltag_detection_info_t* info, apriltag_pose_t* solution) { 451 | double scale = info->tagsize/2.0; 452 | 453 | matd_t *M_H = homography_to_pose(info->det->H, -info->fx, info->fy, info->cx, info->cy); 454 | MATD_EL(M_H, 0, 3) *= scale; 455 | MATD_EL(M_H, 1, 3) *= scale; 456 | MATD_EL(M_H, 2, 3) *= scale; 457 | 458 | matd_t* fix = matd_create(4, 4); 459 | MATD_EL(fix, 0, 0) = 1; 460 | MATD_EL(fix, 1, 1) = -1; 461 | MATD_EL(fix, 2, 2) = -1; 462 | MATD_EL(fix, 3, 3) = 1; 463 | 464 | matd_t* initial_pose = matd_multiply(fix, M_H); 465 | matd_destroy(M_H); 466 | matd_destroy(fix); 467 | 468 | solution->R = matd_create(3, 3); 469 | for (int i = 0; i < 3; i++) { 470 | for (int j = 0; j < 3; j++) { 471 | MATD_EL(solution->R, i, j) = MATD_EL(initial_pose, i, j); 472 | } 473 | } 474 | 475 | solution->t = matd_create(3, 1); 476 | for (int i = 0; i < 3; i++) { 477 | MATD_EL(solution->t, i, 0) = MATD_EL(initial_pose, i, 3); 478 | } 479 | matd_destroy(initial_pose); 480 | } 481 | 482 | /** 483 | * Estimate tag pose using orthogonal iteration. 484 | */ 485 | void estimate_tag_pose_orthogonal_iteration( 486 | apriltag_detection_info_t* info, 487 | double* err1, 488 | apriltag_pose_t* solution1, 489 | double* err2, 490 | apriltag_pose_t* solution2, 491 | int nIters) { 492 | double scale = info->tagsize/2.0; 493 | matd_t* p[4] = { 494 | matd_create_data(3, 1, (double[]) {-scale, scale, 0}), 495 | matd_create_data(3, 1, (double[]) {scale, scale, 0}), 496 | matd_create_data(3, 1, (double[]) {scale, -scale, 0}), 497 | matd_create_data(3, 1, (double[]) {-scale, -scale, 0})}; 498 | matd_t* v[4]; 499 | for (int i = 0; i < 4; i++) { 500 | v[i] = matd_create_data(3, 1, (double[]) { 501 | (info->det->p[i][0] - info->cx)/info->fx, (info->det->p[i][1] - info->cy)/info->fy, 1}); 502 | } 503 | 504 | estimate_pose_for_tag_homography(info, solution1); 505 | *err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters); 506 | solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4); 507 | if (solution2->R) { 508 | solution2->t = matd_create(3, 1); 509 | *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters); 510 | } else { 511 | *err2 = HUGE_VAL; 512 | } 513 | 514 | for (int i = 0; i < 4; i++) { 515 | matd_destroy(p[i]); 516 | matd_destroy(v[i]); 517 | } 518 | } 519 | 520 | /** 521 | * Estimate tag pose. 522 | */ 523 | double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) { 524 | double err1, err2; 525 | apriltag_pose_t pose1, pose2; 526 | estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50); 527 | if (err1 <= err2) { 528 | pose->R = pose1.R; 529 | pose->t = pose1.t; 530 | if (pose2.R) { 531 | matd_destroy(pose2.t); 532 | matd_destroy(pose2.R); 533 | } 534 | return err1; 535 | } else { 536 | pose->R = pose2.R; 537 | pose->t = pose2.t; 538 | if (pose1.R) { 539 | matd_destroy(pose1.R); 540 | matd_destroy(pose1.t); 541 | } 542 | return err2; 543 | } 544 | } 545 | -------------------------------------------------------------------------------- /Machine Vision Toolbox for MATLAB/vision/mex/apriltag/apriltag_pose.c.old: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "apriltag_pose.h" 5 | #include "apriltag_math.h" 6 | #include "common/homography.h" 7 | #include "common/image_u8x3.h" 8 | 9 | 10 | /** 11 | * Calculate projection operator from image points. 12 | */ 13 | matd_t* calculate_F(matd_t* v) { 14 | matd_t* outer_product = matd_op("MM'", v, v, v, v); 15 | matd_t* inner_product = matd_op("M'M", v, v); 16 | matd_scale_inplace(outer_product, 1.0/inner_product->data[0]); 17 | matd_destroy(inner_product); 18 | return outer_product; 19 | } 20 | 21 | /** 22 | * Returns the value of the supplied scalar matrix 'a' and destroys the matrix. 23 | */ 24 | double matd_to_double(matd_t *a) 25 | { 26 | assert(matd_is_scalar(a)); 27 | double d = a->data[0]; 28 | matd_destroy(a); 29 | return d; 30 | } 31 | 32 | /** 33 | * @param v Image points on the image plane. 34 | * @param p Object points in object space. 35 | * @outparam t Optimal translation. 36 | * @param R In/Outparam. Should be set to initial guess at R. Will be modified to be the optimal translation. 37 | * @param n_points Number of points. 38 | * @param n_steps Number of iterations. 39 | * 40 | * @return Object-space error after iteration. 41 | * 42 | * Implementation of Orthogonal Iteration from Lu, 2000. 43 | */ 44 | double orthogonal_iteration(matd_t** v, matd_t** p, matd_t** t, matd_t** R, int n_points, int n_steps) { 45 | matd_t* p_mean = matd_create(3, 1); 46 | for (int i = 0; i < n_points; i++) { 47 | matd_add_inplace(p_mean, p[i]); 48 | } 49 | matd_scale_inplace(p_mean, 1.0/n_points); 50 | 51 | matd_t* p_res[n_points]; 52 | for (int i = 0; i < n_points; i++) { 53 | p_res[i] = matd_op("M-M", p[i], p_mean); 54 | } 55 | 56 | // Compute M1_inv. 57 | matd_t* F[n_points]; 58 | matd_t *avg_F = matd_create(3, 3); 59 | for (int i = 0; i < n_points; i++) { 60 | F[i] = calculate_F(v[i]); 61 | matd_add_inplace(avg_F, F[i]); 62 | } 63 | matd_scale_inplace(avg_F, 1.0/n_points); 64 | matd_t *I3 = matd_identity(3); 65 | matd_t *M1 = matd_subtract(I3, avg_F); 66 | matd_t *M1_inv = matd_inverse(M1); 67 | matd_destroy(avg_F); 68 | matd_destroy(M1); 69 | 70 | double prev_error = HUGE_VAL; 71 | // Iterate. 72 | for (int i = 0; i < n_steps; i++) { 73 | // Calculate translation. 74 | matd_t *M2 = matd_create(3, 1); 75 | for (int j = 0; j < n_points; j++) { 76 | matd_t* M2_update = matd_op("(M - M)*M*M", F[j], I3, *R, p[j]); 77 | matd_add_inplace(M2, M2_update); 78 | matd_destroy(M2_update); 79 | } 80 | matd_scale_inplace(M2, 1.0/n_points); 81 | matd_destroy(*t); 82 | *t = matd_multiply(M1_inv, M2); 83 | matd_destroy(M2); 84 | 85 | // Calculate rotation. 86 | matd_t* q[n_points]; 87 | matd_t* q_mean = matd_create(3, 1); 88 | for (int j = 0; j < n_points; j++) { 89 | q[j] = matd_op("M*(M*M+M)", F[j], *R, p[j], *t); 90 | matd_add_inplace(q_mean, q[j]); 91 | } 92 | matd_scale_inplace(q_mean, 1.0/n_points); 93 | 94 | matd_t* M3 = matd_create(3, 3); 95 | for (int j = 0; j < n_points; j++) { 96 | matd_t *M3_update = matd_op("(M-M)*M'", q[j], q_mean, p_res[j]); 97 | matd_add_inplace(M3, M3_update); 98 | matd_destroy(M3_update); 99 | } 100 | matd_svd_t M3_svd = matd_svd(M3); 101 | matd_destroy(M3); 102 | matd_destroy(*R); 103 | *R = matd_op("M*M'", M3_svd.U, M3_svd.V); 104 | matd_destroy(M3_svd.U); 105 | matd_destroy(M3_svd.S); 106 | matd_destroy(M3_svd.V); 107 | matd_destroy(q_mean); 108 | for (int j = 0; j < n_points; j++) { 109 | matd_destroy(q[j]); 110 | } 111 | 112 | double error = 0; 113 | for (int i = 0; i < 4; i++) { 114 | matd_t* err_vec = matd_op("(M-M)(MM+M)", I3, F[i], *R, p[i], *t); 115 | error += matd_to_double(matd_op("M'M", err_vec, err_vec)); 116 | matd_destroy(err_vec); 117 | } 118 | prev_error = error; 119 | } 120 | 121 | matd_destroy(I3); 122 | matd_destroy(M1_inv); 123 | for (int i = 0; i < n_points; i++) { 124 | matd_destroy(p_res[i]); 125 | matd_destroy(F[i]); 126 | } 127 | matd_destroy(p_mean); 128 | return prev_error; 129 | } 130 | 131 | /** 132 | * Evaluates polynomial p at x. 133 | */ 134 | double polyval(double* p, int degree, double x) { 135 | double ret = 0; 136 | for (int i = 0; i <= degree; i++) { 137 | ret += p[i]*pow(x, i); 138 | } 139 | return ret; 140 | } 141 | 142 | /** 143 | * Numerically solve small degree polynomials. This is a customized method. It 144 | * ignores roots larger than 1000 and only gives small roots approximately. 145 | * 146 | * @param p Array of parameters s.t. p(x) = p[0] + p[1]*x + ... 147 | * @param degree The degree of p(x). 148 | * @outparam roots 149 | * @outparam n_roots 150 | */ 151 | void solve_poly_approx(double* p, int degree, double* roots, int* n_roots) { 152 | static const int MAX_ROOT = 1000; 153 | if (degree == 1) { 154 | if (fabs(p[0]) > MAX_ROOT*fabs(p[1])) { 155 | *n_roots = 0; 156 | } else { 157 | roots[0] = -p[0]/p[1]; 158 | *n_roots = 1; 159 | } 160 | return; 161 | } 162 | 163 | // Calculate roots of derivative. 164 | double p_der[degree]; 165 | for (int i = 0; i < degree; i++) { 166 | p_der[i] = (i + 1) * p[i+1]; 167 | } 168 | 169 | double der_roots[degree - 1]; 170 | int n_der_roots; 171 | solve_poly_approx(p_der, degree - 1, der_roots, &n_der_roots); 172 | 173 | 174 | // Go through all possibilities for roots of the polynomial. 175 | *n_roots = 0; 176 | for (int i = 0; i <= n_der_roots; i++) { 177 | double min; 178 | if (i == 0) { 179 | min = -MAX_ROOT; 180 | } else { 181 | min = der_roots[i - 1]; 182 | } 183 | 184 | double max; 185 | if (i == n_der_roots) { 186 | max = MAX_ROOT; 187 | } else { 188 | max = der_roots[i]; 189 | } 190 | 191 | if (polyval(p, degree, min)*polyval(p, degree, max) < 0) { 192 | // We have a zero-crossing in this interval, use a combination of Newton' and bisection. 193 | // Some thanks to Numerical Recipes in C. 194 | 195 | double lower; 196 | double upper; 197 | if (polyval(p, degree, min) < polyval(p, degree, max)) { 198 | lower = min; 199 | upper = max; 200 | } else { 201 | lower = max; 202 | upper = min; 203 | } 204 | double root = 0.5*(lower + upper); 205 | double dx_old = upper - lower; 206 | double dx = dx_old; 207 | double f = polyval(p, degree, root); 208 | double df = polyval(p_der, degree - 1, root); 209 | 210 | for (int j = 0; j < 100; j++) { 211 | if (((f + df*(upper - root))*(f + df*(lower - root)) > 0) 212 | || (fabs(2*f) > fabs(dx_old*df))) { 213 | dx_old = dx; 214 | dx = 0.5*(upper - lower); 215 | root = lower + dx; 216 | } else { 217 | dx_old = dx; 218 | dx = -f/df; 219 | root += dx; 220 | } 221 | 222 | if (root == upper || root == lower) { 223 | break; 224 | } 225 | 226 | f = polyval(p, degree, root); 227 | df = polyval(p_der, degree - 1, root); 228 | 229 | if (f > 0) { 230 | upper = root; 231 | } else { 232 | lower = root; 233 | } 234 | } 235 | 236 | roots[(*n_roots)++] = root; 237 | } else if(polyval(p, degree, max) == 0) { 238 | // Double/triple root. 239 | roots[(*n_roots)++] = max; 240 | } 241 | } 242 | } 243 | 244 | /** 245 | * Given a local minima of the pose error tries to find the other minima. 246 | */ 247 | matd_t* fix_pose_ambiguities(matd_t** v, matd_t** p, matd_t* t, matd_t* R, int n_points) { 248 | matd_t* I3 = matd_identity(3); 249 | 250 | // 1. Find R_t 251 | matd_t* R_t_3 = matd_vec_normalize(t); 252 | 253 | matd_t* e_x = matd_create(3, 1); 254 | MATD_EL(e_x, 0, 0) = 1; 255 | matd_t* R_t_1_tmp = matd_op("M-(M'*M)*M", e_x, e_x, R_t_3, R_t_3); 256 | matd_t* R_t_1 = matd_vec_normalize(R_t_1_tmp); 257 | matd_destroy(e_x); 258 | matd_destroy(R_t_1_tmp); 259 | 260 | matd_t* R_t_2 = matd_crossproduct(R_t_3, R_t_1); 261 | 262 | matd_t* R_t = matd_create_data(3, 3, (double[]) { 263 | MATD_EL(R_t_1, 0, 0), MATD_EL(R_t_1, 0, 1), MATD_EL(R_t_1, 0, 2), 264 | MATD_EL(R_t_2, 0, 0), MATD_EL(R_t_2, 0, 1), MATD_EL(R_t_2, 0, 2), 265 | MATD_EL(R_t_3, 0, 0), MATD_EL(R_t_3, 0, 1), MATD_EL(R_t_3, 0, 2)}); 266 | matd_destroy(R_t_1); 267 | matd_destroy(R_t_2); 268 | matd_destroy(R_t_3); 269 | 270 | // 2. Find R_z 271 | matd_t* R_1_prime = matd_multiply(R_t, R); 272 | double r31 = MATD_EL(R_1_prime, 2, 0); 273 | double r32 = MATD_EL(R_1_prime, 2, 1); 274 | double hypotenuse = sqrt(r31*r31 + r32*r32); 275 | if (hypotenuse < 1e-100) { 276 | r31 = 1; 277 | r32 = 0; 278 | hypotenuse = 1; 279 | } 280 | matd_t* R_z = matd_create_data(3, 3, (double[]) { 281 | r31/hypotenuse, -r32/hypotenuse, 0, 282 | r32/hypotenuse, r31/hypotenuse, 0, 283 | 0, 0, 1}); 284 | 285 | // 3. Calculate parameters of Eos 286 | matd_t* R_trans = matd_multiply(R_1_prime, R_z); 287 | double sin_gamma = -MATD_EL(R_trans, 0, 1); 288 | double cos_gamma = MATD_EL(R_trans, 1, 1); 289 | matd_t* R_gamma = matd_create_data(3, 3, (double[]) { 290 | cos_gamma, -sin_gamma, 0, 291 | sin_gamma, cos_gamma, 0, 292 | 0, 0, 1}); 293 | 294 | double sin_beta = -MATD_EL(R_trans, 2, 0); 295 | double cos_beta = MATD_EL(R_trans, 2, 2); 296 | double t_initial = atan2(sin_beta, cos_beta); 297 | matd_destroy(R_trans); 298 | 299 | matd_t* v_trans[n_points]; 300 | matd_t* p_trans[n_points]; 301 | matd_t* F_trans[n_points]; 302 | matd_t* avg_F_trans = matd_create(3, 3); 303 | for (int i = 0; i < n_points; i++) { 304 | p_trans[i] = matd_op("M'*M", R_z, p[i]); 305 | v_trans[i] = matd_op("M*M", R_t, v[i]); 306 | F_trans[i] = calculate_F(v_trans[i]); 307 | matd_add_inplace(avg_F_trans, F_trans[i]); 308 | } 309 | matd_scale_inplace(avg_F_trans, 1.0/n_points); 310 | 311 | matd_t* G = matd_op("(M-M)^-1", I3, avg_F_trans); 312 | matd_scale_inplace(G, 1.0/n_points); 313 | 314 | matd_t* M1 = matd_create_data(3, 3, (double[]) { 315 | 0, 0, 2, 316 | 0, 0, 0, 317 | -2, 0, 0}); 318 | matd_t* M2 = matd_create_data(3, 3, (double[]) { 319 | -1, 0, 0, 320 | 0, 1, 0, 321 | 0, 0, -1}); 322 | 323 | matd_t* b0 = matd_create(3, 1); 324 | matd_t* b1 = matd_create(3, 1); 325 | matd_t* b2 = matd_create(3, 1); 326 | for (int i = 0; i < n_points; i++) { 327 | matd_t* op_tmp1 = matd_op("(M-M)MM", F_trans[i], I3, R_gamma, p_trans[i]); 328 | matd_t* op_tmp2 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M1, p_trans[i]); 329 | matd_t* op_tmp3 = matd_op("(M-M)MMM", F_trans[i], I3, R_gamma, M2, p_trans[i]); 330 | 331 | matd_add_inplace(b0, op_tmp1); 332 | matd_add_inplace(b1, op_tmp2); 333 | matd_add_inplace(b2, op_tmp3); 334 | 335 | matd_destroy(op_tmp1); 336 | matd_destroy(op_tmp2); 337 | matd_destroy(op_tmp3); 338 | } 339 | matd_t* b0_ = matd_multiply(G, b0); 340 | matd_t* b1_ = matd_multiply(G, b1); 341 | matd_t* b2_ = matd_multiply(G, b2); 342 | 343 | double a0 = 0; 344 | double a1 = 0; 345 | double a2 = 0; 346 | double a3 = 0; 347 | double a4 = 0; 348 | for (int i = 0; i < n_points; i++) { 349 | matd_t* c0 = matd_op("(M-M)(MM+M)", I3, F_trans[i], R_gamma, p_trans[i], b0_); 350 | matd_t* c1 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M1, p_trans[i], b1_); 351 | matd_t* c2 = matd_op("(M-M)(MMM+M)", I3, F_trans[i], R_gamma, M2, p_trans[i], b2_); 352 | 353 | a0 += matd_to_double(matd_op("M'M", c0, c0)); 354 | a1 += matd_to_double(matd_op("2M'M", c0, c1)); 355 | a2 += matd_to_double(matd_op("M'M+2M'M", c1, c1, c0, c2)); 356 | a3 += matd_to_double(matd_op("2M'M", c1, c2)); 357 | a4 += matd_to_double(matd_op("M'M", c2, c2)); 358 | 359 | matd_destroy(c0); 360 | matd_destroy(c1); 361 | matd_destroy(c2); 362 | } 363 | 364 | matd_destroy(b0); 365 | matd_destroy(b1); 366 | matd_destroy(b2); 367 | matd_destroy(b0_); 368 | matd_destroy(b1_); 369 | matd_destroy(b2_); 370 | 371 | for (int i = 0; i < n_points; i++) { 372 | matd_destroy(p_trans[i]); 373 | matd_destroy(v_trans[i]); 374 | matd_destroy(F_trans[i]); 375 | } 376 | matd_destroy(avg_F_trans); 377 | matd_destroy(G); 378 | 379 | 380 | // 4. Solve for minima of Eos. 381 | double p0 = a1; 382 | double p1 = 2*a2 - 4*a0; 383 | double p2 = 3*a3 - 3*a1; 384 | double p3 = 4*a4 - 2*a2; 385 | double p4 = -a3; 386 | 387 | double roots[4]; 388 | int n_roots; 389 | solve_poly_approx((double []) {p0, p1, p2, p3, p4}, 4, roots, &n_roots); 390 | 391 | double minima[4]; 392 | int n_minima = 0; 393 | for (int i = 0; i < n_roots; i++) { 394 | double t1 = roots[i]; 395 | double t2 = t1*t1; 396 | double t3 = t1*t2; 397 | double t4 = t1*t3; 398 | double t5 = t1*t4; 399 | // Check extrema is a minima. 400 | if (a2 - 2*a0 + (3*a3 - 6*a1)*t1 + (6*a4 - 8*a2 + 10*a0)*t2 + (-8*a3 + 6*a1)*t3 + (-6*a4 + 3*a2)*t4 + a3*t5 >= 0) { 401 | // And that it corresponds to an angle different than the known minimum. 402 | double t = 2*atan(roots[i]); 403 | // We only care about finding a second local minima which is qualitatively 404 | // different than the first. 405 | if (fabs(t - t_initial) > 0.1) { 406 | minima[n_minima++] = roots[i]; 407 | } 408 | } 409 | } 410 | 411 | // 5. Get poses for minima. 412 | matd_t* ret = NULL; 413 | if (n_minima == 1) { 414 | double t = minima[0]; 415 | matd_t* R_beta = matd_copy(M2); 416 | matd_scale_inplace(R_beta, t); 417 | matd_add_inplace(R_beta, M1); 418 | matd_scale_inplace(R_beta, t); 419 | matd_add_inplace(R_beta, I3); 420 | matd_scale_inplace(R_beta, 1/(1 + t*t)); 421 | ret = matd_op("M'MMM'", R_t, R_gamma, R_beta, R_z); 422 | matd_destroy(R_beta); 423 | } else if (n_minima > 1) { 424 | // This can happen if our prior pose estimate was not very good. 425 | fprintf(stderr, "Error, more than one new minima found.\n"); 426 | } 427 | matd_destroy(I3); 428 | matd_destroy(M1); 429 | matd_destroy(M2); 430 | matd_destroy(R_t); 431 | matd_destroy(R_gamma); 432 | matd_destroy(R_z); 433 | matd_destroy(R_1_prime); 434 | return ret; 435 | } 436 | 437 | /** 438 | * Estimate pose of the tag using the homography method. 439 | */ 440 | void estimate_pose_for_tag_homography(apriltag_detection_info_t* info, apriltag_pose_t* solution) { 441 | double scale = info->tagsize/2.0; 442 | 443 | matd_t *M_H = homography_to_pose(info->det->H, -info->fx, info->fy, info->cx, info->cy); 444 | MATD_EL(M_H, 0, 3) *= scale; 445 | MATD_EL(M_H, 1, 3) *= scale; 446 | MATD_EL(M_H, 2, 3) *= scale; 447 | 448 | matd_t* fix = matd_create(4, 4); 449 | MATD_EL(fix, 0, 0) = 1; 450 | MATD_EL(fix, 1, 1) = -1; 451 | MATD_EL(fix, 2, 2) = -1; 452 | MATD_EL(fix, 3, 3) = 1; 453 | 454 | matd_t* initial_pose = matd_multiply(fix, M_H); 455 | matd_destroy(M_H); 456 | matd_destroy(fix); 457 | 458 | solution->R = matd_create(3, 3); 459 | for (int i = 0; i < 3; i++) { 460 | for (int j = 0; j < 3; j++) { 461 | MATD_EL(solution->R, i, j) = MATD_EL(initial_pose, i, j); 462 | } 463 | } 464 | 465 | solution->t = matd_create(3, 1); 466 | for (int i = 0; i < 3; i++) { 467 | MATD_EL(solution->t, i, 0) = MATD_EL(initial_pose, i, 3); 468 | } 469 | matd_destroy(initial_pose); 470 | } 471 | 472 | /** 473 | * Estimate tag pose using orthogonal iteration. 474 | */ 475 | void estimate_tag_pose_orthogonal_iteration( 476 | apriltag_detection_info_t* info, 477 | double* err1, 478 | apriltag_pose_t* solution1, 479 | double* err2, 480 | apriltag_pose_t* solution2, 481 | int nIters) { 482 | double scale = info->tagsize/2.0; 483 | matd_t* p[4] = { 484 | matd_create_data(3, 1, (double[]) {-scale, scale, 0}), 485 | matd_create_data(3, 1, (double[]) {scale, scale, 0}), 486 | matd_create_data(3, 1, (double[]) {scale, -scale, 0}), 487 | matd_create_data(3, 1, (double[]) {-scale, -scale, 0})}; 488 | matd_t* v[4]; 489 | for (int i = 0; i < 4; i++) { 490 | v[i] = matd_create_data(3, 1, (double[]) { 491 | (info->det->p[i][0] - info->cx)/info->fx, (info->det->p[i][1] - info->cy)/info->fy, 1}); 492 | } 493 | 494 | estimate_pose_for_tag_homography(info, solution1); 495 | *err1 = orthogonal_iteration(v, p, &solution1->t, &solution1->R, 4, nIters); 496 | solution2->R = fix_pose_ambiguities(v, p, solution1->t, solution1->R, 4); 497 | if (solution2->R) { 498 | solution2->t = matd_create(3, 1); 499 | *err2 = orthogonal_iteration(v, p, &solution2->t, &solution2->R, 4, nIters); 500 | } else { 501 | *err2 = HUGE_VAL; 502 | } 503 | 504 | for (int i = 0; i < 4; i++) { 505 | matd_destroy(p[i]); 506 | matd_destroy(v[i]); 507 | } 508 | } 509 | 510 | /** 511 | * Estimate tag pose. 512 | */ 513 | double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) { 514 | double err1, err2; 515 | apriltag_pose_t pose1, pose2; 516 | estimate_tag_pose_orthogonal_iteration(info, &err1, &pose1, &err2, &pose2, 50); 517 | if (err1 <= err2) { 518 | pose->R = pose1.R; 519 | pose->t = pose1.t; 520 | if (pose2.R) { 521 | matd_destroy(pose2.t); 522 | matd_destroy(pose2.R); 523 | } 524 | return err1; 525 | } else { 526 | pose->R = pose2.R; 527 | pose->t = pose2.t; 528 | if (pose1.R) { 529 | matd_destroy(pose1.R); 530 | matd_destroy(pose1.t); 531 | } 532 | return err2; 533 | } 534 | } 535 | -------------------------------------------------------------------------------- /Machine Vision Toolbox for MATLAB/vision/mex/apriltags.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "apriltag.h" 8 | #include "image_u8.h" 9 | #include "tag36h11.h" 10 | 11 | #include "zarray.h" 12 | #include "getopt.h" 13 | 14 | #include "mex.h" 15 | 16 | #include "apriltag_pose.h" 17 | 18 | #ifdef __cplusplus 19 | extern "C" { 20 | #endif 21 | 22 | static const char *fields[] = {"id", "hamming", "margin", "H", "center", "corners", "p", "R"}; 23 | 24 | mxArray *getTag(int width, int height, unsigned char *image, double tagSize, double *calibMatrix) 25 | { 26 | apriltag_family_t *tf = NULL; 27 | tf = tag36h11_create(); 28 | 29 | apriltag_detector_t *td = apriltag_detector_create(); 30 | apriltag_detector_add_family(td, tf); 31 | td->quad_decimate = 2.0; // detection of quads done on a lower-resolution image 32 | td->quad_sigma = 0.0; // Gaussian blur (std in pixels) applied to the segmented image 33 | td->nthreads = 4; // number of threads to be used 34 | td->qtp.max_nmaxima = 20; // number of corner candidates to consider when segmenting a group of pixels into a quad. 35 | td->debug = 0; // // When non-zero, write debugging images to the current directory 36 | const int hamm_hist_max = 10; 37 | image_u8_t *im = NULL; 38 | mxArray *out; 39 | int ntags; 40 | 41 | int hamm_hist[hamm_hist_max]; 42 | memset(hamm_hist, 0, sizeof(hamm_hist)); 43 | 44 | // Create the image structure, copy pixels to row-major order 45 | im = image_u8_create(width, height); 46 | 47 | for (int y=0; y < height; y++) { 48 | unsigned char *p = &im->buf[y*im->stride]; 49 | unsigned char *q = &image[y]; 50 | for (int x=0; x < width; x++) { 51 | *p++ = *q; 52 | q += height; 53 | } 54 | } 55 | 56 | // Perform the detections 57 | zarray_t *detections = apriltag_detector_detect(td, im); 58 | ntags = detections->size; 59 | 60 | if (ntags == 0) { 61 | zarray_destroy(detections); 62 | // mxDestroyArray(out); 63 | image_u8_destroy(im); 64 | apriltag_detector_destroy(td); 65 | tag36h11_destroy(tf); 66 | return NULL; 67 | } 68 | 69 | // 6-DOF pose estimation parameters 70 | apriltag_pose_t pose; 71 | apriltag_detection_info_t info; 72 | info.tagsize = tagSize; 73 | info.fx = *(calibMatrix + 0); 74 | info.fy = *(calibMatrix + 4); 75 | info.cx = *(calibMatrix + 6); 76 | info.cy = *(calibMatrix + 7); 77 | /* 'apriltag_detection_info_t' is a struct required for pose estimation 78 | containing: 79 | 1. detector parameters 80 | 2. camera calibration matrix 81 | 3. tag size in meters */ 82 | 83 | // create return structure 84 | out = mxCreateStructMatrix(1, ntags, 8, fields); 85 | 86 | for (int i = 0; i < ntags; i++) { 87 | apriltag_detection_t *det; 88 | double *p, *q; 89 | 90 | // Get the i-th tag 91 | zarray_get(detections, i, &det); 92 | 93 | // Save results into a passed MATLAB strucutre 94 | mxSetField(out, i, fields[0], mxCreateDoubleScalar( (double) det->id ) ); // id 95 | mxSetField(out, i, fields[1], mxCreateDoubleScalar( (double) det->hamming ) ); // hamming 96 | mxSetField(out, i, fields[2], mxCreateDoubleScalar( (double) det->decision_margin ) ); // margin 97 | /* id: The decoded ID of the tag.*/ 98 | /* hamming: How many error bits were corrected. Accepting large numbers of 99 | corrected errors leads to greatly increased false positive rates. 100 | As of this implementation, the detector cannot detect tags with 101 | a hamming distance greater than 2. */ 102 | /* margin: A measure of the quality of the binary decoding process: the 103 | average difference between the intensity of a data bit versus 104 | the decision threshold. Higher numbers roughly indicate better 105 | decodes. This is a reasonable measure of detection accuracy 106 | only for very small tags-- not effective for larger tags (where 107 | we could have sampled anywhere within a bit cell and still 108 | gotten a good detection. */ 109 | 110 | // Save the tag homography as a 3x3 matrix 111 | mxArray *H = mxCreateDoubleMatrix( 3, 3, mxREAL ); 112 | p = mxGetPr(H); 113 | q = det->H->data; 114 | for (int row=0; row<3; row++) { 115 | for (int col=0; col<3; col++) { 116 | p[row+col*3] = *q++; 117 | } 118 | } 119 | mxSetField(out, i, fields[3], H); 120 | /* H: The 3x3 homography matrix describing the projection from an 121 | "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,-1)) 122 | to pixels in the image. */ 123 | 124 | // Save the center 125 | mxArray *center = mxCreateDoubleMatrix( 2, 1, mxREAL ); 126 | p = mxGetPr(center); 127 | p[0] = det->c[0]; 128 | p[1] = det->c[1]; 129 | mxSetField(out, i, fields[4], center); // center 130 | /* center: The center of the detection in image pixel coordinates */ 131 | 132 | // Save the corners as a 2x4 matrix 133 | mxArray *corners = mxCreateDoubleMatrix( 2, 4, mxREAL ); 134 | p = mxGetPr(corners); 135 | for (int row=0; row<4; row++) 136 | for (int col=0; col<2; col++) 137 | p[col+row*2] = det->p[row][col]; // it's transposed 138 | mxSetField(out, i, fields[5], corners ); // corners 139 | /* corners: The corners of the tag in image pixel coordinates. 140 | These always wrap counter-clock wise around the tag. */ 141 | 142 | // 6-DOF pose estimation 143 | info.det = det; 144 | double err = estimate_tag_pose(&info, &pose); 145 | 146 | // Assign pose parameters to MATLAB structs for export 147 | mxArray *worldp = mxCreateDoubleMatrix( 3, 1, mxREAL ); 148 | p = mxGetPr(worldp); 149 | int col=0; 150 | for (int row=0; row<3; row++) 151 | p[row+col*1] = *(pose.t->data + row+col*1); 152 | mxSetField(out, i, fields[6], worldp ); // worldp 153 | /* worldp: The position of the tag center w.r.t. the origin of camera coordinates */ 154 | 155 | mxArray *R = mxCreateDoubleMatrix( 3, 3, mxREAL ); 156 | p = mxGetPr(R); 157 | for (int row=0; row<3; row++) 158 | for (int col=0; col<3; col++) 159 | p[row+col*3] = *(pose.R->data + col+row*3); 160 | mxSetField(out, i, fields[7], R ); // R 161 | /* R: The rotation of the tag center w.r.t. the origin of camera coordinates */ 162 | 163 | hamm_hist[det->hamming]++; 164 | 165 | // Deallocate memory 166 | apriltag_detection_destroy(det); 167 | } 168 | 169 | // Deallocate memory 170 | // Don't deallocate contents of inputs; those are the argv! 171 | matd_destroy(pose.t); 172 | matd_destroy(pose.R); 173 | zarray_destroy(detections); 174 | image_u8_destroy(im); 175 | apriltag_detector_destroy(td); 176 | tag36h11_destroy(tf); 177 | 178 | return out; 179 | } 180 | 181 | 182 | #define IM_IN prhs[0] // grayscale image matrix 183 | #define TAG_SIZE prhs[1] // tag size in meters 184 | #define CALIB_M prhs[2] // camera calibration matrix 185 | 186 | void mexFunction(int nlhs, mxArray *plhs[], 187 | int nrhs, const mxArray *prhs[]) 188 | { 189 | // Check inputs: 190 | if (nrhs < 3) { 191 | mexErrMsgTxt("Not enough input arguments."); 192 | } 193 | if (nrhs > 3) { 194 | mexErrMsgTxt("Too many input arguments."); 195 | } 196 | 197 | // Check outputs: 198 | if (nlhs > 1) { 199 | mexErrMsgTxt("Invalid number of outputs to MEX file."); 200 | } 201 | 202 | // Check image matrix input: 203 | if (mxGetN(IM_IN) < 3 || mxGetM(IM_IN) < 3) { 204 | mexErrMsgTxt("Invalid image."); 205 | } 206 | if (mxGetNumberOfDimensions(IM_IN) > 2) { 207 | mexErrMsgTxt("Color images are not supported"); 208 | } 209 | if (mxGetClassID(IM_IN) != mxUINT8_CLASS && mxGetClassID(IM_IN) != mxDOUBLE_CLASS) { 210 | mexErrMsgTxt("Only 'uint8' or 'double' images allowed."); 211 | } 212 | 213 | // Check tag size input: 214 | if(mxGetN(TAG_SIZE) != 1 || mxGetScalar(TAG_SIZE) <= 0) { 215 | mexErrMsgTxt("Invalid tag size value."); 216 | } 217 | 218 | // Check camera calibration matrix input: 219 | if (mxGetN(CALIB_M) != 3 && mxGetM(CALIB_M) != 3) { 220 | mexErrMsgTxt("Invalid camera calibration matrix."); 221 | } 222 | 223 | // Get number of pixels in the image 224 | int width = mxGetN(IM_IN); 225 | int height = mxGetM(IM_IN); 226 | 227 | unsigned char *im; // image matrix 228 | double ts; // tag size in meters 229 | double *cm; // camera calibration matrix 230 | 231 | switch (mxGetClassID(IM_IN)) { 232 | case mxUINT8_CLASS: { 233 | im = (unsigned char *)mxGetPr(IM_IN); // get pointer to image 234 | break; 235 | } 236 | case mxDOUBLE_CLASS: { 237 | unsigned char *p = im = (unsigned char *)malloc(width*height); 238 | double *q = mxGetPr(IM_IN); 239 | 240 | // Type convert 241 | for (int i=0; i. 17 | 18 | fprintf('** building MEX files for MVTB\n'); 19 | pth = which('imorph.m'); 20 | pth = fileparts(pth); 21 | cd( fullfile(pth, 'mex') ); 22 | 23 | mexfiles = { 24 | 'closest.c', 25 | 'fhist.c', 26 | 'hist2d.c', 27 | 'ilabel.c', 28 | 'imatch.c', 29 | 'imorph.c', 30 | 'irank.c', 31 | 'ivar.c', 32 | 'iwindow.c', 33 | 'stereo_match.c' 34 | }; 35 | 36 | for file = mexfiles' 37 | fprintf('\n* Compiling: %s\n', file{1}); 38 | mex('CFLAGS=-std=c99 -fPIC', file{1}) 39 | end 40 | 41 | % MATLAB script to build apriltags.mex 42 | % 43 | % you must have first installed the standalone C version of AprilTags 44 | % obtained from https://april.eecs.umich.edu/wiki/index.php/AprilTags. 45 | % and named it apriltags 46 | % 47 | % then built the library, run make in the apriltags folder. 48 | 49 | fprintf('\n* Compiling: apriltags.c\n'); 50 | 51 | % test for apriltag folder 52 | if ~exist('apriltag', 'file') 53 | error('no apriltag folder found in current directory, download it from https://april.eecs.umich.edu/wiki/index.php/AprilTags'); 54 | end 55 | 56 | cd apriltag; 57 | p = which('apriltag.h'); 58 | p = fileparts(p); 59 | cd ..; 60 | 61 | % test for a build library 62 | if ~exist(fullfile(p, 'libapriltag.a'), 'file') 63 | error('you need to first build the apriltag library: libapriltag.a'); 64 | end 65 | 66 | system(sprintf('mex apriltags.c -I"%s" -I"%s"/common -L"%s" -lapriltag', p, p, p)); 67 | 68 | -------------------------------------------------------------------------------- /PROCEDURE.md: -------------------------------------------------------------------------------- 1 | # How to compile MATLAB_AprilTag3 - The MEX wrapper for Linux 2 | 3 | ## STEP 1: Compile AprilTag library 4 | 5 | 1. Install the Machine Vision Toolbox (MVTB) by opening MATLAB_AprilTag3 GitHub's file `MVTB-4.3.mltbx` from MATLAB GUI, or download the latest version at:
6 | https://petercorke.com/download/28/mvtb/1109/mvtb-4-3.mltbx 7 | 1. Create auxiliary environmental variables from command line: 8 | ```shell 9 | MLTBX_DIR="$HOME/Documents/MATLAB/Add-Ons/Toolboxes" 10 | MVTB="$MLTBX_DIR/Machine Vision Toolbox for MATLAB" 11 | MVTB_MEX="$MVTB/vision/mex" 12 | MVTB_APRILTAG="$MVTB_MEX/apriltag" 13 | ``` 14 | 1. Download MATLAB_AprilTag3 GitHub's file `AprilTag3.tar.gz` (the AprilTag3 source code) 15 | and uncompress it, or download the latest version at:
16 | https://codeload.github.com/AprilRobotics/apriltags/zip/master 17 | 1. Rename the uncompressed directory to `apriltag` to match the wrapper path. 18 | 1. Copy latter `apriltag` directory to `MVTB_MEX` by running from command line:
19 | ```shell 20 | cp -pr apriltag "$MVTB_MEX" 21 | ``` 22 | 1. Download MATLAB_AprilTag3 GitHub's directory `apriltag` at `Machine Vision Toolbox for MATLAB/vision/mex/apriltag/` 23 | and copy it into `MVTB_MEX` directory: 24 | ```shell 25 | cp -pr apriltag "$MVTB_MEX" 26 | ``` 27 | 1. Compile the AprilTag library: 28 | ```shell 29 | cd "$MVTB_APRILTAG" 30 | make 31 | sudo make install 32 | ``` 33 | 34 | ## STEP 2: Compile MATLAB_AprilTag3 35 | 36 | 1. Download MATLAB_AprilTag3 GitHub's directory `Machine Vision Toolbox for MATLAB` 37 | and copy it into `MLTBX_DIR` directory: 38 | ```shell 39 | cp -pr "Machine Vision Toolbox for MATLAB" "$MLTBX_DIR" 40 | ``` 41 | 1. Compile the MEX file: 42 | ```shell 43 | cd "$MVTB_MEX" 44 | mex apriltags.c -I"$MVTB_APRILTAG" -I"$MVTB_APRILTAG"/common -L"$MVTB_APRILTAG" -lapriltag 45 | ``` 46 | 1. Compile the remaining MVTB libraries from MATLAB's command line: 47 | ```matlab 48 | make 49 | ``` 50 | ## STEP 3: Render your tags with appropriate scale (e.g. 3 to 5 inches) 51 | 52 | **N.B.:** MATLAB_AprilTag3 detector only works with "**tag36h11**" tag family! 53 | 54 | 1. Download and uncompress MATLAB_AprilTag3 GitHub's file `tag36h11.tar.gz` 55 | ```shell 56 | gunzip -dc tag36h11.tar.gz | tar xvf - 57 | ``` 58 | 1. Open a PNG tag file using an appropriate image editor, e.g., Inkscape. 59 | 60 | 1. Choose the tag dimensions before printing. 61 | 62 | 1. Enjoy! =}) 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MATLAB_AprilTag3 2 | The AprilTag3 MEX wrapper for MATLAB on Linux implements tag detection and 3–D pose estimation.
3 | **N.B.:** Only the tag family ''**tag36h11**'' is supported!
4 | 5 | **Please cite this software if you use MATLAB_AprilTag3 in your research:**
6 | [1] A. A. Díaz Salazar, "MATLAB_AprilTag3", Linköping, Sweden, 2019. [Online](https://github.com/alddiaz/MATLAB_AprilTag3). 7 | 8 | # Procedure 9 | Read `PROCEDURE.md` for install instructions.
10 | > The MEX function "**apriltags.c**" implements AprilTag3 detection and 3–D pose estimation (April 2019 version) and contains a **MAJOR FIX** to prevent deallocation of uninitialized memory.
11 | AprilRobotics guide: https://github.com/AprilRobotics/apriltag/wiki/AprilTag-User-Guide 12 | 13 | # Demo 14 | [AprilTag3 detection on MATLAB](https://youtu.be/ptx3UyyvmTA) 15 | 16 | # Example 17 | Run `help apriltags` for syntax details. 18 | 19 | ```matlab 20 | im_rgb = imread("my_RGB_image.png"); 21 | IM = rgb2gray(im_rgb); 22 | tags = apriltags(IM, TAGSIZE, K); 23 | ``` 24 | 25 | > Inputs:
26 | **IM** is a grayscale image.
27 | **TAGSIZE** specifies the actual (printed!) tag size in meters.
28 | **K** specifies camera calibration as a 3x3 matrix defined by **K** = [**fx** 0 **u0**; 0 **fy** **v0**; 0 0 1] where **(fx, fy)** is the focal length and **(u0, v0)** is the principal point (optical center).
29 | Outputs:
30 | **tags** is a vector of structures with the parameters of the AprilTags found in **IM**.
31 | 32 | # Acknowledgments 33 | Basic MEX infrastructure by Peter Corke given in the [Machine Vision Toolbox for MATLAB](http://petercorke.com/toolboxes/machine-vision-toolbox).
34 | MEX fix provided by [Gustaf Hendeby](http://users.isy.liu.se/en/rt/hendeby). 35 | 36 | # References 37 | [1] A. A. Díaz Salazar, "MATLAB_AprilTag3", Linköping, Sweden, 2019. [Online](https://github.com/alddiaz/MATLAB_AprilTag3).
38 | [2] M. Krogius, A. Haggenmiller, and E. Olson, “Flexible Layouts for Fiducial Tags”, In: 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019, pp. 1898–1903. [DOI: 10.1109/IROS40897.2019.8967787](https://ieeexplore.ieee.org/document/8967787).
39 | [3] P. Corke, The Machine Vision Toolbox: a MATLAB toolbox for vision and vision-based control, In: IEEE Robotics and Automation Magazine, vol. 12(4), 2005, pp. 16–25. [DOI: 10.1109/MRA.2005.1577021](https://ieeexplore.ieee.org/document/1577021).
40 | [4] P. Corke, MATLAB toolboxes: robotics and vision for students and teachers, In: IEEE Robotics and Automation Magazine, vol. 14(4), 2007, pp. 16–17. [DOI: 10.1109/M-RA.2007.912004](https://ieeexplore.ieee.org/document/4437745). 41 | -------------------------------------------------------------------------------- /tag36h11.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/alddiaz/MATLAB_AprilTag3/852a575f0a411ff400c7d39c27dd431309ef8be0/tag36h11.tar.gz --------------------------------------------------------------------------------