TransformationCheckersImpl.cpp
Go to the documentation of this file.
00001 // kate: replace-tabs off; indent-width 4; indent-mode normal
00002 // vim: ts=4:sw=4:noexpandtab
00003 /*
00004 
00005 Copyright (c) 2010--2012,
00006 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
00007 You can contact the authors at <f dot pomerleau at gmail dot com> and
00008 <stephane at magnenat dot net>
00009 
00010 All rights reserved.
00011 
00012 Redistribution and use in source and binary forms, with or without
00013 modification, are permitted provided that the following conditions are met:
00014     * Redistributions of source code must retain the above copyright
00015       notice, this list of conditions and the following disclaimer.
00016     * Redistributions in binary form must reproduce the above copyright
00017       notice, this list of conditions and the following disclaimer in the
00018       documentation and/or other materials provided with the distribution.
00019     * Neither the name of the <organization> nor the
00020       names of its contributors may be used to endorse or promote products
00021       derived from this software without specific prior written permission.
00022 
00023 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00024 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00025 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00026 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
00027 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00028 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00030 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00031 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 
00034 */
00035 
00036 #include "TransformationCheckersImpl.h"
00037 #include "Functions.h"
00038 #include <boost/math/special_functions/fpclassify.hpp> 
00039 
00040 using namespace std;
00041 using namespace PointMatcherSupport;
00042 
00043 //--------------------------------------
00044 // max iteration counter
00045 template<typename T>
00046 TransformationCheckersImpl<T>::CounterTransformationChecker::CounterTransformationChecker(const Parameters& params):
00047         TransformationChecker("CounterTransformationChecker", CounterTransformationChecker::availableParameters(), params),
00048         maxIterationCount(Parametrizable::get<unsigned>("maxIterationCount"))
00049 {
00050         this->limits.setZero(1);
00051         this->limits(0) = maxIterationCount;
00052 
00053         this->conditionVariableNames.push_back("Iteration");
00054         this->limitNames.push_back("Max iteration");
00055 }
00056 
00057 template<typename T>
00058 void TransformationCheckersImpl<T>::CounterTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
00059 {
00060         this->conditionVariables.setZero(1);
00061 }
00062 
00063 template<typename T>
00064 void TransformationCheckersImpl<T>::CounterTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00065 {
00066         this->conditionVariables(0)++;
00067         
00068         //std::cout << "Iter: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
00069         //cerr << parameters << endl;
00070         
00071         if (this->conditionVariables(0) >= this->limits(0))
00072                 iterate = false;
00073 }
00074 
00075 template struct TransformationCheckersImpl<float>::CounterTransformationChecker;
00076 template struct TransformationCheckersImpl<double>::CounterTransformationChecker;
00077 
00078 
00079 //--------------------------------------
00080 // error
00081 template<typename T>
00082 TransformationCheckersImpl<T>::DifferentialTransformationChecker::DifferentialTransformationChecker(const Parameters& params):
00083         TransformationChecker("DifferentialTransformationChecker", DifferentialTransformationChecker::availableParameters(), params),
00084         minDiffRotErr(Parametrizable::get<T>("minDiffRotErr")),
00085         minDiffTransErr(Parametrizable::get<T>("minDiffTransErr")),
00086         smoothLength(Parametrizable::get<unsigned>("smoothLength"))
00087 {
00088         this->limits.setZero(2);
00089         this->limits(0) = minDiffRotErr;
00090         this->limits(1) = minDiffTransErr;
00091         
00092         this->conditionVariableNames.push_back("Mean abs differential rot err");
00093         this->conditionVariableNames.push_back("Mean abs differential trans err");
00094         this->limitNames.push_back("Min differential rotation err");
00095         this->limitNames.push_back("Min differential translation err");
00096 
00097 }
00098 
00099 template<typename T>
00100 void TransformationCheckersImpl<T>::DifferentialTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
00101 {
00102         this->conditionVariables.setZero(2);
00103         
00104         rotations.clear();
00105         translations.clear();
00106         
00107         if (parameters.rows() == 4)
00108         {
00109                 rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
00110         }
00111         else
00112         {
00113                 // Handle the 2D case
00114                 Eigen::Matrix<T,3,3> m(Matrix::Identity(3,3));
00115                 m.topLeftCorner(2,2) = parameters.topLeftCorner(2,2);
00116                 rotations.push_back(Quaternion(m));
00117         }
00118         
00119         const unsigned int nbRows = parameters.rows()-1;
00120         translations.push_back(parameters.topRightCorner(nbRows,1));
00121 }
00122 
00123 template<typename T>
00124 void TransformationCheckersImpl<T>::DifferentialTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00125 {
00126         typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00127         
00128         rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
00129         const unsigned int nbRows = parameters.rows()-1;
00130         translations.push_back(parameters.topRightCorner(nbRows,1));
00131         
00132         this->conditionVariables.setZero(2);
00133         if(rotations.size() > smoothLength)
00134         {
00135                 for(size_t i = rotations.size()-1; i >= rotations.size()-smoothLength; i--)
00136                 {
00137                         //Compute the mean derivative
00138                         this->conditionVariables(0) += anyabs(rotations[i].angularDistance(rotations[i-1]));
00139                         this->conditionVariables(1) += anyabs((translations[i] - translations[i-1]).norm());
00140                 }
00141 
00142                 this->conditionVariables /= smoothLength;
00143 
00144                 if(this->conditionVariables(0) < this->limits(0) && this->conditionVariables(1) < this->limits(1))
00145                         iterate = false;
00146         }
00147         
00148         //std::cout << "Abs Rotation: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
00149         //std::cout << "Abs Translation: " << this->conditionVariables(1) << " / " << this->limits(1) << std::endl;
00150         
00151         if (boost::math::isnan(this->conditionVariables(0)))
00152                 throw ConvergenceError("abs rotation norm not a number");
00153         if (boost::math::isnan(this->conditionVariables(1)))
00154                 throw ConvergenceError("abs translation norm not a number");
00155 }
00156 
00157 template struct TransformationCheckersImpl<float>::DifferentialTransformationChecker;
00158 template struct TransformationCheckersImpl<double>::DifferentialTransformationChecker;
00159 
00160 //--------------------------------------
00161 // bound
00162 
00163 template<typename T>
00164 TransformationCheckersImpl<T>::BoundTransformationChecker::BoundTransformationChecker(const Parameters& params):
00165         TransformationChecker("BoundTransformationChecker", BoundTransformationChecker::availableParameters(), params),
00166         maxRotationNorm(Parametrizable::get<T>("maxRotationNorm")),
00167         maxTranslationNorm(Parametrizable::get<T>("maxTranslationNorm"))
00168 {
00169         this->limits.setZero(2);
00170         this->limits(0) = maxRotationNorm;
00171         this->limits(1) = maxTranslationNorm;
00172 
00173         this->limitNames.push_back("Max rotation angle");
00174         this->limitNames.push_back("Max translation norm");
00175         this->conditionVariableNames.push_back("Rotation angle");
00176         this->conditionVariableNames.push_back("Translation norm");
00177 }
00178 
00179 template<typename T>
00180 void TransformationCheckersImpl<T>::BoundTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
00181 {
00182         this->conditionVariables.setZero(2);
00183         if (parameters.rows() == 4)
00184                 initialRotation3D = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
00185         else if (parameters.rows() == 3)
00186                 initialRotation2D = acos(parameters(0,0));
00187         else
00188                 throw runtime_error("BoundTransformationChecker only works in 2D or 3D");
00189                 
00190         const unsigned int nbRows = parameters.rows()-1;
00191         initialTranslation = parameters.topRightCorner(nbRows,1);
00192 }
00193 
00194 template<typename T>
00195 void TransformationCheckersImpl<T>::BoundTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00196 {
00197         typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00198         
00199         if (parameters.rows() == 4)
00200         {
00201                 const Quaternion currentRotation = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
00202                 this->conditionVariables(0) = currentRotation.angularDistance(initialRotation3D);
00203         }
00204         else if (parameters.rows() == 3)
00205         {
00206                 const T currentRotation(acos(parameters(0,0)));
00207                 this->conditionVariables(0) = normalizeAngle(currentRotation - initialRotation2D);
00208         }
00209         else
00210                 assert(false);
00211         const unsigned int nbRows = parameters.rows()-1;
00212         const Vector currentTranslation = parameters.topRightCorner(nbRows,1);
00213         this->conditionVariables(1) = (currentTranslation - initialTranslation).norm();
00214         if (this->conditionVariables(0) > this->limits(0) || this->conditionVariables(1) > this->limits(1))
00215         {
00216                 ostringstream oss;
00217                 oss << "limit out of bounds: ";
00218                 oss << "rot: " << this->conditionVariables(0) << "/" << this->limits(0) << " ";
00219                 oss << "tr: " << this->conditionVariables(1) << "/" << this->limits(1);
00220                 throw ConvergenceError(oss.str());
00221         }
00222 }
00223 
00224 template struct TransformationCheckersImpl<float>::BoundTransformationChecker;
00225 template struct TransformationCheckersImpl<double>::BoundTransformationChecker;


libpointmatcher
Author(s):
autogenerated on Mon Sep 14 2015 02:59:06