cloud collection widget implementation

modules/viz/include/opencv2/viz/widgets.hpp

@@ -202,6 +202,18 @@ namespace cv
             struct CreateCloudWidget;
         };
 
+        class CV_EXPORTS CloudCollectionWidget : public Widget3D
+        {
+        public:
+            CloudCollectionWidget();
+            
+            void addCloud(InputArray cloud, InputArray colors, const Affine3f &pose = Affine3f::Identity());
+            void addCloud(InputArray cloud, const Color &color = Color::white(), const Affine3f &pose = Affine3f::Identity());
+            
+        private:
+            struct CreateCloudWidget;
+        };
+        
         class CV_EXPORTS CloudNormalsWidget : public Widget3D
         {
         public:
@@ -239,6 +251,7 @@ namespace cv
         template<> CV_EXPORTS CameraPositionWidget Widget::cast<CameraPositionWidget>();
         template<> CV_EXPORTS TrajectoryWidget Widget::cast<TrajectoryWidget>();
         template<> CV_EXPORTS CloudWidget Widget::cast<CloudWidget>();
+        template<> CV_EXPORTS CloudCollectionWidget Widget::cast<CloudCollectionWidget>();
         template<> CV_EXPORTS CloudNormalsWidget Widget::cast<CloudNormalsWidget>();
         template<> CV_EXPORTS MeshWidget Widget::cast<MeshWidget>();
 

modules/viz/src/shape_widgets.cpp

@@ -1204,10 +1204,10 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector<Affine3f> &path, i
         // Set the color for polyData
         vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
         colors->SetNumberOfComponents(3);
-        
-        // TODO Make this more efficient
-        for (int i = 0; i < nr_points; ++i)
-            colors->InsertNextTuple3(color[2], color[1], color[0]);
+        colors->SetNumberOfTuples(nr_points);
+        colors->FillComponent(0, color[2]);
+        colors->FillComponent(1, color[1]);
+        colors->FillComponent(2, color[0]);
         
         polyData->GetPointData()->SetScalars(colors);
         appendFilter->AddInputConnection(polyData->GetProducerPort());