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

GUI: Fixed viewpoint including local transform when generating organized mesh

Browse Source
This commit is contained in:
matlabbe
2016-04-12 16:47:55 -04:00
parent 684e9fb8b9
commit b53b861342
4 changed files with 67 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
guilib/src/DatabaseViewer.cpp
View File

@@ -2266,11 +2266,25 @@ void DatabaseViewer::update(int value,
{
if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
{
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModel().localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
cloud,
float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
ui_->checkBox_mesh_quad->isChecked(),
ui_->spinBox_mesh_triangleSize->value());
ui_->spinBox_mesh_triangleSize->value(),
viewpoint);
view3D->removeCloud("0");
view3D->addCloudMesh("0", cloud, polygons);
}

20
guilib/src/ExportCloudsDialog.cpp
View File

@@ -698,11 +698,29 @@ bool ExportCloudsDialog::getExportedClouds(
UASSERT(iter->second->isOrganized());
if(iter->second->size())
{
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModel().localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z();
}
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
iter->second,
_ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0,
_ui->checkBox_mesh_quad->isEnabled() && _ui->checkBox_mesh_quad->isChecked(),
_ui->spinBox_mesh_triangleSize->value());
_ui->spinBox_mesh_triangleSize->value(),
viewpoint);
_progressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(polygons.size()).arg(++i).arg(clouds.size()));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);

37
guilib/src/MainWindow.cpp
View File

@@ -874,12 +874,25 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
output = util3d::extractIndices(cloud, indices, false, true);
// Fast organized mesh
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
{
viewpoint[0] = odom.data().cameraModels()[0].localTransform().x();
viewpoint[1] = odom.data().cameraModels()[0].localTransform().y();
viewpoint[2] = odom.data().cameraModels()[0].localTransform().z();
}
else if(!odom.data().stereoCameraModel().localTransform().isNull())
{
viewpoint[0] = odom.data().stereoCameraModel().localTransform().x();
viewpoint[1] = odom.data().stereoCameraModel().localTransform().y();
viewpoint[2] = odom.data().stereoCameraModel().localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
output,
_preferencesDialog->getCloudMeshingAngle(),
_preferencesDialog->isCloudMeshingQuad(),
_preferencesDialog->getCloudMeshingTriangleSize(),
Eigen::Vector3f(pose.x(), pose.y(), pose.z()));
Eigen::Vector3f(pose.x(), pose.y(), pose.z()) + viewpoint);
if(polygons.size())
{
if(!_ui->widget_cloudViewer->addCloudMesh("cloudOdom", output, polygons, _odometryCorrection))
@@ -2208,9 +2221,9 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first, t);
UWARN("saved new.pcd and old.pcd");
pcl::io::savePCDFile("new.pcd", *cloud, *indices);
pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
//UWARN("saved new.pcd and old.pcd");
//pcl::io::savePCDFile("new.pcd", *cloud, *indices);
//pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
indices = rtabmap::util3d::subtractFiltering(
cloud,
@@ -2243,11 +2256,25 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output;
// we need to extract indices as pcl::OrganizedFastMesh doesn't take indices
output = util3d::extractIndices(cloud, indices, false, true);
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModel().localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
output,
_preferencesDialog->getCloudMeshingAngle(),
_preferencesDialog->isCloudMeshingQuad(),
_preferencesDialog->getCloudMeshingTriangleSize());
_preferencesDialog->getCloudMeshingTriangleSize(),
viewpoint);
if(polygons.size())
{
// remove unused vertices to save memory

2
guilib/src/PreferencesDialog.cpp
View File

@@ -3538,7 +3538,7 @@ bool PreferencesDialog::isCloudMeshing() const
}
double PreferencesDialog::getCloudMeshingAngle() const
{
return _ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0f;
return _ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0;
}
bool PreferencesDialog::isCloudMeshingQuad() const
{