刚体三维空间变换常见函数
常见函数
四元数转欧拉角
def quart_to_rpy(x, y, z, w, deg=False):
roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
pitch = math.asin(2 * (w * y - x * z))
yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
rpy = (roll, pitch, yaw)
if deg:
rpy = np.rad2deg(rpy)
return rpy
return rpy
空间旋转
void TransformPointsFromOdom2Imu(
const drive::common::geometry::Point& odom_point,
drive::common::geometry::Point& imu_point,
const drive::common::pose::Pose3D& imu_pose) {
Eigen::Matrix<double, 3, 1> translation{
imu_pose.position().x(), imu_pose.position().y(), imu_pose.position().z()};
Eigen::Quaternion<double> orientation{imu_pose.orientation().qx(),
imu_pose.orientation().qy(),
imu_pose.orientation().qz(),
imu_pose.orientation().qw()};
logger.debug(5, "translation: %%, orientation:%%", translation, orientation);
Eigen::Matrix<double, 4, 4> transform_matrix = Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix<double, 3, 3> R = orientation.toRotationMatrix();
transform_matrix.block(0, 0, 3, 3) = R;
transform_matrix.block(0, 3, 3, 1) = translation;
Eigen::Matrix<double, 4, 1> p{odom_point.x(), odom_point.y(), odom_point.z(), 1};
Eigen::Matrix<double, 4, 1> tmp_imu_point = transform_matrix.inverse() * p;
imu_point.set_x(tmp_imu_point(0, 0));
imu_point.set_y(tmp_imu_point(1, 0));
imu_point.set_z(tmp_imu_point(2, 0));
}