Skip to content

L
localize_for_ppk
Commit ccc0814e authored by wangdawei's avatar wangdawei
Browse files
Options

unload block

parent 01855b85
Hide whitespace changes
Inline Side-by-side
Showing with 78 additions and 78 deletions
libs/voxel_map/libs/utils/basic_functions.cpp
View file @ ccc0814e
...@@ -24,91 +24,91 @@ vector<pair<uint32_t, uint32_t>> getIndexPeriod(const uint32_t size, ...@@ -24,91 +24,91 @@ vector<pair<uint32_t, uint32_t>> getIndexPeriod(const uint32_t size,
return ret; return ret;
} }
Matrix3d get_rotation(double old_B, double old_L, double old_H, //Matrix3d get_rotation(double old_B, double old_L, double old_H,
double new_B, double new_L, double new_H, // double new_B, double new_L, double new_H,
double old_start_x, double old_start_y, double old_start_z, // double old_start_x, double old_start_y, double old_start_z,
double new_start_x, double new_start_y, double new_start_z, bool print) // double new_start_x, double new_start_y, double new_start_z, bool print)
{ //{
GeographicLib::LocalCartesian old_cartesian(old_B, old_L, old_H); // GeographicLib::LocalCartesian old_cartesian(old_B, old_L, old_H);
GeographicLib::LocalCartesian new_cartesian(new_B, new_L, new_H); // GeographicLib::LocalCartesian new_cartesian(new_B, new_L, new_H);
#define reproj(before, after) \ //#define reproj(before, after) \
{ \ // { \
double B = 0, L = 0, H = 0; \ // double B = 0, L = 0, H = 0; \
after[0] = before[0] + old_start_x; \ // after[0] = before[0] + old_start_x; \
after[1] = before[1] + old_start_y; \ // after[1] = before[1] + old_start_y; \
after[2] = before[2] + old_start_z; \ // after[2] = before[2] + old_start_z; \
old_cartesian.Reverse(after[0], after[1], after[2], B, L, H); \ // old_cartesian.Reverse(after[0], after[1], after[2], B, L, H); \
new_cartesian.Forward(B, L, H, after[0], after[1], after[2]); \ // new_cartesian.Forward(B, L, H, after[0], after[1], after[2]); \
after[0] -= new_start_x; \ // after[0] -= new_start_x; \
after[1] -= new_start_y; \ // after[1] -= new_start_y; \
after[2] -= new_start_z; \ // after[2] -= new_start_z; \
if (print) \ // if (print) \
{ \ // { \
printf("(%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \ // printf("(%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \
before[0], before[1], before[2], \ // before[0], before[1], before[2], \
after[0], after[1], after[2]); \ // after[0], after[1], after[2]); \
LOG(INFO) <<setprecision(15) \ // LOG(INFO) <<setprecision(15) \
<< before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \ // << before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \
<< after[0]<< ", " << after[1] << ", " << after[2]; \ // << after[0]<< ", " << after[1] << ", " << after[2]; \
} \ //} \
} //}
#define rand1000() (rand() % 2000 - 1000) //#define rand1000() (rand() % 2000 - 1000)
ceres::Problem problem; // ceres::Problem problem;
Eigen::Quaterniond q_R = Eigen::Quaterniond::Identity(); // Eigen::Quaterniond q_R = Eigen::Quaterniond::Identity();
Eigen::Vector3d translation = Eigen::Vector3d::Zero(); // Eigen::Vector3d translation = Eigen::Vector3d::Zero();
std::vector<Eigen::Vector3d> points; // std::vector<Eigen::Vector3d> points;
std::vector<Eigen::Vector3d> new_points; // std::vector<Eigen::Vector3d> new_points;
for (int i = 0; i < 10; i++) // for (int i = 0; i < 10; i++)
{ // {
Eigen::Vector3d p(rand1000(), rand1000(), rand1000()); // Eigen::Vector3d p(rand1000(), rand1000(), rand1000());
if (i == 0) // if (i == 0)
p = Eigen::Vector3d::Zero(); // p = Eigen::Vector3d::Zero();
Eigen::Vector3d new_p; // Eigen::Vector3d new_p;
reproj(p, new_p); // reproj(p, new_p);
if (i == 0) // if (i == 0)
translation = new_p; // translation = new_p;
points.push_back(p); // points.push_back(p);
new_points.push_back(new_p); // new_points.push_back(new_p);
ceres::CostFunction *func = DistanceError::Create(p, new_p); // ceres::CostFunction *func = DistanceError::Create(p, new_p);
problem.AddResidualBlock(func, NULL, q_R.coeffs().data(), translation.data()); // problem.AddResidualBlock(func, NULL, q_R.coeffs().data(), translation.data());
} // }
auto *quaternion_parameterization = // auto *quaternion_parameterization =
new ceres::EigenQuaternionParameterization; // new ceres::EigenQuaternionParameterization;
problem.SetParameterization(q_R.coeffs().data(), quaternion_parameterization); // problem.SetParameterization(q_R.coeffs().data(), quaternion_parameterization);
ceres::Solver::Options options; // ceres::Solver::Options options;
options.use_nonmonotonic_steps = false; // options.use_nonmonotonic_steps = false;
options.max_num_iterations = 50; // options.max_num_iterations = 50;
options.num_threads = 10; // options.num_threads = 10;
options.num_linear_solver_threads = options.num_threads; // options.num_linear_solver_threads = options.num_threads;
ceres::Solver::Summary summary; // ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary); // ceres::Solve(options, &problem, &summary);
Eigen::Matrix3d rotation = q_R.toRotationMatrix(); // Eigen::Matrix3d rotation = q_R.toRotationMatrix();
if (print) { // if (print) {
#define test(before) \ //#define test(before) \
{ \ // { \
Eigen::Vector3d after = translation + rotation * before; \ // Eigen::Vector3d after = translation + rotation * before; \
printf("test (%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \ // printf("test (%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \
before[0], before[1], before[2], \ // before[0], before[1], before[2], \
after[0], after[1], after[2]); \ // after[0], after[1], after[2]); \
LOG(INFO) <<setprecision(15) \ // LOG(INFO) <<setprecision(15) \
<< before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \ // << before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \
<< after[0]<< ", " << after[1] << ", " << after[2]; \ // << after[0]<< ", " << after[1] << ", " << after[2]; \
} // }
for (int i = 0; i < 10; i++) // for (int i = 0; i < 10; i++)
{ // {
test(points[i]); // test(points[i]);
} // }
Eigen::Vector3d euler = matrix_2_euler_rpy(rotation); // Eigen::Vector3d euler = matrix_2_euler_rpy(rotation);
} // }
return rotation; // return rotation;
} //}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment