Eigen

行列バージョン

Eigen::Vector3d Rotation(const Eigen::Vector3d& V, const Eigen::Vector3d& RPY)
{
	Eigen::Matrix3d Rot;
	Rot <<	cos(RPY(1))*cos(RPY(2)),	sin(RPY(0))*sin(RPY(1))*cos(RPY(2)) - cos(RPY(0))*sin(RPY(2)),	cos(RPY(0))*sin(RPY(1))*cos(RPY(2)) + sin(RPY(0))*sin(RPY(2)),
		cos(RPY(1))*sin(RPY(2)),	sin(RPY(0))*sin(RPY(1))*sin(RPY(2)) + cos(RPY(0))*cos(RPY(2)),	cos(RPY(0))*sin(RPY(1))*sin(RPY(2)) - sin(RPY(0))*cos(RPY(2)),
		-sin(RPY(1)),	sin(RPY(0))*cos(RPY(1)),	cos(RPY(0))*cos(RPY(1));
	return Rot*V;
}

行列バージョン（逆回転）

Eigen::Vector3d RotationInv(const Eigen::Vector3d& V, const Eigen::Vector3d& RPY)
{
	 Eigen::Matrix3d RotInv;
	RotInv <<	cos(RPY(1))*cos(RPY(2)),	cos(RPY(1))*sin(RPY(2)),	-sin(RPY(1)),
			sin(RPY(0))*sin(RPY(1))*cos(RPY(2)) - cos(RPY(0))*sin(RPY(2)),	sin(RPY(0))*sin(RPY(1))*sin(RPY(2)) + cos(RPY(0))*cos(RPY(2)),	sin(RPY(0))*cos(RPY(1)),
			cos(RPY(0))*sin(RPY(1))*cos(RPY(2)) + sin(RPY(0))*sin(RPY(2)),	cos(RPY(0))*sin(RPY(1))*sin(RPY(2)) - sin(RPY(0))*cos(RPY(2)),	cos(RPY(0))*cos(RPY(1));
	return RotInv*V;
}

クォータニオンバージョン

Eigen::Vector3d Rotation(const Eigen::Vector3d& V, const Eigen::Quaterniond& q)
{
	Eigen::Quaterniond q_xyz(0.0, V(0), V(1), V(2));
	Eigen::Quaterniond q_xyz_trans = q*q_xyz*q.inverse();
	Eigen::Vector3d Vtrans(q_xyz_trans.x(), q_xyz_trans.y(), q_xyz_trans.z());
	return Vtrans;
}

クォータニオンバージョン（逆回転）

Eigen::Vector3d RotationInv(const Eigen::Vector3d& V, const Eigen::Quaterniond& q)
{
	Eigen::Quaterniond q_xyz(0.0, V(0), V(1), V(2));
	Eigen::Quaterniond q_xyz_trans = q.inverse()*q_xyz*q;
	Eigen::Vector3d Vtrans(q_xyz_trans.x(), q_xyz_trans.y(), q_xyz_trans.z());
	return Vtrans;
}

ROS（tf::Quaternion）

ROSユーザー向けの実装です。
あまりメジャーではないと思いますが、私が個人的に好きな実装なので載せておきます笑。

Eigen::Vector3d Rotation(const Eigen::Vector3d& V, const Eigen::Vector3d& RPY)
{
	tf::Quaternion q_rot = tf::createQuaternionFromRPY(RPY(0), RPY(1), RPY(2));
	tf::Quaternion q_xyz(V(0), V(1), V(2), 0.0);
	tf::Quaternion q_xyz_trans = q_rot*q_xyz*q_rot.inverse();
	Eigen::Vector3d Vtrans(q_xyz_trans.x(), q_xyz_trans.y(), q_xyz_trans.z());
	return Vtrans;
}

Eigen::Vector3d RotationInv(const Eigen::Vector3d& V, const Eigen::Vector3d& RPY)
{
	tf::Quaternion q_rot = tf::createQuaternionFromRPY(RPY(0), RPY(1), RPY(2));
	tf::Quaternion q_xyz(V(0), V(1), V(2), 0.0);
	tf::Quaternion q_xyz_trans = q_rot.inverse()*q_xyz*q_rot;
	Eigen::Vector3d Vtrans(q_xyz_trans.x(), q_xyz_trans.y(), q_xyz_trans.z());
	return Vtrans;
}

さいごに

3次元ベクトルの回転の実装を何パターンかご紹介しました。

参考になれば幸いです。

以上です。