mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
337 lines
10 KiB
C++
337 lines
10 KiB
C++
/*
|
|
* CloudViewerInteractorStyl.cpp
|
|
*
|
|
* Created on: Aug 21, 2018
|
|
* Author: mathieu
|
|
*/
|
|
|
|
#include "rtabmap/gui/CloudViewerInteractorStyle.h"
|
|
#include "rtabmap/gui/CloudViewer.h"
|
|
#include "rtabmap/gui/CloudViewerCellPicker.h"
|
|
|
|
#include "rtabmap/utilite/ULogger.h"
|
|
#include "rtabmap/utilite/UConversion.h"
|
|
#include "rtabmap/utilite/UMath.h"
|
|
|
|
#include <vtkRenderer.h>
|
|
#include <vtkRenderWindow.h>
|
|
#include <vtkObjectFactory.h>
|
|
#include <vtkOBBTree.h>
|
|
#include <vtkCamera.h>
|
|
|
|
namespace rtabmap {
|
|
|
|
// Standard VTK macro for *New ()
|
|
vtkStandardNewMacro (CloudViewerInteractorStyle);
|
|
|
|
CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
|
|
pcl::visualization::PCLVisualizerInteractorStyle(),
|
|
viewer_(0),
|
|
NumberOfClicks(0),
|
|
ResetPixelDistance(0),
|
|
pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
|
|
orthoMode_(false)
|
|
{
|
|
PreviousPosition[0] = PreviousPosition[1] = 0;
|
|
PreviousMeasure[0] = PreviousMeasure[1] = PreviousMeasure[2] = 0.0f;
|
|
|
|
this->MotionFactor = 5;
|
|
}
|
|
|
|
void CloudViewerInteractorStyle::Rotate()
|
|
{
|
|
if (this->CurrentRenderer == NULL)
|
|
{
|
|
return;
|
|
}
|
|
|
|
vtkRenderWindowInteractor *rwi = this->Interactor;
|
|
|
|
int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
|
|
int dy = orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
|
|
|
|
int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
|
|
|
|
double delta_elevation = -20.0 / size[1];
|
|
double delta_azimuth = -20.0 / size[0];
|
|
|
|
double rxf = dx * delta_azimuth * this->MotionFactor;
|
|
double ryf = dy * delta_elevation * this->MotionFactor;
|
|
|
|
vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
|
|
UASSERT(camera);
|
|
if(!orthoMode_)
|
|
{
|
|
camera->Azimuth(rxf);
|
|
camera->Elevation(ryf);
|
|
camera->OrthogonalizeViewUp();
|
|
}
|
|
else
|
|
{
|
|
camera->Roll(-rxf);
|
|
}
|
|
|
|
if (this->AutoAdjustCameraClippingRange)
|
|
{
|
|
this->CurrentRenderer->ResetCameraClippingRange();
|
|
}
|
|
|
|
if (rwi->GetLightFollowCamera())
|
|
{
|
|
this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
|
|
}
|
|
|
|
//rwi->Render();
|
|
}
|
|
|
|
void CloudViewerInteractorStyle::setOrthoMode(bool enabled)
|
|
{
|
|
if (this->CurrentRenderer == NULL)
|
|
{
|
|
return;
|
|
}
|
|
|
|
vtkCamera *camera = CurrentRenderer->GetActiveCamera ();
|
|
UASSERT(camera);
|
|
camera->SetParallelProjection (enabled);
|
|
if(enabled)
|
|
{
|
|
double x,y,z;
|
|
camera->GetFocalPoint(x, y, z);
|
|
camera->SetPosition(x, y, z+(camera->GetDistance()<=5?5:camera->GetDistance()));
|
|
camera->SetViewUp(1, 0, 0);
|
|
}
|
|
CurrentRenderer->SetActiveCamera (camera);
|
|
orthoMode_ = enabled;
|
|
}
|
|
|
|
void CloudViewerInteractorStyle::OnMouseMove()
|
|
{
|
|
if(this->CurrentRenderer &&
|
|
this->CurrentRenderer->GetLayer() == 1 &&
|
|
this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
|
|
viewer_ &&
|
|
viewer_->getLocators().size())
|
|
{
|
|
CloudViewerCellPicker * cellPicker = dynamic_cast<CloudViewerCellPicker*>(this->Interactor->GetPicker());
|
|
if(cellPicker)
|
|
{
|
|
int pickPosition[2];
|
|
this->GetInteractor()->GetEventPosition(pickPosition);
|
|
int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
|
|
0, // always zero.
|
|
this->CurrentRenderer);
|
|
if(result)
|
|
{
|
|
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++ )
|
|
{
|
|
intersectPoints->GetPoint(i, intersection);
|
|
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
// Forward events
|
|
PCLVisualizerInteractorStyle::OnMouseMove();
|
|
}
|
|
|
|
void CloudViewerInteractorStyle::OnLeftButtonDown()
|
|
{
|
|
// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
|
|
// http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
|
|
if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
|
|
{
|
|
this->NumberOfClicks++;
|
|
int pickPosition[2];
|
|
this->GetInteractor()->GetEventPosition(pickPosition);
|
|
int xdist = pickPosition[0] - this->PreviousPosition[0];
|
|
int ydist = pickPosition[1] - this->PreviousPosition[1];
|
|
|
|
this->PreviousPosition[0] = pickPosition[0];
|
|
this->PreviousPosition[1] = pickPosition[1];
|
|
|
|
int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
|
|
|
|
// Reset numClicks - If mouse moved further than resetPixelDistance
|
|
if(moveDistance > this->ResetPixelDistance)
|
|
{
|
|
this->NumberOfClicks = 1;
|
|
}
|
|
|
|
if(this->NumberOfClicks >= 2)
|
|
{
|
|
this->NumberOfClicks = 0;
|
|
int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
|
|
0, // always zero.
|
|
this->CurrentRenderer);
|
|
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];
|
|
double focal[3];
|
|
camera->GetPosition(position[0], position[1], position[2]);
|
|
camera->GetFocalPoint(focal[0], focal[1], focal[2]);
|
|
//camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
|
|
camera->SetFocalPoint (picked[0], picked[1], picked[2]);
|
|
camera->OrthogonalizeViewUp();
|
|
|
|
if (this->AutoAdjustCameraClippingRange)
|
|
{
|
|
this->CurrentRenderer->ResetCameraClippingRange();
|
|
}
|
|
|
|
if (this->Interactor->GetLightFollowCamera())
|
|
{
|
|
this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
|
|
}
|
|
}
|
|
else if(viewer_)
|
|
{
|
|
viewer_->removeText("interactor_pose");
|
|
viewer_->removeLine("interactor_line");
|
|
viewer_->removeCloud("interactor_points");
|
|
viewer_->removeLine("interactor_ray");
|
|
viewer_->removeText("interactor_ray_text");
|
|
viewer_->removeCloud("interactor_points_alt");
|
|
viewer_->removeLine("interactor_ray_alt");
|
|
viewer_->removeText("interactor_ray_text_alt");
|
|
PreviousMeasure[0] = 0.0f;
|
|
PreviousMeasure[1] = 0.0f;
|
|
PreviousMeasure[2] = 0.0f;
|
|
}
|
|
}
|
|
else if(this->GetInteractor()->GetControlKey() && viewer_)
|
|
{
|
|
int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
|
|
0, // always zero.
|
|
this->CurrentRenderer);
|
|
if(result)
|
|
{
|
|
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())
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Forward events
|
|
PCLVisualizerInteractorStyle::OnLeftButtonDown();
|
|
}
|
|
|
|
} /* namespace rtabmap */
|