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         translations.push_back(parameters.topRightCorner(parameters.rows()-1,1));
00120 }
00121 
00122 template<typename T>
00123 void TransformationCheckersImpl<T>::DifferentialTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00124 {
00125         typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00126         
00127         rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
00128         translations.push_back(parameters.topRightCorner(parameters.rows()-1,1));
00129         
00130         this->conditionVariables.setZero(2);
00131         if(rotations.size() > smoothLength)
00132         {
00133                 for(size_t i = rotations.size()-1; i >= rotations.size()-smoothLength; i--)
00134                 {
00135                         //Compute the mean derivative
00136                         this->conditionVariables(0) += anyabs(rotations[i].angularDistance(rotations[i-1]));
00137                         this->conditionVariables(1) += anyabs((translations[i] - translations[i-1]).norm());
00138                 }
00139 
00140                 this->conditionVariables /= smoothLength;
00141 
00142                 if(this->conditionVariables(0) < this->limits(0) && this->conditionVariables(1) < this->limits(1))
00143                         iterate = false;
00144         }
00145         
00146         //std::cout << "Abs Rotation: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
00147         //std::cout << "Abs Translation: " << this->conditionVariables(1) << " / " << this->limits(1) << std::endl;
00148         
00149         if (boost::math::isnan(this->conditionVariables(0)))
00150                 throw ConvergenceError("abs rotation norm not a number");
00151         if (boost::math::isnan(this->conditionVariables(1)))
00152                 throw ConvergenceError("abs translation norm not a number");
00153 }
00154 
00155 template struct TransformationCheckersImpl<float>::DifferentialTransformationChecker;
00156 template struct TransformationCheckersImpl<double>::DifferentialTransformationChecker;
00157 
00158 //--------------------------------------
00159 // bound
00160 
00161 template<typename T>
00162 TransformationCheckersImpl<T>::BoundTransformationChecker::BoundTransformationChecker(const Parameters& params):
00163         TransformationChecker("BoundTransformationChecker", BoundTransformationChecker::availableParameters(), params),
00164         maxRotationNorm(Parametrizable::get<T>("maxRotationNorm")),
00165         maxTranslationNorm(Parametrizable::get<T>("maxTranslationNorm"))
00166 {
00167         this->limits.setZero(2);
00168         this->limits(0) = maxRotationNorm;
00169         this->limits(1) = maxTranslationNorm;
00170 
00171         this->limitNames.push_back("Max rotation angle");
00172         this->limitNames.push_back("Max translation norm");
00173         this->conditionVariableNames.push_back("Rotation angle");
00174         this->conditionVariableNames.push_back("Translation norm");
00175 }
00176 
00177 template<typename T>
00178 void TransformationCheckersImpl<T>::BoundTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
00179 {
00180         this->conditionVariables.setZero(2);
00181         if (parameters.rows() == 4)
00182                 initialRotation3D = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
00183         else if (parameters.rows() == 3)
00184                 initialRotation2D = acos(parameters(0,0));
00185         else
00186                 throw runtime_error("BoundTransformationChecker only works in 2D or 3D");
00187                 
00188         initialTranslation = parameters.topRightCorner(parameters.rows()-1,1);
00189 }
00190 
00191 template<typename T>
00192 void TransformationCheckersImpl<T>::BoundTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00193 {
00194         typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00195         
00196         if (parameters.rows() == 4)
00197         {
00198                 const Quaternion currentRotation = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
00199                 this->conditionVariables(0) = currentRotation.angularDistance(initialRotation3D);
00200         }
00201         else if (parameters.rows() == 3)
00202         {
00203                 const T currentRotation(acos(parameters(0,0)));
00204                 this->conditionVariables(0) = normalizeAngle(currentRotation - initialRotation2D);
00205         }
00206         else
00207                 assert(false);
00208         const Vector currentTranslation = parameters.topRightCorner(parameters.rows()-1,1);
00209         this->conditionVariables(1) = (currentTranslation - initialTranslation).norm();
00210         if (this->conditionVariables(0) > this->limits(0) || this->conditionVariables(1) > this->limits(1))
00211         {
00212                 ostringstream oss;
00213                 oss << "limit out of bounds: ";
00214                 oss << "rot: " << this->conditionVariables(0) << "/" << this->limits(0) << " ";
00215                 oss << "tr: " << this->conditionVariables(1) << "/" << this->limits(1);
00216                 throw ConvergenceError(oss.str());
00217         }
00218 }
00219 
00220 template struct TransformationCheckersImpl<float>::BoundTransformationChecker;
00221 template struct TransformationCheckersImpl<double>::BoundTransformationChecker;


upstream_src
Author(s):
autogenerated on Mon Oct 6 2014 10:27:42