i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed odometry mesh with long edges, Optimizer/Robust is not set false because of some "solution not found" from g2o or gtsam

Browse Source
This commit is contained in:
matlabbe
2016-02-18 15:23:25 -05:00
parent 04d153df59
commit 62c7be9f35
4 changed files with 32 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/util3d_surface.cpp
View File

@@ -95,7 +95,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
double angleTolerance,
bool quad,
int trianglePixelSize)
int trianglePixelSize,
const Eigen::Vector3f & viewpoint)
{
UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
UASSERT(cloud->is_dense == false);
@@ -106,6 +107,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGB>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGB>::TRIANGLE_RIGHT_CUT);
ofm.setInputCloud (cloud);
ofm.setAngleTolerance(angleTolerance);
ofm.setViewpoint(viewpoint);
std::vector<pcl::Vertices> vertices;
ofm.reconstruct (vertices);
@@ -130,7 +133,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
double angleTolerance,
bool quad,
int trianglePixelSize)
int trianglePixelSize,
const Eigen::Vector3f & viewpoint)
{
UDEBUG("size=%d angle=%f quad=%d triangleSize=%d", (int)cloud->size(), angleTolerance, quad?1:0, trianglePixelSize);
UASSERT(cloud->is_dense == false);
@@ -141,6 +145,8 @@ std::vector<pcl::Vertices> organizedFastMesh(
ofm.setTriangulationType (quad?pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::QUAD_MESH:pcl::OrganizedFastMesh<pcl::PointXYZRGBNormal>::TRIANGLE_RIGHT_CUT);
ofm.setInputCloud (cloud);
ofm.setAngleTolerance(angleTolerance);
ofm.setViewpoint(viewpoint);
std::vector<pcl::Vertices> vertices;
ofm.reconstruct (vertices);