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

CloudViewer: refactoring + added CloudViewerCellPicker class to ignore picking backfaces (when backface culling is on)

Browse Source
This commit is contained in:
matlabbe
2018-08-21 16:11:05 -04:00
parent 0c790005b2
commit 63af05ef88
7 changed files with 777 additions and 305 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
guilib/src/CMakeLists.txt
View File

@@ -84,6 +84,8 @@ SET(SRC_FILES
./DatabaseViewer.cpp
./utilite/UPlot.cpp
./CloudViewer.cpp
./CloudViewerCellPicker.cpp
./CloudViewerInteractorStyle.cpp
./OdometryViewer.cpp
./LoopClosureViewer.cpp
./DataRecorder.cpp

278
guilib/src/CloudViewer.cpp
View File

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/gui/CloudViewer.h"
#include "rtabmap/gui/CloudViewerCellPicker.h"
#include <rtabmap/core/Version.h>
#include <rtabmap/core/util3d_transforms.h>
@@ -66,7 +67,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <vtkTIFFReader.h>
#include <vtkOpenGLRenderWindow.h>
#include <vtkPointPicker.h>
#include <vtkCellPicker.h>
#include <vtkTextActor.h>
#include <vtkOBBTree.h>
#include <vtkObjectFactory.h>
@@ -78,280 +78,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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>)
{
PreviousPosition[0] = PreviousPosition[1] = 0;
PreviousMeasure[0] = PreviousMeasure[1] = PreviousMeasure[2] = 0.0f;
}
void CloudViewerInteractorStyle::Rotate()
{
if (this->CurrentRenderer == NULL)
{
return;
}
vtkRenderWindowInteractor *rwi = this->Interactor;
int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
int dy = 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);
camera->Azimuth(rxf);
camera->Elevation(ryf);
camera->OrthogonalizeViewUp();
if (this->AutoAdjustCameraClippingRange)
{
this->CurrentRenderer->ResetCameraClippingRange();
}
if (rwi->GetLightFollowCamera())
{
this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
}
//rwi->Render();
}
void CloudViewerInteractorStyle::OnMouseMove()
{
if(this->CurrentRenderer &&
this->CurrentRenderer->GetLayer() == 1 &&
this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
viewer_ &&
viewer_->getLocators().size())
{
vtkCellPicker * cellPicker = dynamic_cast<vtkCellPicker*>(this->Interactor->GetPicker());
if(cellPicker)
{
int pickPosition[2];
this->GetInteractor()->GetEventPosition(pickPosition);
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(255,0,0);
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;
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)
{
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_)
{
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(255,0,0);
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();
}
CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
QVTKWidget(parent),
_aLockCamera(0),
@@ -2240,7 +1966,7 @@ void CloudViewer::setPolygonPicking(bool enabled)
}
else
{
vtkSmartPointer<vtkCellPicker> pp = vtkSmartPointer<vtkCellPicker>::New ();
vtkSmartPointer<CloudViewerCellPicker> pp = vtkSmartPointer<CloudViewerCellPicker>::New ();
pp->SetTolerance (pp->GetTolerance());
this->GetInteractor()->SetPicker (pp);
setMouseTracking(true);

387
guilib/src/CloudViewerCellPicker.cpp Normal file
View File

@@ -0,0 +1,387 @@
/*
* CloudViewerCellPicker.cpp
*
* Created on: Aug 21, 2018
* Author: mathieu
*/
#include "rtabmap/gui/CloudViewerCellPicker.h"
#include <vtkImageData.h>
#include <vtkRenderer.h>
#include <vtkAbstractPicker.h>
#include <vtkPicker.h>
#include <vtkAbstractCellLocator.h>
#include <vtkIdList.h>
#include <vtkCellPicker.h>
#include <vtkLODProp3D.h>
#include <vtkMapper.h>
#include <vtkGenericCell.h>
#include <vtkMath.h>
#include <vtkTexture.h>
#include <vtkObjectFactory.h>
#include <vtkSmartPointer.h>
#include <vtkPoints.h>
#include <vtkProperty.h>
namespace rtabmap {
// Standard VTK macro for *New ()
vtkStandardNewMacro (CloudViewerCellPicker);
CloudViewerCellPicker::CloudViewerCellPicker()
{
cell_ = vtkGenericCell::New();
pointIds_ = vtkIdList::New();
}
CloudViewerCellPicker::~CloudViewerCellPicker()
{
cell_->Delete();
pointIds_->Delete();
}
double CloudViewerCellPicker::IntersectActorWithLine(const double p1[3],
const double p2[3],
double t1, double t2,
double tol,
vtkProp3D *prop,
vtkMapper *mapper)
{
// This code was taken from the original CellPicker with almost no
// modification except for the locator and texture additions.
// Intersect each cell with ray. Keep track of one closest to
// the eye (within the tolerance tol) and within the clipping range).
// Note that we fudge the "closest to" (tMin+this->Tolerance) a little and
// keep track of the cell with the best pick based on parametric
// coordinate (pick the minimum, maximum parametric distance). This
// breaks ties in a reasonable way when cells are the same distance
// from the eye (like cells laying on a 2D plane).
vtkDataSet *data = mapper->GetInput();
double tMin = VTK_DOUBLE_MAX;
double minPCoords[3];
double pDistMin = VTK_DOUBLE_MAX;
vtkIdType minCellId = -1;
int minSubId = -1;
double minXYZ[3];
minXYZ[0] = minXYZ[1] = minXYZ[2] = 0.0;
double ray[3] = {p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]};
vtkMath::Normalize(ray);
vtkActor * actor = vtkActor::SafeDownCast(prop);
// Polydata has no 3D cells
int isPolyData = data->IsA("vtkPolyData");
vtkCollectionSimpleIterator iter;
vtkAbstractCellLocator *locator = 0;
this->Locators->InitTraversal(iter);
while ( (locator = static_cast<vtkAbstractCellLocator *>(
this->Locators->GetNextItemAsObject(iter))) )
{
if (locator->GetDataSet() == data)
{
break;
}
}
// Make a new p1 and p2 using the clipped t1 and t2
double q1[3], q2[3];
q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2];
q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2];
if (t1 != 0.0 || t2 != 1.0)
{
for (int j = 0; j < 3; j++)
{
q1[j] = p1[j]*(1.0 - t1) + p2[j]*t1;
q2[j] = p1[j]*(1.0 - t2) + p2[j]*t2;
}
}
// Use the locator if one exists for this data
if (locator)
{
vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkIdList> intersectCells = vtkSmartPointer<vtkIdList>::New();
locator->IntersectWithLine(q1, q2, intersectPoints, intersectCells);
for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
{
double intersection[3];
intersectPoints->GetPoint(i, intersection);
}
if (!locator->IntersectWithLine(q1, q2, tol, tMin, minXYZ,
minPCoords, minSubId, minCellId,
this->cell_))
{
return VTK_DOUBLE_MAX;
}
// Stretch tMin out to the original range
if (t1 != 0.0 || t2 != 1.0)
{
tMin = t1*(1.0 - tMin) + t2*tMin;
}
// If cell is a strip, then replace cell with a sub-cell
this->SubCellFromCell(this->cell_, minSubId);
}
else
{
vtkIdList *pointIds = this->pointIds_;
vtkIdType numCells = data->GetNumberOfCells();
for (vtkIdType cellId = 0; cellId < numCells; cellId++)
{
double t;
double x[3];
double pcoords[3];
pcoords[0] = pcoords[1] = pcoords[2] = 0;
int newSubId = -1;
int numSubIds = 1;
// If it is a strip, we need to iterate over the subIds
int cellType = data->GetCellType(cellId);
int useSubCells = this->HasSubCells(cellType);
if (useSubCells)
{
// Get the pointIds for the strip and the length of the strip
data->GetCellPoints(cellId, pointIds);
numSubIds = this->GetNumberOfSubCells(pointIds, cellType);
}
// This will only loop once unless we need to deal with a strip
for (int subId = 0; subId < numSubIds; subId++)
{
if (useSubCells)
{
// Get a sub-cell from a the strip
this->GetSubCell(data, pointIds, subId, cellType, this->cell_);
}
else
{
data->GetCell(cellId, this->cell_);
}
int cellPicked = 0;
if (isPolyData)
{
// Polydata can always be picked with original endpoints
cellPicked = this->cell_->IntersectWithLine(
const_cast<double *>(p1), const_cast<double *>(p2),
tol, t, x, pcoords, newSubId);
}
else
{
// Any 3D cells need to be intersected with a line segment that
// has been clipped with the clipping planes, in case one end is
// actually inside the cell.
cellPicked = this->cell_->IntersectWithLine(
q1, q2, tol, t, x, pcoords, newSubId);
// Stretch t out to the original range
if (t1 != 0.0 || t2 != 1.0)
{
t = t1*(1.0 - t) + t2*t;
}
}
if (cellPicked && t <= (tMin + this->Tolerance) && t >= t1 && t <= t2)
{
double pDist = this->cell_->GetParametricDistance(pcoords);
if (pDist < pDistMin || (pDist == pDistMin && t < tMin))
{
////////////////////////////////////////////////////////////////////////////////////
// BEGIN: Modifications from VTK 6.2
////////////////////////////////////////////////////////////////////////////////////
bool visible = true;
if(actor->GetProperty()->GetBackfaceCulling() ||
actor->GetProperty()->GetFrontfaceCulling())
{
// Get the cell weights
vtkIdType numPoints = this->cell_->GetNumberOfPoints();
double *weights = new double[numPoints];
for (vtkIdType i = 0; i < numPoints; i++)
{
weights[i] = 0;
}
// Get the interpolation weights (point is thrown away)
double point[3] = {0.0,0.0,0.0};
this->cell_->EvaluateLocation(minSubId, minPCoords, point, weights);
double normal[3] = {0.0,0.0,0.0};
if (this->ComputeSurfaceNormal(data, this->cell_, weights, normal))
{
if(actor->GetProperty()->GetBackfaceCulling())
{
visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] <= 0;
}
else
{
visible = ray[0]*normal[0] + ray[1]*normal[1] + ray[2]*normal[2] >= 0;
}
}
delete [] weights;
}
if(visible)
{
tMin = t;
pDistMin = pDist;
// save all of these
minCellId = cellId;
minSubId = newSubId;
if (useSubCells)
{
minSubId = subId;
}
for (int k = 0; k < 3; k++)
{
minXYZ[k] = x[k];
minPCoords[k] = pcoords[k];
}
}
////////////////////////////////////////////////////////////////////////////////////
// END: Modifications from VTK 6.2
////////////////////////////////////////////////////////////////////////////////////
} // for all subIds
} // if minimum, maximum
} // if a close cell
} // for all cells
}
// Do this if a cell was intersected
if (minCellId >= 0 && tMin < this->GlobalTMin)
{
this->ResetPickInfo();
// Get the cell, convert to triangle if it is a strip
vtkGenericCell *cell = this->cell_;
// If we used a locator, we already have the picked cell
if (!locator)
{
int cellType = data->GetCellType(minCellId);
if (this->HasSubCells(cellType))
{
data->GetCellPoints(minCellId, this->pointIds_);
this->GetSubCell(data, this->pointIds_, minSubId, cellType, cell);
}
else
{
data->GetCell(minCellId, cell);
}
}
// Get the cell weights
vtkIdType numPoints = cell->GetNumberOfPoints();
double *weights = new double[numPoints];
for (vtkIdType i = 0; i < numPoints; i++)
{
weights[i] = 0;
}
// Get the interpolation weights (point is thrown away)
double point[3];
cell->EvaluateLocation(minSubId, minPCoords, point, weights);
this->Mapper = mapper;
// Get the texture from the actor or the LOD
vtkActor *actor = 0;
vtkLODProp3D *lodActor = 0;
if ( (actor = vtkActor::SafeDownCast(prop)) )
{
this->Texture = actor->GetTexture();
}
else if ( (lodActor = vtkLODProp3D::SafeDownCast(prop)) )
{
int lodId = lodActor->GetPickLODID();
lodActor->GetLODTexture(lodId, &this->Texture);
}
if (this->PickTextureData && this->Texture)
{
// Return the texture's image data to the user
vtkImageData *image = this->Texture->GetInput();
this->DataSet = image;
// Get and check the image dimensions
int extent[6];
image->GetExtent(extent);
int dimensionsAreValid = 1;
int dimensions[3];
for (int i = 0; i < 3; i++)
{
dimensions[i] = extent[2*i + 1] - extent[2*i] + 1;
dimensionsAreValid = (dimensionsAreValid && dimensions[i] > 0);
}
// Use the texture coord to set the information
double tcoord[3];
if (dimensionsAreValid &&
this->ComputeSurfaceTCoord(data, cell, weights, tcoord))
{
// Take the border into account when computing coordinates
double x[3];
x[0] = extent[0] + tcoord[0]*dimensions[0] - 0.5;
x[1] = extent[2] + tcoord[1]*dimensions[1] - 0.5;
x[2] = extent[4] + tcoord[2]*dimensions[2] - 0.5;
this->SetImageDataPickInfo(x, extent);
}
}
else
{
// Return the polydata to the user
this->DataSet = data;
this->CellId = minCellId;
this->SubId = minSubId;
this->PCoords[0] = minPCoords[0];
this->PCoords[1] = minPCoords[1];
this->PCoords[2] = minPCoords[2];
// Find the point with the maximum weight
double maxWeight = 0;
vtkIdType iMaxWeight = -1;
for (vtkIdType i = 0; i < numPoints; i++)
{
if (weights[i] > maxWeight)
{
iMaxWeight = i;
}
}
// If maximum weight is found, use it to get the PointId
if (iMaxWeight != -1)
{
this->PointId = cell->PointIds->GetId(iMaxWeight);
}
}
// Set the mapper position
this->MapperPosition[0] = minXYZ[0];
this->MapperPosition[1] = minXYZ[1];
this->MapperPosition[2] = minXYZ[2];
// Compute the normal
if (!this->ComputeSurfaceNormal(data, cell, weights, this->MapperNormal))
{
// By default, the normal points back along view ray
this->MapperNormal[0] = p1[0] - p2[0];
this->MapperNormal[1] = p1[1] - p2[1];
this->MapperNormal[2] = p1[2] - p2[2];
vtkMath::Normalize(this->MapperNormal);
}
delete [] weights;
}
return tMin;
}
} /* namespace rtabmap */

296
guilib/src/CloudViewerInteractorStyle.cpp Normal file
View File

@@ -0,0 +1,296 @@
/*
* 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 <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>)
{
PreviousPosition[0] = PreviousPosition[1] = 0;
PreviousMeasure[0] = PreviousMeasure[1] = PreviousMeasure[2] = 0.0f;
}
void CloudViewerInteractorStyle::Rotate()
{
if (this->CurrentRenderer == NULL)
{
return;
}
vtkRenderWindowInteractor *rwi = this->Interactor;
int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
int dy = 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);
camera->Azimuth(rxf);
camera->Elevation(ryf);
camera->OrthogonalizeViewUp();
if (this->AutoAdjustCameraClippingRange)
{
this->CurrentRenderer->ResetCameraClippingRange();
}
if (rwi->GetLightFollowCamera())
{
this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
}
//rwi->Render();
}
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);
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(255,0,0);
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;
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)
{
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_)
{
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(255,0,0);
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 */