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,
return ret;
}
Matrix3d get_rotation(double old_B, double old_L, double old_H,
double new_B, double new_L, double new_H,
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)
{
GeographicLib::LocalCartesian old_cartesian(old_B, old_L, old_H);
GeographicLib::LocalCartesian new_cartesian(new_B, new_L, new_H);
//Matrix3d get_rotation(double old_B, double old_L, double old_H,
// double new_B, double new_L, double new_H,
// 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)
//{
// GeographicLib::LocalCartesian old_cartesian(old_B, old_L, old_H);
// GeographicLib::LocalCartesian new_cartesian(new_B, new_L, new_H);
#define reproj(before, after) \
{ \
double B = 0, L = 0, H = 0; \
after[0] = before[0] + old_start_x; \
after[1] = before[1] + old_start_y; \
after[2] = before[2] + old_start_z; \
old_cartesian.Reverse(after[0], after[1], after[2], B, L, H); \
new_cartesian.Forward(B, L, H, after[0], after[1], after[2]); \
after[0] -= new_start_x; \
after[1] -= new_start_y; \
after[2] -= new_start_z; \
if (print) \
{ \
printf("(%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \
before[0], before[1], before[2], \
after[0], after[1], after[2]); \
LOG(INFO) <<setprecision(15) \
<< before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \
<< after[0]<< ", " << after[1] << ", " << after[2]; \
} \
}
//#define reproj(before, after) \
// { \
// double B = 0, L = 0, H = 0; \
// after[0] = before[0] + old_start_x; \
// after[1] = before[1] + old_start_y; \
// after[2] = before[2] + old_start_z; \
// old_cartesian.Reverse(after[0], after[1], after[2], B, L, H); \
// new_cartesian.Forward(B, L, H, after[0], after[1], after[2]); \
// after[0] -= new_start_x; \
// after[1] -= new_start_y; \
// after[2] -= new_start_z; \
// if (print) \
// { \
// printf("(%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \
// before[0], before[1], before[2], \
// after[0], after[1], after[2]); \
// LOG(INFO) <<setprecision(15) \
// << before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \
// << after[0]<< ", " << after[1] << ", " << after[2]; \
//} \
//}
#define rand1000() (rand() % 2000 - 1000)
//#define rand1000() (rand() % 2000 - 1000)
ceres::Problem problem;
Eigen::Quaterniond q_R = Eigen::Quaterniond::Identity();
Eigen::Vector3d translation = Eigen::Vector3d::Zero();
// ceres::Problem problem;
// Eigen::Quaterniond q_R = Eigen::Quaterniond::Identity();
// Eigen::Vector3d translation = Eigen::Vector3d::Zero();
std::vector<Eigen::Vector3d> points;
std::vector<Eigen::Vector3d> new_points;
for (int i = 0; i < 10; i++)
{
Eigen::Vector3d p(rand1000(), rand1000(), rand1000());
if (i == 0)
p = Eigen::Vector3d::Zero();
Eigen::Vector3d new_p;
reproj(p, new_p);
if (i == 0)
translation = new_p;
points.push_back(p);
new_points.push_back(new_p);
// std::vector<Eigen::Vector3d> points;
// std::vector<Eigen::Vector3d> new_points;
// for (int i = 0; i < 10; i++)
// {
// Eigen::Vector3d p(rand1000(), rand1000(), rand1000());
// if (i == 0)
// p = Eigen::Vector3d::Zero();
// Eigen::Vector3d new_p;
// reproj(p, new_p);
// if (i == 0)
// translation = new_p;
// points.push_back(p);
// new_points.push_back(new_p);
ceres::CostFunction *func = DistanceError::Create(p, new_p);
problem.AddResidualBlock(func, NULL, q_R.coeffs().data(), translation.data());
}
// ceres::CostFunction *func = DistanceError::Create(p, new_p);
// problem.AddResidualBlock(func, NULL, q_R.coeffs().data(), translation.data());
// }
auto *quaternion_parameterization =
new ceres::EigenQuaternionParameterization;
problem.SetParameterization(q_R.coeffs().data(), quaternion_parameterization);
// auto *quaternion_parameterization =
// new ceres::EigenQuaternionParameterization;
// problem.SetParameterization(q_R.coeffs().data(), quaternion_parameterization);
ceres::Solver::Options options;
options.use_nonmonotonic_steps = false;
options.max_num_iterations = 50;
options.num_threads = 10;
options.num_linear_solver_threads = options.num_threads;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
// ceres::Solver::Options options;
// options.use_nonmonotonic_steps = false;
// options.max_num_iterations = 50;
// options.num_threads = 10;
// options.num_linear_solver_threads = options.num_threads;
// ceres::Solver::Summary summary;
// ceres::Solve(options, &problem, &summary);
Eigen::Matrix3d rotation = q_R.toRotationMatrix();
// Eigen::Matrix3d rotation = q_R.toRotationMatrix();
if (print) {
#define test(before) \
{ \
Eigen::Vector3d after = translation + rotation * before; \
printf("test (%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \
before[0], before[1], before[2], \
after[0], after[1], after[2]); \
LOG(INFO) <<setprecision(15) \
<< before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \
<< after[0]<< ", " << after[1] << ", " << after[2]; \
}
for (int i = 0; i < 10; i++)
{
test(points[i]);
}
Eigen::Vector3d euler = matrix_2_euler_rpy(rotation);
}
return rotation;
}
// if (print) {
//#define test(before) \
// { \
// Eigen::Vector3d after = translation + rotation * before; \
// printf("test (%.3f, %.3f, %.3f) -> (%.3f, %.3f, %.3f)\n", \
// before[0], before[1], before[2], \
// after[0], after[1], after[2]); \
// LOG(INFO) <<setprecision(15) \
// << before[0]<< ", " << before[1]<< ", " <<before[2]<< ", " \
// << after[0]<< ", " << after[1] << ", " << after[2]; \
// }
// for (int i = 0; i < 10; i++)
// {
// test(points[i]);
// }
// Eigen::Vector3d euler = matrix_2_euler_rpy(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