diff --git a/apriltag_pose.h b/apriltag_pose.h index 07ee37b2..8120502f 100644 --- a/apriltag_pose.h +++ b/apriltag_pose.h @@ -44,9 +44,9 @@ void estimate_pose_for_tag_homography( * used to find a potential second local minima and Orthogonal Iteration is * used to refine this second estimate. * - * [1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in + * [1]: E. Olson, "Apriltag: A robust and flexible visual fiducial system," in * 2011 IEEE International Conference on Robotics and Automation, - * May 2011, pp. 3400–3407. + * May 2011, pp. 3400-3407. * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose * estimation from video images," in IEEE Transactions on Pattern Analysis * and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000. @@ -77,4 +77,3 @@ double estimate_tag_pose(apriltag_detection_info_t* info, apriltag_pose_t* pose) #ifdef __cplusplus } #endif - diff --git a/common/matd.c b/common/matd.c index 6b8a0405..c9a4b98c 100644 --- a/common/matd.c +++ b/common/matd.c @@ -402,7 +402,7 @@ double matd_det_general(const matd_t *a) // The determinant of a can be calculated as // epsilon*det(L)*det(U), // where epsilon is just the sign of the corresponding permutation - // (which is +1 for an even number of permutations and is −1 + // (which is +1 for an even number of permutations and is -1 // for an uneven number of permutations). double det = mlu->pivsign * detL * detU; diff --git a/common/pthreads_cross.c b/common/pthreads_cross.c index 3403863f..04e556cf 100644 --- a/common/pthreads_cross.c +++ b/common/pthreads_cross.c @@ -1,256 +1,256 @@ -/** -Copyright John Schember - -Permission is hereby granted, free of charge, to any person obtaining a copy of -this software and associated documentation files (the "Software"), to deal in -the Software without restriction, including without limitation the rights to -use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies -of the Software, and to permit persons to whom the Software is furnished to do -so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. - */ - -#include "common/pthreads_cross.h" - -#ifdef _WIN32 - -typedef struct { - SRWLOCK lock; - bool exclusive; -} pthread_rwlock_t; - -int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr); -int pthread_rwlock_destroy(pthread_rwlock_t *rwlock); -int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock); -int pthread_rwlock_unlock(pthread_rwlock_t *rwlock); - -int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) -{ - (void) attr; - - if (thread == NULL || start_routine == NULL) - return 1; - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wcast-function-type" - *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL); -#pragma GCC diagnostic pop - if (*thread == NULL) - return 1; - return 0; -} - -int pthread_join(pthread_t thread, void **value_ptr) -{ - (void)value_ptr; - WaitForSingleObject(thread, INFINITE); - CloseHandle(thread); - return 0; -} - -int pthread_detach(pthread_t thread) -{ - CloseHandle(thread); - return 0; -} - -int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr) -{ - (void)attr; - - if (mutex == NULL) - return 1; - - InitializeCriticalSection(mutex); - return 0; -} - -int pthread_mutex_destroy(pthread_mutex_t *mutex) -{ - if (mutex == NULL) - return 1; - DeleteCriticalSection(mutex); - return 0; -} - -int pthread_mutex_lock(pthread_mutex_t *mutex) -{ - if (mutex == NULL) - return 1; - EnterCriticalSection(mutex); - return 0; -} - -int pthread_mutex_unlock(pthread_mutex_t *mutex) -{ - if (mutex == NULL) - return 1; - LeaveCriticalSection(mutex); - return 0; -} - -int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) -{ - (void)attr; - if (cond == NULL) - return 1; - InitializeConditionVariable(cond); - return 0; -} - -int pthread_cond_destroy(pthread_cond_t *cond) -{ - /* Windows does not have a destroy for conditionals */ - (void)cond; - return 0; -} - -int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) -{ - if (cond == NULL || mutex == NULL) - return 1; - return pthread_cond_timedwait(cond, mutex, NULL); -} - -int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, - const struct timespec *abstime) -{ - if (cond == NULL || mutex == NULL) - return 1; - if (!SleepConditionVariableCS(cond, mutex, timespec_to_ms(abstime))) - return 1; - return 0; -} - -int pthread_cond_signal(pthread_cond_t *cond) -{ - if (cond == NULL) - return 1; - WakeConditionVariable(cond); - return 0; -} - -int pthread_cond_broadcast(pthread_cond_t *cond) -{ - if (cond == NULL) - return 1; - WakeAllConditionVariable(cond); - return 0; -} - -int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr) -{ - (void)attr; - if (rwlock == NULL) - return 1; - InitializeSRWLock(&(rwlock->lock)); - rwlock->exclusive = false; - return 0; -} - -int pthread_rwlock_destroy(pthread_rwlock_t *rwlock) -{ - (void)rwlock; - return 0; -} - -int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - AcquireSRWLockShared(&(rwlock->lock)); - return 0; -} - -int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - return !TryAcquireSRWLockShared(&(rwlock->lock)); -} - -int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - AcquireSRWLockExclusive(&(rwlock->lock)); - rwlock->exclusive = true; - return 0; -} - -int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock) -{ - BOOLEAN ret; - - if (rwlock == NULL) - return 1; - - ret = TryAcquireSRWLockExclusive(&(rwlock->lock)); - if (ret) - rwlock->exclusive = true; - return ret; -} - -int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) -{ - if (rwlock == NULL) - return 1; - - if (rwlock->exclusive) { - rwlock->exclusive = false; - ReleaseSRWLockExclusive(&(rwlock->lock)); - } else { - ReleaseSRWLockShared(&(rwlock->lock)); - } - return 0; -} - -int sched_yield() { - return (int)SwitchToThread(); -} - -void ms_to_timespec(struct timespec *ts, unsigned int ms) -{ - if (ts == NULL) - return; - ts->tv_sec = (ms / 1000) + time(NULL); - ts->tv_nsec = (ms % 1000) * 1000000; -} - -unsigned int timespec_to_ms(const struct timespec *abstime) -{ - if (abstime == NULL) - return INFINITE; - - return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000); -} - -unsigned int pcthread_get_num_procs() -{ - SYSTEM_INFO sysinfo; - - GetSystemInfo(&sysinfo); - return sysinfo.dwNumberOfProcessors; -} - -#else - -#include -unsigned int pcthread_get_num_procs() -{ - return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); -} -#endif +/** +Copyright John Schember + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + */ + +#include "common/pthreads_cross.h" + +#ifdef _WIN32 + +typedef struct { + SRWLOCK lock; + bool exclusive; +} pthread_rwlock_t; + +int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr); +int pthread_rwlock_destroy(pthread_rwlock_t *rwlock); +int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock); +int pthread_rwlock_unlock(pthread_rwlock_t *rwlock); + +int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg) +{ + (void) attr; + + if (thread == NULL || start_routine == NULL) + return 1; + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-function-type" + *thread = (HANDLE) CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)start_routine, arg, 0, NULL); +#pragma GCC diagnostic pop + if (*thread == NULL) + return 1; + return 0; +} + +int pthread_join(pthread_t thread, void **value_ptr) +{ + (void)value_ptr; + WaitForSingleObject(thread, INFINITE); + CloseHandle(thread); + return 0; +} + +int pthread_detach(pthread_t thread) +{ + CloseHandle(thread); + return 0; +} + +int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr) +{ + (void)attr; + + if (mutex == NULL) + return 1; + + InitializeCriticalSection(mutex); + return 0; +} + +int pthread_mutex_destroy(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + DeleteCriticalSection(mutex); + return 0; +} + +int pthread_mutex_lock(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + EnterCriticalSection(mutex); + return 0; +} + +int pthread_mutex_unlock(pthread_mutex_t *mutex) +{ + if (mutex == NULL) + return 1; + LeaveCriticalSection(mutex); + return 0; +} + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr) +{ + (void)attr; + if (cond == NULL) + return 1; + InitializeConditionVariable(cond); + return 0; +} + +int pthread_cond_destroy(pthread_cond_t *cond) +{ + /* Windows does not have a destroy for conditionals */ + (void)cond; + return 0; +} + +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex) +{ + if (cond == NULL || mutex == NULL) + return 1; + return pthread_cond_timedwait(cond, mutex, NULL); +} + +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, + const struct timespec *abstime) +{ + if (cond == NULL || mutex == NULL) + return 1; + if (!SleepConditionVariableCS(cond, mutex, timespec_to_ms(abstime))) + return 1; + return 0; +} + +int pthread_cond_signal(pthread_cond_t *cond) +{ + if (cond == NULL) + return 1; + WakeConditionVariable(cond); + return 0; +} + +int pthread_cond_broadcast(pthread_cond_t *cond) +{ + if (cond == NULL) + return 1; + WakeAllConditionVariable(cond); + return 0; +} + +int pthread_rwlock_init(pthread_rwlock_t *rwlock, const pthread_rwlockattr_t *attr) +{ + (void)attr; + if (rwlock == NULL) + return 1; + InitializeSRWLock(&(rwlock->lock)); + rwlock->exclusive = false; + return 0; +} + +int pthread_rwlock_destroy(pthread_rwlock_t *rwlock) +{ + (void)rwlock; + return 0; +} + +int pthread_rwlock_rdlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + AcquireSRWLockShared(&(rwlock->lock)); + return 0; +} + +int pthread_rwlock_tryrdlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + return !TryAcquireSRWLockShared(&(rwlock->lock)); +} + +int pthread_rwlock_wrlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + AcquireSRWLockExclusive(&(rwlock->lock)); + rwlock->exclusive = true; + return 0; +} + +int pthread_rwlock_trywrlock(pthread_rwlock_t *rwlock) +{ + BOOLEAN ret; + + if (rwlock == NULL) + return 1; + + ret = TryAcquireSRWLockExclusive(&(rwlock->lock)); + if (ret) + rwlock->exclusive = true; + return ret; +} + +int pthread_rwlock_unlock(pthread_rwlock_t *rwlock) +{ + if (rwlock == NULL) + return 1; + + if (rwlock->exclusive) { + rwlock->exclusive = false; + ReleaseSRWLockExclusive(&(rwlock->lock)); + } else { + ReleaseSRWLockShared(&(rwlock->lock)); + } + return 0; +} + +int sched_yield() { + return (int)SwitchToThread(); +} + +void ms_to_timespec(struct timespec *ts, unsigned int ms) +{ + if (ts == NULL) + return; + ts->tv_sec = (ms / 1000) + time(NULL); + ts->tv_nsec = (ms % 1000) * 1000000; +} + +unsigned int timespec_to_ms(const struct timespec *abstime) +{ + if (abstime == NULL) + return INFINITE; + + return ((abstime->tv_sec - time(NULL)) * 1000) + (abstime->tv_nsec / 1000000); +} + +unsigned int pcthread_get_num_procs() +{ + SYSTEM_INFO sysinfo; + + GetSystemInfo(&sysinfo); + return sysinfo.dwNumberOfProcessors; +} + +#else + +#include +unsigned int pcthread_get_num_procs() +{ + return (unsigned int)sysconf(_SC_NPROCESSORS_ONLN); +} +#endif diff --git a/common/pthreads_cross.h b/common/pthreads_cross.h index 5970c679..fc40c9ba 100644 --- a/common/pthreads_cross.h +++ b/common/pthreads_cross.h @@ -1,82 +1,82 @@ -/** -Copyright John Schember - -Permission is hereby granted, free of charge, to any person obtaining a copy of -this software and associated documentation files (the "Software"), to deal in -the Software without restriction, including without limitation the rights to -use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies -of the Software, and to permit persons to whom the Software is furnished to do -so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#ifndef __CPTHREAD_H__ -#define __CPTHREAD_H__ - -#ifdef _WIN32 -#include -#include -#else -#include -#include -#endif -#include - -#ifdef _WIN32 - -typedef CRITICAL_SECTION pthread_mutex_t; -typedef void pthread_mutexattr_t; -typedef void pthread_attr_t; -typedef void pthread_condattr_t; -typedef void pthread_rwlockattr_t; -typedef HANDLE pthread_t; -typedef CONDITION_VARIABLE pthread_cond_t; - -#ifdef __cplusplus -extern "C" { -#endif -int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg); -int pthread_join(pthread_t thread, void **value_ptr); -int pthread_detach(pthread_t); - -int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr); -int pthread_mutex_destroy(pthread_mutex_t *mutex); -int pthread_mutex_lock(pthread_mutex_t *mutex); -int pthread_mutex_unlock(pthread_mutex_t *mutex); - -int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr); -int pthread_cond_destroy(pthread_cond_t *cond); -int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex); -int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime); -int pthread_cond_signal(pthread_cond_t *cond); -int pthread_cond_broadcast(pthread_cond_t *cond); - -int sched_yield(void); -#ifdef __cplusplus -} -#endif -#endif - - -#ifdef __cplusplus -extern "C" { -#endif -unsigned int pcthread_get_num_procs(); - -void ms_to_timespec(struct timespec *ts, unsigned int ms); -unsigned int timespec_to_ms(const struct timespec *abstime); -#ifdef __cplusplus -} -#endif - -#endif /* __CPTHREAD_H__ */ +/** +Copyright John Schember + +Permission is hereby granted, free of charge, to any person obtaining a copy of +this software and associated documentation files (the "Software"), to deal in +the Software without restriction, including without limitation the rights to +use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies +of the Software, and to permit persons to whom the Software is furnished to do +so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#ifndef __CPTHREAD_H__ +#define __CPTHREAD_H__ + +#ifdef _WIN32 +#include +#include +#else +#include +#include +#endif +#include + +#ifdef _WIN32 + +typedef CRITICAL_SECTION pthread_mutex_t; +typedef void pthread_mutexattr_t; +typedef void pthread_attr_t; +typedef void pthread_condattr_t; +typedef void pthread_rwlockattr_t; +typedef HANDLE pthread_t; +typedef CONDITION_VARIABLE pthread_cond_t; + +#ifdef __cplusplus +extern "C" { +#endif +int pthread_create(pthread_t *thread, pthread_attr_t *attr, void *(*start_routine)(void *), void *arg); +int pthread_join(pthread_t thread, void **value_ptr); +int pthread_detach(pthread_t); + +int pthread_mutex_init(pthread_mutex_t *mutex, pthread_mutexattr_t *attr); +int pthread_mutex_destroy(pthread_mutex_t *mutex); +int pthread_mutex_lock(pthread_mutex_t *mutex); +int pthread_mutex_unlock(pthread_mutex_t *mutex); + +int pthread_cond_init(pthread_cond_t *cond, pthread_condattr_t *attr); +int pthread_cond_destroy(pthread_cond_t *cond); +int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex); +int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime); +int pthread_cond_signal(pthread_cond_t *cond); +int pthread_cond_broadcast(pthread_cond_t *cond); + +int sched_yield(void); +#ifdef __cplusplus +} +#endif +#endif + + +#ifdef __cplusplus +extern "C" { +#endif +unsigned int pcthread_get_num_procs(); + +void ms_to_timespec(struct timespec *ts, unsigned int ms); +unsigned int timespec_to_ms(const struct timespec *abstime); +#ifdef __cplusplus +} +#endif + +#endif /* __CPTHREAD_H__ */