Skip to content

Fixed bugs/updates/refinements #276

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
JanuszBedkowski merged 1 commit into from
Dec 7, 2025
Merged

Fixed bugs/updates/refinements #276

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions apps/lidar_odometry_step_1/lidar_odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -999,8 +999,7 @@ void load_reference_point_clouds(std::vector<std::string> input_file_names, Lida

std::string save_results_automatic(LidarOdometryParams &params, std::vector<WorkerData> &worker_data, std::string working_directory, double elapsed_seconds)
{
int result = get_next_result_id(working_directory);
fs::path outwd = working_directory / fs::path("lidar_odometry_result_" + std::to_string(result));
fs::path outwd = get_next_result_path(working_directory);
save_result(worker_data, params, outwd, elapsed_seconds);
return outwd.string();
}
Expand Down
47 changes: 23 additions & 24 deletions apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -277,7 +277,7 @@ std::vector<std::vector<Point3Di>> get_batches_of_points(std::string laz_file, i

std::vector<Point3Di> tmp_points = prev_points;
int counter = tmp_points.size();
for (int i = 0; i < points.size(); i++)
for (size_t i = 0; i < points.size(); i++)
{
counter++;
tmp_points.push_back(points[i]);
Expand Down Expand Up @@ -312,7 +312,7 @@ int get_index(set<int> s, int k)

void find_best_stretch(std::vector<Point3Di> points, std::vector<double> timestamps, std::vector<Eigen::Affine3d> poses, std::string fn1, std::string fn2)
{
for (int i = 0; i < points.size(); i++)
for (size_t i = 0; i < points.size(); i++)
{
auto lower = std::lower_bound(timestamps.begin(), timestamps.end(), points[i].timestamp);
points[i].index_pose = std::distance(timestamps.begin(), lower);
Expand All @@ -321,7 +321,7 @@ void find_best_stretch(std::vector<Point3Di> points, std::vector<double> timesta

std::set<int> indexes;

for (int i = 0; i < points.size(); i++)
for (size_t i = 0; i < points.size(); i++)
{
indexes.insert(points[i].index_pose);
}
Expand All @@ -339,7 +339,7 @@ void find_best_stretch(std::vector<Point3Di> points, std::vector<double> timesta
// Sleep(2000);

std::vector<Point3Di> points_reindexed = points;
for (int i = 0; i < points_reindexed.size(); i++)
for (size_t i = 0; i < points_reindexed.size(); i++)
{
points_reindexed[i].index_pose = get_index(indexes, points[i].index_pose);
}
Expand All @@ -356,7 +356,7 @@ void find_best_stretch(std::vector<Point3Di> points, std::vector<double> timesta

Eigen::Affine3d m = trajectory[0];
trajectory_stretched.push_back(m);
for (int i = 1; i < trajectory.size(); i++)
for (size_t i = 1; i < trajectory.size(); i++)
{
Eigen::Affine3d m_update = trajectory[i - 1].inverse() * trajectory[i] * (m_x_offset);
m = m * m_update;
Expand Down Expand Up @@ -396,7 +396,7 @@ void find_best_stretch(std::vector<Point3Di> points, std::vector<double> timesta
}

std::map<double, Eigen::Matrix4d> trajectory_for_interpolation;
for (int i = 0; i < best_trajectory.size(); i++)
for (size_t i = 0; i < best_trajectory.size(); i++)
{
trajectory_for_interpolation[ts[i]] = best_trajectory[i].matrix();
}
Expand Down Expand Up @@ -546,7 +546,7 @@ void alternative_approach()
all_points.push_back(tmp_points[i]);
}

for (int i = 1; i < laz_files.size(); i++)
for (size_t i = 1; i < laz_files.size(); i++)
{
prev_points = tmp_points[tmp_points.size() - 1];
tmp_points = get_batches_of_points(laz_files[i], point_count_threshold, prev_points);
Expand All @@ -567,7 +567,7 @@ void alternative_approach()
poses.push_back(m);
}

for (int i = 0; i < all_points.size(); i++)
for (size_t i = 0; i < all_points.size(); i++)
{
std::cout << all_points[i].size() << std::endl;
std::string fn1 = "C:/data/tmp/" + std::to_string(i) + "_best.laz";
Expand Down Expand Up @@ -665,8 +665,7 @@ void step2(const std::atomic<bool> &loPause)

void save_results(bool info, double elapsed_seconds)
{
int result = get_next_result_id(working_directory);
fs::path outwd = working_directory / fs::path("lidar_odometry_result_" + std::to_string(result));
fs::path outwd = get_next_result_path(working_directory);
save_result(worker_data, params, outwd, elapsed_seconds);
if (info)
{
Expand Down Expand Up @@ -1185,15 +1184,15 @@ void project_gui()
ImGui::Text("Selection: ");
if (ImGui::Button("Select all"))
{
for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
{
worker_data[k].show = true;
}
}
ImGui::SameLine();
if (ImGui::Button("Unselect"))
{
for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
{
worker_data[k].show = false;
}
Expand All @@ -1203,7 +1202,7 @@ void project_gui()
ImGui::SameLine();
if (ImGui::Button("<from, to>"))
{
for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
worker_data[k].show = (k >= index_begin && k <= index_end);
}

Expand All @@ -1214,7 +1213,7 @@ void project_gui()
index_begin--;
index_end--;

for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
worker_data[k].show = (k >= index_begin && k <= index_end);
}
}
Expand All @@ -1226,7 +1225,7 @@ void project_gui()
index_begin++;
index_end++;

for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
worker_data[k].show = (k >= index_begin && k <= index_end);
}
}
Expand All @@ -1238,7 +1237,7 @@ void project_gui()
index_begin -= 10;
index_end -= 10;

for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
worker_data[k].show = (k >= index_begin && k <= index_end);
}
}
Expand All @@ -1250,7 +1249,7 @@ void project_gui()
index_begin += 10;
index_end += 10;

for (int k = 0; k < worker_data.size(); k++)
for (size_t k = 0; k < worker_data.size(); k++)
worker_data[k].show = (k >= index_begin && k <= index_end);
}
}
Expand Down Expand Up @@ -1371,10 +1370,10 @@ void project_gui()
}

std::vector<std::vector<Eigen::Affine3d>> all_poses;
for (int i = 0; i < worker_data.size(); i++)
for (size_t i = 0; i < worker_data.size(); i++)
{
std::vector<Eigen::Affine3d> poses;
for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++)
for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++)
{
poses.push_back(worker_data[i].intermediate_trajectory[j]);
}
Expand All @@ -1387,7 +1386,7 @@ void project_gui()

for (int i = index_begin; i < index_end; i++)
{
for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++)
for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++)
{
TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(worker_data[i].intermediate_trajectory[j]);

Expand All @@ -1403,9 +1402,9 @@ void project_gui()
}
}

for (int i = index_end; i < worker_data.size(); i++)
for (size_t i = index_end; i < worker_data.size(); i++)
{
for (int j = 0; j < worker_data[i].intermediate_trajectory.size(); j++)
for (size_t j = 0; j < worker_data[i].intermediate_trajectory.size(); j++)
{
Eigen::Affine3d m_update = Eigen::Affine3d::Identity();

Expand Down Expand Up @@ -1674,7 +1673,7 @@ void display()
}

#if 0 // ToDo
for (int i = 0; i < worker_data.size(); i++)
for (size_t i = 0; i < worker_data.size(); i++)
{
if (worker_data[i].show)
{
Expand Down Expand Up @@ -1711,7 +1710,7 @@ void display()
{
glColor3f(1, 0, 0);
glBegin(GL_POINTS);
for (int i = 0; i < params.reference_points.size(); i += dec_reference_points)
for (size_t i = 0; i < params.reference_points.size(); i += dec_reference_points)
glVertex3f(params.reference_points[i].point.x(), params.reference_points[i].point.y(), params.reference_points[i].point.z());
glEnd();
}
Expand Down
6 changes: 3 additions & 3 deletions apps/lidar_odometry_step_1/lidar_odometry_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -740,9 +740,9 @@ int MLvxCalib::GetImuIdToUse(const std::unordered_map<int, std::string> &idToSn,
return 0;
}

int get_next_result_id(const std::string working_directory)
fs::path get_next_result_path(const std::string working_directory)
{
std::regex pattern(R"(lidar_odometry_result_(\d+))");
std::regex pattern(R"(lio_result_(\d+))");
int max_number = -1;
for (const auto &entry : fs::directory_iterator(working_directory))
{
Expand All @@ -758,7 +758,7 @@ int get_next_result_id(const std::string working_directory)
}
}
}
return max_number + 1;
return (working_directory / fs::path("lio_result_" + std::to_string(max_number + 1)));
}

bool loadLaz(const std::string &filename, std::vector<Point3Di> &points_out, std::vector<int> index_poses_i, std::vector<Eigen::Affine3d> &intermediate_trajectory, const Eigen::Affine3d &m_pose)
Expand Down
2 changes: 1 addition & 1 deletion apps/lidar_odometry_step_1/lidar_odometry_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,7 +189,7 @@ std::vector<Point3Di> load_point_cloud(const std::string &lazFile, bool ommit_po

bool save_poses(const std::string file_name, std::vector<Eigen::Affine3d> m_poses, std::vector<std::string> filenames);

int get_next_result_id(const std::string working_directory);
fs::path get_next_result_path(const std::string working_directory);

// this function performs main LiDAR odometry calculations
void optimize_lidar_odometry(std::vector<Point3Di> &intermediate_points, std::vector<Eigen::Affine3d> &intermediate_trajectory,
Expand Down
12 changes: 6 additions & 6 deletions apps/mandeye_mission_recorder_calibration/mandeye_mission_recorder_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -339,7 +339,7 @@ void project_gui()
input_file_names = mandeye::fd::OpenFileDialog("Point cloud files", mandeye::fd::LAS_LAZ_filter, true);

if (input_file_names.size() > 0)
for (int i = 0; i < input_file_names.size(); i++)
for (size_t i = 0; i < input_file_names.size(); i++)
load_pc(input_file_names[i].c_str(), point_cloud, true, filter_threshold_xy);

// Initialize imu_lidar according to imuSnToUse
Expand Down Expand Up @@ -390,15 +390,15 @@ void project_gui()
ImGui::Separator();
ImGui::Text("Select LiDAR to calibrate:");

for (int i = 0; i < calibrated_lidar.size(); i++)
for (size_t i = 0; i < calibrated_lidar.size(); i++)
{
std::string name = idToSn.at(i);
ImGui::RadioButton(name.c_str(), &chosen_lidar, i);
}

if (chosen_lidar != -1)
{
for (int i = 0; i < calibrated_lidar.size(); i++)
for (size_t i = 0; i < calibrated_lidar.size(); i++)
calibrated_lidar[i].check = (i == chosen_lidar);
}
}
Expand Down Expand Up @@ -540,7 +540,7 @@ void project_gui()
if (ImGui::IsItemHovered())
ImGui::SetTooltip(hintText);

for (int i = 0; i < imu_lidar.size(); i++)
for (size_t i = 0; i < imu_lidar.size(); i++)
{
std::string name = idToSn.at(i);
ImGui::RadioButton(std::string(name + "##imu").c_str(), &chosen_imu, i);
Expand All @@ -550,7 +550,7 @@ void project_gui()

if (chosen_imu != -1)
{
for (int i = 0; i < imu_lidar.size(); i++)
for (size_t i = 0; i < imu_lidar.size(); i++)
imu_lidar[i].check = (i == chosen_imu);
}

Expand Down Expand Up @@ -727,7 +727,7 @@ void display()
if (manual_calibration)
{
int index_calibrated_lidar = -1;
for (int i = 0; i < calibrated_lidar.size(); i++)
for (size_t i = 0; i < calibrated_lidar.size(); i++)
{
if (calibrated_lidar[i].check)
index_calibrated_lidar = i;
Expand Down
Loading