00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <terminal_tools/time.h>
00039 #include <pcl/win32_macros.h>
00040
00042
00045 template <typename PointT> void
00046 pcl_visualization::PointCloudColorHandlerCustom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00047 {
00048 if (!capable_)
00049 return;
00050
00051 if (!scalars)
00052 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00053 scalars->SetNumberOfComponents (3);
00054
00055 vtkIdType nr_points = cloud_->points.size ();
00056 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00057
00058
00059 unsigned char* colors = new unsigned char[nr_points * 3];
00060
00061
00062 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00063 {
00064 colors[cp * 3 + 0] = r_;
00065 colors[cp * 3 + 1] = g_;
00066 colors[cp * 3 + 2] = b_;
00067 }
00068 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00069 }
00070
00072
00075 template <typename PointT> void
00076 pcl_visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00077 {
00078 if (!capable_)
00079 return;
00080
00081 if (!scalars)
00082 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00083 scalars->SetNumberOfComponents (3);
00084
00085 vtkIdType nr_points = cloud_->points.size ();
00086 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00087
00088
00089 unsigned char* colors = new unsigned char[nr_points * 3];
00090 double r, g, b;
00091 pcl_visualization::getRandomColors (r, g, b);
00092
00093 int r_ = lrint (r * 255.0), g_ = lrint (g * 255.0), b_ = lrint (b * 255.0);
00094
00095
00096 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00097 {
00098 colors[cp * 3 + 0] = r_;
00099 colors[cp * 3 + 1] = g_;
00100 colors[cp * 3 + 2] = b_;
00101 }
00102 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00103 }
00104
00106
00107 template <typename PointT>
00108 pcl_visualization::PointCloudColorHandlerRGBField<PointT>::PointCloudColorHandlerRGBField (const typename pcl_visualization::PointCloudColorHandler<PointT>::PointCloud &cloud) :
00109 pcl_visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
00110 {
00111
00112 field_idx_ = pcl::getFieldIndex (cloud, "rgb", fields_);
00113 if (field_idx_ != -1)
00114 capable_ = true;
00115 else
00116 capable_ = false;
00117 }
00118
00120
00123 template <typename PointT> void
00124 pcl_visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00125 {
00126 if (!capable_)
00127 return;
00128
00129 if (!scalars)
00130 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00131 scalars->SetNumberOfComponents (3);
00132
00133 vtkIdType nr_points = cloud_->points.size ();
00134 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00135
00136
00137 unsigned char* colors = new unsigned char[nr_points * 3];
00138
00139 int j = 0;
00140
00141 int x_idx = -1;
00142 for (size_t d = 0; d < fields_.size (); ++d)
00143 if (fields_[d].name == "x")
00144 x_idx = d;
00145
00146 if (x_idx != -1)
00147 {
00148
00149 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00150 {
00151
00152 if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].rgb))
00153 continue;
00154
00155
00156 if (!pcl_isfinite (cloud_->points[cp].rgb))
00157 continue;
00158
00159 int rgb = *reinterpret_cast<int*>(&cloud_->points[cp].rgb);
00160 colors[j * 3 + 0] = ((rgb >> 16) & 0xff);
00161 colors[j * 3 + 1] = ((rgb >> 8) & 0xff);
00162 colors[j * 3 + 2] = (rgb & 0xff);
00163 j++;
00164 }
00165 }
00166 else
00167 {
00168
00169 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00170 {
00171
00172 if (!pcl_isfinite (cloud_->points[cp].rgb))
00173 continue;
00174
00175 int rgb = *reinterpret_cast<int*>(&cloud_->points[cp].rgb);
00176 colors[j * 3 + 0] = ((rgb >> 16) & 0xff);
00177 colors[j * 3 + 1] = ((rgb >> 8) & 0xff);
00178 colors[j * 3 + 2] = (rgb & 0xff);
00179 j++;
00180 }
00181 }
00182 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * j, 0);
00183
00184 }
00185
00187
00188 template <typename PointT>
00189 pcl_visualization::PointCloudColorHandlerGenericField<PointT>::PointCloudColorHandlerGenericField (const typename pcl_visualization::PointCloudColorHandler<PointT>::PointCloud &cloud,
00190 const std::string &field_name) : pcl_visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
00191 {
00192 field_name_ = field_name;
00193 field_idx_ = pcl::getFieldIndex (cloud, field_name, fields_);
00194 if (field_idx_ != -1)
00195 capable_ = true;
00196 else
00197 capable_ = false;
00198 }
00199
00201
00204 template <typename PointT> void
00205 pcl_visualization::PointCloudColorHandlerGenericField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00206 {
00207 if (!capable_)
00208 return;
00209
00210 if (!scalars)
00211 scalars = vtkSmartPointer<vtkFloatArray>::New ();
00212 scalars->SetNumberOfComponents (1);
00213
00214 vtkIdType nr_points = cloud_->points.size ();
00215 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00216
00217 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00218
00219 float* colors = new float[nr_points];
00220 float field_data;
00221
00222 int j = 0;
00223
00224 int x_idx = -1;
00225 for (size_t d = 0; d < fields_.size (); ++d)
00226 if (fields_[d].name == "x")
00227 x_idx = d;
00228
00229 if (x_idx != -1)
00230 {
00231
00232 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00233 {
00234
00235 if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z))
00236 continue;
00237
00238 uint8_t* pt_data = (uint8_t*)&cloud_->points[cp];
00239 memcpy (&field_data, pt_data + fields_[field_idx_].offset, sizeof (float));
00240
00241 if (!pcl_isfinite (field_data))
00242 continue;
00243
00244 colors[j] = field_data;
00245 j++;
00246 }
00247 }
00248 else
00249 {
00250
00251 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00252 {
00253 uint8_t* pt_data = (uint8_t*)&cloud_->points[cp];
00254 memcpy (&field_data, pt_data + fields_[field_idx_].offset, sizeof (float));
00255
00256 if (!pcl_isfinite (field_data))
00257 continue;
00258
00259 colors[j] = field_data;
00260 j++;
00261 }
00262 }
00263 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
00264 }
00265
00267
00268 template <typename PointT>
00269 pcl_visualization::PointCloudGeometryHandlerXYZ<PointT>::PointCloudGeometryHandlerXYZ (const typename pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloud &cloud)
00270 : pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
00271 {
00272 field_x_idx_ = pcl::getFieldIndex (cloud, "x", fields_);
00273 if (field_x_idx_ == -1)
00274 return;
00275 field_y_idx_ = pcl::getFieldIndex (cloud, "y", fields_);
00276 if (field_y_idx_ == -1)
00277 return;
00278 field_z_idx_ = pcl::getFieldIndex (cloud, "z", fields_);
00279 if (field_z_idx_ == -1)
00280 return;
00281 capable_ = true;
00282 }
00283
00285
00288 template <typename PointT> void
00289 pcl_visualization::PointCloudGeometryHandlerXYZ<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00290 {
00291 if (!capable_)
00292 return;
00293
00294 if (!points)
00295 points = vtkSmartPointer<vtkPoints>::New ();
00296 points->SetDataTypeToFloat ();
00297
00298 vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
00299 data->SetNumberOfComponents (3);
00300 vtkIdType nr_points = cloud_->points.size ();
00301
00302
00303 vtkIdType j = 0;
00304 float* pts = new float[nr_points * 3];
00305
00306
00307 if (cloud_->is_dense)
00308 {
00309 for (vtkIdType i = 0; i < nr_points; ++i)
00310 {
00311 pts[i * 3 + 0] = cloud_->points[i].x;
00312 pts[i * 3 + 1] = cloud_->points[i].y;
00313 pts[i * 3 + 2] = cloud_->points[i].z;
00314 }
00315 data->SetArray (&pts[0], nr_points * 3, 0);
00316 points->SetData (data);
00317 }
00318
00319 else
00320 {
00321 for (vtkIdType i = 0; i < nr_points; ++i)
00322 {
00323
00324 if (!pcl_isfinite (cloud_->points[i].x) || !pcl_isfinite (cloud_->points[i].y) || !pcl_isfinite (cloud_->points[i].z))
00325 continue;
00326
00327 pts[j * 3 + 0] = cloud_->points[i].x;
00328 pts[j * 3 + 1] = cloud_->points[i].y;
00329 pts[j * 3 + 2] = cloud_->points[i].z;
00330
00331 j++;
00332 }
00333 data->SetArray (&pts[0], j * 3, 0);
00334 points->SetData (data);
00335 }
00336 }
00337
00339
00340 template <typename PointT>
00341 pcl_visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::PointCloudGeometryHandlerSurfaceNormal (const typename pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloud &cloud)
00342 : pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
00343 {
00344 field_x_idx_ = pcl::getFieldIndex (cloud, "normal_x", fields_);
00345 if (field_x_idx_ == -1)
00346 return;
00347 field_y_idx_ = pcl::getFieldIndex (cloud, "normal_y", fields_);
00348 if (field_y_idx_ == -1)
00349 return;
00350 field_z_idx_ = pcl::getFieldIndex (cloud, "normal_z", fields_);
00351 if (field_z_idx_ == -1)
00352 return;
00353 capable_ = true;
00354 }
00355
00357
00360 template <typename PointT> void
00361 pcl_visualization::PointCloudGeometryHandlerSurfaceNormal<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00362 {
00363 if (!capable_)
00364 return;
00365
00366 if (!points)
00367 points = vtkSmartPointer<vtkPoints>::New ();
00368 points->SetDataTypeToFloat ();
00369 points->SetNumberOfPoints (cloud_->points.size ());
00370
00371
00372 double p[3];
00373 for (vtkIdType i = 0; i < (int)cloud_->points.size (); ++i)
00374 {
00375 p[0] = cloud_->points[i].normal[0];
00376 p[1] = cloud_->points[i].normal[1];
00377 p[2] = cloud_->points[i].normal[2];
00378
00379 points->SetPoint (i, p);
00380 }
00381 }
00382
00384
00385 template <typename PointT>
00386 pcl_visualization::PointCloudGeometryHandlerCustom<PointT>::PointCloudGeometryHandlerCustom (const typename pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloud &cloud,
00387 const std::string &x_field_name, const std::string &y_field_name, const std::string &z_field_name) : pcl_visualization::PointCloudGeometryHandler<PointT>::PointCloudGeometryHandler (cloud)
00388 {
00389 field_x_idx_ = pcl::getFieldIndex (cloud, x_field_name, fields_);
00390 if (field_x_idx_ == -1)
00391 return;
00392 field_y_idx_ = pcl::getFieldIndex (cloud, y_field_name, fields_);
00393 if (field_y_idx_ == -1)
00394 return;
00395 field_z_idx_ = pcl::getFieldIndex (cloud, z_field_name, fields_);
00396 if (field_z_idx_ == -1)
00397 return;
00398 field_name_ = x_field_name + y_field_name + z_field_name;
00399 capable_ = true;
00400 }
00401
00403
00406 template <typename PointT> void
00407 pcl_visualization::PointCloudGeometryHandlerCustom<PointT>::getGeometry (vtkSmartPointer<vtkPoints> &points) const
00408 {
00409 if (!capable_)
00410 return;
00411
00412 if (!points)
00413 points = vtkSmartPointer<vtkPoints>::New ();
00414 points->SetDataTypeToFloat ();
00415 points->SetNumberOfPoints (cloud_->points.size ());
00416
00417 float data;
00418
00419 double p[3];
00420 for (vtkIdType i = 0; i < (int)cloud_->points.size (); ++i)
00421 {
00422
00423 uint8_t* pt_data = (uint8_t*)&cloud_->points[i];
00424 memcpy (&data, pt_data + fields_[field_x_idx_].offset, sizeof (float));
00425 p[0] = data;
00426
00427 memcpy (&data, pt_data + fields_[field_y_idx_].offset, sizeof (float));
00428 p[1] = data;
00429
00430 memcpy (&data, pt_data + fields_[field_z_idx_].offset, sizeof (float));
00431 p[2] = data;
00432
00433 points->SetPoint (i, p);
00434 }
00435 }
00436