/**
\page yarp_pointcloud YARP PointCloud

\tableofcontents

This tutorial covers how to use the template class yarp::sig::PointCloud with pcl::PointCloud and stand alone.

\section what_is_a_pointcloud What is a PointCloud?

A point cloud is a set of data points in a pre-defined coordinate system.

In a three-dimensional coordinate system, these points are usually defined
by X, Y, and Z coordinates, and often are intended to represent the external surface of an object.

A point cloud can also contain other kinds of information such as the normals,
the RGB, etc.

In YARP we support these point types:

- yarp::sig::DataXY
- yarp::sig::DataXYZ
- yarp::sig::DataNormal
- yarp::sig::DataXYZRGBA
- yarp::sig::DataXYZI
- yarp::sig::DataInterestPointXYZ
- yarp::sig::DataXYZNormal
- yarp::sig::DataXYZNormalRGBA

These structures have been created  to be compatible with the
PCL's ones.

\section read_and_write_point_cloud Write and read PointCloud to/from ports

This code snippet shows how to send and then receive a yarp::sig::PointCloud
through the YARP ports.

~~~{.cpp}
        yarp::os::BufferedPort< yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> > outPort;
        yarp::os::Port inPort;
        // Open the ports
        outPort.open("/test/pointcloud/out");
        inPort.open("/test/pointcloud/in");
        // Connect the ports
        yarp::os::NetworkBase::connect(outPort.getName(), inPort.getName());
        // Declare the point cloud to be sent
        yarp::sig::PointCloud<yarp::sig::DataXYZRGBA>& testPC = outPort.prepare();
        int width  = 100;
        int height = 20;
        testPC.resize(width, height);
        // Fill the point cloud
        for (int i=0; i<width*height; i++)
        {
            testPC(i).x = i;
            testPC(i).y = i + 1;
            testPC(i).z = i + 2;
            testPC(i).r = '1';
            testPC(i).g = '2';
            testPC(i).b = '3';
            testPC(i).a = '4';
        }

        yarp::os::Time::delay(0.3);
        // write the point cloud
        outPort.write();
        // read the point cloud
        yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> inCloud;
        inPort.read(inCloud);
~~~
**Note:** in this example, the type of the `testPC` and `inCloud` is the same
but it is **NOT** mandatory.

If the type of the input yarp::sig::PointCloud is different, only the fields in common
with the output type will be actually read, the missing ones will be set to
default values.

For example, if you write a `yarp::sig::PointCloud<yarp::sig::DataXYZRGBA>` and you
want to read through a `yarp::sig::PointCloud<yarp::sig::DataXYZNormal>`:
- the `XYZ`fields will be filled
- the `Normal` fields will be set to default
- the`RGBA` will be ignored

\section general_usage_point_cloud PointCloud initialization

The flexibility on types has been implemented also for what concerns the
initialization of the point cloud (**copy constructor**
and **assignment operator**):

~~~{.cpp}
        yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA> pc1;
        yarp::sig::PointCloud<yarp::sig::DataXYZNormal> pc2(pc1);
        yarp::sig::PointCloud<yarp::sig::DataXYZNormalRGBA> pc3 = pc2;
~~~

In this case `pc3` will have only the `XYZ` and `Normal` values of `pc1` because
the `RGBA` values have been lost in the construction of `pc2` that not contains
the `RGBA` field.

\section depth_to_PC Compute PointClouds from depth images

YARP provides two functions that allow to compute `yarp::sig::PointCloud`s given depth image
and intrinsic parameters of the camera.

They are included in `yarp/sig/PointCloudUtils.h` and they are:
- `yarp::sig::utils::depthToPC`
- `yarp::sig::utils::depthRgbToPC`

Both functions assume that the images are already **undistorted**, no undistortion is
performed.

`yarp::sig::utilsdepthRgbToPC` needs also the colored frame in order to generate
a colored PointCloud. It assumes that the the two frames are **aligned**.

You have to use intrinsics parameters of the depth sensor if the depth frame IS NOT aligned with the
colored one. On the other hand use the intrinsic parameters of the RGB camera if the frames are aligned.

Here an example of usage:

~~~{.cpp}
yarp::sig::ImageOf<yarp::sig::PixelFloat> depth;
yarp::sig::ImageOf<yarp::sig::PixelRgb> color;
yarp::os::Property propIntrinsic;
yarp::sig::IntrinsicParams intrinsic;
bool aligned{false};
// check if the frames that are coming are aligned.
...
// get the intrinsic parameters from the RGBDClient
yarp::os::Property conf;
conf.put("device","RGBDSensorClient");
conf.put("localImagePort","/clientRgbPort:i");
conf.put("localDepthPort","/clientDepthPort:i");
conf.put("localRpcPort","/clientRpcPort");
conf.put("remoteImagePort","/depthCamera/rgbImage:o");
conf.put("remoteDepthPort","/depthCamera/depthImage:o");
conf.put("remoteRpcPort","/depthCamera/rpc:i");

if (!poly.open(conf))
{
    yError()<<"Unable to open RGBDClient polydriver";
    return false;
}

yarp::dev::IRGBDSensor* iRgbd{nullptr};
if (!poly.view(iRgbd))
{
    yError()<<"Unable to view IRGBD interface";
    return false;
}

bool ok{false};
if (aligned)
{
    ok = iRgbd->getRgbIntrinsicParam(propIntrinsics);
}
else
{
    ok = iRgbd->getDepthIntrinsicParam(propIntrinsics);
}

yarp::sig::IntrinsicParams intrinsics(propIntrinsics);

// read the images from ports

...

// Compute the PointCloud

yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> pc = yarp::sig::utils::depthRGBToPC<yarp::sig::DataXYZRGBA, yarp::sig::PixelRgb>(depth, color, intrinsics);

~~~

Or:

~~~{.cpp}
yarp::sig::PointCloud<yarp::sig::DataXYZ> pc = yarp::sig::utils::depthToPC(depth, intrinsics);
~~~


\section pcl_compatibility PCL compatibility

The yarp::sig::PointCloud class has been implemented to be compatible with
Point Cloud Library (PCL).

`libYARP_pcl` makes yarp::sig::PointCloud compatible with PCL, and it is
compiled if PCL is found during the `YARP` configuration.

This library has two methods to transform between the two point clouds `toPcl<T,T1>(...)` and `fromPcl<T,T1>(...)`
that can be used as follow

~~~{.cpp}
    // Include the compatibility library
    #include <yarp/pcl/Pcl.h>

    pcl::PointCloud<pcl::PointXYZRGBA> cloud;
    cloud.resize(100);
    for(int i=0; i<cloud.size(); i++)
    {
        cloud.points.at(i).x = i;
        cloud.points.at(i).y = i+1;
        cloud.points.at(i).z = i+2;
        cloud.points.at(i).r = 'r';
        cloud.points.at(i).g = 'g';
        cloud.points.at(i).b = 'b';
    }
    // PCL -> YARP
    yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> yarpCloud;
    yarp::pcl::fromPCL<pcl::PointXYZRGBA, yarp::sig::DataXYZRGBA>(cloud, yarpCloud);
    // YARP -> PCL
    pcl::PointCloud<pcl::PointXYZRGBA> cloud2;
    yarp::pcl::toPCL<yarp::sig::DataXYZRGBA, pcl::PointXYZRGBA>(yarpCloud, cloud2);
~~~

Note that these conversions (PCL -> YARP, YARP -> PCL) are made through a **copy**.

\section yarp_pcd Saving/Loading PCDs

The `YARP_pcl` library has also methods to save/load yarp point clouds to PCD files. These functions
`savePCD<T,T1>(...)` and `loadPCD<T,T1>(...)` can be used as follows:

~~~{.cpp}
    yarp::sig::PointCloud<yarp::sig::DataXYZRGBA> cloud1;
    ...
    // save point cloud to file
    const string filename("name.pcd");
    int result = yarp::pcl::savePCD< yarp::sig::DataXYZRGBA, pcl::PointXYZRGBA>(filename, cloud1);

    // load from file
    yarp::sig::PointCloud<yarp::sig::XYZRGBADat> cloud2;
    result = yarp::pcl::loadPCD< pcl::PointXYZRGBA, yarp::sig::DataXYZRGBA >(filename, cloud2);
~~~

Note that these functions implicitly convert to and from PCL types.

*/
