Projecting a texture from a K4W2 Camera via PCL + libfreenect2pclgrabber about libfreenect2pclgrabber

[giacomodabisias]

Hi,
could you specify a bit better the problem? I didn't really understand :D

[lolz0r]

Hello Again! :)

The problem is as follows:

• I want to use a K4W2 to create a textured mesh.
• I am able to generate the mesh via the pointcloud proved by libfreenect2pclgrabber. I also am able to write a JPG file which contains the RGB view of the camera.
• I would like to use this class:
  http://docs.pointclouds.org/trunk/classpcl_1_1_texture_mapping.html to texture the generated mesh. However, I need to provide it camera information in order for it to work.
  http://docs.pointclouds.org/trunk/structpcl_1_1texture__mapping_1_1_camera.html

So the question is:

• What is the appropriate camera pose value? "Eigen::Affine3f pose" I tried of depth2world_ and world2rgb_ and their inverse values but to no avail.
• From research it appears that the camera value 'focal_length' is about 1090f, is that correct?

Thank you for any insight or thoughts you may have. Also, hello from Seattle! :)

[lolz0r]

Hi there!
I have been learning a bit more about the problem. I think it can be expressed most simply as follows:

How do I calculate the 4x4 projection matrix that represents the location and rotation of the kinect's RGB camera from the perspective of the depth camera?

Thanks for any help you may have!

[giacomodabisias]

Hi,
to get from one camera to the other you need the parameters of a stereo calibrate, which are a rotation and a translation. These parameters are already present in my code and if you calibrated your camera with it then you can just read the matrix.

[lolz0r]

That was my first thought as well. However, after a bit of investigation and trial-and-error it appears that the matrix acquired from stereo calibrate does not actually give the XYZ & rotation from RGB to depth but rather it is a projection matrix that can be used to map projected points between the two camera spaces.

I examined the code and saw:
Eigen::Matrix3d rgb2depth = (((orgb_matrix * rototranslation * depth2world)).block<3,3>(0,0)).inverse();

If I wanted to get an 4x4 transformation matrix relative to the depth camera could I do something like:

Eigen::Matrix4d depth2rgb = ( depth2world * rototranslation * orgb_matrix).inverse(); ?

[giacomodabisias]

yes if you want to go from rgb to depth. I created also a function to register color on depth, but it was not fully working so I did not include it:

void 
    createFullCloudFromRgb(const cv::Mat & depth, const cv::Mat & color, typename pcl::PointCloud<PointT>::Ptr & cloud) const
    {

        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > erotation((double*)(rotation_.data));
        Eigen::Matrix3d rotation = erotation;
        Eigen::Map<Eigen::Vector3d> etranslation ((double*)(translation_.data));
        Eigen::Vector3d translation = etranslation;
        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > edepth_matrix((double*)(depth_camera_matrix_.data));
        Eigen::Matrix3d depth_matrix = edepth_matrix;
        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > ergb_matrix((double*)(rgb_camera_matrix_.data));
        Eigen::Matrix3d rgb_matrix = ergb_matrix;

        Eigen::Matrix4d rototranslation = Eigen::Matrix4d::Zero();
        Eigen::Matrix4d odepth_matrix = Eigen::Matrix4d::Zero();
        Eigen::Matrix4d orgb_matrix = Eigen::Matrix4d::Zero(); 

        rototranslation.block<3,3>(0,0) = rotation;
        rototranslation.block<3,1>(0,3) = translation;
        rototranslation(3,3) = 1;

        odepth_matrix.block<3,3>(0,0) = depth_matrix;
        odepth_matrix(3,3) = 1;

        orgb_matrix.block<3,3>(0,0) = rgb_matrix;
        orgb_matrix(3,3) = 1;

        std::cout << rototranslation << std::endl;

        Eigen::Matrix4d depth2world = odepth_matrix.inverse();
        Eigen::Matrix3d rgb2depth = (((orgb_matrix * rototranslation * depth2world)).block<3,3>(0,0)).inverse();

        for(int i =0; i <cloud->points.size(); i++)
        {
            cloud->points[i].x = std::numeric_limits<float>::quiet_NaN();
            cloud->points[i].y = std::numeric_limits<float>::quiet_NaN();
            cloud->points[i].z = std::numeric_limits<float>::quiet_NaN();
            cloud->points[i].rgba = 0;                  
        }

        int invalid = 0;
        int outside = 0;
        int replaced = 0;
        int newone = 0;


//      #pragma omp parallel for
        for(int y = 0; y < color.rows; ++y)
        {   
            for(size_t x = 0; x < (size_t)color.cols; ++x)
            {
                const cv::Vec3b ptcolor = color.at<cv::Vec3b>(y, x);

                Eigen::Vector3d rgb_i(x,y,1); // 2D homo
                Eigen::Vector3d depth_sub_i = rgb2depth * rgb_i; // to 2D homo
                PointT *itP = &cloud->points[(int)depth_sub_i.x() + (int)(depth_sub_i.y()) * color.cols];

                double newdepth = std::numeric_limits<float>::quiet_NaN();

                if(depth_sub_i.x() >= 0 && depth_sub_i.x() < depth.cols && depth_sub_i.y() >= 0 && depth_sub_i.y() < depth.rows )
                {
                    newdepth = (depth.at<uint16_t>((int)depth_sub_i.y(),(int)depth_sub_i.x()))/1000.0;
                    if(isnan(newdepth))
                    {
                        invalid++;
                    }
                }

                if(!isnan(newdepth) && (isnan(itP->z) || newdepth < itP->z) )
                {
                    if(isnan(itP->z))
                        newone++;
                    else
                        replaced++;

                    Eigen::Vector4d psd(depth_sub_i.x(),depth_sub_i.y(), 1.0, 1.0/newdepth);
                    Eigen::Vector4d psddiv = psd * newdepth; // (x*newdepth,y*newdepth,newdepth,1)
                    Eigen::Vector4d pworld =  depth2world  * psddiv;

                    itP->x = pworld.x() ;
                    itP->y = pworld.y() ;
                    itP->z = newdepth; 
                    itP->b = ptcolor.val[0];
                    itP->g = ptcolor.val[1];
                    itP->r = ptcolor.val[2];
                }
            }
        }
        std::cout << "total: " << color.rows*color.cols <<  " nan: "<<invalid << " outside:"<< outside << " AND replaced: " << replaced << " newone:" << newone << std::endl;
    }

[lolz0r]

Why is:
Eigen::Matrix3d rgb2depth = (((orgb_matrix * rototranslation * depth2world)).block<3,3>(0,0)).inverse(); abbridged into a 3x3 matrix?

Also, if I wanted instead depth2rgb as a 4x4 transformation matrix would I do:
Eigen::Matrix4d depth2rgb = (((odepth_matrix * rototranslation.inverse() * rgb2world))).inverse();

Background:
I am trying to create a system where multiple kinects work together to build a large scene. I have kinect 2 kinect point mapping down. From the joined point clouds I want to mesh them and then project a texture onto the resulting mesh. However, I need the exact XYZ and rotation of the RGB camera relative to its depth camera.

[giacomodabisias]

I get a 3x3 beacuse to go from rgb to depth you need just the upper 3x3
matrix.
Second question yes. Right now I saw that they changed libfreenect2 for
automatic calibration and I will try it out in the next days. I have also
changed a bit the grabber and I will
release it soon with the new method.

2015-05-04 12:26 GMT+02:00 lolz0r [email protected]:

Why is:
Eigen::Matrix3d rgb2depth = (((orgb_matrix * rototranslation *
depth2world)).block<3,3>(0,0)).inverse(); abbridged into a 3x3 matrix?

Also, if I wanted instead depth2rgb as a 4x4 transformation matrix would I
do:
Eigen::Matrix4d depth2rgb = (((odepth_matrix * rototranslation.inverse() *
rgb2world))).inverse();

Background:
I am trying to create a system where multiple kinects work together to
build a large scene. I have kinect 2 kinect point mapping down. From the
joined point clouds I want to mesh them and then project a texture onto the
resulting mesh. However, I need the exact XYZ and rotation of the RGB
camera relative to its depth camera.

—
Reply to this email directly or view it on GitHub
#1 (comment)
.

G. Dabisias, PhD Student
PERCRO, (Laboratory of Perceptual Robotics)
Scuola Superiore Sant'Anna
via Luigi Alamanni 13D, San Giuliano Terme 56010 (PI), Italy
mob.: [phone]

[lolz0r]

Great! Thanks for all your help! I can't wait to check out the newest commit ;)