i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

CloudViewer: showing intensity as grayscale instead of generic color handler

Browse Source
This commit is contained in:
matlabbe
2019-09-24 18:18:49 -04:00
parent aeb2804da8
commit 547da3f3ce
1 changed files with 117 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

121
guilib/src/CloudViewer.cpp
View File

@@ -453,6 +453,119 @@ bool CloudViewer::updateCloudPose(
return false;
}
class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
{
typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
typedef PointCloud::Ptr PointCloudPtr;
typedef PointCloud::ConstPtr PointCloudConstPtr;
public:
typedef boost::shared_ptr<PointCloudColorHandlerIntensityField > Ptr;
typedef boost::shared_ptr<const PointCloudColorHandlerIntensityField > ConstPtr;
/** \brief Constructor. */
PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud) :
pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud)
{
field_idx_ = pcl::getFieldIndex (*cloud, "intensity");
if (field_idx_ != -1)
capable_ = true;
else
capable_ = false;
}
/** \brief Empty destructor */
virtual ~PointCloudColorHandlerIntensityField () {}
/** \brief Obtain the actual color for the input dataset as vtk scalars.
* \param[out] scalars the output scalars containing the color for the dataset
* \return true if the operation was successful (the handler is capable and
* the input cloud was given as a valid pointer), false otherwise
*/
virtual bool
getColor (vtkSmartPointer<vtkDataArray> &scalars) const
{
if (!capable_ || !cloud_)
return (false);
if (!scalars)
scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
scalars->SetNumberOfComponents (3);
vtkIdType nr_points = cloud_->width * cloud_->height;
// Allocate enough memory to hold all colors
float * intensities = new float[nr_points];
float intensity;
size_t point_offset = cloud_->fields[field_idx_].offset;
size_t j = 0;
// If XYZ present, check if the points are invalid
int x_idx = pcl::getFieldIndex (*cloud_, "x");
if (x_idx != -1)
{
float x_data, y_data, z_data;
size_t x_point_offset = cloud_->fields[x_idx].offset;
// Color every point
for (vtkIdType cp = 0; cp < nr_points; ++cp,
point_offset += cloud_->point_step,
x_point_offset += cloud_->point_step)
{
// Copy the value at the specified field
memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float));
memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float));
if (!std::isfinite (x_data) || !std::isfinite (y_data) || !std::isfinite (z_data))
continue;
intensities[j++] = intensity;
}
}
// No XYZ data checks
else
{
// Color every point
for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
{
// Copy the value at the specified field
memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
intensities[j++] = intensity;
}
}
if (j != 0)
{
// Allocate enough memory to hold all colors
unsigned char* colors = new unsigned char[j * 3];
float min, max;
uMinMax(intensities, j, min, max);
for(size_t k=0; k<j; ++k)
{
colors[k*3+0] = colors[k*3+1] = colors[k*3+2] = max>0?(unsigned char)(intensities[k]/max*255.0f):0;
}
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (j);
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
}
else
reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
//delete [] colors;
delete [] intensities;
return (true);
}
protected:
/** \brief Get the name of the class. */
virtual std::string
getName () const { return ("PointCloudColorHandlerIntensityField"); }
/** \brief Get the name of the field used. */
virtual std::string
getFieldName () const { return ("intensity"); }
};
bool CloudViewer::addCloud(
const std::string & id,
const pcl::PCLPointCloud2Ptr & binaryCloud,
@@ -513,8 +626,8 @@ bool CloudViewer::addCloud(
}
else if(hasIntensity)
{
//rgb
colorHandler.reset(new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2>(binaryCloud, "intensity"));
//intensity
colorHandler.reset(new PointCloudColorHandlerIntensityField(binaryCloud));
_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
}
else if(previousColorIndex == 5)
@@ -1140,8 +1253,8 @@ bool CloudViewer::addTextureMesh (
vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
if (!tex_manager)
return (false);
if (!tex_manager)
return (false);
vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
// fill vtkTexture from pcl::TexMaterial structure