diff --git a/corelib/src/GainCompensator.cpp b/corelib/src/GainCompensator.cpp index b55551a9..c794e5a9 100644 --- a/corelib/src/GainCompensator.cpp +++ b/corelib/src/GainCompensator.cpp @@ -31,7 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include +#include +#include +#include +#include namespace rtabmap { @@ -90,6 +94,30 @@ void GainCompensator::feed( feed(clouds, indices, links); } +// @see https://studiofreya.com/3d-math-and-physics/simple-aabb-vs-aabb-collision-detection/ +struct AABB +{ + AABB() : c(), r() {} + + AABB(const Eigen::Vector3f & center, const Eigen::Vector3f & halfwidths) + : c(center) + , r(halfwidths) + {} + + Eigen::Vector3f c; // center point + Eigen::Vector3f r; // halfwidths +}; + +bool testAABBAABB(const AABB &a, const AABB &b) +{ + if ( fabs(a.c[0] - b.c[0]) > (a.r[0] + b.r[0]) ) return false; + if ( fabs(a.c[1] - b.c[1]) > (a.r[1] + b.r[1]) ) return false; + if ( fabs(a.c[2] - b.c[2]) > (a.r[2] + b.r[2]) ) return false; + + // We have an overlap + return true; +}; + /** * @see https://github.com/opencv/opencv/blob/master/modules/stitching/src/exposure_compensate.cpp */ @@ -116,71 +144,147 @@ void feedImpl( // make id to index map idToIndex.clear(); + std::vector indexToId(clouds.size()); int oi=0; + std::map > boundingBoxes; for(typename std::map::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) { idToIndex.insert(std::make_pair(iter->first, oi)); + indexToId[oi] = iter->first; UASSERT(indices.empty() || uContains(indices, iter->first)); - N(oi,oi) = iter->second->size(); + Eigen::Vector4f minPt(0,0,0,0); + Eigen::Vector4f maxPt(0,0,0,0); + if(indices.empty() || indices.at(iter->first)->empty()) + { + N(oi,oi) = iter->second->size(); + pcl::getMinMax3D(*iter->second, minPt, maxPt); + } + else + { + N(oi,oi) = indices.at(iter->first)->size(); + pcl::getMinMax3D(*iter->second, *indices.at(iter->first), minPt, maxPt); + } + minPt[0] -= maxCorrespondenceDistance; + minPt[1] -= maxCorrespondenceDistance; + minPt[2] -= maxCorrespondenceDistance; + maxPt[0] += maxCorrespondenceDistance; + maxPt[1] += maxCorrespondenceDistance; + maxPt[2] += maxCorrespondenceDistance; + boundingBoxes.insert(std::make_pair(iter->first, std::make_pair(Eigen::Vector3f(minPt[0], minPt[1], minPt[2]), Eigen::Vector3f(maxPt[0], maxPt[1], maxPt[2])))); ++oi; } - typename pcl::registration::CorrespondenceEstimation::Ptr est; - est.reset(new pcl::registration::CorrespondenceEstimation); - + typename pcl::search::KdTree kdtree; + int lastKdTreeId = 0; for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) { if(uContains(idToIndex, iter->second.from()) && uContains(idToIndex, iter->second.to())) { - UDEBUG("estimate...%d %d", iter->second.from(), iter->second.to()); const typename pcl::PointCloud::Ptr & cloudFrom = clouds.at(iter->second.from()); const typename pcl::PointCloud::Ptr & cloudTo = clouds.at(iter->second.to()); if(cloudFrom->size() && cloudTo->size()) { - est->setInputTarget(cloudFrom); //match - if(iter->second.transform().isIdentity() || iter->second.transform().isNull()) + //Are bounding boxes intersect? + std::pair bbMinMaxFrom = boundingBoxes.at(iter->second.from()); + std::pair bbMinMaxTo = boundingBoxes.at(iter->second.to()); + Eigen::Affine3f t = Transform::getIdentity().toEigen3f(); + if(!iter->second.transform().isIdentity() && !iter->second.transform().isNull()) { - est->setInputSource(cloudTo); //query + t = iter->second.transform().toEigen3f(); + pcl::transformPoint(bbMinMaxTo.first, bbMinMaxTo.first, t); + pcl::transformPoint(bbMinMaxTo.second, bbMinMaxTo.second, t); } - else + AABB bbFrom(Eigen::Vector3f((bbMinMaxFrom.second[0] + bbMinMaxFrom.first[0])/2.0f, (bbMinMaxFrom.second[1] + bbMinMaxFrom.first[1])/2.0f, (bbMinMaxFrom.second[2] + bbMinMaxFrom.first[2])/2.0f), + Eigen::Vector3f((bbMinMaxFrom.second[0] - bbMinMaxFrom.first[0])/2.0f, (bbMinMaxFrom.second[1] - bbMinMaxFrom.first[1])/2.0f, (bbMinMaxFrom.second[2] - bbMinMaxFrom.first[2])/2.0f)); + AABB bbTo(Eigen::Vector3f((bbMinMaxTo.second[0] + bbMinMaxTo.first[0])/2.0f, (bbMinMaxTo.second[1] + bbMinMaxTo.first[1])/2.0f, (bbMinMaxTo.second[2] + bbMinMaxTo.first[2])/2.0f), + Eigen::Vector3f((bbMinMaxTo.second[0] - bbMinMaxTo.first[0])/2.0f, (bbMinMaxTo.second[1] - bbMinMaxTo.first[1])/2.0f, (bbMinMaxTo.second[2] - bbMinMaxTo.first[2])/2.0f)); + //UDEBUG("%d = %f,%f,%f %f,%f,%f", iter->second.from(), bbMinMaxFrom.first[0], bbMinMaxFrom.first[1], bbMinMaxFrom.first[2], bbMinMaxFrom.second[0], bbMinMaxFrom.second[1], bbMinMaxFrom.second[2]); + //UDEBUG("%d = %f,%f,%f %f,%f,%f", iter->second.to(), bbMinMaxTo.first[0], bbMinMaxTo.first[1], bbMinMaxTo.first[2], bbMinMaxTo.second[0], bbMinMaxTo.second[1], bbMinMaxTo.second[2]); + if(testAABBAABB(bbFrom, bbTo)) { - est->setInputSource(util3d::transformPointCloud(cloudTo, iter->second.transform())); //query - } - - if(indices.size()) - { - if(indices.at(iter->second.from())->size()) + if(lastKdTreeId <= 0 || lastKdTreeId!=iter->second.from()) { - est->setIndicesTarget(indices.at(iter->second.from())); + //reconstruct kdtree + if(indices.size() && indices.at(iter->second.from())->size()) + { + kdtree.setInputCloud(cloudFrom, indices.at(iter->second.from())); + } + else + { + kdtree.setInputCloud(cloudFrom); + } } - if(indices.at(iter->second.to())->size()) + + pcl::Correspondences correspondences; + pcl::IndicesPtr indicesTo(new std::vector); + std::set addedFrom; + if(indices.size() && indices.at(iter->second.to())->size()) { - est->setIndicesSource(indices.at(iter->second.to())); + const pcl::IndicesPtr & indicesTo = indices.at(iter->second.to()); + correspondences.resize(indicesTo->size()); + int oi=0; + for(unsigned int i=0; isize(); ++i) + { + std::vector k_indices; + std::vector k_sqr_distances; + if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(indicesTo->at(i)), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1)) + { + if(addedFrom.find(k_indices[0]) == addedFrom.end()) + { + correspondences[oi].index_match = k_indices[0]; + correspondences[oi].index_query = indicesTo->at(i); + correspondences[oi].distance = k_sqr_distances[0]; + addedFrom.insert(k_indices[0]); + ++oi; + } + } + } + correspondences.resize(oi); } - } - - pcl::Correspondences correspondences; - est->determineCorrespondences(correspondences, maxCorrespondenceDistance); - UDEBUG("correspondences = %d", (int)correspondences.size()); - if((minOverlap <= 0.0 && correspondences.size()) || - (double(correspondences.size()) / double(clouds.at(iter->second.from())->size()) >= minOverlap && - double(correspondences.size()) / double(clouds.at(iter->second.to())->size()) >= minOverlap)) - { - int i = idToIndex.at(iter->second.from()); - int j = idToIndex.at(iter->second.to()); - N(i, j) = N(j, i) = correspondences.size(); - - double Isum1 = 0, Isum2 = 0; - for (unsigned int c = 0; c < correspondences.size(); ++c) + else { - const PointT & pt1 = cloudFrom->at(correspondences.at(c).index_match); - const PointT & pt2 = cloudTo->at(correspondences.at(c).index_query); + correspondences.resize(cloudTo->size()); + int oi=0; + for(unsigned int i=0; isize(); ++i) + { + std::vector k_indices; + std::vector k_sqr_distances; + if(kdtree.radiusSearch(pcl::transformPoint(cloudTo->at(i), t), maxCorrespondenceDistance, k_indices, k_sqr_distances, 1)) + { + if(addedFrom.find(k_indices[0]) == addedFrom.end()) + { + correspondences[oi].index_match = k_indices[0]; + correspondences[oi].index_query = i; + correspondences[oi].distance = k_sqr_distances[0]; + addedFrom.insert(k_indices[0]); + ++oi; + } + } + } + correspondences.resize(oi); + } - Isum1 += std::sqrt(static_cast(sqr(pt1.r) + sqr(pt1.g) + sqr(pt1.b))); - Isum2 += std::sqrt(static_cast(sqr(pt2.r) + sqr(pt2.g) + sqr(pt2.b))); + UDEBUG("%d->%d: correspondences = %d", iter->second.from(), iter->second.to(), (int)correspondences.size()); + if((minOverlap <= 0.0 && correspondences.size()) || + (double(correspondences.size()) / double(clouds.at(iter->second.from())->size()) >= minOverlap && + double(correspondences.size()) / double(clouds.at(iter->second.to())->size()) >= minOverlap)) + { + int i = idToIndex.at(iter->second.from()); + int j = idToIndex.at(iter->second.to()); + + double Isum1 = 0, Isum2 = 0; + for (unsigned int c = 0; c < correspondences.size(); ++c) + { + const PointT & pt1 = cloudFrom->at(correspondences.at(c).index_match); + const PointT & pt2 = cloudTo->at(correspondences.at(c).index_query); + + Isum1 += std::sqrt(static_cast(sqr(pt1.r) + sqr(pt1.g) + sqr(pt1.b))); + Isum2 += std::sqrt(static_cast(sqr(pt2.r) + sqr(pt2.g) + sqr(pt2.b))); + } + N(i, j) = N(j, i) = correspondences.size(); + I(i, j) = Isum1 / N(i, j); + I(j, i) = Isum2 / N(i, j); } - I(i, j) = Isum1 / N(i, j); - I(j, i) = Isum2 / N(i, j); } } } @@ -202,6 +306,13 @@ void feedImpl( gains = cv::Mat_(); cv::solve(A, b, gains); + if(ULogger::kDebug) + { + for(int i=0; idoubleSpinBox_gainOverlap, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_gainAlpha, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_gainBeta, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_gainLinkedLocationsOnly, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->groupBox_meshing, SIGNAL(clicked(bool)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); @@ -208,6 +209,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou settings.setValue("gain_overlap", _ui->doubleSpinBox_gainOverlap->value()); settings.setValue("gain_alpha", _ui->doubleSpinBox_gainAlpha->value()); settings.setValue("gain_beta", _ui->doubleSpinBox_gainBeta->value()); + settings.setValue("gain_linked_locations", _ui->checkBox_gainLinkedLocationsOnly->isChecked()); settings.setValue("mesh", _ui->groupBox_meshing->isChecked()); settings.setValue("mesh_radius", _ui->doubleSpinBox_gp3Radius->value()); @@ -271,6 +273,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou _ui->doubleSpinBox_gainOverlap->setValue(settings.value("gain_overlap", _ui->doubleSpinBox_gainOverlap->value()).toDouble()); _ui->doubleSpinBox_gainAlpha->setValue(settings.value("gain_alpha", _ui->doubleSpinBox_gainAlpha->value()).toDouble()); _ui->doubleSpinBox_gainBeta->setValue(settings.value("gain_beta", _ui->doubleSpinBox_gainBeta->value()).toDouble()); + _ui->checkBox_gainLinkedLocationsOnly->setChecked(settings.value("gain_linked_locations", _ui->checkBox_gainLinkedLocationsOnly->isChecked()).toBool()); _ui->groupBox_meshing->setChecked(settings.value("mesh", _ui->groupBox_meshing->isChecked()).toBool()); _ui->doubleSpinBox_gp3Radius->setValue(settings.value("mesh_radius", _ui->doubleSpinBox_gp3Radius->value()).toDouble()); @@ -327,11 +330,12 @@ void ExportCloudsDialog::restoreDefaults() _ui->doubleSpinBox_dilationVoxelSize->setValue(0.01); _ui->spinBox_dilationSteps->setValue(0); - _ui->groupBox_gain->setChecked(true); + _ui->groupBox_gain->setChecked(false); _ui->doubleSpinBox_gainRadius->setValue(0.02); _ui->doubleSpinBox_gainOverlap->setValue(0.05); _ui->doubleSpinBox_gainAlpha->setValue(0.01); _ui->doubleSpinBox_gainBeta->setValue(10); + _ui->checkBox_gainLinkedLocationsOnly->setChecked(false); _ui->groupBox_meshing->setChecked(false); _ui->doubleSpinBox_gp3Radius->setValue(0.04); @@ -713,7 +717,27 @@ bool ExportCloudsDialog::getExportedClouds( QApplication::processEvents(); QApplication::processEvents(); - compensator.feed(clouds, links); + if(!_ui->checkBox_gainLinkedLocationsOnly->isChecked()) + { + std::multimap allLinks; + for(std::map::Ptr, pcl::IndicesPtr> >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) + { + int from = iter->first; + std::map::Ptr, pcl::IndicesPtr> >::const_iterator jter = iter; + ++jter; + for(;jter!=clouds.end(); ++jter) + { + int to = jter->first; + allLinks.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, poses.at(from).inverse()*poses.at(to)))); + } + } + + compensator.feed(clouds, allLinks); + } + else + { + compensator.feed(clouds, links); + } _progressDialog->appendText(tr("Applying gain compensation...")); for(std::map::Ptr, pcl::IndicesPtr> >::iterator jter=clouds.begin();jter!=clouds.end(); ++jter) diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui index 024ee3d7..dd217f7d 100644 --- a/guilib/src/ui/exportCloudsDialog.ui +++ b/guilib/src/ui/exportCloudsDialog.ui @@ -23,9 +23,9 @@ 0 - 0 + -823 773 - 1689 + 1731 @@ -811,6 +811,38 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. true + + + + Alpha + + + true + + + + + + + + + + 0 + + + 1.000000000000000 + + + 1000.000000000000000 + + + 1.000000000000000 + + + 100.000000000000000 + + + @@ -853,16 +885,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - - - - Alpha - - - true - - - @@ -885,28 +907,6 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. - - - - - - - 0 - - - 1.000000000000000 - - - 1000.000000000000000 - - - 1.000000000000000 - - - 100.000000000000000 - - - @@ -939,6 +939,23 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0. + + + + Do compensation just between linked locations. Otherwise, a full compensation between all locations is done (longer to do but quality is better). + + + true + + + + + + + + + +