├── common ├── Epsilon.h ├── Point2D.h ├── Point3D.h ├── IPoint2D.h ├── Vector3D.h ├── Vector3D.cxx ├── RandomNumberGenerator.h ├── Ray3D.cxx ├── Ray3D.h ├── Point.h └── Vector.h ├── examples ├── coin3DViewer │ ├── coin2.dll │ ├── sowin1.dll │ ├── coin3DSceneViewer.exe │ └── coin3dSceneViewer.cxx ├── readme.txt ├── CMakeLists.txt ├── AbsoluteOrientation.cxx ├── linearEquationSystemSolver.cxx ├── crosswireUSCalibration.cxx └── planeUSCalibration.cxx ├── testing ├── SphereParametersEstimatorTest.cxx ├── Data │ ├── crossWirePhantom2DPoints.txt │ └── crossWirePhantomTransformations.txt ├── CMakeLists.txt ├── PivotCalibrationParametersEstimatorTest.cxx ├── RayIntersectionParametersTest.cxx ├── AbsoluteOrientationParametersEstimatorTest.cxx └── DenseLinearEquationSystemParametersEstimatorTest.cxx ├── UseLSQRRecipes.cmake.in ├── LSQRRecipesConfig.cmake.in ├── GenerateLSQRRecipesConfig.cmake ├── license.txt ├── copyright └── copyright.h ├── parametersEstimators ├── ParametersEstimator.h ├── Line2DParametersEstimator.h ├── PlaneParametersEstimator.h ├── PivotCalibrationParametersEstimator.h ├── LineParametersEstimator.h ├── Line2DParametersEstimator.cxx ├── PivotCalibrationParametersEstimator.cxx ├── DenseLinearEquationSystemParametersEstimator.hxx ├── RayIntersectionParametersEstimator.h ├── AbsoluteOrientationParametersEstimator.h ├── LineParametersEstimator.hxx ├── PlanePhantomUSCalibrationParametersEstimator.h ├── RANSAC.h ├── RayIntersectionParametersEstimator.cxx ├── PlaneParametersEstimator.hxx ├── DenseLinearEquationSystemParametersEstimator.h └── SphereParametersEstimator.h ├── readme.txt ├── Cmake └── Modules │ └── FindLSQRRecipes.cmake └── CMakeLists.txt /common/Epsilon.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zivy/LSQRRecipes/HEAD/common/Epsilon.h -------------------------------------------------------------------------------- /examples/coin3DViewer/coin2.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zivy/LSQRRecipes/HEAD/examples/coin3DViewer/coin2.dll -------------------------------------------------------------------------------- /examples/coin3DViewer/sowin1.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zivy/LSQRRecipes/HEAD/examples/coin3DViewer/sowin1.dll -------------------------------------------------------------------------------- /examples/coin3DViewer/coin3DSceneViewer.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zivy/LSQRRecipes/HEAD/examples/coin3DViewer/coin3DSceneViewer.exe -------------------------------------------------------------------------------- /testing/SphereParametersEstimatorTest.cxx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zivy/LSQRRecipes/HEAD/testing/SphereParametersEstimatorTest.cxx -------------------------------------------------------------------------------- /common/Point2D.h: -------------------------------------------------------------------------------- 1 | #ifndef _POINT2D_H_ 2 | #define _POINT2D_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include "Point.h" 7 | 8 | namespace lsqrRecipes { 9 | 10 | typedef Point Point2D; 11 | 12 | } //namespace lsqrRecipes 13 | 14 | #endif //_POINT2D_H_ 15 | -------------------------------------------------------------------------------- /common/Point3D.h: -------------------------------------------------------------------------------- 1 | #ifndef _POINT3D_H_ 2 | #define _POINT3D_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include "Point.h" 7 | 8 | namespace lsqrRecipes { 9 | 10 | typedef Point Point3D; 11 | 12 | } //namespace lsqrRecipes 13 | 14 | #endif //_POINT3D_H_ 15 | -------------------------------------------------------------------------------- /common/IPoint2D.h: -------------------------------------------------------------------------------- 1 | #ifndef _IPOINT2D_H_ 2 | #define _IPOINT2D_H_ 3 | 4 | #include "Copyright.h" 5 | 6 | #include "Point.h" 7 | 8 | namespace lsqrRecipes { 9 | 10 | typedef Point IPoint2D; 11 | 12 | } //namespace lsqrRecipes 13 | 14 | #endif //_IPOINT2D_H_ 15 | -------------------------------------------------------------------------------- /common/Vector3D.h: -------------------------------------------------------------------------------- 1 | #ifndef _VECTOR3D_H_ 2 | #define _VECTOR3D_H_ 3 | 4 | #include "copyright.h" 5 | #include "Vector.h" 6 | 7 | namespace lsqrRecipes { 8 | 9 | typedef Vector Vector3D; 10 | 11 | Vector3D crossProduct(const Vector3D &left, const Vector3D &right); 12 | 13 | } //namespace lsqrRecipes 14 | 15 | #endif //_VECTOR3D_H_ 16 | -------------------------------------------------------------------------------- /common/Vector3D.cxx: -------------------------------------------------------------------------------- 1 | #include "Vector3D.h" 2 | 3 | namespace lsqrRecipes { 4 | 5 | Vector3D crossProduct(const Vector3D &left, const Vector3D &right) 6 | { 7 | Vector3D result; 8 | result[0] = left[1] * right[2] - left[2] * right[1]; 9 | result[1] = left[2] * right[0] - left[0] * right[2]; 10 | result[2] = left[0] * right[1] - left[1] * right[0]; 11 | return result; 12 | } 13 | 14 | } //namespace lsqrRecipes 15 | -------------------------------------------------------------------------------- /UseLSQRRecipes.cmake.in: -------------------------------------------------------------------------------- 1 | # 2 | # This file sets up include directories, and link directories, 3 | # for a project that wants to use LSQRRecipes. It should not be 4 | # included directly, rather use the LSQRRecipes_USE_FILE variable 5 | # value from LSQRRecipesConfig.cmake. 6 | # 7 | 8 | # 9 | # include directories needed to use LSQRRecipes 10 | # 11 | include_directories(${LSQRRecipes_INCLUDE_DIRS}) 12 | 13 | # 14 | #link directories needed to use LSQRRecipes 15 | # 16 | LINK_DIRECTORIES(${LSQRRecipes_LIBRARY_DIRS}) 17 | 18 | -------------------------------------------------------------------------------- /LSQRRecipesConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # 2 | # LSQRRecipes CMake configuration file for external projects, configured through 3 | # the GenerateLSQRRecipesConfig.cmake file (config_file) 4 | # 5 | 6 | # 7 | # LSQRRecipes include file directories 8 | # 9 | set(LSQRRecipes_INCLUDE_DIRS "@LSQRRecipes_INCLUDE_DIRS_CONFIG@") 10 | 11 | # 12 | # LSQRRecipes library directories 13 | # 14 | set(LSQRRecipes_LIBRARY_DIRS "@LSQRRecipes_LIBRARY_DIRS_CONFIG@") 15 | 16 | # 17 | # The LSQRRecipes version number 18 | # 19 | set(LSQRRecipes_VERSION_MAJOR "@LSQRRecipes_VERSION_MAJOR@") 20 | set(LSQRRecipes_VERSION_MINOR "@LSQRRecipes_VERSION_MINOR@") 21 | set(LSQRRecipes_VERSION_PATCH "@LSQRRecipes_VERSION_PATCH@") 22 | 23 | # 24 | # The location of the UseLSQRRecipes.cmake file. 25 | # 26 | set(LSQRRecipes_USE_FILE "@LSQRRecipes_USE_FILE@") 27 | -------------------------------------------------------------------------------- /GenerateLSQRRecipesConfig.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Generate the LSQRRecipesConfig.cmake file for the build tree and installation. 3 | # 4 | 5 | ################################################################################ 6 | # 7 | # Build tree. 8 | # 9 | 10 | set(LSQRRecipes_USE_FILE ${LSQRRecipes_BINARY_DIR}/UseLSQRRecipes.cmake) 11 | set(LSQRRecipes_LIBRARY_DIRS_CONFIG ${LSQRRecipes_BINARY_DIR}) 12 | set(LSQRRecipes_INCLUDE_DIRS_CONFIG ${LSQRRecipes_INCLUDE_DIRS}) 13 | configure_file(${LSQRRecipes_SOURCE_DIR}/LSQRRecipesConfig.cmake.in 14 | ${LSQRRecipes_BINARY_DIR}/LSQRRecipesConfig.cmake @ONLY) 15 | 16 | ################################################################################ 17 | # 18 | # Install tree. 19 | # 20 | 21 | set(LSQRRecipes_USE_FILE ${LSQRRecipes_INSTALL_LIB_DIR}/UseLSQRRecipes.cmake) 22 | set(LSQRRecipes_LIBRARY_DIRS_CONFIG ${LSQRRecipes_INSTALL_LIB_DIR}) 23 | set(LSQRRecipes_INCLUDE_DIRS_CONFIG ${LSQRRecipes_INSTALL_INCLUDE_DIR}) 24 | 25 | #place the LSQRRecipesConfig.cmake for the installation version in a temporary 26 | #location 27 | configure_file(${LSQRRecipes_SOURCE_DIR}/LSQRRecipesConfig.cmake.in 28 | ${PRE_INSTALL_DIR}/LSQRRecipesConfig.cmake @ONLY) 29 | -------------------------------------------------------------------------------- /examples/readme.txt: -------------------------------------------------------------------------------- 1 | This directory contains examples showing the usage of the RANSAC 2 | algorithm to estimate various parametric objects. 3 | 4 | The following examples also output 3D scenes: 5 | 1. lineEstimation 6 | 2. planeEstimation 7 | 3. rayIntersection 8 | 4. sphereEstimation 9 | 5. pivotCalibration 10 | 11 | These 3D scenes visually illustrate the results of using a least 12 | squares solution as compared to wrapping it with the RANSAC 13 | algorithm. The output file format is OpenInventor. You can display 14 | it using the viewer found in the coin3DViewer directory 15 | (win32 command line application). 16 | 17 | On linux (ubuntu) you can use the ivview to view open inventor 18 | files, unfortunately it currently has a bug and does not run, but 19 | it should be worth trying. 20 | 21 | The following examples have data files associated with them: 22 | 1. linearEquationSystemSolver. 23 | 2. pivotCalibration. 24 | 25 | Data/augmentedMatrixWithOutliers.txt 26 | This file contains an augmented matrix [A|b] with some rows being 27 | outliers. The example shows that we can robustly solve the 28 | overdetermined equation system Ax=b even with a significant amount 29 | of outliers. 30 | Data/pivotCalibrationDataWithOutliers.txt 31 | This file contains a set of rigid transformation acquired while 32 | pivoting a tracked tool around a fixed point with about 30% outliers (arbitrary 33 | motion of the tool). Each line in the file is of the form x y z qx qy qz qs, 34 | where the rotation is represented by a unit quaternion. 35 | -------------------------------------------------------------------------------- /common/RandomNumberGenerator.h: -------------------------------------------------------------------------------- 1 | #ifndef _RANDOM_NUMBER_GENERATOR_H_ 2 | #define _RANDOM_NUMBER_GENERATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include 7 | 8 | /** 9 | * A wrapper for the VNL random number generator (facilitates separation of 10 | * the least squares library code from VNL sometime in the future - probably 11 | * never). 12 | * 13 | * @author: Ziv Yaniv 14 | */ 15 | 16 | namespace lsqrRecipes { 17 | 18 | class RandomNumberGenerator { 19 | public: 20 | RandomNumberGenerator() {generator = new vnl_random();} 21 | RandomNumberGenerator(unsigned long seed) {generator = new vnl_random(seed);} 22 | ~RandomNumberGenerator() {delete generator;} 23 | 24 | 25 | /** 26 | * Get a random number uniformly distributed in (a,b). 27 | * @param a Lower bound for random numbers, default is 0.0. 28 | * @param b Upper bound for random numbers, default is 1.0. 29 | */ 30 | virtual double uniform(double a=0.0, double b=1.0) { 31 | return generator->drand64(a,b); 32 | } 33 | 34 | /** 35 | * Get a random number normally distributed with given mean and standard 36 | * deviation. 37 | * @param sigma Normal distributions standard deviation, default is one. 38 | * @param mu Normal distribution's mean, default is zero. 39 | */ 40 | virtual double normal(double sigma=1.0, double mu=0.0) { 41 | return sigma*generator->normal64() + mu; 42 | } 43 | 44 | private: 45 | vnl_random *generator; 46 | }; 47 | 48 | } //namespace lsqrRecipes 49 | 50 | #endif //_RANDOM_NUMBER_GENERATOR_H_ 51 | -------------------------------------------------------------------------------- /license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2010, Ziv Yaniv, All rights reserved. 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions 5 | are met: 6 | (1) Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | (2) Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | (3) Neither the name of Georgetown University nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 19 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 22 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /examples/coin3DViewer/coin3dSceneViewer.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | int main(int argc, char **argv) 10 | { 11 | if(argc!=2) { 12 | std::cout<<"Usage: "<setBackgroundColor(SbColor(1.0,1.0,1.0)); 24 | //open the scene file and load it into the data base 25 | SoInput in; 26 | if(!in.openFile(argv[1])) { 27 | std::cerr<<"Error opening input file ("<setSceneGraph(root); 39 | viewer->show(); 40 | //show window and run the event loop 41 | SoWin::show(window); 42 | SoWin::mainLoop(); 43 | //cleanup and exit 44 | delete viewer; 45 | root->unref(); 46 | return EXIT_SUCCESS; 47 | } -------------------------------------------------------------------------------- /testing/Data/crossWirePhantom2DPoints.txt: -------------------------------------------------------------------------------- 1 | 124.0000000000 304.0000000000 2 | 272.0000000000 323.0000000000 3 | 33.0000000000 245.0000000000 4 | 162.0000000000 255.0000000000 5 | 87.0000000000 117.0000000000 6 | 171.0000000000 103.0000000000 7 | 289.0000000000 110.0000000000 8 | 139.0000000000 354.0000000000 9 | 92.0000000000 280.0000000000 10 | 171.0000000000 291.0000000000 11 | 255.0000000000 301.0000000000 12 | 132.0000000000 264.0000000000 13 | 84.0000000000 220.0000000000 14 | 276.0000000000 250.0000000000 15 | 327.0000000000 211.0000000000 16 | 228.0000000000 215.0000000000 17 | 102.0000000000 139.0000000000 18 | 124.0000000000 259.0000000000 19 | 194.0000000000 257.0000000000 20 | 293.0000000000 261.0000000000 21 | 224.0000000000 153.0000000000 22 | 157.0000000000 248.0000000000 23 | 71.0000000000 259.0000000000 24 | 364.0000000000 243.0000000000 25 | 214.0000000000 121.0000000000 26 | 177.0000000000 330.0000000000 27 | 149.0000000000 292.0000000000 28 | 257.0000000000 311.0000000000 29 | 208.0000000000 201.0000000000 30 | 95.0000000000 154.0000000000 31 | 240.0000000000 153.0000000000 32 | 168.0000000000 221.0000000000 33 | 158.0000000000 113.0000000000 34 | 216.0000000000 470.0000000000 35 | 300.0000000000 505.0000000000 36 | 63.0000000000 482.0000000000 37 | 162.0000000000 445.0000000000 38 | 277.0000000000 438.0000000000 39 | 184.0000000000 449.0000000000 40 | 68.0000000000 454.0000000000 41 | 201.0000000000 469.0000000000 42 | 184.0000000000 226.0000000000 43 | 189.0000000000 164.0000000000 44 | 167.0000000000 106.0000000000 45 | 183.0000000000 346.0000000000 46 | 64.0000000000 358.0000000000 47 | 304.0000000000 358.0000000000 48 | 172.0000000000 292.0000000000 49 | 274.0000000000 315.0000000000 50 | 50.0000000000 322.0000000000 51 | 60.0000000000 232.0000000000 52 | 160.0000000000 208.0000000000 53 | 316.0000000000 217.0000000000 54 | 167.0000000000 146.0000000000 55 | -------------------------------------------------------------------------------- /copyright/copyright.h: -------------------------------------------------------------------------------- 1 | #ifndef _COPYRIGHT_H_ 2 | #define _COPYRIGHT_H_ 3 | 4 | /** 5 | * Copyright (c) 2010-2013, Ziv Yaniv, All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * (1) Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * (2) Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the distribution. 15 | * (3) Neither the name of Georgetown University nor the 16 | * names of its contributors may be used to endorse or promote products 17 | * derived from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 23 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | 33 | namespace lsqrRecipes { 34 | 35 | class Copyright { 36 | public: 37 | static void print( std::ostream& output ) {output<<"Copyright (c) 2010-2014 Ziv Yaniv\n";} 38 | }; 39 | 40 | } //namespace lsqrRecipes 41 | 42 | #endif //_COPYRIGHT_H_ 43 | -------------------------------------------------------------------------------- /parametersEstimators/ParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _PARAMETERS_ESTIMATOR_H_ 2 | #define _PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include 7 | 8 | /** 9 | * This class defines the interface for parameter estimators. 10 | * Classes which inherit from it can be used by the Ransac class to perform robust 11 | * parameter estimation. 12 | * The interface includes three methods: 13 | * 1.estimate() - Estimation of the parameters using the minimal amount of data 14 | * (exact estimate). 15 | * 2.leastSquaresEstimate() - Estimation of the parameters using overdetermined 16 | * data, so that the estimate minimizes a least squares 17 | * error criterion. 18 | * 3.agree() - Does the given data agree with the model parameters. 19 | * 20 | * @author: Ziv Yaniv 21 | * 22 | */ 23 | 24 | namespace lsqrRecipes { 25 | 26 | template 27 | class ParametersEstimator { 28 | public: 29 | 30 | /** 31 | * Constructor which takes the number of data objects required for an exact 32 | * estimate (e.g. 2 for a line where the data objects are points) 33 | */ 34 | ParametersEstimator(unsigned int minElements) {this->minForEstimate = minElements;} 35 | 36 | /** 37 | * Exact estimation of parameters. 38 | * @param data The data used for the estimate. 39 | * @param parameters This vector is cleared and then filled with the computed parameters. 40 | */ 41 | virtual void estimate(std::vector &data, std::vector ¶meters) = 0; 42 | virtual void estimate(std::vector &data, std::vector ¶meters) = 0; 43 | 44 | /** 45 | * Least squares estimation of parameters. 46 | * @param data The data used for the estimate. 47 | * @param parameters This vector is cleared and then filled with the computed parameters. 48 | */ 49 | virtual void leastSquaresEstimate(std::vector &data, std::vector ¶meters) = 0; 50 | virtual void leastSquaresEstimate(std::vector &data, std::vector ¶meters) = 0; 51 | 52 | /** 53 | * This method tests if the given data agrees with the given model parameters. 54 | */ 55 | virtual bool agree(std::vector ¶meters, T &data) = 0; 56 | 57 | /** 58 | * Returns the number of data objects required for an exact 59 | * estimate (e.g. 2 for a line where the data objects are points) 60 | */ 61 | unsigned int numForEstimate() {return this->minForEstimate;} 62 | protected: 63 | unsigned int minForEstimate; 64 | }; 65 | 66 | } //namespace lsqrRecipes 67 | 68 | #endif //_PARAMETERS_ESTIMATOR_H_ 69 | -------------------------------------------------------------------------------- /readme.txt: -------------------------------------------------------------------------------- 1 | This repository contains implementations of algorithms for least squares 2 | estimation of various parametric objects. In addition, a generic implementation of 3 | the RANdom SAmple Consensus (RANSAC) is provided. This adds the ability to perform 4 | robust estimation without integrating the associated logic into the least 5 | squares logic. 6 | 7 | The context within which these algorithms were used is image-guided interventions. 8 | 9 | Example and testing programs are provided. 10 | 11 | The parametric objects include: 12 | 1. hypersphere (2D - circle, 3D - sphere, kD - sphere). 13 | 2. hyperplane (2D - line, 3D - plane, kD - plane). 14 | 3. intersection point of 3D rays. 15 | 4. US calibration matrix via (a) crosswire phantom (b) calibrated pointer 16 | (c) plane phantom. 17 | 5. absolute orientation, rigid transformation, relating corresponding point pairs 18 | in two coordinate systems [unit quaternion solution due to B.K.P. Horn]. 19 | 6. kD line (line in any dimension). 20 | 7. 2D line - doesn't require the VNL library and can be easily incorporated 21 | into an existing project using a minimal subset of the LSQRRecipes library 22 | (see details below). 23 | 8. solution to a set of linear equations. 24 | 9. translations obtained via pivot calibration. 25 | ------------------------------------------------------------------------------- 26 | 27 | Requirements: 28 | 29 | Configuration is done via the Cmake tool (http://www.cmake.org). 30 | 31 | The VNL subcomponent from the VXL (http://vxl.sourceforge.net/) toolkit is required 32 | for linear algebra and nonlinear estimation methods. This set of libraries is also 33 | available as part of ITK (http://www.itk.org/). The LSQRRecipes library can be 34 | configured to use either of these distributions through a Cmake flag 35 | (USING_ITK_VNL) 36 | 37 | ------------------------------------------------------------------------------- 38 | 39 | 2D line 40 | -------- 41 | If all you need is a robust estimation of a 2D line and you don't want to use a 42 | linear algebra package for computing eigenvectors you can directly copy the 43 | following files into your project: 44 | 0. copyright.h 45 | 1. ParametersEstimator.h 46 | 2. Line2DParametersEstimator.{h,cxx} 47 | 3. RANSAC.{h,hxx} 48 | 49 | You will need a 2D point class which has the square bracket operator 50 | to access the point elements (Point2D.h). Either you already have such a class 51 | or you can use the following minimal implementation, just copy paste: 52 | 53 | #ifndef _POINT2D_H_ 54 | #define _POINT2D_H_ 55 | 56 | #include "copyright.h" 57 | 58 | namespace lsqrRecipes { 59 | 60 | class Point2D 61 | { 62 | public: 63 | Point2D(double x=0.0, double y=0.0) {this->data[0] = x; this->data[1] = y;} 64 | const double & operator[](int index) const {return this->data[index];} 65 | 66 | private: 67 | double data[2]; 68 | }; 69 | 70 | } //namespace lsqrRecipes 71 | 72 | #endif //_POINT2D_H_ 73 | ------------------------------------------------------------------------------- 74 | -------------------------------------------------------------------------------- /testing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | add_executable(lineEstimationTest LineParametersEstimatorTest.cxx) 3 | target_link_libraries(lineEstimationTest ${LINK_LIBRARIES}) 4 | 5 | add_executable(planeEstimationTest PlaneParametersEstimatorTest.cxx) 6 | target_link_libraries(planeEstimationTest ${LINK_LIBRARIES}) 7 | 8 | add_executable(sphereEstimationTest SphereParametersEstimatorTest.cxx) 9 | target_link_libraries(sphereEstimationTest ${LINK_LIBRARIES}) 10 | 11 | add_executable(rayIntersectionEstimationTest RayIntersectionParametersTest.cxx) 12 | target_link_libraries(rayIntersectionEstimationTest ${LINK_LIBRARIES}) 13 | 14 | add_executable(singlePointUSCalibrationEstimationTest SinglePointTargetUSCalibrationParametersEstimatorTest.cxx) 15 | target_link_libraries(singlePointUSCalibrationEstimationTest ${LINK_LIBRARIES}) 16 | 17 | add_executable(planePhantomUSCalibrationEstimationTest PlanePhantomUSCalibrationParametersEstimatorTest.cxx) 18 | target_link_libraries(planePhantomUSCalibrationEstimationTest ${LINK_LIBRARIES}) 19 | 20 | add_executable(absoluteOrientationParametersEstimatorTest AbsoluteOrientationParametersEstimatorTest.cxx) 21 | target_link_libraries(absoluteOrientationParametersEstimatorTest ${LINK_LIBRARIES}) 22 | 23 | add_executable(denseLinearEquationSystemParametersEstimatorTest DenseLinearEquationSystemParametersEstimatorTest.cxx) 24 | target_link_libraries(denseLinearEquationSystemParametersEstimatorTest ${LINK_LIBRARIES}) 25 | 26 | add_executable(pivotCalibrationParametersEstimatorTest PivotCalibrationParametersEstimatorTest.cxx) 27 | target_link_libraries(pivotCalibrationParametersEstimatorTest ${LINK_LIBRARIES}) 28 | 29 | add_test(LineEstimationTest lineEstimationTest) 30 | add_test(PlaneEstimationTest planeEstimationTest) 31 | add_test(SphereEstimationTest sphereEstimationTest) 32 | add_test(RayIntersectionEstimationTest rayIntersectionEstimationTest) 33 | add_test(SinglePointTargetUSCalibrationSimulatedTest singlePointUSCalibrationEstimationTest) 34 | add_test(PlanePhantomUSCalibrationSimulatedTest planePhantomUSCalibrationEstimationTest) 35 | add_test(AbsoluteOrientationTest absoluteOrientationParametersEstimatorTest) 36 | add_test(DenseLinearEquationSystemTest denseLinearEquationSystemParametersEstimatorTest) 37 | 38 | if(LSQR_RECIPES_TESTING_DATA_ROOT) 39 | add_test(SinglePointTargetUSCalibrationPhantomExperimentalDataTest singlePointUSCalibrationEstimationTest 40 | ${LSQR_RECIPES_TESTING_DATA_ROOT}/crossWirePhantomTransformations.txt 41 | ${LSQR_RECIPES_TESTING_DATA_ROOT}/crossWirePhantom2DPoints.txt) 42 | add_test(DenseLinearEquationSystemPivotCalibrationExperimentalDataTest denseLinearEquationSystemParametersEstimatorTest 43 | ${LSQR_RECIPES_TESTING_DATA_ROOT}/augmentedMatrix.txt) 44 | add_test(PivotCalibrationParametersEstimatorTest pivotCalibrationParametersEstimatorTest 45 | ${LSQR_RECIPES_TESTING_DATA_ROOT}/pivotCalibrationData.txt) 46 | else() 47 | message(FATAL_ERROR "LSQR_RECIPES_TESTING_DATA_ROOT should be set to the testing data root directory.") 48 | endif() 49 | -------------------------------------------------------------------------------- /Cmake/Modules/FindLSQRRecipes.cmake: -------------------------------------------------------------------------------- 1 | # 2 | #Find a LSQRRecipes installation or build tree. 3 | #Module is based on the FindITK.cmake module. 4 | # 5 | 6 | #search only if the location is not already known. 7 | if(NOT LSQRRecipes_DIR) 8 | #get the system search path as a list. 9 | if(UNIX) 10 | string(REGEX MATCHALL "[^:]+" LSQRRecipes_DIR_SEARCH1 "$ENV{PATH}") 11 | else() 12 | string(REGEX REPLACE "\\\\" "/" LSQRRecipes_DIR_SEARCH1 "$ENV{PATH}") 13 | endif() 14 | string(REGEX REPLACE "/;" ";" LSQRRecipes_DIR_SEARCH2 ${LSQRRecipes_DIR_SEARCH1}) 15 | 16 | #construct a set of paths relative to the system search path. 17 | set(LSQRRecipes_DIR_SEARCH "") 18 | foreach(dir ${LSQRRecipes_DIR_SEARCH2}) 19 | set(LSQRRecipes_DIR_SEARCH ${LSQRRecipes_DIR_SEARCH} "${dir}/../lib/LSQRRecipes") 20 | endforeach(dir) 21 | 22 | # 23 | #search for an installation or build tree. 24 | # 25 | find_path(LSQRRecipes_DIR LSQRRecipesConfig.cmake 26 | #look for an environment variable LSQRRecipes_DIR 27 | $ENV{LSQRRecipes_DIR} 28 | 29 | #look in places relative to the system executable search path. 30 | ${LSQRRecipes_DIR_SEARCH} 31 | 32 | #look in standard UNIX install locations. 33 | /usr/local/lib/LSQRRecipes 34 | /usr/lib/LSQRRecipes 35 | 36 | #read from the CMakeSetup registry entries. It is likely that 37 | #LSQRRecipes will have been recently built. 38 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild1] 39 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild2] 40 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild3] 41 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild4] 42 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild5] 43 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild6] 44 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild7] 45 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild8] 46 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild9] 47 | [HKEY_CURRENT_USER\\Software\\Kitware\\CMakeSetup\\Settings\\StartPath;WhereBuild10] 48 | 49 | #help the user find it if we cannot. 50 | DOC "The directory containing LSQRRecipesConfig.cmake. This is either the root of the build tree, or the installation." 51 | ) 52 | endif() 53 | 54 | #if LSQRRecipes was found, load the configuration file to get the rest of the 55 | #settings. 56 | if(LSQRRecipes_DIR) 57 | set(LSQRRecipes_FOUND 1) 58 | include(${LSQRRecipes_DIR}/LSQRRecipesConfig.cmake) 59 | 60 | #set USE_LSQRRecipes_FILE for backward-compatability. 61 | set(USE_LSQRRecipes_FILE ${LSQRRecipes_USE_FILE}) 62 | else(LSQRRecipes_DIR) 63 | set(LSQRRecipes_FOUND 0) 64 | if(LSQRRecipes_FIND_REQUIRED) 65 | message(FATAL_ERROR "Please set LSQRRecipes_DIR to the directory containing LSQRRecipesConfig.cmake. This is either the root of the build tree, or the installation.") 66 | endif() 67 | endif() 68 | -------------------------------------------------------------------------------- /common/Ray3D.cxx: -------------------------------------------------------------------------------- 1 | #include "Ray3D.h" 2 | #include "Epsilon.h" 3 | 4 | namespace lsqrRecipes { 5 | 6 | bool Ray3D::intersection(Ray3D &r, Point3D &result) 7 | { 8 | const double INTERSECTION_EPSILON = EPS; 9 | Vector3D p21, n1Crossn2, tmp, tmp1, tmp2, n1, n2; 10 | double t, t1, t2; 11 | 12 | n1 = this->n; 13 | n2 = r.n; 14 | 15 | p21 = r.p - this->p; 16 | 17 | n1Crossn2 = crossProduct(n1,n2); 18 | double denominator = n1Crossn2*n1Crossn2; 19 | 20 | //rays are parallel 21 | if(denominator < INTERSECTION_EPSILON) { 22 | if((t=p21*n1) > 0) { 23 | tmp = n1*t; 24 | result[0] = (r.p[0]+ this->p[0]+tmp[0])/2.0; 25 | result[1] = (r.p[1]+ this->p[1]+tmp[1])/2.0; 26 | result[2] = (r.p[2]+ this->p[2]+tmp[2])/2.0; 27 | return true; 28 | } 29 | else if((t=-(p21*n2)) > 0) { 30 | tmp = n2*t; 31 | result[0] = (r.p[0]+ this->p[0]+tmp[0])/2.0; 32 | result[1] = (r.p[1]+ this->p[1]+tmp[1])/2.0; 33 | result[2] = (r.p[2]+ this->p[2]+tmp[2])/2.0; 34 | return true; 35 | } 36 | else return false; //rays are parallel but do not have a common support 37 | } 38 | //rays are not parallel (skew or intersecting) 39 | t1 = (n1Crossn2[0]*(p21[1]*n2[2] - p21[2]*n2[1]) - 40 | n1Crossn2[1]*(p21[0]*n2[2] - p21[2]*n2[0]) + 41 | n1Crossn2[2]*(p21[0]*n2[1] - p21[1]*n2[0]))/denominator; 42 | t2 = (n1Crossn2[0]*(p21[1]*n1[2] - p21[2]*n1[1]) - 43 | n1Crossn2[1]*(p21[0]*n1[2] - p21[2]*n1[0]) + 44 | n1Crossn2[2]*(p21[0]*n1[1] - p21[1]*n1[0]))/denominator; 45 | 46 | if(t1<0 || t2<0) 47 | return false; 48 | 49 | tmp1 = n1*t1; 50 | tmp2 = n2*t2; 51 | 52 | result[0] = (this->p[0] + tmp1[0] + r.p[0] + tmp2[0])/2.0; 53 | result[1] = (this->p[1] + tmp1[1] + r.p[1] + tmp2[1])/2.0; 54 | result[2] = (this->p[2] + tmp1[2] + r.p[2] + tmp2[2])/2.0; 55 | return true; 56 | } 57 | /*****************************************************************************/ 58 | double Ray3D::distance(Point3D &pnt) 59 | { 60 | double x0 = this->p[0]; 61 | double y0 = this->p[1]; 62 | double z0 = this->p[2]; 63 | double x3 = pnt[0]; 64 | double y3 = pnt[1]; 65 | double z3 = pnt[2]; 66 | 67 | double t = this->n[0]*(x3-x0) + this->n[1]*(y3-y0) + this->n[2]*(z3-z0); 68 | t/= ((this->n[0]*this->n[0]) + (this->n[1]*this->n[1]) + 69 | (this->n[2]*this->n[2])); 70 | 71 | double x = x0+t*this->n[0]; 72 | double y = y0+t*this->n[1]; 73 | double z = z0+t*this->n[2]; 74 | 75 | return sqrt((x-x3)*(x-x3) + (y-y3)*(y-y3) + (z-z3)*(z-z3)); 76 | } 77 | /*****************************************************************************/ 78 | void RayBundle::writeOIVData(std::ostream &out, double rayLength, double color[3]) 79 | { 80 | out<<"Separator {\n"; 81 | out<<"\tMaterial {\n"; 82 | out<<"\t\tdiffuseColor "<p[0]<<"\t"<p[1]<<"\t"<p[2]<<",\n"; 95 | int rayNum = static_cast(this->n.size()); 96 | int i; 97 | for(i=0; in[i]; 99 | out<<"\t\t\t"<p[0] + rayLength*v[0]<<"\t"<p[1] + rayLength*v[1]<<"\t"<p[2] + rayLength*v[2]<<",\n"; 100 | } 101 | out<<"\t\t]\t\n}\n"; 102 | out<<"\tIndexedLineSet {\n"; 103 | out<<"\t\tcoordIndex [\n"; 104 | for(i=0; in[0] = other.n[0]; this->n[1] = other.n[1]; this->n[2] = other.n[2]; 28 | this->p[0] = other.p[0]; this->p[1] = other.p[1]; this->p[2] = other.p[2]; 29 | } 30 | 31 | void transform(Frame &transformation) { 32 | transformation.apply(p); 33 | transformation.apply(n); 34 | } 35 | 36 | /** 37 | * This function computes the distance between a point and a ray (line). 38 | * The Ray is treated as a line , this means that I don't care that 39 | * the parameter t isn't in [0,infinity). 40 | * The parametric equation of the line defined by the ray (Point+direction) 41 | * is: 42 | * (*) P(t) = P0+tN 43 | * The closest point on the line P(t) to a given point P3 is on the line that 44 | * goes through P3 and is perpendicular to P. 45 | * This gives us: 46 | * (**) (P3-P(t_i)) dot N = 0; 47 | * Substituting (*) into (**) gives: 48 | * (x3-x0)*Nx + (y3-y0)*Ny + (z3-z0)*Nz 49 | * t_i= -------------------------------------- 50 | * Nx^2 + Ny^2 + Nz^2 51 | * 52 | * Now we have P(t_i) =P0+ t_iN and the distance between P3 and the 53 | * line is the distance between P3 and P(t_i). 54 | * 55 | */ 56 | double distance(Point3D &pnt); 57 | 58 | /** 59 | * Compute the intersection of two rays analytically. Based on the code 60 | * found in Graphics Gems p.304 "Intersection of two 3D lines", Ronald Goldman. 61 | * I add the definition that the intersection point of two parallel rays is 62 | * the point between them as shown in the following figure: 63 | * 64 | * r1(t)*--------------------------> 65 | * the intersection--> X 66 | * r2(s)*---------------> 67 | * The rays must have a common support. 68 | * One of the following is true: 69 | * 1.Projecting r1(0) onto r2 gives a point with a positive ray parameter value. 70 | * 2.Projecting r2(0) onto r1 gives a point with a positive ray parameter value. 71 | * 72 | */ 73 | bool intersection(Ray3D &r, Point3D &result); 74 | }; 75 | 76 | 77 | /** 78 | * Lightweight container for ray bundles. 79 | */ 80 | class RayBundle { 81 | public: 82 | RayBundle(int size) : n(size) {p[0]=p[1]=p[2]=0.0;} 83 | //the ray directions (not necessarily normalized) 84 | std::vector n; 85 | //the rays origin 86 | Point3D p; 87 | 88 | void transform(Frame &transformation) { 89 | transformation.apply(p); 90 | int numRays = static_cast(n.size()); 91 | for(int i=0; i 7 | #include "Vector.h" 8 | 9 | /** 10 | * Template defining nD points. Valid types for point entries are the numeric types that 11 | * can be reasonably cast to double (int, float, etc.). 12 | * 13 | * @author: Ziv Yaniv 14 | */ 15 | 16 | namespace lsqrRecipes { 17 | 18 | template 19 | class Point { 20 | public: 21 | //enable compile time access to the point dimension, ugly but necessary 22 | enum {dimension=n}; 23 | 24 | /** 25 | * Output the point data to the given output stream as the following string: 26 | *[x1,x2,...xn] 27 | */ 28 | friend std::ostream &operator<<(std::ostream& output, const Point &p) { 29 | output<<"[ "< &operator =(const Point &other) {memcpy(data,other.data,n*sizeof(T)); return *this;} 44 | 45 | /** 46 | * Access to the coordinates of this point. No bounds checking is performed. 47 | * Valid indexes are in [0,n-1]. 48 | */ 49 | double & operator[](int index) {return this->data[index];} 50 | const double & operator[](int index) const {return this->data[index];} 51 | 52 | /** 53 | * Default constructor, sets the point to zero. 54 | */ 55 | Point(){memset(this->data,0,n*sizeof(T));} 56 | 57 | /** 58 | * Construct a point from the given pointer to an array. 59 | * @param fillData An array that has at least 'n' elements. 60 | */ 61 | Point(T *fillData) {memcpy(this->data,fillData,n*sizeof(T));} 62 | 63 | /** 64 | * Copy constructor. 65 | * @param other The point we copy. 66 | */ 67 | Point(const Point &other) {memcpy(this->data,other.data,n*sizeof(T));} 68 | 69 | /** 70 | * Return the distance between this and the given point. 71 | */ 72 | double distanceSquared(const Point &other) { 73 | return (this->getVnlVector() - other.getVnlVector()).squared_magnitude(); 74 | } 75 | 76 | /** 77 | * Fill the point from the given pointer to an array. 78 | * @param fillData An array that has at least 'n' elements. 79 | */ 80 | void set(T *fillData) {memcpy(this->data,fillData,n*sizeof(T));} 81 | 82 | /** 83 | * Return the dimensionality of this point. 84 | */ 85 | unsigned int size() {return n;} 86 | 87 | /** 88 | * Add a vector to the current point. 89 | * @param v The point is translated using this vector. 90 | */ 91 | Point operator+(const Vector &v) { 92 | Point result(*this); 93 | for(unsigned int i=0; i &v) { 103 | Point result(*this); 104 | for(unsigned int i=0; i operator-(const Point &p) { 114 | Vector result(this->data); 115 | for(unsigned int i=0; i getVnlVector() const {return vnl_vector_ref(n, const_cast(this->data));} 126 | 127 | T data[n]; 128 | }; 129 | 130 | } //namespace lsqrRecipes 131 | 132 | #endif //_POINT_H_ 133 | -------------------------------------------------------------------------------- /parametersEstimators/Line2DParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _LINE2D_PARAMETERS_ESTIMATOR_H_ 2 | #define _LINE2D_PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include "ParametersEstimator.h" 7 | #include "Point2D.h" 8 | 9 | /** 10 | * This class estimates the parameters of 2D lines. 11 | * It uses exactly the same approach as that used for hyperplane estimation. The 12 | * only reason to use this class instead of the PlaneParametersEstimator with 13 | * dimension 2 is that you can easily rip it out of the Least Squares Recipes 14 | * library and incorporate it into your codebase (it does not require an 15 | * external library to compute the eigenvectors as required by the more generic 16 | * hyperplane estimation class). 17 | * 18 | * Notes: The parameterization is the same as that of the hyperplane [n,a] where 19 | * 'n' is the normal to the line (unique only in 2D), and 'a' is a point 20 | * on the line. This is different from the parameterization used for the 21 | * kD line where 'n' is the direction parallel to the line. 22 | * 23 | * The unit test for this class is incorporated into the unit test of the 24 | * more generic LineParametersEstimator. 25 | * 26 | * Author: Ziv Yaniv 27 | */ 28 | 29 | namespace lsqrRecipes { 30 | 31 | class Line2DParametersEstimator : public ParametersEstimator { 32 | public: 33 | /** 34 | * Object constructor. 35 | * @param delta A point is on the line if its distance from the line is less 36 | * than 'delta'. 37 | */ 38 | Line2DParametersEstimator(double delta); 39 | 40 | /** 41 | * Compute the line defined by the given data points. 42 | * @param data A vector containing two 2D points. 43 | * @param This vector is cleared and then filled with the computed parameters. 44 | * The parameters of the line passing through these points [n_x,n_y,a_x,a_y] 45 | * where ||(n_x,ny)|| = 1. 46 | * If the vector contains less than two points or the points are coincident 47 | * (closer than the user specified delta) then the resulting parameters 48 | * vector is empty (size = 0). 49 | */ 50 | virtual void estimate(std::vector &data, std::vector ¶meters); 51 | virtual void estimate(std::vector &data, std::vector ¶meters); 52 | 53 | /** 54 | * Compute a least squares estimate of the line defined by the given points. 55 | * This implementation is of an orthogonal least squares error. 56 | * 57 | * @param data The line should minimize the least squares error to these points. 58 | * @param parameters This vector is cleared and then filled with the computed parameters. 59 | * Fill this vector with the computed line parameters [n_x,n_y,a_x,a_y] 60 | * where ||(n_x,ny)|| = 1. 61 | * If the vector contains less than two points or all the points are coincident 62 | * then the resulting parameters vector is empty (size = 0). 63 | */ 64 | virtual void leastSquaresEstimate(std::vector &data, std::vector ¶meters); 65 | virtual void leastSquaresEstimate(std::vector &data, std::vector ¶meters); 66 | 67 | /** 68 | * Return true if the distance between the line defined by the parameters and the 69 | * given point is smaller than 'delta' (see constructor). 70 | * @param parameters The line parameters [n_x,n_y,a_x,a_y]. 71 | * @param data Check that the distance between this point and the line is smaller than 'delta'. 72 | */ 73 | virtual bool agree(std::vector ¶meters, Point2D &data); 74 | 75 | /** 76 | * Change the distance defining if a point is on the line or not. 77 | * @param delta A point is on the line if its distance from it is less than 'delta'. 78 | */ 79 | void setDelta(double delta) {this->deltaSquared = delta*delta;} 80 | 81 | private: 82 | //given line L and point P, if dist(L,P)^2 < delta^2 then the point is on the line 83 | double deltaSquared; 84 | }; 85 | 86 | } //namespace lsqrRecipes 87 | 88 | #endif //_LINE2D_PARAMETERS_ESTIMATOR_H_ -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | PROJECT( RANSAC_EXAMPLES ) 4 | 5 | option(USING_ITK_VNL "Build using ITK version of VNL." OFF) 6 | 7 | 8 | # 9 | # we use vnl from the vxl package to do all of our linear algebra and nonlinear 10 | # optimization algorithms, either through ITK or directly through VXL 11 | # 12 | if(USING_ITK_VNL) 13 | # 14 | #remove the VXL_DIR from the cache, we are working with the ITK version 15 | #of vnl 16 | # 17 | unset(VXL_DIR CACHE) 18 | 19 | # 20 | # Using vnl through ITK, need to find ITK 21 | # 22 | find_package(ITK REQUIRED) 23 | if(ITK_FOUND) 24 | include(${ITK_USE_FILE}) 25 | set(LINK_LIBRARIES 26 | LSQRRecipes 27 | itkvnl 28 | itkvnl_algo 29 | itkv3p_netlib 30 | itkvcl 31 | itkv3p_lsqr 32 | ) 33 | endif() 34 | else() 35 | # 36 | #remove the ITK_DIR from the cache, we are working with the VXL version of 37 | #vnl 38 | # 39 | unset(ITK_DIR CACHE) 40 | 41 | # 42 | # Using vnl through VXL, need to find VXL 43 | # 44 | find_package(VXL REQUIRED) 45 | if(VXL_FOUND) 46 | include(${VXL_CMAKE_DIR}/UseVXL.cmake) 47 | set(LINK_LIBRARIES 48 | LSQRRecipes 49 | vnl 50 | vnl_algo 51 | netlib 52 | v3p_netlib 53 | vcl 54 | ) 55 | endif() 56 | endif() 57 | 58 | 59 | # 60 | # get the settings required for using the LSQRRecipes library 61 | # 62 | find_package(LSQRRecipes) 63 | if(LSQRRecipes_FOUND) 64 | include(${LSQRRecipes_USE_FILE}) 65 | endif() 66 | 67 | 68 | # 69 | # Example program showing the use of RANSAC for line fitting in 3D 70 | # 71 | add_executable(lineEstimation lineEstimation.cxx) 72 | target_link_libraries(lineEstimation ${LINK_LIBRARIES}) 73 | 74 | # 75 | # Example program showing the use of RANSAC for plane fitting 76 | # 77 | add_executable(planeEstimation planeEstimation.cxx) 78 | target_link_libraries(planeEstimation ${LINK_LIBRARIES}) 79 | 80 | 81 | # 82 | # Example program showing the use of RANSAC for sphere fitting 83 | # 84 | add_executable(sphereEstimation sphereEstimation.cxx) 85 | target_link_libraries(sphereEstimation ${LINK_LIBRARIES}) 86 | 87 | 88 | # 89 | # Example program showing the use of RANSAC for estimating the 90 | # intersection of multiple 3D rays 91 | # 92 | add_executable(rayIntersectionEstimation rayIntersectionEstimation.cxx) 93 | target_link_libraries(rayIntersectionEstimation ${LINK_LIBRARIES}) 94 | 95 | 96 | # 97 | # Example program showing the use of RANSAC for estimating the 98 | # calibration parameters of an US using a crosswire phantom 99 | # 100 | add_executable(crosswireUSCalibration crosswireUSCalibration.cxx) 101 | target_link_libraries(crosswireUSCalibration ${LINK_LIBRARIES}) 102 | 103 | 104 | # 105 | # Example program showing the use of RANSAC for estimating the 106 | # calibration parameters of an US using a calibrated pointer 107 | # 108 | add_executable(pointerUSCalibration pointerUSCalibration.cxx) 109 | target_link_libraries(pointerUSCalibration ${LINK_LIBRARIES}) 110 | 111 | 112 | # 113 | # Example program showing the use of RANSAC for estimating the 114 | # calibration parameters of an US using a plane phantom 115 | # 116 | add_executable(planeUSCalibration planeUSCalibration.cxx) 117 | target_link_libraries(planeUSCalibration ${LINK_LIBRARIES}) 118 | 119 | 120 | # 121 | # Example program showing the use of the exhaustive search option in the RANSAC 122 | # class for absolute orientation when using a small number of paired points 123 | # 124 | add_executable(absoluteOrientation absoluteOrientation.cxx) 125 | target_link_libraries(absoluteOrientation ${LINK_LIBRARIES}) 126 | 127 | # 128 | # Example program showing the use of RANSAC for solving a linear equation 129 | # system 130 | # 131 | add_executable(linearEquationSystemSolver linearEquationSystemSolver.cxx) 132 | target_link_libraries(linearEquationSystemSolver ${LINK_LIBRARIES}) 133 | 134 | # 135 | # Example program showing the use of RANSAC for pivot calibration 136 | # 137 | add_executable(pivotCalibration pivotCalibration.cxx) 138 | target_link_libraries(pivotCalibration ${LINK_LIBRARIES}) 139 | -------------------------------------------------------------------------------- /parametersEstimators/PlaneParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLANE_PARAMETERS_ESTIMATOR_H_ 2 | #define _PLANE_PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include "ParametersEstimator.h" 7 | #include "Point.h" 8 | 9 | /** 10 | * This class estimates the parameters of a hyperplane (in 2D this is a line, 11 | * in 3D a plane...). 12 | * 13 | * A hyperplane is represented as: (*) dot(n,p-a)=0 14 | * where n is the hyperplane normal (|n| = 1) and 'a' is a 15 | * point on the hyperplane. 16 | * All points 'p' which satisfy equation (*) are on the hyperplane. 17 | * 18 | * @author: Ziv Yaniv 19 | * 20 | */ 21 | namespace lsqrRecipes { 22 | 23 | template< unsigned int dimension > 24 | class PlaneParametersEstimator : public ParametersEstimator< Point,double> { 25 | public: 26 | /** 27 | * Object constructor. 28 | * @param delta A point is on the hyperplane if its distance from the 29 | * hyperplane is less than 'delta'. 30 | */ 31 | PlaneParametersEstimator(double delta); 32 | 33 | /** 34 | * Compute the hyperplane defined by the given data points. 35 | * @param data A vector containing k kD points. 36 | * @param This vector is cleared and then filled with the computed parameters. 37 | * The parameters of the plane passing through these points 38 | * [n_0,...,n_k,a_0,...,a_k] where ||(n_0,...,n_k)|| = 1. 39 | * If the vector contains less than k points or the points are linearly 40 | * dependent then the resulting parameters vector is empty (size = 0). 41 | */ 42 | virtual void estimate(std::vector< Point *> &data, 43 | std::vector ¶meters); 44 | virtual void estimate(std::vector< Point > &data, 45 | std::vector ¶meters); 46 | 47 | /** 48 | * Compute a least squares estimate of the hyperplane defined by the given points. 49 | * This implementation is of an orthogonal least squares error. 50 | * 51 | * @param data The hyperplane should minimize the least squares error to these 52 | * points. 53 | * @param parameters This vector is cleared and then filled with the computed 54 | * parameters. Fill this vector with the computed hyperplane 55 | * parameters [n_0,...,n_k,a_0,...,a_k] where ||(n_0,...,n_k)|| = 1. 56 | * If the vector contains less than two points or all the 57 | * points are coincident then the resulting parameters 58 | * vector is empty (size = 0). 59 | */ 60 | virtual void leastSquaresEstimate(std::vector< Point *> &data, 61 | std::vector ¶meters); 62 | virtual void leastSquaresEstimate(std::vector< Point > &data, 63 | std::vector ¶meters); 64 | 65 | /** 66 | * Return true if the distance between the hyperplane defined by the parameters and 67 | * the given point is smaller than 'delta' (see constructor). 68 | * @param parameters The hyperplane parameters [n_0,...,n_k,a_0,...,a_k]. 69 | * @param data Check that the distance between this point and the hyperplane is 70 | * less than 'delta'. 71 | * 72 | * Note:If the parameters vector is too short the method will throw an 73 | * exception as it tries to access a non existing entry. 74 | * 75 | */ 76 | virtual bool agree(std::vector ¶meters, Point &data); 77 | 78 | /** 79 | * Change the distance defining if a point is on the hyperplane or not. 80 | * @param delta A point is on the hyperplane if its distance from it is less 81 | * than 'delta'. 82 | */ 83 | void setDelta(double delta) {this->deltaSquared = delta*delta;} 84 | 85 | private: 86 | //given hyperplane H(a,n) and point P, if dist(H,P)^2 < delta^2 then the 87 | //point is on the hyperplane 88 | double deltaSquared; 89 | }; 90 | 91 | } //namespace lsqrRecipes 92 | 93 | #include "PlaneParametersEstimator.hxx" //the implementation is in this file 94 | 95 | #endif //_PLANE_PARAMETERS_ESTIMATOR_H_ 96 | -------------------------------------------------------------------------------- /testing/PivotCalibrationParametersEstimatorTest.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "PivotCalibrationParametersEstimator.h" 4 | 5 | using namespace lsqrRecipes; 6 | 7 | int main(int argc, char *argv[]) 8 | { 9 | if(argc != 2) { 10 | std::cerr<<"Unexpected number of command line parameters.\n"; 11 | std::cerr<<"Usage: \n"; 12 | std::cerr<<"\t"< transformations; 18 | Frame f; 19 | unsigned int i; 20 | const unsigned int PARAMETER_NUM = 6; 21 | 22 | //load the data, each row in the file is expected to have the 23 | //following format x y z qx qy qz qs 24 | std::ifstream in; 25 | 26 | in.open(argv[1]); 27 | if(!in.is_open()) 28 | return EXIT_FAILURE; 29 | while(in>> x >> y >> z >> qx >> qy >> qz >> qs) { 30 | f.setRotationQuaternion(qs, qx, qy, qz); 31 | f.setTranslation(x, y, z); 32 | transformations.push_back(f); 33 | } 34 | in.close(); 35 | 36 | if(transformations.empty()) { 37 | std::cerr<<"Failed to load data from pivot calibration file.\n"; 38 | return EXIT_FAILURE; 39 | } 40 | 41 | //configure the estimator 42 | double maxError = 1.0; //distance between the two points is at most 1mm 43 | std::vector estimatedTranslations; 44 | PivotCalibrationEstimator pivotCalibration(maxError); 45 | 46 | //test the exact estimate method 47 | double knownExactTranslations[] = {-18.586, 1.98134, -157.439, 48 | 146.965, -62.0497, -1042.87}; 49 | 50 | std::vector minTransformations(3); 51 | minTransformations[0] = transformations[0]; 52 | minTransformations[1] = transformations[static_cast(transformations.size()/2.0)]; 53 | minTransformations[2] = transformations[transformations.size()-1]; 54 | 55 | pivotCalibration.estimate(minTransformations, estimatedTranslations); 56 | if(estimatedTranslations.empty()) 57 | return EXIT_FAILURE; 58 | 59 | std::cout<<"Exact estimate\n"; 60 | std::cout<<"--------------\n"; 61 | std::cout<<"Known translations [DRF^t, W^t]:\n\t [ "; 62 | for(i=0; i 5 | #include 6 | #include "copyright.h" 7 | 8 | 9 | /** 10 | * Template defining nD vectors. Valid types for vector entries are the numeric 11 | * types that can be reasonably cast to double (int, float, etc.). 12 | * 13 | * @author: Ziv Yaniv 14 | */ 15 | 16 | namespace lsqrRecipes { 17 | 18 | template 19 | class Vector { 20 | public: 21 | //enable compile time access to the point dimension, ugly but necessary 22 | enum {dimension=n}; 23 | 24 | /** 25 | * Ouput the vector data to the given output stream as the following string: 26 | *[x1,x2,...xn] 27 | */ 28 | friend std::ostream &operator<<(std::ostream& output, const Vector &v) { 29 | output<<"[ "< &operator =(const Vector &other) {memcpy(data,other.data,n*sizeof(T)); return *this;} 44 | 45 | /** 46 | * Access to the coordinates of this vector. No bounds checking is performed. 47 | * Valid indexes are in [0,n-1]. 48 | */ 49 | double & operator[](int index) {return this->data[index];} 50 | const double & operator[](int index) const {return this->data[index];} 51 | 52 | /** 53 | * Default constructor, sets the vector to zero. 54 | */ 55 | Vector(){memset(this->data,0,n*sizeof(T));} 56 | 57 | /** 58 | * Construct a vector from the given pointer to an array. 59 | * @param fillData An array that has at least 'n' elements. 60 | */ 61 | Vector(T *fillData) {memcpy(this->data,fillData,n*sizeof(T));} 62 | 63 | /** 64 | * Copy constructor. 65 | * @param other The vector we copy. 66 | */ 67 | Vector(const Vector &other) {memcpy(this->data,other.data,n*sizeof(T));} 68 | 69 | /** 70 | * Multiply vector by scalar. 71 | * @param scalar The vector is scaled with this value. 72 | */ 73 | Vector operator*(const T & scalar) const 74 | { 75 | Vector result(*this); 76 | for(unsigned int i=0; idata,fillData,n*sizeof(T));} 125 | 126 | /** 127 | * Return the dimensionality of this vector. 128 | */ 129 | unsigned int size() {return n;} 130 | 131 | /** 132 | * Compute the l2 norm of the vector. 133 | */ 134 | T l2Norm() { 135 | T result = 0; 136 | for(unsigned int i=0; il2Norm(); 146 | for(unsigned int i=0; i getVnlVector() const {return vnl_vector_ref(n, const_cast(this->data));} 156 | 157 | T data[n]; 158 | }; 159 | 160 | 161 | template 162 | inline Vector operator*(const T & s, const Vector & v) {return v*s;} 163 | 164 | } //namespace lsqrRecipes 165 | 166 | #endif //_VECTOR_H_ 167 | -------------------------------------------------------------------------------- /parametersEstimators/PivotCalibrationParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _PIVOT_CALIBRATION_PARAMETERS_ESTIMATOR_H_ 2 | #define _PIVOT_CALIBRATION_PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include "ParametersEstimator.h" 7 | #include "Frame.h" 8 | 9 | /** 10 | * This class performs pivot calibration using an algebraic method which only 11 | * requires solving a single set of linear equations. It is based on the 12 | * observation that when we are pivoting around a fixed point all transformations 13 | * yield the following three equations: 14 | * R_i*{DRF}^t + t_i = W^t 15 | * 16 | * where [R_i,t_i] is the transformation from the tracking system to the Dynamic 17 | * Reference Frame (DRF), {DRF}^t is the translation from the DRF to the pivoting 18 | * point and W^t is the translation from the tracking system to the pivoting point. 19 | * To obtain {DRF}^t and W^t we solve the overdetermined equation system: 20 | * [R_1 -I] [{DRF}^t] [-t_1] 21 | * [ : ] [ ] = [ : ] 22 | * [R_m -I] [ W^t ] [-t_m] 23 | * 24 | * For additional details see: 25 | * Z. Yaniv, "Which Pivot Calibration?", SPIE Medical Imaging: Image-Guided 26 | * Procedures, Robotic Interventions, and Modeling", 2015. 27 | * 28 | * @author: Ziv Yaniv 29 | */ 30 | namespace lsqrRecipes { 31 | 32 | class PivotCalibrationEstimator : public ParametersEstimator< Frame ,double> { 33 | public: 34 | 35 | /** 36 | * Object constructor. 37 | * @param delta A transformation is consistent with a given estimate of 38 | * [{DRF}^t, W^t] if ||R*{DRF}^t + t - W^t|| &data, 52 | std::vector ¶meters); 53 | virtual void estimate(std::vector< Frame > &data, 54 | std::vector ¶meters); 55 | 56 | /** 57 | * Compute a least squares estimate of the two translation vectors defined by 58 | * the given transformations. 59 | * @param data A vector containing three or more transformations. 60 | * @param parameters This vector is cleared and then filled with the values, 61 | * [{DRF}^t_x, {DRF}^t_y, {DRF}^t_z, W^t_x, W^t_y, W^t_z]). 62 | * If the data contains less than three transformations or 63 | * the equation system that they define is singular then the 64 | * parameters vector is empty (size = 0). 65 | */ 66 | virtual void leastSquaresEstimate(std::vector< Frame *> &data, 67 | std::vector ¶meters); 68 | virtual void leastSquaresEstimate(std::vector< Frame > &data, 69 | std::vector ¶meters); 70 | 71 | /** 72 | * Return true if the given transformation is consistent with the translations 73 | * defined by the parameters, error is smaller than 'delta' (see constructor). 74 | * 75 | * @param parameters The translations [{DRF}^t, W^t]. 76 | * @param data Check that the distance between this point and the hyperplane is 77 | * less than 'delta'. 78 | * @param delta A transformation is consistent with a given estimate of 79 | * [{DRF}^t, W^t] if ||R*{DRF}^t + t - W^t|| ¶meters, Frame &data); 82 | 83 | /** 84 | * Change the error defining if a transformation is consistent with the given 85 | * parameter values. 86 | * @param delta A transformation is consistent with a given estimate of 87 | * [{DRF}^t, W^t] if ||R*{DRF}^t + t - W^t||delta = delta;} 89 | 90 | private: 91 | //given a transformation/frame and the two translations estimated by pivot 92 | //calibration the estimated translations are consistent with the 93 | //transformation if || R{DRF}^t + t - W^t || 35 | class LineParametersEstimator : public ParametersEstimator< Point,double> { 36 | public: 37 | /** 38 | * Object constructor. 39 | * @param delta A point is on the line if its distance from the line is less 40 | * than 'delta'. 41 | */ 42 | LineParametersEstimator(double delta); 43 | 44 | /** 45 | * Compute the line defined by the given data points. 46 | * @param data A vector containing two kD points. 47 | * @param This vector is cleared and then filled with the computed parameters. 48 | * The parameters of the line passing through these points 49 | * [n_0,...,n_k,a_0,...,a_k] where ||(n_0,...,nk)|| = 1. 50 | * If the vector contains less than two points or the points are 51 | * too close (distance between them is less than 'delta') 52 | * then the resulting parameters vector is empty (size = 0). 53 | */ 54 | virtual void estimate(std::vector< Point *> &data, 55 | std::vector ¶meters); 56 | virtual void estimate(std::vector< Point > &data, 57 | std::vector ¶meters); 58 | 59 | /** 60 | * Compute a least squares estimate of the line defined by the given points. 61 | * This implementation is of an orthogonal least squares error. 62 | * 63 | * @param data The line should minimize the least squares error to these points. 64 | * @param parameters This vector is cleared and then filled with the computed parameters. 65 | * Fill this vector with the computed line parameters [n,a] 66 | * where ||n|| = 1. 67 | * If the vector contains less than two points or all the points are coincident 68 | * then the resulting parameters vector is empty (size = 0). 69 | */ 70 | virtual void leastSquaresEstimate(std::vector< Point *> &data, 71 | std::vector ¶meters); 72 | virtual void leastSquaresEstimate(std::vector< Point > &data, 73 | std::vector ¶meters); 74 | 75 | /** 76 | * Return true if the distance between the line defined by the parameters and the 77 | * given point is smaller than 'delta' (see constructor). 78 | * @param parameters The line parameters [n,a]. 79 | * @param data Check that the distance between this point and the line is smaller than 'delta'. 80 | * 81 | * Note:If the parameters vector is too short the method will throw an 82 | * exception as it tries to access a non existing entry. 83 | * 84 | */ 85 | virtual bool agree(std::vector ¶meters, Point &data); 86 | 87 | /** 88 | * Change the distance defining if a point is on the line or not. 89 | * @param delta A point is on the line if its distance from it is less 90 | * than 'delta'. 91 | */ 92 | void setDelta(double delta) {this->deltaSquared = delta*delta;} 93 | 94 | private: 95 | //given line L and point P, if dist(L,P)^2 < delta^2 then the point 96 | //is on the line 97 | double deltaSquared; 98 | }; 99 | 100 | } //namespace lsqrRecipes 101 | 102 | #include "LineParametersEstimator.hxx" //the implementation is in this file 103 | 104 | #endif //_LINE_PARAMETERS_ESTIMATOR_H_ 105 | -------------------------------------------------------------------------------- /parametersEstimators/Line2DParametersEstimator.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Line2DParametersEstimator.h" 3 | 4 | namespace lsqrRecipes { 5 | 6 | Line2DParametersEstimator::Line2DParametersEstimator(double delta) : ParametersEstimator(2) {this->deltaSquared = delta*delta;} 7 | /*****************************************************************************/ 8 | /* 9 | * Compute the line parameters [n_x,n_y,a_x,a_y] 10 | */ 11 | void Line2DParametersEstimator::estimate(std::vector &data, 12 | std::vector ¶meters) 13 | { 14 | parameters.clear(); 15 | //not enough data elements for computation 16 | if(data.size()minForEstimate) 17 | return; 18 | 19 | double nx = (*data[1])[1] - (*data[0])[1]; 20 | double ny = (*data[0])[0] - (*data[1])[0]; 21 | double normSquared = nx*nx + ny*ny; 22 | //the two points are too close to each other 23 | if(normSquareddeltaSquared) 24 | return; 25 | double norm = sqrt(nx*nx + ny*ny); 26 | 27 | 28 | parameters.push_back(nx/norm); 29 | parameters.push_back(ny/norm); 30 | parameters.push_back((*data[0])[0]); 31 | parameters.push_back((*data[0])[1]); 32 | } 33 | /*****************************************************************************/ 34 | /* 35 | * Compute the line parameters [n_x,n_y,a_x,a_y] 36 | */ 37 | void Line2DParametersEstimator::estimate(std::vector &data, 38 | std::vector ¶meters) 39 | { 40 | std::vector usedData; 41 | int dataSize = data.size(); 42 | for(int i=0; i &data, 51 | std::vector ¶meters) 52 | { 53 | double meanX, meanY, nx, ny, norm; 54 | double covMat11, covMat12, covMat21, covMat22; // The entries of the symmetric covariance matrix 55 | int i, dataSize = data.size(); 56 | 57 | parameters.clear(); 58 | if(data.size()minForEstimate) 59 | return; 60 | 61 | meanX = meanY = 0.0; 62 | covMat11 = covMat12 = covMat21 = covMat22 = 0; 63 | for(i=0; i &data, 106 | std::vector ¶meters) 107 | { 108 | std::vector usedData; 109 | int dataSize = data.size(); 110 | for(int i=0; i ¶meters, Point2D &data) 120 | { 121 | double signedDistance = parameters[0]*(data[0]-parameters[2]) + parameters[1]*(data[1]-parameters[3]); 122 | return ((signedDistance*signedDistance) < this->deltaSquared); 123 | } 124 | 125 | } //namespace lsqrRecipes -------------------------------------------------------------------------------- /parametersEstimators/PivotCalibrationParametersEstimator.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Epsilon.h" 3 | #include "PivotCalibrationParametersEstimator.h" 4 | 5 | namespace lsqrRecipes { 6 | 7 | PivotCalibrationEstimator::PivotCalibrationEstimator(double delta) : ParametersEstimator< Frame, double >(3) {this->delta = delta;} 8 | /*****************************************************************************/ 9 | void PivotCalibrationEstimator::estimate(std::vector< Frame *> &data, 10 | std::vector ¶meters) 11 | { 12 | parameters.clear(); 13 | //not enough data elements for computation 14 | if(data.size()minForEstimate) 15 | return; 16 | 17 | double minusIValues[] = {-1.0, 0.0, 0.0, 0.0,-1.0, 0.0, 0.0, 0.0, -1.0}; 18 | vnl_matrix A(9,6), R(3,3), minusI(minusIValues,3,3); 19 | vnl_vector x(6), b(9), t(3); 20 | 21 | 22 | data[0]->getRotationMatrix(R); 23 | A.update(R,0,0); 24 | A.update(minusI,0,3); 25 | data[0]->getTranslation(t); 26 | b.update(-t,0); 27 | 28 | data[1]->getRotationMatrix(R); 29 | A.update(R,3,0); 30 | A.update(minusI,3,3); 31 | data[1]->getTranslation(t); 32 | b.update(-t,3); 33 | 34 | data[2]->getRotationMatrix(R); 35 | A.update(R,6,0); 36 | A.update(minusI,6,3); 37 | data[2]->getTranslation(t); 38 | b.update(-t,6); 39 | 40 | vnl_matrix_inverse Ainv(A); 41 | //explicitly zero out small singular values 42 | //this is ugly as it exposes that the inverse is computed via SVD 43 | Ainv.zero_out_absolute(EPS); 44 | 45 | if(Ainv.rank()<6) //matrix is not invertible 46 | return; 47 | x = Ainv * b; 48 | 49 | for(unsigned int i=0; i<6; i++) 50 | parameters.push_back(x[i]); 51 | } 52 | /*****************************************************************************/ 53 | void PivotCalibrationEstimator::estimate(std::vector< Frame > &data, 54 | std::vector ¶meters) 55 | { 56 | std::vector< Frame *> usedData; 57 | unsigned int dataSize = static_cast(data.size()); 58 | for(unsigned int i=0; i &data, 64 | std::vector ¶meters) 65 | { 66 | parameters.clear(); 67 | //not enough data elements for computation 68 | if(data.size()minForEstimate) 69 | return; 70 | 71 | unsigned int n = data.size(); 72 | double minusIValues[] = {-1.0, 0.0, 0.0, 0.0,-1.0, 0.0, 0.0, 0.0, -1.0}; 73 | vnl_matrix A(3*n,6), R(3,3), minusI(minusIValues,3,3); 74 | vnl_vector x(6), b(3*n), t(3); 75 | 76 | 77 | for(unsigned int i=0; igetRotationMatrix(R); 79 | A.update(R,3*i,0); 80 | A.update(minusI,3*i,3); 81 | data[i]->getTranslation(t); 82 | b.update(-t,3*i); 83 | } 84 | 85 | vnl_matrix_inverse Ainv(A); 86 | //explicitly zero out small singular values 87 | //this is ugly as it exposes that the inverse is computed via SVD 88 | Ainv.zero_out_absolute(EPS); 89 | 90 | if(Ainv.rank()<6) //we don't have a unique solution to the normal equations 91 | return; 92 | x = Ainv * b; 93 | for(unsigned int i=0; i<6; i++) 94 | parameters.push_back(x[i]); 95 | 96 | } 97 | /*****************************************************************************/ 98 | void PivotCalibrationEstimator::leastSquaresEstimate(std::vector< Frame > &data, 99 | std::vector ¶meters) 100 | { 101 | std::vector< Frame *> usedData; 102 | unsigned int dataSize = static_cast(data.size()); 103 | for(unsigned int i=0; i ¶meters, Frame &data) 109 | { 110 | Point3D tDRF, tW; 111 | 112 | tDRF[0] = parameters[0]; 113 | tDRF[1] = parameters[1]; 114 | tDRF[2] = parameters[2]; 115 | tW[0] = parameters[3]; 116 | tW[1] = parameters[4]; 117 | tW[2] = parameters[5]; 118 | 119 | //apply the transformation to the point, result is written in place 120 | data.apply(tDRF); 121 | //distance between the two points is less than the threshold 122 | return (tDRF - tW).l2Norm()delta; 123 | } 124 | 125 | } //namespace lsqrRecipes 126 | -------------------------------------------------------------------------------- /testing/RayIntersectionParametersTest.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include "RandomNumberGenerator.h" 3 | #include "Ray3D.h" 4 | #include "RayIntersectionParametersEstimator.h" 5 | 6 | 7 | /* 8 | * Test the ray intersection methods. 9 | */ 10 | int main(int argc, char *argv[]) 11 | { 12 | std::vector rayData; 13 | std::vector noNoise; 14 | 15 | lsqrRecipes::Ray3D ray; 16 | unsigned int i; 17 | //number of rays that intersect at the same point 18 | const unsigned int NUM_RAYS = 10; 19 | //noise is distributed IID ~N(0,noiseSigma) 20 | const double NOISE_SIGMA = 20.0; 21 | //all data is generated in [-maxRange,maxRange]X[-maxRange,maxRange]X[-maxRange,maxRange] 22 | double maxRange =1000.0; 23 | 24 | lsqrRecipes::Point3D knownIntersectionPoint; 25 | 26 | lsqrRecipes::RandomNumberGenerator random; 27 | //randomly select intersection point 28 | knownIntersectionPoint[0] = random.uniform(-maxRange,maxRange); 29 | knownIntersectionPoint[1] = random.uniform(-maxRange,maxRange); 30 | knownIntersectionPoint[2] = random.uniform(-maxRange,maxRange); 31 | 32 | //randomly create rays intersecting at approximatly the given point 33 | for(i=0; i estimatedIntersectionPoint; 63 | double maxDistanceToRay = 0.5; 64 | lsqrRecipes::RayIntersectionParametersEstimator ripEstimator(maxDistanceToRay); 65 | 66 | //The known intersection point 67 | std::cout<<"Known intersection point [x,y,z]:\n\t [ "< maxDistanceToRay) 93 | return EXIT_FAILURE; 94 | } 95 | //least squares estimate of the intersection point 96 | ripEstimator.leastSquaresEstimate(rayData, estimatedIntersectionPoint); 97 | if(estimatedIntersectionPoint.size() == 0) 98 | std::cout<<"Intersection point using least squares [x,y,z]: DEGENERATE CONFIGURATION\n\n"; 99 | else { 100 | std::cout<<"Intersection point using least squares [x,y,z]:\n\t [ "; 101 | std::cout< 5 | #include "Epsilon.h" 6 | 7 | namespace lsqrRecipes { 8 | 9 | template 10 | DenseLinearEquationSystemParametersEstimator::DenseLinearEquationSystemParametersEstimator( T delta ) : 11 | ParametersEstimator< AugmentedRow, T>(n) 12 | { 13 | this->delta = delta; 14 | } 15 | /*****************************************************************************/ 16 | template 17 | void DenseLinearEquationSystemParametersEstimator::estimate( 18 | std::vector< AugmentedRow *> &data, 19 | std::vector ¶meters ) 20 | { 21 | vnl_matrix A(n,n); 22 | vnl_vector x(n), b(n); 23 | T rowData[n], bValue; 24 | unsigned int i, numRows = static_cast(data.size()); 25 | 26 | parameters.clear(); 27 | //not enough data elements for computation 28 | if(numRowsminForEstimate) 29 | return; 30 | 31 | //set up the equation system 32 | for(i=0; iget(rowData, bValue); 34 | A.set_row(i, rowData); 35 | b[i] = bValue; 36 | } 37 | 38 | vnl_matrix_inverse Ainv(A); 39 | //explicitly zero out small singular values 40 | //this is ugly as it exposes that the inverse is computed via SVD 41 | Ainv.zero_out_absolute(EPS); 42 | 43 | if(Ainv.rank() 52 | void DenseLinearEquationSystemParametersEstimator::estimate( 53 | std::vector< AugmentedRow > &data, 54 | std::vector ¶meters ) 55 | { 56 | std::vector< AugmentedRow *> usedData; 57 | unsigned int dataSize = static_cast(data.size()); 58 | for(unsigned int i=0; i 64 | void DenseLinearEquationSystemParametersEstimator::leastSquaresEstimate( 65 | std::vector< AugmentedRow *> &data, 66 | std::vector ¶meters ) 67 | { 68 | unsigned int i, numRows = static_cast(data.size()); 69 | vnl_matrix A(numRows,n); 70 | vnl_vector x(n), b(numRows); 71 | T rowData[n], bValue; 72 | 73 | parameters.clear(); 74 | //not enough data elements for computation 75 | if(numRowsminForEstimate) 76 | return; 77 | 78 | //set up the equation system 79 | for(i=0; iget(rowData, bValue); 81 | A.set_row(i, rowData); 82 | b[i] = bValue; 83 | } 84 | 85 | vnl_matrix_inverse Ainv(A); 86 | //explicitly zero out small singular values 87 | //this is ugly as it exposes that the inverse is computed via SVD 88 | Ainv.zero_out_absolute(EPS); 89 | 90 | if(Ainv.rank() 99 | void DenseLinearEquationSystemParametersEstimator::leastSquaresEstimate( 100 | std::vector< AugmentedRow > &data, 101 | std::vector ¶meters ) 102 | { 103 | std::vector< AugmentedRow *> usedData; 104 | unsigned int dataSize = static_cast(data.size()); 105 | for(unsigned int i=0; i 111 | bool DenseLinearEquationSystemParametersEstimator::agree( 112 | std::vector ¶meters, AugmentedRow &data ) 113 | { 114 | T sum = static_cast(0.0); 115 | for( unsigned int i=0; idelta; 119 | } 120 | /*****************************************************************************/ 121 | template 122 | void DenseLinearEquationSystemParametersEstimator::getAugmentedRows( 123 | vnl_matrix &A, vnl_vector &b, std::vector< AugmentedRow > &rows ) 124 | { 125 | unsigned int rowNum = A.rows(); 126 | //ensure the vector and matrix dimensions are consistent and that they 127 | //are consistent with the output 128 | if( b.size() != rowNum || A.cols() != n ) 129 | throw std::exception(); 130 | 131 | rows.resize( rowNum ); 132 | for( unsigned int i=0; i { 25 | public: 26 | /** 27 | * Object constructor. 28 | * @param delta A ray is coincident with an intersection point if the 29 | * point-ray distance is less than 'delta'. 30 | * @param minimalAngularDeviation Two rays are considered parallel if the 31 | * angular deviation between them is less than 32 | * this value [radians]. Default is one degree. 33 | */ 34 | RayIntersectionParametersEstimator(double delta, 35 | double minimalAngularDeviation = 0.017453292519943295769236907684886); 36 | 37 | /** 38 | * Compute the intersection point defined by the given rays. This is the 39 | * unique point that minimizes the sum of distances to both rays. 40 | * @param data A vector containing two rays. 41 | * @param parameters This vector is cleared and then filled with the computed 42 | * intersection point parameters [x,y,z]. If the vector 43 | * contains less than two rays or they (a) do not intersect 44 | * or (b) have an infinite number of intersection points 45 | * then the resulting parameters vector is empty (size = 0). 46 | */ 47 | virtual void estimate(std::vector &data, 48 | std::vector ¶meters); 49 | virtual void estimate(std::vector &data, 50 | std::vector ¶meters); 51 | 52 | /** 53 | * Compute a least squares estimate of the intersection point defined by the 54 | * given rays. 55 | * NOTE: In practice the rays are treated as lines so the point we get is the 56 | * one that minimizes its distance from all lines {t in (-inf,inf)} and 57 | * not rays {t in [0,inf)}. Use this method only when line intersection 58 | * and ray intersection are equivalent. In general they are not 59 | * equivalent, the following two rays don't intersect but the lines do: 60 | * 61 | * * *------------> 62 | * | 63 | * | 64 | * \/ 65 | * 66 | * When this method is used as part of the RANSAC based estimation we 67 | * are sure that the ray parameter t is in [0,inf) as the methods 68 | * estimate() and agree() enforce this constraint, which in turn means 69 | * that treating these rays as lines will result in the correct answer. 70 | * @param data The point should minimize the least squares error (distance) to 71 | * these rays. 72 | * @param parameters This vector is cleared and then filled with the computed 73 | * parameters point parameters [x,y,z]. If the vector 74 | * contains less than two rays or they (a) do not intersect 75 | * or (b) have an infinite number of intersection points 76 | * then the resulting parameters vector is empty (size = 0). 77 | */ 78 | virtual void leastSquaresEstimate(std::vector &data, 79 | std::vector ¶meters); 80 | virtual void leastSquaresEstimate(std::vector &data, 81 | std::vector ¶meters); 82 | 83 | /** 84 | * Return true if the distance between the point defined by the parameters and 85 | * the given ray is smaller than 'delta' (see constructor). 86 | * @param parameters The point parameters [x,y,z]. 87 | * @param data Check that the distance between this ray and the point is 88 | * smaller than 'delta'. 89 | * 90 | * Note:If the parameters vector contains less than three entries the method 91 | * will throw an exception as it tries to access a non existing entry. 92 | */ 93 | virtual bool agree(std::vector ¶meters, Ray3D &data); 94 | 95 | /** 96 | * Change the distance defining if a ray and a given point are considered 97 | * coincident. 98 | * @param delta A ray is coincident with an intersection point if the 99 | * point-ray distance is less than 'delta'. 100 | */ 101 | void setDelta(double delta) {this->deltaSquared = delta*delta;} 102 | 103 | private: 104 | //given ray R and point P, if dist(R,P)^2 < delta^2 then the 105 | //point is on the ray 106 | double deltaSquared; 107 | //crossEps = sin(minimalAngularDeviation) two rays with normals n1,n2 are 108 | //parallel if cross(n1,n2),double > { 28 | public: 29 | /** 30 | * Object constructor. 31 | * @param delta Two points p_1,p_2 correspond via the Euclidean 32 | * transformation T if ||p_2 - Tp_1||^2 < delta^2 33 | */ 34 | AbsoluteOrientationParametersEstimator(double delta); 35 | 36 | /** 37 | * Compute the Euclidean transformation defined by the given data points such 38 | * that data[i].second = T*data[i].first. 39 | * @param data A vector containing three 3D point pairs. 40 | * @param parameters This vector is cleared and then filled with the computed 41 | * parameters, [s,q_x,q_y,q_z,t_x,t_y,t_z]. 42 | * If the vector contains less than three point pairs or the points 43 | * are collinear then the resulting parameters vector is empty 44 | * (size = 0). 45 | */ 46 | virtual void estimate(std::vector< std::pair *> &data, 47 | std::vector ¶meters); 48 | virtual void estimate(std::vector< std::pair > &data, 49 | std::vector ¶meters); 50 | 51 | 52 | /** 53 | * Compute a least squares estimate of the transformation defined by the 54 | * given point pairs such that T minimizes sum(||p_2 - Tp_1||^2). 55 | * This implementation is of the analytical least squares method presented in 56 | * "Closed-form solution of absolute orientation using unit quaternions", 57 | * B.K.P. Horn, Journal of the Optical Society of America, Vol. 4(4), 58 | * pp 629--642, 1987. 59 | * @param data The Euclidean transform should minimize the least squares 60 | * error between all point pairs. 61 | * @param parameters This vector is cleared and then filled with the computed 62 | * parameters, [s,q_x,q_y,q_z,t_x,t_y,t_z]. If the vector 63 | * contains less than three point pairs or all the points 64 | * are collinear then the resulting parameters vector is 65 | * empty (size = 0). 66 | */ 67 | virtual void leastSquaresEstimate(std::vector< std::pair *> &data, 68 | std::vector ¶meters); 69 | virtual void leastSquaresEstimate(std::vector< std::pair > &data, 70 | std::vector ¶meters); 71 | 72 | /** 73 | * Compute a weighted least squares estimate of the transformation. This is 74 | * a simple extension to the original algorithm described in Horn's paper. 75 | * @param data The Euclidean transform should minimize the least squares 76 | * error between all point pairs. 77 | * @param weights Each point pair is weighed according to the given weight. 78 | * This vector is assumed to have at least data.size() 79 | * entries. Entries are assumed to be non-negative. 80 | * @param parameters This vector is cleared and then filled with the computed 81 | * parameters, [s,q_x,q_y,q_z,t_x,t_y,t_z]. If the vector 82 | * contains less than three point pairs or all the points 83 | * are collinear then the resulting parameters vector is 84 | * empty (size = 0). 85 | */ 86 | void weightedLeastSquaresEstimate(std::vector< std::pair *> &data, 87 | std::vector &weights, 88 | std::vector ¶meters); 89 | 90 | 91 | /** 92 | * Return true if the squared distance between mapped point and its matching 93 | * point is smaller than 'delta' (see constructor): 94 | * ||(data.second - T(parameters)*data.first)||^2 < delta^2 95 | * 96 | * @param parameters The parameters (unit quaternion and translation) of the 97 | * transformation such that data[i].second = T*data[i].first 98 | * @param data Matching points in the two coordinate systems. 99 | */ 100 | virtual bool agree(std::vector ¶meters, 101 | std::pair &data); 102 | 103 | /** 104 | * Change the distance defining if two points correspond via a given 105 | * Euclidean transformation. 106 | * @param delta The points, p1, p2, correspond if ||p_2 - Tp_1||^2 < delta^2 107 | */ 108 | void setDelta(double delta) {this->deltaSquared = delta*delta;} 109 | 110 | private: 111 | //given transformation T([s,q_x,q_y,q_z,t_x,t_y,t_z]) two points, 112 | //p1, p2, correspond if ||p_2 - Tp_1||^2 < delta^2 113 | double deltaSquared; 114 | }; 115 | 116 | } //namespace lsqrRecipes 117 | 118 | #endif //_ABSOLUTE_ORIENTATION_PARAMETERS_ESTIMATOR_H_ 119 | -------------------------------------------------------------------------------- /parametersEstimators/LineParametersEstimator.hxx: -------------------------------------------------------------------------------- 1 | #ifndef _LINE_PARAMETERS_ESTIMATOR_TXX_ 2 | #define _LINE_PARAMETERS_ESTIMATOR_TXX_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "Epsilon.h" 8 | #include "LineParametersEstimator.h" 9 | 10 | namespace lsqrRecipes { 11 | 12 | template< unsigned int dimension > 13 | LineParametersEstimator::LineParametersEstimator(double delta) : 14 | ParametersEstimator< Point, double>(2) 15 | { 16 | this->deltaSquared = delta*delta; 17 | } 18 | /*****************************************************************************/ 19 | /* 20 | * Estimate the line parameters [n_0,...,n_k,a_0,...,a_k]. 21 | */ 22 | template< unsigned int dimension > 23 | void LineParametersEstimator::estimate(std::vector< Point *> &data, 24 | std::vector ¶meters) 25 | { 26 | unsigned int i, j, numParameters; 27 | double dirNorm; 28 | 29 | parameters.clear(); 30 | //user forgot to initialize the minimal number of required 31 | //elements or there are not enough data elements for computation 32 | //or the points are too close to each other 33 | if( this->minForEstimate==0 || data.size() < this->minForEstimate || 34 | (data[0])->distanceSquared(*(data[1]))< this->deltaSquared ) 35 | return; 36 | 37 | numParameters = 2*dimension; 38 | parameters.resize(numParameters); 39 | dirNorm = 0.0; 40 | for( i=0, j=dimension; i 54 | void LineParametersEstimator::estimate(std::vector< Point > &data, 55 | std::vector ¶meters) 56 | { 57 | std::vector< Point *> usedData; 58 | unsigned int dataSize = static_cast(data.size()); 59 | for(unsigned int i=0; i 68 | void LineParametersEstimator::leastSquaresEstimate(std::vector< Point *> &data, 69 | std::vector ¶meters) 70 | { 71 | parameters.clear(); //not enough data elements for computation 72 | if(data.size()minForEstimate) 73 | return; 74 | 75 | unsigned int i, j, k, pointNum = static_cast(data.size()); 76 | vnl_matrix meanMat(dimension, dimension), covariance(dimension,dimension,0); 77 | vnl_vector mean(dimension,0); 78 | 79 | //create covariance matrix 80 | double sqrtN = sqrt((double)pointNum); 81 | for(i=0; i eigenSystem(covariance); 103 | 104 | //the line direction is the eigenvector corresponding to 105 | //the largest eigenvalue 106 | //I assume ||eigenSystem.V(:,dimension-1)|| = 1 107 | for(i=0; i 117 | void LineParametersEstimator::leastSquaresEstimate(std::vector< Point > &data, 118 | std::vector ¶meters) 119 | { 120 | std::vector< Point *> usedData; 121 | unsigned int dataSize = static_cast(data.size()); 122 | for(unsigned int i=0; i 135 | bool LineParametersEstimator::agree(std::vector ¶meters, 136 | Point &data) 137 | { 138 | double v[dimension], vDotN, distanceSquared; 139 | unsigned int i, j; 140 | 141 | vDotN = 0.0; 142 | for(i=0, j=dimension; ideltaSquared; 150 | } 151 | 152 | } //namespace lsqrRecipes 153 | 154 | #endif //_PLANE_PARAMETERS_ESTIMATOR_TXX_ 155 | -------------------------------------------------------------------------------- /parametersEstimators/PlanePhantomUSCalibrationParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _PLANE_PHANTOM_US_CALIBRATION_PARAMETERS_ESTIMATOR_H_ 2 | #define _PLANE_PHANTOM_US_CALIBRATION_PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include 7 | #include "ParametersEstimator.h" 8 | #include "Point2D.h" 9 | #include "Frame.h" 10 | 11 | 12 | /** 13 | * This class estimates the transformation mapping points from the US image 14 | * coordinate system to the coordinate system of a tracked reference frame that 15 | * is fixed relative to the US probe. The method is based on the use of a planar 16 | * phantom, either the bottom of a water bath or a planar membrane. 17 | * For additional information with regard to US calibration see: 18 | * 19 | * "A review of calibration techniques for freehand 3-D ultrasound systems", 20 | * L. Mercier, T. Lango, F. Lindseth, L.D. Collins, Ultrasound in Med. & Biol., 21 | * vol. 31(2), pp. 143-165, 2005. 22 | * 23 | * @author: Ziv Yaniv 24 | * 25 | */ 26 | 27 | namespace lsqrRecipes { 28 | 29 | /** 30 | * The data elements used by the plane based estimator are pairs that 31 | * include: 32 | * (a). T2 - the transformation mapping points from the US dynamic reference 33 | * frame to the fixed, tracker, coordinate system T. 34 | * (b). q - 2D coordinates in the image coordinate system [u,v,0] corresponding 35 | * to a 3D point on the imaged plane. 36 | */ 37 | struct PlanePhantomUSCalibrationParametersEstimatorDataType { 38 | Frame T2; 39 | Point2D q; 40 | }; 41 | 42 | 43 | class PlanePhantomUSCalibrationParametersEstimator : 44 | public ParametersEstimator { 45 | public: 46 | 47 | enum LeastSquaresType {ANALYTIC = 0, ITERATIVE}; 48 | 49 | typedef PlanePhantomUSCalibrationParametersEstimatorDataType DataType; 50 | 51 | /** 52 | * Object constructor. 53 | * @param delta A data element (pair of US-DRF transformation and 2D point location) 54 | * agrees with specific calibration parameter values if 55 | * the distance between the mapped point and the x-y plane (z=0) 56 | * is less than 'delta'. 57 | * @param lsType When the leastSquaresEstimate() method is called it computes 58 | * an analytic or iterative fit. This flag tells it which one. 59 | */ 60 | PlanePhantomUSCalibrationParametersEstimator(double delta, 61 | LeastSquaresType lsType = ITERATIVE); 62 | 63 | /** 64 | * Compute the affine US calibration transformation defined by the given data 65 | * elements. 66 | * 67 | * @param data A vector containing 31 data elements. 68 | * @param parameters This vector is cleared and then filled with the computed 69 | * transformation parameters 70 | * [omega1_y, omega1_x, t_1z, t_3x, t_3y, t_3z, omega3_z, omega3_y, omega3_x, m_x, m_y, [*]]. 71 | * [*] - 30 entries dependent on the previous 11: 72 | * m_x*R1(3,1)*R3(:,1), 73 | * m_x*R1(3,2)*R3(:,1), 74 | * m_x*R1(3,3)*R3(:,1), 75 | * m_y*R1(3,1)*R3(:,2), 76 | * m_y*R1(3,2)*R3(:,2), 77 | * m_y*R1(3,3)*R3(:,2), 78 | * R1(3,1)*t_3, 79 | * R1(3,2)*t_3, 80 | * R1(3,3)*t_3, 81 | * R1(3,:) 82 | * 83 | *Angles are in radians. 84 | * 85 | *The reason for providing the last 30 entries, is computational efficiency of 86 | *the agree method. 87 | * 88 | *If the data vector contains less than 31 data elements or they do not provide 89 | *a set of linearly independent equation systems the resulting parameters 90 | *vector is empty (size == 0). 91 | */ 92 | virtual void estimate(std::vector &data, 93 | std::vector ¶meters); 94 | virtual void estimate(std::vector &data, 95 | std::vector ¶meters); 96 | 97 | 98 | virtual void leastSquaresEstimate(std::vector &data, 99 | std::vector ¶meters); 100 | virtual void leastSquaresEstimate(std::vector &data, 101 | std::vector ¶meters); 102 | 103 | virtual bool agree(std::vector ¶meters, DataType &data); 104 | 105 | /** 106 | * Change the type of least squares solution. 107 | * @param lsType When the leastSquaresEstimate() method is called it computes 108 | * an analytic or iterative fit. This flag tells it which one. 109 | */ 110 | void setLeastSquaresType(LeastSquaresType lsType) {this->lsType = lsType;} 111 | 112 | /** 113 | * Set the threshold that defines if a data element agrees with a specific 114 | * model estimate. 115 | * @param delta A data element (pair of US-DRF transformation and 2D point location) 116 | * agrees with specific calibration parameter values if 117 | * the distance between the mapped point and the x-y plane (z=0) 118 | * is less than 'delta'. 119 | */ 120 | void setDelta(double delta) {this->deltaSquared = delta*delta;} 121 | 122 | void analyticLeastSquaresEstimate(std::vector< DataType *> &data, 123 | std::vector ¶meters); 124 | 125 | void iterativeLeastSquaresEstimate(std::vector< DataType *> &data, 126 | std::vector &initialParameters, 127 | std::vector &finalParameters); 128 | 129 | static void getDistanceStatistics(const std::vector ¶meters, 130 | const std::vector &data, 131 | std::vector &distances, 132 | double &min, double &max, double &mean); 133 | 134 | private: 135 | 136 | class SumSquaresCalibrationPointsDistanceFunction : 137 | public vnl_least_squares_function { 138 | public: 139 | SumSquaresCalibrationPointsDistanceFunction(std::vector *data); 140 | void setData(std::vector *data); 141 | virtual void f( vnl_vector const &x, vnl_vector &fx ); 142 | virtual void gradf( vnl_vector const& x, vnl_matrix& jacobian ); 143 | private: 144 | std::vector *data; 145 | }; 146 | 147 | double deltaSquared; 148 | //analytic or iterative least squares 149 | LeastSquaresType lsType; 150 | }; 151 | 152 | 153 | 154 | } //namespace lsqrRecipes 155 | 156 | #endif //_PLANE_PHANTOM_US_CALIBRATION_PARAMETERS_ESTIMATOR_H_ 157 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # This is the configuration file for the LSQRRecipes (least-squares recipes) 3 | # library. The library requires the vnl components from the vxl project 4 | # (http://vxl.sourceforge.net/). The configuration supports either the use of vnl 5 | # from a vxl installation or ITK's (www.itk.org) version of vnl, if installed. 6 | # The choice of which version to use is indicated by the Cmake variable USING_ITK_VNL. 7 | # 8 | 9 | PROJECT( LSQRRecipes ) 10 | 11 | cmake_minimum_required(VERSION 2.8) 12 | 13 | # 14 | # LSQRRecipes version number. 15 | # 16 | set(LSQRRecipes_VERSION_MAJOR "0") 17 | set(LSQRRecipes_VERSION_MINOR "2") 18 | set(LSQRRecipes_VERSION_PATCH "0") 19 | 20 | option(USING_ITK_VNL "Build using ITK version of VNL." OFF) 21 | 22 | # 23 | # we use vnl from the vxl package to do all of our linear algebra and nonlinear 24 | # optimization algorithms, either through ITK or directly through VXL 25 | # 26 | if(USING_ITK_VNL) 27 | # 28 | #remove the VXL_DIR from the cache, we are working with the ITK version 29 | #of vnl 30 | # 31 | unset(VXL_DIR CACHE) 32 | 33 | # 34 | # Using vnl through ITK, need to find ITK 35 | # 36 | find_package(ITK REQUIRED) 37 | if(ITK_FOUND) 38 | include(${ITK_USE_FILE}) 39 | set(LINK_LIBRARIES 40 | LSQRRecipes 41 | itkvnl 42 | itkvnl_algo 43 | itkv3p_netlib 44 | itkvcl 45 | itkv3p_lsqr 46 | ) 47 | endif() 48 | else() 49 | # 50 | #remove the ITK_DIR from the cache, we are working with the VXL version of 51 | #vnl 52 | # 53 | unset(ITK_DIR CACHE) 54 | 55 | # 56 | # Using vnl through VXL, need to find VXL 57 | # 58 | find_package(VXL REQUIRED) 59 | if(VXL_FOUND) 60 | include(${VXL_CMAKE_DIR}/UseVXL.cmake) 61 | set(LINK_LIBRARIES 62 | LSQRRecipes 63 | vnl 64 | vnl_algo 65 | netlib 66 | v3p_netlib 67 | vcl 68 | ) 69 | endif() 70 | endif() 71 | 72 | 73 | # 74 | # include directories 75 | # 76 | set(LSQRRecipes_INCLUDE_DIRS 77 | ${LSQRRecipes_SOURCE_DIR}/copyright 78 | ${LSQRRecipes_SOURCE_DIR}/common 79 | ${LSQRRecipes_SOURCE_DIR}/parametersEstimators 80 | ) 81 | 82 | include_directories( 83 | ${LSQRRecipes_INCLUDE_DIRS} 84 | ) 85 | 86 | 87 | set(LSQRRecipes_HDRS 88 | copyright/copyright.h 89 | common/Epsilon.h 90 | common/RandomNumberGenerator.h 91 | common/Point.h 92 | common/Point2D.h 93 | common/IPoint2D.h 94 | common/Point3D.h 95 | common/Vector.h 96 | common/Vector3D.h 97 | common/Ray3D.h 98 | common/Frame.h 99 | parametersEstimators/ParametersEstimator.h 100 | parametersEstimators/RANSAC.h 101 | parametersEstimators/RANSAC.hxx 102 | parametersEstimators/Line2DParametersEstimator.h 103 | parametersEstimators/LineParametersEstimator.h 104 | parametersEstimators/LineParametersEstimator.hxx 105 | parametersEstimators/PlaneParametersEstimator.h 106 | parametersEstimators/PlaneParametersEstimator.hxx 107 | parametersEstimators/SphereParametersEstimator.h 108 | parametersEstimators/SphereParametersEstimator.hxx 109 | parametersEstimators/RayIntersectionParametersEstimator.h 110 | parametersEstimators/SinglePointTargetUSCalibrationParametersEstimator.h 111 | parametersEstimators/PlanePhantomUSCalibrationParametersEstimator.h 112 | parametersEstimators/AbsoluteOrientationParametersEstimator.h 113 | parametersEstimators/DenseLinearEquationSystemParametersEstimator.h 114 | parametersEstimators/DenseLinearEquationSystemParametersEstimator.hxx 115 | parametersEstimators/PivotCalibrationParametersEstimator.h 116 | ) 117 | 118 | set(LSQRRecipes_SRCS 119 | common/Vector3D.cxx 120 | common/Ray3D.cxx 121 | common/Frame.cxx 122 | parametersEstimators/Line2DParametersEstimator.cxx 123 | parametersEstimators/RayIntersectionParametersEstimator.cxx 124 | parametersEstimators/SinglePointTargetUSCalibrationParametersEstimator.cxx 125 | parametersEstimators/PlanePhantomUSCalibrationParametersEstimator.cxx 126 | parametersEstimators/AbsoluteOrientationParametersEstimator.cxx 127 | parametersEstimators/PivotCalibrationParametersEstimator.cxx 128 | ) 129 | 130 | # 131 | # Actually create the LSQRRecipes library 132 | # 133 | add_library(LSQRRecipes ${LSQRRecipes_SRCS} ${LSQRRecipes_HDRS}) 134 | set_target_properties(LSQRRecipes PROPERTIES 135 | VERSION ${LSQRRecipes_VERSION_MAJOR}.${LSQRRecipes_VERSION_MINOR}.${LSQRRecipes_VERSION_PATCH} 136 | SOVERSION ${LSQRRecipes_VERSION_MAJOR} 137 | ) 138 | 139 | ################################################################################ 140 | # 141 | # optional testing of the estimation classes 142 | # 143 | option(BUILD_TESTING "Build the Testing directory." ON) 144 | if(BUILD_TESTING) 145 | enable_testing() 146 | set(LSQR_RECIPES_TESTING_DATA_ROOT ${LSQRRecipes_SOURCE_DIR}/testing/Data) 147 | add_subdirectory(testing) 148 | endif() 149 | 150 | ################################################################################ 151 | # 152 | #settings for installation. 153 | # 154 | 155 | #used by the GenerateLSQRRecipesConfig.cmake file 156 | set(PRE_INSTALL_DIR ${LSQRRecipes_BINARY_DIR}/preInstall) 157 | 158 | # 159 | # where to install the library 160 | # 161 | set(LSQRRecipes_INSTALL_LIB_DIR ${CMAKE_INSTALL_PREFIX}/lib CACHE PATH "Directory where you want to install the LSQRRecipes library.") 162 | 163 | # 164 | # install the static library 165 | # 166 | install(TARGETS LSQRRecipes 167 | ARCHIVE DESTINATION ${LSQRRecipes_INSTALL_LIB_DIR} 168 | LIBRARY DESTINATION ${LSQRRecipes_INSTALL_LIB_DIR} 169 | ) 170 | 171 | # 172 | # where to install the headers 173 | # 174 | set(LSQRRecipes_INSTALL_INCLUDE_DIR ${CMAKE_INSTALL_PREFIX}/include CACHE PATH "Directory where you want to install the LSQRRecipes library headers.") 175 | 176 | # 177 | # install all of the headers 178 | # 179 | install(FILES ${LSQRRecipes_HDRS} 180 | DESTINATION ${LSQRRecipes_INSTALL_INCLUDE_DIR} 181 | ) 182 | 183 | # 184 | # copy USeLSQRRecipes.cmake to the library dir so that external 185 | # projects have the correct settings 186 | # 187 | install(FILES ${LSQRRecipes_BINARY_DIR}/UseLSQRRecipes.cmake 188 | ${PRE_INSTALL_DIR}/LSQRRecipesConfig.cmake 189 | DESTINATION ${LSQRRecipes_INSTALL_LIB_DIR} 190 | ) 191 | 192 | ################################################################################ 193 | # 194 | # settings so external projects can easily use LSQRRecipes 195 | # 196 | 197 | # 198 | # create the LSQRRecipes.cmake file, using the GenerateLSQRRecipes configuration 199 | # 200 | include(${LSQRRecipes_SOURCE_DIR}/GenerateLSQRRecipesConfig.cmake) 201 | 202 | # 203 | #copy the UseLSQRRecipes.cmake file to the binary dir 204 | # 205 | configure_file(${LSQRRecipes_SOURCE_DIR}/UseLSQRRecipes.cmake.in 206 | ${LSQRRecipes_BINARY_DIR}/UseLSQRRecipes.cmake COPYONLY) 207 | -------------------------------------------------------------------------------- /parametersEstimators/RANSAC.h: -------------------------------------------------------------------------------- 1 | #ifndef _RANSAC_H_ 2 | #define _RANSAC_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "ParametersEstimator.h" 12 | 13 | /** 14 | * This class implements the RAndom SAmple Consensus (RANSAC) algorithm, 15 | * an algorithm for robust parameter estimation. 16 | * Given data containing outliers we estimate the model parameters using 17 | * sub-sets of the original data: 18 | * 1. Choose the minimal subset from the data for computing the exact model 19 | * parameters. 20 | * 2. See how much of the input data agrees with the computed parameters. 21 | * 3. Goto step 1. This can be done up to (N choose m) times, where m is the 22 | * number of data objects required for an exact estimate and N is the total 23 | * number of data objects. 24 | * 4. Take the largest subset of objects which agreed on the parameters and 25 | * compute a least squares fit using them. 26 | * 27 | * This is based on: 28 | * Fischler M.A., Bolles R.C., 29 | * "Random Sample Consensus: A Paradigm for Model Fitting with Applications to 30 | * Image Analysis and Automated Cartography", Communications of the ACM, 31 | * Vol. 24(6), 1981. 32 | * 33 | * Hartely R., Zisserman A., "Multiple View Geometry in Computer Vision", 2001. 34 | * 35 | * The class template parameters are T - objects used for the parameter estimation 36 | * (e.g. Point2D in line estimation, 37 | * std::pair in 38 | * homography estimation). 39 | * S - type of parameter (e.g. double). 40 | * 41 | * @author: Ziv Yaniv 42 | * 43 | */ 44 | 45 | namespace lsqrRecipes { 46 | 47 | template 48 | class RANSAC { 49 | 50 | public: 51 | /** 52 | * Estimate the model parameters using the RANSAC framework. 53 | * @param parameters A vector which will contain the estimated parameters. 54 | * If there is an error in the input then this vector will 55 | * be empty. 56 | * Errors are: 1. Less data objects than required for an 57 | * exact fit. 58 | * 2. The given data is in a singular 59 | * configuration (e.g. trying to fit a circle 60 | * to a set of collinear points). 61 | * 3. The given parameter 62 | * desiredProbabilityForNoOutliers is not in 63 | * (0,1). 64 | * @param paramEstimator An object which can estimate the desired parameters 65 | * using either an exact fit or a least squares fit. 66 | * @param data The input from which the parameters will be estimated. 67 | * @param desiredProbabilityForNoOutliers The probability that at least one of 68 | * the selected subsets doesn't contain 69 | * an outlier, must be in (0,1). 70 | * @param consensusSet Optional parameter. The consensus set, same length as 71 | * the data vector, true if a data element belongs to the 72 | * set, false otherwise. 73 | * @return Returns the percentage of data used in the least squares estimate. 74 | */ 75 | static double compute(std::vector ¶meters, 76 | ParametersEstimator *paramEstimator , 77 | std::vector &data, 78 | double desiredProbabilityForNoOutliers, 79 | std::vector *consensusSet=NULL); 80 | 81 | 82 | /** 83 | * Estimate the model parameters using the maximal consensus set by going over 84 | * ALL possible subsets (brute force approach). 85 | * Given: n - data.size() 86 | * k - numForEstimate 87 | * We go over all n choose k subsets n! 88 | * ------------ 89 | * (n-k)! * k! 90 | * @param parameters A vector which will contain the estimated parameters. 91 | * If there is an error in the input then this vector will 92 | * be empty. 93 | * Errors are: 1. Less data objects than required for an 94 | * exact fit. 95 | * 2. The given data is in a singular 96 | * configuration (e.g. trying to fit a circle 97 | * to a set of collinear points). 98 | * @param paramEstimator An object which can estimate the desired parameters 99 | * using either an exact fit or a least squares fit. 100 | * @param data The input from which the parameters will be estimated. 101 | * @param numForEstimate The number of data objects required for an exact fit. 102 | * @param consensusSet Optional parameter. The consensus set, same length as 103 | * the data vector, true if a data element belongs to the 104 | * set, false otherwise. 105 | * @return Returns the percentage of data used in the least squares estimate. 106 | * 107 | * NOTE: This method should be used only when n choose k is small 108 | * (i.e. k or (n-k) are approximately equal to n) 109 | * 110 | */ 111 | static double compute(std::vector ¶meters, 112 | ParametersEstimator *paramEstimator , 113 | std::vector &data, std::vector *consensusSet=NULL); 114 | private: 115 | 116 | /** 117 | * Compute n choose m [ n!/(m!*(n-m)!)]. 118 | * If choose(n,m)>std::numeric_limits::max(), or there is an 119 | * overflow during the computations then we return 120 | * std::numeric_limits::max(), otherwise the correct value 121 | * is returned. 122 | */ 123 | static unsigned int choose(unsigned int n, unsigned int m); 124 | 125 | static void computeAllChoices(ParametersEstimator *paramEstimator, 126 | std::vector &data, 127 | bool *bestVotes, bool *curVotes, 128 | unsigned int &numVotesForBest, int startIndex, int k, 129 | int arrIndex, int *arr); 130 | 131 | static void estimate(ParametersEstimator *paramEstimator, 132 | std::vector &data, bool *bestVotes, bool *curVotes, 133 | unsigned int &numVotesForBest, int *arr); 134 | 135 | class SubSetIndexComparator { 136 | private: 137 | int length; 138 | public: 139 | SubSetIndexComparator(int arrayLength) : length(arrayLength){} 140 | bool operator()(const int *arr1, const int *arr2) const { 141 | for(int i=0; ilength; i++) { 142 | if(arr1[i] < arr2[i]) 143 | return true; 144 | else if(arr1[i] > arr2[i]) 145 | return false; 146 | } 147 | return false; 148 | } 149 | }; 150 | }; 151 | 152 | } //namespace lsqrRecipes 153 | 154 | #include "RANSAC.hxx" 155 | 156 | #endif //_RANSAC_H_ 157 | -------------------------------------------------------------------------------- /parametersEstimators/RayIntersectionParametersEstimator.cxx: -------------------------------------------------------------------------------- 1 | #include "RayIntersectionParametersEstimator.h" 2 | #include 3 | #include "Epsilon.h" 4 | #include "Vector3D.h" 5 | 6 | 7 | namespace lsqrRecipes { 8 | 9 | RayIntersectionParametersEstimator::RayIntersectionParametersEstimator(double delta, 10 | double minimalAngularDeviation) : 11 | ParametersEstimator(2) 12 | { 13 | this->deltaSquared = delta*delta; 14 | this->crossEps = sin(minimalAngularDeviation); 15 | this->crossEps*=this->crossEps; 16 | } 17 | /*****************************************************************************/ 18 | /** 19 | * Compute the intersection of two rays. Based on the code 20 | * found in Graphics Gems p.304 "Intersection of Two Lines in Three-Space", 21 | * Ronald Goldman. 22 | */ 23 | void RayIntersectionParametersEstimator::estimate(std::vector &data, 24 | std::vector ¶meters) 25 | { 26 | parameters.clear(); 27 | //not enough data elements for computation 28 | if(data.size()minForEstimate) 29 | return; 30 | 31 | lsqrRecipes::Vector3D &n1 = data[0]->n; 32 | lsqrRecipes::Vector3D &n2 = data[1]->n; 33 | lsqrRecipes::Point3D &p1 = data[0]->p; 34 | lsqrRecipes::Point3D &p2 = data[1]->p; 35 | 36 | lsqrRecipes::Vector3D p21, n1Crossn2; 37 | double t1, t2; 38 | 39 | p21[0] = p2[0] - p1[0]; 40 | p21[1] = p2[1] - p1[1]; 41 | p21[2] = p2[2] - p1[2]; 42 | 43 | n1Crossn2[0] = n1[1]*n2[2] - n1[2]*n2[1]; 44 | n1Crossn2[1] = n1[2]*n2[0] - n1[0]*n2[2]; 45 | n1Crossn2[2] = n1[0]*n2[1] - n1[1]*n2[0]; 46 | 47 | double denominator = n1Crossn2[0]*n1Crossn2[0] + 48 | n1Crossn2[1]*n1Crossn2[1] + 49 | n1Crossn2[2]*n1Crossn2[2]; 50 | 51 | //rays are parallel they don't intersect (this is where I 52 | //make the assumption that ||n1||=||n2||=1 as the condition 53 | //is ||cross(n1,n2)||^2<(||n1||^2*||n2||^2sin(minimalAngularDeviation)^2 = sin(minimalAngularDeviation)^2 = this->crossEps 54 | if(denominatorcrossEps) 55 | return; 56 | //rays are not parallel (skew or intersecting) 57 | t1 = (n1Crossn2[0]*(p21[1]*n2[2] - p21[2]*n2[1]) - 58 | n1Crossn2[1]*(p21[0]*n2[2] - p21[2]*n2[0]) + 59 | n1Crossn2[2]*(p21[0]*n2[1] - p21[1]*n2[0]))/denominator; 60 | t2 = (n1Crossn2[0]*(p21[1]*n1[2] - p21[2]*n1[1]) - 61 | n1Crossn2[1]*(p21[0]*n1[2] - p21[2]*n1[0]) + 62 | n1Crossn2[2]*(p21[0]*n1[1] - p21[1]*n1[0]))/denominator; 63 | //the lines {t in (-inf, inf)} intersect, but the rays {t in [0, inf)} don't 64 | if(t1<0 || t2<0) 65 | return; 66 | //the intersection is the mid-point 67 | parameters.push_back((p1[0]+t1*n1[0] + p2[0]+t2*n2[0])/2.0); 68 | parameters.push_back((p1[1]+t1*n1[1] + p2[1]+t2*n2[1])/2.0); 69 | parameters.push_back((p1[2]+t1*n1[2] + p2[2]+t2*n2[2])/2.0); 70 | } 71 | /*****************************************************************************/ 72 | /* 73 | * Compute the intersection point parameters [x,y,z] 74 | */ 75 | void RayIntersectionParametersEstimator::estimate(std::vector &data, 76 | std::vector ¶meters) 77 | { 78 | std::vector usedData; 79 | size_t dataSize = data.size(); 80 | for(size_t i=0; i &data, 101 | std::vector ¶meters) 102 | { 103 | vnl_matrix A(3,3,0); 104 | vnl_vector b(3,0), x(3); 105 | size_t rayNum = data.size(); 106 | 107 | //create the matrix A and vector b 108 | for(size_t i=0; i Ainv(A); 133 | //explicitly zero out small singular values 134 | //this is ugly as it exposes that the inverse is computed via SVD 135 | Ainv.zero_out_absolute(EPS); 136 | 137 | if(Ainv.rank()<3) //all rays are parallel 138 | return; 139 | x = Ainv * b; 140 | 141 | parameters.push_back(x[0]); 142 | parameters.push_back(x[1]); 143 | parameters.push_back(x[2]); 144 | } 145 | /*****************************************************************************/ 146 | /* 147 | * Compute the intersection point parameters [x,y,z] 148 | */ 149 | void RayIntersectionParametersEstimator::leastSquaresEstimate(std::vector &data, 150 | std::vector ¶meters) 151 | { 152 | std::vector usedData; 153 | size_t dataSize = data.size(); 154 | for(size_t i=0; i ¶meters, 165 | Ray3D &data) 166 | { 167 | lsqrRecipes::Vector3D &n = data.n; 168 | lsqrRecipes::Point3D &p = data.p; 169 | 170 | double t = n[0]*(parameters[0]-p[0]) + 171 | n[1]*(parameters[1]-p[1]) + 172 | n[2]*(parameters[2]-p[2]); 173 | double dx = parameters[0] - p[0] - t*n[0]; 174 | double dy = parameters[1] - p[1] - t*n[1]; 175 | double dz = parameters[2] - p[2] - t*n[2]; 176 | 177 | //closest point is on the ray and the distance is less than delta 178 | return t>=0 && (dx*dx+dy*dy+dz*dz < this->deltaSquared); 179 | } 180 | 181 | } //namespace lsqrRecipes 182 | -------------------------------------------------------------------------------- /testing/AbsoluteOrientationParametersEstimatorTest.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Frame.h" 3 | #include "AbsoluteOrientationParametersEstimator.h" 4 | #include "RandomNumberGenerator.h" 5 | 6 | bool reportAndCheckResults(const std::string &title, 7 | const double distanceThreshold, 8 | const std::vector &estimatedTransformationParameters, 9 | const std::vector< std::pair > &evaluationData); 10 | 11 | /* 12 | * Test the absolute orientation estimator's methods. 13 | */ 14 | int main(int argc, char *argv[]) 15 | { 16 | typedef std::pair DataType; 17 | 18 | int i, pairNum = 10; 19 | double bounds = 100.0; //point coordinates are in [-100,100] 20 | double maxTranslation = 1000.0; //translations are in [-1000,1000] 21 | std::vector< DataType > estimationData, cleanEstimationData, targetData; 22 | double noiseSigma = 5.0/3.0; //noise is distributed IID ~N(0,noisSigma) 23 | DataType pointPair, outlierPair; 24 | std::vector knownTransformationParameters, 25 | estimatedTransformationParameters; 26 | lsqrRecipes::Frame knownTransformation; 27 | 28 | lsqrRecipes::RandomNumberGenerator random; 29 | 30 | //create a random transformation 31 | //random unit quaternion 32 | double qx,qy,qz; 33 | qx = random.uniform(0.0,1.0); 34 | qy = random.uniform(0.0,sqrt(1.0-qx*qx)); 35 | qz = random.uniform(0.0,sqrt(1.0-qx*qx-qy*qy)); 36 | knownTransformationParameters.push_back(sqrt(1.0 - qx*qx - qy*qy - qz*qz)); 37 | knownTransformationParameters.push_back(qx); 38 | knownTransformationParameters.push_back(qy); 39 | knownTransformationParameters.push_back(qz); 40 | //random translation 41 | knownTransformationParameters.push_back(random.uniform(-maxTranslation,maxTranslation)); 42 | knownTransformationParameters.push_back(random.uniform(-maxTranslation,maxTranslation)); 43 | knownTransformationParameters.push_back(random.uniform(-maxTranslation,maxTranslation)); 44 | 45 | knownTransformation.setRotationQuaternion(knownTransformationParameters[0], 46 | knownTransformationParameters[1], 47 | knownTransformationParameters[2], 48 | knownTransformationParameters[3]); 49 | knownTransformation.setTranslation(knownTransformationParameters[4], 50 | knownTransformationParameters[5], 51 | knownTransformationParameters[6]); 52 | 53 | //create data without noise, data with noise and targets 54 | for(i=0; i &estimatedTransformationParameters, 113 | const std::vector< std::pair > &evaluationData) 114 | { 115 | std::cout<<"\n"< >::const_iterator it, end; 132 | end = evaluationData.end(); 133 | double maxDistance = 0.0; 134 | for(it = evaluationData.begin(); it!=end; it++) { 135 | lsqrRecipes::Point3D pTransformed; 136 | estimatedTransformation.apply((*it).first,pTransformed); 137 | double distance = (pTransformed - (*it).second).l2Norm(); 138 | if(distance>maxDistance) 139 | maxDistance = distance; 140 | } 141 | std::cout<<"Maximal TRE to random targets is: "< 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "RandomNumberGenerator.h" 7 | #include "DenseLinearEquationSystemParametersEstimator.h" 8 | 9 | using namespace lsqrRecipes; 10 | 11 | /** 12 | * Use simulated data, clean data to test the exact estimate and data with 13 | * noise to test the least squares estimate. 14 | */ 15 | int simulatedDataTest(); 16 | 17 | /** 18 | * Use experimental data acquired during pivot calibration to test the least 19 | * squares method. This deals with actual noise and not noise artificially 20 | * generated using a noise model we selected. 21 | */ 22 | int pivotCalibrationMatrixTest(const std::string &augmentedMatrixFileName); 23 | 24 | int main(int argc, char *argv[]) 25 | { 26 | if(argc == 1) 27 | return simulatedDataTest(); 28 | else if(argc == 2) 29 | return pivotCalibrationMatrixTest(argv[1]); 30 | else { 31 | std::cerr<<"Unexpected number of command line parameters.\n"; 32 | std::cerr<<"Usage: \n"; 33 | std::cerr<<"\t"< A( rowNum, colNum ), ASquare( colNum, colNum ); 51 | vnl_vector b( rowNum ), bNoise( rowNum ), bSquare( colNum), x( colNum ); 52 | 53 | RandomNumberGenerator random; 54 | 55 | //random invertible matrix 56 | ok = false; 57 | while (!ok) { 58 | for( i=0; i > augmentedRows; 69 | DenseLinearEquationSystemParametersEstimator::getAugmentedRows( ASquare, bSquare, 70 | augmentedRows ); 71 | 72 | double maxEquationError = 1.0e-10; 73 | std::vector estimatedSolution; 74 | DenseLinearEquationSystemParametersEstimator equationSolver( maxEquationError ); 75 | 76 | equationSolver.estimate( augmentedRows, estimatedSolution ); 77 | if(estimatedSolution.empty()) 78 | return EXIT_FAILURE; 79 | 80 | std::cout<<"Exact solution with invertible matrix\n"; 81 | std::cout<<"-------------------------------------\n"; 82 | std::cout<<"Known solution [x_0,..., x_{n-1}]:\n\t [ "; 83 | for( i=0; i::getAugmentedRows(A, b, 122 | augmentedRows); 123 | maxEquationError = 0.1; 124 | equationSolver.setDelta(maxEquationError); 125 | equationSolver.leastSquaresEstimate(augmentedRows, estimatedSolution); 126 | if(estimatedSolution.empty()) 127 | return EXIT_FAILURE; 128 | 129 | std::cout<<"Least squares solution with overdetermined matrix and noise\n"; 130 | std::cout<<"------------------------------------------------------------\n"; 131 | std::cout<<"Known solution [x_0,..., x_{n-1}]:\n\t [ "; 132 | for( i=0; i row; 160 | std::vector< AugmentedRow > augmentedRows; 161 | //known solution to equation system 162 | double x[6] = { -1.777985584409468e+001, 1.111302171667757e+000, 163 | -1.568653413096010e+002, 1.469013927556186e+002, 164 | -6.296891425314718e+001,-1.042139650090033e+003}; 165 | 166 | //load the data 167 | std::ifstream in; 168 | in.open(augmentedMatrixFileName.c_str()); 169 | if(!in.is_open()) 170 | return false; 171 | while(in>>data[0]>>data[1]>>data[2]>>data[3]>>data[4]>>data[5]>>data[6]) { 172 | row.set(data); 173 | augmentedRows.push_back(row); 174 | } 175 | in.close(); 176 | 177 | if(augmentedRows.empty()) { 178 | std::cerr<<"Failed to load augmented matrix file.\n"; 179 | return EXIT_FAILURE; 180 | } 181 | 182 | //set the equation solver 183 | double maxEquationError = 0.5; 184 | std::vector estimatedSolution; 185 | DenseLinearEquationSystemParametersEstimator equationSolver( maxEquationError ); 186 | 187 | equationSolver.leastSquaresEstimate(augmentedRows, estimatedSolution); 188 | if(estimatedSolution.empty()) 189 | return EXIT_FAILURE; 190 | 191 | unsigned int i; 192 | 193 | std::cout<<"Least squares solution experimental overdetermined matrix\n"; 194 | std::cout<<"----------------------------------------------------------\n"; 195 | std::cout<<"Known solution [x_0,..., x_{n-1}]:\n\t [ "; 196 | for( i=0; i &estimatedTransformationParameters, 8 | const std::vector< std::pair > &estimationData, 9 | const std::vector< std::pair > &targetData); 10 | 11 | /* 12 | * Example of using the exhaustive search method that is part of the RANSAC 13 | * implementation. For absolute orientation with a small number of points this 14 | * is computationally reasonable. The program estimates the transformation using 15 | * a least squares method and a RANSAC wrapper for the same least squares method. 16 | * The program then prints the estimated/known transformations and associated 17 | * FREs and TREs. Look at the FRE to identify the outlying point(s). 18 | */ 19 | int main(int argc, char *argv[]) 20 | { 21 | typedef std::pair DataType; 22 | 23 | unsigned int inliers = 4; 24 | unsigned int outliers = 1; 25 | unsigned int i; 26 | double bounds = 100.0; //point coordinates are in [-100,100] 27 | double maxTranslation = 1000.0; //translations are in [-1000,1000] 28 | std::vector< DataType > estimationData, targetData; 29 | double noiseSigma = 1.0; //noise is distributed IID ~N(0,noisSigma) 30 | DataType pointPair; 31 | std::vector knownTransformationParameters, 32 | estimatedTransformationParameters; 33 | lsqrRecipes::Frame knownTransformation, estimatedTransformation; 34 | 35 | lsqrRecipes::RandomNumberGenerator random; 36 | 37 | //create a random transformation 38 | //random unit quaternion 39 | double qx,qy,qz; 40 | qx = random.uniform(0.0,1.0); 41 | qy = random.uniform(0.0,sqrt(1.0-qx*qx)); 42 | qz = random.uniform(0.0,sqrt(1.0-qx*qx-qy*qy)); 43 | knownTransformationParameters.push_back(sqrt(1.0 - qx*qx - qy*qy - qz*qz)); 44 | knownTransformationParameters.push_back(qx); 45 | knownTransformationParameters.push_back(qy); 46 | knownTransformationParameters.push_back(qz); 47 | //random translation 48 | knownTransformationParameters.push_back(random.uniform(-maxTranslation,maxTranslation)); 49 | knownTransformationParameters.push_back(random.uniform(-maxTranslation,maxTranslation)); 50 | knownTransformationParameters.push_back(random.uniform(-maxTranslation,maxTranslation)); 51 | 52 | knownTransformation.setRotationQuaternion(knownTransformationParameters[0], 53 | knownTransformationParameters[1], 54 | knownTransformationParameters[2], 55 | knownTransformationParameters[3]); 56 | knownTransformation.setTranslation(knownTransformationParameters[4], 57 | knownTransformationParameters[5], 58 | knownTransformationParameters[6]); 59 | 60 | //create data and targets 61 | for(i=0; iconsensusSet; 107 | double percentageOfDataUsed; 108 | percentageOfDataUsed = 109 | lsqrRecipes::RANSAC< std::pair, double>::compute(estimatedTransformationParameters, 110 | &aopEstimator, 111 | estimationData, &consensusSet); 112 | reportResults("exhaustive search estimate:", estimatedTransformationParameters, 113 | estimationData, targetData); 114 | std::cout<<"Fiducials used in final estimate: "; 115 | for(std::vector::const_iterator it = consensusSet.begin(); it!=consensusSet.end(); it++) 116 | std::cout<<(*it)<<" "; 117 | std::cout<<"\n"; 118 | 119 | return EXIT_SUCCESS; 120 | } 121 | 122 | /*****************************************************************************/ 123 | void reportResults(const std::string &title, 124 | const std::vector &estimatedTransformationParameters, 125 | const std::vector< std::pair > &estimationData, 126 | const std::vector< std::pair > &targetData) 127 | { 128 | std::cout<<"\n"< >::const_iterator it, end; 145 | end = estimationData.end(); 146 | std::cout<<"Fiducial registration errors: "; 147 | for(it = estimationData.begin(); it!=end; it++) { 148 | lsqrRecipes::Point3D pTransformed; 149 | estimatedTransformation.apply((*it).first,pTransformed); 150 | double distance = (pTransformed - (*it).second).l2Norm(); 151 | std::cout< 5 | #include 6 | #include 7 | #include "Epsilon.h" 8 | #include "PlaneParametersEstimator.h" 9 | 10 | namespace lsqrRecipes { 11 | 12 | template< unsigned int dimension > 13 | PlaneParametersEstimator::PlaneParametersEstimator(double delta) : 14 | ParametersEstimator< Point, double>(dimension) 15 | { 16 | this->deltaSquared = delta*delta; 17 | } 18 | /*****************************************************************************/ 19 | /* 20 | * Estimate the plane parameters [n_0,...,n_k,a_0,...,a_k]. 21 | * The plane is given as dot(n,p-a) = dot(n,p) - dot(n,a) = 0 22 | * The second dot product is a constant, d, so each point gives us one 23 | * equation in the equation system (Ax=0): 24 | * [n_0] 25 | * . 26 | * [p_0,...,p_k,-1] . = 0 27 | * . 28 | * [n_k] 29 | * [ d ] 30 | * 31 | * If all k+1 points are linearly independent then the matrix A has a one 32 | * dimensional null space [A is a (k+1)X(k+2) matrix] which is the answer we seek. 33 | * 34 | */ 35 | template< unsigned int dimension > 36 | void PlaneParametersEstimator::estimate(std::vector< Point *> &data, 37 | std::vector ¶meters) 38 | { 39 | unsigned int i, j; 40 | double norm; 41 | 42 | parameters.clear(); 43 | //user forgot to initialize the minimal number of required 44 | //elements or there are not enough data elements for computation 45 | if( this->minForEstimate==0 || data.size() < this->minForEstimate ) 46 | return; 47 | 48 | if( dimension == 3 ) { //compute plane normal directly 49 | double nx,ny,nz; 50 | vnl_vector v1(3), v2(3); 51 | 52 | v1[0] = (*data[1])[0] - (*data[0])[0]; 53 | v1[1] = (*data[1])[1] - (*data[0])[1]; 54 | v1[2] = (*data[1])[2] - (*data[0])[2]; 55 | v2[0] = (*data[2])[0] - (*data[0])[0]; 56 | v2[1] = (*data[2])[1] - (*data[0])[1]; 57 | v2[2] = (*data[2])[2] - (*data[0])[2]; 58 | 59 | nx = v1[1]*v2[2] - v1[2]*v2[1]; 60 | ny = v1[2]*v2[0] - v1[0]*v2[2]; 61 | nz = v1[0]*v2[1] - v1[1]*v2[0]; 62 | norm = sqrt(nx*nx+ny*ny+nz*nz); 63 | 64 | if(norm< EPS) //points are collinear 65 | return; 66 | parameters.push_back(nx/norm); 67 | parameters.push_back(ny/norm); 68 | parameters.push_back(nz/norm); 69 | } 70 | else { //get the plane normal as the null space of the matrix described above 71 | vnl_matrix A( this->minForEstimate, 72 | this->minForEstimate+1 ); 73 | 74 | for( i=0; iminForEstimate; i++ ) { 75 | Point &pnt = *(data[i]); 76 | for( j=0; jminForEstimate; j++ ) 77 | A(i,j) = pnt[j]; 78 | A(i,j) = -1; 79 | } 80 | 81 | vnl_svd svdA( A ); 82 | //explicitly zero out small singular values 83 | svdA.zero_out_absolute( EPS ); 84 | //the points are linearly dependent, need at least k linearly 85 | //independent points (gives us rank(A)=k) 86 | if( svdA.rank()minForEstimate ) 87 | return; 88 | 89 | //the one dimensional null space of A is the solution we seek 90 | vnl_vector x(this->minForEstimate+1); 91 | x = svdA.nullvector(); 92 | //get the hyperplane normal, we need to set it so ||n||=1, this 93 | //means we need to scale our solution to be 94 | // 1/||n_computed||*[n_computed,d] which is also a solution to the 95 | //equation system. 96 | norm = 0; 97 | for( i=0; iminForEstimate; i++ ) { 98 | norm+=x[i]*x[i]; 99 | parameters.push_back( x[i] ); 100 | } 101 | norm = 1.0/sqrt(norm); 102 | for( i=0; iminForEstimate; i++ ) 103 | parameters[i]*=norm; 104 | } 105 | //first point is arbitrarily chosen to be the 106 | //"point on plane" 107 | for( i=0; i 115 | void PlaneParametersEstimator::estimate(std::vector< Point > &data, 116 | std::vector ¶meters) 117 | { 118 | std::vector< Point *> usedData; 119 | unsigned int dataSize = static_cast(data.size()); 120 | for(unsigned int i=0; i 129 | void PlaneParametersEstimator::leastSquaresEstimate(std::vector< Point *> &data, 130 | std::vector ¶meters) 131 | { 132 | parameters.clear(); //not enough data elements for computation 133 | if(data.size()minForEstimate) 134 | return; 135 | 136 | unsigned int i, j, k, pointNum = static_cast(data.size()); 137 | vnl_matrix meanMat(dimension, dimension), covariance(dimension,dimension,0); 138 | vnl_vector mean(dimension,0); 139 | 140 | //create covariance matrix 141 | double sqrtN = sqrt((double)pointNum); 142 | for(i=0; i eigenSystem(covariance); 164 | 165 | //the hyperplane normal is the eigenvector corresponding to 166 | //the smallest eigenvalue 167 | //I assume ||eigenSystem.V(i,0)|| = 1 168 | for(i=0; i 178 | void PlaneParametersEstimator::leastSquaresEstimate(std::vector< Point > &data, 179 | std::vector ¶meters) 180 | { 181 | std::vector< Point *> usedData; 182 | unsigned int dataSize = static_cast(data.size()); 183 | for(unsigned int i=0; i 196 | bool PlaneParametersEstimator::agree(std::vector ¶meters, 197 | Point &data) 198 | { 199 | double signedDistance = 0; 200 | for(unsigned int i=0; ideltaSquared); 203 | } 204 | 205 | } //namespace lsqrRecipes 206 | 207 | #endif //_PLANE_PARAMETERS_ESTIMATOR_TXX_ 208 | -------------------------------------------------------------------------------- /parametersEstimators/DenseLinearEquationSystemParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _DENSE_LINEAR_EQUATION_SYSTEM_PARAMETERS_ESTIMATOR_H_ 2 | #define _DENSE_LINEAR_EQUATION_SYSTEM_PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include 5 | #include "copyright.h" 6 | 7 | #include "ParametersEstimator.h" 8 | 9 | 10 | namespace lsqrRecipes { 11 | 12 | 13 | /** 14 | * Template defining the n+1 augmented row structure of the matrix equation 15 | * system Ax=b, [a_0,a_1,...a_{n-1},b]. Valid types for row entries are the 16 | * numeric types that represent real numbers (float, double). 17 | * 18 | * @author: Ziv Yaniv 19 | */ 20 | template 21 | class AugmentedRow { 22 | public: 23 | //enable compile time access to the row dimension, ugly but necessary 24 | enum {dimension=n}; 25 | 26 | /** 27 | * Output the augmented row data to the given output stream as the following string: 28 | *[a_0,a_1,...a_{n-1},b] 29 | */ 30 | friend std::ostream &operator<<(std::ostream& output, const AugmentedRow &r) { 31 | output<<"[ "; 32 | for(unsigned int i=0; i &operator =(const AugmentedRow &other) { 42 | memcpy(this->aValues, other.aValues, n*sizeof(T)); this->bValue = other.bValue; 43 | return *this; 44 | } 45 | 46 | /** 47 | * Access to the entries of the augmented row. No bounds checking is performed. 48 | * Valid indexes are in [0,n], corresponding to [a_0...a_{n-1},b]. 49 | */ 50 | T & operator[](unsigned int index) {return index==n ? this->bValue : this->aValues[index];} 51 | const T & operator[](unsigned int index) const {return index==n ? this->bValue : this->aValues[index];} 52 | 53 | /** 54 | * Default constructor, sets the augmented row to zero. 55 | */ 56 | AugmentedRow() { 57 | memset(this->aValues,0,n*sizeof(T)); 58 | this->bValue = static_cast(0.0); 59 | } 60 | 61 | /** 62 | * Construct an augmented row from the given pointer to an array. 63 | * @param fillData An array that has at least 'n+1' elements. 64 | */ 65 | AugmentedRow(T *fillData) { 66 | memcpy(this->aValues,fillData,n*sizeof(T)); 67 | this->bValue = fillData[n]; 68 | } 69 | 70 | /** 71 | * Construct an augmented row from the given pointer to an array and b value. 72 | * @param fillData An array that has at least 'n' elements. 73 | * @param bData Set the augmented element to this value. 74 | */ 75 | AugmentedRow(T *fillData, T bData) { 76 | memcpy(this->aValues,fillData,n*sizeof(T)); 77 | this->bValue = bData; 78 | } 79 | 80 | /** 81 | * Copy constructor. 82 | * @param other The augmented row we copy. 83 | */ 84 | AugmentedRow(const AugmentedRow &other) { 85 | memcpy(this->aValues,other.aValues,n*sizeof(T)); 86 | this->bValue = other.bValue; 87 | } 88 | 89 | /** 90 | * Fill the augmented row from the given pointer to an array. 91 | * @param fillData An array that has at least 'n+1' elements. 92 | */ 93 | void set(T *fillData) { 94 | memcpy(this->aValues,fillData,n*sizeof(T)); 95 | this->bValue = fillData[n]; 96 | } 97 | 98 | /** 99 | * Fill the augmented row from the given pointer to an array and b value. 100 | * @param fillData An array that has at least 'n' elements. 101 | * @param bData Set the augmented element to this value. 102 | */ 103 | void set(T *fillData, T bData) { 104 | memcpy(this->aValues,fillData,n*sizeof(T)); 105 | this->bValue = bData; 106 | } 107 | 108 | /** 109 | * Get the augmented row data. 110 | * @param aValues An array that has at least 'n' elements. 111 | * @param bValue A scalar of the same type as the array. 112 | */ 113 | void get(T *aValues, T &bValue) { 114 | memcpy(aValues, this->aValues,n*sizeof(T)); 115 | bValue = this->bValue; 116 | } 117 | 118 | /** 119 | * Get the augmented row data. 120 | * @param aValues An array that has at least 'n+1' elements. 121 | */ 122 | void get(T *aValues) { 123 | memcpy(aValues, this->aValues,n*sizeof(T)); 124 | aValues[n] = this->bValue; 125 | } 126 | 127 | /** 128 | * Return the dimensionality of the augmented row. 129 | */ 130 | unsigned int size() {return n+1;} 131 | 132 | private: 133 | T aValues[n]; 134 | T bValue; 135 | }; 136 | 137 | 138 | /** 139 | * This class estimates the parameters/solution of a dense overdetermined linear 140 | * equation system in a least squares sense, x = argmin_x \|Ax-b\|. 141 | * The solution is given by the normal equations: x = (A^TA)^{-1}A^Tb 142 | * We provide the unique solution if it exists (rank(A)=colNum), otherwise we 143 | * return an empty vector. 144 | * 145 | * @author: Ziv Yaniv 146 | * 147 | */ 148 | template //n is the number of columns in A 149 | class DenseLinearEquationSystemParametersEstimator : public ParametersEstimator< AugmentedRow, T> { 150 | public: 151 | 152 | 153 | 154 | /** 155 | * Object constructor. 156 | * @param delta An equation a^Tx= b is consistent with the solution x if 157 | * abs(a^Tx - b)< delta . 158 | */ 159 | DenseLinearEquationSystemParametersEstimator(T delta); 160 | 161 | /** 162 | * Compute the solution to the equation system defined by the given augmented 163 | * rows. 164 | * @param data A vector containing n augmented rows of the form [a_0,a_1,...a_{n-1},b]. 165 | * @param parameters This vector is cleared and then filled with the computed 166 | * parameters, The solution to the equation system: 167 | * [a_{0,0} a_{0,1} ... a_{0,n-1}] [x_0] [b_0] 168 | * [ : : : ] [ : ] = [ : ] 169 | * [a_{n,0} a_{n,1} ... a_{n,n-1}] [x_{n-1}] [b_{n-1}] 170 | * 171 | * If the data contains less than n augmented rows, or 172 | * A is not invertible then the resulting parameters 173 | * vector is empty (size = 0). 174 | */ 175 | virtual void estimate(std::vector< AugmentedRow *> &data, 176 | std::vector ¶meters); 177 | virtual void estimate(std::vector< AugmentedRow > &data, 178 | std::vector ¶meters); 179 | 180 | /** 181 | * Compute a least squares solution to the equation system defined by the given 182 | * augmented rows. 183 | * 184 | * @param data The solution minimizes the least squares error of the equation 185 | * system. 186 | * @param parameters This vector is cleared and then filled with the computed 187 | * parameters, The solution to the over determined equation 188 | * system: Ax = b 189 | * 190 | * If the data contains less than n augmented rows, or 191 | * rank(A)!= n (no unique solution) then the resulting 192 | * parameters vector is empty (size = 0). 193 | */ 194 | virtual void leastSquaresEstimate(std::vector< AugmentedRow *> &data, 195 | std::vector ¶meters); 196 | virtual void leastSquaresEstimate(std::vector< AugmentedRow > &data, 197 | std::vector ¶meters); 198 | 199 | /** 200 | * Return true if the error for the specific equation is smaller than 'delta' 201 | * (see constructor). 202 | * @param parameters The potential solution [x_0,...,x_{n-1}]. 203 | * @param data Check that the error, |dot(a,x) - b|, is less than 'delta'. 204 | * 205 | * Note:If the parameters vector is too short the method will throw an 206 | * exception as it tries to access a non existing entry. 207 | * 208 | */ 209 | virtual bool agree(std::vector ¶meters, AugmentedRow &data); 210 | 211 | /** 212 | * Change the tolerance defining whether an equation is consistent with a 213 | * given solution. 214 | * @param delta An equation a^Tx= b is consistent with the solution x if 215 | * abs(a^Tx - b)< delta. 216 | */ 217 | void setDelta(T delta) {this->delta = delta;} 218 | 219 | /** 220 | * Utility method for transforming an equation system represented by a vnl 221 | * matrix and vector into the representation used by our class, augmented rows. 222 | */ 223 | static void getAugmentedRows(vnl_matrix &A, vnl_vector &b, 224 | std::vector< AugmentedRow > &rows); 225 | 226 | private: 227 | //given solution x and equation a^Tx=b, if abs(a^Tx - b) < delta then the 228 | //equation is consistent with the solution 229 | double delta; 230 | }; 231 | 232 | } //namespace lsqrRecipes 233 | 234 | #include "DenseLinearEquationSystemParametersEstimator.hxx" //the implementation is in this file 235 | 236 | #endif //_DENSE_LINEAR_EQUATION_SYSTEM_PARAMETERS_ESTIMATOR_H_ 237 | -------------------------------------------------------------------------------- /examples/linearEquationSystemSolver.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "RANSAC.h" 7 | #include "RandomNumberGenerator.h" 8 | #include "DenseLinearEquationSystemParametersEstimator.h" 9 | #include "Vector.h" 10 | 11 | /** 12 | * Create a random equation system, scale a number of the equations so that they 13 | * become outliers and estimate the solution. This system only includes outliers 14 | * and no noise. 15 | */ 16 | void simulatedExample(); 17 | 18 | /** 19 | * Estimate the solution of an equation system that includes both noise and 20 | * outliers. This equation system solves the pivot calibration task, with data 21 | * acquired using an optically tracked pointer tool. We intentionally introduced 22 | * 30% outliers by arbitrarily rotation and translating the tool. 23 | */ 24 | void experimentalExample(char *inputFileName); 25 | 26 | 27 | /** 28 | * Show how to use the RANSAC framework as a robust approach to solving a set of 29 | * linear equations. 30 | * 31 | * @author Ziv Yaniv 32 | */ 33 | int main(int argc, char *argv[]) 34 | { 35 | simulatedExample(); 36 | if(argc==2) 37 | experimentalExample(argv[1]); 38 | } 39 | 40 | /*****************************************************************************/ 41 | 42 | void simulatedExample() 43 | { 44 | const unsigned int ROW_NUM = 200; 45 | const unsigned int COL_NUM = 5; 46 | double scaleFactor = 20.0; 47 | bool ok; 48 | unsigned int i,j; 49 | 50 | const unsigned int OUTLIER_NUM = 5; 51 | // indexes to equations that will be outliers (for the fun 52 | //of it use prime numbers) 53 | unsigned int outlierIndexes[OUTLIER_NUM] = {13, 37, 67, 137, 179}; 54 | 55 | vnl_matrix A(ROW_NUM, COL_NUM); 56 | vnl_vector b(ROW_NUM), x(COL_NUM); 57 | lsqrRecipes::Vector tmp; 58 | lsqrRecipes::RandomNumberGenerator random; 59 | 60 | //display two digits after the decimal point, settings are returned 61 | //to their original values at the end of the method 62 | std::ios::fmtflags previousFlags = 63 | std::cout.setf(std::ios::fixed, std::ios::floatfield); 64 | std::streamsize previousPrecision = std::cout.precision(2); 65 | 66 | ok = false; 67 | while (!ok) { 68 | for( i=0; i > augmentedRows; 85 | lsqrRecipes::DenseLinearEquationSystemParametersEstimator::getAugmentedRows( A, b, 86 | augmentedRows ); 87 | 88 | std::cout<<"Simulated example (without noise, with outliers)\n"; 89 | std::cout<<"------------------------------------------------\n"; 90 | 91 | std::cout<<"Known solution [x_0,..., x_{n-1}]:\n\t [ "; 92 | for( i=0; i estimatedSolution; 99 | lsqrRecipes::DenseLinearEquationSystemParametersEstimator equationSystemSolver(maximalEquationError); 100 | 101 | //estimate using a least squares approach 102 | equationSystemSolver.leastSquaresEstimate(augmentedRows, estimatedSolution); 103 | if( estimatedSolution.empty() ) 104 | std::cout<<"Least squares estimate failed, degenerate configuration?\n"; 105 | else { 106 | std::cout<<"Least squares estimated solution [x_0,..., x_{n-1}]:\n\t [ "; 107 | for(i=0; i, double>::compute(estimatedSolution, 122 | &equationSystemSolver, 123 | augmentedRows, 124 | desiredProbabilityForNoOutliers); 125 | if( estimatedSolution.empty() ) 126 | std::cout<<"RANSAC estimate failed, degenerate configuration?\n"; 127 | else 128 | { 129 | std::cout<<"RANSAC estimated solution [x_0,..., x_{n-1}]:\n\t [ "; 130 | for(i=0; i row; 151 | std::vector< lsqrRecipes::AugmentedRow > augmentedRows; 152 | unsigned int i; 153 | 154 | std::ifstream in; 155 | in.open(inputFileName); 156 | if(!in.is_open()) { 157 | std::cerr<<"Failed to open input file ("<>data[0]>>data[1]>>data[2]>>data[3]>>data[4]>>data[5]>>data[6]) { 161 | row.set(data); 162 | augmentedRows.push_back(row); 163 | } 164 | in.close(); 165 | 166 | if(augmentedRows.empty()) { 167 | std::cerr<<"Failed to load augmented matrix file.\n"; 168 | return; 169 | } 170 | 171 | //display two digits after the decimal point, settings are returned 172 | //to their original values at the end of the method 173 | std::ios::fmtflags previousFlags = 174 | std::cout.setf(std::ios::fixed, std::ios::floatfield); 175 | std::streamsize previousPrecision = std::cout.precision(2); 176 | 177 | std::cout<<"Experimental example acquired for pivot calibration (noise + outliers)\n"; 178 | std::cout<<"----------------------------------------------------------------------\n"; 179 | 180 | std::cout<<"Correct and approximately known solution [x_0,..., x_{n-1}]:\n\t"; 181 | std::cout<<"[ -17.0, 1.0, -157.0, 147.0, -63.0, -1042.0]\n"; 182 | 183 | //create and initialize the parameter estimator 184 | double maximalEquationError = sqrt(static_cast(1.0/3.0));//0.3; 185 | std::vector estimatedSolution; 186 | lsqrRecipes::DenseLinearEquationSystemParametersEstimator equationSystemSolver(maximalEquationError); 187 | 188 | //estimate using a least squares approach 189 | equationSystemSolver.leastSquaresEstimate(augmentedRows, estimatedSolution); 190 | if( estimatedSolution.empty() ) 191 | std::cout<<"Least squares estimate failed, degenerate configuration?\n"; 192 | else { 193 | std::cout<<"Least squares estimated solution [x_0,..., x_{n-1}]:\n\t [ "; 194 | for(i=0; i, double>::compute(estimatedSolution, 204 | &equationSystemSolver, 205 | augmentedRows, 206 | desiredProbabilityForNoOutliers); 207 | if( estimatedSolution.empty() ) 208 | std::cout<<"RANSAC estimate failed, degenerate configuration?\n"; 209 | else 210 | { 211 | std::cout<<"RANSAC estimated solution [x_0,..., x_{n-1}]:\n\t [ "; 212 | for(i=0; i 2 | #include 3 | #include "SinglePointTargetUSCalibrationParametersEstimator.h" 4 | #include "RANSAC.h" 5 | 6 | 7 | bool loadTransformations(const std::string & transformationsFileName, 8 | std::vector &transformations); 9 | 10 | bool loadImagePoints(const std::string &imagePointsFileName, 11 | size_t numberOfPoints, 12 | std::vector &imagePoints); 13 | 14 | int saveResults(const std::vector &estimatedUSCalibrationParameters, 15 | const std::vector &data, 16 | const std::vector &consensusSet, 17 | double percentageOfDataUsed, 18 | char *outputFileName); 19 | 20 | std::string getCurrentDateTime(const char* format); 21 | 22 | /** 23 | * Given the US-probe transformations and the corresponding 2D image coordinates 24 | * compute the US calibration, write the result to the command line console and 25 | * save in an xml file using the relevant IGSTK (www.igstk.org) tags for an 26 | * affine transformation. 27 | * 28 | * @author Ziv Yaniv 29 | */ 30 | int main(int argc, char *argv[]) 31 | { 32 | typedef lsqrRecipes::SingleUnknownPointTargetUSCalibrationParametersEstimator::DataType DataType; 33 | 34 | std::vector transformations; 35 | std::vector imagePoints; 36 | DataType dataElement; 37 | std::vector< DataType > data; 38 | std::vector estimatedUSCalibrationParameters; 39 | size_t i, n; 40 | 41 | if(argc != 4) { 42 | std::cerr<<"Unexpected number of command line parameters.\n"; 43 | std::cerr<<"Usage: \n"; 44 | std::cerr<<"\t"< consensusSet; 72 | percentageOfDataUsed = 73 | lsqrRecipes::RANSAC< DataType, double>::compute(estimatedUSCalibrationParameters, 74 | &usCalibration, 75 | data, 76 | desiredProbabilityForNoOutliers, 77 | &consensusSet); 78 | 79 | if(estimatedUSCalibrationParameters.size() == 0) { 80 | std::cout<<"FAILED CALIBRATION, possibly degenerate configuration\n\n\n"; 81 | return EXIT_FAILURE; 82 | } 83 | else 84 | return saveResults(estimatedUSCalibrationParameters, data, consensusSet, percentageOfDataUsed, argv[3]); 85 | } 86 | 87 | /*****************************************************************************/ 88 | bool loadTransformations(const std::string & transformationsFileName, 89 | std::vector &transformations) 90 | { 91 | double R[3][3], t[3]; 92 | lsqrRecipes::Frame f; 93 | 94 | transformations.clear(); 95 | 96 | std::ifstream in; 97 | in.open(transformationsFileName.c_str()); 98 | if(!in.is_open()) 99 | return false; 100 | while(in>>R[0][0]>>R[0][1]>>R[0][2]>>t[0]>>R[1][0]>>R[1][1]>>R[1][2]>>t[1]>>R[2][0]>>R[2][1]>>R[2][2]>>t[2]) { 101 | f.setRotationMatrix(R); 102 | f.setTranslation(t); 103 | transformations.push_back(f); 104 | } 105 | in.close(); 106 | 107 | if(!transformations.empty()) 108 | return true; 109 | return false; 110 | } 111 | 112 | /*****************************************************************************/ 113 | bool loadImagePoints(const std::string &imagePointsFileName, 114 | size_t numberOfPoints, 115 | std::vector &imagePoints) 116 | { 117 | lsqrRecipes::Point2D p; 118 | 119 | imagePoints.clear(); 120 | 121 | std::ifstream in; 122 | in.open(imagePointsFileName.c_str()); 123 | if(!in.is_open()) 124 | return false; 125 | 126 | while(in>>p[0]>>p[1] && imagePoints.size() &estimatedUSCalibrationParameters, 138 | const std::vector &data, 139 | const std::vector &consensusSet, 140 | double percentageOfDataUsed, 141 | char *outputFileName) 142 | { //should never happen, but just in case 143 | if(estimatedUSCalibrationParameters.size() == 0) 144 | return EXIT_FAILURE; 145 | 146 | std::cout<<"Percentage of data used in estimate: "< errors; 161 | double minErr, maxErr, meanErr; 162 | 163 | std::vector dataUsed; 164 | size_t i,n; 165 | n= data.size(); 166 | for(i=0; i\n\n\n\n"; 192 | out<<"\n\n"; 193 | out<<"\t\n"; 194 | out<<"\t"<<"US calibration - Crosswire Phantom"<<"\n"; 195 | out<<"\t\n\n"; 196 | out<<"\t\n"; 197 | out<<"\t"<\n\n"; 199 | out<<"\t \n"; 201 | out<<"\t"<\n\n"; 208 | out<<"\n"; 209 | out.close(); 210 | return EXIT_SUCCESS; 211 | } 212 | /*****************************************************************************/ 213 | std::string getCurrentDateTime(const char* format) 214 | { 215 | char buf[1024]; 216 | time_t t; 217 | time(&t); 218 | strftime(buf, sizeof(buf), format, localtime(&t)); 219 | return std::string(buf); 220 | } 221 | -------------------------------------------------------------------------------- /parametersEstimators/SphereParametersEstimator.h: -------------------------------------------------------------------------------- 1 | #ifndef _SPHERE_PARAMETERS_ESTIMATOR_H_ 2 | #define _SPHERE_PARAMETERS_ESTIMATOR_H_ 3 | 4 | #include "copyright.h" 5 | 6 | #include 7 | #include "ParametersEstimator.h" 8 | #include "Point.h" 9 | 10 | 11 | /** 12 | * This class estimates the parameters of a hypersphere (in 2D this is a circle, 13 | * in 3D this is a sphere...). 14 | * A sphere is represented as: (1) (p-c)^T(p-c) = sum(p_i-c_i)^2 = r^2 15 | * where p in R^n is a point on the hypersphere, c in R^n is the sphere's 16 | * center, and r its radius. 17 | * 18 | * @author: Ziv Yaniv 19 | * 20 | */ 21 | namespace lsqrRecipes { 22 | 23 | template< unsigned int dimension > 24 | class SphereParametersEstimator : 25 | public ParametersEstimator< Point,double> { 26 | public: 27 | 28 | enum LeastSquaresType {ALGEBRAIC = 0, GEOMETRIC}; 29 | 30 | /** 31 | * Object constructor. 32 | * @param delta A point is on the sphere if its distance from the sphere is 33 | * less than 'delta'. 34 | * @param lsType When the leastSquaresEstimate() method is called it computes 35 | * an algebraic or geometric fit. This flag tells it which one. 36 | */ 37 | SphereParametersEstimator(double delta, LeastSquaresType lsType = GEOMETRIC); 38 | 39 | /** 40 | * Compute the hypersphere defined by the given data points. 41 | * @param data A vector containing k+1 kD points. 42 | * @param parameters This vector is cleared and then filled with the computed 43 | * parameter values. The parameters of the (hyper)sphere 44 | * passing through these points [c_0,...,c_k,r]. 45 | * If the vector contains less than k+1 points or the points 46 | * are linearly dependent (e.g. four coplanar points in the 47 | * 3D case) then the resulting parameters vector is empty 48 | * (size = 0). 49 | */ 50 | virtual void estimate(std::vector< Point * > &data, 51 | std::vector ¶meters); 52 | virtual void estimate(std::vector< Point > &data, 53 | std::vector ¶meters); 54 | 55 | /** 56 | * Compute a least squares estimate of the hypersphere defined by the given 57 | * points. This may be either an algebraic or geometric least squares 58 | * computation depending on the LeastSquaresType settings. For further 59 | * information see geometricLeastSquaresEstimate(), and 60 | * algebraicLeastSquaresEstimate(). 61 | * @param data The sphere should minimize the least squares error to these 62 | * points. 63 | * @param parameters This vector is cleared and then filled with the computed 64 | * parameters. The parameters of the (hyper)sphere passing 65 | * through these points [c_0,...,c_k,r]. If the vector 66 | * contains less than k+1 points or the points are linearly 67 | * dependent (e.g. all points are coplanar in the 3D case) 68 | * then the resulting parameters vector is empty (size = 0). 69 | */ 70 | virtual void leastSquaresEstimate(std::vector< Point *> &data, 71 | std::vector ¶meters); 72 | virtual void leastSquaresEstimate(std::vector< Point > &data, 73 | std::vector ¶meters); 74 | 75 | /** 76 | * Return true if the geometric distance between the hypersphere and the 77 | * given point is smaller than 'delta' (see constructor). 78 | * 79 | * @param parameters The sphere parameters [c_0,...,c_k,r]. 80 | * @param data Check that the geometric distance between this point and the 81 | * sphere is smaller than 'delta'. 82 | * Note:If the parameters vector is too short the method 83 | * will throw an exception as it tries to access a non existing entry. 84 | */ 85 | virtual bool agree(std::vector ¶meters, 86 | Point &data); 87 | 88 | /** 89 | * Change the distance defining if a point is on the hypersphere or not. 90 | * @param delta A point is on the sphere if its distance from it is less than 91 | * 'delta'. 92 | */ 93 | void setDelta(double delta) {this->delta = delta;} 94 | 95 | /** 96 | * Change the type of least squares solution. 97 | * @param lsType When the leastSquaresEstimate() method is called it computes 98 | * an algebraic or geometric fit. This flag tells it which one. 99 | */ 100 | void setLeastSquaresType(LeastSquaresType lsType) {this->lsType = lsType;} 101 | 102 | /** 103 | * Compute a least squares estimate of the hypersphere defined by the given 104 | * points. 105 | * This implementation is of an algebraic least squares error: 106 | * min||Ax-b||, where A = [-2p_0,-2p_1,...,-2p_{k-1},1], 107 | * x = [c_0,c_1,...,c_{k-1},d], 108 | * b = [-p_0^2 - p_1^2 - ... -p_{k-1}^2] 109 | * 110 | * @param data The hypersphere should minimize the algebraic least squares 111 | * error to these points. 112 | * @param parameters This vector is cleared and then filled with the computed 113 | * parameters. The parameters of the (hyper)sphere passing 114 | * through these points [c_0,...,c_k,r]. If the vector 115 | * contains less than k+1 points or the points are linearly 116 | * dependent (e.g. all points are coplanar in the 3D case) 117 | * then the resulting parameters vector is empty (size = 0). 118 | */ 119 | void algebraicLeastSquaresEstimate(std::vector< Point *> &data, 120 | std::vector ¶meters); 121 | 122 | /** 123 | * Compute a least squares estimate of the circle defined by the given points. 124 | * This implementation is of a geometric least squares error: 125 | * min (sum (r - sqrt((p_0-c_0)^2 + (p_1-c_1)^2 +...+ (p_{k-1}-c_{k-1})^2))^2 126 | * 127 | * @param data The (hyper)sphere should minimize the geometric least squares 128 | * error to these points. 129 | * @param initialParameters This vector contains the initial parameter values 130 | * for nonlinear estimation. 131 | * @param finalParameters This vector is cleared and then filled with the 132 | * computed sphere parameters [centerX,centerY,centerZ,r]. 133 | * 134 | * NOTE: As this method encapsulates nonlinear optimization (Levenberg-Marquardt) 135 | * it may not converge due to the specific choice of LM parameter settings. 136 | * Possibly modify these values to be appropriate for user's specific input. 137 | */ 138 | void geometricLeastSquaresEstimate(std::vector< Point *> &data, 139 | std::vector &initialParameters, 140 | std::vector &finalParameters); 141 | 142 | /** 143 | * Get the distances between the given hypersphere and the given points. 144 | * These are pushed at the end of the 'distances' vector. 145 | * 146 | * @param parameters The (hyper)sphere parameters [c,r]. 147 | * @param data The points whose distance from the (hyper)sphere we want. 148 | * @param distances The computed distances. If the vector is empty then the 149 | * point and distance indexes match, 150 | * otherwise there is an offset due to the original number 151 | * of entries previously found in the vector. 152 | * @param min Minimal distance. 153 | * @param max Maximal distance. 154 | * @param mean Mean distance. 155 | */ 156 | static void getDistanceStatistics( std::vector ¶meters, 157 | std::vector< Point > &data, 158 | std::vector &distances, 159 | double &min, double &max, double &mean ); 160 | 161 | private: 162 | enum {CIRCLE = 2, SPHERE=3}; 163 | double delta; 164 | //algebraic or geometric least squares 165 | LeastSquaresType lsType; 166 | 167 | static const double SPHERE_EPS; 168 | 169 | inline void estimate2D(std::vector< Point * > &data, 170 | std::vector ¶meters); 171 | inline void estimate3D(std::vector< Point * > &data, 172 | std::vector ¶meters); 173 | inline void estimateND(std::vector< Point * > &data, 174 | std::vector ¶meters); 175 | 176 | class SumSquaresSpherePointsDistanceFunction : public vnl_least_squares_function { 177 | public: 178 | SumSquaresSpherePointsDistanceFunction(std::vector *> *data); 179 | void setPoints(std::vector *> *data); 180 | virtual void f( vnl_vector const &x, vnl_vector &fx ); 181 | virtual void gradf( vnl_vector const& x, vnl_matrix& jacobian ); 182 | private: 183 | std::vector *> *data; 184 | }; 185 | 186 | }; 187 | 188 | } //namespace lsqrRecipes 189 | 190 | #include "SphereParametersEstimator.hxx" //the implementation is in this file 191 | 192 | #endif //_SPHERE_PARAMETERS_ESTIMATOR_H_ 193 | -------------------------------------------------------------------------------- /examples/planeUSCalibration.cxx: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "PlanePhantomUSCalibrationParametersEstimator.h" 4 | #include "RANSAC.h" 5 | 6 | 7 | bool loadTransformations(const std::string & transformationsFileName, 8 | std::vector &transformations); 9 | 10 | bool loadImagePoints(const std::string &imagePointsFileName, 11 | size_t numberOfPoints, 12 | std::vector &imagePoints); 13 | 14 | int saveResults(const std::vector &estimatedUSCalibrationParameters, 15 | const std::vector &data, 16 | const std::vector &consensusSet, 17 | double percentageOfDataUsed, 18 | char *outputFileName); 19 | 20 | std::string getCurrentDateTime(const char* format); 21 | 22 | /** 23 | * Given the US-probe transformations and the corresponding 2D image coordinates 24 | * compute the US calibration, write the result to the command line console and 25 | * save in an xml file using the relevant IGSTK (www.igstk.org) tags for an 26 | * affine transformation. 27 | * 28 | * @author Ziv Yaniv 29 | */ 30 | int main(int argc, char *argv[]) 31 | { 32 | typedef lsqrRecipes::PlanePhantomUSCalibrationParametersEstimator::DataType DataType; 33 | 34 | std::vector transformations; 35 | std::vector imagePoints; 36 | DataType dataElement; 37 | std::vector< DataType > data; 38 | std::vector estimatedUSCalibrationParameters; 39 | size_t i, n; 40 | 41 | if(argc != 4) { 42 | std::cerr<<"Unexpected number of command line parameters.\n"; 43 | std::cerr<<"Usage: \n"; 44 | std::cerr<<"\t"< consensusSet; 72 | 73 | percentageOfDataUsed = 74 | lsqrRecipes::RANSAC< DataType, double>::compute(estimatedUSCalibrationParameters, 75 | &usCalibration, 76 | data, 77 | desiredProbabilityForNoOutliers, 78 | &consensusSet); 79 | if(estimatedUSCalibrationParameters.size() == 0) { 80 | std::cout<<"FAILED CALIBRATION, possibly degenerate configuration\n\n\n"; 81 | return EXIT_FAILURE; 82 | } 83 | else 84 | return saveResults(estimatedUSCalibrationParameters, data, consensusSet, percentageOfDataUsed, argv[3]); 85 | } 86 | 87 | /*****************************************************************************/ 88 | bool loadTransformations(const std::string & transformationsFileName, 89 | std::vector &transformations) 90 | { 91 | double R[3][3], t[3]; 92 | lsqrRecipes::Frame f; 93 | 94 | transformations.clear(); 95 | 96 | std::ifstream in; 97 | in.open(transformationsFileName.c_str()); 98 | if(!in.is_open()) 99 | return false; 100 | while(in>>R[0][0]>>R[0][1]>>R[0][2]>>t[0]>>R[1][0]>>R[1][1]>>R[1][2]>>t[1]>>R[2][0]>>R[2][1]>>R[2][2]>>t[2]) { 101 | f.setRotationMatrix(R); 102 | f.setTranslation(t); 103 | transformations.push_back(f); 104 | } 105 | in.close(); 106 | 107 | if(!transformations.empty()) 108 | return true; 109 | return false; 110 | } 111 | 112 | /*****************************************************************************/ 113 | bool loadImagePoints(const std::string &imagePointsFileName, 114 | size_t numberOfPoints, 115 | std::vector &imagePoints) 116 | { 117 | lsqrRecipes::Point2D p; 118 | 119 | imagePoints.clear(); 120 | 121 | std::ifstream in; 122 | in.open(imagePointsFileName.c_str()); 123 | if(!in.is_open()) 124 | return false; 125 | 126 | while(in>>p[0]>>p[1] && imagePoints.size() &estimatedUSCalibrationParameters, 138 | const std::vector &data, 139 | const std::vector &consensusSet, 140 | double percentageOfDataUsed, 141 | char *outputFileName) 142 | { //should never happen, but just in case 143 | if(estimatedUSCalibrationParameters.size() == 0) 144 | return EXIT_FAILURE; 145 | 146 | std::cout<<"Percentage of data used in estimate: "< errors; 162 | double minErr, maxErr, meanErr; 163 | 164 | std::vector dataUsed; 165 | size_t i,n; 166 | n= data.size(); 167 | for(i=0; i\n\n\n\n"; 193 | out<<"\n\n"; 194 | out<<"\t\n"; 195 | out<<"\t"<<"US calibration - Plane Phantom"<<"\n"; 196 | out<<"\t\n\n"; 197 | out<<"\t\n"; 198 | out<<"\t"<\n\n"; 200 | out<<"\t \n"; 202 | 203 | double cx,cy,cz,sx,sy,sz,m_x,m_y; 204 | cx = cos(estimatedUSCalibrationParameters[8]); 205 | cy = cos(estimatedUSCalibrationParameters[7]); 206 | cz = cos(estimatedUSCalibrationParameters[6]); 207 | sx = sin(estimatedUSCalibrationParameters[8]); 208 | sy = sin(estimatedUSCalibrationParameters[7]); 209 | sz = sin(estimatedUSCalibrationParameters[6]); 210 | m_x = estimatedUSCalibrationParameters[9]; 211 | m_y = estimatedUSCalibrationParameters[10]; 212 | out<<"\t"<\n\n"; 219 | out<<"\n"; 220 | out.close(); 221 | return EXIT_SUCCESS; 222 | } 223 | /*****************************************************************************/ 224 | std::string getCurrentDateTime(const char* format) 225 | { 226 | char buf[1024]; 227 | time_t t; 228 | time(&t); 229 | strftime(buf, sizeof(buf), format, localtime(&t)); 230 | return std::string(buf); 231 | } 232 | --------------------------------------------------------------------------------