$search
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: pcl_histogram_visualizer.hpp 31709 2010-08-11 08:11:54Z rusu $ 00035 * 00036 */ 00037 00039 template <typename PointT> bool 00040 pcl_visualization::PCLHistogramVisualizer::addFeatureHistogram ( 00041 const pcl::PointCloud<PointT> &cloud, int hsize, 00042 const std::string &id, int win_width, int win_height) 00043 { 00044 RenWinInteractMap::iterator am_it = wins_.find (id); 00045 if (am_it != wins_.end ()) 00046 { 00047 pcl::console::print_warn ("[addFeatureHistogram] A window with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00048 return (false); 00049 } 00050 00051 // Create the actor 00052 RenWinInteract renwinint; 00053 renwinint.xy_plot_->SetDataObjectPlotModeToColumns (); 00054 renwinint.xy_plot_->SetXValuesToValue (); 00055 00056 vtkSmartPointer<vtkDoubleArray> xy_array = vtkSmartPointer<vtkDoubleArray>::New (); 00057 xy_array->SetNumberOfComponents (2); 00058 xy_array->SetNumberOfTuples (hsize); 00059 00060 // Parse the cloud data and store it in the array 00061 double xy[2]; 00062 for (int d = 0; d < hsize; ++d) 00063 { 00064 xy[0] = d; 00065 xy[1] = cloud.points[0].histogram[d]; 00066 xy_array->SetTuple (d, xy); 00067 } 00068 double min_max[2]; 00069 xy_array->GetRange (min_max, 1); 00070 00071 // Create the data structures 00072 vtkSmartPointer<vtkFieldData> field_values = vtkSmartPointer<vtkFieldData>::New (); 00073 field_values->AddArray (xy_array); 00074 00075 vtkSmartPointer<vtkDataObject> field_data = vtkSmartPointer<vtkDataObject>::New (); 00076 field_data->SetFieldData (field_values); 00077 00078 renwinint.xy_plot_->AddDataObjectInput (field_data); 00079 // Set the plot color 00080 renwinint.xy_plot_->SetPlotColor (0, 1.0, 0.0, 0.0); 00081 00082 renwinint.xy_plot_->SetDataObjectXComponent (0, 0); renwinint.xy_plot_->SetDataObjectYComponent (0, 1); 00083 renwinint.xy_plot_->PlotPointsOn (); 00084 //renwinint.xy_plot_->PlotCurvePointsOn (); 00085 //renwinint.xy_plot_->PlotLinesOn (); 00086 renwinint.xy_plot_->PlotCurveLinesOn (); 00087 00088 renwinint.xy_plot_->SetYTitle (""); renwinint.xy_plot_->SetXTitle (""); 00089 renwinint.xy_plot_->SetYRange (min_max[0], min_max[1]); 00090 renwinint.xy_plot_->SetXRange (0, hsize - 1); 00091 00092 //renwinint.xy_plot_->SetTitle (id.c_str ()); 00093 renwinint.xy_plot_->GetProperty ()->SetColor (0, 0, 0); 00094 00095 // Adjust text properties 00096 vtkSmartPointer<vtkTextProperty> tprop = renwinint.xy_plot_->GetTitleTextProperty (); 00097 renwinint.xy_plot_->AdjustTitlePositionOn (); 00098 tprop->SetFontSize (8); 00099 tprop->ShadowOff (); tprop->ItalicOff (); 00100 tprop->SetColor (renwinint.xy_plot_->GetProperty ()->GetColor ()); 00101 00102 renwinint.xy_plot_->SetAxisLabelTextProperty (tprop); 00103 renwinint.xy_plot_->SetAxisTitleTextProperty (tprop); 00104 renwinint.xy_plot_->SetNumberOfXLabels (8); 00105 renwinint.xy_plot_->GetProperty ()->SetPointSize (3); 00106 renwinint.xy_plot_->GetProperty ()->SetLineWidth (2); 00107 00108 renwinint.xy_plot_->SetPosition (0, 0); 00109 renwinint.xy_plot_->SetWidth (1); renwinint.xy_plot_->SetHeight (1); 00110 00111 // Create the new window with its interactor and renderer 00112 renwinint.ren_->AddActor2D (renwinint.xy_plot_); 00113 renwinint.ren_->SetBackground (1, 1, 1); 00114 renwinint.win_->SetWindowName (id.c_str ()); 00115 renwinint.win_->AddRenderer (renwinint.ren_); 00116 renwinint.win_->SetSize (win_width, win_height); 00117 renwinint.win_->SetBorders (1); 00118 00119 // Create the interactor style 00120 vtkSmartPointer<pcl_visualization::PCLHistogramVisualizerInteractorStyle> style_ = vtkSmartPointer<pcl_visualization::PCLHistogramVisualizerInteractorStyle>::New (); 00121 style_->Initialize (); 00122 renwinint.style_ = style_; 00123 renwinint.style_->UseTimersOn (); 00124 renwinint.interactor_ = vtkSmartPointer<PCLVisualizerInteractor>::New (); 00125 renwinint.interactor_->SetRenderWindow (renwinint.win_); 00126 renwinint.interactor_->SetInteractorStyle (renwinint.style_); 00127 // Initialize and create timer 00128 renwinint.interactor_->Initialize (); 00129 renwinint.interactor_->CreateRepeatingTimer (5000L); 00130 00131 exit_main_loop_timer_callback_->right_timer_id = -1; 00132 renwinint.interactor_->AddObserver (vtkCommand::TimerEvent, exit_main_loop_timer_callback_); 00133 00134 renwinint.interactor_->AddObserver (vtkCommand::ExitEvent, exit_callback_); 00135 00136 // Save the pointer/ID pair to the global window map 00137 wins_[id] = renwinint; 00138 00139 resetStoppedFlag (); 00140 return (true); 00141 } 00142 00143