From 6e55a7a6c9c920cb0cb8ea50a6a3b6adb60ab1b2 Mon Sep 17 00:00:00 2001 From: simonpierredeschenes Date: Mon, 10 Sep 2018 12:15:14 -0400 Subject: [PATCH] Fix PointToPlaneWithCovErrorMinimizer duplication. --- pointmatcher/ErrorMinimizersImpl.cpp | 385 --------------------------- pointmatcher/ErrorMinimizersImpl.h | 29 -- pointmatcher/Registry.cpp | 2 - 3 files changed, 416 deletions(-) diff --git a/pointmatcher/ErrorMinimizersImpl.cpp b/pointmatcher/ErrorMinimizersImpl.cpp index 43e565ae..48a6fe2f 100644 --- a/pointmatcher/ErrorMinimizersImpl.cpp +++ b/pointmatcher/ErrorMinimizersImpl.cpp @@ -495,388 +495,3 @@ typename ErrorMinimizersImpl::Matrix ErrorMinimizersImpl::PointToPointWith template struct ErrorMinimizersImpl::PointToPointWithCovErrorMinimizer; template struct ErrorMinimizersImpl::PointToPointWithCovErrorMinimizer; - -/////////////////////////////////////////////////////////////////////// -// Point To PLANE WITH COV ErrorMinimizer -/////////////////////////////////////////////////////////////////////// -template -ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer::PointToPlaneWithCovErrorMinimizer(const Parameters& params): - ErrorMinimizer("PointToPlaneWithCovErrorMinimizer", PointToPlaneWithCovErrorMinimizer::availableParameters(), params), - force2D(Parametrizable::get("force2D")), - sensorStdDev(Parametrizable::get("sensorStdDev")) -{ -} - -template -typename PointMatcher::TransformationParameters ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer::compute( - const DataPoints& filteredReading, - const DataPoints& filteredReference, - const OutlierWeights& outlierWeights, - const Matches& matches) -{ - assert(matches.ids.rows() > 0); - - // Fetch paired points - typename ErrorMinimizer::ErrorElements mPts(filteredReading, filteredReference, outlierWeights, matches); - - const int dim = mPts.reading.features.rows(); - const int nbPts = mPts.reading.features.cols(); - - // Adjust if the user forces 2D minimization on XY-plane - int forcedDim = dim - 1; - if(force2D && dim == 4) - { - mPts.reading.features.conservativeResize(3, Eigen::NoChange); - mPts.reading.features.row(2) = Matrix::Ones(1, nbPts); - mPts.reference.features.conservativeResize(3, Eigen::NoChange); - mPts.reference.features.row(2) = Matrix::Ones(1, nbPts); - forcedDim = dim - 2; - } - - // Fetch normal vectors of the reference point cloud (with adjustment if needed) - const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim)); - - // Note: Normal vector must be precalculated to use this error. Use appropriate input filter. - assert(normalRef.rows() > 0); - - // Compute cross product of cross = cross(reading X normalRef) - const Matrix cross = this->crossProduct(mPts.reading.features, normalRef); - - // wF = [weights*cross, weight*normals] - // F = [cross, normals] - Matrix wF(normalRef.rows()+ cross.rows(), normalRef.cols()); - Matrix F(normalRef.rows()+ cross.rows(), normalRef.cols()); - - for(int i=0; i < cross.rows(); i++) - { - wF.row(i) = mPts.weights.array() * cross.row(i).array(); - F.row(i) = cross.row(i); - } - for(int i=0; i < normalRef.rows(); i++) - { - wF.row(i + cross.rows()) = mPts.weights.array() * normalRef.row(i).array(); - F.row(i + cross.rows()) = normalRef.row(i); - } - - // Unadjust covariance A = wF * F' - const Matrix A = wF * F.transpose(); - - const Matrix deltas = mPts.reading.features - mPts.reference.features; - - // dot product of dot = dot(deltas, normals) - Matrix dotProd = Matrix::Zero(1, normalRef.cols()); - - for(int i=0; i(A, b, x); - - // Transform parameters to matrix - Matrix mOut; - if(dim == 4 && !force2D) - { - Eigen::Transform transform; - // IT IS NOT CORRECT TO USE EULER ANGLES! - // Rotation in Eular angles follow roll-pitch-yaw (1-2-3) rule - /*transform = Eigen::AngleAxis(x(0), Eigen::Matrix::UnitX()) - * Eigen::AngleAxis(x(1), Eigen::Matrix::UnitY()) - * Eigen::AngleAxis(x(2), Eigen::Matrix::UnitZ()); */ - // Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time! - /*const T pitch = -asin(transform(2,0)); - const T roll = atan2(transform(2,1), transform(2,2)); - const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch)); - std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/ - - transform = Eigen::AngleAxis(x.head(3).norm(),x.head(3).normalized()); - transform.translation() = x.segment(3, 3); - mOut = transform.matrix(); - } - else - { - Eigen::Transform transform; - transform = Eigen::Rotation2D (x(0)); - transform.translation() = x.segment(1, 2); - - if(force2D) - { - mOut = Matrix::Identity(dim, dim); - mOut.topLeftCorner(2, 2) = transform.matrix().topLeftCorner(2, 2); - mOut.topRightCorner(2, 1) = transform.matrix().topRightCorner(2, 1); - } - else - { - mOut = transform.matrix(); - } - } - - if (force2D) - { - this->covMatrix = this->estimateCovariance2D(filteredReading, filteredReference, matches, outlierWeights, mOut); - //std::cout << this->covMatrix << std::endl; - } - else - { - this->covMatrix = this->estimateCovariance(filteredReading, filteredReference, matches, outlierWeights, mOut); - } - - return mOut; -} - -template -typename ErrorMinimizersImpl::Matrix -ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer::estimateCovariance(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation) -{ - int max_nbr_point = outlierWeights.cols(); - - Matrix covariance(Matrix::Zero(6,6)); - Matrix J_hessian(Matrix::Zero(6,6)); - Matrix d2J_dReadingdX(Matrix::Zero(6, max_nbr_point)); - Matrix d2J_dReferencedX(Matrix::Zero(6, max_nbr_point)); - - Vector reading_point(Vector::Zero(3)); - Vector reference_point(Vector::Zero(3)); - Vector normal(3); - Vector reading_direction(Vector::Zero(3)); - Vector reference_direction(Vector::Zero(3)); - - Matrix normals = reference.getDescriptorViewByName("normals"); - - if (normals.rows() < 3) // Make sure there are normals in DataPoints - return std::numeric_limits::max() * Matrix::Identity(6,6); - - T beta = -asin(transformation(2,0)); - T alpha = atan2(transformation(2,1), transformation(2,2)); - T gamma = atan2(transformation(1,0)/cos(beta), transformation(0,0)/cos(beta)); - T t_x = transformation(0,3); - T t_y = transformation(1,3); - T t_z = transformation(2,3); - - Vector tmp_vector_6(Vector::Zero(6)); - - int valid_points_count = 0; - - for(int i = 0; i < max_nbr_point; ++i) - { - const int reference_idx = matches.ids(0,i); - if (reference_idx != Matches::InvalidId && outlierWeights(0,i) > 0.0) - { - reading_point = reading.features.block(0,i,3,1); - reference_point = reference.features.block(0,reference_idx,3,1); - - normal = normals.block(0,reference_idx,3,1); - - T reading_range = reading_point.norm(); - reading_direction = reading_point / reading_range; - T reference_range = reference_point.norm(); - reference_direction = reference_point / reference_range; - - T n_alpha = normal(2)*reading_direction(1) - normal(1)*reading_direction(2); - T n_beta = normal(0)*reading_direction(2) - normal(2)*reading_direction(0); - T n_gamma = normal(1)*reading_direction(0) - normal(0)*reading_direction(1); - - T E = normal(0)*(reading_point(0) - gamma*reading_point(1) + beta*reading_point(2) + t_x - reference_point(0)); - E += normal(1)*(gamma*reading_point(0) + reading_point(1) - alpha*reading_point(2) + t_y - reference_point(1)); - E += normal(2)*(-beta*reading_point(0) + alpha*reading_point(1) + reading_point(2) + t_z - reference_point(2)); - - T N_reading = normal(0)*(reading_direction(0) - gamma*reading_direction(1) + beta*reading_direction(2)); - N_reading += normal(1)*(gamma*reading_direction(0) + reading_direction(1) - alpha*reading_direction(2)); - N_reading += normal(2)*(-beta*reading_direction(0) + alpha*reading_direction(1) + reading_direction(2)); - - T N_reference = -(normal(0)*reference_direction(0) + normal(1)*reference_direction(1) + normal(2)*reference_direction(2)); - - // update the hessian and d2J/dzdx - tmp_vector_6 << normal(0), normal(1), normal(2), reading_range * n_alpha, reading_range * n_beta, reading_range * n_gamma; - - J_hessian += tmp_vector_6 * tmp_vector_6.transpose(); - - 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); - - d2J_dReadingdX.block(0,valid_points_count,6,1) = tmp_vector_6; - - 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; - - d2J_dReferencedX.block(0,valid_points_count,6,1) = tmp_vector_6; - - valid_points_count++; - } // if (outlierWeights(0,i) > 0.0) - } - - Matrix d2J_dZdX(Matrix::Zero(6, 2 * valid_points_count)); - d2J_dZdX.block(0,0,6,valid_points_count) = d2J_dReadingdX.block(0,0,6,valid_points_count); - d2J_dZdX.block(0,valid_points_count,6,valid_points_count) = d2J_dReferencedX.block(0,0,6,valid_points_count); - - Matrix inv_J_hessian = J_hessian.inverse(); - - covariance = d2J_dZdX * d2J_dZdX.transpose(); - covariance = inv_J_hessian * covariance * inv_J_hessian; - - return (sensorStdDev * sensorStdDev) * covariance; -} - -template -typename ErrorMinimizersImpl::Matrix -ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer::estimateCovariance2D(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation) -{ - int max_nbr_point = outlierWeights.cols(); - - Matrix covariance(Matrix::Zero(3,3)); - - Matrix d2jX2(Matrix::Zero(3,3)); - Matrix d2jpX(Matrix::Zero(3, max_nbr_point)); - Matrix d2jqX(Matrix::Zero(3, max_nbr_point)); - - Vector reading_point(Vector::Zero(2)); - Vector reference_point(Vector::Zero(2)); - Vector normal(2); - - Vector reading_direction(Vector::Zero(2)); - Vector reference_direction(Vector::Zero(2)); - - Matrix normals = reference.getDescriptorViewByName("normals"); - - if (normals.rows() != 2) // Make sure there are normals in DataPoints - return std::numeric_limits::max() * Matrix::Identity(3,3); - - T t_x = transformation(0,2); - T t_y = transformation(1,2); - T t_t = asin(transformation(1,0)); - - T sin_t = sin(t_t); - T cos_t = cos(t_t); - - int valid_points_count = 0; - - for(int i = 0; i < max_nbr_point; ++i) - { - const int reference_idx = matches.ids(0,i); - if (reference_idx != Matches::InvalidId && outlierWeights(0,i) > 0.0) - { - reading_point = reading.features.block(0,i,2,1); - T reading_range = reading_point.norm(); - - if (reading_range > 0) // skip the case when reading_range is 0. TODO: check why this happens - { - reference_point = reference.features.block(0,reference_idx,2,1); - normal = normals.block(0,reference_idx,2,1); - - - reading_direction = reading_point / reading_range; - T reference_range = reference_point.norm(); - reference_direction = reference_point / reference_range; - - T d2jx2 = 2*normal(0)*normal(0); - T d2jxy = 2*normal(0)*normal(1); - T d2jy2 = 2*normal(1)*normal(1); - - T d2jxt = -2*normal(0)*( normal(0)*(reading_range *reading_direction(1) *cos_t + reading_range * reading_direction(0) *sin_t ) - - normal(1)*(reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t)); - T d2jyt = -2*normal(1)*( normal(0)*(reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t) - - normal(1)*(reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t)); - - T d2jt2 = 2*pow(normal(0)*(reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t) - - normal(1)*(reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t),2) - - 2*(normal(0)*(reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t) + - normal(1)*(reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t)) - *( normal(0)*(t_x - reference_range*reference_direction(0) + reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t) - + normal(1)*(t_y - reference_range*reference_direction(1) + reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t)); - - //f = (normal(0)*(t_x - reference_range*reference_direction(0) + reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t) + normal(1)*(t_y - reference_range*reference_direction(1) + reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t))^2 - - d2jX2 += (Matrix(3,3) << d2jx2, d2jxy, d2jxt, - d2jxy, d2jy2, d2jyt, - d2jxt, d2jyt, d2jt2).finished(); - - T d2jpx = -2*normal(0)*(normal(0)*reference_direction(0) + normal(1)*reference_direction(1)); - T d2jpy = -2*normal(1)*(normal(0)*reference_direction(0) + normal(1)*reference_direction(1)); - T d2jpt = 2*( normal(0)*(reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t) - - normal(1)*(reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t)) - *( normal(0)*reference_direction(0) - + normal(1)*reference_direction(1)); - - T d2jqx = 2*normal(0)*( normal(0)*(reading_direction(0)*cos_t - reading_direction(1)*sin_t) - + normal(1)*(reading_direction(1)*cos_t + reading_direction(0)*sin_t)); - T d2jqy = 2*normal(1)*( normal(0)*(reading_direction(0)*cos_t - reading_direction(1)*sin_t) - + normal(1)*(reading_direction(1)*cos_t + reading_direction(0)*sin_t)); - - T d2jqt = - 2*( normal(0)*(reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t) - - normal(1)*(reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t)) - *(normal(0)*(reading_direction(0)*cos_t - reading_direction(1)*sin_t) + normal(1)*(reading_direction(1)*cos_t + reading_direction(0)*sin_t)) - - 2*(normal(0)*(reading_direction(1)*cos_t + reading_direction(0)*sin_t) - normal(1)*(reading_direction(0)*cos_t - reading_direction(1)*sin_t)) - *( normal(0)*(t_x - reference_range*reference_direction(0) + reading_range*reading_direction(0)*cos_t - reading_range*reading_direction(1)*sin_t) - + normal(1)*(t_y - reference_range*reference_direction(1) + reading_range*reading_direction(1)*cos_t + reading_range*reading_direction(0)*sin_t)); - - d2jpX.block(0,valid_points_count,3,1 ) = (Vector(3) << d2jpx,d2jpy,d2jpt ).finished(); - d2jqX.block(0,valid_points_count,3,1 ) = (Vector(3) << d2jqx,d2jqy,d2jqt ).finished(); - - valid_points_count++; - } // if (reading_range>0) - } // if (outlierWeights(0,i) > 0.0) - } - - Matrix d2jZX(Matrix::Zero(3, 2 * valid_points_count)); - d2jZX.block(0,0,3,valid_points_count) = d2jpX.block(0,0,3,valid_points_count); - d2jZX.block(0,valid_points_count,3,valid_points_count) = d2jqX.block(0,0,3,valid_points_count); - - Matrix d2jX2_inv = d2jX2.inverse(); - - Matrix covZ(Matrix::Identity(2*valid_points_count,2*valid_points_count)); - covZ *= (sensorStdDev * sensorStdDev); - //sensorStdDev;// - - - covariance = d2jZX * covZ * d2jZX.transpose(); - covariance = d2jX2_inv * covariance * d2jX2_inv; - - return covariance; -} - -template -T ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer::getOverlap() const -{ - const int nbPoints = this->lastErrorElements.reading.features.cols(); - const int dim = this->lastErrorElements.reading.features.rows(); - if(nbPoints == 0) - { - throw std::runtime_error("Error, last error element empty. Error minimizer needs to be called at least once before using this method."); - } - - if (!this->lastErrorElements.reading.descriptorExists("simpleSensorNoise") || - !this->lastErrorElements.reading.descriptorExists("normals")) - { - LOG_INFO_STREAM("PointToPlaneErrorMinimizer - warning, no sensor noise or normals found. Using best estimate given outlier rejection instead."); - return this->getWeightedPointUsedRatio(); - } - - const BOOST_AUTO(noises, this->lastErrorElements.reading.getDescriptorViewByName("simpleSensorNoise")); - const BOOST_AUTO(normals, this->lastErrorElements.reading.getDescriptorViewByName("normals")); - int count = 0; - for(int i=0; i < nbPoints; i++) - { - if(this->lastErrorElements.matches.dists(0, i) != numeric_limits::infinity()) - { - const Vector d = this->lastErrorElements.reading.features.col(i) - this->lastErrorElements.reference.features.col(i); - const Vector n = normals.col(i); - const T projectionDist = d.head(dim-1).dot(n.normalized()); - if(anyabs(projectionDist) < noises(0,i)) - count++; - } - } - - return (T)count/(T)nbPoints; -} - -template -typename ErrorMinimizersImpl::Matrix ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer::getCovariance() const -{ - return covMatrix; -} - -template struct ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer; -template struct ErrorMinimizersImpl::PointToPlaneWithCovErrorMinimizer; diff --git a/pointmatcher/ErrorMinimizersImpl.h b/pointmatcher/ErrorMinimizersImpl.h index 2772aab3..73c6d2df 100644 --- a/pointmatcher/ErrorMinimizersImpl.h +++ b/pointmatcher/ErrorMinimizersImpl.h @@ -58,8 +58,6 @@ struct ErrorMinimizersImpl typedef typename PointMatcher::Vector Vector; typedef typename PointMatcher::Matrix Matrix; - typedef ::PointToPlaneErrorMinimizer PointToPlaneErrorMinimizer; - struct IdentityErrorMinimizer: ErrorMinimizer { inline static const std::string description() @@ -124,33 +122,6 @@ struct ErrorMinimizersImpl virtual Matrix getCovariance() const; Matrix estimateCovariance(const ErrorElements& mPts, const TransformationParameters& transformation); }; - - struct PointToPlaneWithCovErrorMinimizer: ErrorMinimizer - { - inline static const std::string description() - { - return "Point-to-plane error (or point-to-line in 2D). Based on \\cite{Chen1991Point2Plane}. Covariance estimation based on \\cite{Censi2007ICPCovariance}."; - } - - inline static const ParametersDoc availableParameters() - { - return boost::assign::list_of - ( "force2D", "If set to true(1), the minimization will be force to give a solution in 2D (i.e., on the XY-plane) even with 3D inputs.", "0", "0", "1", &P::Comp) - ( "sensorStdDev", "sensor standard deviation", "0.01", "0.", "inf", &P::Comp) - ; - } - - const bool force2D; - const T sensorStdDev; - Matrix covMatrix; - - PointToPlaneWithCovErrorMinimizer(const Parameters& params = Parameters()); - virtual TransformationParameters compute(const DataPoints& filteredReading, const DataPoints& filteredReference, const OutlierWeights& outlierWeights, const Matches& matches); - virtual T getOverlap() const; - virtual Matrix getCovariance() const; - Matrix estimateCovariance(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation); - Matrix estimateCovariance2D(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation); - }; }; // ErrorMinimizersImpl #endif // __POINTMATCHER_ERRORMINIMIZER_H diff --git a/pointmatcher/Registry.cpp b/pointmatcher/Registry.cpp index dccb5095..15cd1e63 100644 --- a/pointmatcher/Registry.cpp +++ b/pointmatcher/Registry.cpp @@ -41,8 +41,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "MatchersImpl.h" #include "OutlierFiltersImpl.h" #include "ErrorMinimizersImpl.h" -#include "ErrorMinimizers/PointToPlane.h" -#include "ErrorMinimizers/PointToPlaneWithCov.h" #include "TransformationCheckersImpl.h" #include "InspectorsImpl.h"