C++ (Cpp) ang_velの例

コード例 #1

ファイル: ImuDeadReckon.cpp プロジェクト: mpkuse/rgbd_odometry

/// @brief Callback for IMU data received. Better be very fast. data arrive at 100Hz
void ImuDeadReckon::imuDataRcvd( const sensor_msgs::Imu& msg )
{
    isIMUDataReady = false;
    //ROS_INFO( "Linear acc : %f %f %f", msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z );
    lin_acc(0) = msg.linear_acceleration.x;
    lin_acc(1) = msg.linear_acceleration.y;
    lin_acc(2) = msg.linear_acceleration.z;


    ang_vel(0) = msg.angular_velocity.x;
    ang_vel(1) = msg.angular_velocity.y;
    ang_vel(2) = msg.angular_velocity.z;


    isIMUDataReady = true;

}

コード例 #2

ファイル: OrientationFilter.cpp プロジェクト: Figuration/PSMoveService

static Eigen::Vector3f
quaternion_derivative_to_angular_velocity(
    const Eigen::Quaternionf &current_orientation,
    const Eigen::Quaternionf &quaternion_derivative)
{
    Eigen::Quaternionf inv_orientation = current_orientation.conjugate();
    auto q_ang_vel = (quaternion_derivative*inv_orientation).coeffs() * 2.f;
    Eigen::Vector3f ang_vel(q_ang_vel.x(), q_ang_vel.y(), q_ang_vel.z());

    return ang_vel;
}