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
15 changes: 0 additions & 15 deletions apps/compare_trajectories/mandeye_compare_trajectories.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -496,32 +496,17 @@ void project_gui()
}
if (ImGui::Button("(Step 3) Calculate ATE (Absolute Trajectory Error)"))
{
// std::cout << std::setprecision(20);
// for (const auto &p : Data::trajectory_est)
//{
// std::cout << p.first << std::endl;
// }
// std::cout << " ---------------------------" << std::endl;
// for (const auto &p : Data::trajectory_gt)
//{
// std::cout << p.first << std::endl;
// }

double ATE = 0.0;
double max_ATE = -1000000.0;
double min_ATE = 1000000.0;
int count = 0;
for (const auto& p : Data::trajectory_est)
{
// std::cout << p.second.matrix() << std::endl;

auto it = getInterpolatedPose(Data::trajectory_gt, p.first);
if (!it.isZero())
{
auto tp = Data::trajectory_offset * p.second;
// std::cout << tp.matrix() << " " << std::endl;

// std::cout << "ATE " << ATE << std::endl;
double ate = (tp - it).norm();

if (ate > max_ATE)
Expand Down
14 changes: 7 additions & 7 deletions apps/console_tools/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ target_include_directories(
matrix_mul PRIVATE include ${REPOSITORY_DIRECTORY}/core/include
${EIGEN3_INCLUDE_DIR})

target_link_libraries(matrix_mul PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS})
target_link_libraries(matrix_mul PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog)


add_executable(multiply_timestamps_session_point_cloud_laz multiply_timestamps_session_point_cloud_laz.cpp)
Expand All @@ -18,28 +18,28 @@ target_include_directories(
${EIGEN3_INCLUDE_DIR}
${LASZIP_INCLUDE_DIR}/LASzip/include)

target_link_libraries(multiply_timestamps_session_point_cloud_laz PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} ${PLATFORM_LASZIP_LIB})
target_link_libraries(multiply_timestamps_session_point_cloud_laz PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} ${PLATFORM_LASZIP_LIB} spdlog::spdlog)

add_executable(multiply_timestamps_session_trajectory_csv multiply_timestamps_session_trajectory_csv.cpp)

target_include_directories(
multiply_timestamps_session_trajectory_csv PRIVATE include ${REPOSITORY_DIRECTORY}/core/include
${EIGEN3_INCLUDE_DIR})

target_link_libraries(multiply_timestamps_session_trajectory_csv PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS})
target_link_libraries(multiply_timestamps_session_trajectory_csv PRIVATE ${PLATFORM_MISCELLANEOUS_LIBS} spdlog::spdlog)

add_executable(laz_to_ply laz_to_ply.cpp)
target_link_libraries(laz_to_ply PRIVATE ${PLATFORM_LASZIP_LIB})
target_link_libraries(laz_to_ply PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog)
target_include_directories(laz_to_ply PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include)

add_executable(laz_to_pcd laz_to_pcd.cpp)
target_link_libraries(laz_to_pcd PRIVATE ${PLATFORM_LASZIP_LIB})
target_link_libraries(laz_to_pcd PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog)
target_include_directories(laz_to_pcd PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include)

add_executable(pcd_to_laz pcd_to_laz.cpp)
target_link_libraries(pcd_to_laz PRIVATE ${PLATFORM_LASZIP_LIB})
target_link_libraries(pcd_to_laz PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog)
target_include_directories(pcd_to_laz PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include)

add_executable(laz_to_txt laz_to_txt.cpp)
target_link_libraries(laz_to_txt PRIVATE ${PLATFORM_LASZIP_LIB})
target_link_libraries(laz_to_txt PRIVATE ${PLATFORM_LASZIP_LIB} spdlog::spdlog)
target_include_directories(laz_to_txt PRIVATE ${LASZIP_INCLUDE_DIR}/LASzip/include ${PROJECT_BINARY_DIR}/include)
16 changes: 9 additions & 7 deletions apps/console_tools/laz_to_pcd.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,13 @@
#include <cstring>
#include <filesystem>
#include <fstream>
#include <iostream>
#include <limits>
#include <span>
#include <string>
#include <vector>

#include <spdlog/spdlog.h>

namespace fs = std::filesystem;

bool check_path_ext(const char* path, const char* ext)
Expand Down Expand Up @@ -104,8 +106,8 @@ int main(const int argc, const char** argv)

if (argc != expected_argc)
{
std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc);
std::fprintf(stderr, "Usage : %s </PATH/FROM/POINT/CLOUD.laz> </PATH/TO/POINT/CLOUD.pcd>\n", argv[0]);
spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc);
spdlog::error("Usage : {} </PATH/FROM/POINT/CLOUD.laz> </PATH/TO/POINT/CLOUD.pcd>", argv[0]);

return EXIT_FAILURE;
}
Expand All @@ -115,28 +117,28 @@ int main(const int argc, const char** argv)

if (!check_path_ext(from, expected_laz_extension))
{
std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_laz_extension);
spdlog::error("Invalid extension for input file {} - expected {}", from, expected_laz_extension);

return EXIT_FAILURE;
}

if (!check_path_ext(to, expected_ply_extension))
{
std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", from, expected_ply_extension);
spdlog::error("Invalid extension for output file {} - expected {}", from, expected_ply_extension);

return EXIT_FAILURE;
}

if (!std::filesystem::exists(from))
{
std::fprintf(stderr, "Input file %s - does not exist\n", from);
spdlog::error("Input file {} - does not exist", from);

return EXIT_FAILURE;
}

if (!convert_and_save(from, to))
{
std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to);
spdlog::error("Conversion from {} to {} failed\n", from, to);

return EXIT_FAILURE;
}
Expand Down
14 changes: 8 additions & 6 deletions apps/console_tools/laz_to_ply.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@
#include <fstream>
#include <vector>

#include <spdlog/spdlog.h>

struct Point
{
float x = 0.0f;
Expand Down Expand Up @@ -100,8 +102,8 @@ int main(const int argc, const char** argv)

if (argc != expected_argc)
{
std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc);
std::fprintf(stderr, "Usage : %s </PATH/FROM/POINT/CLOUD.laz> </PATH/TO/POINT/CLOUD.ply>\n", argv[0]);
spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc);
spdlog::error("Usage : {} </PATH/FROM/POINT/CLOUD.laz> </PATH/TO/POINT/CLOUD.ply>", argv[0]);

return EXIT_FAILURE;
}
Expand All @@ -111,28 +113,28 @@ int main(const int argc, const char** argv)

if (!check_path_ext(from, expected_laz_extension))
{
std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_laz_extension);
spdlog::error("Invalid extension for input file {} - expected {}", from, expected_laz_extension);

return EXIT_FAILURE;
}

if (!check_path_ext(to, expected_ply_extension))
{
std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", from, expected_ply_extension);
spdlog::error("Invalid extension for output file {} - expected {}", from, expected_ply_extension);

return EXIT_FAILURE;
}

if (!std::filesystem::exists(from))
{
std::fprintf(stderr, "Input file %s - does not exist\n", from);
spdlog::error("Input file {} - does not exist", from);

return EXIT_FAILURE;
}

if (!convert_and_save(from, to))
{
std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to);
spdlog::error("Conversion from {} to {} failed\n", from, to);

return EXIT_FAILURE;
}
Expand Down
14 changes: 8 additions & 6 deletions apps/console_tools/laz_to_txt.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@
#include <fstream>
#include <vector>

#include <spdlog/spdlog.h>

struct Point
{
float x = 0.0f;
Expand Down Expand Up @@ -95,8 +97,8 @@ int main(const int argc, const char** argv)

if (argc != expected_argc)
{
std::fprintf(stderr, "Invalid argument count. Got %d expected %d.\n", argc, expected_argc);
std::fprintf(stderr, "Usage : %s </PATH/FROM/POINT/CLOUD.laz> </PATH/TO/POINT/CLOUD.txt>\n", argv[0]);
spdlog::error("Invalid argument count. Got {} expected {}", argc, expected_argc);
spdlog::error("Usage : {} </PATH/FROM/POINT/CLOUD.laz> </PATH/TO/POINT/CLOUD.txt>", argv[0]);

return EXIT_FAILURE;
}
Expand All @@ -106,28 +108,28 @@ int main(const int argc, const char** argv)

if (!check_path_ext(from, expected_laz_extension))
{
std::fprintf(stderr, "Invalid extension for input file %s - expected %s\n", from, expected_laz_extension);
spdlog::error("Invalid extension for input file {} - expected {}", from, expected_laz_extension);

return EXIT_FAILURE;
}

if (!check_path_ext(to, expected_ply_extension))
{
std::fprintf(stderr, "Invalid extension for output file %s - expected %s\n", from, expected_ply_extension);
spdlog::error("Invalid extension for output file {} - expected {}", from, expected_ply_extension);

return EXIT_FAILURE;
}

if (!std::filesystem::exists(from))
{
std::fprintf(stderr, "Input file %s - does not exist\n", from);
spdlog::error("Input file {} - does not exist", from);

return EXIT_FAILURE;
}

if (!convert_and_save(from, to))
{
std::fprintf(stderr, "Conversion from %s to %s failed\n", from, to);
spdlog::error("Conversion from {} to {} failed", from, to);

return EXIT_FAILURE;
}
Expand Down
82 changes: 7 additions & 75 deletions apps/console_tools/multiply_timestamps_session_point_cloud_laz.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,71 +10,35 @@

#include <export_laz.h>

#include <spdlog/spdlog.h>

bool load_pc(PointCloud& pc, const std::string& input_file_name)
{
laszip_POINTER laszip_reader;
if (laszip_create(&laszip_reader))
{
fprintf(stderr, ":DLL ERROR: creating laszip reader\n");
/*PointCloud pc;
pc.m_pose = Eigen::Affine3d::Identity();
pc.m_initial_pose = pc.m_pose;
pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose);
pc.gui_translation[0] = pc.pose.px;
pc.gui_translation[1] = pc.pose.py;
pc.gui_translation[2] = pc.pose.pz;
pc.gui_rotation[0] = rad2deg(pc.pose.om);
pc.gui_rotation[1] = rad2deg(pc.pose.fi);
pc.gui_rotation[2] = rad2deg(pc.pose.ka);
pc.file_name = input_file_names[i];
point_clouds.push_back(pc);*/
spdlog::error(":DLL ERROR: creating laszip reader");
return false;
}

laszip_BOOL is_compressed = 0;
if (laszip_open_reader(laszip_reader, input_file_name.c_str(), &is_compressed))
{
fprintf(stderr, ":DLL ERROR: opening laszip reader for '%s'\n", input_file_name.c_str());
/*PointCloud pc;
pc.m_pose = Eigen::Affine3d::Identity();
pc.m_initial_pose = pc.m_pose;
pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose);
pc.gui_translation[0] = pc.pose.px;
pc.gui_translation[1] = pc.pose.py;
pc.gui_translation[2] = pc.pose.pz;
pc.gui_rotation[0] = rad2deg(pc.pose.om);
pc.gui_rotation[1] = rad2deg(pc.pose.fi);
pc.gui_rotation[2] = rad2deg(pc.pose.ka);
pc.file_name = input_file_names[i];
point_clouds.push_back(pc);*/
spdlog::error(":DLL ERROR: opening laszip reader for '{}'", input_file_name);
return false;
}
laszip_header* header;

if (laszip_get_header_pointer(laszip_reader, &header))
{
fprintf(stderr, ":DLL ERROR: getting header pointer from laszip reader\n");
/*PointCloud pc;
pc.m_pose = Eigen::Affine3d::Identity();
pc.m_initial_pose = pc.m_pose;
pc.pose = pose_tait_bryan_from_affine_matrix(pc.m_pose);
pc.gui_translation[0] = pc.pose.px;
pc.gui_translation[1] = pc.pose.py;
pc.gui_translation[2] = pc.pose.pz;
pc.gui_rotation[0] = rad2deg(pc.pose.om);
pc.gui_rotation[1] = rad2deg(pc.pose.fi);
pc.gui_rotation[2] = rad2deg(pc.pose.ka);
pc.file_name = input_file_names[i];
point_clouds.push_back(pc);*/
spdlog::error(":DLL ERROR: getting header pointer from laszip reader");
return false;
}

// fprintf(stderr, "file '%s' contains %u points\n", input_file_name.c_str(), header->number_of_point_records);

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

Expand All @@ -88,28 +52,6 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name)
pc.gui_rotation[1] = rad2deg(pc.pose.fi);
pc.gui_rotation[2] = rad2deg(pc.pose.ka);

/*for (int j = 0; j < header->number_of_point_records; j++)
{
if (laszip_read_point(laszip_reader))
{
fprintf(stderr, ":DLL ERROR: reading point %u\n", j);
laszip_close_reader(laszip_reader);
return true;
// continue;
}

LAZPoint p;
p.x = header->x_offset + header->x_scale_factor * static_cast<double>(point->X);
p.y = header->y_offset + header->y_scale_factor * static_cast<double>(point->Y);
p.z = header->z_offset + header->z_scale_factor * static_cast<double>(point->Z);
p.timestamp = point->gps_time;

Eigen::Vector3d pp(p.x, p.y, p.z);
pc.points_local.push_back(pp);
pc.intensities.push_back(point->intensity);
pc.timestamps.push_back(p.timestamp);
}*/

laszip_I64 npoints = (header->number_of_point_records ? header->number_of_point_records : header->extended_number_of_point_records);

std::cout << (is_compressed ? "" : "un") << "compressed file '" << (std::filesystem::path(input_file_name).filename().string())
Expand All @@ -121,7 +63,7 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name)
{
if (laszip_read_point(laszip_reader))
{
fprintf(stderr, "DLL ERROR: reading point %I64d\n", p_count);
spdlog::error("DLL ERROR: reading point {}", p_count);
laszip_close_reader(laszip_reader);
return false;
}
Expand All @@ -137,27 +79,17 @@ bool load_pc(PointCloud& pc, const std::string& input_file_name)
pc.intensities.push_back(point->intensity);
pc.timestamps.push_back(p.timestamp);

// Eigen::Vector3d color(
// static_cast<uint8_t>(0xFFU * ((point->rgb[0] > 0) ? static_cast<float>(point->rgb[0]) / static_cast<float>(0xFFFFU) : 1.0f))
/// 256.0, static_cast<uint8_t>(0xFFU * ((point->rgb[1] > 0) ? static_cast<float>(point->rgb[1]) / static_cast<float>(0xFFFFU)
//: 1.0f)) / 256.0, static_cast<uint8_t>(0xFFU * ((point->rgb[2] > 0) ? static_cast<float>(point->rgb[2]) /
// static_cast<float>(0xFFFFU) : 1.0f)) / 256.0);

Eigen::Vector3d color(
static_cast<float>(point->rgb[0]) / 256.0,
static_cast<float>(point->rgb[1]) / 256.0,
static_cast<float>(point->rgb[2]) / 256.0);

// std::cout << point->rgb[0] << " " << point->rgb[1] << " " << point->rgb[2] << std::endl;

pc.colors.push_back(color);

p_count++;
}

laszip_close_reader(laszip_reader);
// laszip_clean(laszip_reader);
// laszip_destroy(laszip_reader);

return true;
}
Expand Down
Loading
Loading