Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
project(hd-mapping)

set (HDMAPPING_VERSION_MAJOR 0)
set (HDMAPPING_VERSION_MINOR 96)
set (HDMAPPING_VERSION_MINOR 97)
set (HDMAPPING_VERSION_PATCH 0)

add_definitions(-DHDMAPPING_VERSION_MAJOR=${HDMAPPING_VERSION_MAJOR})
Expand Down
60 changes: 51 additions & 9 deletions apps/lidar_odometry_step_1/lidar_odometry_gui.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
#ifdef _WIN32
#include "resource.h"
#include <windows.h>

#endif

///////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -985,7 +984,7 @@ void settings_gui()
{
ImGui::BeginTooltip();
ImGui::Text("This process makes trajectory smooth, point cloud will be more consistent");
ImGui::SetTooltip("Press optionally before pressing 'Save result'");
ImGui::Text("Press optionally before pressing 'Save result'");
ImGui::EndTooltip();
}

Expand All @@ -996,11 +995,13 @@ void settings_gui()

if (ImGui::Button("Save result"))
save_results(true, 0.0);

ImGui::SameLine();
ImGui::Text(
"Press this button for saving resulting trajectory and point clouds as single session for "
"'multi_view_tls_registration_step_2' program");
if (ImGui::IsItemHovered())
{
ImGui::BeginTooltip();
ImGui::Text("Press this button for saving resulting trajectory and point clouds");
ImGui::Text("as single session for 'multi_view_tls_registration_step_2' program");
ImGui::EndTooltip();
}
}
if (step_1_done && step_2_done)
{
Expand Down Expand Up @@ -1515,6 +1516,18 @@ void progress_window()
ImGui::ProgressBar(progress, ImVec2(-1.0f, 0.0f), progressText);
ImGui::Text("%s", timeInfo);

#ifdef _WIN32
// Update Windows taskbar progress
if (progress > 0.01f && progress < 1.0f)
{
SetTaskbarProgress(progress);
}
else if (progress >= 1.0f)
{
ClearTaskbarProgress();
}
#endif

ImGui::NewLine();

if (!loPause)
Expand Down Expand Up @@ -1873,7 +1886,7 @@ void display()
{
if (ImGui::BeginMenu("Presets"))
{
if (ImGui::MenuItem("1 Velocity < 8km/h, tiny spaces", nullptr, (lastPar == 1)))
if (ImGui::MenuItem("1 Velocity < 8km/h, tiny spaces (default)", nullptr, (lastPar == 1)))
{
lastPar = 1;

Expand Down Expand Up @@ -2336,7 +2349,32 @@ int main(int argc, char* argv[])
return 0;
}

if (argc == 4) // runnning from command line
if (argc == 2) // running from command line
{
auto path = fs::path(argv[1]);
if (is_directory(path))
{
std::string working_directory;
std::vector<WorkerData> worker_data;

std::chrono::time_point<std::chrono::system_clock> start, end;
start = std::chrono::system_clock::now();

std::atomic<bool> loPause{ false };
step1(path.string(), params, pointsPerFile, imu_data, working_directory, trajectory, worker_data, loPause);

step2(worker_data, params, loPause);

end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
std::time_t end_time = std::chrono::system_clock::to_time_t(end);
std::cout << "calculations finished computation at " << std::ctime(&end_time)
<< "Elapsed time: " << formatTime(elapsed_seconds.count()).c_str() << "s\n";

save_results(false, elapsed_seconds.count(), working_directory, worker_data, params, argv[3]);
}
}
else if (argc == 4) // runnning from command line with custom params
{
// Load parameters from file using original TomlIO class
TomlIO toml_io;
Expand Down Expand Up @@ -2369,6 +2407,10 @@ int main(int argc, char* argv[])
initGL(&argc, argv, winTitle, display, mouse);
glutCloseFunc(on_exit);

#ifdef _WIN32
InitTaskbarProgress();
#endif

glutMainLoop();

ImGui_ImplOpenGL2_Shutdown();
Expand Down
40 changes: 20 additions & 20 deletions apps/lidar_odometry_step_1/lidar_odometry_utils_optimizers.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,7 @@ std::vector<std::pair<int, int>> nns(const std::vector<Point3Di>& points_global,
}
}

// std::cout << "------------" << std::endl;
// spdlog::info("------------");
for (size_t y = 0; y < min_distances.size(); y++)
if (indexes_target[y] != -1)
nn.emplace_back(index_element_source, indexes_target[y]);
Expand Down Expand Up @@ -596,12 +596,12 @@ void optimize_sf(

if (points_local.size() > 100)
{
// std::cout << "start adding lidar observations" << std::endl;
// spdlog::info("start adding lidar observations");
if (multithread)
std::for_each(std::execution::par_unseq, std::begin(points_local), std::end(points_local), hessian_fun);
else
std::for_each(std::begin(points_local), std::end(points_local), hessian_fun);
// std::cout << "adding lidar observations finished" << std::endl;
// spdlog::info("adding lidar observations finished");
}

std::vector<std::pair<int, int>> odo_edges;
Expand Down Expand Up @@ -1220,12 +1220,12 @@ void optimize_rigid_sf(

if ((infm.array() > threshold).any())
{
// std::cout << "infm.array() " << infm.array() << std::endl;
// spdlog::info("infm.array() {}", infm.array());
return { Eigen::Matrix<double, 6, 6, Eigen::RowMajor>::Zero(), Eigen::Matrix<double, 6, 1>::Zero(), c };
}
if ((infm.array() < -threshold).any())
{
// std::cout << "infm.array() " << infm.array() << std::endl;
// spdlog::info("infm.array() {}", infm.array());
return { Eigen::Matrix<double, 6, 6, Eigen::RowMajor>::Zero(), Eigen::Matrix<double, 6, 1>::Zero(), c };
}

Expand Down Expand Up @@ -1314,7 +1314,7 @@ void optimize_rigid_sf(

SumBlocks(blocks, AtPAndt, AtPBndt);

// std::cout << "number_of_observations " << number_of_observations << std::endl;
// spdlog::info("number_of_observations {}", number_of_observations);

std::vector<Eigen::Triplet<double>> tripletListA;
std::vector<Eigen::Triplet<double>> tripletListP;
Expand Down Expand Up @@ -1387,7 +1387,7 @@ void optimize_rigid_sf(
{
for (Eigen::SparseMatrix<double>::InnerIterator it(x, k); it; ++it)
{
// std::cout << "it.value() " << it.value() << std::endl;
// spdlog::info("it.value() {}", it.value());
h_x.push_back(it.value());
}
}
Expand Down Expand Up @@ -2083,7 +2083,7 @@ void optimize_lidar_odometry(
Eigen::Vector3d p1(prev_pose.px, prev_pose.py, prev_pose.pz);
Eigen::Vector3d p2(pose.px, pose.py, pose.pz);

// std::cout << "(p1 - p2).norm() " << (p1 - p2).norm() << std::endl;
// spdlog::info("(p1 - p2).norm() {}", (p1 - p2).norm());

if ((p1 - p2).norm() < 1.0)
intermediate_trajectory[i] = affine_matrix_from_pose_tait_bryan(pose);
Expand Down Expand Up @@ -2513,7 +2513,7 @@ bool compute_step_2(
lookup_stats);
if (delta < params.convergence_delta_threshold)
{
// std::cout << "finished at iteration " << iter + 1 << "/" << params.nr_iter;
// spdlog::info("finished at iteration {}/{}", iter + 1, params.nr_iter);
break;
}

Expand All @@ -2538,24 +2538,24 @@ bool compute_step_2(

if (delta > params.convergence_delta_threshold)
spdlog::info(
"finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta {:e}!!!",
iter_end + 1,
params.nr_iter,
"finished optimizing worker_data {}/{} at iteration {}/{} in {:.3f}[s] with acc_distance {:.3f}[m], delta {:e}!!!",
i + 1,
worker_data.size(),
acc_distance,
iter_end + 1,
params.nr_iter,
elapsed_seconds1,
acc_distance,
delta);
else
spdlog::info(
"finished at iteration {}/{} optimizing worker_data {}/{} with acc_distance {:.3f}[m] in {:.3f}[s], delta < {:.1e} "
"finished optimizing worker_data {}/{} at iteration {}/{} in {:.3f}[s] with acc_distance {:.3f}[m], delta<{:.1e} "
"(converged)",
iter_end + 1,
params.nr_iter,
i + 1,
worker_data.size(),
acc_distance,
iter_end + 1,
params.nr_iter,
elapsed_seconds1,
acc_distance,
params.convergence_delta_threshold);

loProgress.store((float)(i + 1) / worker_data.size());
Expand Down Expand Up @@ -3456,7 +3456,7 @@ void Consistency2(std::vector<WorkerData>& worker_data, const LidarOdometryParam
}
spdlog::info("build regular grid decomposition FINISHED");

spdlog::info("ndt observations start START");
spdlog::info("NDT observations start START");

counter = 0;

Expand Down Expand Up @@ -3494,7 +3494,7 @@ void Consistency2(std::vector<WorkerData>& worker_data, const LidarOdometryParam
Eigen::Vector3d viewport = trajectory[points_global[i].index_pose].translation();
if (nv.dot(viewport - this_bucket.mean) < 0)
{
// std::cout << "nv!";
// spdlog::warning("nv!");
continue;
}

Expand Down Expand Up @@ -3611,7 +3611,7 @@ void Consistency2(std::vector<WorkerData>& worker_data, const LidarOdometryParam
AtPB += AtPBndt;
}

spdlog::info("ndt observations start FINISHED");
spdlog::info("NDT observations start FINISHED");

Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);
Eigen::SparseMatrix<double> x = solver.solve(AtPB);
Expand Down
8 changes: 4 additions & 4 deletions apps/lidar_odometry_step_1/resource.rc
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ IDI_ICON1 ICON "icon.ico"
//

VS_VERSION_INFO VERSIONINFO
FILEVERSION 0, 0, 9, 6
PRODUCTVERSION 0, 0, 9, 6
FILEVERSION 0, 0, 9, 7
PRODUCTVERSION 0, 0, 9, 7
FILEFLAGSMASK 0x3fL
FILEOS 0x40004
FILETYPE 0x1
Expand All @@ -32,11 +32,11 @@ BLOCK "040904B0"
BEGIN
VALUE "CompanyName", "Mandeye\0"
VALUE "FileDescription", "HDMapping Step 1\0"
VALUE "FileVersion", "0.9.6\0"
VALUE "FileVersion", "0.9.7\0"
VALUE "InternalName", "Odometry\0"
VALUE "LegalCopyright", "(c) 2026 github.com/MapsHD/HDMapping\0"
VALUE "OriginalFilename", "lidar_odometry_step_1.exe\0"
VALUE "ProductVersion", "0.9.6\0"
VALUE "ProductVersion", "0.9.7\0"
VALUE "ProgramID", "github.com/MapsHD/HDMapping\0"
VALUE "ProductName", "HDMapping\0"
END
Expand Down
Loading