i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 10:07:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.15.3: util3d::computeVarianceAndCorrespondences(): added max angle parameters for normal type. util3d::downsample(): added Normal types support. OccupancyGrid: fixed 2d scan with 5 channels not handled correctly on update.

Browse Source
This commit is contained in:
matlabbe
2017-12-13 18:12:40 -05:00
parent 7091406abc
commit 398ca1f8e4
7 changed files with 122 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_registration.cpp
+38 -9
View File
@@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/common/common.h>
#include <rtabmap/utilite/ULogger.h>
namespace rtabmap
@@ -237,6 +238,7 @@ void computeVarianceAndCorrespondences(
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
double maxCorrespondenceDistance,
double maxCorrespondenceAngle,
double & variance,
int & correspondencesOut)
{
@@ -244,26 +246,53 @@ void computeVarianceAndCorrespondences(
correspondencesOut = 0;
pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
const pcl::PointCloud<pcl::PointNormal>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
est->setInputTarget(target);
est->setInputSource(source);
pcl::Correspondences correspondences;
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
if(correspondences.size()>=3)
if(correspondences.size())
{
std::vector<double> distances(correspondences.size());
correspondencesOut = 0;
for(unsigned int i=0; i<correspondences.size(); ++i)
{
distances[i] = correspondences[i].distance;
if(maxCorrespondenceAngle <= 0.0)
{
++correspondencesOut;
}
else
{
Eigen::Vector4f v1(
target->at(correspondences[i].index_match).normal_x,
target->at(correspondences[i].index_match).normal_y,
target->at(correspondences[i].index_match).normal_z,
0);
Eigen::Vector4f v2(
source->at(correspondences[i].index_query).normal_x,
source->at(correspondences[i].index_query).normal_y,
source->at(correspondences[i].index_query).normal_z,
0);
float angle = pcl::getAngle3D(v1, v2);
if(angle < maxCorrespondenceAngle)
{
++correspondencesOut;
}
}
}
if(correspondencesOut)
{
distances.resize(correspondencesOut);
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
variance = (2.1981 * median_error_sqr);
//variance
std::sort(distances.begin (), distances.end ());
double median_error_sqr = distances[distances.size () >> 1];
variance = (2.1981 * median_error_sqr);
}
}
correspondencesOut = (int)correspondences.size();
}
void computeVarianceAndCorrespondences(