void KinectSensor::transformPointByPoint(XnPoint3D* points, int numPoints)
{
	if (idCam != idRefCam)
	{
		for (int i = 0; i < numPoints; i++)
		{
			Matx31f p(points[i].X, points[i].Y, points[i].Z);
			Matx31f out = rotation*p+translation;

			if (tiltAngle != 0)	
			{
				out = (out.t() * rotTilt).t();
			}
		
			points[i].X = out(0);
			points[i].Y = out(1);
			points[i].Z = out(2);
		}
	}
	else if(tiltAngle != 0)
	{
		for (int i = 0; i < numPoints; i++)
		{
			Matx31f p(points[i].X, points[i].Y, points[i].Z);
			Matx13f out = p.t()*rotTilt;

			points[i].X = out(0);
			points[i].Y = out(1);
			points[i].Z = out(2);
		}
	}
}
float epipolarError (const Matx44f& pose2, const Matx31f& p1, const Matx31f& p2)
{
    cv::Matx<float, 1, 1> error = p2.t() * pose2E(pose2) * p1;
    return std::abs(error(0));
}
inline float dot (const Matx31f& a, const Matx31f& b)
{
    return a.dot(b);
}
int main( int argc, char* argv[] )
{
    if( argc != 6)
      {
      std::cerr << "Usage: " << argv[0] << " <InputImage> <Mask> <colorspace> <OutputMu> <OutputSigma>" << std::endl;
      return EXIT_FAILURE;
      }
    cv::Mat image = cv::imread( argv[1] );
    if(!image.data)
      {
      return EXIT_FAILURE;
      }

    cv::Mat mask = cv::imread( argv[2],0 );
    if(!mask.data)
      {
      return EXIT_FAILURE;
      }


    if (strcmp(argv[3],"YCrCb")==0)
    {
       cv::cvtColor( image, image, CV_BGR2YCrCb );
    }
    else if (strcmp(argv[3],"HSV")==0 )
    {
       cv::cvtColor( image, image, CV_BGR2HSV );
    }
    else if (strcmp(argv[3],"RGB")!=0)
    {
        std::cerr<<"Colorspaces: RGB or YCrCb or HSV"<<std::endl;
        return EXIT_FAILURE;
    }

    typedef cv::Matx<float, 1, 3> Matx13f;
    typedef cv::Matx<float, 3, 1> Matx31f;
    typedef cv::Matx<float, 3, 3> Matx33f;

    //Mu
    Matx31f mu;
    mu.zeros();
    int den=0;

    for (int i=0; i<image.rows;i++)
    {
        for (int j=0; j<image.cols; j++)
        {
            if(mask.at<float>(i,j)!=0)
            {
                den++;
                mu+=image.at<cv::Vec3b>(i,j);
            }
        }
    }
    mu(0)/=static_cast<float>(den);
    mu(1)/=static_cast<float>(den);
    mu(2)/=static_cast<float>(den);

    //Sigma
    Matx31f pixel;
    Matx13f transPixel;
    Matx33f sigma;
    sigma.zeros();

    for (int i=0; i<image.rows;i++)
    {
        for (int j=0; j<image.cols; j++)
        {
            if(mask.at<float>(i,j)!=0)
            {
                pixel=image.at<cv::Vec3b>(i,j);
                pixel-=mu;
                transPixel=pixel.t();
                sigma+=pixel*transPixel;
            }
        }
    }

    for(int i=0;i<3;i++)
        for (int j=0;j<3;j++)
        {
            sigma (i,j)/=den-1;
        }

    std::ofstream fileMu(argv[4], std::ios::out | std::ios::trunc);
    if(fileMu)
    {
        for (int i=0; i<3; i++)
        {
            fileMu<<mu(i)<<endl;
        }
        fileMu.close();
    }
    else
    {
        std::cerr << "Can't open file "<<argv[4] << std::endl;
    }

    std::ofstream fileSigma(argv[5], std::ios::out | std::ios::trunc);
    if(fileSigma)
    {
        for (int i=0; i<3; i++)
        {
            for (int j=0; j<3; j++)
            {
                fileSigma<<sigma(i,j)<<endl;
            }
        }
        fileSigma.close();
    }
    else
    {
        std::cerr << "Can't open file "<<argv[5] << std::endl;
    }

    return 0;
}