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

CloudViewer: fixed camera clipping the grid

Browse Source
This commit is contained in:
matlabbe
2020-06-03 13:41:36 -04:00
parent ff3c6c8e06
commit b6d4c6f024
2 changed files with 150 additions and 139 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
guilib/src/CloudViewer.cpp
View File

@@ -138,7 +138,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
int argc = 0;
UASSERT(style!=0);
style->setCloudViewer(this);
style->SetAutoAdjustCameraClippingRange(true);
style->AutoAdjustCameraClippingRangeOff();
_visualizer = new pcl::visualization::PCLVisualizer(
argc,
0,
@@ -195,7 +195,7 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
_visualizer->setCameraPosition(
-1, 0, 0,
0, 0, 0,
0, 0, 1, 1);
0, 0, 1, 0);
#ifndef _WIN32
// Crash on startup on Windows (vtk issue)
this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
@@ -286,14 +286,12 @@ void CloudViewer::createMenu()
_aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
_aSetEDLShading->setCheckable(true);
_aSetEDLShading->setChecked(false);
#if VTK_MAJOR_VERSION < 7
_aSetEDLShading->setEnabled(false);
#endif
_aSetLighting = new QAction("Lighting", this);
_aSetLighting->setCheckable(true);
_aSetLighting->setChecked(false);
#if VTK_MAJOR_VERSION < 7
_aSetLighting->setEnabled(false);
#endif
_aSetFlatShading = new QAction("Flat Shading", this);
_aSetFlatShading->setCheckable(true);
_aSetFlatShading->setChecked(false);
@@ -2254,21 +2252,21 @@ void CloudViewer::resetCamera()
_visualizer->setCameraPosition(
_lastPose.x(), _lastPose.y(), _lastPose.z()+5,
_lastPose.x(), _lastPose.y(), _lastPose.z(),
1, 0, 0, 1);
1, 0, 0, 0);
}
else if(_aLockViewZ->isChecked())
{
_visualizer->setCameraPosition(
pt.x, pt.y, pt.z,
_lastPose.x(), _lastPose.y(), _lastPose.z(),
0, 0, 1, 1);
0, 0, 1, 0);
}
else
{
_visualizer->setCameraPosition(
pt.x, pt.y, pt.z,
_lastPose.x(), _lastPose.y(), _lastPose.z(),
_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 1);
_lastPose.r31(), _lastPose.r32(), _lastPose.r33(), 0);
}
}
else if(_aCameraOrtho->isChecked())
@@ -2276,14 +2274,14 @@ void CloudViewer::resetCamera()
_visualizer->setCameraPosition(
0, 0, 5,
0, 0, 0,
1, 0, 0, 1);
1, 0, 0, 0);
}
else
{
_visualizer->setCameraPosition(
-1, 0, 0,
0, 0, 0,
0, 0, 1, 1);
0, 0, 1, 0);
}
this->update();
}
@@ -2593,7 +2591,7 @@ void CloudViewer::setCameraPosition(
float upX, float upY, float upZ)
{
_lastCameraOrientation= _lastCameraPose= cv::Vec3f(0,0,0);
_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ, 1);
_visualizer->setCameraPosition(x,y,z, focalX,focalY,focalX, upX,upY,upZ, 0);
}
void CloudViewer::updateCameraTargetPosition(const Transform & pose)
@@ -2713,7 +2711,7 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose)
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 0);
}
}
@@ -3252,7 +3250,7 @@ void CloudViewer::keyPressEvent(QKeyEvent * event)
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 0);
update();
@@ -3280,12 +3278,12 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
{
QVTKWidget::mouseMoveEvent(event);
std::vector<pcl::visualization::Camera> cameras;
_visualizer->getCameras(cameras);
// camera view up z locked?
if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
{
std::vector<pcl::visualization::Camera> cameras;
_visualizer->getCameras(cameras);
cv::Vec3d newCameraOrientation = cv::Vec3d(0,0,1).cross(cv::Vec3d(cameras.front().pos)-cv::Vec3d(cameras.front().focal));
if( _lastCameraOrientation!=cv::Vec3d(0,0,0) &&
@@ -3317,13 +3315,13 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
cameras.front().view[0] = 0;
cameras.front().view[1] = 0;
cameras.front().view[2] = 1;
}
_visualizer->setCameraPosition(
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 1);
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 0);
}
this->update();
Q_EMIT configChanged();
@@ -3332,12 +3330,19 @@ void CloudViewer::mouseMoveEvent(QMouseEvent * event)
void CloudViewer::wheelEvent(QWheelEvent * event)
{
QVTKWidget::wheelEvent(event);
std::vector<pcl::visualization::Camera> cameras;
_visualizer->getCameras(cameras);
if(_aLockViewZ->isChecked() && !_aCameraOrtho->isChecked())
{
std::vector<pcl::visualization::Camera> cameras;
_visualizer->getCameras(cameras);
_lastCameraPose = cv::Vec3d(cameras.front().pos);
}
_visualizer->setCameraPosition(
cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2],
cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2],
cameras.front().view[0], cameras.front().view[1], cameras.front().view[2], 0);
Q_EMIT configChanged();
}

238
guilib/src/CloudViewerInteractorStyle.cpp
View File

@@ -118,76 +118,79 @@ void CloudViewerInteractorStyle::OnMouseMove()
{
int pickPosition[2];
this->GetInteractor()->GetEventPosition(pickPosition);
this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
0, // always zero.
this->CurrentRenderer);
double picked[3];
this->Interactor->GetPicker()->GetPickPosition(picked);
UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
float textSize = 0.05;
viewer_->removeCloud("interactor_points_alt");
pointsHolder_->resize(2);
pcl::PointXYZRGB pt;
pt.r = 255;
pt.x = picked[0];
pt.y = picked[1];
pt.z = picked[2];
pointsHolder_->at(0) = pt;
viewer_->removeLine("interactor_ray_alt");
viewer_->removeText("interactor_ray_text_alt");
// Intersect the locator with the line
double length = 5.0;
double pickedNormal[3];
cellPicker->GetPickNormal(pickedNormal);
double lineP0[3] = {picked[0], picked[1], picked[2]};
double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
// Display list of intersections
double intersection[3];
double previous[3] = {picked[0], picked[1], picked[2]};
for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
if(result)
{
intersectPoints->GetPoint(i, intersection);
double picked[3];
this->Interactor->GetPicker()->GetPickPosition(picked);
Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
float n = v.norm();
if(n > 0.01f)
UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
float textSize = 0.05;
viewer_->removeCloud("interactor_points_alt");
pointsHolder_->resize(2);
pcl::PointXYZRGB pt;
pt.r = 255;
pt.x = picked[0];
pt.y = picked[1];
pt.z = picked[2];
pointsHolder_->at(0) = pt;
viewer_->removeLine("interactor_ray_alt");
viewer_->removeText("interactor_ray_text_alt");
// Intersect the locator with the line
double length = 5.0;
double pickedNormal[3];
cellPicker->GetPickNormal(pickedNormal);
double lineP0[3] = {picked[0], picked[1], picked[2]};
double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
// Display list of intersections
double intersection[3];
double previous[3] = {picked[0], picked[1], picked[2]};
for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
{
v/=n;
v *= n/2.0f;
pt.r = 125;
pt.g = 125;
pt.b = 125;
pt.x = intersection[0];
pt.y = intersection[1];
pt.z = intersection[2];
pointsHolder_->at(1) = pt;
viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
textSize,
Qt::gray);
viewer_->addOrUpdateLine("interactor_ray_alt",
Transform(previous[0], previous[1], previous[2], 0, 0, 0),
Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
Qt::gray);
intersectPoints->GetPoint(i, intersection);
previous[0] = intersection[0];
previous[1] = intersection[1];
previous[2] = intersection[2];
break;
Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
float n = v.norm();
if(n > 0.01f)
{
v/=n;
v *= n/2.0f;
pt.r = 125;
pt.g = 125;
pt.b = 125;
pt.x = intersection[0];
pt.y = intersection[1];
pt.z = intersection[2];
pointsHolder_->at(1) = pt;
viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
textSize,
Qt::gray);
viewer_->addOrUpdateLine("interactor_ray_alt",
Transform(previous[0], previous[1], previous[2], 0, 0, 0),
Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
Qt::gray);
previous[0] = intersection[0];
previous[1] = intersection[1];
previous[2] = intersection[2];
break;
}
}
viewer_->addCloud("interactor_points_alt", pointsHolder_);
viewer_->setCloudPointSize("interactor_points_alt", 15);
viewer_->setCloudOpacity("interactor_points_alt", 0.5);
}
viewer_->addCloud("interactor_points_alt", pointsHolder_);
viewer_->setCloudPointSize("interactor_points_alt", 15);
viewer_->setCloudOpacity("interactor_points_alt", 0.5);
}
}
// Forward events
@@ -220,14 +223,14 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
if(this->NumberOfClicks >= 2)
{
this->NumberOfClicks = 0;
this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
0, // always zero.
this->CurrentRenderer);
double picked[3];
this->Interactor->GetPicker()->GetPickPosition(picked);
UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
if(this->GetInteractor()->GetControlKey()==0)
if(result && this->GetInteractor()->GetControlKey()==0)
{
double picked[3];
this->Interactor->GetPicker()->GetPickPosition(picked);
UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
UASSERT(camera);
double position[3];
@@ -265,61 +268,64 @@ void CloudViewerInteractorStyle::OnLeftButtonDown()
}
else if(this->GetInteractor()->GetControlKey() && viewer_)
{
this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
0, // always zero.
this->CurrentRenderer);
double picked[3];
this->Interactor->GetPicker()->GetPickPosition(picked);
UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
float textSize = 0.05;
viewer_->removeCloud("interactor_points");
pointsHolder_->clear();
pcl::PointXYZRGB pt;
pt.r = 255;
pt.x = picked[0];
pt.y = picked[1];
pt.z = picked[2];
pointsHolder_->push_back(pt);
viewer_->removeLine("interactor_ray");
viewer_->removeText("interactor_ray_text");
if( PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
if(result)
{
viewer_->addOrUpdateLine("interactor_line",
Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
Transform(picked[0], picked[1], picked[2], 0, 0, 0),
Qt::red);
pt.x = PreviousMeasure[0];
pt.y = PreviousMeasure[1];
pt.z = PreviousMeasure[2];
double picked[3];
this->Interactor->GetPicker()->GetPickPosition(picked);
UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
float textSize = 0.05;
viewer_->removeCloud("interactor_points");
pointsHolder_->clear();
pcl::PointXYZRGB pt;
pt.r = 255;
pt.x = picked[0];
pt.y = picked[1];
pt.z = picked[2];
pointsHolder_->push_back(pt);
Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
float n = v.norm();
v/=n;
v *= n/2.0f;
viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
textSize,
Qt::red);
}
else
{
viewer_->removeText("interactor_pose");
viewer_->removeLine("interactor_line");
}
PreviousMeasure[0] = picked[0];
PreviousMeasure[1] = picked[1];
PreviousMeasure[2] = picked[2];
viewer_->removeLine("interactor_ray");
viewer_->removeText("interactor_ray_text");
viewer_->addCloud("interactor_points", pointsHolder_);
viewer_->setCloudPointSize("interactor_points", 15);
viewer_->setCloudOpacity("interactor_points", 0.5);
if( PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
{
viewer_->addOrUpdateLine("interactor_line",
Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
Transform(picked[0], picked[1], picked[2], 0, 0, 0),
Qt::red);
pt.x = PreviousMeasure[0];
pt.y = PreviousMeasure[1];
pt.z = PreviousMeasure[2];
pointsHolder_->push_back(pt);
Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
float n = v.norm();
v/=n;
v *= n/2.0f;
viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
textSize,
Qt::red);
}
else
{
viewer_->removeText("interactor_pose");
viewer_->removeLine("interactor_line");
}
PreviousMeasure[0] = picked[0];
PreviousMeasure[1] = picked[1];
PreviousMeasure[2] = picked[2];
viewer_->addCloud("interactor_points", pointsHolder_);
viewer_->setCloudPointSize("interactor_points", 15);
viewer_->setCloudOpacity("interactor_points", 0.5);
}
}
}