diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..b39ac68 --- /dev/null +++ b/.clang-format @@ -0,0 +1,65 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: { + AfterClass: 'true' + AfterControlStatement: 'true' + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'true' + BeforeElse : 'true' + IndentBraces : 'false' +} +... diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..12e6329 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,10 @@ +- repo: local + hooks: + - id: clang-format + name: Run clang-format + description: Clang C/C++ code formatter. + entry: clang-format -i + args: [-style=file] + language: system + types: [file, c++] + # note: formatting style in .clang-format diff --git a/ar_track_alvar/include/ar_track_alvar/Alvar.h b/ar_track_alvar/include/ar_track_alvar/Alvar.h index 3418557..0e5da2c 100644 --- a/ar_track_alvar/include/ar_track_alvar/Alvar.h +++ b/ar_track_alvar/include/ar_track_alvar/Alvar.h @@ -29,44 +29,52 @@ * * \section Introduction * - * ALVAR is a software library for creating virtual and augmented reality (AR) applications. ALVAR has - * been developed by the VTT Technical Research Centre of Finland. ALVAR is released under the terms of - * the GNU Lesser General Public License, version 2.1, or (at your option) any later version. - * - * ALVAR is designed to be as flexible as possible. It offers high-level tools and methods for creating - * augmented reality applications with just a few lines of code. The library also includes interfaces - * for all of the low-level tools and methods, which makes it possible for the user to develop their - * own solutions using alternative approaches or completely new algorithms. - * - * ALVAR is currently provided on Windows and Linux operating systems and only depends on one third - * party library (OpenCV). ALVAR is independent of any graphical libraries and can be easily integrated - * into existing applications. The sample applications use GLUT and the demo applications use OpenSceneGraph. + * ALVAR is a software library for creating virtual and augmented reality (AR) + * applications. ALVAR has been developed by the VTT Technical Research Centre + * of Finland. ALVAR is released under the terms of the GNU Lesser General + * Public License, version 2.1, or (at your option) any later version. + * + * ALVAR is designed to be as flexible as possible. It offers high-level tools + * and methods for creating augmented reality applications with just a few lines + * of code. The library also includes interfaces for all of the low-level tools + * and methods, which makes it possible for the user to develop their own + * solutions using alternative approaches or completely new algorithms. + * + * ALVAR is currently provided on Windows and Linux operating systems and only + * depends on one third party library (OpenCV). ALVAR is independent of any + * graphical libraries and can be easily integrated into existing applications. + * The sample applications use GLUT and the demo applications use + * OpenSceneGraph. * * \section Features * - * - Detecting and tracking 2D markers (\e MarkerDetector). Currently two types of square matrix markers - * are supported (\e MarkerData and \e MarkerArtoolkit). Future marker types can easily be added. ALVAR - * keeps the \e Marker \e Pose estimation as accurate as possible. Furthermore, ALVAR uses some tracking - * heuristics to identify markers that are "too far" and to recover from occlusions in the multimarker - * case for example. - * - Using a setup of multiple markers for pose detection (\e MultiMarker). The marker setup coordinates - * can be set manually or they can be automatically deduced using various methods (\e MultiMarkerFiltered - * and \e MultiMarkerBundle). - * - Tools for calibrating \e Camera. Distorting and undistorting points, projecting points and finding - * exterior orientation using point-sets. + * - Detecting and tracking 2D markers (\e MarkerDetector). Currently two types + * of square matrix markers are supported (\e MarkerData and \e + * MarkerArtoolkit). Future marker types can easily be added. ALVAR keeps the \e + * Marker \e Pose estimation as accurate as possible. Furthermore, ALVAR uses + * some tracking heuristics to identify markers that are "too far" and to + * recover from occlusions in the multimarker case for example. + * - Using a setup of multiple markers for pose detection (\e MultiMarker). The + * marker setup coordinates can be set manually or they can be automatically + * deduced using various methods (\e MultiMarkerFiltered and \e + * MultiMarkerBundle). + * - Tools for calibrating \e Camera. Distorting and undistorting points, + * projecting points and finding exterior orientation using point-sets. * - Hiding markers from the view (\e BuildHideTexture and \e DrawTexture). - * - Several basic filters: \e FilterAverage, \e FilterMedian, \e FilterRunningAverage, - * \e FilterDoubleExponentialSmoothing. - * - \e Kalman filters for sensor fusion: \e Kalman Filter, \e Extended Kalman Filter and Unscented Kalman - * Filter (\e KalmanSensor, \e KalmanSensorEkf, \e KalmanEkf, \e UnscentedKalman). - * - Several methods for tracking using optical flow: \e TrackerPsa , \e TrackerPsaRot , \e TrackerFeatures - * and \e TrackerStat. + * - Several basic filters: \e FilterAverage, \e FilterMedian, \e + * FilterRunningAverage, \e FilterDoubleExponentialSmoothing. + * - \e Kalman filters for sensor fusion: \e Kalman Filter, \e Extended Kalman + * Filter and Unscented Kalman Filter (\e KalmanSensor, \e KalmanSensorEkf, \e + * KalmanEkf, \e UnscentedKalman). + * - Several methods for tracking using optical flow: \e TrackerPsa , \e + * TrackerPsaRot , \e TrackerFeatures and \e TrackerStat. * - etc... * * \section Platforms * * ALVAR is officially supported and tested on the following platforms. - * - Windows XP 32-bit, Microsoft Visual Studio 2005 (8.0), 2008 (9.0) and 2010 (10.0) + * - Windows XP 32-bit, Microsoft Visual Studio 2005 (8.0), 2008 (9.0) and 2010 + * (10.0) * - Linux 32-bit, GCC 4.3 and 4.4 * - Linux 64-bit, GCC 4.3 and 4.4 * @@ -96,61 +104,68 @@ * - OpenSceneGraph (http://www.openscenegraph.org) * * \example SampleCamCalib.cpp - * This is an example of how to use \e ProjPoints and \e Camera classes to perform camera calibration - * using a chessboard pattern. + * This is an example of how to use \e ProjPoints and \e Camera classes to + * perform camera calibration using a chessboard pattern. * * \example SampleCvTestbed.cpp - * This is an example of how to use the \e CvTestbed and \e Capture classes in order to make quick OpenCV - * prototype applications. + * This is an example of how to use the \e CvTestbed and \e Capture classes in + * order to make quick OpenCV prototype applications. * * \example SampleFilter.cpp - * This is an example of how to use various filters: \e FilterAverage, \e FilterMedian, - * \e FilterRunningAverage, \e FilterDoubleExponentialSmoothing and \e Kalman. + * This is an example of how to use various filters: \e FilterAverage, \e + * FilterMedian, \e FilterRunningAverage, \e FilterDoubleExponentialSmoothing + * and \e Kalman. * * \example SampleIntegralImage.cpp - * This is an example of how to use the \e IntegralImage and \e IntegralGradient classes for image - * gradient analysis. + * This is an example of how to use the \e IntegralImage and \e IntegralGradient + * classes for image gradient analysis. * * \example SampleLabeling.cpp - * This is an example of how to label images using \e LabelImage and \e MarchEdge. + * This is an example of how to label images using \e LabelImage and \e + * MarchEdge. * * \example SampleMarkerCreator.cpp - * This is an example that demonstrates the generation of \e MarkerData markers and saving the image - * using \e SaveMarkerImage. + * This is an example that demonstrates the generation of \e MarkerData markers + * and saving the image using \e SaveMarkerImage. * * \example SampleMarkerDetector.cpp - * This is an example that shows how to detect \e MarkerData markers and visualize them using\e GlutViewer. + * This is an example that shows how to detect \e MarkerData markers and + * visualize them using\e GlutViewer. * * \example SampleMarkerHide.cpp - * This is an example that shows how to detect \e MarkerData markers, visualize them using \e GlutViewer - * and hide them with \e BuildHideTexture and \e DrawTexture. + * This is an example that shows how to detect \e MarkerData markers, visualize + * them using \e GlutViewer and hide them with \e BuildHideTexture and \e + * DrawTexture. * * \example SampleMarkerlessCreator.cpp - * This is an example that demonstrates the use of FernImageDetector to train a Fern classifier. + * This is an example that demonstrates the use of FernImageDetector to train a + * Fern classifier. * * \example SampleMarkerlessDetector.cpp - * This is an example that demonstrates the use of FernImageDetector to detect an image as a marker. + * This is an example that demonstrates the use of FernImageDetector to detect + * an image as a marker. * * \example SampleMultiMarker.cpp - * This is an example that demonstrates the use of a preconfigured \e MultiMarker setup. + * This is an example that demonstrates the use of a preconfigured \e + * MultiMarker setup. * * \example SampleMultiMarkerBundle.cpp - * This is an example that automatically recognising \e MultiMarker setups using \e MultiMarkerFiltered and - * optimizes it with \e MultiMarkerBundle. + * This is an example that automatically recognising \e MultiMarker setups using + * \e MultiMarkerFiltered and optimizes it with \e MultiMarkerBundle. * * \example SampleOptimization.cpp - * This is an example of how to use the \e Optimization class by fitting curves of increasing degree to - * random data. + * This is an example of how to use the \e Optimization class by fitting curves + * of increasing degree to random data. * * \example SamplePointcloud.cpp - * This is an example showing how to use \e SimpleSfM for tracking the environment using features in - * addition to \e MultiMarker. + * This is an example showing how to use \e SimpleSfM for tracking the + * environment using features in addition to \e MultiMarker. * * \example SampleTrack.cpp - * This is an example that shows how to perform tracking of the optical flow using \e TrackerPsa, - * \e TrackerPsaRot, \e TrackerFeatures or \e TrackerStat. + * This is an example that shows how to perform tracking of the optical flow + * using \e TrackerPsa, \e TrackerPsaRot, \e TrackerFeatures or \e TrackerStat. */ - + /** * \file Alvar.h * @@ -159,20 +174,20 @@ */ #if defined(WIN32) && !defined(ALVAR_STATIC) - #ifdef ALVAR_BUILD - #define ALVAR_EXPORT __declspec(dllexport) - #else - #define ALVAR_EXPORT __declspec(dllimport) - #endif +#ifdef ALVAR_BUILD +#define ALVAR_EXPORT __declspec(dllexport) #else - #define ALVAR_EXPORT +#define ALVAR_EXPORT __declspec(dllimport) +#endif +#else +#define ALVAR_EXPORT #endif /** * \brief Main ALVAR namespace. */ -namespace alvar { - +namespace alvar +{ /** * \brief Major version number. */ @@ -193,35 +208,35 @@ static const int ALVAR_VERSION_PATCH = 0; * * The tag contains alpha, beta and release candidate versions. */ -static const char *ALVAR_VERSION_TAG = ""; +static const char* ALVAR_VERSION_TAG = ""; /** * \brief Revision version string. * * The revision contains an identifier from the source control system. */ -static const char *ALVAR_VERSION_REVISION = ""; +static const char* ALVAR_VERSION_REVISION = ""; /** * \brief Entire version string. */ -static const char *ALVAR_VERSION = "2.0.0"; +static const char* ALVAR_VERSION = "2.0.0"; /** * \brief Entire version string without dots. */ -static const char *ALVAR_VERSION_NODOTS = "200"; +static const char* ALVAR_VERSION_NODOTS = "200"; /** * \brief Date the library was built. */ -static const char *ALVAR_DATE = "2012-06-20"; +static const char* ALVAR_DATE = "2012-06-20"; /** * \brief System the library was built on. */ -static const char *ALVAR_SYSTEM = "Linux 3.2.0-24-generic x86_64"; +static const char* ALVAR_SYSTEM = "Linux 3.2.0-24-generic x86_64"; -} +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/AlvarException.h b/ar_track_alvar/include/ar_track_alvar/AlvarException.h index 21b3c1a..99c971d 100644 --- a/ar_track_alvar/include/ar_track_alvar/AlvarException.h +++ b/ar_track_alvar/include/ar_track_alvar/AlvarException.h @@ -32,17 +32,19 @@ #include -namespace alvar { - +namespace alvar +{ /** * \brief ALVAR exception class. */ class AlvarException : public std::runtime_error { public: - AlvarException(const char *s) : std::runtime_error(s) { } + AlvarException(const char* s) : std::runtime_error(s) + { + } }; -} +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Bitset.h b/ar_track_alvar/include/ar_track_alvar/Bitset.h index 5cc9df8..66f4772 100644 --- a/ar_track_alvar/include/ar_track_alvar/Bitset.h +++ b/ar_track_alvar/include/ar_track_alvar/Bitset.h @@ -37,14 +37,16 @@ #include #include -namespace alvar { - +namespace alvar +{ /** * \brief \e Bitset is a basic class for handling bit sequences * - * The bits are stored internally using deque (See http://osdir.com/ml/gis.geos.devel/2006-04/msg00142.html). - * The bitset is assumed to have most significant bits left i.e. the push_back() methods add to the least - * significant end of the bit sequence. The usage is clarified by the following example. + * The bits are stored internally using deque (See + * http://osdir.com/ml/gis.geos.devel/2006-04/msg00142.html). The bitset is + * assumed to have most significant bits left i.e. the push_back() methods add + * to the least significant end of the bit sequence. The usage is clarified by + * the following example. * * \section Usage * \code @@ -59,101 +61,108 @@ namespace alvar { * b.Output(std::cout); // b contains now: 00000000 00000000 10001000 10001000 * \endcode */ -class ALVAR_EXPORT Bitset { +class ALVAR_EXPORT Bitset +{ protected: - std::deque bits; - + std::deque bits; + public: - /** \brief The length of the \e Bitset */ - int Length(); - /** \brief Output the bits to selected ostream - * \param os The output stream to be used for outputting e.g. std::cout - */ - std::ostream &Output(std::ostream &os) const; - /** \brief Clear the bits */ - void clear(); - /** \brief Push back one bit - * \param bit Boolean (true/false) to be pushed to the end of bit sequence. - */ - void push_back(const bool bit); - /** \brief Push back \e bit_count bits from 'byte' \e b - * \param b Unsigned character (8-bits) to be pushed to the end of bit sequence. - * \param bit_count Number of bits to be pushed (default/max is 8 bits) - */ - void push_back(const unsigned char b, const int bit_count=8); - /** \brief Push back \e bit_count bits from 'short' \e s - * \param s Unsigned short (16-bits) to be pushed to the end of bit sequence. - * \param bit_count Number of bits to be pushed (default/max is 16 bits) - */ - void push_back(const unsigned short s, const int bit_count=16); - /** \brief Push back \e bit_count bits from 'long' \e l - * \param l Unsigned long (32-bits) to be pushed to the end of bit sequence. - * \param bit_count Number of bits to be pushed (default/max is 32 bits) - */ - void push_back(const unsigned long l, const int bit_count=32); - /** \brief Push back meaningful bits from 'long' \e l - * \param l The meaningful bits of the given unsigned long (32-bits) are pushed to the end of bit sequence. - */ - void push_back_meaningful(const unsigned long l); - /** \brief Fill the \e Bitset with non-meaningful zeros - * \param bit_count Non-meaningful zeros are added until this given \e bit_count is reached. - */ - void fill_zeros_left(const size_t bit_count); - /** \brief Push back a string of characters - * \param s String of characters to be pushed to the end of bit sequence. - */ - void push_back(std::string s); - /** \brief Pop the front bit */ - bool pop_front(); - /** \brief Pop the back bit */ - bool pop_back(); - /** \brief Flip the selected bit - * \param pos The bit in this given position is flipped. - */ - void flip(size_t pos); - /** \brief The \e Bitset as a hex string */ - std::string hex(); - /** \brief The \e Bitset as 'unsigned long' */ - unsigned long ulong(); - /** \brief The \e Bitset as 'unsigned char' */ - unsigned char uchar(); - /** \brief The \e Bitset as 'deque' */ - inline std::deque& GetBits() - { - return bits; - } + /** \brief The length of the \e Bitset */ + int Length(); + /** \brief Output the bits to selected ostream + * \param os The output stream to be used for outputting e.g. std::cout + */ + std::ostream& Output(std::ostream& os) const; + /** \brief Clear the bits */ + void clear(); + /** \brief Push back one bit + * \param bit Boolean (true/false) to be pushed to the end of bit sequence. + */ + void push_back(const bool bit); + /** \brief Push back \e bit_count bits from 'byte' \e b + * \param b Unsigned character (8-bits) to be pushed to the end of bit + * sequence. \param bit_count Number of bits to be pushed (default/max is 8 + * bits) + */ + void push_back(const unsigned char b, const int bit_count = 8); + /** \brief Push back \e bit_count bits from 'short' \e s + * \param s Unsigned short (16-bits) to be pushed to the end of bit sequence. + * \param bit_count Number of bits to be pushed (default/max is 16 bits) + */ + void push_back(const unsigned short s, const int bit_count = 16); + /** \brief Push back \e bit_count bits from 'long' \e l + * \param l Unsigned long (32-bits) to be pushed to the end of bit sequence. + * \param bit_count Number of bits to be pushed (default/max is 32 bits) + */ + void push_back(const unsigned long l, const int bit_count = 32); + /** \brief Push back meaningful bits from 'long' \e l + * \param l The meaningful bits of the given unsigned long (32-bits) are + * pushed to the end of bit sequence. + */ + void push_back_meaningful(const unsigned long l); + /** \brief Fill the \e Bitset with non-meaningful zeros + * \param bit_count Non-meaningful zeros are added until this given \e + * bit_count is reached. + */ + void fill_zeros_left(const size_t bit_count); + /** \brief Push back a string of characters + * \param s String of characters to be pushed to the end of bit sequence. + */ + void push_back(std::string s); + /** \brief Pop the front bit */ + bool pop_front(); + /** \brief Pop the back bit */ + bool pop_back(); + /** \brief Flip the selected bit + * \param pos The bit in this given position is flipped. + */ + void flip(size_t pos); + /** \brief The \e Bitset as a hex string */ + std::string hex(); + /** \brief The \e Bitset as 'unsigned long' */ + unsigned long ulong(); + /** \brief The \e Bitset as 'unsigned char' */ + unsigned char uchar(); + /** \brief The \e Bitset as 'deque' */ + inline std::deque& GetBits() + { + return bits; + } }; /** - * \brief An extended \e Bitset ( \e BitsetExt ) for handling e.g. Hamming encoding/decoding + * \brief An extended \e Bitset ( \e BitsetExt ) for handling e.g. Hamming + * encoding/decoding * - * This class is based on the basic \e Bitset. It provides additional features for Hamming coding - * (See http://en.wikipedia.org/wiki/Hamming_code). + * This class is based on the basic \e Bitset. It provides additional features + * for Hamming coding (See http://en.wikipedia.org/wiki/Hamming_code). * * The \e BitsetExt is used e.g by \e MarkerData */ -class ALVAR_EXPORT BitsetExt : public Bitset { +class ALVAR_EXPORT BitsetExt : public Bitset +{ protected: - bool verbose; - void hamming_enc_block(unsigned long block_len, std::deque::iterator &iter); - int hamming_dec_block(unsigned long block_len, std::deque::iterator &iter); + bool verbose; + void hamming_enc_block(unsigned long block_len, std::deque::iterator& iter); + int hamming_dec_block(unsigned long block_len, std::deque::iterator& iter); + public: - /** \brief Constructor */ - BitsetExt(); - /** \brief Constructor */ - BitsetExt(bool _verbose); - /** \brief Set the verbose/silent mode */ - void SetVerbose(bool _verbose); - /** \brief Count how many bits will be in the Bitset after hamming encoding */ - static int count_hamming_enc_len(int block_len, int dec_len); - /** \brief Count how many bits will be in the Bitset after hamming decoding */ - static int count_hamming_dec_len(int block_len, int enc_len); - /** \brief Hamming encoding 'in-place' using the defined block length */ - void hamming_enc(int block_len); - /** \brief Hamming decoding 'in-place' using the defined block length */ - int hamming_dec(int block_len); + /** \brief Constructor */ + BitsetExt(); + /** \brief Constructor */ + BitsetExt(bool _verbose); + /** \brief Set the verbose/silent mode */ + void SetVerbose(bool _verbose); + /** \brief Count how many bits will be in the Bitset after hamming encoding */ + static int count_hamming_enc_len(int block_len, int dec_len); + /** \brief Count how many bits will be in the Bitset after hamming decoding */ + static int count_hamming_dec_len(int block_len, int enc_len); + /** \brief Hamming encoding 'in-place' using the defined block length */ + void hamming_enc(int block_len); + /** \brief Hamming decoding 'in-place' using the defined block length */ + int hamming_dec(int block_len); }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Camera.h b/ar_track_alvar/include/ar_track_alvar/Camera.h index beca09d..961ee45 100644 --- a/ar_track_alvar/include/ar_track_alvar/Camera.h +++ b/ar_track_alvar/include/ar_track_alvar/Camera.h @@ -46,244 +46,269 @@ #include #include -namespace alvar { - -/** \brief Simple structure for collecting 2D and 3D points e.g. for camera calibration */ -struct ALVAR_EXPORT ProjPoints { - int width; - int height; - - /** \brief 3D object points corresponding with the detected 2D image points. */ - std::vector object_points; - /** \brief Detected 2D object points - * If point_counts[0] == 10, then the - * first 10 points are detected in the first frame. If - * point_counts[1] == 6, then the next 6 of these points are - * detected in the next frame... etc. - */ - std::vector image_points; - /** \brief Vector indicating how many points are detected for each frame */ - std::vector point_counts; - - /** \brief Reset \e object_points , \e image_points and \e point_counts */ - void Reset(); - - /** \brief Add elements to \e object_points , \e image_points and \e point_counts using Chessboard pattern */ - bool AddPointsUsingChessboard(IplImage *image, double etalon_square_size, int etalon_rows, int etalon_columns, bool visualize); - - /** \brief Add elements to \e object_points , \e image_points and \e point_counts using detected markers */ - bool AddPointsUsingMarkers(std::vector &marker_corners, std::vector &marker_corners_img, IplImage* image); +namespace alvar +{ +/** \brief Simple structure for collecting 2D and 3D points e.g. for camera + * calibration */ +struct ALVAR_EXPORT ProjPoints +{ + int width; + int height; + + /** \brief 3D object points corresponding with the detected 2D image points. + */ + std::vector object_points; + /** \brief Detected 2D object points + * If point_counts[0] == 10, then the + * first 10 points are detected in the first frame. If + * point_counts[1] == 6, then the next 6 of these points are + * detected in the next frame... etc. + */ + std::vector image_points; + /** \brief Vector indicating how many points are detected for each frame */ + std::vector point_counts; + + /** \brief Reset \e object_points , \e image_points and \e point_counts */ + void Reset(); + + /** \brief Add elements to \e object_points , \e image_points and \e + * point_counts using Chessboard pattern */ + bool AddPointsUsingChessboard(IplImage* image, double etalon_square_size, int etalon_rows, int etalon_columns, + bool visualize); + + /** \brief Add elements to \e object_points , \e image_points and \e + * point_counts using detected markers */ + bool AddPointsUsingMarkers(std::vector& marker_corners, std::vector& marker_corners_img, + IplImage* image); }; - /** - * \brief Simple \e Camera class for calculating distortions, orientation or projections with pre-calibrated camera + * \brief Simple \e Camera class for calculating distortions, orientation or + * projections with pre-calibrated camera */ -class ALVAR_EXPORT Camera { - +class ALVAR_EXPORT Camera +{ public: - - CvMat calib_K; double calib_K_data[3][3]; - CvMat calib_D; double calib_D_data[4]; - int calib_x_res; - int calib_y_res; - int x_res; - int y_res; - bool getCamInfo_; + CvMat calib_K; + double calib_K_data[3][3]; + CvMat calib_D; + double calib_D_data[4]; + int calib_x_res; + int calib_y_res; + int x_res; + int y_res; + bool getCamInfo_; protected: - std::string cameraInfoTopic_; - sensor_msgs::CameraInfo cam_info_; - void camInfoCallback (const sensor_msgs::CameraInfoConstPtr &); - ros::Subscriber sub_; - ros::NodeHandle n_; + std::string cameraInfoTopic_; + sensor_msgs::CameraInfo cam_info_; + void camInfoCallback(const sensor_msgs::CameraInfoConstPtr&); + ros::Subscriber sub_; + ros::NodeHandle n_; private: - bool LoadCalibXML(const char *calibfile); - bool LoadCalibOpenCV(const char *calibfile); - bool SaveCalibXML(const char *calibfile); - bool SaveCalibOpenCV(const char *calibfile); + bool LoadCalibXML(const char* calibfile); + bool LoadCalibOpenCV(const char* calibfile); + bool SaveCalibXML(const char* calibfile); + bool SaveCalibOpenCV(const char* calibfile); public: - /** \brief One of the two methods to make this class serializable by \e Serialization class */ - std::string SerializeId() { return "camera"; }; - /** \brief One of the two methods to make this class serializable by \e Serialization class - * - * You can serialize the \e Camera class using filename or any std::iostream - * as follows: - * \code - * alvar::Camera cam; - * cam.SetCalib("calib.xml", 320, 240); - * Serialization sero("calib1.xml"); - * sero<>cam; - * cam.SetRes(320, 240); - * \endcode - * \code - * std::stringstream ss; - * Serialization sero(ss); - * sero<>cam; - * \endcode - */ - bool Serialize(Serialization *ser) { - if (!ser->Serialize(calib_x_res, "width")) return false; - if (!ser->Serialize(calib_y_res, "height")) return false; - if (!ser->Serialize(calib_K, "intrinsic_matrix")) return false; - if (!ser->Serialize(calib_D, "distortion")) return false; - return true; - } - - /** \brief Constructor */ - Camera(); - Camera(ros::NodeHandle & n, std::string cam_info_topic); - - /** Sets the intrinsic calibration */ - void SetCameraInfo(const sensor_msgs::CameraInfo& camInfo); - - /** \brief Get x-direction FOV in radians */ - double GetFovX() { - return (2.0f * atan2(double(x_res) / 2.0f, (double)calib_K_data[0][0])); - } - /** \brief Get y-direction FOV in radians */ - double GetFovY() { - return (2.0f * atan2(double(y_res) / 2.0f, (double)calib_K_data[1][1])); - } - - void SetSimpleCalib(int _x_res, int _y_res, double f_fac=1.); - - /** \brief Set the calibration file and the current resolution for which the calibration is adjusted to - * \param calibfile File to load. - * \param _x_res Width of images captured from the real camera. - * \param _y_res Height of images captured from the real camera. - * \param format FILE_FORMAT_OPENCV (default) or FILE_FORMAT_XML (see doc/Camera.xsd). - */ - bool SetCalib(const char *calibfile, int _x_res, int _y_res, - FILE_FORMAT format = FILE_FORMAT_DEFAULT); - - /** \brief Save the current calibration information to a file - * \param calibfile File to save. - * \param format FILE_FORMAT_OPENCV (default) or FILE_FORMAT_XML (see doc/Camera.xsd). - */ - bool SaveCalib(const char *calibfile, FILE_FORMAT format = FILE_FORMAT_DEFAULT); - - /** \brief Calibrate using the collected \e ProjPoints */ - void Calibrate(ProjPoints &pp); - - /** \brief If we have no calibration file we can still adjust the default calibration to current resolution */ - void SetRes(int _x_res, int _y_res); - - /** \brief Get OpenGL matrix - * Generates the OpenGL projection matrix based on OpenCV intrinsic camera matrix K. - * \code - * 2*K[0][0]/width 2*K[0][1]/width -(2*K[0][2]/width+1) 0 - * 0 2*K[1][1]/height 2*K[1][2]/height-1 0 - * 0 0 -(f+n)/(f-n) -2*f*n/(f-n) - * 0 0 -1 0 - * \endcode - * - * Note, that the sign of the element [2][0] is changed. It should be - * \code - * 2*K[0][2]/width+1 - * \endcode - * - * The sign change is due to the fact that with OpenCV and OpenGL projection - * matrices both y and z should be mirrored. With other matrix elements - * the sign changes eliminate each other, but with principal point - * in x-direction we need to make the change by hand. - */ - void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height, const float far_clip = 1000.0f, const float near_clip = 0.1f); - - /** \brief Invert operation for \e GetOpenglProjectionMatrix */ - void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height); - - /** \brief Unapplys the lens distortion for points on image plane. */ - void Undistort(std::vector& points); - - /** \brief Unapplys the lens distortion for one point on an image plane. */ - void Undistort(PointDouble &point); - - /** \brief Unapplys the lens distortion for one point on an image plane. */ - void Undistort(CvPoint2D32f& point); - - /** \brief Applys the lens distortion for one point on an image plane. */ - void Distort(CvPoint2D32f& point); - - /** \brief Applys the lens distortion for points on image plane. */ - void Distort(std::vector& points); - - /** \brief Applys the lens distortion for points on image plane. */ - void Distort(PointDouble &point); - - /** \brief Calculate exterior orientation */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, Pose *pose); - - /** \brief Calculate exterior orientation - */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, - CvMat *rodriques, CvMat *tra); - - /** \brief Calculate exterior orientation - */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, - CvMat *rodriques, CvMat *tra); - - /** \brief Calculate exterior orientation - */ - void CalcExteriorOrientation(std::vector& pw, std::vector& pi, Pose *pose); - - /** \brief Update existing pose based on new observations. Use (CV_32FC3 and CV_32FC2) for matrices. */ - bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, Pose *pose); - - /** \brief Update existing pose (in rodriques&tra) based on new observations. Use (CV_32FC3 and CV_32FC2) for matrices. */ - bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, CvMat *rodriques, CvMat *tra); - - /** \brief Project one point */ - void ProjectPoint(const CvPoint3D64f pw, const Pose *pose, CvPoint2D64f &pi) const; - - /** \brief Project one point */ - void ProjectPoint(const CvPoint3D32f pw, const Pose *pose, CvPoint2D32f &pi) const; - - /** \brief Project points */ - void ProjectPoints(std::vector& pw, Pose *pose, std::vector& pi) const; - - /** \brief Project points - */ - void ProjectPoints(const CvMat* object_points, const CvMat* rotation_vector, - const CvMat* translation_vector, CvMat* image_points) const; - - /** \brief Project points - */ - void ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const; - - /** \brief Project points */ - void ProjectPoints(const CvMat* object_points, const Pose* pose, CvMat* image_points) const; + /** \brief One of the two methods to make this class serializable by \e + * Serialization class */ + std::string SerializeId() + { + return "camera"; + }; + /** \brief One of the two methods to make this class serializable by \e + * Serialization class + * + * You can serialize the \e Camera class using filename or any std::iostream + * as follows: + * \code + * alvar::Camera cam; + * cam.SetCalib("calib.xml", 320, 240); + * Serialization sero("calib1.xml"); + * sero<>cam; + * cam.SetRes(320, 240); + * \endcode + * \code + * std::stringstream ss; + * Serialization sero(ss); + * sero<>cam; + * \endcode + */ + bool Serialize(Serialization* ser) + { + if (!ser->Serialize(calib_x_res, "width")) + return false; + if (!ser->Serialize(calib_y_res, "height")) + return false; + if (!ser->Serialize(calib_K, "intrinsic_matrix")) + return false; + if (!ser->Serialize(calib_D, "distortion")) + return false; + return true; + } + + /** \brief Constructor */ + Camera(); + Camera(ros::NodeHandle& n, std::string cam_info_topic); + + /** Sets the intrinsic calibration */ + void SetCameraInfo(const sensor_msgs::CameraInfo& camInfo); + + /** \brief Get x-direction FOV in radians */ + double GetFovX() + { + return (2.0f * atan2(double(x_res) / 2.0f, (double)calib_K_data[0][0])); + } + /** \brief Get y-direction FOV in radians */ + double GetFovY() + { + return (2.0f * atan2(double(y_res) / 2.0f, (double)calib_K_data[1][1])); + } + + void SetSimpleCalib(int _x_res, int _y_res, double f_fac = 1.); + + /** \brief Set the calibration file and the current resolution for which the + * calibration is adjusted to \param calibfile File to load. \param _x_res + * Width of images captured from the real camera. \param _y_res Height of + * images captured from the real camera. \param format FILE_FORMAT_OPENCV + * (default) or FILE_FORMAT_XML (see doc/Camera.xsd). + */ + bool SetCalib(const char* calibfile, int _x_res, int _y_res, FILE_FORMAT format = FILE_FORMAT_DEFAULT); + + /** \brief Save the current calibration information to a file + * \param calibfile File to save. + * \param format FILE_FORMAT_OPENCV (default) or FILE_FORMAT_XML (see + * doc/Camera.xsd). + */ + bool SaveCalib(const char* calibfile, FILE_FORMAT format = FILE_FORMAT_DEFAULT); + + /** \brief Calibrate using the collected \e ProjPoints */ + void Calibrate(ProjPoints& pp); + + /** \brief If we have no calibration file we can still adjust the default + * calibration to current resolution */ + void SetRes(int _x_res, int _y_res); + + /** \brief Get OpenGL matrix + * Generates the OpenGL projection matrix based on OpenCV intrinsic camera + * matrix K. \code 2*K[0][0]/width 2*K[0][1]/width -(2*K[0][2]/width+1) 0 + * 0 2*K[1][1]/height 2*K[1][2]/height-1 0 + * 0 0 -(f+n)/(f-n) -2*f*n/(f-n) + * 0 0 -1 0 + * \endcode + * + * Note, that the sign of the element [2][0] is changed. It should be + * \code + * 2*K[0][2]/width+1 + * \endcode + * + * The sign change is due to the fact that with OpenCV and OpenGL projection + * matrices both y and z should be mirrored. With other matrix elements + * the sign changes eliminate each other, but with principal point + * in x-direction we need to make the change by hand. + */ + void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height, + const float far_clip = 1000.0f, const float near_clip = 0.1f); + + /** \brief Invert operation for \e GetOpenglProjectionMatrix */ + void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height); + + /** \brief Unapplys the lens distortion for points on image plane. */ + void Undistort(std::vector& points); + + /** \brief Unapplys the lens distortion for one point on an image plane. */ + void Undistort(PointDouble& point); + + /** \brief Unapplys the lens distortion for one point on an image plane. */ + void Undistort(CvPoint2D32f& point); + + /** \brief Applys the lens distortion for one point on an image plane. */ + void Distort(CvPoint2D32f& point); + + /** \brief Applys the lens distortion for points on image plane. */ + void Distort(std::vector& points); + + /** \brief Applys the lens distortion for points on image plane. */ + void Distort(PointDouble& point); + + /** \brief Calculate exterior orientation */ + void CalcExteriorOrientation(std::vector& pw, std::vector& pi, Pose* pose); + + /** \brief Calculate exterior orientation + */ + void CalcExteriorOrientation(std::vector& pw, std::vector& pi, CvMat* rodriques, + CvMat* tra); + + /** \brief Calculate exterior orientation + */ + void CalcExteriorOrientation(std::vector& pw, std::vector& pi, CvMat* rodriques, + CvMat* tra); + + /** \brief Calculate exterior orientation + */ + void CalcExteriorOrientation(std::vector& pw, std::vector& pi, Pose* pose); + + /** \brief Update existing pose based on new observations. Use (CV_32FC3 and + * CV_32FC2) for matrices. */ + bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, Pose* pose); + + /** \brief Update existing pose (in rodriques&tra) based on new observations. + * Use (CV_32FC3 and CV_32FC2) for matrices. */ + bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, CvMat* rodriques, CvMat* tra); + + /** \brief Project one point */ + void ProjectPoint(const CvPoint3D64f pw, const Pose* pose, CvPoint2D64f& pi) const; + + /** \brief Project one point */ + void ProjectPoint(const CvPoint3D32f pw, const Pose* pose, CvPoint2D32f& pi) const; + + /** \brief Project points */ + void ProjectPoints(std::vector& pw, Pose* pose, std::vector& pi) const; + + /** \brief Project points + */ + void ProjectPoints(const CvMat* object_points, const CvMat* rotation_vector, const CvMat* translation_vector, + CvMat* image_points) const; + + /** \brief Project points + */ + void ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const; + + /** \brief Project points */ + void ProjectPoints(const CvMat* object_points, const Pose* pose, CvMat* image_points) const; }; /** - * \brief Simple \e Homography class for finding and projecting points between two planes. + * \brief Simple \e Homography class for finding and projecting points between + * two planes. */ -struct ALVAR_EXPORT Homography { - double H_data[3][3]; - CvMat H; - - /** \brief Constructor */ - Homography(); - - /** \brief Find Homography for two point-sets */ - void Find(const std::vector& pw, const std::vector& pi); - - /** \brief Project points using the Homography */ - void ProjectPoints(const std::vector& from, std::vector& to); +struct ALVAR_EXPORT Homography +{ + double H_data[3][3]; + CvMat H; + + /** \brief Constructor */ + Homography(); + + /** \brief Find Homography for two point-sets */ + void Find(const std::vector& pw, const std::vector& pi); + + /** \brief Project points using the Homography */ + void ProjectPoints(const std::vector& from, std::vector& to); }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Capture.h b/ar_track_alvar/include/ar_track_alvar/Capture.h index b6f3f90..b462d28 100644 --- a/ar_track_alvar/include/ar_track_alvar/Capture.h +++ b/ar_track_alvar/include/ar_track_alvar/Capture.h @@ -34,155 +34,175 @@ #include "CaptureDevice.h" #include "Util.h" -namespace alvar { - +namespace alvar +{ /** * \brief Capture interface that plugins must implement. * - * All plugins must implement the Capture interface. This is the class that implements - * all of the camera capture funtionality. This class is created by the CapturePlugin - * implementation. + * All plugins must implement the Capture interface. This is the class that + * implements all of the camera capture funtionality. This class is created by + * the CapturePlugin implementation. */ class ALVAR_EXPORT Capture { public: - /** - * \brief Constructor. - * - * \param captureDevice Information of which camera to create. - */ - Capture(const CaptureDevice captureDevice) - : mCaptureDevice(captureDevice) - , mXResolution(0) - , mYResolution(0) - , mIsCapturing(false) + /** + * \brief Constructor. + * + * \param captureDevice Information of which camera to create. + */ + Capture(const CaptureDevice captureDevice) + : mCaptureDevice(captureDevice), mXResolution(0), mYResolution(0), mIsCapturing(false) + { + } + + /** + * \brief Destructor + */ + virtual ~Capture() + { + } + + /** + * \brief The camera information associated to this capture object. + */ + CaptureDevice captureDevice() + { + return mCaptureDevice; + } + + /** + * \brief The resolution along the x axis (horizontal). + */ + unsigned long xResolution() + { + return mXResolution; + } + + /** + * \brief The resolution along the y axis (vertical). + */ + unsigned long yResolution() + { + return mYResolution; + } + + /** + * \brief Test if the camera was properly initialized. + */ + bool isCapturing() + { + return mIsCapturing; + } + + /** + * \brief Set the resolution. + * + * \param xResolution The resolution along the x axis (horizontal). + * \param yResolution The resolution along the y axis (vertical). + */ + virtual void setResolution(const unsigned long xResolution, const unsigned long yResolution) + { + } + + /** + * \brief Starts the camera capture. + * + * \return True if the camera was properly initialized, false otherwise. + */ + virtual bool start() = 0; + + /** + * \brief Stops the camera capture. + */ + virtual void stop() = 0; + + /** + * \brief Capture one image from the camera. + * + * Do not modify this image. + * + * \return The captured image. + */ + virtual IplImage* captureImage() = 0; + + /** + * \brief Save camera settings to a file. + * + * \param filename The filename to write to. + * \return True if the settings were sucessfully saved, false otherwise. + */ + virtual bool saveSettings(std::string filename) + { + if (!isCapturing()) { + return false; } - /** - * \brief Destructor - */ - virtual ~Capture() {} - - /** - * \brief The camera information associated to this capture object. - */ - CaptureDevice captureDevice() {return mCaptureDevice;} - - /** - * \brief The resolution along the x axis (horizontal). - */ - unsigned long xResolution() {return mXResolution;} - - /** - * \brief The resolution along the y axis (vertical). - */ - unsigned long yResolution() {return mYResolution;} - - /** - * \brief Test if the camera was properly initialized. - */ - bool isCapturing() {return mIsCapturing;} - - /** - * \brief Set the resolution. - * - * \param xResolution The resolution along the x axis (horizontal). - * \param yResolution The resolution along the y axis (vertical). - */ - virtual void setResolution(const unsigned long xResolution, const unsigned long yResolution) + Serialization serialization(filename); + try + { + serialization << (*this); + } + catch (...) + { + return false; + } + return true; + } + + /** + * \brief Load camera settings from a file. + * + * \param filename The filename to read from. + * \return True if the settings were sucessfully loaded, false otherwise. + */ + virtual bool loadSettings(std::string filename) + { + if (!isCapturing()) { + return false; } - /** - * \brief Starts the camera capture. - * - * \return True if the camera was properly initialized, false otherwise. - */ - virtual bool start() = 0; - - /** - * \brief Stops the camera capture. - */ - virtual void stop() = 0; - - /** - * \brief Capture one image from the camera. - * - * Do not modify this image. - * - * \return The captured image. - */ - virtual IplImage *captureImage() = 0; - - /** - * \brief Save camera settings to a file. - * - * \param filename The filename to write to. - * \return True if the settings were sucessfully saved, false otherwise. - */ - virtual bool saveSettings(std::string filename) { - if (!isCapturing()) { - return false; - } - - Serialization serialization(filename); - try { - serialization << (*this); - } - catch (...) { - return false; - } - return true; - } - - /** - * \brief Load camera settings from a file. - * - * \param filename The filename to read from. - * \return True if the settings were sucessfully loaded, false otherwise. - */ - virtual bool loadSettings(std::string filename) { - if (!isCapturing()) { - return false; - } - - Serialization serialization(filename); - try { - serialization >> (*this); - } - catch (...) { - return false; - } - return true; - } - - /** - * \brief Show the settings dialog of the camera. - * \return True if the settings dialog was shown, false otherwise. - */ - virtual bool showSettingsDialog() = 0; - - /** - * \brief The identification of the class for serialization. - */ - virtual std::string SerializeId() = 0; - - /** - * \brief Performs serialization of the class members and configuration. - * - * \param serialization The Serialization object. - * \return True if the serialization of the class was successful, false otherwise. - */ - virtual bool Serialize(Serialization *serialization) = 0; + Serialization serialization(filename); + try + { + serialization >> (*this); + } + catch (...) + { + return false; + } + return true; + } + + /** + * \brief Show the settings dialog of the camera. + * \return True if the settings dialog was shown, false otherwise. + */ + virtual bool showSettingsDialog() = 0; + + /** + * \brief The identification of the class for serialization. + */ + virtual std::string SerializeId() = 0; + + /** + * \brief Performs serialization of the class members and configuration. + * + * \param serialization The Serialization object. + * \return True if the serialization of the class was successful, false + * otherwise. + */ + virtual bool Serialize(Serialization* serialization) = 0; protected: - CaptureDevice mCaptureDevice; - unsigned long mXResolution; - unsigned long mYResolution; - bool mIsCapturing; + CaptureDevice mCaptureDevice; + unsigned long mXResolution; + unsigned long mYResolution; + bool mIsCapturing; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h b/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h index aae94ba..428c491 100644 --- a/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h +++ b/ar_track_alvar/include/ar_track_alvar/CaptureDevice.h @@ -34,56 +34,57 @@ #include -namespace alvar { - +namespace alvar +{ /** * \brief CaptureDevice holder for camera information. * - * CaptureDevice contains the desired backend, the id and description of the camera. + * CaptureDevice contains the desired backend, the id and description of the + * camera. */ class ALVAR_EXPORT CaptureDevice { public: - /** - * \brief Constructor. - * - * \param captureType The type of capture backend. - * \param id The id of the camera. - * \param description A human readable description of the camera. - */ - CaptureDevice(const std::string captureType, const std::string id, const std::string description = ""); + /** + * \brief Constructor. + * + * \param captureType The type of capture backend. + * \param id The id of the camera. + * \param description A human readable description of the camera. + */ + CaptureDevice(const std::string captureType, const std::string id, const std::string description = ""); - /** - * \brief Destructor. - */ - ~CaptureDevice(); + /** + * \brief Destructor. + */ + ~CaptureDevice(); - /** - * \brief The type of capture backend. - */ - std::string captureType() const; + /** + * \brief The type of capture backend. + */ + std::string captureType() const; - /** - * \brief The id of the camera. - */ - std::string id() const; + /** + * \brief The id of the camera. + */ + std::string id() const; - /** - * \brief The description of the camera. - */ - std::string description() const; + /** + * \brief The description of the camera. + */ + std::string description() const; - /** - * \brief A unique name consisting of the capture type and the id. - */ - std::string uniqueName() const; + /** + * \brief A unique name consisting of the capture type and the id. + */ + std::string uniqueName() const; private: - std::string mCaptureType; - std::string mId; - std::string mDescription; + std::string mCaptureType; + std::string mId; + std::string mDescription; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h b/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h index 2e4833a..553a707 100644 --- a/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h +++ b/ar_track_alvar/include/ar_track_alvar/CaptureFactory.h @@ -38,101 +38,114 @@ #include "Platform.h" #include "Util.h" -namespace alvar { - +namespace alvar +{ class CaptureFactoryPrivate; /** * \brief CaptureFactory for creating Capture classes. * * CaptureFactory is a singleton that creates Capture classes used to perform - * camera acquisition. Different backends are implemented as dynamicly loaded plugins - * so that platform dependancies can be handled at runtime and not compile time. + * camera acquisition. Different backends are implemented as dynamicly loaded + * plugins so that platform dependancies can be handled at runtime and not + * compile time. */ class ALVAR_EXPORT CaptureFactory { public: - /** - * \brief The singleton instance of CaptureFactory. - */ - static CaptureFactory *instance(); - - /** - * \brief Vector of strings. - */ - typedef std::vector CapturePluginVector; - - /** - * \brief Enumerate capture plugins currently available. - * - * This method should be used carfully since it will load all the available plugins. - * - * \return A vector of strings identifying all currently available capture plugins. - */ - CapturePluginVector enumeratePlugins(); - - /** - * \brief Vector of CaptureDevices. - */ - typedef std::vector CaptureDeviceVector; - - /** - * \brief Enumerate capture devices currently available. - * - * This method should be used carfully since it will load all the known plugins - * and call their respective enumeration methods. The vector of CaptureDevice - * objects returned should be cached. - * - * \param captureType Force the enumeration of only one type of plugin. - * \return A vector of CaptureDevice objects that are currently available. - */ - CaptureDeviceVector enumerateDevices(const std::string &captureType = ""); - - /** - * \brief Create Capture class. Transfers onwership to the caller. - * - * If the needed backend plugin is not loaded, an attempt is made to load it and - * an instance of it is kept such that it is available for subsequent calls. - * - * \param captureDevice CaptureDevice object specifying the plugin to use. - * \return A new Capture class for which the caller takes ownership. - */ - Capture *createCapture(const CaptureDevice captureDevice); + /** + * \brief The singleton instance of CaptureFactory. + */ + static CaptureFactory* instance(); + + /** + * \brief Vector of strings. + */ + typedef std::vector CapturePluginVector; + + /** + * \brief Enumerate capture plugins currently available. + * + * This method should be used carfully since it will load all the available + * plugins. + * + * \return A vector of strings identifying all currently available capture + * plugins. + */ + CapturePluginVector enumeratePlugins(); + + /** + * \brief Vector of CaptureDevices. + */ + typedef std::vector CaptureDeviceVector; + + /** + * \brief Enumerate capture devices currently available. + * + * This method should be used carfully since it will load all the known + * plugins and call their respective enumeration methods. The vector of + * CaptureDevice objects returned should be cached. + * + * \param captureType Force the enumeration of only one type of plugin. + * \return A vector of CaptureDevice objects that are currently available. + */ + CaptureDeviceVector enumerateDevices(const std::string& captureType = ""); + + /** + * \brief Create Capture class. Transfers onwership to the caller. + * + * If the needed backend plugin is not loaded, an attempt is made to load it + * and an instance of it is kept such that it is available for subsequent + * calls. + * + * \param captureDevice CaptureDevice object specifying the plugin to use. + * \return A new Capture class for which the caller takes ownership. + */ + Capture* createCapture(const CaptureDevice captureDevice); protected: - /** - * \brief Destructor. - */ - ~CaptureFactory(); + /** + * \brief Destructor. + */ + ~CaptureFactory(); private: - /** - * \brief CaptureFactoryDestroyer for deleting the CaptureFactory singleton. - */ - class CaptureFactoryDestroyer + /** + * \brief CaptureFactoryDestroyer for deleting the CaptureFactory singleton. + */ + class CaptureFactoryDestroyer + { + public: + CaptureFactoryDestroyer(CaptureFactory* instance = NULL) : mInstance(instance) + { + } + ~CaptureFactoryDestroyer() + { + delete mInstance; + } + void set(CaptureFactory* instance) { - public: - CaptureFactoryDestroyer(CaptureFactory *instance = NULL) : mInstance(instance) {} - ~CaptureFactoryDestroyer() {delete mInstance;} - void set(CaptureFactory *instance) {mInstance = instance;} - private: - CaptureFactory *mInstance; - }; - - // private constructors and assignment operator for singleton implementation - CaptureFactory(); - CaptureFactory(const CaptureFactory&); - CaptureFactory &operator=(const CaptureFactory&); - - // static members for singleton implementation - static CaptureFactory *mInstance; - static Mutex mMutex; - static CaptureFactoryDestroyer mDestroyer; - - // members - CaptureFactoryPrivate *d; + mInstance = instance; + } + + private: + CaptureFactory* mInstance; + }; + + // private constructors and assignment operator for singleton implementation + CaptureFactory(); + CaptureFactory(const CaptureFactory&); + CaptureFactory& operator=(const CaptureFactory&); + + // static members for singleton implementation + static CaptureFactory* mInstance; + static Mutex mMutex; + static CaptureFactoryDestroyer mDestroyer; + + // members + CaptureFactoryPrivate* d; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h b/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h index f3506d1..41f1470 100644 --- a/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h +++ b/ar_track_alvar/include/ar_track_alvar/CaptureFactory_private.h @@ -30,38 +30,38 @@ #include "Plugin.h" -namespace alvar { - +namespace alvar +{ class CapturePlugin; class CaptureFactoryPrivate { public: - CaptureFactoryPrivate(); - ~CaptureFactoryPrivate(); + CaptureFactoryPrivate(); + ~CaptureFactoryPrivate(); - void setupPluginPaths(); - void parseEnvironmentVariable(const std::string &variable); - std::string pluginPrefix(); - std::string pluginExtension(); + void setupPluginPaths(); + void parseEnvironmentVariable(const std::string& variable); + std::string pluginPrefix(); + std::string pluginExtension(); - void loadPlugins(); - void loadPlugin(const std::string &captureType); - void loadPlugin(const std::string &captureType, const std::string &filename); - CapturePlugin *getPlugin(const std::string &captureType); + void loadPlugins(); + void loadPlugin(const std::string& captureType); + void loadPlugin(const std::string& captureType, const std::string& filename); + CapturePlugin* getPlugin(const std::string& captureType); - typedef std::vector PluginPathsVector; - PluginPathsVector mPluginPaths; - std::string mPluginPrefix; - std::string mPluginPostfix; + typedef std::vector PluginPathsVector; + PluginPathsVector mPluginPaths; + std::string mPluginPrefix; + std::string mPluginPostfix; - bool mLoadedAllPlugins; - typedef std::map PluginMap; - PluginMap mPluginMap; - typedef std::map CapturePluginMap; - CapturePluginMap mCapturePluginMap; + bool mLoadedAllPlugins; + typedef std::map PluginMap; + PluginMap mPluginMap; + typedef std::map CapturePluginMap; + CapturePluginMap mCapturePluginMap; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h b/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h index 3ba0ca3..b8ed826 100644 --- a/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h +++ b/ar_track_alvar/include/ar_track_alvar/CapturePlugin.h @@ -33,56 +33,56 @@ #include "Alvar.h" #include "CaptureDevice.h" -namespace alvar { - +namespace alvar +{ /** * \brief CapturePlugin interface that plugins must implement. * - * All plugins must implement the CapturePlugin interface. When the plugin is loaded, - * the CapturePlugin implementation will register itself with the CaptureFactory. + * All plugins must implement the CapturePlugin interface. When the plugin is + * loaded, the CapturePlugin implementation will register itself with the + * CaptureFactory. */ class ALVAR_EXPORT CapturePlugin { public: - /** - * \brief Constructor. - * - * \param captureType A unique identifier for the capture plugin. - */ - CapturePlugin(const std::string &captureType) - : mCaptureType(captureType) - { - } + /** + * \brief Constructor. + * + * \param captureType A unique identifier for the capture plugin. + */ + CapturePlugin(const std::string& captureType) : mCaptureType(captureType) + { + } - /** - * \brief Destructor. - */ - virtual ~CapturePlugin() {}; + /** + * \brief Destructor. + */ + virtual ~CapturePlugin(){}; - /** - * \brief Vector of CaptureDevices. - */ - typedef std::vector CaptureDeviceVector; + /** + * \brief Vector of CaptureDevices. + */ + typedef std::vector CaptureDeviceVector; - /** - * \brief Enumerate capture devices currently available. - * - * \return A vector of CaptureDevice objects that are currently available. - */ - virtual CaptureDeviceVector enumerateDevices() = 0; + /** + * \brief Enumerate capture devices currently available. + * + * \return A vector of CaptureDevice objects that are currently available. + */ + virtual CaptureDeviceVector enumerateDevices() = 0; - /** - * \brief Create Capture class. Transfers onwership to the caller. - * - * \param captureDevice Information of which camera to create. - * \return A new Capture class for which the caller takes ownership. - */ - virtual Capture *createCapture(const CaptureDevice captureDevice) = 0; + /** + * \brief Create Capture class. Transfers onwership to the caller. + * + * \param captureDevice Information of which camera to create. + * \return A new Capture class for which the caller takes ownership. + */ + virtual Capture* createCapture(const CaptureDevice captureDevice) = 0; protected: - std::string mCaptureType; + std::string mCaptureType; }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h b/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h index 8228e86..4a7fce7 100644 --- a/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h +++ b/ar_track_alvar/include/ar_track_alvar/ConnectedComponents.h @@ -35,105 +35,105 @@ #include "Line.h" #include "Camera.h" -namespace alvar { - +namespace alvar +{ /** * \brief Connected components labeling methods. -*/ + */ enum ALVAR_EXPORT LabelingMethod { - CVSEQ + CVSEQ }; /** * \brief Base class for labeling connected components from binary image. -*/ + */ class ALVAR_EXPORT Labeling { +protected: + Camera* cam; + int thresh_param1, thresh_param2; -protected : - - Camera *cam; - int thresh_param1, thresh_param2; - -public : - - /** - * \brief Pointer to grayscale image that is thresholded for labeling. - */ - IplImage *gray; - /** - * \brief Pointer to binary image that is then labeled. - */ - IplImage *bw; - - /** - * \brief Vector of 4-length vectors where the corners of detected blobs are stored. - */ - std::vector > blob_corners; - - /** - * \brief Two alternatives for thresholding the gray image. ADAPT (adaptive threshold) is only supported currently. - */ - enum ThresholdMethod - { - THRESH, - ADAPT - }; - - /** Constructor */ - Labeling(); - - /** Destructor*/ - virtual ~Labeling(); - - /** - * \brief Sets the camera object that is used to correct lens distortions. - */ - void SetCamera(Camera* camera) {cam = camera;} - - /** - * \brief Labels image and filters blobs to obtain square-shaped objects from the scene. - */ - virtual void LabelSquares(IplImage* image, bool visualize=false) = 0; - - bool CheckBorder(CvSeq* contour, int width, int height); - - void SetThreshParams(int param1, int param2) - { - thresh_param1 = param1; - thresh_param2 = param2; - } +public: + /** + * \brief Pointer to grayscale image that is thresholded for labeling. + */ + IplImage* gray; + /** + * \brief Pointer to binary image that is then labeled. + */ + IplImage* bw; + + /** + * \brief Vector of 4-length vectors where the corners of detected blobs are + * stored. + */ + std::vector > blob_corners; + + /** + * \brief Two alternatives for thresholding the gray image. ADAPT (adaptive + * threshold) is only supported currently. + */ + enum ThresholdMethod + { + THRESH, + ADAPT + }; + + /** Constructor */ + Labeling(); + + /** Destructor*/ + virtual ~Labeling(); + + /** + * \brief Sets the camera object that is used to correct lens distortions. + */ + void SetCamera(Camera* camera) + { + cam = camera; + } + + /** + * \brief Labels image and filters blobs to obtain square-shaped objects from + * the scene. + */ + virtual void LabelSquares(IplImage* image, bool visualize = false) = 0; + + bool CheckBorder(CvSeq* contour, int width, int height); + + void SetThreshParams(int param1, int param2) + { + thresh_param1 = param1; + thresh_param2 = param2; + } }; /** * \brief Labeling class that uses OpenCV routines to find connected components. -*/ + */ class ALVAR_EXPORT LabelingCvSeq : public Labeling { +protected: + int _n_blobs; + int _min_edge; + int _min_area; + bool detect_pose_grayscale; -protected : - - int _n_blobs; - int _min_edge; - int _min_area; - bool detect_pose_grayscale; - - CvMemStorage* storage; + CvMemStorage* storage; public: + LabelingCvSeq(); + ~LabelingCvSeq(); - LabelingCvSeq(); - ~LabelingCvSeq(); - - void SetOptions(bool _detect_pose_grayscale=false); + void SetOptions(bool _detect_pose_grayscale = false); - void LabelSquares(IplImage* image, bool visualize=false); + void LabelSquares(IplImage* image, bool visualize = false); - // TODO: Releases memory inside, cannot return CvSeq* - CvSeq* LabelImage(IplImage* image, int min_size, bool approx=false); + // TODO: Releases memory inside, cannot return CvSeq* + CvSeq* LabelImage(IplImage* image, int min_size, bool approx = false); }; -} // namespace alvar +} // namespace alvar #endif diff --git a/ar_track_alvar/include/ar_track_alvar/Container3d.h b/ar_track_alvar/include/ar_track_alvar/Container3d.h index 2a24270..10a5556 100644 --- a/ar_track_alvar/include/ar_track_alvar/Container3d.h +++ b/ar_track_alvar/include/ar_track_alvar/Container3d.h @@ -35,80 +35,102 @@ #include #include -namespace alvar { - -template class Container3d; +namespace alvar +{ +template +class Container3d; -/** \brief Functor class for \e Container3d \e Sort() to sort the search base using distance to specified origin. */ +/** \brief Functor class for \e Container3d \e Sort() to sort the search base + * using distance to specified origin. */ template -class Container3dSortDist { +class Container3dSortDist +{ protected: - CvPoint3D32f orig; - Container3d &container; + CvPoint3D32f orig; + Container3d& container; + public: - Container3dSortDist(Container3d &_container, const CvPoint3D32f _orig) : container(_container), orig(_orig) {} - bool operator()(size_t i1, size_t i2) - { - float x1 = container[i1].first.x-orig.x, x2 = container[i2].first.x-orig.x; - float y1 = container[i1].first.y-orig.y, y2 = container[i2].first.y-orig.y; - float z1 = container[i1].first.z-orig.z, z2 = container[i2].first.z-orig.z; - float d1 = x1*x1 + y1*y1 + z1*z1; - float d2 = x2*x2 + y2*y2 + z2*z2; - return d1& _container, const CvPoint3D32f _orig) : container(_container), orig(_orig) + { + } + bool operator()(size_t i1, size_t i2) + { + float x1 = container[i1].first.x - orig.x, x2 = container[i2].first.x - orig.x; + float y1 = container[i1].first.y - orig.y, y2 = container[i2].first.y - orig.y; + float z1 = container[i1].first.z - orig.z, z2 = container[i2].first.z - orig.z; + float d1 = x1 * x1 + y1 * y1 + z1 * z1; + float d2 = x2 * x2 + y2 * y2 + z2 * z2; + return d1 < d2; + } }; -/** \brief Functor class for \e Container3d \e Sort() to sort the search base using content size. */ +/** \brief Functor class for \e Container3d \e Sort() to sort the search base + * using content size. */ template -class Container3dSortSize { +class Container3dSortSize +{ protected: - Container3d &container; + Container3d& container; + public: - Container3dSortSize(Container3d &_container) : container(_container) {} - bool operator()(size_t i1, size_t i2) - { - return (container[i1].second->size() > container[i2].second->size()); - } + Container3dSortSize(Container3d& _container) : container(_container) + { + } + bool operator()(size_t i1, size_t i2) + { + return (container[i1].second->size() > container[i2].second->size()); + } }; -/** \brief Functor class for \e Container3d \e Limit() to limit the search space with distance */ +/** \brief Functor class for \e Container3d \e Limit() to limit the search space + * with distance */ template -class Container3dLimitDist { +class Container3dLimitDist +{ protected: - Container3d &container; - CvPoint3D32f orig; - float dist_limit; + Container3d& container; + CvPoint3D32f orig; + float dist_limit; + public: - Container3dLimitDist(Container3d &_container, const CvPoint3D32f _orig, float _dist_limit) - : container(_container),orig(_orig),dist_limit(_dist_limit) {} - bool operator()(size_t i) const { - float x = container[i].first.x-orig.x; - float y = container[i].first.y-orig.y; - float z = container[i].first.z-orig.z; - float d = x*x + y*y + z*z; - if (d <= dist_limit*dist_limit) return true; - return false; - } + Container3dLimitDist(Container3d& _container, const CvPoint3D32f _orig, float _dist_limit) + : container(_container), orig(_orig), dist_limit(_dist_limit) + { + } + bool operator()(size_t i) const + { + float x = container[i].first.x - orig.x; + float y = container[i].first.y - orig.y; + float z = container[i].first.z - orig.z; + float d = x * x + y * y + z * z; + if (d <= dist_limit * dist_limit) + return true; + return false; + } }; /** - * \brief Generic container to store any information in 3D (features, photos, ...) + * \brief Generic container to store any information in 3D (features, photos, + * ...) * - * You can store any information in 3D using this container. Each element in the container has - * an unique id that it can be referenced with using \e operator[](). The indices are from 0 - * to \e size(). You can find specific index using \e GetIndex(Iterator &iter) or \e GetIndex(T *p) . + * You can store any information in 3D using this container. Each element in the + * container has an unique id that it can be referenced with using \e + * operator[](). The indices are from 0 to \e size(). You can find specific + * index using \e GetIndex(Iterator &iter) or \e GetIndex(T *p) . * - * In addition the Container3d contains also a 'search space' that can be iterated through - * using \e begin() and \e end(). This 'search space' can be limited using \e Limit() , - * sorted using \e Sort() and reseted using \e ResetSearchSpace(). You specify what to limit/sort - * using specified functors. In ALVAR there exists functors \e Container3dLimitDist , - * \e Container3dSortSize and \e Container3dSortDist . But you can quite well make your own - * versions (see example below). - * - * The implementation is optimized for a situation where there are a lot of searches between every - * time the search space is limited/ordered. This is the reason we use vector containers - * internally. The idea is that later we will improve the class by providing more ways for limiting - * and sorting the search space; for example Frustum culling. + * In addition the Container3d contains also a 'search space' that can be + * iterated through using \e begin() and \e end(). This 'search space' can be + * limited using \e Limit() , sorted using \e Sort() and reseted using \e + * ResetSearchSpace(). You specify what to limit/sort using specified functors. + * In ALVAR there exists functors \e Container3dLimitDist , \e + * Container3dSortSize and \e Container3dSortDist . But you can quite well make + * your own versions (see example below). + * + * The implementation is optimized for a situation where there are a lot of + * searches between every time the search space is limited/ordered. This is the + * reason we use vector containers internally. The idea is that later we will + * improve the class by providing more ways for limiting and sorting the search + * space; for example Frustum culling. * * Usage: * @@ -130,14 +152,14 @@ class Container3dLimitDist { * int x_min, x_max; * Container3d &container; * public: - * Container3dLimitX(Container3d &_container, int _x_min, int _x_max) + * Container3dLimitX(Container3d &_container, int _x_min, int _x_max) * : container(_container),x_min(_x_min),x_max(_x_max) {} * bool operator()(size_t i1) const { - * if ((container[i1].first.x >= x_min) && (container[i1].first.x <= x_max)) return true; - * return false; + * if ((container[i1].first.x >= x_min) && (container[i1].first.x <= x_max)) + * return true; return false; * } * }; - * + * * ... * Container3d c3d; * c3d.Add(CvPoint3D32f(0,0,0), 0); @@ -165,120 +187,174 @@ class Container3dLimitDist { template class Container3d { - public: - /** \brief \e node_type for storing data. 3D-position is paired with the data content. */ - typedef std::pair node_type; - protected: - /** \brief the actual data in using node_type: pair */ - std::vector data; - /** \brief Possibly limited set of indices for \e data in somehow "optimal" search order */ - std::vector search_space; +public: + /** \brief \e node_type for storing data. 3D-position is paired with the data + * content. */ + typedef std::pair node_type; + +protected: + /** \brief the actual data in using node_type: pair */ + std::vector data; + /** \brief Possibly limited set of indices for \e data in somehow "optimal" + * search order */ + std::vector search_space; + +public: + /** \brief Add \e _data in the container and associate it with 3D position \e + * _pos */ + void Add(const CvPoint3D32f& _pos, const T& _data) + { + data.push_back(node_type(_pos, _data)); + search_space.push_back(data.size() - 1); + } + /** \brief Clear the container */ + void Clear() + { + data.clear(); + search_space.clear(); + } + /** \brief Reset the search space to contain whole data */ + void ResetSearchSpace() + { + search_space.resize(data.size()); + for (size_t i = 0; i < search_space.size(); i++) + { + search_space[i] = i; + } + } + /** \brief Erase item in the container */ + void Erase(size_t index) + { + typename std::vector::iterator iter_d; + iter_d = data.begin(); + for (size_t i = 0; i < index; i++) + iter_d++; + data.erase(iter_d); + ResetSearchSpace(); + } + /** \brief Sort using external Compare method */ + template + int Sort(Compare comp) + { + stable_sort(search_space.begin(), search_space.end(), comp); + return search_space.size(); + } - public: - /** \brief Add \e _data in the container and associate it with 3D position \e _pos */ - void Add(const CvPoint3D32f& _pos, const T& _data){ - data.push_back(node_type(_pos, _data)); - search_space.push_back(data.size()-1); - } - /** \brief Clear the container */ - void Clear() { - data.clear(); - search_space.clear(); - } - /** \brief Reset the search space to contain whole data */ - void ResetSearchSpace() { - search_space.resize(data.size()); - for (size_t i=0; i::iterator iter_d; - iter_d = data.begin(); - for (size_t i=0; i - int Sort(Compare comp) { - stable_sort(search_space.begin(), search_space.end(), comp); - return search_space.size(); - } + /** \brief Limit the search space with external limitation */ + template + int Limit(Test test) + { + std::vector::iterator iter; + for (iter = search_space.begin(); iter != search_space.end();) + { + if (!test(*iter)) + { + iter = search_space.erase(iter); + } + else + { + iter++; + } + } + return search_space.size(); + } - /** \brief Limit the search space with external limitation */ - template - int Limit(Test test) { - std::vector::iterator iter; - for (iter = search_space.begin(); iter != search_space.end();) { - if (!test(*iter)) { - iter = search_space.erase(iter); - } else { - iter++; - } - } - return search_space.size(); - } + /** \brief Iterator for going through the items in \e Container3d in the + * specified order + * + * The idea is that the content in \e Container3d can be sorted and limited in + * different ways. After sorting/limiting the content the \e iterator (\e + * Begin() and \e End() ) can be used for accessing the data items in optimal + * order. + */ + class Iterator : public std::iterator + { + protected: + Container3d* container; + std::vector::iterator iter; - /** \brief Iterator for going through the items in \e Container3d in the specified order - * - * The idea is that the content in \e Container3d can be sorted and limited in different - * ways. After sorting/limiting the content the \e iterator (\e Begin() and \e End() ) can - * be used for accessing the data items in optimal order. - */ - class Iterator : public std::iterator - { - protected: - Container3d *container; - std::vector::iterator iter; - public: - Iterator() {} - Iterator(Container3d *_container, std::vector::iterator _iter) : container(_container), iter(_iter) {} - node_type &operator*() const { return container->data[*iter]; } - node_type *operator->() const { return &(operator*()); } - virtual Iterator& operator++() { ++iter; return *this; } - bool operator==(const Iterator& _m) const { return iter == _m.iter; } - bool operator!=(const Iterator& _m) const { return iter != _m.iter; } - size_t GetIndex() { return *iter; } - }; + public: + Iterator() + { + } + Iterator(Container3d* _container, std::vector::iterator _iter) : container(_container), iter(_iter) + { + } + node_type& operator*() const + { + return container->data[*iter]; + } + node_type* operator->() const + { + return &(operator*()); + } + virtual Iterator& operator++() + { + ++iter; + return *this; + } + bool operator==(const Iterator& _m) const + { + return iter == _m.iter; + } + bool operator!=(const Iterator& _m) const + { + return iter != _m.iter; + } + size_t GetIndex() + { + return *iter; + } + }; - /** \brief Provides an iterator pointing to the beginning of the limited/sorted 3D content */ - Iterator begin() { - return Iterator(this, search_space.begin()); - } + /** \brief Provides an iterator pointing to the beginning of the + * limited/sorted 3D content */ + Iterator begin() + { + return Iterator(this, search_space.begin()); + } - /** \brief Provides an iterator pointing to the end of the limited/sorted 3D content */ - Iterator end() { - return Iterator(this, search_space.end()); - } + /** \brief Provides an iterator pointing to the end of the limited/sorted 3D + * content */ + Iterator end() + { + return Iterator(this, search_space.end()); + } - /** \brief Get number of items that can be referenced using \e operator[]() */ - size_t size() const { return data.size(); } + /** \brief Get number of items that can be referenced using \e operator[]() */ + size_t size() const + { + return data.size(); + } - /** \brief Get absolute reference usable with \e operator[]() based on the iterator */ - size_t GetIndex(Iterator &iter) { - return iter.GetIndex(); - } + /** \brief Get absolute reference usable with \e operator[]() based on the + * iterator */ + size_t GetIndex(Iterator& iter) + { + return iter.GetIndex(); + } - /** \brief Instead of \e Iterator we can use also absolute references for data with \e operator[]() */ - node_type &operator[](size_t index) { - return data[index]; - } + /** \brief Instead of \e Iterator we can use also absolute references for data + * with \e operator[]() */ + node_type& operator[](size_t index) + { + return data[index]; + } - /** \brief Get absolute reference usable with \e operator[]() based on the content */ - size_t GetIndex(T *p) { - size_t i=0; - for (; i