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  int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
122  0, // always zero.
123  this->CurrentRenderer);
124  if(result)
125  {
126  double picked[3];
127  this->Interactor->GetPicker()->GetPickPosition(picked);
128 
129  UDEBUG("Control move! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
130 
131  float textSize = 0.05;
132 
133  viewer_->removeCloud("interactor_points_alt");
134  pointsHolder_->resize(2);
135  pcl::PointXYZRGB pt;
136  pt.r = 255;
137  pt.x = picked[0];
138  pt.y = picked[1];
139  pt.z = picked[2];
140  pointsHolder_->at(0) = pt;
141 
142  viewer_->removeLine("interactor_ray_alt");
143  viewer_->removeText("interactor_ray_text_alt");
144 
145  // Intersect the locator with the line
146  double length = 5.0;
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};
151  vtkSmartPointer<vtkPoints> intersectPoints = vtkSmartPointer<vtkPoints>::New();
152 
153  viewer_->getLocators().begin()->second->IntersectWithLine(lineP0, lineP1, intersectPoints, NULL);
154 
155  // Display list of intersections
156  double intersection[3];
157  double previous[3] = {picked[0], picked[1], picked[2]};
158  for(int i = 0; i < intersectPoints->GetNumberOfPoints(); i++ )
159  {
160  intersectPoints->GetPoint(i, intersection);
161 
162  Eigen::Vector3f v(intersection[0]-previous[0], intersection[1]-previous[1], intersection[2]-previous[2]);
163  float n = v.norm();
164  if(n > 0.01f)
165  {
166  v/=n;
167  v *= n/2.0f;
168  pt.r = 125;
169  pt.g = 125;
170  pt.b = 125;
171  pt.x = intersection[0];
172  pt.y = intersection[1];
173  pt.z = intersection[2];
174  pointsHolder_->at(1) = pt;
175  viewer_->addOrUpdateText("interactor_ray_text_alt", uFormat("%.2f m", n),
176  Transform(previous[0]+v[0], previous[1]+v[1],previous[2]+v[2], 0, 0, 0),
177  textSize,
178  Qt::gray);
179  viewer_->addOrUpdateLine("interactor_ray_alt",
180  Transform(previous[0], previous[1], previous[2], 0, 0, 0),
181  Transform(intersection[0], intersection[1], intersection[2], 0, 0, 0),
182  Qt::gray);
183 
184  previous[0] = intersection[0];
185  previous[1] = intersection[1];
186  previous[2] = intersection[2];
187  break;
188  }
189  }
190  viewer_->addCloud("interactor_points_alt", pointsHolder_);
191  viewer_->setCloudPointSize("interactor_points_alt", 15);
192  viewer_->setCloudOpacity("interactor_points_alt", 0.5);
193  }
194  }
195  }
196  // Forward events
197  PCLVisualizerInteractorStyle::OnMouseMove();
198 }
199 
200 void CloudViewerInteractorStyle::OnLeftButtonDown()
201 {
202  // http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/DoubleClick
203  // http://www.vtk.org/Wiki/VTK/Examples/Cxx/Interaction/PointPicker
204  if(this->CurrentRenderer && this->CurrentRenderer->GetLayer() == 1)
205  {
206  this->NumberOfClicks++;
207  int pickPosition[2];
208  this->GetInteractor()->GetEventPosition(pickPosition);
209  int xdist = pickPosition[0] - this->PreviousPosition[0];
210  int ydist = pickPosition[1] - this->PreviousPosition[1];
211 
212  this->PreviousPosition[0] = pickPosition[0];
213  this->PreviousPosition[1] = pickPosition[1];
214 
215  int moveDistance = (int)sqrt((double)(xdist*xdist + ydist*ydist));
216 
217  // Reset numClicks - If mouse moved further than resetPixelDistance
218  if(moveDistance > this->ResetPixelDistance)
219  {
220  this->NumberOfClicks = 1;
221  }
222 
223  if(this->NumberOfClicks >= 2)
224  {
225  this->NumberOfClicks = 0;
226  int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
227  0, // always zero.
228  this->CurrentRenderer);
229  if(result && this->GetInteractor()->GetControlKey()==0)
230  {
231  double picked[3];
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();
235  UASSERT(camera);
236  double position[3];
237  double focal[3];
238  camera->GetPosition(position[0], position[1], position[2]);
239  camera->GetFocalPoint(focal[0], focal[1], focal[2]);
240  //camera->SetPosition (position[0] + (picked[0]-focal[0]), position[1] + (picked[1]-focal[1]), position[2] + (picked[2]-focal[2]));
241  camera->SetFocalPoint (picked[0], picked[1], picked[2]);
242  camera->OrthogonalizeViewUp();
243 
244  if (this->AutoAdjustCameraClippingRange)
245  {
246  this->CurrentRenderer->ResetCameraClippingRange();
247  }
248 
249  if (this->Interactor->GetLightFollowCamera())
250  {
251  this->CurrentRenderer->UpdateLightsGeometryToFollowCamera();
252  }
253  }
254  else if(viewer_)
255  {
256  viewer_->removeText("interactor_pose");
257  viewer_->removeLine("interactor_line");
258  viewer_->removeCloud("interactor_points");
259  viewer_->removeLine("interactor_ray");
260  viewer_->removeText("interactor_ray_text");
261  viewer_->removeCloud("interactor_points_alt");
262  viewer_->removeLine("interactor_ray_alt");
263  viewer_->removeText("interactor_ray_text_alt");
264  PreviousMeasure[0] = 0.0f;
265  PreviousMeasure[1] = 0.0f;
266  PreviousMeasure[2] = 0.0f;
267  }
268  }
269  else if(this->GetInteractor()->GetControlKey() && viewer_)
270  {
271  int result = this->Interactor->GetPicker()->Pick(pickPosition[0], pickPosition[1],
272  0, // always zero.
273  this->CurrentRenderer);
274  if(result)
275  {
276  double picked[3];
277  this->Interactor->GetPicker()->GetPickPosition(picked);
278 
279  UDEBUG("Shift clicked! Picked value: %f %f %f", picked[0], picked[1], picked[2]);
280 
281  float textSize = 0.05;
282 
283  viewer_->removeCloud("interactor_points");
284  pointsHolder_->clear();
285  pcl::PointXYZRGB pt;
286  pt.r = 255;
287  pt.x = picked[0];
288  pt.y = picked[1];
289  pt.z = picked[2];
290  pointsHolder_->push_back(pt);
291 
292  viewer_->removeLine("interactor_ray");
293  viewer_->removeText("interactor_ray_text");
294 
295  if( PreviousMeasure[0] != 0.0f && PreviousMeasure[1] != 0.0f && PreviousMeasure[2] != 0.0f &&
296  viewer_->getAddedLines().find("interactor_line") == viewer_->getAddedLines().end())
297  {
298  viewer_->addOrUpdateLine("interactor_line",
299  Transform(PreviousMeasure[0], PreviousMeasure[1], PreviousMeasure[2], 0, 0, 0),
300  Transform(picked[0], picked[1], picked[2], 0, 0, 0),
301  Qt::red);
302  pt.x = PreviousMeasure[0];
303  pt.y = PreviousMeasure[1];
304  pt.z = PreviousMeasure[2];
305  pointsHolder_->push_back(pt);
306 
307  Eigen::Vector3f v(picked[0]-PreviousMeasure[0], picked[1]-PreviousMeasure[1], picked[2]-PreviousMeasure[2]);
308  float n = v.norm();
309  v/=n;
310  v *= n/2.0f;
311  viewer_->addOrUpdateText("interactor_pose", uFormat("%.2f m", n),
312  Transform(PreviousMeasure[0]+v[0], PreviousMeasure[1]+v[1],PreviousMeasure[2]+v[2], 0, 0, 0),
313  textSize,
314  Qt::red);
315  }
316  else
317  {
318  viewer_->removeText("interactor_pose");
319  viewer_->removeLine("interactor_line");
320  }
321  PreviousMeasure[0] = picked[0];
322  PreviousMeasure[1] = picked[1];
323  PreviousMeasure[2] = picked[2];
324 
325  viewer_->addCloud("interactor_points", pointsHolder_);
326  viewer_->setCloudPointSize("interactor_points", 15);
327  viewer_->setCloudOpacity("interactor_points", 0.5);
328  }
329  }
330  }
331 
332  // Forward events
333  PCLVisualizerInteractorStyle::OnLeftButtonDown();
334 }
335 
336 } /* namespace rtabmap */
rtabmap::CloudViewerInteractorStyle::viewer_
CloudViewer * viewer_
Definition: CloudViewerInteractorStyle.h:39
int
int
pcl
Definition: CameraOpenni.h:47
rtabmap::CloudViewerInteractorStyle::ResetPixelDistance
int ResetPixelDistance
Definition: CloudViewerInteractorStyle.h:44
rtabmap::CloudViewerInteractorStyle::setOrthoMode
void setOrthoMode(bool enabled)
Definition: CloudViewerInteractorStyle.cpp:87
rtabmap::CloudViewer::removeLine
void removeLine(const std::string &id)
Definition: CloudViewer.cpp:1851
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::CloudViewer::getLocators
const std::map< std::string, vtkSmartPointer< vtkOBBTree > > & getLocators() const
Definition: CloudViewer.h:402
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(), int viewport=1)
Definition: CloudViewer.cpp:745
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3249
CloudViewerCellPicker.h
size
Index size
this
this
rtabmap::CloudViewerInteractorStyle::PreviousMeasure
float PreviousMeasure[3]
Definition: CloudViewerInteractorStyle.h:45
vtkSmartPointer
Definition: CloudViewer.h:72
y
Matrix3f y
rtabmap::CloudViewerInteractorStyle::CloudViewerInteractorStyle
CloudViewerInteractorStyle()
Definition: CloudViewerInteractorStyle.cpp:27
UMath.h
Basic mathematics functions.
rtabmap::CloudViewerInteractorStyle::orthoMode_
bool orthoMode_
Definition: CloudViewerInteractorStyle.h:47
n
int n
rtabmap::CloudViewer::removeText
void removeText(const std::string &id)
Definition: CloudViewer.cpp:2390
rtabmap::CloudViewer::getAddedLines
const std::set< std::string > & getAddedLines() const
Definition: CloudViewer.h:237
UConversion.h
Some conversion functions.
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::CloudViewerCellPicker
Definition: CloudViewerCellPicker.h:17
CloudViewer.h
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:1810
UASSERT
#define UASSERT(condition)
z
z
x
x
rtabmap::CloudViewerInteractorStyle::OnLeftButtonDown
virtual void OnLeftButtonDown()
Definition: CloudViewerInteractorStyle.cpp:200
intersection
Definition: dummy.cpp:142
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2588
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CloudViewerInteractorStyle::PreviousPosition
int PreviousPosition[2]
Definition: CloudViewerInteractorStyle.h:43
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
rtabmap::CloudViewerInteractorStyle::Rotate
virtual void Rotate()
Definition: CloudViewerInteractorStyle.cpp:41
v
Array< int, Dynamic, 1 > v
position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
UDEBUG
#define UDEBUG(...)
rtabmap::CloudViewerInteractorStyle::NumberOfClicks
unsigned int NumberOfClicks
Definition: CloudViewerInteractorStyle.h:42
NULL
#define NULL
false
#define false
Definition: ConvertUTF.c:56
CloudViewerInteractorStyle.h
rtabmap::CloudViewerInteractorStyle::OnMouseMove
virtual void OnMouseMove()
Definition: CloudViewerInteractorStyle.cpp:108
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:2359
rtabmap::CloudViewerInteractorStyle::pointsHolder_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr pointsHolder_
Definition: CloudViewerInteractorStyle.h:46
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
rtabmap
Definition: CameraARCore.cpp:35
i
int i
result
RESULT & result
rtabmap::vtkStandardNewMacro
vtkStandardNewMacro(CloudViewerCellPicker)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:08