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 "TransformationsImpl.h"
00037 #include "Functions.h"
00038
00039 #include <iostream>
00040
00041 using namespace PointMatcherSupport;
00042
00044 TransformationError::TransformationError(const std::string& reason):
00045 runtime_error(reason)
00046 {}
00047
00049 template<typename T>
00050 typename PointMatcher<T>::DataPoints TransformationsImpl<T>::RigidTransformation::compute(
00051 const DataPoints& input,
00052 const TransformationParameters& parameters) const
00053 {
00054 assert(input.features.rows() == parameters.rows());
00055 assert(parameters.rows() == parameters.cols());
00056
00057 const unsigned int nbRows = parameters.rows()-1;
00058 const unsigned int nbCols = parameters.cols()-1;
00059
00060 const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
00061
00062 if(this->checkParameters(parameters) == false)
00063 throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal.");
00064
00065
00066 DataPoints transformedCloud = input;
00067
00068
00069 transformedCloud.features = parameters * input.features;
00070
00071
00072 int row(0);
00073 const int descCols(input.descriptors.cols());
00074 for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
00075 {
00076 const int span(input.descriptorLabels[i].span);
00077 const std::string& name(input.descriptorLabels[i].text);
00078 const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
00079 BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
00080 if (name == "normals" || name == "observationDirections")
00081 outputDesc = R * inputDesc;
00082
00083 row += span;
00084 }
00085
00086 return transformedCloud;
00087 }
00088
00090 template<typename T>
00091 bool TransformationsImpl<T>::RigidTransformation::checkParameters(const TransformationParameters& parameters) const
00092 {
00093
00094 const T epsilon = 0.001;
00095 const unsigned int nbRows = parameters.rows()-1;
00096 const unsigned int nbCols = parameters.cols()-1;
00097
00098 const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
00099
00100 if(anyabs(1 - R.determinant()) > epsilon)
00101 return false;
00102 else
00103 return true;
00104
00105 }
00106
00108 template<typename T>
00109 typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const
00110 {
00111 TransformationParameters ortho = parameters;
00112 if(ortho.cols() == 4)
00113 {
00114
00115 const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
00116 const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();
00117
00118 const Eigen::Matrix<T, 3, 1> newCol0 = col1.cross(col2);
00119 const Eigen::Matrix<T, 3, 1> newCol1 = col2.cross(newCol0);
00120 const Eigen::Matrix<T, 3, 1> newCol2 = col2;
00121
00122 ortho.block(0, 0, 3, 1) = newCol0;
00123 ortho.block(0, 1, 3, 1) = newCol1;
00124 ortho.block(0, 2, 3, 1) = newCol2;
00125 }
00126 else if(ortho.cols() == 3)
00127 {
00128 const T epsilon = 0.001;
00129
00130
00131
00132 if(anyabs(parameters(0,0) - parameters(1,1)) > epsilon || anyabs(parameters(1,0) + parameters(0,1)) > epsilon)
00133 {
00134 throw TransformationError("RigidTransformation: Error, only proper rigid transformations are supported.");
00135 }
00136
00137
00138 T a = (parameters(0,0) + parameters(1,1))/2;
00139 T b = (-parameters(1,0) + parameters(0,1))/2;
00140 T sum = sqrt(pow(a,2) + pow(b,2));
00141
00142 a = a/sum;
00143 b = b/sum;
00144
00145 ortho(0,0) = a; ortho(0,1) = b;
00146 ortho(1,0) = -b; ortho(1,1) = a;
00147 }
00148
00149
00150 return ortho;
00151 }
00152
00153 template struct TransformationsImpl<float>::RigidTransformation;
00154 template struct TransformationsImpl<double>::RigidTransformation;
00155
00157 template<typename T>
00158 typename PointMatcher<T>::DataPoints TransformationsImpl<T>::SimilarityTransformation::compute(
00159 const DataPoints& input,
00160 const TransformationParameters& parameters) const
00161 {
00162 assert(input.features.rows() == parameters.rows());
00163 assert(parameters.rows() == parameters.cols());
00164
00165 const unsigned int nbRows = parameters.rows()-1;
00166 const unsigned int nbCols = parameters.cols()-1;
00167
00168 const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));
00169
00170 if(this->checkParameters(parameters) == false)
00171 throw TransformationError("SimilarityTransformation: Error, invalid similarity transform.");
00172
00173
00174 DataPoints transformedCloud = input;
00175
00176
00177 transformedCloud.features = parameters * input.features;
00178
00179
00180 int row(0);
00181 const int descCols(input.descriptors.cols());
00182 for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
00183 {
00184 const int span(input.descriptorLabels[i].span);
00185 const std::string& name(input.descriptorLabels[i].text);
00186 const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
00187 BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
00188 if (name == "normals" || name == "observationDirections")
00189 outputDesc = R * inputDesc;
00190
00191 row += span;
00192 }
00193
00194 return transformedCloud;
00195 }
00196
00198 template<typename T>
00199 bool TransformationsImpl<T>::SimilarityTransformation::checkParameters(const TransformationParameters& parameters) const
00200 {
00201
00202 return true;
00203 }
00204
00206 template<typename T>
00207 typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::SimilarityTransformation::correctParameters(const TransformationParameters& parameters) const
00208 {
00209 return parameters;
00210 }
00211
00212 template struct TransformationsImpl<float>::SimilarityTransformation;
00213 template struct TransformationsImpl<double>::SimilarityTransformation;
00214
00215 template<typename T>
00216 typename PointMatcher<T>::DataPoints TransformationsImpl<T>::PureTranslation::compute(const DataPoints& input,
00217 const TransformationParameters& parameters) const {
00218 assert(input.features.rows() == parameters.rows());
00219 assert(parameters.rows() == parameters.cols());
00220
00221 if(this->checkParameters(parameters) == false)
00222 throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity.");
00223
00224
00225 DataPoints transformedCloud = input;
00226
00227
00228 transformedCloud.features = parameters * input.features;
00229
00230 return transformedCloud;
00231 }
00232
00233 template<typename T>
00234 typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::PureTranslation::correctParameters(
00235 const TransformationParameters& parameters) const {
00236 const int rows = parameters.rows();
00237 const int cols = parameters.cols();
00238
00239
00240 TransformationParameters correctedParameters(parameters);
00241
00242
00243 correctedParameters.block(0,0,rows-1,cols-1).setIdentity();
00244
00245
00246 correctedParameters.block(rows-1,0,1,cols-1).setZero();
00247 correctedParameters(rows-1,cols-1) = 1;
00248
00249 return correctedParameters;
00250 }
00251
00252 template<typename T>
00253 bool TransformationsImpl<T>::PureTranslation::checkParameters(
00254 const TransformationParameters& parameters) const {
00255 const int rows = parameters.rows();
00256 const int cols = parameters.cols();
00257
00258
00259 TransformationParameters parameters_(parameters);
00260
00261
00262 parameters_.block(0,cols-1,rows-1,1).setZero();
00263
00264
00265 if (parameters_.isApprox(TransformationParameters::Identity(rows,cols)))
00266 return true;
00267 else
00268 return false;
00269 }
00270
00271 template struct TransformationsImpl<float>::PureTranslation;
00272 template struct TransformationsImpl<double>::PureTranslation;