16 if (!args.contains(
"common"))
19 const std::string common_params_path =
resolve_path(args[
"common"], args[
"root_path"]);
21 if (common_params_path.empty())
24 std::ifstream file(common_params_path);
29 file >> common_params;
33 const bool has_root_path = common_params.contains(
"root_path");
35 common_params[
"root_path"] =
resolve_path(common_params[
"root_path"], common_params_path);
37 common_params[
"root_path"] = common_params_path;
44 args[
"root_path"] = common_params[
"root_path"];
47 if (args.contains(
"patch"))
49 patch = args[
"patch"];
53 common_params.merge_patch(args);
55 common_params = common_params.patch(patch);
63 std::transform(mode.begin(), mode.end(), mode.begin(), ::tolower);
65 if (jr.is_array() && jr.empty())
67 return Eigen::Matrix3d::Identity(3, 3);
74 assert(mode.size() == 1);
75 int i = mode[0] -
'x';
76 assert(i >= 0 && i < 3);
77 r[i] = jr.get<
double>();
81 assert(jr.is_array());
85 if (mode ==
"axis_angle")
87 assert(r.size() == 4);
89 Eigen::Vector3d axis = r.tail<3>().normalized();
90 return Eigen::AngleAxisd(angle, axis).toRotationMatrix();
93 if (mode ==
"quaternion")
95 assert(r.size() == 4);
96 Eigen::Vector4d q = r.normalized();
97 return Eigen::Quaterniond(q).toRotationMatrix();
103 if (mode ==
"rotation_vector")
105 assert(r.size() == 3);
106 double angle = r.norm();
109 return Eigen::AngleAxisd(angle, r / angle).toRotationMatrix();
113 return Eigen::Matrix3d::Identity();
117 Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
119 for (
int i = 0; i < mode.size(); i++)
121 int j = mode[i] -
'x';
122 assert(j >= 0 && j < 3);
123 Eigen::Vector3d axis = Eigen::Vector3d::Zero();
125 R = Eigen::AngleAxisd(r[j], axis).toRotationMatrix() * R;