Получение высоты вращения в с ++ - PullRequest
Новая
       42

Получение высоты вращения в с ++

0 голосов
/ 23 октября 2018

Я получил эту функцию в своем коде, написанном кем-то другим.Я не понимаю теорию, стоящую за этим, но это, кажется, работает.Может ли кто-нибудь вести меня в правильном направлении?

Мы вычисляем наклон дороги из облака точек с помощью Ransac.Вращение - это мир в локальную матрицу, поэтому plane_normal_rot - это вектор нормали плоскости в мире.Но после этого я не понимаю, что происходит .. 

ransac.getModelCoefficients(model_coefficients);                                                                                                     

std::cout << "#############################" << std::endl;                                                                                           
std::cout << "PLANE MODEL: " << model_coefficients[0]  << " "<< model_coefficients[1] << " "<< model_coefficients[2] << " " << model_coefficients[3];
std::cout << "#############################" << std::endl;                                                                                           

double a = model_coefficients[0];                                                                                                                    
double b = model_coefficients[1];                                                                                                                    
double c = model_coefficients[2];                                                                                                                    

tf::Vector3 plane_normal(a,b,c);                                                                                                                     
tf::Vector3 plane_normal_rot(0,0,0);                                                                                                                 

//tf::Matrix3x3 rotation_tr = rotation.transpose();                                                                                                  
tf::Matrix3x3 rotation_tr = rotation;                                                                                                                

plane_normal_rot.setX( (plane_normal.getX() *  rotation_tr[0][0])                                                                                    
        + (plane_normal.getY() *  rotation_tr[0][1])                                                                                                 
        + (plane_normal.getZ() *  rotation_tr[0][2]));                                                                                               

plane_normal_rot.setY( (plane_normal.getX() *  rotation_tr[1][0])                                                                                    
        + (plane_normal.getY() *  rotation_tr[1][1])                                                                                                 
        + (plane_normal.getZ() *  rotation_tr[1][2]));                                                                                               

plane_normal_rot.setZ( (plane_normal.getX() *  rotation_tr[2][0])                                                                                    
        + (plane_normal.getY() *  rotation_tr[2][1])                                                                                                 
        + (plane_normal.getZ() *  rotation_tr[2][2]));                                                                                               

//Check sign                                                                                                                                         
if(plane_normal_rot.getZ() < 0)                                                                                                                      
{                                                                                                                                                    
    plane_normal_rot *= (-1);                                                                                                                        
}                                                                                                                                                    

pitch = asin(plane_normal_rot.getX());

Если у меня не было ясности или вы чувствуете, что вам не хватает информации, пожалуйста, сообщите мне.

...