Skip to content

Commit

Permalink
work on lo
Browse files Browse the repository at this point in the history
  • Loading branch information
@JanuszBedkowski
JanuszBedkowski committed Jan 18, 2025
1 parent 0fa1fd6 commit 12a2d45
Show file tree
Hide file tree
Showing 9 changed files with 3,683 additions and 589 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,7 @@ add_subdirectory(apps/compare_trajectories)
add_subdirectory(apps/quick_start_demo)
add_subdirectory(apps/mandeye_mission_recorder_calibration)
add_subdirectory(apps/mandeye_single_session_viewer)
add_subdirectory(apps/livox_mid_360_intrinsic_calibration)

# CPack configuration
set(CPACK_PACKAGE_NAME "hd_mapping")
Expand Down
204 changes: 8 additions & 196 deletions apps/lidar_odometry_step_1/lidar_odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -994,6 +994,14 @@ void lidar_odometry_gui()
}
if (!simple_gui)
{
ImGui::Text("------- robust and accurate lidar odometry -----------");
ImGui::Checkbox("use_robust_and_accurate_lidar_odometry", &params.use_robust_and_accurate_lidar_odometry);
ImGui::InputDouble("distance_bucket", &params.distance_bucket);
ImGui::InputDouble("polar_angle_deg", &params.polar_angle_deg);
ImGui::InputDouble("azimutal_angle_deg", &params.azimutal_angle_deg);
ImGui::InputInt("number of iterations", &params.robust_and_accurate_lidar_odometry_iterations);
ImGui::InputDouble("max distance lidar", &params.max_distance_lidar);
ImGui::Text("------------------------------------------------------");
// if (step_1_done && step_2_done)
//{
/*ImGui::Text("'Consistency' makes trajectory smooth, point cloud will be more consistent");
Expand Down Expand Up @@ -2369,28 +2377,6 @@ void alternative_approach()
std::cout << fn << std::endl;
}

// for (const auto &fn : laz_files)
//{
// std::vector<Point3Di> points = load_point_cloud(fn);
// std::cout << "points.cloud(): " << points.size() << std::endl;
//}

/*int current_file_index = 0;
int current_point_index = 0;
std::vector<Point3Di> points;
bool do_not_stop = true;
while (do_not_stop){
bool do_not_stop_internal_loop = true;
while (do_not_stop_internal_loop){
bool collected_all_points = get_next_batch_of_points(
point_count_threshold, laz_files, current_file_index, int &current_point_index_offset,
std::vector<Point3Di> &points)
}
}*/
// get_next_batch_of_points(point_count_threshold, laz_files, current_file_index, current_point_index, points);

std::vector<Point3Di> prev_points;
std::vector<std::vector<Point3Di>> all_points;
std::vector<std::vector<Point3Di>> tmp_points = get_batches_of_points(laz_files[0], point_count_threshold, prev_points);
Expand Down Expand Up @@ -2508,177 +2494,3 @@ Eigen::Vector3d rayIntersection(const LaserBeam &laser_beam, const RegistrationP

return out_point;
}

#if 0
bool exportLaz(const std::string &filename,
const std::vector<Eigen::Vector3d> &pointcloud,
const std::vector<unsigned short> &intensity,
const std::vector<double> &timestamps,
double offset_x, double offset_y, double offset_alt)
{

constexpr float scale = 0.0001f; // one tenth of milimeter
// find max
Eigen::Vector3d _max(-1000000000.0, -1000000000.0, -1000000000.0);
Eigen::Vector3d _min(1000000000.0, 1000000000.0, 1000000000.0);

for (auto &p : pointcloud)
{
if (p.x() < _min.x())
{
_min.x() = p.x();
}
if (p.y() < _min.y())
{
_min.y() = p.y();
}
if (p.z() < _min.z())
{
_min.z() = p.z();
}

if (p.x() > _max.x())
{
_max.x() = p.x();
}
if (p.y() > _max.y())
{
_max.y() = p.y();
}
if (p.z() > _max.z())
{
_max.z() = p.z();
}
}

// create the writer
laszip_POINTER laszip_writer;
if (laszip_create(&laszip_writer))
{
fprintf(stderr, "DLL ERROR: creating laszip writer\n");
return false;
}

// get a pointer to the header of the writer so we can populate it

laszip_header *header;

if (laszip_get_header_pointer(laszip_writer, &header))
{
fprintf(stderr, "DLL ERROR: getting header pointer from laszip writer\n");
return false;
}

// populate the header

header->file_source_ID = 4711;
header->global_encoding = (1 << 0); // see LAS specification for details
header->version_major = 1;
header->version_minor = 2;
// header->file_creation_day = 120;
// header->file_creation_year = 2013;
header->point_data_format = 1;
header->point_data_record_length = 0;
header->number_of_point_records = pointcloud.size();
header->number_of_points_by_return[0] = pointcloud.size();
header->number_of_points_by_return[1] = 0;
header->point_data_record_length = 28;
header->x_scale_factor = scale;
header->y_scale_factor = scale;
header->z_scale_factor = scale;

header->max_x = _max.x() + offset_x;
header->min_x = _min.x() + offset_x;
header->max_y = _max.y() + offset_y;
header->min_y = _min.y() + offset_y;
header->max_z = _max.z() + offset_alt;
header->min_z = _min.z() + offset_alt;

header->x_offset = offset_x;
header->y_offset = offset_y;
header->z_offset = offset_alt;

// optional: use the bounding box and the scale factor to create a "good" offset
// open the writer
laszip_BOOL compress = (strstr(filename.c_str(), ".laz") != 0);

if (laszip_open_writer(laszip_writer, filename.c_str(), compress))
{
fprintf(stderr, "DLL ERROR: opening laszip writer for '%s'\n", filename.c_str());
return false;
}

fprintf(stderr, "writing file '%s' %scompressed\n", filename.c_str(), (compress ? "" : "un"));

// get a pointer to the point of the writer that we will populate and write

laszip_point *point;
if (laszip_get_point_pointer(laszip_writer, &point))
{
fprintf(stderr, "DLL ERROR: getting point pointer from laszip writer\n");
return false;
}

laszip_I64 p_count = 0;
laszip_F64 coordinates[3];

for (int i = 0; i < pointcloud.size(); i++)
{
point->intensity = intensity[i];

const auto &p = pointcloud[i];
point->gps_time = timestamps[i] * 1e9;

p_count++;
coordinates[0] = p.x() + offset_x;
coordinates[1] = p.y() + offset_y;
coordinates[2] = p.z() + offset_alt;
if (laszip_set_coordinates(laszip_writer, coordinates))
{
fprintf(stderr, "DLL ERROR: setting coordinates for point %I64d\n", p_count);
return false;
}

// p.SetIntensity(pp.intensity);

// if (i < intensity.size()) {
// point->intensity = intensity[i];
// }
// laszip_set_point

if (laszip_write_point(laszip_writer))
{
fprintf(stderr, "DLL ERROR: writing point %I64d\n", p_count);
return false;
}
}

if (laszip_get_point_count(laszip_writer, &p_count))
{
fprintf(stderr, "DLL ERROR: getting point count\n");
return false;
}

fprintf(stderr, "successfully written %I64d points\n", p_count);

// close the writer

if (laszip_close_writer(laszip_writer))
{
fprintf(stderr, "DLL ERROR: closing laszip writer\n");
return false;
}

// destroy the writer

if (laszip_destroy(laszip_writer))
{
fprintf(stderr, "DLL ERROR: destroying laszip writer\n");
return false;
}

std::cout << "exportLaz DONE" << std::endl;

return true;
}
#endif
76 changes: 76 additions & 0 deletions apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,6 +183,82 @@ void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets,
}
}

void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets,
std::vector<Point3Di> &points_global, std::vector<Eigen::Vector3d> &points_global_spherical, Eigen::Vector3d viewport)
{
Eigen::Vector3d b(rgd_params.resolution_X, rgd_params.resolution_Y, rgd_params.resolution_Z);

for (int i = 0; i < points_global.size(); i++)
{
auto index_of_bucket = get_rgd_index(points_global_spherical[i], b);

auto bucket_it = buckets.find(index_of_bucket);

if (bucket_it != buckets.end())
{
auto &this_bucket = bucket_it->second;
this_bucket.number_of_points++;
const auto &curr_mean = points_global[i].point;
const auto &mean = this_bucket.mean;
// buckets[index_of_bucket].mean += (mean - curr_mean) / buckets[index_of_bucket].number_of_points;

auto mean_diff = mean - curr_mean;
Eigen::Matrix3d cov_update;
cov_update.row(0) = mean_diff.x() * mean_diff;
cov_update.row(1) = mean_diff.y() * mean_diff;
cov_update.row(2) = mean_diff.z() * mean_diff;

// this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points +
// cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points);

if (this_bucket.number_of_points == 2)
{
this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points +
cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points);
}

if (this_bucket.number_of_points == 3)
{
this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points +
cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points);

// calculate normal vector
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver(this_bucket.cov, Eigen::ComputeEigenvectors);
Eigen::Matrix3d eigenVectorsPCA = eigen_solver.eigenvectors();

Eigen::Vector3d nv = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2));
nv.normalize();

// flip towards viewport
if (nv.dot(viewport - this_bucket.mean) < 0.0)
{
nv *= -1.0;
}
this_bucket.normal_vector = nv;
}

if (this_bucket.number_of_points > 3)
{
Eigen::Vector3d &nv = this_bucket.normal_vector;

if (nv.dot(viewport - this_bucket.mean) >= 0.0)
{
this_bucket.cov = this_bucket.cov * (this_bucket.number_of_points - 1) / this_bucket.number_of_points +
cov_update * (this_bucket.number_of_points - 1) / (this_bucket.number_of_points * this_bucket.number_of_points);
}
}
}
else
{
NDT::Bucket bucket_to_add;
bucket_to_add.mean = points_global[i].point;
bucket_to_add.cov = Eigen::Matrix3d::Identity() * 0.03 * 0.03;
bucket_to_add.number_of_points = 1;
buckets.emplace(index_of_bucket, bucket_to_add);
}
}
}

bool saveLaz(const std::string &filename, const WorkerData &data, double threshould_output_filter)
{
constexpr float scale = 0.0001f; // one tenth of milimeter
Expand Down
19 changes: 19 additions & 0 deletions apps/lidar_odometry_step_1/lidar_odometry_utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,14 @@ struct LidarOdometryParams
double sliding_window_trajectory_length_threshold = 50.0;
bool save_calibration_validation = true;
int calibration_validation_points = 1000000;

//rgd_sf
bool use_robust_and_accurate_lidar_odometry = false;
double distance_bucket = 0.5;
double polar_angle_deg = 5.0;
double azimutal_angle_deg = 5.0;
int robust_and_accurate_lidar_odometry_iterations = 20;
double max_distance_lidar = 30.0;
};

unsigned long long int get_index(const int16_t x, const int16_t y, const int16_t z);
Expand All @@ -81,6 +89,9 @@ std::vector<Point3Di> decimate(const std::vector<Point3Di> &points, double bucke
void update_rgd(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets,
std::vector<Point3Di> &points_global, Eigen::Vector3d viewport = Eigen::Vector3d(0, 0, 0));

void update_rgd_spherical_coordinates(NDT::GridParameters &rgd_params, NDTBucketMapType &buckets,
std::vector<Point3Di> &points_global, std::vector<Eigen::Vector3d> &points_global_spherical, Eigen::Vector3d viewport);

//! This function load inertial measurement unit data.
//! This function expects a file with the following format:
//! timestamp angular_velocity_x angular_velocity_y angular_velocity_z linear_acceleration_x linear_acceleration_y linear_acceleration_z imu_id
Expand Down Expand Up @@ -116,6 +127,14 @@ void optimize(std::vector<Point3Di> &intermediate_points, std::vector<Eigen::Aff
bool add_pitch_roll_constraint, const std::vector<std::pair<double, double>> &imu_roll_pitch*/
);

void optimize_sf(std::vector<Point3Di> &intermediate_points, std::vector<Eigen::Affine3d> &intermediate_trajectory,
std::vector<Eigen::Affine3d> &intermediate_trajectory_motion_model,
NDT::GridParameters &rgd_params, NDTBucketMapType &buckets, bool useMultithread );

void optimize_sf2(std::vector<Point3Di> &intermediate_points, std::vector<Point3Di> &intermediate_points_sf, std::vector<Eigen::Affine3d> &intermediate_trajectory,
std::vector<Eigen::Affine3d> &intermediate_trajectory_motion_model,
NDT::GridParameters &rgd_params, bool useMultithread);

void optimize_icp(std::vector<Point3Di> &intermediate_points, std::vector<Eigen::Affine3d> &intermediate_trajectory,
std::vector<Eigen::Affine3d> &intermediate_trajectory_motion_model,
NDT::GridParameters &rgd_params, /*NDTBucketMapType &buckets*/ std::vector<Point3Di> points_global, bool useMultithread /*,
Expand Down
Loading

0 comments on commit 12a2d45

Please sign in to comment.