Skip to content

Commit

Permalink
syncing trunk to pcl-1.x and pcl-1.1.x (apps, common)
Browse files Browse the repository at this point in the history
git-svn-id: svn+ssh://svn.pointclouds.org/pcl/branches/pcl-1.1.x@2319 a9d63959-f2ad-4865-b262-bf0e56cfafb6
  • Loading branch information
jspricke committed Aug 30, 2011
1 parent 8fa3866 commit 2d64332
Show file tree
Hide file tree
Showing 8 changed files with 93 additions and 69 deletions.
1 change: 0 additions & 1 deletion apps/src/openni_3d_concave_hull.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,6 @@
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/concave_hull.h>
Expand Down
2 changes: 1 addition & 1 deletion apps/src/openni_3d_convex_hull.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,12 +39,12 @@
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/openni_camera/openni_driver.h>
#include <pcl/filters/passthrough.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace pcl;
using namespace pcl::visualization;
Expand Down
5 changes: 2 additions & 3 deletions common/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@ if(build)
src/time_trigger.cpp
)

set(incs include/pcl/exceptions.h
set(incs
include/pcl/exceptions.h
include/pcl/pcl_base.h
include/pcl/pcl_macros.h
include/pcl/point_cloud.h
Expand All @@ -32,9 +33,7 @@ if(build)
include/pcl/PolygonMesh.h
include/pcl/Vertices.h
include/pcl/PointIndices.h
# Texture mesh
include/pcl/TextureMesh.h
# end
)

set(sensor_msg_incs
Expand Down
44 changes: 23 additions & 21 deletions common/include/pcl/TextureMesh.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -34,23 +36,25 @@
* $Id: TextureMesh.h 1003 2011-07-13 13:07:00 ktran $
*
*/
/** \author Khai Tran */

#ifndef TEXTUREMESH_H_
#define TEXTUREMESH_H_
#ifndef PCL_TEXTUREMESH_H_
#define PCL_TEXTUREMESH_H_

// Include the correct Header path here
#include <Eigen/Core>
#include <std_msgs/Header.h>
#include <pcl/PolygonMesh.h>
#include <string>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/Vertices.h>
#include "std_msgs/Header.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/Vertices.h"
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>

namespace pcl
{
/** \author Khai Tran */
struct RGB{
float r;
float g;
Expand All @@ -73,26 +77,24 @@ namespace pcl

struct TextureMesh
{
TextureMesh () : header (), cloud ()
{}
TextureMesh () : header (), cloud () {}

::std_msgs::Header header;
::sensor_msgs::PointCloud2 cloud;
std_msgs::Header header;
sensor_msgs::PointCloud2 cloud;

std::vector<std::vector< ::pcl::Vertices> > tex_polygons; // polygon which is mapped with specific texture defined in TexMaterial
std::vector<std::vector< ::pcl::PointXY> > tex_coordinates; // UV coordinates
std::vector<std::vector<TexMaterial> > tex_materials; // define texture material
std::vector<std::vector<pcl::Vertices> > tex_polygons; // polygon which is mapped with specific texture defined in TexMaterial
std::vector<std::vector<pcl::PointXY> > tex_coordinates; // UV coordinates
std::vector<std::vector<TexMaterial> > tex_materials; // define texture material
std::vector<std::vector<size_t> > tex_material_idx; // define texture material

public:
typedef boost::shared_ptr< ::pcl::TextureMesh> Ptr;
typedef boost::shared_ptr< ::pcl::TextureMesh const> ConstPtr;

typedef boost::shared_ptr<pcl::TextureMesh> Ptr;
typedef boost::shared_ptr<pcl::TextureMesh const> ConstPtr;
}; // struct TextureMesh

typedef boost::shared_ptr< ::pcl::TextureMesh> TextureMeshPtr;
typedef boost::shared_ptr< ::pcl::TextureMesh const> TextureMeshConstPtr;

typedef boost::shared_ptr<pcl::TextureMesh> TextureMeshPtr;
typedef boost::shared_ptr<pcl::TextureMesh const> TextureMeshConstPtr;
} // namespace pcl

#endif /* TEXTUREMESH_H_ */
#endif /* PCL_TEXTUREMESH_H_ */

19 changes: 10 additions & 9 deletions common/include/pcl/common/concatenate.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -40,12 +42,11 @@

namespace pcl
{
/**
* \brief Helper functor structure for concatenate.
* \ingroup common
*/
/** \brief Helper functor structure for concatenate.
* \ingroup common
*/
template<typename PointInT, typename PointOutT>
struct NdConcatenateFunctor
struct NdConcatenateFunctor
{
typedef typename traits::POD<PointInT>::type PodIn;
typedef typename traits::POD<PointOutT>::type PodOut;
Expand All @@ -63,9 +64,9 @@ namespace pcl
BOOST_MPL_ASSERT_MSG((boost::is_same<InT, OutT>::value),
POINT_IN_AND_POINT_OUT_HAVE_DIFFERENT_TYPES_FOR_FIELD,
(Key, PointInT&, InT, PointOutT&, OutT));
memcpy(reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointOutT, Key>::value,
reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointInT, Key>::value,
sizeof(InT));
memcpy (reinterpret_cast<uint8_t*>(&p2_) + pcl::traits::offset<PointOutT, Key>::value,
reinterpret_cast<const uint8_t*>(&p1_) + pcl::traits::offset<PointInT, Key>::value,
sizeof (InT));
}

private:
Expand Down
87 changes: 54 additions & 33 deletions common/include/pcl/point_representation.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
Expand Down Expand Up @@ -42,12 +44,12 @@

namespace pcl
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b PointRepresentation provides a set of methods for converting a point structs/object into an
* n-dimensional vector.
* @note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor
* and provide an implemention of the pure virtual copyToFloatArray method.
*/
* n-dimensional vector.
* \note This is an abstract class. Subclasses must set nr_dimensions_ to the appropriate value in the constructor
* and provide an implemention of the pure virtual copyToFloatArray method.
* \author Michael Dixon
*/
template <typename PointT>
class PointRepresentation
{
Expand All @@ -62,10 +64,10 @@ namespace pcl
PointRepresentation () : nr_dimensions_ (0), alpha_ (0) {}

/** \brief Copy point data from input point to a float array. This method must be overriden in all subclasses.
* \param p The input point
* \param out A pointer to a float array.
* \param[in] p The input point
* \param[out] out A pointer to a float array.
*/
virtual void copyToFloatArray (const PointT &p, float * out) const = 0;
virtual void copyToFloatArray (const PointT &p, float *out) const = 0;

/** \brief Verify that the input point is valid.
* \param p The point to validate
Expand All @@ -89,11 +91,11 @@ namespace pcl
}

/** \brief Convert input point into a vector representation, rescaling by \a alpha.
* \param p
* \param out The output vector. Can be of any type that implements the [] operator.
*/
* \param[in] p the input point
* \param[out] out The output vector. Can be of any type that implements the [] operator.
*/
template <typename OutputType> void
vectorize (const PointT &p, OutputType &out) const
vectorize (const PointT &p, OutputType &out) const
{
float *temp = new float[nr_dimensions_];
copyToFloatArray (p, temp);
Expand All @@ -111,12 +113,10 @@ namespace pcl
}

/** \brief Set the rescale values to use when vectorizing points
* \param rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator.
*/
//template <typename InputType>
//void setRescaleValues (const InputType &rescale_array)
* \param[in] rescale_array The array/vector of rescale values. Can be of any type that implements the [] operator.
*/
void
setRescaleValues (const float * rescale_array)
setRescaleValues (const float *rescale_array)
{
alpha_.resize (nr_dimensions_);
for (int i = 0; i < nr_dimensions_; ++i)
Expand Down Expand Up @@ -148,20 +148,23 @@ namespace pcl
if (nr_dimensions_ > 3) nr_dimensions_ = 3;
}

inline Ptr makeShared () const { return Ptr (new DefaultPointRepresentation<PointDefault> (*this)); }
inline Ptr
makeShared () const
{
return (Ptr (new DefaultPointRepresentation<PointDefault> (*this)));
}

virtual void
copyToFloatArray (const PointDefault &p, float * out) const
copyToFloatArray (const PointDefault &p, float * out) const
{
// If point type is unknown, treat it as a struct/array of floats
const float * ptr = (float *)&p;
for (int i = 0; i < nr_dimensions_; ++i)
{
out[i] = ptr[i];
}
}
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation <PointXYZ> : public PointRepresentation <PointXYZ>
{
Expand All @@ -172,14 +175,15 @@ namespace pcl
}

virtual void
copyToFloatArray (const PointXYZ &p, float * out) const
copyToFloatArray (const PointXYZ &p, float * out) const
{
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
}
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation <PointXYZI> : public PointRepresentation <PointXYZI>
{
Expand All @@ -190,7 +194,7 @@ namespace pcl
}

virtual void
copyToFloatArray (const PointXYZI &p, float * out) const
copyToFloatArray (const PointXYZI &p, float * out) const
{
out[0] = p.x;
out[1] = p.y;
Expand All @@ -199,6 +203,7 @@ namespace pcl
}
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation <PointNormal> : public PointRepresentation <PointNormal>
{
Expand All @@ -209,14 +214,15 @@ namespace pcl
}

virtual void
copyToFloatArray (const PointNormal &p, float * out) const
copyToFloatArray (const PointNormal &p, float * out) const
{
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
}
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation <PFHSignature125> : public PointRepresentation <PFHSignature125>
{
Expand All @@ -234,6 +240,7 @@ namespace pcl
}
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation <FPFHSignature33> : public PointRepresentation <FPFHSignature33>
{
Expand All @@ -244,7 +251,7 @@ namespace pcl
}

virtual void
copyToFloatArray (const FPFHSignature33 &p, float * out) const
copyToFloatArray (const FPFHSignature33 &p, float * out) const
{
for (int i = 0; i < nr_dimensions_; ++i)
out[i] = p.histogram[i];
Expand All @@ -268,6 +275,7 @@ namespace pcl
}
};

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <>
class DefaultPointRepresentation <NormalBasedSignature12> : public PointRepresentation <NormalBasedSignature12>
{
Expand All @@ -285,6 +293,7 @@ namespace pcl
}
};


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b CustomPointRepresentation extends PointRepresentation to allow for sub-part selection on the point.
*/
Expand All @@ -303,25 +312,37 @@ namespace pcl
typedef boost::shared_ptr<CustomPointRepresentation<PointDefault> > Ptr;
typedef boost::shared_ptr<const CustomPointRepresentation<PointDefault> > ConstPtr;

CustomPointRepresentation (int max_dim = 3, int start_dim = 0) : max_dim_(max_dim), start_dim_(start_dim)
/** \brief Constructor
* \param[in] max_dim the maximum number of dimensions to use
* \param[in] start_dim the starting dimension
*/
CustomPointRepresentation (int max_dim = 3, int start_dim = 0)
: max_dim_(max_dim), start_dim_(start_dim)
{
// If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
nr_dimensions_ = sizeof (PointDefault) / sizeof (float) - start_dim_;
// Limit the default representation to the first 3 elements
if (nr_dimensions_ > max_dim_) nr_dimensions_ = max_dim_;
if (nr_dimensions_ > max_dim_)
nr_dimensions_ = max_dim_;
}

inline Ptr makeShared () const { return Ptr (new CustomPointRepresentation<PointDefault> (*this)); }
inline Ptr
makeShared () const
{
return Ptr (new CustomPointRepresentation<PointDefault> (*this));
}

/** \brief Copy the point data into a float array
* \param[in] p the input point
* \param[out] out the resultant output array
*/
virtual void
copyToFloatArray (const PointDefault &p, float * out) const
copyToFloatArray (const PointDefault &p, float *out) const
{
// If point type is unknown, treat it as a struct/array of floats
const float * ptr = ((float *)&p) + start_dim_;
const float *ptr = ((float*)&p) + start_dim_;
for (int i = 0; i < nr_dimensions_; ++i)
{
out[i] = ptr[i];
}
}
};
}
Expand Down
Loading

0 comments on commit 2d64332

Please sign in to comment.