16 #include <vtkRenderer.h> 17 #include <vtkRenderWindow.h> 18 #include <vtkObjectFactory.h> 19 #include <vtkOBBTree.h> 20 #include <vtkCamera.h> 28 pcl::visualization::PCLVisualizerInteractorStyle(),
31 ResetPixelDistance(0),
32 pointsHolder_(new
pcl::PointCloud<
pcl::PointXYZRGB>),
38 this->MotionFactor = 5;
43 if (this->CurrentRenderer ==
NULL)
48 vtkRenderWindowInteractor *rwi = this->Interactor;
50 int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
51 int dy =
orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
53 int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
55 double delta_elevation = -20.0 / size[1];
56 double delta_azimuth = -20.0 / size[0];
58 double rxf = dx * delta_azimuth * this->MotionFactor;
59 double ryf = dy * delta_elevation * this->MotionFactor;
61 vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
66 camera->Elevation(ryf);
67 camera->OrthogonalizeViewUp();
74 if (this->AutoAdjustCameraClippingRange)
76 this->CurrentRenderer->ResetCameraClippingRange();
79 if (rwi->GetLightFollowCamera())
81 this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
89 if (this->CurrentRenderer ==
NULL)
94 vtkCamera *camera = CurrentRenderer->GetActiveCamera ();
96 camera->SetParallelProjection (enabled);
100 camera->GetFocalPoint(x, y, z);
101 camera->SetPosition(x, y, z+(camera->GetDistance()<=5?5:camera->GetDistance()));
102 camera->SetViewUp(1, 0, 0);
104 CurrentRenderer->SetActiveCamera (camera);
110 if(this->CurrentRenderer &&
111 this->CurrentRenderer->GetLayer() == 1 &&
112 this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
120 this->GetInteractor()->GetEventPosition(pickPosition);
121 int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
123 this->CurrentRenderer);
127 this->Interactor->GetPicker()->GetPickPosition(picked);
129 UDEBUG(
"Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
131 float textSize = 0.05;
147 double pickedNormal[3];
148 cellPicker->GetPickNormal(pickedNormal);
149 double lineP0[3] = {picked[0], picked[1], picked[2]};
150 double lineP1[3] = {picked[0]+pickedNormal[0]*
length, picked[1]+pickedNormal[1]*
length, picked[2]+pickedNormal[2]*length};
157 double previous[3] = {picked[0], picked[1], picked[2]};
158 for(
int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
160 intersectPoints->GetPoint(i, intersection);
162 Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
171 pt.x = intersection[0];
172 pt.y = intersection[1];
173 pt.z = intersection[2];
176 Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
180 Transform(previous[0], previous[1], previous[2], 0, 0, 0),
181 Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
184 previous[0] = intersection[0];
185 previous[1] = intersection[1];
186 previous[2] = intersection[2];
197 PCLVisualizerInteractorStyle::OnMouseMove();
204 if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
208 this->GetInteractor()->GetEventPosition(pickPosition);
210 int ydist = pickPosition[1] - this->PreviousPosition[1];
212 this->PreviousPosition[0] = pickPosition[0];
213 this->PreviousPosition[1] = pickPosition[1];
215 int moveDistance = (int)
sqrt((
double)(xdist*xdist + ydist*ydist));
226 int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
228 this->CurrentRenderer);
229 if(result && this->GetInteractor()->GetControlKey()==0)
232 this->Interactor->GetPicker()->GetPickPosition(picked);
233 UDEBUG(
"Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
234 vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
238 camera->GetPosition(position[0], position[1], position[2]);
239 camera->GetFocalPoint(focal[0], focal[1], focal[2]);
241 camera->SetFocalPoint (picked[0], picked[1], picked[2]);
242 camera->OrthogonalizeViewUp();
244 if (this->AutoAdjustCameraClippingRange)
246 this->CurrentRenderer->ResetCameraClippingRange();
249 if (this->Interactor->GetLightFollowCamera())
251 this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
269 else if(this->GetInteractor()->GetControlKey() &&
viewer_)
271 int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
273 this->CurrentRenderer);
277 this->Interactor->GetPicker()->GetPickPosition(picked);
279 UDEBUG(
"Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
281 float textSize = 0.05;
300 Transform(picked[0], picked[1], picked[2], 0, 0, 0),
333 PCLVisualizerInteractorStyle::OnLeftButtonDown();
pcl::PointCloud< pcl::PointXYZRGB >::Ptr pointsHolder_
vtkStandardNewMacro(CloudViewerCellPicker)
void addOrUpdateText(const std::string &id, const std::string &text, const Transform &position, double scale, const QColor &color, bool foreground=true)
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
bool removeCloud(const std::string &id)
Basic mathematics functions.
Some conversion functions.
void removeLine(const std::string &id)
void addOrUpdateLine(const std::string &id, const Transform &from, const Transform &to, const QColor &color, bool arrow=false, bool foreground=false)
#define UASSERT(condition)
void removeText(const std::string &id)
void setCloudPointSize(const std::string &id, int size)
CloudViewerInteractorStyle()
virtual void OnLeftButtonDown()
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
unsigned int NumberOfClicks
void setOrthoMode(bool enabled)
const std::set< std::string > & getAddedLines() const
ULogger class and convenient macros.
virtual void OnMouseMove()
const std::map< std::string, vtkSmartPointer< vtkOBBTree > > & getLocators() const
GLM_FUNC_DECL genType::value_type length(genType const &x)
std::string UTILITE_EXP uFormat(const char *fmt,...)
void setCloudOpacity(const std::string &id, double opacity=1.0)