PointMatcher<T>::ErrorMinimizer::ErrorElements::ErrorElements(const DataPoints& reading, const DataPoints reference, const OutlierWeights weights, const Matches matches): reading(reading), reference(reference), weights(weights), matches(matches) { assert(reading.features.cols() == reference.features.cols()); assert(reading.features.cols() == weights.cols()); assert(reading.features.cols() == matches.dists.cols()); // May have no descriptors... size 0 }
void PointMatcher<T>::ICPChainBase::filterGrossOutliersAndCalcErrors (const DataPoints& referenceIn, double maxDistSq, DataPoints& reading, Matrix & errors // in-out ){ // Remove points in reading further than sqrt(maxDistSq) from reference. initICP(); typedef Parametrizable::Parameters Parameters; Parameters p; ostringstream os; os.precision(16); os << maxDistSq; p["maxDist"] = os.str(); this->outlierFilters.clear(); this->outlierFilters.push_back(new typename OutlierFiltersImpl<T>::MaxDistOutlierFilter(p)); // Match to closest point in Reference const Matches matches = this->matcher->findClosests(reading); //----------------------------- // Detect outliers const OutlierWeights outlierWeights ( this->outlierFilters.compute(reading, referenceIn, matches) ); assert(outlierWeights.rows() == matches.ids.rows()); assert(outlierWeights.cols() == matches.ids.cols()); typename ErrorMinimizer::ErrorElements& mPts = this->errorMinimizer->getMatchedPoints(reading, referenceIn, matches, outlierWeights); errors = mPts.matches.dists.cwiseSqrt(); // take square root reading = mPts.reading; }
typename PointMatcher<T>::TransformationParameters PointMatcher<T>::ICP::computeWithTransformedReference( const DataPoints& readingIn, const DataPoints& reference, const TransformationParameters& T_refIn_refMean, const TransformationParameters& T_refIn_dataIn) { timer t; // Print how long take the algo // Apply readings filters // reading is express in frame <dataIn> DataPoints reading(readingIn); //const int nbPtsReading = reading.features.cols(); this->readingDataPointsFilters.init(); this->readingDataPointsFilters.apply(reading); readingFiltered = reading; // Reajust reading position: // from here reading is express in frame <refMean> TransformationParameters T_refMean_dataIn = T_refIn_refMean.inverse() * T_refIn_dataIn; this->transformations.apply(reading, T_refMean_dataIn); // Prepare reading filters used in the loop this->readingStepDataPointsFilters.init(); // Since reading and reference are express in <refMean> // the frame <refMean> is equivalent to the frame <iter(0)> const int dim(reference.features.rows()); TransformationParameters T_iter = Matrix::Identity(dim, dim); bool iterate(true); this->transformationCheckers.init(T_iter, iterate); size_t iterationCount(0); // statistics on last step this->inspector->addStat("ReadingPreprocessingDuration", t.elapsed()); this->inspector->addStat("ReadingInPointCount", readingIn.features.cols()); this->inspector->addStat("ReadingPointCount", reading.features.cols()); LOG_INFO_STREAM("PointMatcher::icp - reading pre-processing took " << t.elapsed() << " [s]"); this->prefilteredReadingPtsCount = reading.features.cols(); t.restart(); // iterations while (iterate) { DataPoints stepReading(reading); //----------------------------- // Apply step filter this->readingStepDataPointsFilters.apply(stepReading); //----------------------------- // Transform Readings this->transformations.apply(stepReading, T_iter); //----------------------------- // Match to closest point in Reference const Matches matches( this->matcher->findClosests(stepReading) ); //----------------------------- // Detect outliers const OutlierWeights outlierWeights( this->outlierFilters.compute(stepReading, reference, matches) ); assert(outlierWeights.rows() == matches.ids.rows()); assert(outlierWeights.cols() == matches.ids.cols()); //cout << "outlierWeights: " << outlierWeights << "\n"; //----------------------------- // Dump this->inspector->dumpIteration( iterationCount, T_iter, reference, stepReading, matches, outlierWeights, this->transformationCheckers ); //----------------------------- // Error minimization // equivalent to: // T_iter(i+1)_iter(0) = T_iter(i+1)_iter(i) * T_iter(i)_iter(0) T_iter = this->errorMinimizer->compute( stepReading, reference, outlierWeights, matches) * T_iter; // Old version //T_iter = T_iter * this->errorMinimizer->compute( // stepReading, reference, outlierWeights, matches); // in test this->transformationCheckers.check(T_iter, iterate); ++iterationCount; } this->inspector->addStat("IterationsCount", iterationCount); this->inspector->addStat("PointCountTouched", this->matcher->getVisitCount()); this->matcher->resetVisitCount(); this->inspector->addStat("OverlapRatio", this->errorMinimizer->getWeightedPointUsedRatio()); this->inspector->addStat("ConvergenceDuration", t.elapsed()); this->inspector->finish(iterationCount); LOG_INFO_STREAM("PointMatcher::icp - " << iterationCount << " iterations took " << t.elapsed() << " [s]"); // Move transformation back to original coordinate (without center of mass) // T_iter is equivalent to: T_iter(i+1)_iter(0) // the frame <iter(0)> equals <refMean> // so we have: // T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_refMean_dataIn // T_iter(i+1)_dataIn = T_iter(i+1)_iter(0) * T_iter(0)_dataIn // T_refIn_refMean remove the temperary frame added during initialization return (T_refIn_refMean * T_iter * T_refMean_dataIn); }
typename ErrorMinimizersImpl<T>::Matrix ErrorMinimizersImpl<T>::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<T>::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) { if (outlierWeights(0,i) > 0.0) { reading_point = reading.features.block(0,i,3,1); int reference_idx = matches.ids(0,i); 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; }