Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
00069
00070
00071 if (this->conditionVariables(0) >= this->limits(0))
00072 {
00073 iterate = false;
00074 throw MaxNumIterationsReached();
00075 }
00076 }
00077
00078 template struct TransformationCheckersImpl<float>::CounterTransformationChecker;
00079 template struct TransformationCheckersImpl<double>::CounterTransformationChecker;
00080
00081
00082
00083
00084 template<typename T>
00085 TransformationCheckersImpl<T>::DifferentialTransformationChecker::DifferentialTransformationChecker(const Parameters& params):
00086 TransformationChecker("DifferentialTransformationChecker", DifferentialTransformationChecker::availableParameters(), params),
00087 minDiffRotErr(Parametrizable::get<T>("minDiffRotErr")),
00088 minDiffTransErr(Parametrizable::get<T>("minDiffTransErr")),
00089 smoothLength(Parametrizable::get<unsigned>("smoothLength"))
00090 {
00091 this->limits.setZero(2);
00092 this->limits(0) = minDiffRotErr;
00093 this->limits(1) = minDiffTransErr;
00094
00095 this->conditionVariableNames.push_back("Mean abs differential rot err");
00096 this->conditionVariableNames.push_back("Mean abs differential trans err");
00097 this->limitNames.push_back("Min differential rotation err");
00098 this->limitNames.push_back("Min differential translation err");
00099
00100 }
00101
00102 template<typename T>
00103 void TransformationCheckersImpl<T>::DifferentialTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
00104 {
00105 this->conditionVariables.setZero(2);
00106
00107 rotations.clear();
00108 translations.clear();
00109
00110 if (parameters.rows() == 4)
00111 {
00112 rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
00113 }
00114 else
00115 {
00116
00117 Eigen::Matrix<T,3,3> m(Matrix::Identity(3,3));
00118 m.topLeftCorner(2,2) = parameters.topLeftCorner(2,2);
00119 rotations.push_back(Quaternion(m));
00120 }
00121
00122 const unsigned int nbRows = parameters.rows()-1;
00123 translations.push_back(parameters.topRightCorner(nbRows,1));
00124 }
00125
00126 template<typename T>
00127 void TransformationCheckersImpl<T>::DifferentialTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00128 {
00129 typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00130
00131 rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
00132 const unsigned int nbRows = parameters.rows()-1;
00133 translations.push_back(parameters.topRightCorner(nbRows,1));
00134
00135 this->conditionVariables.setZero(2);
00136 if(rotations.size() > smoothLength)
00137 {
00138 for(size_t i = rotations.size()-1; i >= rotations.size()-smoothLength; i--)
00139 {
00140
00141 this->conditionVariables(0) += anyabs(rotations[i].angularDistance(rotations[i-1]));
00142 this->conditionVariables(1) += anyabs((translations[i] - translations[i-1]).norm());
00143 }
00144
00145 this->conditionVariables /= smoothLength;
00146
00147 if(this->conditionVariables(0) < this->limits(0) && this->conditionVariables(1) < this->limits(1))
00148 iterate = false;
00149 }
00150
00151
00152
00153
00154 if (boost::math::isnan(this->conditionVariables(0)))
00155 throw ConvergenceError("abs rotation norm not a number");
00156 if (boost::math::isnan(this->conditionVariables(1)))
00157 throw ConvergenceError("abs translation norm not a number");
00158 }
00159
00160 template struct TransformationCheckersImpl<float>::DifferentialTransformationChecker;
00161 template struct TransformationCheckersImpl<double>::DifferentialTransformationChecker;
00162
00163
00164
00165
00166 template<typename T>
00167 TransformationCheckersImpl<T>::BoundTransformationChecker::BoundTransformationChecker(const Parameters& params):
00168 TransformationChecker("BoundTransformationChecker", BoundTransformationChecker::availableParameters(), params),
00169 maxRotationNorm(Parametrizable::get<T>("maxRotationNorm")),
00170 maxTranslationNorm(Parametrizable::get<T>("maxTranslationNorm"))
00171 {
00172 this->limits.setZero(2);
00173 this->limits(0) = maxRotationNorm;
00174 this->limits(1) = maxTranslationNorm;
00175
00176 this->limitNames.push_back("Max rotation angle");
00177 this->limitNames.push_back("Max translation norm");
00178 this->conditionVariableNames.push_back("Rotation angle");
00179 this->conditionVariableNames.push_back("Translation norm");
00180 }
00181
00182 template<typename T>
00183 void TransformationCheckersImpl<T>::BoundTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
00184 {
00185 this->conditionVariables.setZero(2);
00186 if (parameters.rows() == 4)
00187 initialRotation3D = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
00188 else if (parameters.rows() == 3)
00189 initialRotation2D = acos(parameters(0,0));
00190 else
00191 throw runtime_error("BoundTransformationChecker only works in 2D or 3D");
00192
00193 const unsigned int nbRows = parameters.rows()-1;
00194 initialTranslation = parameters.topRightCorner(nbRows,1);
00195 }
00196
00197 template<typename T>
00198 void TransformationCheckersImpl<T>::BoundTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
00199 {
00200 typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
00201
00202 if (parameters.rows() == 4)
00203 {
00204 const Quaternion currentRotation = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
00205 this->conditionVariables(0) = currentRotation.angularDistance(initialRotation3D);
00206 }
00207 else if (parameters.rows() == 3)
00208 {
00209 const T currentRotation(acos(parameters(0,0)));
00210 this->conditionVariables(0) = normalizeAngle(currentRotation - initialRotation2D);
00211 }
00212 else
00213 assert(false);
00214 const unsigned int nbRows = parameters.rows()-1;
00215 const Vector currentTranslation = parameters.topRightCorner(nbRows,1);
00216 this->conditionVariables(1) = (currentTranslation - initialTranslation).norm();
00217 if (this->conditionVariables(0) > this->limits(0) || this->conditionVariables(1) > this->limits(1))
00218 {
00219 ostringstream oss;
00220 oss << "limit out of bounds: ";
00221 oss << "rot: " << this->conditionVariables(0) << "/" << this->limits(0) << " ";
00222 oss << "tr: " << this->conditionVariables(1) << "/" << this->limits(1);
00223 throw ConvergenceError(oss.str());
00224 }
00225 }
00226
00227 template struct TransformationCheckersImpl<float>::BoundTransformationChecker;
00228 template struct TransformationCheckersImpl<double>::BoundTransformationChecker;