/home/wu/Desktop/kiretu-0.8/KinectCloud.h

Go to the documentation of this file.

/* This file is part of the Kiretu, a Kinect reconstruction tutor.
 * 
 * http://pille.iwr.uni-heidelberg.de/~kinect01/
 * 
 * The code ist licensed to you under the terms of the GNU General Public 
 * License, version 2.0. See the GPL2 file for the text of the license or the 
 * following URL:
 * 
 * http://www.gnu.org/licenses/gpl-2.0.txt
 * 
 * If you redistribute this file in source form, modified or unmodified, you
 * have to leave this header intact and distribute it under the same terms with 
 * the GPL2 file.
 * 
 * Binary distributions must follow the binary distribution requirements of
 * the License.
 * 
 */
 
 /** \file KinectCloud.h
 * 
 * Header file of the KinectCloud-class.
 * 
 * \author      Daniel Wunderlich (d.wunderlich@stud.uni-heidelberg.de)
 * \version     0.8
 * \date        2011-01-26
 * 
 * \see         KinectCloud
 */

#include <vector>
#include <iostream>
#include <cmath>

//~ #include <vcg/simplex/vertex/base.h>   
//~ #include <vcg/simplex/vertex/component.h>
//~ #include <vcg/space/normal_extrapolation.h>
//~ #include <vcg/complex/used_types.h>

#include "Util.h"
//~ #include "NormalReconstruction.h"

/** \class  KinectCloud 
 *  \brief  C++-class which represents a point-cloud of Microsoft's Kinect and 
 *          allows the reconstruction of a point-cloud applying the steps 
 *          explained at the documentation.
 *  
 *  \author     Daniel Wunderlich (d.wunderlich@stud.uni-heidelberg.de)
 *  \version    0.8
 *  \date       2011-01-26
 *  \see        \ref reconstruction and \ref kinectcloud-section
 */
class KinectCloud {
    
    public:

/** \brief Constructor of the KinectCloud-class.
 */ 
        KinectCloud();


/** \brief Destructor of the KinectCloud-class.
 */ 
        ~KinectCloud();


/** \brief  Adds the z-values of a Kinect depth-image grabbed by FrameGrabber 
 *          and stores them as a point-cloud (std::vector< std::vector<float> >).
 *  
 *  \param  zValues Pointer to a \f$640 \times 480\f$-dimensional array of 
 *                  z-values.
 */ 
        void addZValues(int* zValues);


/** \brief  Adds the RGB-values of a Kinect RGB-image grabbed by FrameGrabber 
 *          and stores them as a point-cloud (std::vector< std::vector<float> >).   
 *  
 *  \param  rgbValues   Pointer to a \f$640 \times 480\f$-dimensional array of 
 *                      RGB-values.
 */ 
        void addRgbValues(int* rgbValues);
        
        //~ void undistortDepthImage(std::vector<float> depthDist);
        //~ void undistortRgbImage(std::vector<float> rgbDist);


/** \brief  Converts the added depth-values of the Kinect (\f$ Z_\mathrm{raw} \in
 *          [0, 2047) \f$) into meter using the formula
 *          \f[
 *              Z_\mathrm{meter} = \frac{1}{Z_\mathrm{raw} \cdot (-0.0030711016) + 3.3309495161},
 *          \f]
 *          which was experimentaly determined (http://nicolas.burrus.name/index.php/Research/KinectCalibration).
 *          
 *          Consider that the z-values must be added (addZValues(int* zValues)) 
 *          before before calling this function.
 *  
 *  \see    \ref kinectcloud-step1
 */ 
        void rawToMeter();


/** \brief  Applies the intrinsic camera-parameters to the point-cloud. The 
 *          parameters should be imported using YMLParser. With the help of 
 *          these parameters, each point \f$ (x,y)^T \f$ of the depth-image with 
 *          its depth-value \f$Z\f$ is projected on its equivalent point 
 *          \f$ (X,Y,Z)^T \f$ in 3D-space regarding its z-value and the 
 *          depth-camera-parameters. 
 *          This is done with the following formula:
 *          \f{align*}
 *              \begin{pmatrix} 
 *                  x_\mathsf{d} \\ y_\mathsf{d}\\ 1
 *              \end{pmatrix}
 *              &= 
 *              \begin{pmatrix}
 *                   & & & 0 \\
 *                   & \mathbf{C} & & 0 \\
 *                   & & & 0
 *              \end{pmatrix}
 *              \begin{pmatrix} 
 *                  X_\mathsf{d} \\ Y_\mathsf{d} \\ Z_\mathsf{d} \\ 1
 *              \end{pmatrix}
 *              =
 *              \begin{pmatrix} 
 *                  f_x & 0 & c_x & 0 \\
 *                  0 & f_y & c_y & 0 \\
 *                  0 & 0 & 1 & 0
 *              \end{pmatrix}
 *              \cdot
 *              \begin{pmatrix} 
 *                  X_\mathsf{d} \\ Y_\mathsf{d} \\ Z_\mathsf{d} \\ 1
 *              \end{pmatrix} \\
 *              &=
 *              \begin{pmatrix} 
 *                  f_x \cdot X_\mathsf{d} + c_x \cdot Z_\mathsf{d} \\ 
 *                  f_y \cdot Y_\mathsf{d} + c_y \cdot Z_\mathsf{d} \\ 
 *                  Z
 *              \end{pmatrix}
 *              =
 *              \begin{pmatrix} 
 *                  f_x \cdot X_\mathsf{d} / Z_\mathsf{d} + c_x \\ 
 *                  f_y \cdot Y_\mathsf{d} / Z_\mathsf{d} + c_y \\ 
 *                  1
 *              \end{pmatrix} \\
 *              &\Rightarrow\quad
 *              \begin{cases} 
 *                  X_\mathsf{d} = (x_\mathsf{d} - c_x) \cdot Z_\mathsf{d} / f_x \\
 *                  Y_\mathsf{d} = (y_\mathsf{d} - c_y) \cdot Z_\mathsf{d} / f_y
 *              \end{cases},
 *          \f}
 *          where \f$\mathbf{C}\f$ contains the intrinsic parameters of the 
 *          depth-camera. Notice that the z-value doesn't change during this 
 *          procedure and is already available at the depth-image.
 *          
 *          Consider that the extrinsic parameters should already be applied to 
 *          the point-cloud (applyExtrinsics(std::vector< std::vector<float> > rot, std::vector<float> trans)) 
 *          before calling this function.
 * 
 * \param   depthCam    Intrisic parameters of the depth-cam as a 
 *                      \f$ 3 \times 3 \f$-matrix as written in the specific 
 *                      yml-file and imported using YMLParser.
 * 
 * \see     \ref kinectcloud-step2
 */ 
        void depthToCloud(std::vector< std::vector<float> > depthCam);


/** \brief  Applies the extrinsic camera-parameters to the point-cloud. The 
 *          parameters should be imported using YMLParser. This is done with a 
 *          rotation and translation of each point:
 *          \f[
 *              \begin{pmatrix} 
 *                  x_\mathrm{rgb} \\ y_\mathrm{rgb} \\ z_\mathrm{rgb}
 *              \end{pmatrix}
 *              = 
 *              R \cdot 
 *              \begin{pmatrix} 
 *                  x_\mathrm{d} \\ y_\mathrm{d} \\ z_\mathrm{d}
 *              \end{pmatrix}
 *              + T
 *              =
 *              \begin{pmatrix} 
 *                  r_{11} & r_{12} & r_{13} \\
 *                  r_{21} & r_{22} & r_{23} \\
 *                  r_{31} & r_{32} & r_{33}
 *              \end{pmatrix}
 *              \cdot
 *              \begin{pmatrix} 
 *                  x_\mathrm{d} \\ y_\mathrm{d} \\ z_\mathrm{d}
 *              \end{pmatrix}
 *              +
 *              \begin{pmatrix} 
 *                  t_1 \\ t_2 \\ t_3
 *              \end{pmatrix},
 *          \f]
 *          where \f$R\f$ and \f$T\f$ are the extrinsic parameters of the Kinect 
 *          and describe a rotation and translation.
 *          
 *          Consider that the z-values should be converted into meters 
 *          (rawToMeter()) before calling this function for a correct 
 *          reconstruction.
 * 
 * \param   rot     Rotation-matrix (\f$ 3 \times 3 \f$) of the Kinect as 
 *                  written in the specific yml-file and imported using 
 *                  YMLParser.
 *  \param  trans   Translation-vector (3-dimensional) of the Kinect as 
 *                  written in the specific yml-file and imported using 
 *                  YMLParser.
 * 
 * \see     \ref kinectcloud-step3
 */ 
        void applyExtrinsics(std::vector< std::vector<float> > rot, std::vector<float> trans);
        
        
/** \brief  Computes the mapping between the point-cloud and the equivalent 
 *          points of the RGB-image by reprojecting the point 
 *          \f$ (X_\mathsf{rgb},Y_\mathsf{rgb},Z_\mathsf{rgb}) \f$ 
 *          of the point-cloud to its equivalent point 
 *          \f$ (x_\mathsf{rgb},y_\mathsf{rgb}) \f$ in the plane 
 *          using the intrinsics of the RGB-camera imported by YMLParser. The 
 *          projection is done with the following formula:
 *          \f{align*}
 *              \begin{pmatrix} 
 *                  x_\mathsf{rgb} \\ y_\mathsf{rgb} \\ 1
 *              \end{pmatrix}
 *              &= 
 *              \begin{pmatrix} 
 *                   &  & & 0 \\
 *                   & \mathbf{C} &  & 0 \\
 *                   &  & & 0 
 *              \end{pmatrix}
 *              \begin{pmatrix} 
 *                  X_\mathsf{rgb} \\ Y_\mathsf{rgb} \\ Z_\mathsf{rgb} \\ 1
 *              \end{pmatrix}
 *              =
 *              \begin{pmatrix} 
 *                  f_x & 0 & c_x & 0 \\
 *                  0 & f_y & c_y & 0 \\
 *                  0 & 0 & 1 & 0
 *              \end{pmatrix}
 *              \cdot
 *              \begin{pmatrix} 
 *                  X_\mathsf{rgb} \\ Y_\mathsf{rgb} \\ Z_\mathsf{rgb} \\ 1
 *              \end{pmatrix} \\
 *              &=
 *              \begin{pmatrix} 
 *                  f_x \cdot X_\mathsf{rgb} + c_x \cdot Z_\mathsf{rgb} \\ 
 *                  f_y \cdot Y_\mathsf{rgb} + c_y \cdot Z_\mathsf{rgb} \\ 
 *                  Z
 *              \end{pmatrix}
 * =
 *              \begin{pmatrix} 
 *                  f_x \cdot X_\mathsf{rgb} / Z + c_x \\ 
 *                  f_y \cdot Y_\mathsf{rgb} / Z + c_y \\ 
 *                  1
 *              \end{pmatrix} \\
 *              &\Rightarrow
 *              \begin{cases} 
 *                  x_\mathsf{rgb} = (f_x \cdot X_\mathsf{rgb} / Z_\mathsf{rgb}) + c_x \\
 *                  y_\mathsf{rgb} = (f_y \cdot Y_\mathsf{rgb} / Z_\mathsf{rgb}) + c_y
 *              \end{cases},
 *          \f}
 *          where \f$C\f$ contains the intrinsic parameters of the RGB-camera. 
 *          
 *          Consider that the depth-image needs to be projected into the 
 *          3D-space using (depthToCloud(std::vector< std::vector<float> > depthCam)) 
 *          before calling this function for correct reconstruction.
 * 
 * \param   rgbCam  Intrisic parameters of the RGB-cam as a 
 *                  \f$ 3 \times 3 \f$-matrix as written in the specific 
 *                  yml-file and imported using YMLParser.
 * 
 * \see     \ref kinectcloud-step4
 */ 
        void computeRgbMapping(std::vector< std::vector<float> > rgbCam);
        
        //~ void computeNormals();


/** \brief Get the cloud.
 * 
 *  \return Vector of 3-dimensional points.
 */ 
        std::vector< std::vector<float> > getCloud();


/** \brief Get the RGB-cloud.
 * 
 *  \return Vector of 3-dimensional vectors containing the RGB-values of the 
 *          cloud.
 */ 
        std::vector< std::vector<int> > getCloudRgb();


/** \brief Get the cloud-RGB-mapping.
 * 
 *  \return Vector of 2-dimensional vectors, which contain the x- and y-value of 
 *          the pixel at the RGB-image of every point at the point-cloud. It is 
 *          used to find the corresponding color of every point of the 
 *          point-cloud at the RGB-image.
 */ 
        std::vector< std::vector<int> > getCloudRgbMapping();
        
        //~ std::vector< std::vector<float> > getNormals();


/** \brief  Get a vector which indicates if a point of the point-cloud is valid
 *          (\f$depth < 2047\f$).
 * 
 *  \return Indicates for every of the \f$640 \times 480\f$ points if it is 
 *          valid (\f$depth < 2047\f$).
 */ 
        std::vector<bool> getValidValues();


/** \brief Get the reconstruction-steps.
 * 
 *  \return Vector boolean values indicating if the reconstruction step was 
 *          done. The values of a returned vector \c recon indicate:
 *          \li \c recon[0]: Raw depth-values of the Kinect converted into 
 *              meters using rawToMeter().
 *          \li \c recon[1]: The intrinsic parameters of the depth-camera were 
 *              applied using 
 *              depthToCloud(std::vector< std::vector<float> > depthCam).
 *          \li \c recon[2]: The extrinsic camera-parameters were applied using 
 *              applyExtrinsics(std::vector< std::vector<float> > rot, std::vector<float> trans).
 *          \li \c recon[3]: The intrinsic parameters of the RGB-camera were 
 *              applied using  
 *              computeRgbMapping(std::vector< std::vector<float> > rgbCam).
 *          \li (\c recon[4]: Normals were reconstructed using
 *              computeNormals() &ndash; not implemented yet.)
 *          
 */ 
        std::vector<bool> getReconstructionSteps();
        
    private:
        std::vector< std::vector<float> > cloud;            /**< The point-cloud */
        std::vector< std::vector<int> > cloudRgb;           /**< The RGB-values */
        std::vector< std::vector<int> > cloudRgbMapping;    /**< Coordinates of a point's corresponding color at the RGB-image */
        //~ std::vector< std::vector<float> > normals;          /**< Normals */
        std::vector<bool> validValues;                      /**< Valid flags for each point */
        std::vector<bool> reconstructionSteps;              /**< Reconstruction flags */
    
};


    
    //~ class VCGVertex;
    //~ class VCGUsedTypes : public vcg::UsedTypes<vcg::Use<VCGVertex>::AsVertexType> {};
    //~ class VCGVertex : public vcg::Vertex<VCGUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3d> {};