double getYaw(const Eigen::Matrix3d& rotation){
Eigen::Matrix<double, 3, 1> direction = rotation * Eigen::Matrix<double, 3, 1>::UnitX();
return atan2(direction.y(), direction.x());
}
Eigen::Matrix3d tmp_R = location_T_world_base_link.block<3, 3>(0, 0);
double yaw = getYaw(tmp_R);
cout << "yaw " << yaw << endl;
Eigen::AngleAxisd rollAngle(AngleAxisd(0, Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(0, Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(yaw, Vector3d::UnitZ()));
tmp_R = yawAngle * pitchAngle * rollAngle;
location_T_world_base_link.block<3, 3>(0, 0) = tmp_R;
location_T_world_base_link(2, 3) = 0;