VisualizationsHelper.hpp
Go to the documentation of this file.
1 
20 #pragma once
21 
22 #include "typedef.hpp"
23 #include <vector>
24 
25 #include <boost/filesystem.hpp>
26 #include <boost/foreach.hpp>
27 #include <boost/range/algorithm_ext/iota.hpp>
28 #include <boost/lexical_cast.hpp>
29 #include <boost/shared_ptr.hpp>
30 #include <sensor_msgs/PointCloud2.h>
31 #include <pcl_conversions/pcl_conversions.h>
32 
33 #include "next_best_view/camera_model_filter/CameraModelFilter.hpp"
34 #include "next_best_view/helper/MarkerHelper.hpp"
35 #include "next_best_view/helper/DebugHelper.hpp"
36 #include "next_best_view/helper/TypeHelper.hpp"
37 #include "next_best_view/space_sampler/SpaceSampler.hpp"
38 #include "next_best_view/rating/RatingModule.hpp"
39 #include "next_best_view/rating/impl/DefaultScoreContainer.hpp"
40 #include "next_best_view/crop_box/CropBoxWrapper.hpp"
41 #include "asr_msgs/AsrAttributedPointCloud.h"
42 #include "asr_msgs/AsrAttributedPoint.h"
43 #include "next_best_view/helper/MapHelper.hpp"
44 #include <visualization_msgs/Marker.h>
45 #include <visualization_msgs/MarkerArray.h>
46 #include <geometry_msgs/Point.h>
47 #include <geometry_msgs/Quaternion.h>
48 #include "tf/transform_datatypes.h"
49 #include <std_msgs/ColorRGBA.h>
50 
51 namespace next_best_view
52 {
53 
54 class VisualizationHelper
55 {
56 
57 private:
58 
59  ros::Publisher mIterationMarkerArrayPublisher;
60  ros::Publisher mFrustumMarkerArrayPublisher;
61  ros::Publisher mObjectMeshMarkerPublisher;
62  ros::Publisher mFrustumObjectMeshMarkerPublisher;
63  ros::Publisher mPointObjectNormalPublisher;
64  ros::Publisher mCropBoxMarkerPublisher;
65  ros::Publisher mSamplingPublisher;
66 
67  ros::NodeHandle mNodeHandle;
68 
69  visualization_msgs::MarkerArray::Ptr mIterationMarkerArrayPtr;
70  visualization_msgs::MarkerArray::Ptr mNewFrustumMarkerArrayPtr;
71  visualization_msgs::MarkerArray::Ptr mOldFrustumMarkerArrayPtr;
72  visualization_msgs::MarkerArray::Ptr mObjectMeshMarkerArrayPtr;
73  visualization_msgs::MarkerArray::Ptr mObjectNormalsMarkerArrayPtr;
74  visualization_msgs::MarkerArray::Ptr mFrustumObjectMeshMarkerArrayPtr;
75  visualization_msgs::MarkerArray::Ptr mFrustumObjectNormalsMarkerArrayPtr;
76  visualization_msgs::MarkerArray::Ptr mCropBoxMarkerArrayPtr;
77  visualization_msgs::MarkerArray::Ptr mSamplingMarkerArrayPtr;
78 
79  MapHelperPtr mMapHelperPtr;
80  DebugHelperPtr mDebugHelperPtr;
81 
82  int m_i;
83  float m_j;
84  int mIterationStep;
85  bool mBoolClearBetweenIterations;
86  int mSampleCounter;
87 
88  // mutex to lock ray tracing visualization.
89  boost::mutex mutex;
90 
91 public:
92 
93  VisualizationHelper(MapHelperPtr mapHelperPtr):mMapHelperPtr(mapHelperPtr),m_i(0),m_j(0),mIterationStep(0) {
94  mDebugHelperPtr = DebugHelper::getInstance();
95 
96  // TODO: create something like a visualization package class that contains publisher, parameter and markers
97  std::string iterationVisualization;
98  std::string frustumVisualization;
99  std::string objectsVisualization;
100  std::string objectNormalsVisualization;
101  std::string frustumObjectsVisualization;
102  std::string frustumObjectNormalsVisualization;
103  std::string cropBoxVisualization;
104  std::string samplingVisualization;
105 
106  mNodeHandle.getParam("/nbv/iterationVisualization", iterationVisualization);
107  mNodeHandle.getParam("/nbv/frustumVisualization", frustumVisualization);
108  mNodeHandle.getParam("/nbv/objectsVisualization", objectsVisualization);
109  mNodeHandle.getParam("/nbv/objectNormalsVisualization", objectNormalsVisualization);
110  mNodeHandle.getParam("/nbv/frustumObjectsVisualization", frustumObjectsVisualization);
111  mNodeHandle.getParam("/nbv/frustumObjectNormalsVisualization", frustumObjectNormalsVisualization);
112  mNodeHandle.getParam("/nbv/cropBoxVisualization", cropBoxVisualization);
113  mNodeHandle.getParam("/nbv/samplingVisualization", samplingVisualization);
114 
115  // initialize publishers
116  mIterationMarkerArrayPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(iterationVisualization, 1000);
117  mFrustumMarkerArrayPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(frustumVisualization, 1000);
118  mObjectMeshMarkerPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(objectsVisualization, 100, false);
119  mFrustumObjectMeshMarkerPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(frustumObjectsVisualization, 100, false);
120  mPointObjectNormalPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(objectNormalsVisualization, 100, false);
121  mCropBoxMarkerPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(cropBoxVisualization, 100, false);
122  mSamplingPublisher = mNodeHandle.advertise<visualization_msgs::MarkerArray>(samplingVisualization, 10000, false);
123 
124  if (!mIterationMarkerArrayPublisher) {
125  ROS_ERROR("mIterationMarkerArrayPublisher is invalid.");
126  throw "Publisher invalid";
127  }
128  if (!mFrustumMarkerArrayPublisher) {
129  ROS_ERROR("mFrustumMarkerArrayPublisher is invalid.");
130  throw "Publisher invalid";
131  }
132  if (!mObjectMeshMarkerPublisher) {
133  ROS_ERROR("mObjectMeshMarkerPublisher is invalid.");
134  throw "Publisher invalid";
135  }
136  if (!mFrustumObjectMeshMarkerPublisher) {
137  ROS_ERROR("mFrustumObjectMeshMarkerPublisher is invalid.");
138  throw "Publisher invalid";
139  }
140  if (!mPointObjectNormalPublisher) {
141  ROS_ERROR("mPointObjectNormalPublisher is invalid.");
142  throw "Publisher invalid";
143  }
144 
145  // initalize data
146  mNodeHandle.getParam("/nbv/boolClearBetweenIterations", mBoolClearBetweenIterations);
147 
148  // initialize marker arrays
149  mIterationMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
150  mNewFrustumMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
151  mOldFrustumMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
152  mObjectMeshMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
153  mObjectNormalsMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
154  mFrustumObjectMeshMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
155  mFrustumObjectNormalsMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
156  mCropBoxMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
157  mSamplingMarkerArrayPtr = boost::make_shared<visualization_msgs::MarkerArray>();
158 
159  mSampleCounter = 0;
160  }
161 
162  bool getBoolClearBetweenIterations() {
163  return mBoolClearBetweenIterations;
164  }
165 
166  void triggerIterationVisualization(int iterationStep, const SimpleQuaternionCollectionPtr &sampledOrientationsPtr,
167  ViewportPoint currentBestViewport,
168  SamplePointCloudPtr pointcloud,
169  SpaceSamplerPtr spaceSamplerPtr) {
170 
171  mDebugHelperPtr->writeNoticeably("STARTING ITERATION VISUALIZATION", DebugHelper::VISUALIZATION);
172 
173  if(!sampledOrientationsPtr){
174  ROS_ERROR("triggerIterationVisualizations call with pointer sampledOrientationsPtr being null.");
175  mDebugHelperPtr->writeNoticeably("ENDING ITERATION VISUALIZATION", DebugHelper::VISUALIZATION);
176  return;
177  }
178  if(!pointcloud){
179  ROS_ERROR("triggerIterationVisualizations call with pointer pointcloud being null.");
180  mDebugHelperPtr->writeNoticeably("ENDING ITERATION VISUALIZATION", DebugHelper::VISUALIZATION);
181  return;
182  }
183  if(!spaceSamplerPtr){
184  ROS_ERROR("triggerIterationVisualizations call with pointer spaceSamplerPtr being null.");
185  mDebugHelperPtr->writeNoticeably("ENDING ITERATION VISUALIZATION", DebugHelper::VISUALIZATION);
186  return;
187  }
188  if(!mIterationMarkerArrayPtr){
189  ROS_ERROR("triggerIterationVisualizations call with pointer mIterationMarkerArrayPtr being null.");
190  mDebugHelperPtr->writeNoticeably("ENDING ITERATION VISUALIZATION", DebugHelper::VISUALIZATION);
191  return;
192  }
193 
194  if (iterationStep == 0 && mBoolClearBetweenIterations == true) {
195  // clear iteration visualization
196  mDebugHelperPtr->write("Deleting last iteration visualization", DebugHelper::VISUALIZATION);
197  this->deleteMarkerArray(mIterationMarkerArrayPtr, mIterationMarkerArrayPublisher);
198  m_i = 0;
199  }
200 
201  std::string s = boost::lexical_cast<std::string>(iterationStep);
202  this->mIterationStep=iterationStep;
203  m_j = iterationStep*0.25;
204  m_j = std::min(1.0f, m_j); //Prevent overflow
205 
206  triggerSpaceSampling(pointcloud,s);
207  triggerGrid(spaceSamplerPtr, s);
208  triggerCameraVis(s, sampledOrientationsPtr, currentBestViewport);
209 
210  mDebugHelperPtr->write("Publishing markers", DebugHelper::VISUALIZATION);
211  mIterationMarkerArrayPublisher.publish(mIterationMarkerArrayPtr);
212 
213  mDebugHelperPtr->writeNoticeably("ENDING ITERATION VISUALIZATION", DebugHelper::VISUALIZATION);
214  }
215 
216 
217 
218  void triggerSamplingVisualization(ViewportPointCloudPtr samples, Color markerColor, std::string ns) {
219 
220  mDebugHelperPtr->writeNoticeably("STARTING SAMPLING VISUALIZATION", DebugHelper::VISUALIZATION);
221  if (!samples) {
222  ROS_ERROR("triggerSamplingVisualization call with pointer samples being null");
223  mDebugHelperPtr->writeNoticeably("ENDING SAMPLING VISUALIZATION", DebugHelper::VISUALIZATION);
224  return;
225  }
226  if (!mSamplingMarkerArrayPtr) {
227  ROS_ERROR("triggerSamplingVisualization call with pointer mSamplingMarkerArrayPtr being null");
228  mDebugHelperPtr->writeNoticeably("ENDING SAMPLING VISUALIZATION", DebugHelper::VISUALIZATION);
229  return;
230  }
231 
232  // filter multiple viewports that are on the same position
233  SimpleVector3Collection filteredPositions;
234 
235  for (ViewportPoint &viewportPoint : *samples) {
236  SimpleVector3 viewportPointPosition = viewportPoint.getPosition();
237  viewportPointPosition[2] = 0.1;
238  bool isInFilteredPositions = false;
239  for (SimpleVector3 &filteredPosition : filteredPositions) {
240  if (MathHelper::vector3Equal(filteredPosition, viewportPointPosition)) {
241  isInFilteredPositions = true;
242  break;
243  }
244  }
245  if (!isInFilteredPositions) {
246  filteredPositions.push_back(viewportPointPosition);
247  }
248  }
249 
250  // create markers
251  ROS_INFO_STREAM("size: " << filteredPositions.size());
252  double SpaceSamplingMarkerScale;
253  mNodeHandle.getParam("/nbv/SpaceSamplingMarker_Scale", SpaceSamplingMarkerScale);
254  SimpleVector3 scale(SpaceSamplingMarkerScale, SpaceSamplingMarkerScale, SpaceSamplingMarkerScale);
255  for (SimpleVector3 &samplePosition : filteredPositions) {
256  visualization_msgs::Marker samplingMarker = MarkerHelper::getCylinderMarker(mSampleCounter, samplePosition, 1, scale, markerColor, ns);
257  mSamplingMarkerArrayPtr->markers.push_back(samplingMarker);
258  mSampleCounter++;
259  }
260 
261  mSamplingPublisher.publish(mSamplingMarkerArrayPtr);
262 
263  mDebugHelperPtr->writeNoticeably("ENDING SAMPLING VISUALIZATION", DebugHelper::VISUALIZATION);
264  }
265 
266  void resetSamplingVisualization() {
267  deleteMarkerArray(mSamplingMarkerArrayPtr, mSamplingPublisher);
268  }
269 
274  void triggerFrustumsVisualization(CameraModelFilterPtr newCamera) {
275  this->triggerOldFrustumVisualization();
276  this->triggerNewFrustumVisualization(newCamera);
277  }
278 
279  void triggerNewFrustumVisualization(CameraModelFilterPtr newCamera) {
280 
281  mDebugHelperPtr->writeNoticeably("STARTING NEW FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
282 
283  if(!newCamera){
284  ROS_ERROR("triggerNewFrustumVisualization call with pointer newCamera being null.");
285  mDebugHelperPtr->writeNoticeably("ENDING NEW FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
286  return;
287  }
288  if(!mNewFrustumMarkerArrayPtr){
289  ROS_ERROR("triggerNewFrustumVisualization call with pointer mNewFrustumMarkerArrayPtr being null.");
290  mDebugHelperPtr->writeNoticeably("ENDING NEW FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
291  return;
292  }
293 
294  mDebugHelperPtr->write("Deleting last frustum visualization", DebugHelper::VISUALIZATION);
295  this->deleteMarkerArray(mNewFrustumMarkerArrayPtr, mFrustumMarkerArrayPublisher);
296 
297  uint32_t sequence = 0;
298  mNewFrustumMarkerArrayPtr = newCamera->getVisualizationMarkerArray(sequence, 0.0);
299 
300  mDebugHelperPtr->write(std::stringstream() << "Frustum Pivot Point : "
301  << newCamera->getPivotPointPosition()[0]
302  << " , " << newCamera->getPivotPointPosition()[1]
303  << " , " << newCamera->getPivotPointPosition()[2],
304  DebugHelper::VISUALIZATION);
305 
306  std::string ns = "new_nbv_frustum";
307 
308  for (unsigned int i = 0; i < mNewFrustumMarkerArrayPtr->markers.size(); i++)
309  {
310  mNewFrustumMarkerArrayPtr->markers.at(i).color.r = 0;
311  mNewFrustumMarkerArrayPtr->markers.at(i).color.g = 1;
312  mNewFrustumMarkerArrayPtr->markers.at(i).color.b = 1;
313  mNewFrustumMarkerArrayPtr->markers.at(i).ns = ns;
314  }
315 
316  mDebugHelperPtr->write("Publishing markers", DebugHelper::VISUALIZATION);
317  mFrustumMarkerArrayPublisher.publish(*mNewFrustumMarkerArrayPtr);
318 
319  mDebugHelperPtr->writeNoticeably("ENDING NEW FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
320  }
321 
322  void triggerOldFrustumVisualization(CameraModelFilterPtr camera = NULL) {
323  mDebugHelperPtr->writeNoticeably("STARTING OLD FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
324 
325  if(!mOldFrustumMarkerArrayPtr){
326  ROS_ERROR("triggerOldFrustumVisualization call with pointer mOldFrustumMarkerArrayPtr being null.");
327  mDebugHelperPtr->writeNoticeably("ENDING OLD FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
328  return;
329  }
330 
331  mDebugHelperPtr->write("Deleting last frustum visualization", DebugHelper::VISUALIZATION);
332  this->deleteMarkerArray(mOldFrustumMarkerArrayPtr, mFrustumMarkerArrayPublisher);
333 
334  std::string ns = "old_nbv_frustum";
335 
336  if (camera) {
337  uint32_t sequence = 0;
338  mOldFrustumMarkerArrayPtr = camera->getVisualizationMarkerArray(sequence, 0.0);
339  for (unsigned int i = 0; i < mOldFrustumMarkerArrayPtr->markers.size(); i++)
340  {
341  mOldFrustumMarkerArrayPtr->markers.at(i).color.r = 1;
342  mOldFrustumMarkerArrayPtr->markers.at(i).color.g = 0;
343  mOldFrustumMarkerArrayPtr->markers.at(i).color.b = 1;
344  mOldFrustumMarkerArrayPtr->markers.at(i).lifetime = ros::Duration(4.0);
345  mOldFrustumMarkerArrayPtr->markers.at(i).ns = ns;
346  }
347  }
348  else {
349 
350  if(!mNewFrustumMarkerArrayPtr){
351  ROS_ERROR("triggerOldFrustumVisualization call with pointer mNewFrustumMarkerArrayPtr being null.");
352  mDebugHelperPtr->writeNoticeably("ENDING OLD FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
353  return;
354  }
355 
356  // use old data in mNewFrustumMarkerArrayPtr if no camera is given
357  if (mNewFrustumMarkerArrayPtr->markers.size() != 0)
358  {
359  mDebugHelperPtr->write("Copying old frustum marker array", DebugHelper::VISUALIZATION);
360  std::copy(mNewFrustumMarkerArrayPtr->markers.begin(), mNewFrustumMarkerArrayPtr->markers.end(),
361  back_inserter(mOldFrustumMarkerArrayPtr->markers));
362  mDebugHelperPtr->write("Old frustum marker array copied.", DebugHelper::VISUALIZATION);
363 
364  for (unsigned int i = 0; i < mOldFrustumMarkerArrayPtr->markers.size(); i++)
365  {
366  mOldFrustumMarkerArrayPtr->markers.at(i).lifetime = ros::Duration(4.0);
367  mOldFrustumMarkerArrayPtr->markers.at(i).color.r = 1;
368  mOldFrustumMarkerArrayPtr->markers.at(i).color.g = 0;
369  mOldFrustumMarkerArrayPtr->markers.at(i).color.b = 1;
370  mOldFrustumMarkerArrayPtr->markers.at(i).ns = ns;
371  }
372  }
373  }
374 
375  if (mOldFrustumMarkerArrayPtr->markers.size() != 0) {
376  mDebugHelperPtr->write("Publishing markers", DebugHelper::VISUALIZATION);
377  mFrustumMarkerArrayPublisher.publish(mOldFrustumMarkerArrayPtr);
378  }
379  mDebugHelperPtr->writeNoticeably("ENDING OLD FRUSTUM VISUALIZATION", DebugHelper::VISUALIZATION);
380  }
381 
382  void clearFrustumVisualization()
383  {
384 
385  if(!mNewFrustumMarkerArrayPtr){
386  ROS_ERROR("clearFrustumVisualization call with pointer mNewFrustumMarkerArrayPtr being null.");
387  return;
388  }
389 
390  if (mNewFrustumMarkerArrayPtr->markers.size() == 0) {
391  return;
392  }
393 
394  mDebugHelperPtr->write("Deleting last frustum visualization", DebugHelper::VISUALIZATION);
395 
396  this->deleteMarkerArray(mNewFrustumMarkerArrayPtr, mFrustumMarkerArrayPublisher);
397  this->deleteMarkerArray(mOldFrustumMarkerArrayPtr, mFrustumMarkerArrayPublisher);
398  }
399 
405  void triggerObjectPointCloudVisualization(ObjectPointCloud& objectPointCloud, std::map<std::string, std::string>& typeToMeshResource) {
406  mDebugHelperPtr->writeNoticeably("STARTING OBJECT POINT CLOUD VISUALIZATION", DebugHelper::VISUALIZATION);
407 
408  if(!mObjectMeshMarkerArrayPtr){
409  ROS_ERROR("triggerObjectPointCloudVisualization call with pointer mObjectMeshMarkerArrayPtr being null.");
410  mDebugHelperPtr->writeNoticeably("ENDING OBJECT POINT CLOUD VISUALIZATION", DebugHelper::VISUALIZATION);
411  return;
412  }
413 
414  visualizePointCloudObjects(objectPointCloud, typeToMeshResource,
415  mObjectMeshMarkerArrayPtr, mObjectMeshMarkerPublisher);
416 
417  mDebugHelperPtr->writeNoticeably("ENDING OBJECT POINT CLOUD VISUALIZATION", DebugHelper::VISUALIZATION);
418  }
419 
425  void triggerFrustumObjectPointCloudVisualization(ObjectPointCloud& frustumObjectPointCloud, std::map<std::string, std::string>& typeToMeshResource) {
426  mDebugHelperPtr->writeNoticeably("STARTING FRUSTUM OBJECT POINT CLOUD VISUALIZATION", DebugHelper::VISUALIZATION);
427 
428  if(!mFrustumObjectMeshMarkerArrayPtr){
429  ROS_ERROR("triggerFrustumObjectPointCloudVisualization call with pointer mFrustumObjectMeshMarkerArrayPtr being null.");
430  mDebugHelperPtr->writeNoticeably("ENDING FRUSTUM OBJECT POINT CLOUD VISUALIZATION", DebugHelper::VISUALIZATION);
431  return;
432  }
433 
434  std_msgs::ColorRGBA::Ptr colorFrustumMeshMarkerPtr(new std_msgs::ColorRGBA(this->createColorRGBA(0, 0, 1, 0.8)));
435 
436  visualizePointCloudObjects(frustumObjectPointCloud, typeToMeshResource,
437  mFrustumObjectMeshMarkerArrayPtr, mFrustumObjectMeshMarkerPublisher,
438  colorFrustumMeshMarkerPtr);
439 
440  mDebugHelperPtr->writeNoticeably("ENDING FRUSTUM OBJECT POINT CLOUD VISUALIZATION", DebugHelper::VISUALIZATION);
441  }
442 
443  void clearFrustumObjectPointCloudVisualization() {
444  deleteMarkerArray(mFrustumObjectMeshMarkerArrayPtr, mFrustumObjectMeshMarkerPublisher);
445  }
446 
447  void triggerCropBoxVisualization(const boost::shared_ptr<std::vector<CropBoxWrapperPtr>> cropBoxWrapperPtrList)
448  {
449  if(!mCropBoxMarkerArrayPtr)
450  {
451  ROS_ERROR_STREAM("triggerCropBoxVisualization::mCropBoxMarkerArrayPtr is empty.");
452  return;
453  }
454 
455  std::vector<double> CropBoxMarkerRGBA;
456  mNodeHandle.getParam("/nbv/CropBoxMarker_RGBA", CropBoxMarkerRGBA);
457  SimpleVector4 color = TypeHelper::getSimpleVector4(CropBoxMarkerRGBA);
458 
459  int id = 0;
460  for(std::vector<CropBoxWrapperPtr>::const_iterator it = cropBoxWrapperPtrList->begin(); it != cropBoxWrapperPtrList->end(); ++it)
461  {
462  CropBoxWrapperPtr cropBoxWrapperPtr = *it;
463  CropBoxPtr cropBoxPtr = cropBoxWrapperPtr->getCropBox();
464  Eigen::Vector4f ptMin,ptMax;
465  ptMin = cropBoxPtr->getMin();
466  ptMax = cropBoxPtr->getMax();
467  Eigen::Vector3f rotation, translation;
468  rotation = cropBoxPtr->getRotation();
469  translation = cropBoxPtr->getTranslation();
470 
471  Eigen::Matrix3f rotationMatrix;
472  rotationMatrix = Eigen::AngleAxisf(rotation[0], Eigen::Vector3f::UnitX())
473  * Eigen::AngleAxisf(rotation[1], Eigen::Vector3f::UnitY())
474  * Eigen::AngleAxisf(rotation[2], Eigen::Vector3f::UnitZ());
475 
476 
477  SimpleVector3 position_cb_frame;
478  position_cb_frame[0] = (ptMax[0] + ptMin[0])/2;
479  position_cb_frame[1] = (ptMax[1] + ptMin[1])/2;
480  position_cb_frame[2] = (ptMax[2] + ptMin[2])/2;
481 
482  SimpleVector3 position_map_frame;
483  position_map_frame = rotationMatrix * position_cb_frame + translation;
484 
485  SimpleQuaternion orientation(rotationMatrix);
486 
487  SimpleVector3 scale;
488  scale[0] = std::abs(ptMax[0] - ptMin[0]);
489  scale[1] = std::abs(ptMax[1] - ptMin[1]);
490  scale[2] = std::abs(ptMax[2] - ptMin[2]);
491 
492  std::stringstream ns;
493  ns << "cropbox_ns" << id;
494 
495  mCropBoxMarkerArrayPtr->markers.push_back(MarkerHelper::getCubeMarker(id,
496  position_map_frame, orientation, scale, color, ns.str()));
497  id++;
498  }
499  mCropBoxMarkerPublisher.publish(*mCropBoxMarkerArrayPtr);
500  }
501 
502  void triggerObjectNormalsVisualization(ObjectPointCloud& objectPointCloud) {
503  mDebugHelperPtr->writeNoticeably("STARTING OBJECT POINT CLOUD HYPOTHESIS VISUALIZATION", DebugHelper::VISUALIZATION);
504 
505  if(!mObjectNormalsMarkerArrayPtr){
506  ROS_ERROR("triggerObjectPointCloudHypothesisVisualization call with pointer mObjectNormalsMarkerArrayPtr being null.");
507  mDebugHelperPtr->writeNoticeably("ENDING OBJECT POINT CLOUD HYPOTHESIS VISUALIZATION", DebugHelper::VISUALIZATION);
508  return;
509  }
510 
511  visualizePointCloudNormals(objectPointCloud,
512  mObjectNormalsMarkerArrayPtr, mPointObjectNormalPublisher);
513 
514  mDebugHelperPtr->writeNoticeably("ENDING OBJECT POINT CLOUD HYPOTHESIS VISUALIZATION", DebugHelper::VISUALIZATION);
515  }
516 
517  /* only working because the shape-based recognizer sets the observedId with the object color */
518  static std_msgs::ColorRGBA getMeshColor(std::string observedId)
519  {
520  std_msgs::ColorRGBA retColor = VisualizationHelper::createColorRGBA(0.0, 0.0, 0.0, 0.0);
521 
522  if ( ( observedId.length() == 12 ) && ( observedId.find_first_not_of("0123456789") == std::string::npos ) )
523  {
524  float rgba[4];
525  bool isColor = true;
526  try
527  {
528  for (int i = 0; i <= 3; i++)
529  {
530  std::string temp;
531 
532  temp = observedId.substr( (i * 3), 3 );
533  rgba[i] = std::stof(temp) / 100.0;
534  }
535  }
536  catch (std::invalid_argument& ia)
537  {
538  DebugHelper::getInstance()->write(ia.what(), DebugHelper::VISUALIZATION);
539  isColor = false;
540  }
541 
542  if(isColor)
543  {
544  retColor = VisualizationHelper::createColorRGBA(rgba[0], rgba[1], rgba[2], rgba[3]);
545  }
546  }
547 
548  return retColor;
549  }
550 
551 private:
552 
553  void triggerCameraVis(std::string s, const SimpleQuaternionCollectionPtr &sampledOrientationsPtr,
554  ViewportPoint currentBestViewport) {
555 
556  if(!sampledOrientationsPtr){
557  ROS_ERROR("triggerCameraVis call with pointer sampledOrientationsPtr being null.");
558  return;
559  }
560  if(!mIterationMarkerArrayPtr){
561  ROS_ERROR("triggerCameraVis call with pointer mIterationMarkerArrayPtr being null.");
562  return;
563  }
564 
565  // get parameters
566  std::vector<double> ViewPortMarkerScales;
567  std::vector<double> ViewPortMarkerRGBA;
568  std::vector<double> ViewPortDirectionsRGBA;
569  std::vector<double> ViewPortDirectionsScales;
570  std::vector<double> ColumnPositionMarkerRGBA;
571  double ViewPortMarkerHeightFactor;
572  double ViewPortMarkerShrinkFactor;
573  double ColumnPositionMarkerWidth;
574 
575  mNodeHandle.getParam("/nbv/ViewPortMarker_Scales", ViewPortMarkerScales);
576  mNodeHandle.getParam("/nbv/ViewPortMarker_HeightFactor", ViewPortMarkerHeightFactor);
577  mNodeHandle.getParam("/nbv/ViewPortMarker_ShrinkFactor", ViewPortMarkerShrinkFactor);
578  mNodeHandle.getParam("/nbv/ViewPortMarker_RGBA", ViewPortMarkerRGBA);
579  mNodeHandle.getParam("/nbv/ViewPortDirections_RGBA", ViewPortDirectionsRGBA);
580  mNodeHandle.getParam("/nbv/ViewPortDirections_Scales", ViewPortDirectionsScales);
581  mNodeHandle.getParam("/nbv/ColumnPositionMarker_Width", ColumnPositionMarkerWidth);
582  mNodeHandle.getParam("/nbv/ColumnPositionMarker_RGBA", ColumnPositionMarkerRGBA);
583 
584  SimpleVector3 position = currentBestViewport.getPosition();
585 
586  SimpleVector3 scaleViewPortDirection = TypeHelper::getSimpleVector3(ViewPortDirectionsScales);
587  SimpleVector4 colorViewPortDirection = TypeHelper::getSimpleVector4(ViewPortDirectionsRGBA);
588  colorViewPortDirection[0] -= m_j;
589  colorViewPortDirection[1] += m_j;
590 
591  SimpleVector3 scaleViewPort = TypeHelper::getSimpleVector3(ViewPortMarkerScales);
592  SimpleVector4 colorViewPort = TypeHelper::getSimpleVector4(ViewPortMarkerRGBA);
593  colorViewPort[0] -= m_j;
594  colorViewPort[1] += m_j;
595 
596  BOOST_FOREACH(SimpleQuaternion q, *sampledOrientationsPtr)
597  {
598  // get the data for the markers
599  SimpleVector3 visualAxis = MathHelper::getVisualAxis(q);
600  visualAxis[0] = visualAxis[0]/ViewPortMarkerShrinkFactor + position[0];
601  visualAxis[1] = visualAxis[1]/ViewPortMarkerShrinkFactor + position[1];
602  visualAxis[2] = visualAxis[2]/ViewPortMarkerShrinkFactor + position[2]
603  + mIterationStep * ViewPortMarkerHeightFactor;
604 
605  // get viewport direction marker
606  m_i++;
607  std::string ns = "ViewPortDirections" + s;
608 
609  visualization_msgs::Marker ViewPortDirectionsMarker = MarkerHelper::getArrowMarker(m_i, visualAxis, q, scaleViewPortDirection, colorViewPortDirection, ns);
610  mIterationMarkerArrayPtr->markers.push_back(ViewPortDirectionsMarker);
611 
612  // get viewport marker
613  m_i++;
614  ns = "visualAxis" + s;
615 
616  visualization_msgs::Marker ViewPortMarker = MarkerHelper::getCubeMarker(m_i, visualAxis, q, scaleViewPort, colorViewPort, ns);
617  mIterationMarkerArrayPtr->markers.push_back(ViewPortMarker);
618  }
619 
620  // get unit sphere marker
621  SimpleVector3 spherePosition(position);
622  spherePosition[2] += mIterationStep * ViewPortMarkerHeightFactor;
623 
624  SimpleVector3 scale(2.0/ViewPortMarkerShrinkFactor, 2.0/ViewPortMarkerShrinkFactor, 2.0/ViewPortMarkerShrinkFactor);
625 
626  SimpleVector4 color = TypeHelper::getSimpleVector4(ViewPortMarkerRGBA);
627  color[0] -= m_j;
628  color[1] += m_j;
629  color[3] /= 2.0;
630 
631  std::string ns = "visualAxisSphere" + s;
632 
633  m_i++;
634 
635  visualization_msgs::Marker ViewPortSphereMarker = MarkerHelper::getSphereMarker(m_i, spherePosition, scale, color, ns);
636  mIterationMarkerArrayPtr->markers.push_back(ViewPortSphereMarker);
637 
638  // get column position marker
639  SimpleVector3 point1(position);
640  SimpleVector3 point2(position);
641  point1[2] = 0;
642  point2[2] += mIterationStep * ViewPortMarkerHeightFactor;
643  std::vector<SimpleVector3> points;
644  points.push_back(point1);
645  points.push_back(point2);
646 
647  color = TypeHelper::getSimpleVector4(ColumnPositionMarkerRGBA);
648 
649  ns = "LineVizu" + s;
650 
651  m_i++;
652 
653  visualization_msgs::Marker ColumnPositionMarker = MarkerHelper::getLineListMarker(m_i, points, ColumnPositionMarkerWidth,
654  color, ns);
655  mIterationMarkerArrayPtr->markers.push_back(ColumnPositionMarker);
656 
657  // get nbv camera direction
658  scale = SimpleVector3(1, ColumnPositionMarkerWidth, ColumnPositionMarkerWidth);
659 
660  color = TypeHelper::getSimpleVector4(ViewPortMarkerRGBA);
661  color[0] -= m_j;
662  color[1] += m_j;
663 
664  ns = "ArrowVizu" +s ;
665 
666  m_i++;
667 
668  visualization_msgs::Marker NextBestViewCameraDirectionMarker = MarkerHelper::getArrowMarker(m_i, position,
669  currentBestViewport.getSimpleQuaternion(),
670  scale, color, ns);
671  mIterationMarkerArrayPtr->markers.push_back(NextBestViewCameraDirectionMarker);
672  }
673 
674  void triggerSpaceSampling(SamplePointCloudPtr pointcloud, std::string s){
675 
676  if(!pointcloud){
677  ROS_ERROR("triggerSpaceSampling call with pointer pointcloud being null.");
678  return;
679  }
680  if(!mIterationMarkerArrayPtr){
681  ROS_ERROR("triggerSpaceSampling call with pointer mIterationMarkerArrayPtr being null.");
682  return;
683  }
684 
685  // get parameters
686  double SpaceSamplingMarkerScale;
687  std::vector<double> SpaceSamplingMarkerRGBA;
688  mNodeHandle.getParam("/nbv/SpaceSamplingMarker_Scale", SpaceSamplingMarkerScale);
689  mNodeHandle.getParam("/nbv/SpaceSamplingMarker_RGBA", SpaceSamplingMarkerRGBA);
690 
691  SimpleVector3 scale(SpaceSamplingMarkerScale, SpaceSamplingMarkerScale, SpaceSamplingMarkerScale);
692 
693  SimpleVector4 color = TypeHelper::getSimpleVector4(SpaceSamplingMarkerRGBA);
694  color[0] -= m_j;
695  color[1] += m_j;
696 
697 
698  for(SamplePointCloud::iterator it = pointcloud->points.begin(); it < pointcloud->points.end(); it++)
699  {
700  // get space sampling marker
701  gm::Point point = it->getPoint();
702  SimpleVector3 position = TypeHelper::getSimpleVector3(point);
703  position[2] = 0.1;
704 
705  std::string ns = "SamplePoints_NS" + s;
706 
707  m_i++;
708 
709  visualization_msgs::Marker SpaceSamplingMarker = MarkerHelper::getCylinderMarker(m_i, position, 1, scale, color, ns);
710  mIterationMarkerArrayPtr->markers.push_back(SpaceSamplingMarker);
711  }
712 
713  }
714 
715  void triggerGrid(SpaceSamplerPtr spaceSamplerPtr, std::string s){
716 
717  if(!spaceSamplerPtr){
718  ROS_ERROR("triggerGrid call with pointer spaceSamplerPtr being null.");
719  return;
720  }
721  if(!mIterationMarkerArrayPtr){
722  ROS_ERROR("triggerGrid call with pointer mIterationMarkerArrayPtr being null.");
723  return;
724  }
725 
726  // get parameters
727  double GridMarkerScaleZ;
728  std::vector<double> GridMarkerRGBA;
729  mNodeHandle.getParam("/nbv/GridMarker_ScaleZ", GridMarkerScaleZ);
730  mNodeHandle.getParam("/nbv/GridMarker_RGBA", GridMarkerRGBA);
731 
732  double xwidth = std::abs(spaceSamplerPtr->getXtop() - spaceSamplerPtr->getXbot());
733  double ywidth = std::abs(spaceSamplerPtr->getYtop() - spaceSamplerPtr->getYbot());
734  double xmid = (spaceSamplerPtr->getXtop() + spaceSamplerPtr->getXbot())/2.0;
735  double ymid = (spaceSamplerPtr->getYtop() + spaceSamplerPtr->getYbot())/2.0;
736 
737  SimpleVector3 position;
738  position[0] = xmid;
739  position[1] = ymid;
740  position[2] = 0;
741 
742  SimpleQuaternion orientation(1,0,0,0);
743 
744  SimpleVector3 scale(xwidth, ywidth, GridMarkerScaleZ);
745  SimpleVector4 color = TypeHelper::getSimpleVector4(GridMarkerRGBA);
746 
747  std::string ns = "Radius" + s;
748 
749  m_i++;
750 
751  visualization_msgs::Marker GridMarker = MarkerHelper::getCubeMarker(m_i, position, orientation, scale, color, ns);
752  mIterationMarkerArrayPtr->markers.push_back(GridMarker);
753  }
754 
755  visualization_msgs::Marker getVoxelMarker(GridVector3 voxelPos, double worldVoxelSize, SimpleVector4 color, int id, std::string ns) {
756  double mapVoxelSize;
757  mMapHelperPtr->worldToMapSize(worldVoxelSize, mapVoxelSize);
758 
759  SimpleVector3 mapPosition(((double) voxelPos[0] + 0.5) * mapVoxelSize, ((double) voxelPos[1] + 0.5) * mapVoxelSize, ((double) voxelPos[2] + 0.5) * mapVoxelSize);
760  SimpleVector3 worldPosition;
761  mMapHelperPtr->mapToWorldCoordinates(mapPosition, worldPosition);
762 
763  SimpleQuaternion orientation(1,0,0,0);
764  SimpleVector3 scale(worldVoxelSize,worldVoxelSize,worldVoxelSize);
765  return MarkerHelper::getCubeMarker(id, worldPosition, orientation, scale, color, ns);
766  }
767 
776  static void visualizePointCloudObjects(ObjectPointCloud& objectPointCloud, std::map<std::string, std::string>& typeToMeshResource,
777  visualization_msgs::MarkerArray::Ptr objectMarkerArrayPtr, ros::Publisher& objectPublisher,
778  std_msgs::ColorRGBA::Ptr objectColorPtr = NULL) {
779  DebugHelperPtr debugHelperPtr = DebugHelper::getInstance();
780 
781  debugHelperPtr->write("Deleting old object point cloud visualization", DebugHelper::VISUALIZATION);
782  deleteMarkerArray(objectMarkerArrayPtr, objectPublisher);
783 
784  unsigned int index = 0;
785  std::string ns = "ObjectMeshes";
786 
787  for(ObjectPointCloud::iterator it = objectPointCloud.begin(); it < objectPointCloud.end(); it++)
788  {
789  geometry_msgs::Pose pose = it->getPose();
790  std_msgs::ColorRGBA color;
791 
792  if (!objectColorPtr) {
793  color = it->color;
794  }
795  else {
796  color = *objectColorPtr;
797  }
798 
799  visualization_msgs::Marker objectMarker = getObjectMarker(pose, it->type, color, typeToMeshResource, index, ns);
800 
801  objectMarkerArrayPtr->markers.push_back(objectMarker);
802 
803  index++;
804  }
805 
806  debugHelperPtr->write(std::stringstream() << "Publishing " << objectPointCloud.size() <<" object points",
807  DebugHelper::VISUALIZATION);
808  objectPublisher.publish(*objectMarkerArrayPtr);
809  }
810 
817  static void visualizePointCloudNormals(ObjectPointCloud& objectPointCloud,
818  visualization_msgs::MarkerArray::Ptr objectNormalsMarkerArrayPtr, ros::Publisher& objectNormalsPublisher) {
819  DebugHelperPtr debugHelperPtr = DebugHelper::getInstance();
820 
821  debugHelperPtr->write("Deleting old object normals visualization", DebugHelper::VISUALIZATION);
822  deleteMarkerArray(objectNormalsMarkerArrayPtr, objectNormalsPublisher);
823 
824  unsigned int index = 0;
825  SimpleVector4 color = SimpleVector4(1.0, 1.0, 0.0, 1.0);
826  SimpleVector3 scale = SimpleVector3(0.005, 0.01, 0.005);
827  std::string ns = "ObjectNormals";
828 
829  for(unsigned int i = 0; i < objectPointCloud.points.size(); i++)
830  {
831  ObjectPoint point = objectPointCloud.points[i];
832 
833  for(Indices::iterator it = point.active_normal_vectors->begin(); it < point.active_normal_vectors->end(); ++it)
834  {
835  SimpleVector3 start = point.getPosition();
836  SimpleVector3 end = 0.07 * point.normal_vectors->at(*it);
837  end[0] += start[0];
838  end[1] += start[1];
839  end[2] += start[2];
840 
841 
842  visualization_msgs::Marker objectNormalMarker = MarkerHelper::getArrowMarker(index, start, end,
843  scale, color, ns);
844  objectNormalsMarkerArrayPtr->markers.push_back(objectNormalMarker);
845 
846  index++;
847  }
848  }
849 
850  debugHelperPtr->write("Publishing object normals", DebugHelper::VISUALIZATION);
851  objectNormalsPublisher.publish(*objectNormalsMarkerArrayPtr);
852  }
853 
854  static void deleteMarkerArray(visualization_msgs::MarkerArray::Ptr &array, ros::Publisher &publisher)
855  {
856  if(!array || !publisher)
857  return;
858 
859  if (array->markers.size() == 0) {
860  return;
861  }
862 
863  for (unsigned int i = 0; i < array->markers.size(); i++)
864  {
865  array->markers.at(i).action = visualization_msgs::Marker::DELETE;
866  }
867 
868  publisher.publish(array);
869  array->markers.clear();
870  }
871 
872  static visualization_msgs::Marker getObjectMarker(geometry_msgs::Pose pose, std::string type, std_msgs::ColorRGBA color,
873  std::map<std::string, std::string> typeToMeshResource, int id, std::string ns) {
874  visualization_msgs::Marker objectMarker;
875 
876  SimpleVector3 position = TypeHelper::getSimpleVector3(pose);
877  SimpleQuaternion orientation = TypeHelper::getSimpleQuaternion(pose);
878 
879  if (typeToMeshResource.count(type) == 1) {
880  std::string meshResource = typeToMeshResource[type];
881 
882  // Cut off .iv, append .dae
883  boost::filesystem::path meshResourcePath = boost::filesystem::path(meshResource).replace_extension(".dae");
884  meshResource = meshResourcePath.string();
885 
886  // the model size unit is mm
887  SimpleVector3 scale(0.0005, 0.0005, 0.0005);
888 
889  objectMarker = MarkerHelper::getMeshMarker(id, meshResource, position, orientation, scale, ns);
890  objectMarker.color = color;
891  }
892  else {
893  SimpleVector3 scale(0.01, 0.01, 0.01);
894  SimpleVector4 sphereColor = TypeHelper::getSimpleVector4(color);
895  objectMarker = MarkerHelper::getSphereMarker(id, position, scale, sphereColor, ns);
896  }
897 
898  return objectMarker;
899  }
900 
901  static std_msgs::ColorRGBA createColorRGBA(float red, float green, float blue, float alpha)
902  {
903  std_msgs::ColorRGBA color;
904 
905  color.r = red;
906  color.g = green;
907  color.b = blue;
908  color.a = alpha;
909 
910  return color;
911  }
912 
913 };
914 
915 typedef boost::shared_ptr<VisualizationHelper> VisualizationHelperPtr;
916 
917 }
ros::NodeHandle
next_best_view::VisualizationHelper::mMapHelperPtr
MapHelperPtr mMapHelperPtr
Definition: VisualizationsHelper.hpp:79
next_best_view::ObjectPointCloud
pcl::PointCloud< ObjectPoint > ObjectPointCloud
Definition: typedef.hpp:85
next_best_view::MathHelper::vector3Equal
static bool vector3Equal(SimpleVector3 v1, SimpleVector3 v2)
Definition: MathHelper.cpp:181
next_best_view::VisualizationHelper::mSampleCounter
int mSampleCounter
Definition: VisualizationsHelper.hpp:86
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
f
f
next_best_view::TypeHelper::getSimpleVector4
static SimpleVector4 getSimpleVector4(const std::vector< double > &vector)
Definition: TypeHelper.cpp:56
next_best_view::VisualizationHelper::triggerGrid
void triggerGrid(SpaceSamplerPtr spaceSamplerPtr, std::string s)
Definition: VisualizationsHelper.hpp:715
next_best_view::RealObjectPoint::normal_vectors
SimpleVector3CollectionPtr normal_vectors
Definition: RealObjectPoint.hpp:72
next_best_view::MarkerHelper::getCubeMarker
static visualization_msgs::Marker getCubeMarker(int id, SimpleVector3 position, SimpleQuaternion orientation, SimpleVector3 scale, SimpleVector4 color, std::string ns="my_namespace")
returns a cube marker with the given settings
Definition: MarkerHelper.cpp:123
next_best_view::ViewportPointCloudPtr
ViewportPointCloud::Ptr ViewportPointCloudPtr
Definition: typedef.hpp:97
next_best_view::SimpleVector3
Eigen::Matrix< Precision, 3, 1 > SimpleVector3
Definition: typedef.hpp:53
s
XmlRpcServer s
next_best_view::VisualizationHelper::getBoolClearBetweenIterations
bool getBoolClearBetweenIterations()
Definition: VisualizationsHelper.hpp:162
next_best_view::VisualizationHelper::clearFrustumObjectPointCloudVisualization
void clearFrustumObjectPointCloudVisualization()
Definition: VisualizationsHelper.hpp:443
next_best_view::VisualizationHelper::m_i
int m_i
Definition: VisualizationsHelper.hpp:82
next_best_view::DefaultViewportPoint
DefaultViewportPoint.
Definition: DefaultViewportPoint.hpp:60
next_best_view::VisualizationHelper::triggerObjectPointCloudVisualization
void triggerObjectPointCloudVisualization(ObjectPointCloud &objectPointCloud, std::map< std::string, std::string > &typeToMeshResource)
triggerObjectPointCloudVisualization shows only objects without hypothesis
Definition: VisualizationsHelper.hpp:405
next_best_view::VisualizationHelper::triggerCropBoxVisualization
void triggerCropBoxVisualization(const boost::shared_ptr< std::vector< CropBoxWrapperPtr >> cropBoxWrapperPtrList)
Definition: VisualizationsHelper.hpp:447
next_best_view::VisualizationHelper::getVoxelMarker
visualization_msgs::Marker getVoxelMarker(GridVector3 voxelPos, double worldVoxelSize, SimpleVector4 color, int id, std::string ns)
Definition: VisualizationsHelper.hpp:755
next_best_view::DebugHelper::VISUALIZATION
Definition: DebugHelper.hpp:39
next_best_view::VisualizationHelper::mPointObjectNormalPublisher
ros::Publisher mPointObjectNormalPublisher
Definition: VisualizationsHelper.hpp:63
next_best_view::VisualizationHelper::mFrustumObjectNormalsMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mFrustumObjectNormalsMarkerArrayPtr
Definition: VisualizationsHelper.hpp:75
next_best_view::VisualizationHelper::triggerSpaceSampling
void triggerSpaceSampling(SamplePointCloudPtr pointcloud, std::string s)
Definition: VisualizationsHelper.hpp:674
next_best_view::GridVector3
Eigen::Matrix< int, 3, 1 > GridVector3
Definition: typedef.hpp:57
next_best_view::VisualizationHelper::mCropBoxMarkerPublisher
ros::Publisher mCropBoxMarkerPublisher
Definition: VisualizationsHelper.hpp:64
next_best_view::VisualizationHelper::triggerSamplingVisualization
void triggerSamplingVisualization(ViewportPointCloudPtr samples, Color markerColor, std::string ns)
Definition: VisualizationsHelper.hpp:218
next_best_view::VisualizationHelper::triggerCameraVis
void triggerCameraVis(std::string s, const SimpleQuaternionCollectionPtr &sampledOrientationsPtr, ViewportPoint currentBestViewport)
Definition: VisualizationsHelper.hpp:553
next_best_view::MarkerHelper::getCylinderMarker
static visualization_msgs::Marker getCylinderMarker(int id, SimpleVector3 position, double w, SimpleVector3 scale, SimpleVector4 color, std::string ns="my_namespace")
returns a cylinder marker with the given settings
Definition: MarkerHelper.cpp:157
transform_datatypes.h
next_best_view::VisualizationHelper::mObjectMeshMarkerPublisher
ros::Publisher mObjectMeshMarkerPublisher
Definition: VisualizationsHelper.hpp:61
next_best_view::SamplePointCloudPtr
SamplePointCloud::Ptr SamplePointCloudPtr
Definition: typedef.hpp:93
boost::shared_ptr< MapHelper >
next_best_view::VisualizationHelper::mNodeHandle
ros::NodeHandle mNodeHandle
Definition: VisualizationsHelper.hpp:67
next_best_view::DefaultViewportPoint::getSimpleQuaternion
SimpleQuaternion getSimpleQuaternion() const
Definition: DefaultViewportPoint.hpp:138
next_best_view::VisualizationHelper
Definition: VisualizationsHelper.hpp:54
next_best_view::VisualizationHelper::createColorRGBA
static std_msgs::ColorRGBA createColorRGBA(float red, float green, float blue, float alpha)
Definition: VisualizationsHelper.hpp:901
next_best_view::VisualizationHelper::triggerFrustumObjectPointCloudVisualization
void triggerFrustumObjectPointCloudVisualization(ObjectPointCloud &frustumObjectPointCloud, std::map< std::string, std::string > &typeToMeshResource)
triggerFrustumObjectPointCloudVisualization shows only objects without hypothesis.
Definition: VisualizationsHelper.hpp:425
next_best_view::VisualizationHelper::mObjectMeshMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mObjectMeshMarkerArrayPtr
Definition: VisualizationsHelper.hpp:72
next_best_view::SimpleVector4
Eigen::Matrix< Precision, 4, 1 > SimpleVector4
Definition: typedef.hpp:55
next_best_view::VisualizationHelper::mIterationMarkerArrayPublisher
ros::Publisher mIterationMarkerArrayPublisher
Definition: VisualizationsHelper.hpp:59
next_best_view
this namespace contains all generally usable classes.
Definition: CameraModelFilter.hpp:26
next_best_view::VisualizationHelper::getObjectMarker
static visualization_msgs::Marker getObjectMarker(geometry_msgs::Pose pose, std::string type, std_msgs::ColorRGBA color, std::map< std::string, std::string > typeToMeshResource, int id, std::string ns)
Definition: VisualizationsHelper.hpp:872
next_best_view::VisualizationHelper::m_j
float m_j
Definition: VisualizationsHelper.hpp:83
next_best_view::VisualizationHelper::triggerObjectNormalsVisualization
void triggerObjectNormalsVisualization(ObjectPointCloud &objectPointCloud)
Definition: VisualizationsHelper.hpp:502
next_best_view::VisualizationHelper::mObjectNormalsMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mObjectNormalsMarkerArrayPtr
Definition: VisualizationsHelper.hpp:73
next_best_view::VisualizationHelper::deleteMarkerArray
static void deleteMarkerArray(visualization_msgs::MarkerArray::Ptr &array, ros::Publisher &publisher)
Definition: VisualizationsHelper.hpp:854
next_best_view::DebugHelper::getInstance
static boost::shared_ptr< DebugHelper > getInstance()
Definition: DebugHelper.cpp:29
next_best_view::VisualizationHelper::mDebugHelperPtr
DebugHelperPtr mDebugHelperPtr
Definition: VisualizationsHelper.hpp:80
next_best_view::MathHelper::getVisualAxis
static SimpleVector3 getVisualAxis(const SimpleQuaternion &orientation)
Definition: MathHelper.cpp:53
next_best_view::CropBoxPtr
CropBox::Ptr CropBoxPtr
Definition: typedef.hpp:106
next_best_view::VisualizationHelper::mOldFrustumMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mOldFrustumMarkerArrayPtr
Definition: VisualizationsHelper.hpp:71
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
next_best_view::VisualizationHelper::mFrustumObjectMeshMarkerPublisher
ros::Publisher mFrustumObjectMeshMarkerPublisher
Definition: VisualizationsHelper.hpp:62
next_best_view::VisualizationHelper::mSamplingPublisher
ros::Publisher mSamplingPublisher
Definition: VisualizationsHelper.hpp:65
next_best_view::VisualizationHelper::mCropBoxMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mCropBoxMarkerArrayPtr
Definition: VisualizationsHelper.hpp:76
next_best_view::VisualizationHelper::triggerFrustumsVisualization
void triggerFrustumsVisualization(CameraModelFilterPtr newCamera)
visualizes the frustum of the last camera that was given and the frustum of the new given camera...
Definition: VisualizationsHelper.hpp:274
next_best_view::DefaultViewportPoint::getPosition
SimpleVector3 getPosition() const
Definition: DefaultViewportPoint.hpp:134
next_best_view::VisualizationHelper::mIterationMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mIterationMarkerArrayPtr
Definition: VisualizationsHelper.hpp:69
next_best_view::VisualizationHelperPtr
boost::shared_ptr< VisualizationHelper > VisualizationHelperPtr
Definition: VisualizationsHelper.hpp:915
next_best_view::VisualizationHelper::getMeshColor
static std_msgs::ColorRGBA getMeshColor(std::string observedId)
Definition: VisualizationsHelper.hpp:518
next_best_view::Color
SimpleVector4 Color
Definition: typedef.hpp:130
next_best_view::VisualizationHelper::resetSamplingVisualization
void resetSamplingVisualization()
Definition: VisualizationsHelper.hpp:266
next_best_view::RealObjectPoint::getPosition
SimpleVector3 getPosition() const
Definition: RealObjectPoint.hpp:119
next_best_view::MarkerHelper::getArrowMarker
static visualization_msgs::Marker getArrowMarker(int id, SimpleVector3 startPoint, SimpleVector3 endPoint, SimpleVector3 scale=SimpleVector3(0.025, 0.05, 0.05), SimpleVector4 color=SimpleVector4(1.0, 0.0, 0.0, 1.0), std::string ns="my_namespace")
returns an arrow marker with the given settings
Definition: MarkerHelper.cpp:86
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
next_best_view::VisualizationHelper::triggerIterationVisualization
void triggerIterationVisualization(int iterationStep, const SimpleQuaternionCollectionPtr &sampledOrientationsPtr, ViewportPoint currentBestViewport, SamplePointCloudPtr pointcloud, SpaceSamplerPtr spaceSamplerPtr)
Definition: VisualizationsHelper.hpp:166
next_best_view::VisualizationHelper::mFrustumMarkerArrayPublisher
ros::Publisher mFrustumMarkerArrayPublisher
Definition: VisualizationsHelper.hpp:60
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
next_best_view::TypeHelper::getSimpleVector3
static SimpleVector3 getSimpleVector3(const geometry_msgs::Pose &pose)
Definition: TypeHelper.cpp:44
next_best_view::VisualizationHelper::VisualizationHelper
VisualizationHelper(MapHelperPtr mapHelperPtr)
Definition: VisualizationsHelper.hpp:93
next_best_view::SimpleVector3Collection
std::vector< SimpleVector3, Eigen::aligned_allocator< SimpleVector3 > > SimpleVector3Collection
Definition: typedef.hpp:60
next_best_view::VisualizationHelper::triggerOldFrustumVisualization
void triggerOldFrustumVisualization(CameraModelFilterPtr camera=NULL)
Definition: VisualizationsHelper.hpp:322
next_best_view::MarkerHelper::getLineListMarker
static visualization_msgs::Marker getLineListMarker(int id, std::vector< SimpleVector3 > points, double scale, SimpleVector4 color, std::string ns="my_namespace")
returns a line list marker with the given settings
Definition: MarkerHelper.cpp:141
next_best_view::VisualizationHelper::mutex
boost::mutex mutex
Definition: VisualizationsHelper.hpp:89
next_best_view::VisualizationHelper::triggerNewFrustumVisualization
void triggerNewFrustumVisualization(CameraModelFilterPtr newCamera)
Definition: VisualizationsHelper.hpp:279
next_best_view::TypeHelper::getSimpleQuaternion
static SimpleQuaternion getSimpleQuaternion(const geometry_msgs::Pose &pose)
Definition: TypeHelper.cpp:64
next_best_view::VisualizationHelper::mBoolClearBetweenIterations
bool mBoolClearBetweenIterations
Definition: VisualizationsHelper.hpp:85
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
next_best_view::SimpleQuaternion
Eigen::Quaternion< Precision > SimpleQuaternion
Definition: typedef.hpp:67
next_best_view::RealObjectPoint
RealObjectPoint.
Definition: RealObjectPoint.hpp:56
next_best_view::VisualizationHelper::mNewFrustumMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mNewFrustumMarkerArrayPtr
Definition: VisualizationsHelper.hpp:70
ROS_ERROR
#define ROS_ERROR(...)
next_best_view::RealObjectPoint::active_normal_vectors
IndicesPtr active_normal_vectors
Definition: RealObjectPoint.hpp:73
next_best_view::VisualizationHelper::mIterationStep
int mIterationStep
Definition: VisualizationsHelper.hpp:84
next_best_view::VisualizationHelper::mFrustumObjectMeshMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mFrustumObjectMeshMarkerArrayPtr
Definition: VisualizationsHelper.hpp:74
next_best_view::VisualizationHelper::visualizePointCloudNormals
static void visualizePointCloudNormals(ObjectPointCloud &objectPointCloud, visualization_msgs::MarkerArray::Ptr objectNormalsMarkerArrayPtr, ros::Publisher &objectNormalsPublisher)
visualizePointCloudNormals visualizes all hypothesis of each object.
Definition: VisualizationsHelper.hpp:817
next_best_view::VisualizationHelper::visualizePointCloudObjects
static void visualizePointCloudObjects(ObjectPointCloud &objectPointCloud, std::map< std::string, std::string > &typeToMeshResource, visualization_msgs::MarkerArray::Ptr objectMarkerArrayPtr, ros::Publisher &objectPublisher, std_msgs::ColorRGBA::Ptr objectColorPtr=NULL)
visualizePointCloudObjects visualizes objects of objectPointCloud, using typeToMeshResource to get me...
Definition: VisualizationsHelper.hpp:776
next_best_view::MarkerHelper::getSphereMarker
static visualization_msgs::Marker getSphereMarker(int id, SimpleVector3 position, SimpleVector3 scale, SimpleVector4 color, std::string ns="my_namespace")
returns a sphere marker with the given settings
Definition: MarkerHelper.cpp:132
next_best_view::MarkerHelper::getMeshMarker
static visualization_msgs::Marker getMeshMarker(int id, std::string meshResource, SimpleVector3 centroid, SimpleQuaternion quaternion, SimpleVector3 scale=SimpleVector3(0.001, 0.001, 0.001), std::string ns="my_namespace")
returns a mesh marker with the given settings
Definition: MarkerHelper.cpp:72
next_best_view::VisualizationHelper::mSamplingMarkerArrayPtr
visualization_msgs::MarkerArray::Ptr mSamplingMarkerArrayPtr
Definition: VisualizationsHelper.hpp:77
next_best_view::VisualizationHelper::clearFrustumVisualization
void clearFrustumVisualization()
Definition: VisualizationsHelper.hpp:382

asr_next_best_view
Author(s): Aumann Florian, Borella Jocelyn, Heller Florian, Meißner Pascal, Schleicher Ralf, Stöckle Patrick, Stroh Daniel, Trautmann Jeremias, Walter Milena, Wittenbeck Valerij
autogenerated on Thu Jan 9 2020 07:20:18