Projecting to yaw only

test.cpp

#include <iostream>                                                                       
#include <Eigen/Dense>                                                                    
#include <Eigen/Geometry>                                                                 
#include <sophus/so3.hpp>                                                                 
#include <cmath>                                                                          

// compile with
// g++ -I /usr/include/eigen3 test.cpp -lm -o test

int main(int argc, char **argv)                                                           
{                                                                                         
    (void)argc;                                                                           
    (void)argv;                                                                           
    Sophus::SO3d r;                                                                       
    Eigen::Vector3d dq1(0.1, 0.1, 0.0);                                                   
    Eigen::Vector3d dq2(0.0, 0.0, 0.1);                                                   
    r = r*Sophus::SO3d::exp(dq1);                                                         
    r = r*Sophus::SO3d::exp(dq2);                                                         
    r = r*Sophus::SO3d::exp(dq1);                                                         
    r = r*Sophus::SO3d::exp(dq1);                                                         
    r = r*Sophus::SO3d::exp(dq2);                                                         
    Eigen::Vector3d t = Sophus::SO3d::log(r);                                             
    std::cout << "log " << t.transpose() << std::endl;                                    
    t.segment<2>(0).setZero();                                                            
    r = Sophus::SO3d::exp(t);                                                             
    std::cout << "projected " << r.unit_quaternion().coeffs().transpose() << std::endl;   
    double yaw = 2*acos(r.unit_quaternion().coeffs()[3]);                                 
    std::cout << "projected yaw " << yaw << std::endl;                                    
    Eigen::Quaterniond q = r.unit_quaternion();                                           
    q.x() = 0;                                                                            
    q.y() = 0;                                                                            
    q.normalize();                                                                        
    yaw = 2*acos(q.coeffs()[3]);                                                          
    std::cout << "zeroed yaw " << yaw << std::endl;                                       
    return 0;                                                                             
}