CloudViewerInteractorStyle.cpp
Go to the documentation of this file.
1 /*
2  * CloudViewerInteractorStyl.cpp
3  *
4  * Created on: Aug 21, 2018
5  * Author: mathieu
6  */
7 
8 #include "rtabmap/gui/CloudViewerInteractorStyle.h"
9 #include "rtabmap/gui/CloudViewer.h"
10 #include "rtabmap/gui/CloudViewerCellPicker.h"
11 
12 #include "rtabmap/utilite/ULogger.h"
13 #include "rtabmap/utilite/UConversion.h"
14 #include "rtabmap/utilite/UMath.h"
15 
16 #include <vtkRenderer.h>
17 #include <vtkRenderWindow.h>
18 #include <vtkObjectFactory.h>
19 #include <vtkOBBTree.h>
20 #include <vtkCamera.h>
21 
22 namespace rtabmap {
23 
24 // Standard VTK macro for *New ()
25 vtkStandardNewMacro (CloudViewerInteractorStyle);
26 
27 CloudViewerInteractorStyle::CloudViewerInteractorStyle() :
28  pcl::visualization::PCLVisualizerInteractorStyle(),
29  viewer_(0),
30  NumberOfClicks(0),
31  ResetPixelDistance(0),
32  pointsHolder_(new pcl::PointCloud<pcl::PointXYZRGB>),
33  orthoMode_(false)
34 {
35  PreviousPosition[0] = PreviousPosition[1] = 0;
36  PreviousMeasure[0] = PreviousMeasure[1] = PreviousMeasure[2] = 0.0f;
37 
38  this->MotionFactor = 5;
39 }
40 
41 void CloudViewerInteractorStyle::Rotate()
42 {
43  if (this->CurrentRenderer == NULL)
44  {
45  return;
46  }
47 
48  vtkRenderWindowInteractor *rwi = this->Interactor;
49 
50  int dx = rwi->GetEventPosition()[0] - rwi->GetLastEventPosition()[0];
51  int dy = orthoMode_?0:rwi->GetEventPosition()[1] - rwi->GetLastEventPosition()[1];
52 
53  int *size = this->CurrentRenderer->GetRenderWindow()->GetSize();
54 
55  double delta_elevation = -20.0 / size[1];
56  double delta_azimuth = -20.0 / size[0];
57 
58  double rxf = dx * delta_azimuth * this->MotionFactor;
59  double ryf = dy * delta_elevation * this->MotionFactor;
60 
61  vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
62  UASSERT(camera);
63  if(!orthoMode_)
64  {
65  camera->Azimuth(rxf);
66  camera->Elevation(ryf);
67  camera->OrthogonalizeViewUp();
68  }
69  else
70  {
71  camera->Roll(-rxf);
72  }
73 
74  if (this->AutoAdjustCameraClippingRange)
75  {
76  this->CurrentRenderer->ResetCameraClippingRange();
77  }
78 
79  if (rwi->GetLightFollowCamera())
80  {
81  this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
82  }
83 
84  //rwi->Render();
85 }
86 
87 void CloudViewerInteractorStyle::setOrthoMode(bool enabled)
88 {
89  if (this->CurrentRenderer == NULL)
90  {
91  return;
92  }
93 
94  vtkCamera *camera = CurrentRenderer->GetActiveCamera ();
95  UASSERT(camera);
96  camera->SetParallelProjection (enabled);
97  if(enabled)
98  {
99  double x,y,z;
100  camera->GetFocalPoint(x, y, z);
101  camera->SetPosition(x, y, z+(camera->GetDistance()<=5?5:camera->GetDistance()));
102  camera->SetViewUp(1, 0, 0);
103  }
104  CurrentRenderer->SetActiveCamera (camera);
105  orthoMode_ = enabled;
106 }
107 
108 void CloudViewerInteractorStyle::OnMouseMove()
109 {
110  if(this->CurrentRenderer &&
111  this->CurrentRenderer->GetLayer() == 1 &&
112  this->GetInteractor()->GetShiftKey() && this->GetInteractor()->GetControlKey() &&
113  viewer_ &&
114  viewer_->getLocators().size())
115  {
116  CloudViewerCellPicker * cellPicker = dynamic_cast<CloudViewerCellPicker*>(this->Interactor->GetPicker());
117  if(cellPicker)
118  {
119  int pickPosition[2];
120  this->GetInteractor()->GetEventPosition(pickPosition);
121  this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
122  0, // always zero.
123  this->CurrentRenderer);
124  double picked[3];
125  this->Interactor->GetPicker()->GetPickPosition(picked);
126 
127  UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
128 
129  float textSize = 0.05;
130 
131  viewer_->removeCloud("interactor_points_alt");
132  pointsHolder_->resize(2);
133  pcl::PointXYZRGB pt(255,0,0);
134  pt.x = picked[0];
135  pt.y = picked[1];
136  pt.z = picked[2];
137  pointsHolder_->at(0) = pt;
138 
139  viewer_->removeLine("interactor_ray_alt");
140  viewer_->removeText("interactor_ray_text_alt");
141 
142  // Intersect the locator with the line
143  double length = 5.0;
144  double pickedNormal[3];
145  cellPicker->GetPickNormal(pickedNormal);
146  double lineP0[3] = {picked[0], picked[1], picked[2]};
147  double lineP1[3] = {picked[0]+pickedNormal[0]*length, picked[1]+pickedNormal[1]*length, picked[2]+pickedNormal[2]*length};
148  vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
149 
150  viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
151 
152  // Display list of intersections
153  double intersection[3];
154  double previous[3] = {picked[0], picked[1], picked[2]};
155  for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
156  {
157  intersectPoints->GetPoint(i, intersection);
158 
159  Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
160  float n = v.norm();
161  if(n > 0.01f)
162  {
163  v/=n;
164  v *= n/2.0f;
165  pt.r = 125;
166  pt.g = 125;
167  pt.b = 125;
168  pt.x = intersection[0];
169  pt.y = intersection[1];
170  pt.z = intersection[2];
171  pointsHolder_->at(1) = pt;
172  viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
173  Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
174  textSize,
175  Qt::gray);
176  viewer_->addOrUpdateLine("interactor_ray_alt",
177  Transform(previous[0], previous[1], previous[2], 0, 0, 0),
178  Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
179  Qt::gray);
180 
181  previous[0] = intersection[0];
182  previous[1] = intersection[1];
183  previous[2] = intersection[2];
184  break;
185  }
186  }
187  viewer_->addCloud("interactor_points_alt", pointsHolder_);
188  viewer_->setCloudPointSize("interactor_points_alt", 15);
189  viewer_->setCloudOpacity("interactor_points_alt", 0.5);
190  }
191  }
192  // Forward events
193  PCLVisualizerInteractorStyle::OnMouseMove();
194 }
195 
196 void CloudViewerInteractorStyle::OnLeftButtonDown()
197 {
198  // http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
199  // http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
200  if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
201  {
202  this->NumberOfClicks++;
203  int pickPosition[2];
204  this->GetInteractor()->GetEventPosition(pickPosition);
205  int xdist = pickPosition[0] - this->PreviousPosition[0];
206  int ydist = pickPosition[1] - this->PreviousPosition[1];
207 
208  this->PreviousPosition[0] = pickPosition[0];
209  this->PreviousPosition[1] = pickPosition[1];
210 
211  int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
212 
213  // Reset numClicks - If mouse moved further than resetPixelDistance
214  if(moveDistance > this->ResetPixelDistance)
215  {
216  this->NumberOfClicks = 1;
217  }
218 
219  if(this->NumberOfClicks >= 2)
220  {
221  this->NumberOfClicks = 0;
222  this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
223  0, // always zero.
224  this->CurrentRenderer);
225  double picked[3];
226  this->Interactor->GetPicker()->GetPickPosition(picked);
227  UDEBUG("Double clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
228  if(this->GetInteractor()->GetControlKey()==0)
229  {
230  vtkCamera *camera = this->CurrentRenderer->GetActiveCamera();
231  UASSERT(camera);
232  double position[3];
233  double focal[3];
234  camera->GetPosition(position[0], position[1], position[2]);
235  camera->GetFocalPoint(focal[0], focal[1], focal[2]);
236  //camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
237  camera->SetFocalPoint (picked[0], picked[1], picked[2]);
238  camera->OrthogonalizeViewUp();
239 
240  if (this->AutoAdjustCameraClippingRange)
241  {
242  this->CurrentRenderer->ResetCameraClippingRange();
243  }
244 
245  if (this->Interactor->GetLightFollowCamera())
246  {
247  this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
248  }
249  }
250  else if(viewer_)
251  {
252  viewer_->removeText("interactor_pose");
253  viewer_->removeLine("interactor_line");
254  viewer_->removeCloud("interactor_points");
255  viewer_->removeLine("interactor_ray");
256  viewer_->removeText("interactor_ray_text");
257  viewer_->removeCloud("interactor_points_alt");
258  viewer_->removeLine("interactor_ray_alt");
259  viewer_->removeText("interactor_ray_text_alt");
260  PreviousMeasure[0] = 0.0f;
261  PreviousMeasure[1] = 0.0f;
262  PreviousMeasure[2] = 0.0f;
263  }
264  }
265  else if(this->GetInteractor()->GetControlKey() && viewer_)
266  {
267  this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
268  0, // always zero.
269  this->CurrentRenderer);
270  double picked[3];
271  this->Interactor->GetPicker()->GetPickPosition(picked);
272 
273  UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
274 
275  float textSize = 0.05;
276 
277  viewer_->removeCloud("interactor_points");
278  pointsHolder_->clear();
279  pcl::PointXYZRGB pt(255,0,0);
280  pt.x = picked[0];
281  pt.y = picked[1];
282  pt.z = picked[2];
283  pointsHolder_->push_back(pt);
284 
285  viewer_->removeLine("interactor_ray");
286  viewer_->removeText("interactor_ray_text");
287 
288  if( PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
289  viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
290  {
291  viewer_->addOrUpdateLine("interactor_line",
292  Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
293  Transform(picked[0], picked[1], picked[2], 0, 0, 0),
294  Qt::red);
295  pt.x = PreviousMeasure[0];
296  pt.y = PreviousMeasure[1];
297  pt.z = PreviousMeasure[2];
298  pointsHolder_->push_back(pt);
299 
300  Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
301  float n = v.norm();
302  v/=n;
303  v *= n/2.0f;
304  viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
305  Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
306  textSize,
307  Qt::red);
308  }
309  else
310  {
311  viewer_->removeText("interactor_pose");
312  viewer_->removeLine("interactor_line");
313  }
314  PreviousMeasure[0] = picked[0];
315  PreviousMeasure[1] = picked[1];
316  PreviousMeasure[2] = picked[2];
317 
318  viewer_->addCloud("interactor_points", pointsHolder_);
319  viewer_->setCloudPointSize("interactor_points", 15);
320  viewer_->setCloudOpacity("interactor_points", 0.5);
321  }
322  }
323 
324  // Forward events
325  PCLVisualizerInteractorStyle::OnLeftButtonDown();
326 }
327 
328 } /* namespace rtabmap */
NULL
#define NULL
rtabmap::CloudViewerInteractorStyle::pointsHolder_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr pointsHolder_
Definition: CloudViewerInteractorStyle.h:46
rtabmap::vtkStandardNewMacro
vtkStandardNewMacro(CloudViewerCellPicker)
f
f
rtabmap::CloudViewer::addOrUpdateText
void addOrUpdateText(const std::string &id, const std::string &text, const Transform &position, double scale, const QColor &color, bool foreground=true)
Definition: CloudViewer.cpp:1886
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
pcl
Definition: CameraRGBD.h:59
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2082
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
rtabmap::CloudViewer::removeLine
void removeLine(const std::string &id)
Definition: CloudViewer.cpp:1382
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor())
Definition: CloudViewer.cpp:455
rtabmap::CloudViewer::addOrUpdateLine
void addOrUpdateLine(const std::string &id, const Transform &from, const Transform &to, const QColor &color, bool arrow=false, bool foreground=false)
Definition: CloudViewer.cpp:1341
UASSERT
#define UASSERT(condition)
rtabmap::CloudViewer::removeText
void removeText(const std::string &id)
Definition: CloudViewer.cpp:1917
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:2592
false
#define false
Definition: ConvertUTF.c:56
UDEBUG
#define UDEBUG(...)
rtabmap::CloudViewer::getAddedLines
const std::set< std::string > & getAddedLines() const
Definition: CloudViewer.h:212
ULogger.h
ULogger class and convenient macros.
rtabmap::CloudViewer::getLocators
const std::map< std::string, vtkSmartPointer< vtkOBBTree > > & getLocators() const
Definition: CloudViewer.h:356
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:2582

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:30