Матрица вычисления задачи, теоретический вывод и результаты кода не совпадают - PullRequest
Новая
       32

Матрица вычисления задачи, теоретический вывод и результаты кода не совпадают

0 голосов
/ 10 апреля 2019

Я хочу решить вопрос ниже: точка Pw находится в мировой системе координат.Система координат C0 и Pw в координате Co - это Pc0.Теперь я преобразовываю систему координат C0 в систему координат C1, спрашиваю: как я преобразую Pw с учетом Pc0 = Pc1. image показывает, как я вычисляю преобразование T.

И код для подтверждения результата приведен ниже, к сожалению, Pc0! = Pc1.Я не знаю, что не так с вычислениями.Можете ли вы дать совет?Большое спасибо. 

int main() {
    //Pw
    Eigen::Vector3d Pw ( 1,0,0 );

    //Tc0w
    Eigen::AngleAxisd rotation_vector_c0w ( M_PI/2, Eigen::Vector3d ( 1,0,0 ) );
    Eigen::Matrix3d Rc0w = rotation_vector_c0w.toRotationMatrix();
    Eigen::Vector3d tc0w (1,2,3);
    Eigen::Isometry3d Tc0w=Eigen::Isometry3d::Identity();               
    Tc0w.rotate ( rotation_vector_c0w );                                     
    Tc0w.pretranslate ( tc0w );
    std::cout << "Transform matrix Tc0w= \n" << Tc0w.matrix() <<std::endl;

    //Pc0
    Eigen::Vector3d Pc0 = Tc0w*Pw;
    std::cout<<"Pc0:"<<std::endl<<Pc0<<std::endl;

    //Tc1c0
    Eigen::AngleAxisd rotation_vector_c1c0 ( M_PI/4, Eigen::Vector3d ( 1,1,0 ) );
    Eigen::Vector3d tc1c0 (1,2,3);
    Eigen::Isometry3d Tc1c0=Eigen::Isometry3d::Identity();                
    Tc1c0.rotate ( rotation_vector_c1c0 );                                     
    Tc1c0.pretranslate ( tc1c0 );
    std::cout << "Transform matrix Tc1c0= \n" << Tc1c0.matrix() <<std::endl;
    std::cout << "Transform matrix Tc0c1= \n" << Tc1c0.inverse().matrix() <<std::endl;

    //compute T
    Eigen::Isometry3d T = Tc0w.inverse()* Tc1c0.inverse()*Tc0w;
    std::cout << "Transform matrix T= \n" << T.matrix() <<std::endl;

    //confirm Pc1
    Eigen::Vector3d Pc1 = Tc1c0*Tc0w*T*Pw;
    std::cout<<"Pc1 = "<<std::endl<<Pc1<<std::endl;
    std::cout << "Hello, World!" << std::endl;
    return 0;
}

1 Ответ

1 голос
/ 10 апреля 2019

AngleAxis требуется нормализованная ось вращения.Попробуйте это: 

Eigen::AngleAxisd rotation_vector_c1c0 ( M_PI/4, Eigen::Vector3d ( 1,1,0 ).normalized() );

Соответствующая ссылка на документацию: http://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html#ab58bae23f0af86d66d8aa1dc5c1dbe39

...