Kinect: Приведение координат сенсора в метрические

07 Nov 2014

В одниокм из своих проектов с использованием сенсора пришлось задаваться вопросом: как привиодить координаты сенсора (глубину) в координаты метрические. Если этого не сделать, то прямые углы у стен прямыми не будут.

Будет это выглядеть как на картинке ниже.

Kinect: сырой рендер (без преобразования в метрические координаты)А как это сделать?

В пакете freenect есть демка glpclview в коде которой можно увидеть матрицу преобразований координат сенсора в нужные нам координаты.

// Do the projection from u,v,depth to X,Y,Z directly in an opengl matrix  
// These numbers come from a combination of the ros kinect\_node wiki, and  
// nicolas burrus' posts.  
void LoadVertexMatrix()  
{  
 float fx = 594.21f;  
 float fy = 591.04f;  
 float a = -0.0030711f;  
 float b = 3.3309495f;  
 float cx = 339.5f;  
 float cy = 242.7f;  
 GLfloat mat[16] = {  
 1/fx,     0,  0, 0,  
 0,    -1/fy,  0, 0,  
 0,       0,  0, a,  
 -cx/fx, cy/fy, -1, b  
 };  
 glMultMatrixf(mat);  
}

Не будем заострять внимание на очень понятных комментариях в коде :), а попробуем понять, чтоже эта штука делает.

Сходу информации почти нет - гугл говорит, что это есть лишь приведение координат согласно калибровочным данным самого кинекта (тыц).

Но это нифига не проясняет.

Дальнейшее гугление нашло пруф на форуме ROS, а так же пруф в гуглогруппах.

В итоге на скорую руку был состряпан код, который делает нужное преобразование.

/\*\*  
 \* Преобразует глубину в реальное значение (в миллиметрах)  
 \*/  
double raw\_depth\_to\_millimeters(int depth\_value){  
 double depth\_value\_f = (float) depth\_value;  
 if (depth\_value \< 2047){  
 float depth = 1000.0 / (depth\_value\_f&nbsp; \* -0.0030711016 + 3.3309495161);  
 return depth;  
 }  
 return 0.0f;  
}

/\*\*  
 \* Преобразует вируальную точку point в точку с реальными координатами (в миллиметрах)  
 \*/  
Ogre::Vector3 depth\_to\_realword(Ogre::Vector3 point){  
 double fx\_d = 1.0 / 5.9421434211923247e+02;  
 double fy\_d = 1.0 / 5.9104053696870778e+02;  
 double cx\_d = 3.3930780975300314e+02;  
 double cy\_d = 2.4273913761751615e+02;

double depth = raw\_depth\_to\_millimeters(point.z);

return Ogre::Vector3(  
 (point.x - cx\_d) \* depth \* fx\_d,  
 (point.y - cy\_d) \* depth \* fy\_d,  
 depth);  
}

Про этот код важно помнить, что координаты x и y должны назодиться в первой четверти (больше нуля).

Теперь изображение выглядит куда лучше. :)

Изображение с сенсора kineck после преобразования в метрические координаты.