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;
}
