ErrorMinimizersImpl.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 "ErrorMinimizersImpl.h"
00037 #include "PointMatcherPrivate.h"
00038 #include "Functions.h"
00039 
00040 #include "Eigen/SVD"
00041 #include <iostream>
00042 
00043 using namespace Eigen;
00044 using namespace std;
00045 using namespace PointMatcherSupport;
00046 
00047 // Identity Error Minimizer
00048 template<typename T>
00049 typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::IdentityErrorMinimizer::compute(
00050         const DataPoints& filteredReading,
00051         const DataPoints& filteredReference,
00052         const OutlierWeights& outlierWeights,
00053         const Matches& matches)
00054 {
00055         return TransformationParameters::Identity(filteredReading.features.rows(), filteredReading.features.rows());
00056 }
00057 
00058 template struct ErrorMinimizersImpl<float>::IdentityErrorMinimizer;
00059 template struct ErrorMinimizersImpl<double>::IdentityErrorMinimizer;
00060 
00061 
00062 // Point To POINT ErrorMinimizer
00063 template<typename T>
00064 typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPointErrorMinimizer::compute(
00065         const DataPoints& filteredReading,
00066         const DataPoints& filteredReference,
00067         const OutlierWeights& outlierWeights,
00068         const Matches& matches)
00069 {       
00070         assert(matches.ids.rows() > 0);
00071 
00072         typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
00073         
00074         // now minimize on kept points
00075         const int dimCount(mPts.reading.features.rows());
00076         const int ptsCount(mPts.reading.features.cols()); //But point cloud have now the same number of (matched) point
00077 
00078         // Compute the mean of each point cloud
00079         const Vector meanReading = mPts.reading.features.rowwise().sum() / ptsCount;
00080         const Vector meanReference = mPts.reference.features.rowwise().sum() / ptsCount;
00081         
00082         // Remove the mean from the point clouds
00083         mPts.reading.features.colwise() -= meanReading;
00084         mPts.reference.features.colwise() -= meanReference;
00085 
00086         // Singular Value Decomposition
00087         const Matrix m(mPts.reference.features.topRows(dimCount-1) * mPts.reading.features.topRows(dimCount-1).transpose());
00088         const JacobiSVD<Matrix> svd(m, ComputeThinU | ComputeThinV);
00089         const Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
00090         const Vector trVector(meanReference.head(dimCount-1)- rotMatrix * meanReading.head(dimCount-1));
00091         
00092         Matrix result(Matrix::Identity(dimCount, dimCount));
00093         result.corner(TopLeft, dimCount-1, dimCount-1) = rotMatrix;
00094         result.corner(TopRight, dimCount-1, 1) = trVector;
00095         
00096         return result;
00097 }
00098 
00099 template<typename T>
00100 T ErrorMinimizersImpl<T>::PointToPointErrorMinimizer::getOverlap() const
00101 {
00102         //NOTE: computing overlap of 2 point clouds can be complicated due to
00103         // the sparse nature of the representation. Here is only an estimate 
00104         // of the true overlap.
00105         const int nbPoints = this->lastErrorElements.reading.features.cols();
00106         const int dim = this->lastErrorElements.reading.features.rows();
00107         if(nbPoints == 0)
00108         {
00109                 throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
00110         }
00111 
00112         if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise"))
00113         {
00114                 LOG_INFO_STREAM("PointToPointErrorMinimizer - warning, no sensor noise found. Using best estimate given outlier rejection instead.");
00115                 return this->weightedPointUsedRatio;
00116         }
00117 
00118         const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
00119 
00120         const Vector dists = (this->lastErrorElements.reading.features.topRows(dim-1) - this->lastErrorElements.reference.features.topRows(dim-1)).colwise().norm();
00121         const T mean = dists.sum()/nbPoints;
00122 
00123         int count = 0;
00124         for(int i=0; i < nbPoints; i++)
00125         {
00126                 if(dists(i) < (mean + noises(0,i)))
00127                 {
00128                         count++;
00129                 }
00130         }
00131 
00132         return (T)count/(T)nbPoints;
00133 }
00134 
00135 template struct ErrorMinimizersImpl<float>::PointToPointErrorMinimizer;
00136 template struct ErrorMinimizersImpl<double>::PointToPointErrorMinimizer;
00137 
00138 
00139 // Point To PLANE ErrorMinimizer
00140 template<typename T>
00141 ErrorMinimizersImpl<T>::PointToPlaneErrorMinimizer::PointToPlaneErrorMinimizer(const Parameters& params):
00142         ErrorMinimizer("PointToPlaneErrorMinimizer", PointToPlaneErrorMinimizer::availableParameters(), params),
00143         force2D(Parametrizable::get<T>("force2D"))
00144 {
00145 }
00146 
00147 
00148 template<typename T>
00149 typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPlaneErrorMinimizer::compute(
00150         const DataPoints& filteredReading,
00151         const DataPoints& filteredReference,
00152         const OutlierWeights& outlierWeights,
00153         const Matches& matches)
00154 {
00155         assert(matches.ids.rows() > 0);
00156 
00157         // Fetch paired points
00158         typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
00159 
00160         const int dim = mPts.reading.features.rows();
00161         const int nbPts = mPts.reading.features.cols();
00162 
00163         // Adjust if the user forces 2D minimization on XY-plane
00164         int forcedDim = dim - 1;
00165         if(force2D && dim == 4)
00166         {
00167                 mPts.reading.features.conservativeResize(3, Eigen::NoChange);
00168                 mPts.reading.features.row(2) = Matrix::Ones(1, nbPts);
00169                 mPts.reference.features.conservativeResize(3, Eigen::NoChange);
00170                 mPts.reference.features.row(2) = Matrix::Ones(1, nbPts);
00171                 forcedDim = dim - 2;
00172         }
00173 
00174         // Fetch normal vectors of the reference point cloud (with adjustment if needed)
00175         const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim));
00176 
00177         // Note: Normal vector must be precalculated to use this error. Use appropriate input filter.
00178         assert(normalRef.rows() > 0);
00179 
00180         // Compute cross product of cross = cross(reading X normalRef)
00181         const Matrix cross = this->crossProduct(mPts.reading.features, normalRef);
00182 
00183         // wF = [weights*cross, weights*normals]
00184         // F  = [cross, normals]
00185         Matrix wF(normalRef.rows()+ cross.rows(), normalRef.cols());
00186         Matrix F(normalRef.rows()+ cross.rows(), normalRef.cols());
00187         
00188         for(int i=0; i < cross.rows(); i++)
00189         {
00190                 wF.row(i) = mPts.weights.cwise() * cross.row(i);
00191                 F.row(i) = cross.row(i);
00192         }
00193         for(int i=0; i < normalRef.rows(); i++)
00194         {
00195                 wF.row(i + cross.rows()) = mPts.weights.cwise() * normalRef.row(i);
00196                 F.row(i + cross.rows()) = normalRef.row(i);
00197         }
00198 
00199         // Unadjust covariance A = wF * F'
00200         const Matrix A = wF * F.transpose();
00201         if (A.fullPivHouseholderQr().rank() != A.rows())
00202         {
00203                 // TODO: handle that properly
00204                 //throw ConvergenceError("encountered singular while minimizing point to plane distance");
00205         }
00206 
00207         const Matrix deltas = mPts.reading.features - mPts.reference.features;
00208 
00209         // dot product of dot = dot(deltas, normals)
00210         Matrix dotProd = Matrix::Zero(1, normalRef.cols());
00211         
00212         for(int i=0; i<normalRef.rows(); i++)
00213         {
00214                 dotProd += (deltas.row(i).cwise() * normalRef.row(i));
00215         }
00216 
00217         // b = -(wF' * dot)
00218         const Vector b = -(wF * dotProd.transpose());
00219 
00220         // Cholesky decomposition
00221         Vector x(A.rows());
00222         A.llt().solve(b, &x);
00223         
00224         // Transform parameters to matrix
00225         Matrix mOut;
00226         if(dim == 4 && !force2D)
00227         {
00228                 Eigen::Transform<T, 3, Eigen::Affine> transform;
00229                 // PLEASE DONT USE EULAR ANGLES!!!!
00230                 // Rotation in Eular angles follow roll-pitch-yaw (1-2-3) rule
00231                 /*transform = Eigen::AngleAxis<T>(x(0), Eigen::Matrix<T,1,3>::UnitX())
00232                                 * Eigen::AngleAxis<T>(x(1), Eigen::Matrix<T,1,3>::UnitY())
00233                                 * Eigen::AngleAxis<T>(x(2), Eigen::Matrix<T,1,3>::UnitZ());*/
00234 
00235                 transform = Eigen::AngleAxis<T>(x.head(3).norm(),x.head(3).normalized());
00236 
00237                 // Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time!
00238                 /*const T pitch = -asin(transform(2,0));
00239                 const T roll = atan2(transform(2,1), transform(2,2));
00240                 const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch));
00241                 std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/
00242                 transform.translation() = x.segment(3, 3);
00243                 mOut = transform.matrix();
00244         }
00245         else
00246         {
00247                 Eigen::Transform<T, 2, Eigen::Affine> transform;
00248                 transform = Eigen::Rotation2D<T> (x(0));
00249                 transform.translation() = x.segment(1, 2);
00250 
00251                 if(force2D)
00252                 {
00253                         mOut = Matrix::Identity(dim, dim);
00254                         mOut.corner(TopLeft, 2, 2) = transform.matrix().corner(TopLeft, 2, 2);
00255                         mOut.corner(TopRight, 2, 1) = transform.matrix().corner(TopRight, 2, 1);
00256                 }
00257                 else
00258                 {
00259                         mOut = transform.matrix();
00260                 }
00261         }
00262         return mOut; 
00263 }
00264 
00265 template<typename T>
00266 T ErrorMinimizersImpl<T>::PointToPlaneErrorMinimizer::getOverlap() const
00267 {
00268         const int nbPoints = this->lastErrorElements.reading.features.cols();
00269         const int dim = this->lastErrorElements.reading.features.rows();
00270         if(nbPoints == 0)
00271         {
00272                 throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
00273         }
00274         
00275         if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise") ||
00276                 !this->lastErrorElements.reading.descriptorExists("normals"))
00277         {
00278                 LOG_INFO_STREAM("PointToPlaneErrorMinimizer - warning, no sensor noise or normals found. Using best estimate given outlier rejection instead.");
00279                 return this->weightedPointUsedRatio;
00280         }
00281 
00282         const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
00283         const BOOST_AUTO(normals, this->lastErrorElements.reading.getDescriptorViewByName("normals"));
00284         
00285 
00286         const Matrix delta = (this->lastErrorElements.reading.features.topRows(dim-1) - this->lastErrorElements.reference.features.topRows(dim-1));
00287         const T mean = delta.colwise().norm().sum()/nbPoints;
00288         cerr << "mean:" << mean << endl;
00289 
00290         int count = 0;
00291         for(int i=0; i < nbPoints; i++)
00292         {
00293                 const Vector n = normals.col(i);
00294                 const T projectionDist = delta.col(i).dot(n.normalized());
00295                 if(anyabs(projectionDist) < (mean + noises(0,i)))
00296                 {
00297                         count++;
00298                 }
00299         }
00300 
00301         return (T)count/(T)nbPoints;
00302 }
00303 
00304 template struct ErrorMinimizersImpl<float>::PointToPlaneErrorMinimizer;
00305 template struct ErrorMinimizersImpl<double>::PointToPlaneErrorMinimizer;
00306 
00307 
00308 
00309 // Point To POINT WITH COV ErrorMinimizer
00310 template<typename T>
00311 ErrorMinimizersImpl<T>::PointToPointWithCovErrorMinimizer::PointToPointWithCovErrorMinimizer(const Parameters& params):
00312         ErrorMinimizer("PointToPointWithCovErrorMinimizer", PointToPointWithCovErrorMinimizer::availableParameters(), params),
00313         sensorStdDev(Parametrizable::get<T>("sensorStdDev"))
00314 {
00315 }
00316 
00317 template<typename T>
00318 typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPointWithCovErrorMinimizer::compute(
00319         const DataPoints& filteredReading,
00320         const DataPoints& filteredReference,
00321         const OutlierWeights& outlierWeights,
00322         const Matches& matches)
00323 {       
00324         assert(matches.ids.rows() > 0);
00325 
00326         typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
00327         
00328         // now minimize on kept points
00329         const int dimCount(mPts.reading.features.rows());
00330         const int ptsCount(mPts.reading.features.cols()); //But point cloud have now the same number of (matched) point
00331 
00332         // Compute the mean of each point cloud
00333         const Vector meanReading = mPts.reading.features.rowwise().sum() / ptsCount;
00334         const Vector meanReference = mPts.reference.features.rowwise().sum() / ptsCount;
00335         
00336         // Remove the mean from the point clouds
00337         mPts.reading.features.colwise() -= meanReading;
00338         mPts.reference.features.colwise() -= meanReference;
00339 
00340         // Singular Value Decomposition
00341         const Matrix m(mPts.reference.features.topRows(dimCount-1) * mPts.reading.features.topRows(dimCount-1).transpose());
00342         const JacobiSVD<Matrix> svd(m, ComputeThinU | ComputeThinV);
00343         const Matrix rotMatrix(svd.matrixU() * svd.matrixV().transpose());
00344         const Vector trVector(meanReference.head(dimCount-1)- rotMatrix * meanReading.head(dimCount-1));
00345         
00346         Matrix result(Matrix::Identity(dimCount, dimCount));
00347         result.corner(TopLeft, dimCount-1, dimCount-1) = rotMatrix;
00348         result.corner(TopRight, dimCount-1, 1) = trVector;
00349 
00350         this->covMatrix = this->estimateCovariance(filteredReading, filteredReference, matches, outlierWeights, result);
00351 
00352         return result;
00353 }
00354 
00355 template<typename T>
00356 typename ErrorMinimizersImpl<T>::Matrix
00357 ErrorMinimizersImpl<T>::PointToPointWithCovErrorMinimizer::estimateCovariance(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation)
00358 {
00359         int max_nbr_point = outlierWeights.cols();
00360 
00361         Matrix covariance(Matrix::Zero(6,6));
00362         Matrix J_hessian(Matrix::Zero(6,6));
00363         Matrix d2J_dReadingdX(Matrix::Zero(6, max_nbr_point));
00364         Matrix d2J_dReferencedX(Matrix::Zero(6, max_nbr_point));
00365 
00366         Vector reading_point(Vector::Zero(3));
00367         Vector reference_point(Vector::Zero(3));
00368         Vector normal(3);
00369         Vector reading_direction(Vector::Zero(3));
00370         Vector reference_direction(Vector::Zero(3));
00371 
00372         normal << 1.0, 1.0, 1.0;    // Used for point-to-point computation
00373 
00374         T beta = -asin(transformation(2,0));
00375         T alpha = atan2(transformation(2,1), transformation(2,2));
00376         T gamma = atan2(transformation(1,0)/cos(beta), transformation(0,0)/cos(beta));
00377         T t_x = transformation(0,3);
00378         T t_y = transformation(1,3);
00379         T t_z = transformation(2,3);
00380 
00381         Vector tmp_vector_6(Vector::Zero(6));
00382 
00383         int valid_points_count = 0;
00384 
00385         for(int i = 0; i < max_nbr_point; ++i)
00386         {
00387                 if (outlierWeights(0,i) > 0.0)
00388                 {
00389                         reading_point = reading.features.block(0,i,3,1);
00390                         int reference_idx = matches.ids(0,i);
00391                         reference_point = reference.features.block(0,reference_idx,3,1);
00392 
00393                         T reading_range = reading_point.norm();
00394                         reading_direction = reading_point / reading_range;
00395                         T reference_range = reference_point.norm();
00396                         reference_direction = reference_point / reference_range;
00397 
00398                         T n_alpha = normal(2)*reading_direction(1) - normal(1)*reading_direction(2);
00399                         T n_beta = normal(0)*reading_direction(2) - normal(2)*reading_direction(0);
00400                         T n_gamma = normal(1)*reading_direction(0) - normal(0)*reading_direction(1);
00401 
00402                         T E = normal(0)*(reading_point(0) - gamma*reading_point(1) + beta*reading_point(2) + t_x - reference_point(0));
00403                         E +=  normal(1)*(gamma*reading_point(0) + reading_point(1) - alpha*reading_point(2) + t_y - reference_point(1));
00404                         E +=  normal(2)*(-beta*reading_point(0) + alpha*reading_point(1) + reading_point(2) + t_z - reference_point(2));
00405 
00406                         T N_reading = normal(0)*(reading_direction(0) - gamma*reading_direction(1) + beta*reading_direction(2));
00407                         N_reading +=  normal(1)*(gamma*reading_direction(0) + reading_direction(1) - alpha*reading_direction(2));
00408                         N_reading +=  normal(2)*(-beta*reading_direction(0) + alpha*reading_direction(1) + reading_direction(2));
00409 
00410                         T N_reference = -(normal(0)*reference_direction(0) + normal(1)*reference_direction(1) + normal(2)*reference_direction(2));
00411 
00412                         // update the hessian and d2J/dzdx
00413                         tmp_vector_6 << normal(0), normal(1), normal(2), reading_range * n_alpha, reading_range * n_beta, reading_range * n_gamma;
00414 
00415                         J_hessian += tmp_vector_6 * tmp_vector_6.transpose();
00416 
00417                         tmp_vector_6 << normal(0) * N_reading, normal(1) * N_reading, normal(2) * N_reading, n_alpha * (E + reading_range * N_reading), n_beta * (E + reading_range * N_reading), n_gamma * (E + reading_range * N_reading);
00418 
00419                         d2J_dReadingdX.block(0,valid_points_count,6,1) = tmp_vector_6;
00420 
00421                         tmp_vector_6 << normal(0) * N_reference, normal(1) * N_reference, normal(2) * N_reference, reference_range * n_alpha * N_reference, reference_range * n_beta * N_reference, reference_range * n_gamma * N_reference;
00422 
00423                         d2J_dReferencedX.block(0,valid_points_count,6,1) = tmp_vector_6;
00424 
00425                         valid_points_count++;
00426                 } // if (outlierWeights(0,i) > 0.0)
00427         }
00428 
00429         Matrix d2J_dZdX(Matrix::Zero(6, 2 * valid_points_count));
00430         d2J_dZdX.block(0,0,6,valid_points_count) = d2J_dReadingdX.block(0,0,6,valid_points_count);
00431         d2J_dZdX.block(0,valid_points_count,6,valid_points_count) = d2J_dReferencedX.block(0,0,6,valid_points_count);
00432 
00433         Matrix inv_J_hessian = J_hessian.inverse();
00434 
00435         covariance = d2J_dZdX * d2J_dZdX.transpose();
00436         covariance = inv_J_hessian * covariance * inv_J_hessian;
00437 
00438         return (sensorStdDev * sensorStdDev) * covariance;
00439 }
00440 
00441 template<typename T>
00442 T ErrorMinimizersImpl<T>::PointToPointWithCovErrorMinimizer::getOverlap() const
00443 {
00444         const int nbPoints = this->lastErrorElements.reading.features.cols();
00445         if(nbPoints == 0)
00446         {
00447                 throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
00448         }
00449 
00450         if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise"))
00451         {
00452                 LOG_INFO_STREAM("PointToPointErrorMinimizer - warning, no sensor noise found. Using best estimate given outlier rejection instead.");
00453                 return this->weightedPointUsedRatio;
00454         }
00455 
00456         const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
00457         int count = 0;
00458         for(int i=0; i < nbPoints; i++)
00459         {
00460                 const T dist = (this->lastErrorElements.reading.features.col(i) - this->lastErrorElements.reference.features.col(i)).norm();
00461                 if(dist < noises(0,i))
00462                         count++;
00463 
00464         }
00465 
00466         return (T)count/(T)nbPoints;
00467 }
00468 
00469 template<typename T>
00470 typename ErrorMinimizersImpl<T>::Matrix ErrorMinimizersImpl<T>::PointToPointWithCovErrorMinimizer::getCovariance() const
00471 {
00472   return covMatrix;
00473 }
00474 
00475 template struct ErrorMinimizersImpl<float>::PointToPointWithCovErrorMinimizer;
00476 template struct ErrorMinimizersImpl<double>::PointToPointWithCovErrorMinimizer;
00477 
00478 
00479 // Point To PLANE WITH COV ErrorMinimizer
00480 template<typename T>
00481 ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::PointToPlaneWithCovErrorMinimizer(const Parameters& params):
00482         ErrorMinimizer("PointToPlaneWithCovErrorMinimizer", PointToPlaneWithCovErrorMinimizer::availableParameters(), params),
00483         force2D(Parametrizable::get<T>("force2D")),
00484         sensorStdDev(Parametrizable::get<T>("sensorStdDev"))
00485 {
00486 }
00487 
00488 
00489 template<typename T>
00490 typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::compute(
00491         const DataPoints& filteredReading,
00492         const DataPoints& filteredReference,
00493         const OutlierWeights& outlierWeights,
00494         const Matches& matches)
00495 {
00496         assert(matches.ids.rows() > 0);
00497 
00498         // Fetch paired points
00499         typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
00500 
00501         const int dim = mPts.reading.features.rows();
00502         const int nbPts = mPts.reading.features.cols();
00503 
00504         // Adjust if the user forces 2D minimization on XY-plane
00505         int forcedDim = dim - 1;
00506         if(force2D && dim == 4)
00507         {
00508                 mPts.reading.features.conservativeResize(3, Eigen::NoChange);
00509                 mPts.reading.features.row(2) = Matrix::Ones(1, nbPts);
00510                 mPts.reference.features.conservativeResize(3, Eigen::NoChange);
00511                 mPts.reference.features.row(2) = Matrix::Ones(1, nbPts);
00512                 forcedDim = dim - 2;
00513         }
00514 
00515         // Fetch normal vectors of the reference point cloud (with adjustment if needed)
00516         const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim));
00517 
00518         // Note: Normal vector must be precalculated to use this error. Use appropriate input filter.
00519         assert(normalRef.rows() > 0);
00520 
00521         // Compute cross product of cross = cross(reading X normalRef)
00522         const Matrix cross = this->crossProduct(mPts.reading.features, normalRef);
00523 
00524         // wF = [weights*cross, weight*normals]
00525         // F  = [cross, normals]
00526         Matrix wF(normalRef.rows()+ cross.rows(), normalRef.cols());
00527         Matrix F(normalRef.rows()+ cross.rows(), normalRef.cols());
00528         
00529         for(int i=0; i < cross.rows(); i++)
00530         {
00531                 wF.row(i) = mPts.weights.cwise() * cross.row(i);
00532                 F.row(i) = cross.row(i);
00533         }
00534         for(int i=0; i < normalRef.rows(); i++)
00535         {
00536                 wF.row(i + cross.rows()) = mPts.weights.cwise() * normalRef.row(i);
00537                 F.row(i + cross.rows()) = normalRef.row(i);
00538         }
00539 
00540         // Unadjust covariance A = wF * F'
00541         const Matrix A = wF * F.transpose();
00542         if (A.fullPivHouseholderQr().rank() != A.rows())
00543         {
00544                 // TODO: handle that properly
00545                 //throw ConvergenceError("encountered singular while minimizing point to plane distance");
00546         }
00547 
00548         const Matrix deltas = mPts.reading.features - mPts.reference.features;
00549 
00550         // dot product of dot = dot(deltas, normals)
00551         Matrix dotProd = Matrix::Zero(1, normalRef.cols());
00552         
00553         for(int i=0; i<normalRef.rows(); i++)
00554         {
00555                 dotProd += (deltas.row(i).cwise() * normalRef.row(i));
00556         }
00557 
00558         // b = -(wF' * dot)
00559         const Vector b = -(wF * dotProd.transpose());
00560 
00561         // Cholesky decomposition
00562         Vector x(A.rows());
00563         A.llt().solve(b, &x);
00564         
00565         // Transform parameters to matrix
00566         Matrix mOut;
00567         if(dim == 4 && !force2D)
00568         {
00569                 Eigen::Transform<T, 3, Eigen::Affine> transform;
00570                 // IT IS NOT CORRECT TO USE EULER ANGLES!
00571                 // Rotation in Eular angles follow roll-pitch-yaw (1-2-3) rule
00572                 /*transform = Eigen::AngleAxis<T>(x(0), Eigen::Matrix<T,1,3>::UnitX())
00573                                 * Eigen::AngleAxis<T>(x(1), Eigen::Matrix<T,1,3>::UnitY())
00574                                 * Eigen::AngleAxis<T>(x(2), Eigen::Matrix<T,1,3>::UnitZ()); */
00575                 // Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time!
00576                 /*const T pitch = -asin(transform(2,0));
00577                 const T roll = atan2(transform(2,1), transform(2,2));
00578                 const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch));
00579                 std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/
00580 
00581                 transform = Eigen::AngleAxis<T>(x.head(3).norm(),x.head(3).normalized());
00582                 transform.translation() = x.segment(3, 3);
00583                 mOut = transform.matrix();
00584         }
00585         else
00586         {
00587                 Eigen::Transform<T, 2, Eigen::Affine> transform;
00588                 transform = Eigen::Rotation2D<T> (x(0));
00589                 transform.translation() = x.segment(1, 2);
00590 
00591                 if(force2D)
00592                 {
00593                         mOut = Matrix::Identity(dim, dim);
00594                         mOut.corner(TopLeft, 2, 2) = transform.matrix().corner(TopLeft, 2, 2);
00595                         mOut.corner(TopRight, 2, 1) = transform.matrix().corner(TopRight, 2, 1);
00596                 }
00597                 else
00598                 {
00599                         mOut = transform.matrix();
00600                 }
00601         }
00602 
00603         this->covMatrix = this->estimateCovariance(filteredReading, filteredReference, matches, outlierWeights, mOut);
00604 
00605         return mOut; 
00606 }
00607 
00608 template<typename T>
00609 typename ErrorMinimizersImpl<T>::Matrix
00610 ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::estimateCovariance(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation)
00611 {
00612         int max_nbr_point = outlierWeights.cols();
00613 
00614         Matrix covariance(Matrix::Zero(6,6));
00615         Matrix J_hessian(Matrix::Zero(6,6));
00616         Matrix d2J_dReadingdX(Matrix::Zero(6, max_nbr_point));
00617         Matrix d2J_dReferencedX(Matrix::Zero(6, max_nbr_point));
00618 
00619         Vector reading_point(Vector::Zero(3));
00620         Vector reference_point(Vector::Zero(3));
00621         Vector normal(3);
00622         Vector reading_direction(Vector::Zero(3));
00623         Vector reference_direction(Vector::Zero(3));
00624 
00625         Matrix normals = reference.getDescriptorViewByName("normals");
00626 
00627         if (normals.rows() < 3)    // Make sure there are normals in DataPoints
00628                 return Matrix::Zero(6,6);
00629 
00630         T beta = -asin(transformation(2,0));
00631         T alpha = atan2(transformation(2,1), transformation(2,2));
00632         T gamma = atan2(transformation(1,0)/cos(beta), transformation(0,0)/cos(beta));
00633         T t_x = transformation(0,3);
00634         T t_y = transformation(1,3);
00635         T t_z = transformation(2,3);
00636 
00637         Vector tmp_vector_6(Vector::Zero(6));
00638 
00639         int valid_points_count = 0;
00640 
00641         for(int i = 0; i < max_nbr_point; ++i)
00642         {
00643                 if (outlierWeights(0,i) > 0.0)
00644                 {
00645                         reading_point = reading.features.block(0,i,3,1);
00646                         int reference_idx = matches.ids(0,i);
00647                         reference_point = reference.features.block(0,reference_idx,3,1);
00648 
00649                         normal = normals.block(0,reference_idx,3,1);
00650 
00651                         T reading_range = reading_point.norm();
00652                         reading_direction = reading_point / reading_range;
00653                         T reference_range = reference_point.norm();
00654                         reference_direction = reference_point / reference_range;
00655 
00656                         T n_alpha = normal(2)*reading_direction(1) - normal(1)*reading_direction(2);
00657                         T n_beta = normal(0)*reading_direction(2) - normal(2)*reading_direction(0);
00658                         T n_gamma = normal(1)*reading_direction(0) - normal(0)*reading_direction(1);
00659 
00660                         T E = normal(0)*(reading_point(0) - gamma*reading_point(1) + beta*reading_point(2) + t_x - reference_point(0));
00661                         E +=  normal(1)*(gamma*reading_point(0) + reading_point(1) - alpha*reading_point(2) + t_y - reference_point(1));
00662                         E +=  normal(2)*(-beta*reading_point(0) + alpha*reading_point(1) + reading_point(2) + t_z - reference_point(2));
00663 
00664                         T N_reading = normal(0)*(reading_direction(0) - gamma*reading_direction(1) + beta*reading_direction(2));
00665                         N_reading +=  normal(1)*(gamma*reading_direction(0) + reading_direction(1) - alpha*reading_direction(2));
00666                         N_reading +=  normal(2)*(-beta*reading_direction(0) + alpha*reading_direction(1) + reading_direction(2));
00667 
00668                         T N_reference = -(normal(0)*reference_direction(0) + normal(1)*reference_direction(1) + normal(2)*reference_direction(2));
00669 
00670                         // update the hessian and d2J/dzdx
00671                         tmp_vector_6 << normal(0), normal(1), normal(2), reading_range * n_alpha, reading_range * n_beta, reading_range * n_gamma;
00672 
00673                         J_hessian += tmp_vector_6 * tmp_vector_6.transpose();
00674 
00675                         tmp_vector_6 << normal(0) * N_reading, normal(1) * N_reading, normal(2) * N_reading, n_alpha * (E + reading_range * N_reading), n_beta * (E + reading_range * N_reading), n_gamma * (E + reading_range * N_reading);
00676 
00677                         d2J_dReadingdX.block(0,valid_points_count,6,1) = tmp_vector_6;
00678 
00679                         tmp_vector_6 << normal(0) * N_reference, normal(1) * N_reference, normal(2) * N_reference, reference_range * n_alpha * N_reference, reference_range * n_beta * N_reference, reference_range * n_gamma * N_reference;
00680 
00681                         d2J_dReferencedX.block(0,valid_points_count,6,1) = tmp_vector_6;
00682 
00683                         valid_points_count++;
00684                 } // if (outlierWeights(0,i) > 0.0)
00685         }
00686 
00687         Matrix d2J_dZdX(Matrix::Zero(6, 2 * valid_points_count));
00688         d2J_dZdX.block(0,0,6,valid_points_count) = d2J_dReadingdX.block(0,0,6,valid_points_count);
00689         d2J_dZdX.block(0,valid_points_count,6,valid_points_count) = d2J_dReferencedX.block(0,0,6,valid_points_count);
00690 
00691         Matrix inv_J_hessian = J_hessian.inverse();
00692 
00693         covariance = d2J_dZdX * d2J_dZdX.transpose();
00694         covariance = inv_J_hessian * covariance * inv_J_hessian;
00695 
00696         return (sensorStdDev * sensorStdDev) * covariance;
00697 }
00698 
00699 
00700 
00701 template<typename T>
00702 T ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::getOverlap() const
00703 {
00704         const int nbPoints = this->lastErrorElements.reading.features.cols();
00705         const int dim = this->lastErrorElements.reading.features.rows();
00706         if(nbPoints == 0)
00707         {
00708                 throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method.");
00709         }
00710         
00711         if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise") ||
00712                 !this->lastErrorElements.reading.descriptorExists("normals"))
00713         {
00714                 LOG_INFO_STREAM("PointToPlaneErrorMinimizer - warning, no sensor noise or normals found. Using best estimate given outlier rejection instead.");
00715                 return this->weightedPointUsedRatio;
00716         }
00717 
00718         const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise"));
00719         const BOOST_AUTO(normals, this->lastErrorElements.reading.getDescriptorViewByName("normals"));
00720         int count = 0;
00721         for(int i=0; i < nbPoints; i++)
00722         {
00723                 if(this->lastErrorElements.matches.dists(0, i) != numeric_limits<T>::infinity())
00724                 {
00725                         const Vector d = this->lastErrorElements.reading.features.col(i) - this->lastErrorElements.reference.features.col(i);
00726                         const Vector n = normals.col(i);
00727                         const T projectionDist = d.head(dim-1).dot(n.normalized());
00728                         if(anyabs(projectionDist) < noises(0,i))
00729                                 count++;
00730                 }
00731         }
00732 
00733         return (T)count/(T)nbPoints;
00734 }
00735 
00736 template<typename T>
00737 typename ErrorMinimizersImpl<T>::Matrix ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::getCovariance() const
00738 {
00739   return covMatrix;
00740 }
00741 
00742 template struct ErrorMinimizersImpl<float>::PointToPlaneWithCovErrorMinimizer;
00743 template struct ErrorMinimizersImpl<double>::PointToPlaneWithCovErrorMinimizer;
00744 


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