Go to the documentation of this file.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 #ifndef PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
00039 #define PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
00040
00041 #include <pcl/pcl_macros.h>
00042
00044 template <typename PointT> bool
00045 pcl::visualization::PointCloudColorHandlerCustom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00046 {
00047 if (!capable_ || !cloud_)
00048 return (false);
00049
00050 if (!scalars)
00051 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00052 scalars->SetNumberOfComponents (3);
00053
00054 vtkIdType nr_points = cloud_->points.size ();
00055 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00056
00057
00058 unsigned char* colors = new unsigned char[nr_points * 3];
00059
00060
00061 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00062 {
00063 colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
00064 colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
00065 colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
00066 }
00067 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00068 return (true);
00069 }
00070
00072 template <typename PointT> bool
00073 pcl::visualization::PointCloudColorHandlerRandom<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00074 {
00075 if (!capable_ || !cloud_)
00076 return (false);
00077
00078 if (!scalars)
00079 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00080 scalars->SetNumberOfComponents (3);
00081
00082 vtkIdType nr_points = cloud_->points.size ();
00083 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00084
00085
00086 unsigned char* colors = new unsigned char[nr_points * 3];
00087 double r, g, b;
00088 pcl::visualization::getRandomColors (r, g, b);
00089
00090 int r_ = static_cast<int> (pcl_lrint (r * 255.0)),
00091 g_ = static_cast<int> (pcl_lrint (g * 255.0)),
00092 b_ = static_cast<int> (pcl_lrint (b * 255.0));
00093
00094
00095 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00096 {
00097 colors[cp * 3 + 0] = static_cast<unsigned char> (r_);
00098 colors[cp * 3 + 1] = static_cast<unsigned char> (g_);
00099 colors[cp * 3 + 2] = static_cast<unsigned char> (b_);
00100 }
00101 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, 3 * nr_points, 0);
00102 return (true);
00103 }
00104
00106 template <typename PointT> void
00107 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::setInputCloud (
00108 const PointCloudConstPtr &cloud)
00109 {
00110 PointCloudColorHandler<PointT>::setInputCloud (cloud);
00111
00112 field_idx_ = pcl::getFieldIndex (*cloud, "rgb", fields_);
00113 if (field_idx_ != -1)
00114 {
00115 capable_ = true;
00116 return;
00117 }
00118 else
00119 {
00120 field_idx_ = pcl::getFieldIndex (*cloud, "rgba", fields_);
00121 if (field_idx_ != -1)
00122 capable_ = true;
00123 else
00124 capable_ = false;
00125 }
00126 }
00127
00129 template <typename PointT> bool
00130 pcl::visualization::PointCloudColorHandlerRGBField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00131 {
00132 if (!capable_ || !cloud_)
00133 return (false);
00134
00135 if (!scalars)
00136 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00137 scalars->SetNumberOfComponents (3);
00138
00139 vtkIdType nr_points = cloud_->points.size ();
00140 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00141 unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
00142
00143 int j = 0;
00144
00145 int x_idx = -1;
00146 for (size_t d = 0; d < fields_.size (); ++d)
00147 if (fields_[d].name == "x")
00148 x_idx = static_cast<int> (d);
00149
00150 if (x_idx != -1)
00151 {
00152
00153 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00154 {
00155
00156 if (!pcl_isfinite (cloud_->points[cp].x) ||
00157 !pcl_isfinite (cloud_->points[cp].y) ||
00158 !pcl_isfinite (cloud_->points[cp].z))
00159 continue;
00160
00161 colors[j ] = cloud_->points[cp].r;
00162 colors[j + 1] = cloud_->points[cp].g;
00163 colors[j + 2] = cloud_->points[cp].b;
00164 j += 3;
00165 }
00166 }
00167 else
00168 {
00169
00170 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00171 {
00172 int idx = static_cast<int> (cp) * 3;
00173 colors[idx ] = cloud_->points[cp].r;
00174 colors[idx + 1] = cloud_->points[cp].g;
00175 colors[idx + 2] = cloud_->points[cp].b;
00176 }
00177 }
00178 return (true);
00179 }
00180
00182 template <typename PointT>
00183 pcl::visualization::PointCloudColorHandlerHSVField<PointT>::PointCloudColorHandlerHSVField (const PointCloudConstPtr &cloud) :
00184 pcl::visualization::PointCloudColorHandler<PointT>::PointCloudColorHandler (cloud)
00185 {
00186
00187 field_idx_ = pcl::getFieldIndex (*cloud, "h", fields_);
00188 if (field_idx_ == -1)
00189 {
00190 capable_ = false;
00191 return;
00192 }
00193
00194
00195 s_field_idx_ = pcl::getFieldIndex (*cloud, "s", fields_);
00196 if (s_field_idx_ == -1)
00197 {
00198 capable_ = false;
00199 return;
00200 }
00201
00202
00203 v_field_idx_ = pcl::getFieldIndex (*cloud, "v", fields_);
00204 if (v_field_idx_ == -1)
00205 {
00206 capable_ = false;
00207 return;
00208 }
00209 capable_ = true;
00210 }
00211
00213 template <typename PointT> bool
00214 pcl::visualization::PointCloudColorHandlerHSVField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00215 {
00216 if (!capable_ || !cloud_)
00217 return (false);
00218
00219 if (!scalars)
00220 scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00221 scalars->SetNumberOfComponents (3);
00222
00223 vtkIdType nr_points = cloud_->points.size ();
00224 reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00225 unsigned char* colors = reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->GetPointer (0);
00226
00227 int j = 0;
00228
00229 int x_idx = -1;
00230
00231 for (size_t d = 0; d < fields_.size (); ++d)
00232 if (fields_[d].name == "x")
00233 x_idx = static_cast<int> (d);
00234
00235 if (x_idx != -1)
00236 {
00237
00238 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00239 {
00240
00241 if (!pcl_isfinite (cloud_->points[cp].x) ||
00242 !pcl_isfinite (cloud_->points[cp].y) ||
00243 !pcl_isfinite (cloud_->points[cp].z))
00244 continue;
00245
00246 int idx = j * 3;
00247
00249
00250
00251 if (cloud_->points[cp].s == 0)
00252 {
00253 colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v;
00254 return;
00255 }
00256 float a = cloud_->points[cp].h / 60;
00257 int i = floor (a);
00258 float f = a - i;
00259 float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s);
00260 float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f);
00261 float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f));
00262
00263 switch (i)
00264 {
00265 case 0:
00266 colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break;
00267 case 1:
00268 colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break;
00269 case 2:
00270 colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break;
00271 case 3:
00272 colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break;
00273 case 4:
00274 colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break;
00275 default:
00276 colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break;
00277 }
00278 j++;
00279 }
00280 }
00281 else
00282 {
00283
00284 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00285 {
00286 int idx = cp * 3;
00287
00288
00289 if (cloud_->points[cp].s == 0)
00290 {
00291 colors[idx] = colors[idx+1] = colors[idx+2] = cloud_->points[cp].v;
00292 return;
00293 }
00294 float a = cloud_->points[cp].h / 60;
00295 int i = floor (a);
00296 float f = a - i;
00297 float p = cloud_->points[cp].v * (1 - cloud_->points[cp].s);
00298 float q = cloud_->points[cp].v * (1 - cloud_->points[cp].s * f);
00299 float t = cloud_->points[cp].v * (1 - cloud_->points[cp].s * (1 - f));
00300
00301 switch (i)
00302 {
00303 case 0:
00304 colors[idx] = cloud_->points[cp].v; colors[idx+1] = t; colors[idx+2] = p; break;
00305 case 1:
00306 colors[idx] = q; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = p; break;
00307 case 2:
00308 colors[idx] = p; colors[idx+1] = cloud_->points[cp].v; colors[idx+2] = t; break;
00309 case 3:
00310 colors[idx] = p; colors[idx+1] = q; colors[idx+2] = cloud_->points[cp].v; break;
00311 case 4:
00312 colors[idx] = t; colors[idx+1] = p; colors[idx+2] = cloud_->points[cp].v; break;
00313 default:
00314 colors[idx] = cloud_->points[cp].v; colors[idx+1] = p; colors[idx+2] = q; break;
00315 }
00316 }
00317 }
00318 return (true);
00319 }
00320
00322 template <typename PointT> void
00323 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::setInputCloud (
00324 const PointCloudConstPtr &cloud)
00325 {
00326 PointCloudColorHandler<PointT>::setInputCloud (cloud);
00327 field_idx_ = pcl::getFieldIndex (*cloud, field_name_, fields_);
00328 if (field_idx_ != -1)
00329 capable_ = true;
00330 else
00331 capable_ = false;
00332 }
00333
00335 template <typename PointT> bool
00336 pcl::visualization::PointCloudColorHandlerGenericField<PointT>::getColor (vtkSmartPointer<vtkDataArray> &scalars) const
00337 {
00338 if (!capable_ || !cloud_)
00339 return (false);
00340
00341 if (!scalars)
00342 scalars = vtkSmartPointer<vtkFloatArray>::New ();
00343 scalars->SetNumberOfComponents (1);
00344
00345 vtkIdType nr_points = cloud_->points.size ();
00346 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetNumberOfTuples (nr_points);
00347
00348 typedef typename pcl::traits::fieldList<PointT>::type FieldList;
00349
00350 float* colors = new float[nr_points];
00351 float field_data;
00352
00353 int j = 0;
00354
00355 int x_idx = -1;
00356 for (size_t d = 0; d < fields_.size (); ++d)
00357 if (fields_[d].name == "x")
00358 x_idx = static_cast<int> (d);
00359
00360 if (x_idx != -1)
00361 {
00362
00363 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00364 {
00365
00366 if (!pcl_isfinite (cloud_->points[cp].x) || !pcl_isfinite (cloud_->points[cp].y) || !pcl_isfinite (cloud_->points[cp].z))
00367 continue;
00368
00369 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
00370 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
00371
00372 colors[j] = field_data;
00373 j++;
00374 }
00375 }
00376 else
00377 {
00378
00379 for (vtkIdType cp = 0; cp < nr_points; ++cp)
00380 {
00381 const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&cloud_->points[cp]);
00382 memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype));
00383
00384 if (!pcl_isfinite (field_data))
00385 continue;
00386
00387 colors[j] = field_data;
00388 j++;
00389 }
00390 }
00391 reinterpret_cast<vtkFloatArray*>(&(*scalars))->SetArray (colors, j, 0);
00392 return (true);
00393 }
00394
00395 #endif // PCL_POINT_CLOUD_COLOR_HANDLERS_IMPL_HPP_
00396