Skip to content

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

unload block

parent ccc0814e
Hide whitespace changes
Inline Side-by-side
Showing with 78 additions and 78 deletions
libs/voxel_map/libs/utils/basic_functions.cpp
View file @ c7dde94b
...@@ -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