diff --git a/.gitignore b/.gitignore index 4b81de55..105973fd 100644 --- a/.gitignore +++ b/.gitignore @@ -72,6 +72,10 @@ imgui.ini /.gitmodules /build2 /build3 +/build-* + +# local test data +/rosbags.zip # deploy_mandeye.bat output /deploy diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f1eff56..464a1f9f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # Versioning settings set(HDMAPPING_VERSION_MAJOR 0) -set(HDMAPPING_VERSION_MINOR 104) +set(HDMAPPING_VERSION_MINOR 105) set(HDMAPPING_VERSION_PATCH 0) # Set up common paths @@ -72,6 +72,13 @@ set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) +# Bundled dylibs (e.g. freeglut) use an @rpath install name, and install(TARGETS) +# strips the build-tree RPATH, so installed executables need an explicit one +# pointing at /lib or dyld fails with "no LC_RPATH's found". +if(APPLE) + set(CMAKE_INSTALL_RPATH "@executable_path/../lib") +endif() + # TODO(mwlasiuk) : fix # if(WIN32) # set(CMAKE_PDB_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin) @@ -113,6 +120,7 @@ option(BUILD_TESTING "Build HDMapping unit tests" OFF) if(BUILD_TESTING) enable_testing() add_subdirectory(shared/tests) + add_subdirectory(calib_core/tests) add_subdirectory(apps/lidar_odometry_step_1/tests) add_subdirectory(rosbags/tests) endif() @@ -138,13 +146,11 @@ add_subdirectory(apps/multi_session_registration) add_subdirectory(apps/multi_session_registration_legacy) if(BUILD_WITH_UTILITY_APPLICATION) - add_subdirectory(apps/manual_color) add_subdirectory(apps/split_multi_livox) add_subdirectory(apps/precision_forestry_tools) add_subdirectory(apps/compare_trajectories) add_subdirectory(apps/mandeye_mission_recorder_calibration) add_subdirectory(apps/livox_mid_360_intrinsic_calibration) - add_subdirectory(apps/single_session_manual_coloring) add_subdirectory(apps/concatenate_multi_livox) add_subdirectory(apps/camera_lidar_calibration) add_subdirectory(apps/camera_lidar_trajectory_viewer) diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index 4ad06ec0..c15fbc19 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include // ── AppState::rebuildImageTexture ───────────────────────────────────────────── @@ -27,7 +28,13 @@ void AppState::rebuildImageTexture() cv::Mat display = originalImage; imageRectified = false; - if (intrinsicsLoaded) + // initUndistortRectifyMap assumes OpenCV's rational pinhole model -- + // running it for Mei would silently mis-warp the image rather than + // undistort it, and rectifying a fisheye onto a pinhole plane would crop + // away the wide field of view it exists for. Both are shown raw instead, + // with the projection overlay and GPU shaders applying their distortion + // directly to the raw image (see Renderer.cpp/RendererShaders.h). + if (intrinsicsLoaded && intrinsics.model == CameraModel::Pinhole) { cv::Mat K = (cv::Mat_(3, 3) << intrinsics.fx, 0, intrinsics.cx, 0, intrinsics.fy, intrinsics.cy, 0, 0, 1); // OpenCV distCoeffs order: k1 k2 p1 p2 k3 k4 k5 k6 (rational model) @@ -61,6 +68,30 @@ void AppState::rebuildImageTexture() imageLoaded = true; } +// ── AppState::autoScaleIntrinsicsToImage ────────────────────────────────────── +std::string AppState::autoScaleIntrinsicsToImage() +{ + if (!intrinsicsLoaded || intrinsicsW <= 0 || imageW <= 0) + return ""; + if (intrinsicsW == imageW && intrinsicsH == imageH) + return ""; + + // calib::scaleIntrinsics takes a single factor, so the width ratio is it; + // sy exists only to detect and warn about a real aspect-ratio change. + float sx = static_cast(imageW) / static_cast(intrinsicsW); + float sy = static_cast(imageH) / static_cast(intrinsicsH); + intrinsics = calib::scaleIntrinsics(intrinsics, sx); + intrinsicsW = imageW; + intrinsicsH = imageH; + + char buf[192]; + std::snprintf(buf, sizeof(buf), "intrinsics auto-scaled %.4fx to match the %dx%d image", static_cast(sx), imageW, imageH); + std::string note = buf; + if (std::fabs(sx - sy) > 0.01f * sx) + note += " (WARNING: aspect ratio differs from the calibration -- scaled by width only, results may be off)"; + return note; +} + // ── AppState correspondence picking ─────────────────────────────────────────── void AppState::setPendingImagePoint(float u, float v) { @@ -139,10 +170,16 @@ bool AppState::solvePairs() } double rms = -1.0; - bool ok = calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); + std::string solveErr; + // Pinhole's observation equations are a plain rectilinear projection, so + // Mei and Fisheye use solveExtrinsicsCeres instead. See + // CameraCalibrationSolver.h. + bool ok = (intrinsics.model == CameraModel::Mei || intrinsics.model == CameraModel::Fisheye) + ? calib::solveExtrinsicsCeres(corr, intrinsics, extrinsics, solveErr, &rms, lockTranslation) + : calib::solveExtrinsicsFromCorrespondences(corr, intrinsics, extrinsics, &rms, lockTranslation); if (!ok) { - statusMsg = "Solve failed (degenerate correspondences)"; + statusMsg = !solveErr.empty() ? ("Solve failed: " + solveErr) : "Solve failed (degenerate correspondences)"; return false; } @@ -168,9 +205,14 @@ void AppState::loadImage(const char* path) imageW = originalImage.cols; imageH = originalImage.rows; imagePath = path; + // Intrinsics may already be loaded for a different resolution (e.g. a + // calibration taken at full res, then a downscaled image loaded here). + std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); renderer.init(imageW, imageH); statusMsg = imageRectified ? "Image loaded and rectified" : "Image loaded (raw)"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; } // ── AppState::loadCloud ─────────────────────────────────────────────────────── @@ -204,6 +246,8 @@ void AppState::loadCloud(const char* path) rebuildCloudPointsRaylib(*this); centerOrbitOnCloud(*this); statusMsg = ""; + // load status sidecar + lidarId = GetLidarSerial(path); } void AppState::addCloud(const char* path) @@ -242,7 +286,10 @@ void AppState::addCloud(const char* path) // data: (block) // - a // - b -// Distortion order is OpenCV distCoeffs: k1 k2 p1 p2 k3 [k4 k5 k6 ...] +// Distortion order is OpenCV distCoeffs: k1 k2 p1 p2 k3 [k4 k5 k6 ...], or +// k1 k2 k3 k4 when `distortion_model:` is equidistant/fisheye. Only that key +// selects the fisheye model: four coefficients alone are also a valid pinhole +// k1 k2 p1 p2. static void extractNumbers(const std::string& s, std::vector& out) { const char* p = s.c_str(); @@ -273,6 +320,7 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img } std::vector camMat, dist; + std::string distortionModel; // lower-cased, quotes stripped std::vector* active = nullptr; // section whose data we collect std::vector* collecting = nullptr; bool inFlow = false; @@ -309,6 +357,12 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img imgW = std::atoi(trimmed.c_str() + 12); else if (trimmed.rfind("image_height:", 0) == 0) imgH = std::atoi(trimmed.c_str() + 13); + else if (trimmed.rfind("distortion_model:", 0) == 0) + { + for (char c : trimmed.substr(17)) + if (std::isalnum(static_cast(c)) || c == '_') + distortionModel += static_cast(std::tolower(static_cast(c))); + } } continue; } @@ -346,6 +400,15 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img err = "camera_matrix needs 9 values"; return false; } + const bool fisheye = distortionModel == "equidistant" || distortionModel == "fisheye"; + // Unlike the pinhole list, whose trailing terms may be left off, a + // fisheye has exactly four -- a missing one defaulting to 0 would + // reproject wrongly with nothing to show for it. + if (fisheye && dist.size() != 4) + { + err = "distortion_model " + distortionModel + " needs 4 distortion_coefficients (k1 k2 k3 k4), got " + std::to_string(dist.size()); + return false; + } // Row-major 3x3: [fx 0 cx; 0 fy cy; 0 0 1] K.fx = static_cast(camMat[0]); @@ -353,6 +416,18 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img K.fy = static_cast(camMat[4]); K.cy = static_cast(camMat[5]); + if (fisheye) + { + K.model = CameraModel::Fisheye; + K.k1 = static_cast(dist[0]); + K.k2 = static_cast(dist[1]); + K.k3 = static_cast(dist[2]); + K.k4 = static_cast(dist[3]); + K.k5 = K.k6 = K.p1 = K.p2 = 0.f; + return true; + } + + K.model = CameraModel::Pinhole; auto d = [&](size_t i) { return i < dist.size() ? static_cast(dist[i]) : 0.f; @@ -368,6 +443,20 @@ static bool parseOpenCVYaml(const char* path, Intrinsics& K, int& imgW, int& img return true; } +// The flat camera_info.yaml that insta360-to-images and insta360-test-calib +// write has top-level fx:/fy:/cx:/cy: keys; an OpenCV/ROS YAML keeps them in +// a camera_matrix: block instead. Peeked at as text so each goes to the +// parser that understands it. +static bool yamlIsFlatCameraInfo(const char* path) +{ + std::ifstream f(path); + std::string line; + while (std::getline(f, line)) + if (line.rfind("fx:", 0) == 0) + return true; + return false; +} + // ── AppState::loadIntrinsics ────────────────────────────────────────────────── void AppState::loadIntrinsics(const char* path) { @@ -377,6 +466,29 @@ void AppState::loadIntrinsics(const char* path) for (auto& c : ext) c = static_cast(tolower(c)); + if ((ext == "yml" || ext == "yaml") && yamlIsFlatCameraInfo(path)) + { + if (!calib::loadCameraInfoYaml(path, intrinsics)) + { + statusMsg = std::string("camera_info.yaml failed to load (see console): ") + path; + return; + } + intrinsicsW = intrinsics.width; + intrinsicsH = intrinsics.height; + intrinsicsLoaded = true; + calib::loadCameraIdentity(path, cameraId); + std::string scaleNote = autoScaleIntrinsicsToImage(); + rebuildImageTexture(); // rectifies a pinhole; Mei and Fisheye stay raw + statusMsg = std::string("Intrinsics loaded (") + modelToString(intrinsics.model) + ")"; + if (imageRectified) + statusMsg += ", image rectified"; + if (intrinsicsW > 0) + statusMsg += " (calibration " + std::to_string(intrinsicsW) + "x" + std::to_string(intrinsicsH) + ")"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; + return; + } + if (ext == "yml" || ext == "yaml") { int imgW = 0, imgH = 0; @@ -386,17 +498,20 @@ void AppState::loadIntrinsics(const char* path) statusMsg = std::string("YAML error: ") + err + " (" + path + ")"; return; } + intrinsics.xi = 0.f; // parseOpenCVYaml set the model: Pinhole or Fisheye + intrinsicsW = imgW; + intrinsicsH = imgH; intrinsicsLoaded = true; - rebuildImageTexture(); // re-rectify with the new coefficients - statusMsg = "Intrinsics loaded"; + calib::loadCameraIdentity(path, cameraId); + std::string scaleNote = autoScaleIntrinsicsToImage(); + rebuildImageTexture(); // re-rectify with the new (possibly auto-scaled) coefficients + statusMsg = intrinsics.model == CameraModel::Fisheye ? "Fisheye intrinsics loaded" : "Intrinsics loaded"; + if (imgW > 0) + statusMsg += " (calibration " + std::to_string(imgW) + "x" + std::to_string(imgH) + ")"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; if (imageRectified) statusMsg += ", image rectified"; - if (imgW > 0) - { - statusMsg += " (camera " + std::to_string(imgW) + "x" + std::to_string(imgH) + ")"; - if (imageLoaded && (imgW != imageW || imgH != imageH)) - statusMsg += " WARNING: image is " + std::to_string(imageW) + "x" + std::to_string(imageH); - } return; } @@ -408,10 +523,12 @@ void AppState::loadIntrinsics(const char* path) } nlohmann::json j; f >> j; + intrinsics.model = modelFromString(j.value("model", std::string("pinhole"))); intrinsics.fx = j.value("fx", intrinsics.fx); intrinsics.fy = j.value("fy", intrinsics.fy); intrinsics.cx = j.value("cx", intrinsics.cx); intrinsics.cy = j.value("cy", intrinsics.cy); + intrinsics.xi = j.value("xi", 0.f); intrinsics.k1 = j.value("k1", 0.f); intrinsics.k2 = j.value("k2", 0.f); intrinsics.k3 = j.value("k3", 0.f); @@ -420,9 +537,21 @@ void AppState::loadIntrinsics(const char* path) intrinsics.k6 = j.value("k6", 0.f); intrinsics.p1 = j.value("p1", 0.f); intrinsics.p2 = j.value("p2", 0.f); + // No "width"/"height" in the file -- assume it matches whatever image is + // already loaded (this format historically had no resolution field at + // all, so anything already loaded is the best guess available). + intrinsicsW = j.value("width", imageLoaded ? imageW : 0); + intrinsicsH = j.value("height", imageLoaded ? imageH : 0); intrinsicsLoaded = true; + cameraId.serial = j.value("serial", "unknown"); + cameraId.model = j.value("model", "unknown"); + cameraId.firmware = j.value("firmware", "unknown"); + cameraId.frameId = j.value("frameId", "unknown"); + std::string scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); statusMsg = "Intrinsics loaded."; + if (!scaleNote.empty()) + statusMsg += " " + scaleNote; } // ── AppState::loadCalibration ───────────────────────────────────────────────── @@ -446,13 +575,30 @@ void AppState::loadCalibration(const char* path) bool gotIntrinsics = false, gotExtrinsics = false; + // "camera" identifies the hardware the intrinsics were measured on, so it + // is replaced exactly when they are: a file carrying new intrinsics but no + // "camera" block clears the previous serial instead of leaving it attached + // to a different camera's numbers. A file with only a "camera" block still + // sets it, so an identity can be attached to extrinsics on their own. + if (j.contains("intrinsics") || j.contains("camera")) + { + cameraId = CameraIdentity{}; + const nlohmann::json jc = j.value("camera", nlohmann::json::object()); + cameraId.serial = jc.value("serial", std::string{}); + cameraId.model = jc.value("model", std::string{}); + cameraId.firmware = jc.value("firmware", std::string{}); + cameraId.frameId = jc.value("frame_id", std::string{}); + } + if (j.contains("intrinsics")) { auto& ji = j["intrinsics"]; + intrinsics.model = modelFromString(ji.value("model", std::string("pinhole"))); intrinsics.fx = ji.value("fx", intrinsics.fx); intrinsics.fy = ji.value("fy", intrinsics.fy); intrinsics.cx = ji.value("cx", intrinsics.cx); intrinsics.cy = ji.value("cy", intrinsics.cy); + intrinsics.xi = ji.value("xi", 0.f); intrinsics.k1 = ji.value("k1", 0.f); intrinsics.k2 = ji.value("k2", 0.f); intrinsics.k3 = ji.value("k3", 0.f); @@ -461,6 +607,8 @@ void AppState::loadCalibration(const char* path) intrinsics.k6 = ji.value("k6", 0.f); intrinsics.p1 = ji.value("p1", 0.f); intrinsics.p2 = ji.value("p2", 0.f); + intrinsicsW = ji.value("width", imageLoaded ? imageW : 0); + intrinsicsH = ji.value("height", imageLoaded ? imageH : 0); intrinsicsLoaded = true; gotIntrinsics = true; } @@ -498,8 +646,12 @@ void AppState::loadCalibration(const char* path) return; } + std::string scaleNote; if (gotIntrinsics) + { + scaleNote = autoScaleIntrinsicsToImage(); rebuildImageTexture(); + } statusMsg = "Loaded"; if (gotIntrinsics) @@ -509,6 +661,10 @@ void AppState::loadCalibration(const char* path) if (gotExtrinsics) statusMsg += " extrinsics"; statusMsg += std::string(" from ") + path; + if (!cameraId.serial.empty()) + statusMsg += " (serial " + cameraId.serial + ")"; + if (!scaleNote.empty()) + statusMsg += "; " + scaleNote; } // ── AppState::saveCalibration ───────────────────────────────────────────────── @@ -521,9 +677,34 @@ void AppState::saveCalibration(const char* path) Eigen::Vector3f ti = -(R.transpose() * C); // translation of T_lidar_to_camera nlohmann::json j; - j["intrinsics"] = { { "fx", intrinsics.fx }, { "fy", intrinsics.fy }, { "cx", intrinsics.cx }, { "cy", intrinsics.cy }, - { "k1", intrinsics.k1 }, { "k2", intrinsics.k2 }, { "k3", intrinsics.k3 }, { "k4", intrinsics.k4 }, - { "k5", intrinsics.k5 }, { "k6", intrinsics.k6 }, { "p1", intrinsics.p1 }, { "p2", intrinsics.p2 } }; + // Which camera this calibration was measured on, when a tracked source + // named it. Omitted entirely when unknown, so an absent block and an empty + // one mean the same thing on the way back in. + + j["lidar"]["serial"] = lidarId; + j["camera"]["model"] = cameraId.model; + j["camera"]["serial"] = cameraId.serial; + j["camera"]["frame_id"] = cameraId.frameId; + + // width/height record the resolution these intrinsics are valid for (see + // App.h) so a later load against a different-size image can auto-scale + // rather than just warn. 0 means unknown. + j["intrinsics"] = { { "model", modelToString(intrinsics.model) }, + { "fx", intrinsics.fx }, + { "fy", intrinsics.fy }, + { "cx", intrinsics.cx }, + { "cy", intrinsics.cy }, + { "xi", intrinsics.xi }, + { "k1", intrinsics.k1 }, + { "k2", intrinsics.k2 }, + { "k3", intrinsics.k3 }, + { "k4", intrinsics.k4 }, + { "k5", intrinsics.k5 }, + { "k6", intrinsics.k6 }, + { "p1", intrinsics.p1 }, + { "p2", intrinsics.p2 }, + { "width", intrinsicsW }, + { "height", intrinsicsH } }; // Rotation is stored as a matrix only -- convention-independent (no // Euler/Tait-Bryan angle order or units to document/misread) and // directly portable to any external tool. camera_rotation_matrix_in_world diff --git a/apps/camera_lidar_calibration/App.h b/apps/camera_lidar_calibration/App.h index aa81695b..f72eff2a 100644 --- a/apps/camera_lidar_calibration/App.h +++ b/apps/camera_lidar_calibration/App.h @@ -40,6 +40,20 @@ struct AppState // ── calibration params ─────────────────────────────────────────────────── Intrinsics intrinsics; Extrinsics extrinsics; + // Which camera `intrinsics` describe, when the file said so. Replaced + // whenever the intrinsics are -- an untracked source (an OpenCV YAML, the + // flat intrinsics JSON) clears it rather than leaving the previous + // camera's serial attached to someone else's numbers. + CameraIdentity cameraId; + + // Lidar id from status side car to laz + std::string lidarId; + + // Resolution `intrinsics` are currently valid for: the calibration file's + // own width/height, else whatever image was loaded at the time. 0 = + // unknown. autoScaleIntrinsicsToImage() keeps this in sync, so it names + // the size the *current* intrinsics apply to, not the file's original. + int intrinsicsW = 0, intrinsicsH = 0; // ── visualization ───────────────────────────────────────────────────────── VisualizationParams vizParams; @@ -92,6 +106,12 @@ struct AppState // (Re)build the displayed texture: undistorts with current intrinsics // when they were loaded from a file, otherwise shows the raw image. void rebuildImageTexture(); + // Rescales `intrinsics` to the current imageW/imageH when intrinsicsW/H + // names a different resolution, so a calibration and an image of + // different sizes just work instead of silently mis-projecting. Called + // after whichever of the two loads comes second. No-op (returns "") if + // either size is unknown or they match. Caller owns rebuildImageTexture(). + std::string autoScaleIntrinsicsToImage(); }; class App diff --git a/apps/camera_lidar_calibration/Renderer.cpp b/apps/camera_lidar_calibration/Renderer.cpp index 431f9e78..7f5db1b0 100644 --- a/apps/camera_lidar_calibration/Renderer.cpp +++ b/apps/camera_lidar_calibration/Renderer.cpp @@ -90,6 +90,12 @@ void Renderer::initPointShader() locCamK = rlGetLocationUniform(pointShader.id, "K"); locCamImgSize = rlGetLocationUniform(pointShader.id, "imgSize"); locCamTex = rlGetLocationUniform(pointShader.id, "imageTex"); + locCamModel = rlGetLocationUniform(pointShader.id, "model"); + locCamXi = rlGetLocationUniform(pointShader.id, "xi"); + locCamRad1 = rlGetLocationUniform(pointShader.id, "kRad1"); + locCamRad2 = rlGetLocationUniform(pointShader.id, "kRad2"); + locCamTan = rlGetLocationUniform(pointShader.id, "pTan"); + locCamThetaMax = rlGetLocationUniform(pointShader.id, "thetaMax"); } projShader = LoadShaderFromMemory(kProjVS, kProjFS.c_str()); @@ -106,6 +112,9 @@ void Renderer::initPointShader() locPrjRad1 = rlGetLocationUniform(projShader.id, "kRad1"); locPrjRad2 = rlGetLocationUniform(projShader.id, "kRad2"); locPrjTan = rlGetLocationUniform(projShader.id, "pTan"); + locPrjModel = rlGetLocationUniform(projShader.id, "model"); + locPrjXi = rlGetLocationUniform(projShader.id, "xi"); + locPrjThetaMax = rlGetLocationUniform(projShader.id, "thetaMax"); locPrjDepthRange = rlGetLocationUniform(projShader.id, "depthRange"); locPrjOpacity = rlGetLocationUniform(projShader.id, "opacity"); locPrjPointSize = rlGetLocationUniform(projShader.id, "pointSize"); @@ -176,6 +185,15 @@ void Renderer::renderImageOverlay( float rad1[3] = { 0.f, 0.f, 0.f }; float rad2[3] = { 0.f, 0.f, 0.f }; float tan2[2] = { 0.f, 0.f }; + // model/xi only take effect when applyDistortion is set too, same as + // rad1/rad2/tan2 below -- applyDistortion==false means "treat as + // already rectified" regardless of model (kept exactly as before + // for Pinhole; Mei and Fisheye in practice always have + // applyDistortion==true, since AppState::rebuildImageTexture never + // rectifies them). + int model = 0; + float xiVal = 0.f; + float thetaMax = 0.f; if (applyDistortion) { rad1[0] = K.k1; @@ -186,6 +204,16 @@ void Renderer::renderImageOverlay( rad2[2] = K.k6; tan2[0] = K.p1; tan2[1] = K.p2; + if (K.model == CameraModel::Mei) + { + model = 2; + xiVal = K.xi; + } + else if (K.model == CameraModel::Fisheye) + { + model = 3; + thetaMax = calib::fisheyeMaxTheta(K); + } } float depthRange[2] = { vp.depthMin, vp.depthMax }; @@ -196,6 +224,9 @@ void Renderer::renderImageOverlay( rlSetUniform(locPrjRad1, rad1, RL_SHADER_UNIFORM_VEC3, 1); rlSetUniform(locPrjRad2, rad2, RL_SHADER_UNIFORM_VEC3, 1); rlSetUniform(locPrjTan, tan2, RL_SHADER_UNIFORM_VEC2, 1); + rlSetUniform(locPrjModel, &model, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locPrjXi, &xiVal, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(locPrjThetaMax, &thetaMax, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locPrjDepthRange, depthRange, RL_SHADER_UNIFORM_VEC2, 1); rlSetUniform(locPrjOpacity, &vp.opacity, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locPrjPointSize, &vp.pointSize, RL_SHADER_UNIFORM_FLOAT, 1); @@ -243,6 +274,15 @@ void Renderer::draw3DCloud( Matrix camXform = buildLidarToCamMatrix(E); float k[4] = { K.fx, K.fy, K.cx, K.cy }; float imgSize[2] = { (float)std::max(imgW, 1), (float)std::max(imgH, 1) }; + // Camera RGB sampling always applies Mei's or Fisheye's own distortion + // (unlike the Pinhole path, their displayed image is never rectified -- + // see AppState::rebuildImageTexture and kPointVS's branches for them). + int model = (K.model == CameraModel::Mei) ? 2 : (K.model == CameraModel::Fisheye) ? 3 : 0; + float xi = K.xi; + float rad1[3] = { K.k1, K.k2, K.k3 }; + float rad2[3] = { K.k4, K.k5, K.k6 }; + float tan2[2] = { K.p1, K.p2 }; + float thetaMax = (K.model == CameraModel::Fisheye) ? calib::fisheyeMaxTheta(K) : 0.f; rlEnableShader(pointShader.id); rlSetUniformMatrix(locMVP, mvp); @@ -254,6 +294,12 @@ void Renderer::draw3DCloud( rlSetUniform(locOpacity, &vp.opacity, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniformMatrix(locCamXform, camXform); rlSetUniform(locCamK, k, RL_SHADER_UNIFORM_VEC4, 1); + rlSetUniform(locCamModel, &model, RL_SHADER_UNIFORM_INT, 1); + rlSetUniform(locCamXi, &xi, RL_SHADER_UNIFORM_FLOAT, 1); + rlSetUniform(locCamRad1, rad1, RL_SHADER_UNIFORM_VEC3, 1); + rlSetUniform(locCamRad2, rad2, RL_SHADER_UNIFORM_VEC3, 1); + rlSetUniform(locCamTan, tan2, RL_SHADER_UNIFORM_VEC2, 1); + rlSetUniform(locCamThetaMax, &thetaMax, RL_SHADER_UNIFORM_FLOAT, 1); rlSetUniform(locCamImgSize, imgSize, RL_SHADER_UNIFORM_VEC2, 1); if (colorMode == 3) @@ -276,6 +322,25 @@ void Renderer::drawCameraFrustum(const Intrinsics& K, const Extrinsics& E, int i // Camera position in LiDAR frame is directly (E.tx, E.ty, E.tz) Vector3 origin = { E.tx, E.tz, -E.ty }; // LiDAR→raylib + if (K.model != CameraModel::Pinhole) + { + // A rectangular pyramid built from fx/fy/cx/cy/imgW/imgH (below) + // assumes a narrow rectilinear FOV, which misrepresents a Mei or + // equidistant fisheye's much wider one -- draw a position marker + camera + // forward/right/up axis triad instead, same fallback + // camera_lidar_trajectory_viewer uses for CameraModel::Mei. + auto toWorld = [&](const Eigen::Vector3f& axis_c) -> Vector3 + { + Eigen::Vector3f pl = R * (axis_c * scale * 0.5f) + Eigen::Vector3f(E.tx, E.ty, E.tz); + return { pl.x(), pl.z(), -pl.y() }; + }; + DrawSphereWires(origin, scale * 0.08f, 8, 8, YELLOW); + DrawLine3D(origin, toWorld(Eigen::Vector3f(0.f, 0.f, 1.f)), BLUE); // camera forward (Z) + DrawLine3D(origin, toWorld(Eigen::Vector3f(1.f, 0.f, 0.f)), RED); // camera right (X) + DrawLine3D(origin, toWorld(Eigen::Vector3f(0.f, -1.f, 0.f)), GREEN); // camera up (-Y: camera Y is down) + return; + } + // Four image corners in camera frame, at depth=scale float corners[4][2] = { { (0.f - K.cx) / K.fx, (0.f - K.cy) / K.fy }, diff --git a/apps/camera_lidar_calibration/Renderer.h b/apps/camera_lidar_calibration/Renderer.h index 4d0e4fe1..8a804e9e 100644 --- a/apps/camera_lidar_calibration/Renderer.h +++ b/apps/camera_lidar_calibration/Renderer.h @@ -85,12 +85,17 @@ class Renderer int locMVP = -1, locColorMode = -1, locHeightRange = -1; int locMaxDist = -1, locOpacity = -1, locPointSize = -1, locDecim = -1; int locCamXform = -1, locCamK = -1, locCamImgSize = -1, locCamTex = -1; + // CameraModel::Mei/Fisheye only -- see kPointVS's branches for them (RendererShaders.h) + int locCamModel = -1, locCamXi = -1, locCamRad1 = -1, locCamTan = -1; + int locCamRad2 = -1, locCamThetaMax = -1; // CameraModel::Fisheye only // 2D image-projection shader Shader projShader = {}; bool projShaderValid = false; int locPrjXform = -1, locPrjK = -1, locPrjImgSize = -1; int locPrjRad1 = -1, locPrjRad2 = -1, locPrjTan = -1; + int locPrjModel = -1, locPrjXi = -1; // CameraModel::Mei only + int locPrjThetaMax = -1; // CameraModel::Fisheye only int locPrjDepthRange = -1, locPrjOpacity = -1; int locPrjPointSize = -1, locPrjColorMode = -1, locPrjDecim = -1; }; diff --git a/apps/camera_lidar_calibration/RendererShaders.h b/apps/camera_lidar_calibration/RendererShaders.h index 3f803aea..69e07745 100644 --- a/apps/camera_lidar_calibration/RendererShaders.h +++ b/apps/camera_lidar_calibration/RendererShaders.h @@ -22,6 +22,12 @@ uniform int drawDecim; // draw only every Nth point; 1 = draw all uniform mat4 lidarToCam; // extrinsics (for RGB mode) uniform vec4 K; // fx, fy, cx, cy uniform vec2 imgSize; +uniform int model; // set by Renderer.cpp, not a CameraModel ordinal: 0 = Pinhole, 2 = Mei, 3 = Fisheye +uniform float xi; // CameraModel::Mei only +uniform vec3 kRad1; // k1 k2 k3, CameraModel::Mei and Fisheye only +uniform vec3 kRad2; // k4 in .x, CameraModel::Fisheye only +uniform vec2 pTan; // p1 p2, CameraModel::Mei only +uniform float thetaMax; // calib::fisheyeMaxTheta, CameraModel::Fisheye only out vec3 fragPos; out float fragIntensity; out vec2 fragUV; @@ -37,12 +43,46 @@ void main() { gl_Position = mvp * vec4(vertexPosition, 1.0); gl_PointSize = pointSize; - // Project into the camera image for RGB sampling (rectified → pinhole) vec3 lidar = vec3(vertexPosition.x, -vertexPosition.z, vertexPosition.y); vec3 pc = (lidarToCam * vec4(lidar, 1.0)).xyz; - fragCamDepth = pc.z; - vec2 uv = (K.xy * (pc.xy / max(pc.z, 1e-6)) + K.zw) / imgSize; - fragUV = uv; + + if (model == 2) { + // Mei -- unlike Pinhole (below), AppState::rebuildImageTexture never + // undistorts the displayed image for this model, so sampling it + // needs the actual Mei distortion applied here too. Mirrors + // calib::projectPoint's Mei branch (Camera.cpp) and kProjVS's own Mei + // branch below. + float n = length(pc); + vec3 Xs = pc / max(n, 1e-6); + float denom = Xs.z + xi; + // Validity domain, same rule as calib::projectPoint: the projection + // folds back past cos(theta) = -1/xi for xi > 1, and blows up past + // -xi otherwise. fragCamDepth only carries this sign (kPointFS tests + // fragCamDepth > 0.0), not a real depth. + fragCamDepth = Xs.z - ((xi > 1.0) ? -1.0 / xi : -xi); + vec2 xy = Xs.xy / denom; + float r2 = dot(xy, xy); + float radial = 1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2; + vec2 d = xy*radial + vec2(2.0*pTan.x*xy.x*xy.y + pTan.y*(r2 + 2.0*xy.x*xy.x), + pTan.x*(r2 + 2.0*xy.y*xy.y) + 2.0*pTan.y*xy.x*xy.y); + fragUV = (K.xy * d + K.zw) / imgSize; + } else if (model == 3) { + // Fisheye -- never undistorted either, so the same reasoning as Mei. + // Mirrors calib::projectPoint's Fisheye branch. fragCamDepth again + // only carries validity: > 0 short of the fold-back angle. + float r = length(pc.xy); + float theta = atan(r, pc.z); + float t2 = theta*theta; + float thetaD = theta * (1.0 + t2*(kRad1.x + t2*(kRad1.y + t2*(kRad1.z + t2*kRad2.x)))); + fragCamDepth = (r > 0.0 || pc.z > 0.0) ? thetaMax - theta : -1.0; + vec2 d = (r > 0.0) ? pc.xy * (thetaD / r) : vec2(0.0); + fragUV = (K.xy * d + K.zw) / imgSize; + } else { + // Project into the camera image for RGB sampling (rectified → pinhole) + fragCamDepth = pc.z; + vec2 uv = (K.xy * (pc.xy / max(pc.z, 1e-6)) + K.zw) / imgSize; + fragUV = uv; + } } )"; @@ -82,9 +122,15 @@ void main() { )"; // Projects lidar points directly onto the image plane. Position attribute is - // in raylib coords, converted back to lidar frame here. With w = z_cam the - // hardware clip rejects points behind the camera; optional rational+tangential - // distortion handles non-rectified images (pass zeros when rectified). + // in raylib coords, converted back to lidar frame here. Pinhole (model==0): + // rational+tangential distortion (zeros when rectified), w = z_cam so the + // hardware clip rejects points behind the camera. Mei (model==2): unified- + // sphere + polynomial distortion (mirrors calib::projectPoint), with w the + // distance inside the model's valid dome -- Xs.z + min(xi, 1/xi) -- so the + // hardware clip drops both the blow-up (xi <= 1) and the fold-back + // (xi > 1, where far-off-axis directions otherwise re-enter the image). + // Fisheye (model==3): OpenCV's equidistant theta polynomial, with w the + // angle left before calib::fisheyeMaxTheta -- the same clip trick. inline constexpr const char* kProjVS = R"( #version 330 layout(location = 0) in vec3 vertexPosition; @@ -93,8 +139,11 @@ uniform mat4 lidarToCam; // extrinsics uniform vec4 K; // fx, fy, cx, cy uniform vec2 imgSize; uniform vec3 kRad1; // k1 k2 k3 -uniform vec3 kRad2; // k4 k5 k6 +uniform vec3 kRad2; // k4 k5 k6 for Pinhole (model==0), k4 in .x for Fisheye -- Mei has no rational denominator uniform vec2 pTan; // p1 p2 +uniform int model; // set by Renderer.cpp, not a CameraModel ordinal: 0 = Pinhole, 2 = Mei, 3 = Fisheye +uniform float xi; // CameraModel::Mei only +uniform float thetaMax; // calib::fisheyeMaxTheta, CameraModel::Fisheye only uniform float pointSize; uniform int drawDecim; // draw only every Nth point; 1 = draw all out float fragDepth; @@ -108,23 +157,50 @@ void main() { // raylib coords -> lidar: x = rx, y = -rz, z = ry vec3 lidar = vec3(vertexPosition.x, -vertexPosition.z, vertexPosition.y); vec3 pc = (lidarToCam * vec4(lidar, 1.0)).xyz; - fragDepth = pc.z; fragIntensity = vertexIntensity; - vec2 n = pc.xy / max(pc.z, 1e-6); - float r2 = dot(n, n); - float radial = (1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2) - / (1.0 + kRad2.x*r2 + kRad2.y*r2*r2 + kRad2.z*r2*r2*r2); - vec2 d = n * radial - + vec2(2.0*pTan.x*n.x*n.y + pTan.y*(r2 + 2.0*n.x*n.x), - pTan.x*(r2 + 2.0*n.y*n.y) + 2.0*pTan.y*n.x*n.y); + vec2 d; + float w; + if (model == 2) { + float n = length(pc); + fragDepth = n; // range -- physical distance, for depthRange/jet coloring + vec3 Xs = pc / max(n, 1e-6); + float denom = Xs.z + xi; + vec2 xy = Xs.xy / denom; + float r2 = dot(xy, xy); + float radial = 1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2; + d = xy*radial + vec2(2.0*pTan.x*xy.x*xy.y + pTan.y*(r2 + 2.0*xy.x*xy.x), + pTan.x*(r2 + 2.0*xy.y*xy.y) + 2.0*pTan.y*xy.x*xy.y); + // >0 exactly inside the valid dome -- see the block comment above kProjVS + w = Xs.z - ((xi > 1.0) ? -1.0 / xi : -xi); + } else if (model == 3) { + fragDepth = length(pc); // range, like Mei + float r = length(pc.xy); + float theta = atan(r, pc.z); + float t2 = theta*theta; + float thetaD = theta * (1.0 + t2*(kRad1.x + t2*(kRad1.y + t2*(kRad1.z + t2*kRad2.x)))); + d = (r > 0.0) ? pc.xy * (thetaD / r) : vec2(0.0); + // Straight behind (r == 0, z < 0) has no direction and is clipped, + // as calib::projectPoint rejects it. + w = (r > 0.0 || pc.z > 0.0) ? thetaMax - theta : -1.0; + } else { + fragDepth = pc.z; + vec2 n = pc.xy / max(pc.z, 1e-6); + float r2 = dot(n, n); + float radial = (1.0 + kRad1.x*r2 + kRad1.y*r2*r2 + kRad1.z*r2*r2*r2) + / (1.0 + kRad2.x*r2 + kRad2.y*r2*r2 + kRad2.z*r2*r2*r2); + d = n * radial + + vec2(2.0*pTan.x*n.x*n.y + pTan.y*(r2 + 2.0*n.x*n.x), + pTan.x*(r2 + 2.0*n.y*n.y) + 2.0*pTan.y*n.x*n.y); + w = pc.z; + } vec2 uv = K.xy * d + K.zw; // pixel coords // pixel -> clip space (y down, like raylib's render-texture ortho) - gl_Position = vec4((2.0*uv.x/imgSize.x - 1.0) * pc.z, - -(2.0*uv.y/imgSize.y - 1.0) * pc.z, + gl_Position = vec4((2.0*uv.x/imgSize.x - 1.0) * w, + -(2.0*uv.y/imgSize.y - 1.0) * w, 0.0, - pc.z); + w); gl_PointSize = pointSize; } )"; diff --git a/apps/camera_lidar_calibration/UI.cpp b/apps/camera_lidar_calibration/UI.cpp index 4517e7e2..6f45e78e 100644 --- a/apps/camera_lidar_calibration/UI.cpp +++ b/apps/camera_lidar_calibration/UI.cpp @@ -40,6 +40,35 @@ static void helpMarker(const char* desc) } } +// Which physical sensors the loaded data belongs to: the camera's serial and +// frame come from the rig's camera_info.yaml, the LiDAR's from the mandeye +// status sidecar beside the LAZ. Shown together, above everything else, +// because a calibration is only valid for the one pair it was measured on. +static void drawSensorIds(const AppState& state) +{ + if (state.cameraId.empty() && state.lidarId.empty()) + return; + + auto dimmed = [](const std::string& text) + { + ImGui::PushStyleColor(ImGuiCol_Text, ImGui::GetStyleColorVec4(ImGuiCol_TextDisabled)); + ImGui::TextWrapped("%s", text.c_str()); + ImGui::PopStyleColor(); + }; + + if (!state.cameraId.serial.empty()) + ImGui::TextWrapped("Camera: %s (%s)", state.cameraId.serial.c_str(), state.cameraId.model.c_str()); + else if (!state.cameraId.frameId.empty()) + dimmed("Camera: (file named no serial)"); + if (!state.cameraId.frameId.empty()) + dimmed(" frame " + state.cameraId.frameId); + + if (!state.lidarId.empty()) + ImGui::TextWrapped("LiDAR: %s", state.lidarId.c_str()); + + ImGui::Separator(); +} + // ── Main draw ──────────────────────────────────────────────────────────────── void UI::draw(AppState& state) { @@ -61,6 +90,8 @@ void UI::draw(AppState& state) ImGui::TextColored(ImVec4(0.4f, 0.8f, 1.f, 1.f), "LiDAR-Camera Calibration"); ImGui::Separator(); + drawSensorIds(state); + // Alt/Cmd = toggle Camera RGB ↔ Intensity (works anywhere in the window). // Cmd (Super) alongside Alt for macOS, where Option is awkward to use as // a modifier (it composes special characters). @@ -447,22 +478,71 @@ void UI::panelIntrinsics(AppState& state) }; ImGui::PushItemWidth(-80.f); + + static const CameraModel kModels[] = { CameraModel::Pinhole, CameraModel::Mei, CameraModel::Fisheye }; + static const char* kModelNames[] = { "Pinhole", "Mei", "Fisheye (equidistant)" }; + static_assert(IM_ARRAYSIZE(kModels) == IM_ARRAYSIZE(kModelNames)); + int modelIdx = 0; + for (int i = 0; i < IM_ARRAYSIZE(kModels); ++i) + if (K.model == kModels[i]) + modelIdx = i; + if (ImGui::Combo("Model", &modelIdx, kModelNames, IM_ARRAYSIZE(kModelNames))) + { + K.model = kModels[modelIdx]; + edited = true; + } + ImGui::Separator(); + drag("fx", &K.fx, 1.f, 1.f, 10000.f, "%.1f"); drag("fy", &K.fy, 1.f, 1.f, 10000.f, "%.1f"); drag("cx", &K.cx, 0.5f, 0.f, 10000.f, "%.1f"); drag("cy", &K.cy, 0.5f, 0.f, 10000.f, "%.1f"); ImGui::Separator(); - ImGui::Text("Radial (rational model):"); - drag("k1", &K.k1, 0.001f, -100.f, 100.f, "%.4f"); - drag("k2", &K.k2, 0.001f, -100.f, 100.f, "%.4f"); - drag("k3", &K.k3, 0.001f, -100.f, 100.f, "%.4f"); - drag("k4", &K.k4, 0.001f, -100.f, 100.f, "%.4f"); - drag("k5", &K.k5, 0.001f, -100.f, 100.f, "%.4f"); - drag("k6", &K.k6, 0.001f, -100.f, 100.f, "%.4f"); - ImGui::Text("Tangential:"); - drag("p1", &K.p1, 0.0001f, -1.f, 1.f, "%.5f"); - drag("p2", &K.p2, 0.0001f, -1.f, 1.f, "%.5f"); - helpMarker("Drag to adjust. Hold Ctrl+click to type a value."); + + if (K.model == CameraModel::Mei) + { + // Unified-sphere fisheye (see calib::projectPoint): xi + a plain k1/k2/k3 + + // p1/p2 polynomial, no rational denominator -- k4/k5/k6 don't apply + // here, so they're hidden instead of shown as dead controls. + drag("xi", &K.xi, 0.001f, 0.f, 3.f, "%.4f"); + ImGui::Text("Radial (Mei polynomial):"); + drag("k1", &K.k1, 0.001f, -100.f, 100.f, "%.4f"); + drag("k2", &K.k2, 0.001f, -100.f, 100.f, "%.4f"); + drag("k3", &K.k3, 0.001f, -100.f, 100.f, "%.4f"); + ImGui::Text("Tangential:"); + drag("p1", &K.p1, 0.0001f, -1.f, 1.f, "%.5f"); + drag("p2", &K.p2, 0.0001f, -1.f, 1.f, "%.5f"); + helpMarker( + "Drag to adjust. Hold Ctrl+click to type a value.\nUnlike Pinhole, the displayed image is never undistorted for " + "Mei -- the projection overlay and Camera RGB coloring apply this distortion to the raw image directly."); + } + else if (K.model == CameraModel::Fisheye) + { + // OpenCV cv::fisheye (see calib::projectPoint): k1..k4 act on the + // incidence angle, with no tangential terms and no k5/k6. + ImGui::Text("Radial (theta polynomial):"); + drag("k1", &K.k1, 0.001f, -100.f, 100.f, "%.4f"); + drag("k2", &K.k2, 0.001f, -100.f, 100.f, "%.4f"); + drag("k3", &K.k3, 0.001f, -100.f, 100.f, "%.4f"); + drag("k4", &K.k4, 0.001f, -100.f, 100.f, "%.4f"); + helpMarker( + "Drag to adjust. Hold Ctrl+click to type a value.\nUnlike Pinhole, the displayed image is never undistorted for " + "Fisheye -- the projection overlay and Camera RGB coloring apply this distortion to the raw image directly."); + } + else + { + ImGui::Text("Radial (rational model):"); + drag("k1", &K.k1, 0.001f, -100.f, 100.f, "%.4f"); + drag("k2", &K.k2, 0.001f, -100.f, 100.f, "%.4f"); + drag("k3", &K.k3, 0.001f, -100.f, 100.f, "%.4f"); + drag("k4", &K.k4, 0.001f, -100.f, 100.f, "%.4f"); + drag("k5", &K.k5, 0.001f, -100.f, 100.f, "%.4f"); + drag("k6", &K.k6, 0.001f, -100.f, 100.f, "%.4f"); + ImGui::Text("Tangential:"); + drag("p1", &K.p1, 0.0001f, -1.f, 1.f, "%.5f"); + drag("p2", &K.p2, 0.0001f, -1.f, 1.f, "%.5f"); + helpMarker("Drag to adjust. Hold Ctrl+click to type a value."); + } ImGui::PopItemWidth(); if (edited && state.intrinsicsLoaded) diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.cpp b/apps/camera_lidar_trajectory_viewer/RosExport.cpp index 584be0e7..b4421446 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.cpp +++ b/apps/camera_lidar_trajectory_viewer/RosExport.cpp @@ -27,9 +27,7 @@ bool exportRos2Bag(const RosExportInput&, const RosExportOptions&, std::string& #include #include -#include #include -#include #include @@ -199,26 +197,27 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s // ── camera images (+ camera_info) ───────────────────────────────────── if (opt.exportCamera && !in.imageFiles.empty()) { - // Rectification maps (built lazily once the image size is known). - // Mirrors App.cpp: undistort to the same K so that a pinhole - // projection — which is all RViz uses — lines up with the image. - const cv::Mat Km = (cv::Mat_(3, 3) << in.K.fx, 0, in.K.cx, 0, in.K.fy, in.K.cy, 0, 0, 1); - const cv::Mat Dm = (cv::Mat_(1, 8) << in.K.k1, in.K.k2, in.K.p1, in.K.p2, in.K.k3, in.K.k4, in.K.k5, in.K.k6); - cv::Mat map1, map2; - bool mapsReady = false; int camW = 0, camH = 0; - const bool rectify = opt.undistortCamera && in.calibLoaded; - // Original jpeg bytes can be copied verbatim only when we neither - // rectify nor need to re-encode (compressed + no undistort). - const bool copyJpegBytes = opt.compressCamera && !rectify; + // Frames go out exactly as captured, and CameraInfo describes them + // with the real distortion. Rectifying here would only ever have + // worked for Pinhole -- Mei's k1/k2/k3/p1/p2 are its own polynomial + // applied after a unit-sphere step that OpenCV's + // initUndistortRectifyMap and a K/D pair cannot express -- so it + // was a per-model special case that also re-encoded every jpeg. + // Consumers that want rectified images can undistort from the + // published CameraInfo. + const bool mei = in.K.model == CameraModel::Mei; + const bool fisheye = in.K.model == CameraModel::Fisheye; for (const auto& [ts, path] : in.imageFiles) { std::vector outBytes; // jpeg, when compressed cv::Mat outImg; // bgr8, when raw - if (copyJpegBytes) + if (opt.compressCamera) { + // Verbatim: imageFiles is jpeg-only (see imageTsFromName), + // so this neither decodes nor re-encodes. std::ifstream f(path, std::ios::binary); if (!f) continue; @@ -231,29 +230,11 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s cv::Mat bgr = cv::imread(path, cv::IMREAD_COLOR); if (bgr.empty()) continue; - if (rectify) - { - if (!mapsReady) - { - cv::initUndistortRectifyMap(Km, Dm, cv::noArray(), Km, bgr.size(), CV_16SC2, map1, map2); - mapsReady = true; - } - cv::Mat und; - cv::remap(bgr, und, map1, map2, cv::INTER_LINEAR); - bgr = und; - } camW = bgr.cols; camH = bgr.rows; - if (opt.compressCamera) - { - cv::imencode(".jpg", bgr, outBytes); - } - else - { - if (!bgr.isContinuous()) - bgr = bgr.clone(); - outImg = bgr; - } + if (!bgr.isContinuous()) + bgr = bgr.clone(); + outImg = bgr; } if (opt.compressCamera) @@ -299,14 +280,43 @@ bool exportRos2Bag(const RosExportInput& in, const RosExportOptions& opt, std::s ci.header.frame_id = in.cameraFrame; ci.height = static_cast(camH); ci.width = static_cast(camW); - ci.distortion_model = "rational_polynomial"; - if (rectify) // image already rectified → no distortion - ci.d = { 0, 0, 0, 0, 0, 0, 0, 0 }; + if (mei) + { + // No standard ROS model is a unified sphere, so + // this reports the rig's own tag rather than + // claiming plumb_bob/rational_polynomial, which a + // consumer would undistort with badly wrong math. + // + // d is the yaml's (k1, k2, k3, p1, p2) order -- NOT + // OpenCV's (k1, k2, p1, p2, k3) -- with xi appended, + // since CameraInfo has nowhere else to put it and + // the model is unusable without it. K/P stay + // populated: fx/fy/cx/cy mean the usual thing, just + // applied after the unit-sphere step. + ci.distortion_model = "insta360_mei_v2"; + ci.d = { in.K.k1, in.K.k2, in.K.k3, in.K.p1, in.K.p2, in.K.xi }; + ci.k = { in.K.fx, 0.f, in.K.cx, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 1.f }; + ci.r = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + ci.p = { in.K.fx, 0.f, in.K.cx, 0.f, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 0.f, 1.f, 0.f }; + } + else if (fisheye) + { + // ROS's name for OpenCV's fisheye model, which + // image_pipeline undistorts with cv::fisheye. + ci.distortion_model = "equidistant"; + ci.d = { in.K.k1, in.K.k2, in.K.k3, in.K.k4 }; + ci.k = { in.K.fx, 0.f, in.K.cx, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 1.f }; + ci.r = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + ci.p = { in.K.fx, 0.f, in.K.cx, 0.f, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 0.f, 1.f, 0.f }; + } else + { + ci.distortion_model = "rational_polynomial"; ci.d = { in.K.k1, in.K.k2, in.K.p1, in.K.p2, in.K.k3, in.K.k4, in.K.k5, in.K.k6 }; - ci.k = { in.K.fx, 0.f, in.K.cx, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 1.f }; - ci.r = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; - ci.p = { in.K.fx, 0.f, in.K.cx, 0.f, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 0.f, 1.f, 0.f }; + ci.k = { in.K.fx, 0.f, in.K.cx, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 1.f }; + ci.r = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + ci.p = { in.K.fx, 0.f, in.K.cx, 0.f, 0.f, in.K.fy, in.K.cy, 0.f, 0.f, 0.f, 1.f, 0.f }; + } writer.write(ci, kTopicCamInfo, rclcpp::Time(ts)); } } diff --git a/apps/camera_lidar_trajectory_viewer/RosExport.h b/apps/camera_lidar_trajectory_viewer/RosExport.h index 26df0753..5c99eab8 100644 --- a/apps/camera_lidar_trajectory_viewer/RosExport.h +++ b/apps/camera_lidar_trajectory_viewer/RosExport.h @@ -55,11 +55,10 @@ struct RosExportOptions bool exportTf = true; // /tf (dynamic) + /tf_static bool exportCamera = true; // /camera/image_raw[/compressed] + /camera/camera_info - bool compressCamera = true; // true: CompressedImage (jpeg) ; false: raw Image (bgr8) - // Rectify (undistort) images to the pinhole model before writing. Needed for - // RViz-style overlays, which project with the pinhole P and ignore the - // distortion coefficients. When on, CameraInfo is published with zero D. - bool undistortCamera = true; + // true: CompressedImage, the source jpeg copied verbatim; false: raw Image + // (bgr8). Frames are always written as captured -- see RosExport.cpp on why + // nothing is rectified -- so CameraInfo always carries the real distortion. + bool compressCamera = true; // LiDAR can be exported in two flavours, independently: // - undistorted: points as registered by LIO, in the map frame (already diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp index e80ee8b8..911594f3 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewer.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -43,9 +45,9 @@ using namespace calib; namespace fs = std::filesystem; -// Shortcuts help table (Help menu). Only lists this app's actual bindings -- -// no A-Z scaffold like multi_view_tls_registration_step_2's, since -// ShowShortcutsTable() just renders whatever it's given. +//! Shortcuts help table (Help menu). Only lists this app's actual bindings -- +//! no A-Z scaffold like multi_view_tls_registration_step_2's, since +//! ShowShortcutsTable() just renders whatever it's given. static const std::vector appShortcuts = { { "Normal keys", "C", "Toggle compass/ruler" }, { "", "P", "Toggle show path" }, @@ -74,8 +76,8 @@ static const std::vector appShortcuts = { { "", "Shift+R", "Open 'Center of rotation' dialog" }, }; -// Copies `path` into `buf` (truncating to fit), for wiring a native-dialog -// result back into the same fixed-size char[] the matching text field edits. +//! Copies `path` into `buf` (truncating to fit), for wiring a native-dialog +//! result back into the same fixed-size char[] the matching text field edits. static void setBuf(char* buf, size_t bufSize, const std::string& path) { if (path.empty()) @@ -84,7 +86,7 @@ static void setBuf(char* buf, size_t bufSize, const std::string& path) buf[bufSize - 1] = '\0'; } -// Build a time(seconds) -> T_world_lidar map suitable for getInterpolatedPose(). +//! Build a time(seconds) -> T_world_lidar map suitable for getInterpolatedPose(). static std::map buildTrajMap(const Trajectory& traj) { std::map m; @@ -93,8 +95,12 @@ static std::map buildTrajMap(const Trajectory& traj) return m; } -// Interpolated T_world_lidar at ts_ns. Returns false when ts_ns lies outside the -// trajectory range — getInterpolatedPose() signals that with a zero matrix. +//! Interpolated T_world_lidar at a timestamp. +//! @param trajMap trajectory to sample +//! @param ts_ns timestamp, nanoseconds +//! @param out receives the pose +//! @return false when ts_ns lies outside the trajectory range, which +//! getInterpolatedPose() signals with a zero matrix static bool interpPose(const std::map& trajMap, int64_t ts_ns, Eigen::Affine3f& out) { Eigen::Matrix4d T = getInterpolatedPose(trajMap, ts_ns * 1e-9); @@ -106,10 +112,10 @@ static bool interpPose(const std::map& trajMap, int64_t static constexpr double kRad2Deg = 57.295779513082320876; -// Angular speed (deg/s) for every trajectory pose: the rotation change to the next -// pose divided by the time step. Result is parallel to traj.poses; the last entry -// repeats the previous one. Fewer than two poses -> all zeros. Non-increasing -// timestamps (chunk boundaries, duplicates) reuse the previous value. +//! Angular speed (deg/s) for every trajectory pose: the rotation change to the next +//! pose divided by the time step. Result is parallel to traj.poses; the last entry +//! repeats the previous one. Fewer than two poses -> all zeros. Non-increasing +//! timestamps (chunk boundaries, duplicates) reuse the previous value. static std::vector computePoseAngularSpeedDeg(const Trajectory& traj) { const auto& poses = traj.poses; @@ -131,8 +137,8 @@ static std::vector computePoseAngularSpeedDeg(const Trajectory& traj) return speed; } -// Angular speed (deg/s) at the trajectory pose nearest ts_ns. 0 when there's no -// per-pose data (not loaded, or size mismatch with the trajectory). +//! Angular speed (deg/s) at the trajectory pose nearest ts_ns. 0 when there's no +//! per-pose data (not loaded, or size mismatch with the trajectory). static float angularSpeedDegAt(const Trajectory& traj, const std::vector& perPose, int64_t ts_ns) { if (traj.poses.empty() || perPose.size() != traj.poses.size()) @@ -194,6 +200,7 @@ struct ColorPt uint8_t r, g, b; float intensity; int64_t ts_ns; + bool validColor; //!< RGB sampled from an image; false = intensity-gray fallback }; // ── Application state ───────────────────────────────────────────────────────── @@ -201,78 +208,100 @@ struct AppState { Trajectory traj; std::vector imageTsNs; - Intrinsics K; - Extrinsics E; // tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka - Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); // camera orientation in world/LiDAR frame + Intrinsics K; //!< K.model selects pinhole / Mei / fisheye (see CalibCore/Camera.h) + Extrinsics E; //!< tx/ty/tz (camera position); rotation lives in R_wc below, not E.om/fi/ka + Eigen::Matrix3f R_wc = Eigen::Matrix3f::Identity(); //!< camera orientation in world/LiDAR frame Roi roi; + //! Free-form counterpart of `roi`: a per-pixel mask whose rejected pixels + //! are excluded from coloring. Needed to drop the operator/backpack a + //! fisheye rig has permanently in frame, which no rectangle can cut out + //! without taking the scene with it. Kept at the file's own resolution, strictly + //! 0/255 (see loadMask), and resampled where used since images are read at + //! s.imgScale. Coloring only -- the ROS 2 and COLMAP exports are not masked. + cv::Mat mask; //!< empty = none loaded + bool maskEnabled = false; //!< acted on only while `mask` is non-empty + bool maskInvert = false; //!< UI state; loadMask and the toggle flip `mask` itself + char maskBuf[512] = {}; + float maskRejectFrac = 0.f; //!< share of pixels the mask drops, for the UI + bool showMaskOverlay = true; //!< tint the rejected area over the image preview + Texture2D maskTex = {}; //!< that tint, RGBA, built by refreshMaskDerived + bool maskTexValid = false; bool calibLoaded = false; - int imgW = 4656, imgH = 3496; + int imgW = 4656, imgH = 3496; //!< overwritten from the first scanned image by loadImages() - // loaded camera images: timestamp → resized BGR Mat + //! loaded camera images: timestamp → resized BGR Mat std::map imagesFilenamesInTime; - const float imgScale = 1.0f; + //! Downscale applied to every image used for coloring: full-resolution + //! camera frames add up when multiImgColoring holds a chunk's worth at + //! once. Intrinsics are scaled to match. + float imgScale = 1.0f; + //! Manual correction for a constant camera/LiDAR clock offset (e.g. a fixed + //! trigger/USB latency the camera's own timestamps don't account for): + //! t_traj = t_image + timeOffsetSec. Applied wherever an image timestamp is + //! matched against the LiDAR/pose timeline (loadCloud's chunk selection + + //! point matching, exportColmap's per-image pose lookup) -- never to the raw + //! timestamps used for filename lookup or image-list indexing + //! (s.imageTsNs/imagesFilenamesInTime). + double timeOffsetSec = 0.0; GpuCloud cloud; Shader shader = {}; bool shaderOk = false; int locMVP = -1, locPS = -1, locCM = -1, locDecim = -1, locSel = -1; - // Driving orbit's Euler mode (rotateX/rotateY/translate/rotationCenter/ - // isOrtho), not its azimuth/elevation/distance/target mode -- the same - // camera engine multi_view_tls_registration_step_2 uses, manually - // driven through rlgl (see display()'s camera setup) instead of - // raylib's Camera3D/BeginMode3D. + //! Driven in Euler mode (rotateX/rotateY/translate/rotationCenter/isOrtho), + //! not azimuth/elevation/distance/target, through rlgl rather than raylib's + //! Camera3D/BeginMode3D -- see display()'s camera setup. raylib_widgets::OrbitCamera orbit; - // Rebuilt from orbit.euler every frame in display() -- used only for - // drawCompassRuler()'s right/up vectors, same reasoning as step2's own - // app_state.viewLocal (OrbitCamera itself stays Eigen-free). + //! Rebuilt from orbit.euler every frame in display() -- used only for + //! drawCompassRuler()'s right/up vectors, same reasoning as step2's own + //! app_state.viewLocal (OrbitCamera itself stays Eigen-free). Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); bool showCenterOfRotationWindow = false; - // controls + //! controls bool showPath = true; bool showFrustums = true; bool showCompassRuler = true; bool showHelp = false; - bool isolateCamera = false; // render only points colored by the selected (preview) image + bool isolateCamera = false; //!< render only points colored by the selected (preview) image float frustumScale = 0.5f; float pointSize = 1.f; int cloudDecim = 1; int drawDecim = 1; - bool multiImgColoring = true; // false = single image per chunk (midpoint) - // How each point is matched to a camera image: - // 0 = temporal — image nearest in time (± maxWiggle frames, within maxTemporalDist) - // 1 = geometry — among all chunk images the point projects into, the one - // with the smallest depth (closest camera) + bool multiImgColoring = true; //!< false = single image per chunk (midpoint) + //! How each point is matched to a camera image: + //! 0 = temporal — image nearest in time (± maxWiggle frames, within maxTemporalDist) + //! 1 = geometry — among all chunk images the point projects into, the one + //! with the smallest depth (closest camera) int colorStrategy = 0; - float maxTemporalDist = 0.5f; // s: skip images farther than this from the point (temporal) - int maxWiggle = 1; // frames: search startIdx ± maxWiggle for a frustum hit (temporal) + float maxTemporalDist = 0.5f; //!< s: skip images farther than this from the point (temporal) + int maxWiggle = 1; //!< frames: search startIdx ± maxWiggle for a frustum hit (temporal) // ── fast-rotation image filter ───────────────────────────────────────────── - // Per-pose angular speed (deg/s), parallel to traj.poses — filled by - // loadSession(). Images captured while the rig turns faster than - // maxImageAngSpeedDeg are dropped from the colorize pass (motion-smeared). + //! Per-pose angular speed (deg/s), parallel to traj.poses — filled by + //! loadSession(). Images captured while the rig turns faster than + //! maxImageAngSpeedDeg are dropped from the colorize pass (motion-smeared). std::vector poseAngSpeedDeg; - float poseAngSpeedMax = 0.f; // deg/s: peak over the whole session (display only) - bool filterFastImages = true; // drop motion-smeared frames from the colorize pass - float maxImageAngSpeedDeg = 60.f; // deg/s threshold - int angFilteredImgs = 0; // images skipped by the filter in the last colorize pass + float poseAngSpeedMax = 0.f; //!< deg/s: peak over the whole session (display only) + bool filterFastImages = true; //!< drop motion-smeared frames from the colorize pass + float maxImageAngSpeedDeg = 60.f; //!< deg/s threshold + int angFilteredImgs = 0; //!< images skipped by the filter in the last colorize pass - bool useImageColor = false; // true once a colorize pass produced RGB data - int colorMode = 0; // 0=intensity (jet), 1=RGB by image, 2=camera id - int coloredPts = 0; // points that received RGB from an image - int uncoloredPts = 0; // points left as intensity-gray (no image / out of frustum / outside ROI) + bool useImageColor = false; //!< true once a colorize pass produced RGB data + int colorMode = 0; //!< 0=intensity (jet), 1=RGB by image, 2=camera id + int coloredPts = 0; //!< points that received RGB from an image + int uncoloredPts = 0; //!< points left as intensity-gray (no image / out of frustum / outside ROI) char sessionBuf[512] = {}; char calibBuf[512] = {}; char cameraBuf[512] = {}; char exportBuf[512] = "colored.laz"; + bool exportOnlyValidColor = false; //!< LAS/LAZ export skips points without image RGB std::vector exportCloud; - // One entry per loaded LIO chunk ("scan_lio_N"), pointing at a contiguous - // [begin, begin+count) slice of exportCloud. `pose` is the chunk's MRP - // correction transform (identity when there is no session_poses.mrp). Used - // by the "Save session as E57" export to keep the segments as separate - // Data3D blocks instead of one collapsed cloud. + //! One entry per loaded LIO chunk ("scan_lio_N"), naming a contiguous + //! [begin, begin+count) slice of exportCloud. Lets the E57 session export + //! keep the chunks as separate Data3D blocks instead of one collapsed cloud. struct ExportSegment { std::string name; @@ -286,7 +315,7 @@ struct AppState // ── ROS 2 export ────────────────────────────────────────────────────────── char rosOutBuf[512] = "ros2_export"; - int rosStorageIdx = 0; // 0 = mcap, 1 = sqlite3 + int rosStorageIdx = 0; //!< 0 = mcap, 1 = sqlite3 RosExportOptions ros; std::thread rosThread; std::atomic rosBusy{ false }; @@ -297,16 +326,16 @@ struct AppState // ── COLMAP export ───────────────────────────────────────────────────────── char colmapBuf[512] = "colmap_out"; bool colmapCopyImages = false; - int colmapPtDecim = 50; // splat-friendly default (~500k from a 25M cloud) + int colmapPtDecim = 50; //!< splat-friendly default (~500k from a 25M cloud) // ── image viewer ──────────────────────────────────────────────────────── int imgViewIdx = 0; Texture2D imgViewTex = {}; bool imgViewTexValid = false; std::atomic imgViewRequest{ -1 }; - // Bumped when the image set itself is replaced (a camera directory dropped). The loader - // thread skips a request whose index it already served, so without this a swap that keeps - // the same index would leave the previous frame on screen. + //! Bumped when the image set itself is replaced (a camera directory dropped). The loader + //! thread skips a request whose index it already served, so without this a swap that keeps + //! the same index would leave the previous frame on screen. std::atomic imgViewEpoch{ 0 }; std::atomic imgViewStop{ false }; std::atomic imgViewLoading{ false }; @@ -314,25 +343,33 @@ struct AppState cv::Mat imgViewPending; bool imgViewHasNew = false; std::thread imgViewThread; + + // ── synthetic intensity-projection image (drawn next to the photo) ───── + //! Reprojects exportCloud through the same calibration as the colorize + //! pass, jet-colormapped over intensity -- a reference image to check the + //! calibration against the photo by eye. + bool showIntensityProjection = false; + bool intensityProjNeedsUpdate = false; //!< set on toggle/refresh/image change + Texture2D intensityProjTex = {}; + bool intensityProjTexValid = false; + int intensityProjDecim = 1; //!< use every Nth point of exportCloud (perf) + float intensityProjPointRadius = 1.5f; //!< splat radius, in output-image pixels + bool intensityProjOverlay = false; //!< true: alpha-blend on top of the photo instead of side-by-side + float intensityProjAlpha = 0.6f; //!< blend strength when intensityProjOverlay is on }; // ── helpers ─────────────────────────────────────────────────────────────────── -// Plain Eigen::Vector3f -> raylib Vector3 conversion. Used to be an axis -// remap (x, z, -y) that made this app's native Z-up LiDAR data render -// correctly under raylib's Y-up Camera3D/BeginMode3D convention; now that -// the camera is multi_view_tls_registration_step_2's own Z-up rlgl-driven -// one, geometry renders in its native coordinates and this is a no-op -// component copy. +//! Eigen::Vector3f -> raylib Vector3. A plain component copy: the camera is +//! Z-up, so geometry renders in its native coordinates with no axis remap. static Vector3 toVec3(const Eigen::Vector3f& v) { return { v.x(), v.y(), v.z() }; } -// Finds the trajectory pose closest to `ray` (unconditional nearest, no -// distance cutoff) and returns its world-space position -- mirrors -// multi_view_tls_registration_step_2's getClosestTrajectoryPoint(), backed -// by the same shared raylib_widgets::pickNearestPointOnLine() picker. -// Returns false (outPoint untouched) when the trajectory is empty. +//! Trajectory pose closest to `ray` -- unconditional nearest, no distance +//! cutoff. Backed by the same picker step2's getClosestTrajectoryPoint() uses. +//! @param outPoint receives the world-space position +//! @return false, outPoint untouched, when the trajectory is empty static bool nearestTrajectoryPoint(const Trajectory& traj, const Ray& ray, Vector3& outPoint) { if (traj.poses.empty()) @@ -351,12 +388,11 @@ static bool nearestTrajectoryPoint(const Trajectory& traj, const Ray& ray, Vecto return true; } -// Intersects `ray` with the Z=0 ground plane -- same plane -// multi_view_tls_registration_step_2's setNewRotationCenter() intersects -// (via RegistrationPlaneFeature::Plane{0,0,1,0} + rayIntersection()), -// reimplemented directly in raylib/raymath terms since those two types live -// in `core`, which this app deliberately doesn't link. Returns false -// (outPoint untouched) when the ray is ~parallel to the plane. +//! Intersects `ray` with the Z=0 ground plane, as step2's +//! setNewRotationCenter() does -- in raylib/raymath terms, since step2's types +//! live in `core`, which this app deliberately doesn't link. +//! @param outPoint receives the intersection +//! @return false, outPoint untouched, when the ray is ~parallel to the plane static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) { const float kTolerance = 0.0001f; @@ -368,45 +404,93 @@ static bool intersectGroundPlaneZ0(const Ray& ray, Vector3& outPoint) return true; } -// Load all cam0_*.jpg from CAMERA_0 (sibling of session dir) into s.images, resized by s.imgScale. -static void loadImages(AppState& s) +//! Timestamp for a camera frame, or -1 when the file isn't one. Prefers the +//! `.meta.json` sidecar's FRAME_WALL_CLOCK (@ref calib::LoadTimestampFromSideCar) +//! -- the camera's own capture wall clock -- falling back to the timestamp +//! encoded in the filename when no sidecar is found. Layout is "_.jpg" or a bare ".jpg" -- everything up +//! to the last '_' is ignored, so Mandeye's "cam0_" parses without a list +//! of rigs here. +//! @param p file to parse +//! @return the timestamp, or -1 when the name doesn't match. The all-digits +//! check rejects unrelated .jpgs, which would reach std::stoll. +//! @note The filename timestamp is when the frame was saved to disk; the +//! sidecar's FRAME_WALL_CLOCK is a few ms earlier and more accurate, so +//! it wins whenever present rather than merely filling a gap. +static int64_t parseImageTsNs(const fs::path& p) { - s.imagesFilenamesInTime.clear(); - fs::path camDir; - if (s.cameraBuf[0]) + if (p.extension() != ".jpg") + return -1; + std::string stem = p.stem().string(); + if (auto us = stem.rfind('_'); us != std::string::npos) + stem = stem.substr(us + 1); + if (stem.empty() || stem.find_first_not_of("0123456789") != std::string::npos) + return -1; + int64_t ts; + try { - camDir = fs::path(s.cameraBuf); - } - else + ts = std::stoll(stem); + } catch (...) { - camDir = fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; + return -1; } + + if (const auto sidecarTs = calib::LoadTimestampFromSideCar(p.string())) + return static_cast(std::llround(*sidecarTs)); + return ts; +} + +//! Directory holding the camera frames: whatever the user picked, else the +//! CAMERA_0 sibling of the session dir. +static fs::path cameraDir(const AppState& s) +{ + return s.cameraBuf[0] ? fs::path(s.cameraBuf) : fs::path(s.sessionBuf).parent_path() / "CAMERA_0"; +} + +//! AppState::timeOffsetSec in nanoseconds, to match the timestamps. +static int64_t imageTimeOffsetNs(const AppState& s) +{ + return (int64_t)std::llround(s.timeOffsetSec * 1e9); +} + +//! Index every camera frame in the camera directory by timestamp. Also picks up +//! the image dimensions -- read by the ROI default, the frustums and COLMAP's +//! cameras.txt. +static void loadImages(AppState& s) +{ + s.imagesFilenamesInTime.clear(); + fs::path camDir = cameraDir(s); if (!fs::is_directory(camDir)) { - s.status = "No CAMERA_0 dir found"; + s.status = "No camera image dir found: " + camDir.string(); return; } int loaded = 0; for (auto& e : fs::directory_iterator(camDir)) { - std::string n = e.path().filename().string(); - if (n.rfind("cam0_", 0) != 0 || e.path().extension() != ".jpg") + int64_t ts = parseImageTsNs(e.path()); + if (ts < 0) continue; - try - { - // filename: cam0_.jpg → strip prefix (5) and ext (4) - int64_t ts = std::stoll(n.substr(5, n.size() - 9)); - s.imagesFilenamesInTime[ts] = e.path().string(); - ++loaded; - } catch (...) + s.imagesFilenamesInTime[ts] = e.path().string(); + ++loaded; + } + if (!s.imagesFilenamesInTime.empty()) + { + cv::Mat probe = cv::imread(s.imagesFilenamesInTime.begin()->second, cv::IMREAD_COLOR); + if (!probe.empty()) { + s.imgW = probe.cols; + s.imgH = probe.rows; } } + + s.K.width = s.imgW; + s.K.height = s.imgH; s.status = "Images loaded: " + std::to_string(loaded) + " from " + camDir.string(); } -// Parse session_poses.mrp → map from chunk stem (e.g. "scan_lio_0") to Affine3f. +//! Parse session_poses.mrp → map from chunk stem (e.g. "scan_lio_0") to Affine3f. static std::map parseMRP(const fs::path& mrpPath) { std::map result; @@ -483,22 +567,14 @@ static void loadSession(AppState& s) s.poseAngSpeedMax = s.poseAngSpeedDeg.empty() ? 0.f : *std::max_element(s.poseAngSpeedDeg.begin(), s.poseAngSpeedDeg.end()); // camera image timestamps - fs::path camDir = s.cameraBuf[0] ? fs::path(s.cameraBuf) : d.parent_path() / "CAMERA_0"; + fs::path camDir = cameraDir(s); if (fs::is_directory(camDir)) { for (auto& e : fs::directory_iterator(camDir)) { - std::string n = e.path().filename().string(); - if (n.rfind("cam0_", 0) == 0 && e.path().extension() == ".jpg") - { - try - { - int64_t ts = std::stoll(n.substr(5, n.size() - 9)); - s.imageTsNs.push_back(ts); - } catch (...) - { - } - } + int64_t ts = parseImageTsNs(e.path()); + if (ts >= 0) + s.imageTsNs.push_back(ts); } std::sort(s.imageTsNs.begin(), s.imageTsNs.end()); } @@ -507,40 +583,9 @@ static void loadSession(AppState& s) (mrp.empty() ? " (no MRP)" : " +MRP") + " — press Load cloud"; } -// Radius (in normalized camera coords, squared) past which the rational distortion model -// stops being usable. r -> r*radial(r) is only injective up to its turning point; beyond it -// the model folds, so directions far outside the lens' actual field of view map back onto -// valid pixel coordinates. With a strongly-fitted model that is not a corner case: for the -// intrinsics this app is used with, a direction 56 deg off the optical axis lands mid-image -// and one at 60 deg lands exactly on the principal point, painting whatever is at the centre -// of the frame onto geometry the camera never saw. The projection alone cannot tell such a -// fold-back from a genuine hit, so find the turning point once and reject everything past -// it. Scanned numerically -- the turning point of a 6th-order rational function has no -// useful closed form. It always lies outside the image itself (otherwise the calibration -// could not reach its own corners), so no legitimate pixel is lost. -static float maxValidRadiusSq(float k1, float k2, float k3, float k4, float k5, float k6) -{ - auto g = [&](float r) - { - float r2 = r * r; - float den = 1.f + (k4 + (k5 + k6 * r2) * r2) * r2; - if (std::fabs(den) < 1e-9f) - return -1.f; // pole -- certainly past the turning point - return r * (1.f + (k1 + (k2 + k3 * r2) * r2) * r2) / den; - }; - // 8.0 == tan(83 deg), wider than any lens this app sees. A distortion-free model is - // monotonic everywhere and so keeps the whole range, i.e. no behaviour change. - const float kLimit = 8.f, kStep = 0.005f; - float prev = 0.f; - for (float r = kStep; r <= kLimit; r += kStep) - { - float cur = g(r); - if (cur <= prev) - return (r - kStep) * (r - kStep); - prev = cur; - } - return kLimit * kLimit; -} +// The off-axis fold-back cutoff that used to live here now lives in +// calib_core (Camera.cpp's maxValidRadiusSq), applied inside +// calib::projectPoint so every caller gets it -- not just this one. static void loadCloud(AppState& s) { @@ -571,25 +616,32 @@ static void loadCloud(AppState& s) bool canColor = s.calibLoaded && !s.imagesFilenamesInTime.empty(); Eigen::Matrix3f R_wc = canColor ? s.R_wc : Eigen::Matrix3f::Identity(); Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); - float K_fx = s.K.fx * s.imgScale, K_fy = s.K.fy * s.imgScale; - float K_cx = s.K.cx * s.imgScale, K_cy = s.K.cy * s.imgScale; - // OpenCV rational + tangential distortion applied to each projected point, so + // Images are read at s.imgScale, so the intrinsics must match. For pinhole + // calib::projectPoint applies the rational + tangential distortion, so // colours are sampled from the raw (distorted) images at the right pixel. - // With all-zero coefficients this reduces exactly to the pinhole model. - const float d_k1 = s.K.k1, d_k2 = s.K.k2, d_k3 = s.K.k3; - const float d_k4 = s.K.k4, d_k5 = s.K.k5, d_k6 = s.K.k6; - const float d_p1 = s.K.p1, d_p2 = s.K.p2; - // (x, y) = normalized camera coords (X/Z, Y/Z) → distorted normalized coords. - auto distort = [=](float x, float y, float& xd, float& yd) - { - float r2 = x * x + y * y; - float radial = (1.f + (d_k1 + (d_k2 + d_k3 * r2) * r2) * r2) / (1.f + (d_k4 + (d_k5 + d_k6 * r2) * r2) * r2); - xd = x * radial + 2.f * d_p1 * x * y + d_p2 * (r2 + 2.f * x * x); - yd = y * radial + d_p1 * (r2 + 2.f * y * y) + 2.f * d_p2 * x * y; + const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + // The ROI is in full-resolution pixels (see calib::Roi) but probe() tests + // it against pixels read at s.imgScale, so it scales like the intrinsics. + const Roi roiS = scaleRoi(s.roi, s.imgScale); + const int64_t offNs = imageTimeOffsetNs(s); + // The mask is at its file's resolution while images are read at s.imgScale, + // so it is resampled -- lazily, on the first image probed, since the frame + // size isn't known until one has been read. + const bool haveMask = !s.mask.empty(); + cv::Mat maskFit; + // Every image of a chunk is held in memory at once (multiImgColoring), so + // for large frames the scale is what keeps that bounded. + auto readImage = [&](const std::string& path) + { + cv::Mat img = cv::imread(path); + if (!img.empty() && s.imgScale != 1.0f) + { + cv::Mat small; + cv::resize(img, small, cv::Size(), s.imgScale, s.imgScale, cv::INTER_AREA); + img = std::move(small); + } + return img; }; - // Off-axis cutoff for the model above -- see maxValidRadiusSq(). - const float rMaxSq = maxValidRadiusSq(d_k1, d_k2, d_k3, d_k4, d_k5, d_k6); - auto packGray = [](float intensity) -> float { uint8_t g = (uint8_t)(std::min(1.f, std::max(0.f, intensity)) * 255.f); @@ -640,10 +692,11 @@ static void loadCloud(AppState& s) if (line.empty()) continue; std::istringstream ss(line); - int64_t ts; - ss >> ts; + double tsD; // may carry a fraction, see Trajectory::loadCSV + ss >> tsD; if (!ss) continue; + const int64_t ts = std::llround(tsD); if (!chunkFirst) chunkFirst = ts; chunkLast = ts; @@ -657,12 +710,15 @@ static void loadCloud(AppState& s) { if (s.multiImgColoring) { - // new: every image whose timestamp falls inside the chunk range - auto it0 = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkFirst); - auto it1 = std::upper_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkLast); + // new: every image whose timestamp falls inside the chunk range. + // Search bounds are shifted by -offNs since s.imageTsNs holds raw + // (unshifted) camera timestamps: imgTs+offNs in [chunkFirst, + // chunkLast] <=> imgTs in [chunkFirst-offNs, chunkLast-offNs]. + auto it0 = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkFirst - offNs); + auto it1 = std::upper_bound(s.imageTsNs.begin(), s.imageTsNs.end(), chunkLast - offNs); for (auto it = it0; it != it1; ++it) { - int64_t imgTs = *it; + int64_t imgTs = *it; // raw camera-clock timestamp; keyed as-is into imagesFilenamesInTime auto fnIt = s.imagesFilenamesInTime.find(imgTs); if (fnIt == s.imagesFilenamesInTime.end()) continue; @@ -672,19 +728,22 @@ static void loadCloud(AppState& s) continue; } Eigen::Affine3f pose; - if (!interpPose(trajMap, imgTs, pose)) + if (!interpPose(trajMap, imgTs + offNs, pose)) continue; - cv::Mat img = cv::imread(fnIt->second); + cv::Mat img = readImage(fnIt->second); if (img.empty()) continue; int gidx = (int)(it - s.imageTsNs.begin()); - chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); + // ImgEntry.ts is stored already shifted into the LiDAR clock, + // since it's compared against pt.ts_ns further below. + chunkImgs.push_back({ imgTs + offNs, pose, std::move(img), gidx }); } } else { - // legacy: single image nearest to chunk midpoint - int64_t mid = chunkFirst; + // legacy: single image nearest to chunk midpoint (see note above + // on why the search target is shifted by -offNs) + int64_t mid = chunkFirst - offNs; auto it = std::lower_bound(s.imageTsNs.begin(), s.imageTsNs.end(), mid); if (it == s.imageTsNs.end()) --it; @@ -700,12 +759,12 @@ static void loadCloud(AppState& s) const bool tooFast = dropFastImgs && angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, imgTs) > s.maxImageAngSpeedDeg; if (tooFast) ++angFilteredImgs; - if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs, pose)) + if (!tooFast && fnIt != s.imagesFilenamesInTime.end() && interpPose(trajMap, imgTs + offNs, pose)) { - cv::Mat img = cv::imread(fnIt->second); + cv::Mat img = readImage(fnIt->second); int gidx = (int)(it - s.imageTsNs.begin()); if (!img.empty()) - chunkImgs.push_back({ imgTs, pose, std::move(img), gidx }); + chunkImgs.push_back({ imgTs + offNs, pose, std::move(img), gidx }); } } } @@ -781,34 +840,46 @@ static void loadCloud(AppState& s) return h; auto& e = chunkImgs[idx]; Eigen::Vector3f pl = e.pose.inverse() * pw; - Eigen::Vector3f pc_ = R_wc.transpose() * (pl - C); - if (pc_.z() <= 0.05f) + float u, v, depth; + if (!projectPoint(pl.x(), pl.y(), pl.z(), Ks, R_wc, C, u, v, depth)) return h; - float xn = pc_.x() / pc_.z(), yn = pc_.y() / pc_.z(); - // Outside the cone the lens model is valid over: distorting this would - // fold it back into the frame. See maxValidRadiusSq(). - if (xn * xn + yn * yn > rMaxSq) + // Too close to the lens to be a real observation. Mei and + // Fisheye too: their depth is a range rather than a z, but + // 5 cm means the same thing physically, and projectPoint's + // guards only reject a point essentially AT the camera. + if (depth <= 0.05f) return h; - float xd, yd; - distort(xn, yn, xd, yd); - int iu = (int)std::round(K_fx * xd + K_cx); - int iv = (int)std::round(K_fy * yd + K_cy); + int iu = (int)std::round(u); + int iv = (int)std::round(v); if (iu < 0 || iu >= e.img.cols || iv < 0 || iv >= e.img.rows) return h; - // point projects into this image — record ROI membership so - // the "In ROI" render mode can show it, independent of whether - // the ROI filter is currently enabled. - bool haveRoi = s.roi.w > 0 && s.roi.h > 0; - bool insideRoi = !haveRoi || (iu >= s.roi.x && iu < s.roi.x + s.roi.w && iv >= s.roi.y && iv < s.roi.y + s.roi.h); - h.inRoiF = insideRoi ? 1.f : 0.f; - // outside the region of interest? leave the point uncolored - if (s.roi.enabled && !insideRoi) + // point projects into this image — record ROI/mask membership + // so the "In ROI / mask" render mode can show it, independent + // of whether either filter is currently enabled. + bool haveRoi = roiS.w > 0 && roiS.h > 0; + bool insideRoi = !haveRoi || (iu >= roiS.x && iu < roiS.x + roiS.w && iv >= roiS.y && iv < roiS.y + roiS.h); + bool insideMask = true; + if (haveMask) + { + // INTER_NEAREST, so the mask stays strictly 0/255: a + // bilinear resize would invent half-masked pixels along + // every edge, which the test below would then silently + // round one way. Every frame of a session is the same + // size, so this resizes once. + if (maskFit.cols != e.img.cols || maskFit.rows != e.img.rows) + cv::resize(s.mask, maskFit, e.img.size(), 0, 0, cv::INTER_NEAREST); + insideMask = maskFit.at(iv, iu) != 0; + } + h.inRoiF = (insideRoi && insideMask) ? 1.f : 0.f; + // outside the region of interest, or masked out? leave the + // point uncolored + if ((s.roi.enabled && !insideRoi) || (s.maskEnabled && !insideMask)) return h; cv::Vec3b bgr = e.img.at(iv, iu); uint32_t p = (uint32_t(bgr[2]) << 16) | (uint32_t(bgr[1]) << 8) | uint32_t(bgr[0]); std::memcpy(&h.colorF, &p, 4); h.globalIdx = e.globalIdx; - h.depth = pc_.z(); + h.depth = depth; h.ok = true; return h; }; @@ -892,7 +963,8 @@ static void loadCloud(AppState& s) (uint8_t)((packed >> 8) & 0xFF), (uint8_t)(packed & 0xFF), rawIntensity, - pt.ts_ns }); + pt.ts_ns, + camIdF >= 0.f }); float d2 = pw.squaredNorm(); if (d2 > mx * mx) @@ -921,13 +993,9 @@ static void loadCloud(AppState& s) { s.cloud.upload(gpuData, mx); - // Frame the loaded cloud -- instant, not eased (this runs once on - // load, before there's anything to transition from). Same "recenter - // and look at" formula as OrbitCamera::moveEulerRotationCenterTo() - // (translate.xy = -center.xy keeps the point centered on screen - // regardless of the current rotate angles), applied directly to - // both euler and eulerGoal so there's no stale transition target - // left over from a previous session. + // Frame the loaded cloud, instant rather than eased -- this runs once on + // load, with nothing to transition from. Set on both euler and eulerGoal + // so no stale transition target survives from a previous session. Vector3 center = { sumX / cnt, sumY / cnt, sumZ / cnt }; float dist = std::max(5.f, mx * 0.3f); s.orbit.euler.rotationCenter = center; @@ -948,6 +1016,91 @@ static void loadCloud(AppState& s) s.status += " | Fast-img filtered: " + std::to_string(angFilteredImgs); } +//! Small CPU jet colormap approximation, matching the GLSL one used by the +//! GPU point renderer's Intensity color mode (raylib_widgets::kJetColormapGLSL) +//! closely enough for a visual reference image. Returns BGR (OpenCV order). +static cv::Vec3b jetColorBGR(float t) +{ + t = std::clamp(t, 0.f, 1.f); + float r = std::clamp(1.5f - std::fabs(4.f * t - 3.f), 0.f, 1.f); + float g = std::clamp(1.5f - std::fabs(4.f * t - 2.f), 0.f, 1.f); + float b = std::clamp(1.5f - std::fabs(4.f * t - 1.f), 0.f, 1.f); + return cv::Vec3b((uchar)(b * 255.f), (uchar)(g * 255.f), (uchar)(r * 255.f)); +} + +//! Rasterizes a synthetic "intensity image" for the camera pose at imgTsAdj, +//! reprojecting s.exportCloud through the same extrinsics and projectPoint() as +//! the colorize pass, jet-colormapped over intensity with a per-pixel depth test +//! so occluded points don't bleed through. Points more than s.maxTemporalDist +//! (1s fallback) from imgTsAdj are skipped, the same temporal gate the +//! "Temporal" coloring strategy applies -- otherwise every preview would test +//! the whole session's cloud. +static cv::Mat renderIntensityProjection(const AppState& s, int64_t imgTsAdj) +{ + const Intrinsics Ks = scaleIntrinsics(s.K, s.imgScale); + cv::Mat out(std::max(1, Ks.height), std::max(1, Ks.width), CV_8UC3, cv::Scalar(25, 25, 25)); + if (s.exportCloud.empty() || Ks.width <= 0 || Ks.height <= 0) + return out; + + auto trajMap = buildTrajMap(s.traj); + Eigen::Affine3f pose; + if (!interpPose(trajMap, imgTsAdj, pose)) + return out; + const Eigen::Affine3f poseInv = pose.inverse(); + const Eigen::Matrix3f& R_wc = s.R_wc; + const Eigen::Vector3f C(s.E.tx, s.E.ty, s.E.tz); + + const int64_t windowNs = (int64_t)((s.maxTemporalDist > 0.f ? s.maxTemporalDist : 1.0f) * 1e9); + const int step = std::max(1, s.intensityProjDecim); + const int radius = std::max(1, (int)std::lround(s.intensityProjPointRadius)); + + cv::Mat depthBuf(out.rows, out.cols, CV_32F, cv::Scalar(std::numeric_limits::max())); + for (size_t i = 0; i < s.exportCloud.size(); i += step) + { + const auto& p = s.exportCloud[i]; + if (std::abs(p.ts_ns - imgTsAdj) > windowNs) + continue; + Eigen::Vector3f pl = poseInv * Eigen::Vector3f(p.x, p.y, p.z); + float u, v, depth; + if (!projectPoint(pl.x(), pl.y(), pl.z(), Ks, R_wc, C, u, v, depth)) + continue; + if (Ks.model == CameraModel::Pinhole && depth <= 0.05f) + continue; + // Points near-grazing the camera plane get blown up to huge u/v by the + // perspective divide, and this function pulls in a whole time window's + // worth, so it hits that far more often than colorize() does. Casting + // such a value with (int)std::round() is UB -- the "bowtie" artifact -- + // so reject before the cast. + if (!std::isfinite(u) || !std::isfinite(v) || std::fabs(u) > 1e6f || std::fabs(v) > 1e6f) + continue; + int iu = (int)std::round(u); + int iv = (int)std::round(v); + const cv::Vec3b col = jetColorBGR(p.intensity); + + for (int dy = -radius; dy <= radius; ++dy) + { + int yy = iv + dy; + if (yy < 0 || yy >= out.rows) + continue; + for (int dx = -radius; dx <= radius; ++dx) + { + if (dx * dx + dy * dy > radius * radius) + continue; + int xx = iu + dx; + if (xx < 0 || xx >= out.cols) + continue; + float& zb = depthBuf.at(yy, xx); + if (depth < zb) + { + zb = depth; + out.at(yy, xx) = col; + } + } + } + } + return out; +} + static void loadCalib(AppState& s) { std::ifstream f(s.calibBuf); @@ -958,6 +1111,28 @@ static void loadCalib(AppState& s) } nlohmann::json j; f >> j; + // Any name calib::modelFromString knows ("mei", "fisheye"/"equidistant"), + // plus the rig's "insta360_mei_v2"; anything else pinhole. Accepted at the + // top level or inside "intrinsics". Assigned unconditionally, so loading + // a pinhole calibration after another model does not inherit it. + { + const bool topLevel = j.contains("model"); + const bool nested = j.contains("intrinsics") && j["intrinsics"].contains("model"); + std::string model; + if (topLevel) + model = j.value("model", std::string{}); + else if (nested) + model = j["intrinsics"].value("model", std::string{}); + std::transform( + model.begin(), + model.end(), + model.begin(), + [](unsigned char c) + { + return (char)std::tolower(c); + }); + s.K.model = model == "insta360_mei_v2" ? CameraModel::Mei : modelFromString(model); + } if (j.contains("intrinsics")) { auto& ji = j["intrinsics"]; @@ -965,7 +1140,10 @@ static void loadCalib(AppState& s) s.K.fy = ji.value("fy", s.K.fy); s.K.cx = ji.value("cx", s.K.cx); s.K.cy = ji.value("cy", s.K.cy); - // rational distortion model (used by ROS export to rectify images) + // Pinhole: the rational distortion model (also what the ROS export + // rectifies with). Mei reuses k1/k2/k3 and p1/p2 as its own plain + // polynomial and adds xi, leaving k4/k5/k6 unused; Fisheye reads + // k1..k4 as its theta polynomial -- see CalibCore/Camera.h. s.K.k1 = ji.value("k1", s.K.k1); s.K.k2 = ji.value("k2", s.K.k2); s.K.k3 = ji.value("k3", s.K.k3); @@ -974,6 +1152,7 @@ static void loadCalib(AppState& s) s.K.k6 = ji.value("k6", s.K.k6); s.K.p1 = ji.value("p1", s.K.p1); s.K.p2 = ji.value("p2", s.K.p2); + s.K.xi = ji.value("xi", s.K.xi); } if (j.contains("extrinsics")) { @@ -1012,19 +1191,104 @@ static void loadCalib(AppState& s) s.status = "Calibration loaded"; } +//! Rebuilds what is derived from s.mask: the rejected-pixel share the UI +//! reports, and the translucent red overlay drawn over the image preview. Call +//! after anything that changes the mask. Main thread only -- it creates a GL +//! texture. +static void refreshMaskDerived(AppState& s) +{ + if (s.maskTexValid) + { + UnloadTexture(s.maskTex); + s.maskTexValid = false; + } + if (s.mask.empty()) + { + s.maskRejectFrac = 0.f; + return; + } + const int total = s.mask.rows * s.mask.cols; + const int kept = cv::countNonZero(s.mask); + s.maskRejectFrac = total ? (float)(total - kept) / (float)total : 0.f; + + // The overlay only has to read correctly in a preview pane, so it is capped + // well below the camera's frame size rather than uploading a full-resolution + // texture to show a hand-painted blob. + cv::Mat m = s.mask; + const int kMaxSide = 1024; + const int longSide = std::max(m.cols, m.rows); + if (longSide > kMaxSide) + cv::resize(s.mask, m, cv::Size(), (double)kMaxSide / longSide, (double)kMaxSide / longSide, cv::INTER_NEAREST); + cv::Mat rgba(m.rows, m.cols, CV_8UC4); + for (int y = 0; y < m.rows; ++y) + { + const uint8_t* srcRow = m.ptr(y); + cv::Vec4b* dstRow = rgba.ptr(y); + for (int x = 0; x < m.cols; ++x) + dstRow[x] = srcRow[x] ? cv::Vec4b(0, 0, 0, 0) : cv::Vec4b(255, 40, 40, 110); + } + Image ri = { rgba.data, rgba.cols, rgba.rows, 1, PIXELFORMAT_UNCOMPRESSED_R8G8B8A8 }; + s.maskTex = LoadTextureFromImage(ri); + s.maskTexValid = s.maskTex.id > 0; +} + +//! Loads the mask named by s.maskBuf. Any format OpenCV reads is reduced to one +//! 8-bit channel thresholded at 128, so a pixel is either kept or dropped, never +//! partly, and a jpeg mask's compression noise can't leak in as almost-black. +//! White keeps, black drops, unless "Invert mask" is on. Any resolution works -- +//! the mask is resampled to the frame size in loadCloud. +static void loadMask(AppState& s) +{ + if (!s.maskBuf[0]) + { + s.status = "No mask file selected"; + return; + } + cv::Mat img = cv::imread(s.maskBuf, cv::IMREAD_GRAYSCALE); + if (img.empty()) + { + s.status = std::string("Failed to read mask: ") + s.maskBuf; + return; + } + cv::threshold(img, s.mask, 128, 255, s.maskInvert ? cv::THRESH_BINARY_INV : cv::THRESH_BINARY); + s.maskEnabled = true; + refreshMaskDerived(s); + char msg[160]; + std::snprintf(msg, sizeof(msg), "Mask loaded: %dx%d, %.1f%% masked out", s.mask.cols, s.mask.rows, s.maskRejectFrac * 100.f); + s.status = msg; +} + +//! Drops the mask entirely, as opposed to unticking "Image mask", which keeps +//! it loaded and ready to re-enable. +static void clearMask(AppState& s) +{ + s.mask.release(); + s.maskEnabled = false; + s.maskBuf[0] = '\0'; + refreshMaskDerived(s); + s.status = "Mask cleared"; +} + static void exportLAZ(AppState& s) { - if (s.exportCloud.empty()) + auto keep = [&](const ColorPt& p) { - s.status = "No cloud to export"; + return !s.exportOnlyValidColor || p.validColor; + }; + const size_t nOut = std::count_if(s.exportCloud.begin(), s.exportCloud.end(), keep); + if (nOut == 0) + { + s.status = s.exportCloud.empty() ? "No cloud to export" : "No points with valid color to export"; return; } - double xmin = s.exportCloud[0].x, xmax = xmin; - double ymin = s.exportCloud[0].y, ymax = ymin; - double zmin = s.exportCloud[0].z, zmax = zmin; + double xmin = std::numeric_limits::max(), xmax = std::numeric_limits::lowest(); + double ymin = xmin, ymax = xmax; + double zmin = xmin, zmax = xmax; for (auto& p : s.exportCloud) { + if (!keep(p)) + continue; xmin = std::min(xmin, (double)p.x); xmax = std::max(xmax, (double)p.x); ymin = std::min(ymin, (double)p.y); @@ -1049,7 +1313,7 @@ static void exportLAZ(AppState& s) header->offset_to_point_data = 227; header->point_data_format = 3; // XYZ + RGB + GPS time header->point_data_record_length = 34; - header->number_of_point_records = (uint32_t)s.exportCloud.size(); + header->number_of_point_records = (uint32_t)nOut; header->x_scale_factor = 0.001; header->y_scale_factor = 0.001; header->z_scale_factor = 0.001; @@ -1079,6 +1343,8 @@ static void exportLAZ(AppState& s) laszip_F64 coords[3]; for (auto& p : s.exportCloud) { + if (!keep(p)) + continue; coords[0] = p.x; coords[1] = p.y; coords[2] = p.z; @@ -1095,11 +1361,11 @@ static void exportLAZ(AppState& s) laszip_close_writer(writer); laszip_destroy(writer); - s.status = "Exported " + std::to_string(s.exportCloud.size()) + " pts → " + s.exportBuf; + s.status = "Exported " + std::to_string(nOut) + " pts → " + s.exportBuf; } -// E57 counterpart of exportLAZ(): one Data3D block, points already in world -// coordinates (identity pose), RGB + intensity + per-point timestamp. +//! E57 counterpart of exportLAZ(): one Data3D block, points already in world +//! coordinates (identity pose), RGB + intensity + per-point timestamp. static void exportE57(AppState& s) { if (s.exportCloud.empty()) @@ -1139,11 +1405,9 @@ static void exportE57(AppState& s) s.status = std::string("Export failed: ") + err; } -// Save the colored cloud as a *session*: one E57 Data3D block per loaded LIO -// chunk ("scan_lio_N"), NOT one collapsed cloud. Each block holds that -// segment's points in its own frame with the chunk's MRP correction as the -// block pose (identity when there is no session_poses.mrp), so the result -// re-opens as a multi-scan session (e.g. in step 2). +//! Save the colored cloud as a session: one E57 Data3D block per LIO chunk +//! rather than one collapsed cloud, each in its own frame with the chunk's MRP +//! correction as the block pose, so it re-opens as a multi-scan session. static void exportE57Session(AppState& s) { if (s.exportSegments.empty()) @@ -1203,9 +1467,9 @@ static void exportE57Session(AppState& s) } // ── File actions ───────────────────────────────────────────────────────────── -// Factored out so the File menu items and their keyboard shortcuts (in the -// main loop below) call the exact same code, matching the openSession()-style -// convention used by mandeye_single_session_viewer/multi_view_tls_registration. +//! Factored out so the File menu items and their keyboard shortcuts (in the +//! main loop below) call the exact same code, matching the openSession()-style +//! convention used by mandeye_single_session_viewer/multi_view_tls_registration. static void actionSelectLioResultDir(AppState& s) { setBuf(s.sessionBuf, sizeof(s.sessionBuf), mandeye::fd::SelectFolder("Select LIO result directory")); @@ -1226,32 +1490,43 @@ static void actionOpenCalibration(AppState& s) } } -// A directory holding this app's camera frames (cam0_.jpg). -static bool isCameraDir(const fs::path& dir) +//! Whether a directory holds a *.mjs session manifest directly -- lidar_odometry_step_1 +//! writes session.mjs alongside session_poses.mrp/session_ini_poses.mri, so this is a +//! reliable positive marker for "this is a LIO result (session) directory". +static bool hasMjsFile(const fs::path& dir) { for (const auto& e : fs::directory_iterator(dir)) - { - std::string n = e.path().filename().string(); - if (n.rfind("cam0_", 0) == 0 && e.path().extension() == ".jpg") + if (e.path().extension() == ".mjs") return true; - } return false; } -// Drag & drop equivalent of actionSelectLioResultDir()/actionSelectCamera0Dir()/ -// actionOpenCalibration(), and unlike those menu actions it applies immediately instead of -// waiting for the "Load session" button, since a drop is already an explicit "load this" -// gesture. A dropped directory of cam0_*.jpg is the camera directory (only the images are -// swapped, so the trajectory and the loaded cloud survive); any other directory is this -// app's session (LIO result dir). A dropped *.json is treated as a calibration file. Used by -// the drag & drop handler in main()'s loop below. +//! Menu action: pick a mask image and load it into s.mask. +static void actionOpenMask(AppState& s) +{ + std::string path = mandeye::fd::OpenFileDialogOneFile("Select image mask", mandeye::fd::ImageFilter); + if (!path.empty()) + { + setBuf(s.maskBuf, sizeof(s.maskBuf), path); + loadMask(s); + } +} + +//! Drag & drop equivalent of the menu load actions, applied immediately rather +//! than waiting for "Load session" -- a drop is already an explicit "load this". +//! A dropped directory containing a *.mjs manifest is a session (LIO result +//! dir); any other dropped directory is the camera directory (only the images +//! are swapped, so the trajectory and cloud survive); a *.mjs file is a +//! session manifest (its parent directory is the session, as with --mjs); a +//! *.json is a calibration file. static void handleDroppedPath(AppState& s, const std::string& path) { if (fs::is_directory(path)) { - // Checked before the session branch: a CAMERA_0 folder is never a LIO result dir, - // and dropping one onto a loaded session must not wipe the trajectory. - if (isCameraDir(path)) + // Checked before the session branch: only a *.mjs manifest marks a LIO + // result dir, so dropping a plain image folder onto a loaded session + // must not wipe the trajectory. + if (!hasMjsFile(path)) { setBuf(s.cameraBuf, sizeof(s.cameraBuf), path); loadImages(s); @@ -1280,6 +1555,20 @@ static void handleDroppedPath(AppState& s, const std::string& path) setBuf(s.calibBuf, sizeof(s.calibBuf), path); loadCalib(s); } + else if (ext == ".mjs") + { + // Session manifest, same convention as --mjs: the session directory is its parent. + setBuf(s.sessionBuf, sizeof(s.sessionBuf), fs::path(path).parent_path().string()); + loadSession(s); + } + else if (ext == ".png" || ext == ".bmp" || ext == ".jpg" || ext == ".jpeg") + { + // The only single image this app takes as input is a mask -- camera + // frames arrive as the session's whole CAMERA_0 directory, never one + // file at a time. + setBuf(s.maskBuf, sizeof(s.maskBuf), path); + loadMask(s); + } else { s.status = "Unsupported dropped file: " + path; @@ -1329,8 +1618,8 @@ static void actionSelectColmapOutputDir(AppState& s) setBuf(s.colmapBuf, sizeof(s.colmapBuf), mandeye::fd::SelectFolder("Select COLMAP output directory")); } -// Export a COLMAP sparse text model (cameras/images/points3D) from the current -// state. Poses are world->camera; the colored cloud becomes points3D. +//! Export a COLMAP sparse text model (cameras/images/points3D) from the current +//! state. Poses are world->camera; the colored cloud becomes points3D. static void exportColmap(AppState& s) { if (!s.calibLoaded) @@ -1343,6 +1632,14 @@ static void exportColmap(AppState& s) s.status = "COLMAP: no images"; return; } + if (s.K.model == CameraModel::Mei) + { + // None of COLMAP's fisheye types is the unified-sphere (Mei) model -- + // none carries an xi -- so the cameras.txt line below would + // misdescribe the images. + s.status = "COLMAP: Mei camera model is not supported by COLMAP"; + return; + } fs::path out(s.colmapBuf); fs::path sparse = out / "sparse"; @@ -1359,15 +1656,20 @@ static void exportColmap(AppState& s) T_lc.linear() = s.R_wc; T_lc.translation() = Eigen::Vector3f(s.E.tx, s.E.ty, s.E.tz); - // cameras.txt — rational OpenCV model == COLMAP FULL_OPENCV (12 params) + // cameras.txt — rational OpenCV model == COLMAP FULL_OPENCV (12 params), + // OpenCV fisheye == COLMAP OPENCV_FISHEYE (8 params) { std::ofstream f(sparse / "cameras.txt"); f << std::setprecision(12); f << "# Camera list with one line of data per camera:\n" "# CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]\n"; - f << "1 FULL_OPENCV " << s.imgW << ' ' << s.imgH << ' ' << s.K.fx << ' ' << s.K.fy << ' ' << s.K.cx << ' ' << s.K.cy << ' ' - << s.K.k1 << ' ' << s.K.k2 << ' ' << s.K.p1 << ' ' << s.K.p2 << ' ' << s.K.k3 << ' ' << s.K.k4 << ' ' << s.K.k5 << ' ' << s.K.k6 - << '\n'; + if (s.K.model == CameraModel::Fisheye) + f << "1 OPENCV_FISHEYE " << s.imgW << ' ' << s.imgH << ' ' << s.K.fx << ' ' << s.K.fy << ' ' << s.K.cx << ' ' << s.K.cy << ' ' + << s.K.k1 << ' ' << s.K.k2 << ' ' << s.K.k3 << ' ' << s.K.k4 << '\n'; + else + f << "1 FULL_OPENCV " << s.imgW << ' ' << s.imgH << ' ' << s.K.fx << ' ' << s.K.fy << ' ' << s.K.cx << ' ' << s.K.cy << ' ' + << s.K.k1 << ' ' << s.K.k2 << ' ' << s.K.p1 << ' ' << s.K.p2 << ' ' << s.K.k3 << ' ' << s.K.k4 << ' ' << s.K.k5 << ' ' + << s.K.k6 << '\n'; } // images.txt — one image per camera frame, pose = world->camera @@ -1379,11 +1681,12 @@ static void exportColmap(AppState& s) "# IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n" "# POINTS2D[] as (X, Y, POINT3D_ID)\n"; auto trajMap = buildTrajMap(s.traj); + const int64_t offNs = imageTimeOffsetNs(s); int id = 1; for (auto& [ts, path] : s.imagesFilenamesInTime) { Eigen::Affine3f pose; - if (!interpPose(trajMap, ts, pose)) + if (!interpPose(trajMap, ts + offNs, pose)) continue; Eigen::Affine3f T_wc = pose * T_lc; // camera in world Eigen::Affine3f T_cw = T_wc.inverse(); // world -> camera @@ -1447,11 +1750,14 @@ static void exportColmap(AppState& s) s.status = "COLMAP: " + std::to_string(nImg) + " images, " + std::to_string(nPts) + " points (+ply) -> " + sparse.string(); } -// Gather everything the ROS exporter needs from current viewer state. +//! Gather everything the ROS exporter needs from current viewer state. static void buildRosInput(AppState& s, RosExportInput& in) { in.traj = s.traj; - in.imageFiles = s.imagesFilenamesInTime; + // Stamps go into the bag on the trajectory clock, like every other topic. + in.imageFiles.clear(); + for (const auto& [ts, path] : s.imagesFilenamesInTime) + in.imageFiles[ts + imageTimeOffsetNs(s)] = path; in.calibLoaded = s.calibLoaded; in.K = s.K; in.E = s.E; @@ -1549,12 +1855,31 @@ static void drawScene(AppState& s) for (int64_t ts : s.imageTsNs) { - const TrajPose* pose = s.traj.nearest(ts); + const TrajPose* pose = s.traj.nearest(ts + imageTimeOffsetNs(s)); if (!pose) continue; Vector3 origin = toVec3(pose->T * C); + bool hl = (ts == hlTs); + Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; + float sc = hl ? fs * 1.05f : fs; + + if (s.K.model != CameraModel::Pinhole) + { + // A fisheye camera has no frustum the fx/fy/cx/cy pyramid + // describes, so draw position and axes instead -- the usual + // X=red, Y=green, Z=blue. + DrawSphere(origin, fs * (hl ? 0.08f : 0.05f), fc); + const Color axisColors[3] = { RED, GREEN, BLUE }; + for (int k = 0; k < 3; k++) + { + Eigen::Vector3f tip = R_wc.col(k) * (sc * 0.5f) + C; + DrawLine3D(origin, toVec3(pose->T * tip), hl ? fc : axisColors[k]); + } + continue; + } + Vector3 w[4]; for (int k = 0; k < 4; k++) { @@ -1562,10 +1887,6 @@ static void drawScene(AppState& s) w[k] = toVec3(pose->T * pl); } - bool hl = (ts == hlTs); - Color fc = hl ? Color{ 255, 255, 50, 255 } : ORANGE; - float sc = hl ? fs * 1.05f : fs; - if (hl) { // filled quad highlight @@ -1628,9 +1949,13 @@ int main(int argc, char* argv[]) AppState s; // --mjs gives the session manifest; the session directory is its parent. + // Also accepts the session directory itself, for symmetry with drag & drop. std::string sessionDir; if (args.has("mjs")) - sessionDir = fs::path(args.get("mjs")).parent_path().string(); + { + fs::path mjsPath(args.get("mjs")); + sessionDir = fs::is_directory(mjsPath) ? mjsPath.string() : mjsPath.parent_path().string(); + } else if (!args.positional.empty()) sessionDir = args.positional.front(); // back-compat if (!sessionDir.empty()) @@ -1753,17 +2078,9 @@ int main(int argc, char* argv[]) if (IsKeyPressed(KEY_LEFT_CONTROL) || IsKeyPressed(KEY_RIGHT_CONTROL)) s.colorMode = (s.colorMode == 1) ? 0 : 1; - // Chord choices avoid colliding in MEANING with - // multi_view_tls_registration_step_2's shortcuts (Ctrl+L there - // is manual loop closure, Ctrl+E is the lio segments editor; - // bare F there is the "camera Front" preset). Ctrl+O and bare - // C/P are kept aligned with step2 (Ctrl+O = open/load session, - // C = compass/ruler). - // KEY_LEFT/RIGHT_SUPER too: on macOS Cmd (Super) is a distinct - // key from Ctrl, and users -- including whoever asked for this - // binding -- reach for Cmd as "the" modifier there. Treating - // either as ctrlDown matches that expectation instead of - // requiring the literal Ctrl key. + // Chords avoid colliding in meaning with step2's, and keep Ctrl+O + // and bare C/P aligned with it. Super counts as ctrlDown so macOS + // Cmd works, where it is a distinct key from Ctrl. bool ctrlDown = IsKeyDown(KEY_LEFT_CONTROL) || IsKeyDown(KEY_RIGHT_CONTROL) || IsKeyDown(KEY_LEFT_SUPER) || IsKeyDown(KEY_RIGHT_SUPER); bool shiftDown = IsKeyDown(KEY_LEFT_SHIFT) || IsKeyDown(KEY_RIGHT_SHIFT); @@ -1783,18 +2100,10 @@ int main(int argc, char* argv[]) if (!ctrlDown && IsKeyPressed(KEY_C)) s.showCompassRuler = !s.showCompassRuler; - // Camera drag/zoom -- same raylib_widgets::OrbitCamera Euler - // methods multi_view_tls_registration_step_2's motion()/wheel() - // call, driven from continuous per-frame deltas the way - // OrbitCamera::update() (the other, azimuth/elevation half of - // this struct) already reads input, rather than resurrecting - // step2's GLUT-shaped mouse_old_x/y/mouse_buttons bookkeeping - // (nothing about sharing the camera *math* requires reproducing - // that plumbing too). Gated off while Ctrl/Shift is held -- - // both are reserved for the picking actions below, same - // reasoning as step2's own motion() guard (a trackpad's - // click jitter while a modifier is held must never get read as - // a drag, or it breaks any transition that same click started). + // Camera drag/zoom via the same OrbitCamera Euler methods step2 + // uses, driven from per-frame deltas. Gated off while Ctrl/Shift is + // held: those are the picking modifiers, and click jitter under a + // modifier must not read as a drag. if (!imguiWants && !ctrlDown && !shiftDown) { Vector2 d = GetMouseDelta(); @@ -1867,11 +2176,13 @@ int main(int argc, char* argv[]) { s.imgViewIdx = std::max(s.imgViewIdx - 1, 0); s.imgViewRequest.store(s.imgViewIdx); + s.intensityProjNeedsUpdate = true; } if (IsKeyPressed(KEY_RIGHT)) { s.imgViewIdx = std::min(s.imgViewIdx + 1, (int)s.imageTsNs.size()); s.imgViewRequest.store(s.imgViewIdx); + s.intensityProjNeedsUpdate = true; } } @@ -1961,6 +2272,23 @@ int main(int argc, char* argv[]) } } + // ── (re)build the intensity-projection texture on demand ──────────────── + // Rasterization is cheap enough (already-decimated, in-memory + // exportCloud) to do synchronously on toggle/refresh/image-change, + // unlike the photo loader above which reads a file off disk. + if (s.showIntensityProjection && s.intensityProjNeedsUpdate && s.imgViewIdx >= 0 && s.imgViewIdx < (int)s.imageTsNs.size()) + { + s.intensityProjNeedsUpdate = false; + int64_t imgTsAdj = s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s); + cv::Mat proj = renderIntensityProjection(s, imgTsAdj); + cv::cvtColor(proj, proj, cv::COLOR_BGR2RGB); + if (s.intensityProjTexValid) + UnloadTexture(s.intensityProjTex); + Image ri = { proj.data, proj.cols, proj.rows, 1, PIXELFORMAT_UNCOMPRESSED_R8G8B8 }; + s.intensityProjTex = LoadTextureFromImage(ri); + s.intensityProjTexValid = s.intensityProjTex.id > 0; + } + // ── ImGui panel ─────────────────────────────────────────────────────── rlImGuiBegin(); @@ -1975,6 +2303,8 @@ int main(int argc, char* argv[]) ImGui::Separator(); if (ImGui::MenuItem("Open Calibration...", "Ctrl+Shift+C")) actionOpenCalibration(s); + if (ImGui::MenuItem("Open Image Mask...")) + actionOpenMask(s); ImGui::Separator(); if (ImGui::MenuItem("Export Colored Point Cloud (LAS/LAZ)...", "Ctrl+S")) actionExportColoredLAZ(s); @@ -2041,7 +2371,7 @@ int main(int argc, char* argv[]) s.colorMode = 1; if (ImGui::MenuItem("Camera ID", nullptr, s.colorMode == 2)) s.colorMode = 2; - if (ImGui::MenuItem("In ROI", nullptr, s.colorMode == 3)) + if (ImGui::MenuItem("In ROI / mask", nullptr, s.colorMode == 3)) s.colorMode = 3; } ImGui::EndMenu(); @@ -2069,27 +2399,13 @@ int main(int argc, char* argv[]) // double now = ImGui::GetTime(); // ImGui’s built-in timer (in seconds) - // ImGui::Checkbox("dynamic", &dynamicSubsampling); - // if (ImGui::IsItemHovered()) - // ImGui::SetTooltip("automatically control subsampling vs FPS: increase bellow 10, decrease above 60"); - // if (dynamicSubsampling && (fps_avg < 15) && (now - lastAdjustTime > cooldownSeconds)) - //{ - // app_state.viewer_decimate_point_cloud += 1; - // lastAdjustTime = now; - //} - // ImGui::SameLine(); - // ImGui::Text("(avg %.1f)", fps_avg); - if (s.drawDecim < 1) s.drawDecim = 1; ImGui::SameLine(); - // GetFPS()/point-cloud draw-call/vertex count via raylib/ScanRenderer, - // rather than ImGui's own Framerate tracker -- raylib doesn't - // expose a general "draw calls" counter (rlgl's own internal one - // only tracks its immediate-mode batch renderer, not custom - // glDrawArrays calls like ScanRenderer's), so these are scan_renderer's - // own per-frame counts of the calls/points it issued in draw(). + // Counts come from ScanRenderer's own per-frame tally: rlgl's + // internal counter only sees its immediate-mode batch, not the + // custom glDrawArrays calls ScanRenderer issues. ImGui::Text("(%d FPS)", GetFPS()); ImGui::EndMainMenuBar(); @@ -2182,8 +2498,42 @@ int main(int argc, char* argv[]) loadCalib(s); if (s.calibLoaded) { - ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); - ImGui::Text("cx=%.0f cy=%.0f", s.K.cx, s.K.cy); + if (s.K.model == CameraModel::Mei) + { + ImGui::Text("Model: mei (xi=%.4f)", s.K.xi); + ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); + ImGui::Text("cx=%.0f cy=%.0f", s.K.cx, s.K.cy); + } + else if (s.K.model == CameraModel::Fisheye) + { + ImGui::Text("Model: fisheye (equidistant)"); + ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); + ImGui::Text("cx=%.0f cy=%.0f", s.K.cx, s.K.cy); + } + else + { + ImGui::Text("fx=%.0f fy=%.0f", s.K.fx, s.K.fy); + ImGui::Text("cx=%.0f cy=%.0f", s.K.cx, s.K.cy); + } + + ImGui::Separator(); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-140.f); + // Bounds every image the colorizer holds in memory: a whole + // chunk's worth is resident at once when multi-image coloring + // is on, which full-resolution frames make expensive. + ImGui::SliderFloat("Image scale", &s.imgScale, 0.125f, 1.0f, "%.3f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Downscale applied to images before coloring.\nLower = less RAM and faster, at coarser color detail."); + ImGui::InputDouble("Time offset (s)", &s.timeOffsetSec, 0.001, 0.01, "%.4f"); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Camera clock minus trajectory clock: t_traj = t_image + offset.\n" + "Fixes colors smeared along the direction of travel.\n" + "Re-run Colorize to apply."); + ImGui::PopItemWidth(); + ImGui::PushItemWidth(-1); ImGui::Separator(); if (ImGui::Checkbox("Region of interest", &s.roi.enabled)) @@ -2198,7 +2548,10 @@ int main(int argc, char* argv[]) } } if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Only points projecting inside the ROI get colored.\nDrawn on the image preview."); + ImGui::SetTooltip( + "Only points projecting inside the ROI get colored.\n" + "Full-resolution image pixels, scaled along with Image scale.\n" + "Drawn on the image preview."); if (s.roi.enabled) { ImGui::PopItemWidth(); @@ -2210,6 +2563,37 @@ int main(int argc, char* argv[]) ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); } + + ImGui::Separator(); + ImGui::BeginDisabled(s.mask.empty()); + ImGui::Checkbox("Image mask", &s.maskEnabled); + ImGui::EndDisabled(); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip( + "Points projecting onto a masked-out (black) pixel stay uncolored.\n" + "Free-form counterpart of the ROI -- for the operator, the rig itself,\n" + "the sky. Coloring only: exported images are never masked.\n" + "Re-run Load cloud to apply."); + ImGui::Text("Mask image:"); + ImGui::InputText("##mask", s.maskBuf, sizeof(s.maskBuf)); + if (ImGui::Button("Load mask", ImVec2(-1, 0))) + loadMask(s); + if (!s.mask.empty()) + { + ImGui::TextDisabled("%dx%d, %.1f%% masked out", s.mask.cols, s.mask.rows, s.maskRejectFrac * 100.f); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Resampled to the image size in use; any resolution with the same framing works."); + if (ImGui::Checkbox("Invert mask", &s.maskInvert)) + { + // The mask is strictly 0/255, so flipping it in place is + // exact and its own inverse -- no need to re-read the file. + cv::bitwise_not(s.mask, s.mask); + refreshMaskDerived(s); + } + ImGui::Checkbox("Show mask on preview", &s.showMaskOverlay); + if (ImGui::Button("Clear mask", ImVec2(-1, 0))) + clearMask(s); + } } ImGui::PopItemWidth(); } @@ -2232,8 +2616,14 @@ int main(int argc, char* argv[]) { s.imgViewIdx = std::clamp(s.imgViewIdx, 0, nImgs - 1); s.imgViewRequest.store(s.imgViewIdx); + s.intensityProjNeedsUpdate = true; } ImGui::TextDisabled("ts: %lld", (long long)s.imageTsNs[s.imgViewIdx]); + if (s.timeOffsetSec != 0.0) + { + ImGui::SameLine(); + ImGui::TextDisabled("(adj: %lld)", (long long)(s.imageTsNs[s.imgViewIdx] + imageTimeOffsetNs(s))); + } { float as = angularSpeedDegAt(s.traj, s.poseAngSpeedDeg, s.imageTsNs[s.imgViewIdx]); bool fast = s.filterFastImages && s.maxImageAngSpeedDeg > 0.f && as > s.maxImageAngSpeedDeg; @@ -2250,6 +2640,37 @@ int main(int argc, char* argv[]) ImGui::TextColored(ImVec4(1, 1, 0, 1), "Loading..."); else if (s.imgViewTexValid) ImGui::TextColored(ImVec4(0, 1, 0, 1), "%dx%d", s.imgViewTex.width, s.imgViewTex.height); + + ImGui::Separator(); + if (ImGui::Checkbox("Show intensity projection", &s.showIntensityProjection)) + { + if (s.showIntensityProjection) + s.intensityProjNeedsUpdate = true; + } + if (ImGui::IsItemHovered()) + ImGui::SetTooltip( + "Draws a synthetic intensity image next to the photo, by\n" + "reprojecting the colorized cloud through the current\n" + "calibration -- a reference to check it against the photo.\n" + "Requires 'Load cloud' to have run first."); + if (s.showIntensityProjection) + { + ImGui::PushItemWidth(-140.f); + if (ImGui::InputInt("Point decimation##proj", &s.intensityProjDecim)) + s.intensityProjDecim = std::max(1, s.intensityProjDecim); + if (ImGui::InputFloat("Point radius (px)##proj", &s.intensityProjPointRadius, 0.5f, 1.f, "%.1f")) + s.intensityProjPointRadius = std::max(1.f, s.intensityProjPointRadius); + ImGui::Checkbox("Overlay on photo", &s.intensityProjOverlay); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("ON: alpha-blended on top of the photo\nOFF: shown side-by-side with it"); + if (s.intensityProjOverlay) + ImGui::SliderFloat("Overlay alpha", &s.intensityProjAlpha, 0.f, 1.f, "%.2f"); + ImGui::PopItemWidth(); + if (ImGui::Button("Refresh projection", ImVec2(-1, 0))) + s.intensityProjNeedsUpdate = true; + if (s.exportCloud.empty()) + ImGui::TextColored(ImVec4(1, 0.6f, 0, 1), "No colorized cloud yet -- run 'Load cloud'."); + } } } @@ -2258,6 +2679,9 @@ int main(int argc, char* argv[]) ImGui::PushItemWidth(-1); ImGui::Text("Output file:"); ImGui::InputText("##out", s.exportBuf, sizeof(s.exportBuf)); + ImGui::Checkbox("LAZ: only points with valid color", &s.exportOnlyValidColor); + if (ImGui::IsItemHovered()) + ImGui::SetTooltip("Skip points that got no RGB from an image (no image / out of frustum / outside ROI or mask)"); if (ImGui::Button("Export colored LAZ", ImVec2(-1, 0))) actionExportColoredLAZ(s); if (ImGui::Button("Export colored E57", ImVec2(-1, 0))) @@ -2294,10 +2718,11 @@ int main(int argc, char* argv[]) ImGui::Indent(); ImGui::Checkbox("Compressed (jpeg)", &s.ros.compressCamera); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("ON: CompressedImage (jpeg)\nOFF: raw Image bgr8"); - ImGui::Checkbox("Undistort (rectify)", &s.ros.undistortCamera); + ImGui::SetTooltip("ON: CompressedImage, the source jpeg copied verbatim\nOFF: raw Image bgr8"); + ImGui::TextDisabled("Frames are exported as captured."); if (ImGui::IsItemHovered()) - ImGui::SetTooltip("Rectify to pinhole so RViz overlays line up\n(CameraInfo published with zero distortion)."); + ImGui::SetTooltip( + "Images are never rectified. CameraInfo carries the real\ndistortion, so consumers can undistort from it."); ImGui::Unindent(); } ImGui::Checkbox("LiDAR undistorted (map frame)", &s.ros.exportLidarUndistorted); @@ -2346,8 +2771,13 @@ int main(int argc, char* argv[]) ImGui::PopItemWidth(); ImGui::PushItemWidth(-1); s.colmapPtDecim = std::max(1, s.colmapPtDecim); + const bool colmapUnsupported = s.K.model == CameraModel::Mei; + ImGui::BeginDisabled(colmapUnsupported); if (ImGui::Button("Export COLMAP model", ImVec2(-1, 0))) exportColmap(s); + ImGui::EndDisabled(); + if (colmapUnsupported && ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled)) + ImGui::SetTooltip("COLMAP has no unified-sphere (Mei) camera model."); ImGui::TextDisabled("Writes sparse/{cameras,images,points3D}.txt"); if (ImGui::IsItemHovered()) ImGui::SetTooltip( @@ -2385,9 +2815,13 @@ int main(int argc, char* argv[]) ImGui::SetNextWindowSize(ImVec2(640, 480), ImGuiCond_Once); ImGui::Begin("Image##viewer", nullptr, ImGuiWindowFlags_NoScrollbar); ImVec2 avail = ImGui::GetContentRegionAvail(); + const bool showProj = s.showIntensityProjection && s.intensityProjTexValid; + const bool overlayMode = showProj && s.intensityProjOverlay; + const float colW = (showProj && !overlayMode) ? (avail.x - 4.f) * 0.5f : avail.x; + float aspect = (float)s.imgViewTex.height / (float)s.imgViewTex.width; - int dispW = (int)avail.x; - int dispH = (int)(avail.x * aspect); + int dispW = (int)colW; + int dispH = (int)(colW * aspect); if (dispH > (int)avail.y) { dispH = (int)avail.y; @@ -2395,6 +2829,32 @@ int main(int argc, char* argv[]) } ImVec2 imgPos = ImGui::GetCursorScreenPos(); rlImGuiImageSize(&s.imgViewTex, dispW, dispH); + + if (overlayMode) + { + // Redraw the projection texture at the same screen rect, tinted + // with a reduced alpha -- ImGui's renderer alpha-blends draw + // commands, so this composites over the photo just drawn above. + ImGui::SetCursorScreenPos(imgPos); + ImVec4 tint(1.f, 1.f, 1.f, std::clamp(s.intensityProjAlpha, 0.f, 1.f)); + ImGui::ImageWithBg( + ImTextureID(s.intensityProjTex.id), + ImVec2((float)dispW, (float)dispH), + ImVec2(0.f, 0.f), + ImVec2(1.f, 1.f), + ImVec4(0.f, 0.f, 0.f, 0.f), + tint); + } + + // masked-out pixels, tinted red over the same rect as the photo (the + // mask is resampled wherever it is used, so a mask of a different + // resolution is expected and stretches to fit here too) + if (s.maskEnabled && s.showMaskOverlay && s.maskTexValid) + { + ImGui::SetCursorScreenPos(imgPos); + rlImGuiImageSize(&s.maskTex, dispW, dispH); + } + // overlay the ROI, mapping full-res image pixels to the displayed rect if (s.roi.enabled && s.imgViewTex.width > 0 && s.imgViewTex.height > 0) { @@ -2409,6 +2869,20 @@ int main(int argc, char* argv[]) /*rounding=*/0.f, /*thickness=*/2.f); } + + if (showProj && !overlayMode) + { + ImGui::SameLine(); + float pAspect = (float)s.intensityProjTex.height / (float)s.intensityProjTex.width; + int pDispW = (int)colW; + int pDispH = (int)(colW * pAspect); + if (pDispH > (int)avail.y) + { + pDispH = (int)avail.y; + pDispW = (int)(avail.y / pAspect); + } + rlImGuiImageSize(&s.intensityProjTex, pDispW, pDispH); + } ImGui::End(); } @@ -2422,6 +2896,10 @@ int main(int argc, char* argv[]) s.rosThread.join(); if (s.imgViewTexValid) UnloadTexture(s.imgViewTex); + if (s.intensityProjTexValid) + UnloadTexture(s.intensityProjTex); + if (s.maskTexValid) + UnloadTexture(s.maskTex); s.cloud.unload(); if (s.shaderOk) diff --git a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h index bd24a1c8..a8907f43 100644 --- a/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h +++ b/apps/camera_lidar_trajectory_viewer/TrajectoryViewerShaders.h @@ -9,14 +9,14 @@ namespace trajectory_viewer_shaders { - // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI + // colorPacked: float bits = 0x00RRGGBB; colorMode: 0=jet depth, 1=RGB, 2=camera id, 3=in ROI/mask inline constexpr const char* kVS = R"( #version 330 layout(location = 0) in vec3 pos; layout(location = 1) in float colorPacked; layout(location = 2) in float lidarIntensity; layout(location = 3) in float colorCameraId; // global image index that colored this point, or -1 -layout(location = 4) in float inRoi; // 1=inside ROI, 0=outside ROI, -1=projects into no image +layout(location = 4) in float inRoi; // 1=kept by ROI+mask, 0=rejected by either, -1=projects into no image uniform mat4 mvp; uniform float pointSize; uniform int drawDecim; @@ -86,8 +86,9 @@ void main() { } else if (colorMode == 3) { - // ROI membership: green = inside ROI, red = projects into an image but - // outside ROI, dim gray = projects into no image (spatial context). + // ROI/mask membership: green = a pixel the ROI and the image mask both + // keep, red = projects into an image but is rejected by one of them, + // dim gray = projects into no image (spatial context). if (fragInRoi < 0.0) finalColor = vec4(0.28, 0.28, 0.28, 1.0); else diff --git a/apps/console_tools/CMakeLists.txt b/apps/console_tools/CMakeLists.txt index 860920f2..c6fe6c21 100644 --- a/apps/console_tools/CMakeLists.txt +++ b/apps/console_tools/CMakeLists.txt @@ -119,6 +119,54 @@ if (MSVC) target_compile_options(laz_to_mcap PRIVATE /bigobj) endif() +# session_to_mcap: a processed lidar_odometry_step_1 session (session.json) +# -> MCAP exporter (undistorted lidar in the map frame + /tf map->lidar, +# optionally /imu re-read from the original recording via --raw-dir). Unlike +# laz_to_mcap it touches Core::Session, whose layout branches on WITH_GUI (see +# core/CMakeLists.txt's add_core_target) -- so this links core_no_gui, not +# ${CORE_LIBRARIES} (= core, the WITH_GUI=1 build laz_to_mcap gets away with +# only because it never includes Core/session.h). Mirrors the include/link set +# apps/lidar_odometry_step_1/tests uses to combine lidar_odometry_utils.cpp +# with core_no_gui in one binary. +add_executable( + session_to_mcap session_to_mcap.cpp + ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.h + ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1/lidar_odometry_utils.cpp + ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.h ${REPOSITORY_DIRECTORY}/rosbags/McapWriter.cpp + ) + +target_include_directories( + session_to_mcap + PRIVATE ${REPOSITORY_DIRECTORY}/apps/lidar_odometry_step_1 + ${REPOSITORY_DIRECTORY}/core/include + ${REPOSITORY_DIRECTORY}/rosbags + ${THIRDPARTY_DIRECTORY} # csv.hpp (used by load_imu) + ${THIRDPARTY_DIRECTORY}/glm + ${EIGEN3_INCLUDE_DIR} + ${THIRDPARTY_DIRECTORY}/tomlplusplus/include + ${THIRDPARTY_DIRECTORY}/json/include + ${LASZIP_INCLUDE_DIR}/LASzip/include + ${THIRDPARTY_DIRECTORY}/observation_equations/codes + ${THIRDPARTY_DIRECTORY}/vqf/vqf/cpp + ${THIRDPARTY_DIRECTORY}/Fusion/Fusion) + +target_link_libraries( + session_to_mcap + PRIVATE + mcap + core_no_gui + vqf + Fusion + unordered_dense::unordered_dense + spdlog::spdlog + UTL::include + ${PLATFORM_LASZIP_LIB} + ${PLATFORM_MISCELLANEOUS_LIBS}) + +if (MSVC) + target_compile_options(session_to_mcap PRIVATE /bigobj) +endif() + # These are built whenever BUILD_WITH_CLI_TOOLS is ON (the default), so they # ship in the DEB package alongside the GUI apps. hdmapping_install_app( @@ -130,4 +178,5 @@ hdmapping_install_app( pcd_to_laz laz_to_txt laz_to_mcap - mcap_to_laz) + mcap_to_laz + session_to_mcap) diff --git a/apps/console_tools/session_to_mcap.cpp b/apps/console_tools/session_to_mcap.cpp new file mode 100644 index 00000000..1b8aa96f --- /dev/null +++ b/apps/console_tools/session_to_mcap.cpp @@ -0,0 +1,526 @@ +// Processed lidar_odometry_step_1 session (session.json) -> MCAP exporter. +// +// Unlike laz_to_mcap (which exports a *raw* mandeye recording, points still +// in each scan's own moving sensor frame), a session's point clouds are +// already motion-compensated: PointCloud::points_local is undistorted and +// expressed relative to that chunk's own first pose, and PointCloud::m_pose +// places the chunk in the map frame -- see core/include/Core/export_laz.h's +// save_all_to_las() for the same "m_pose * points_local[i]" composition. +// This tool writes those already-registered points straight into the map +// frame (matching apps/camera_lidar_trajectory_viewer/RosExport.h's +// "exportLidarUndistorted" convention: frame_id = map, no further motion +// compensation needed), plus a /tf stream of map -> lidar samples taken from +// each chunk's local_trajectory (falling back to one static-ish sample per +// chunk for older sessions saved without a trajectory_lio_*.csv). +// +// A session keeps no raw IMU samples (WorkerData::raw_imu_data only exists +// during the live lidar_odometry_step_1 run and isn't serialized), so /imu +// is optional and, if wanted, is re-read from the *original* mandeye +// recording directory via load_imu() -- the same function laz_to_mcap uses. +#include "McapWriter.h" +#include "lidar_odometry_utils.h" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +namespace fs = std::filesystem; + +namespace +{ + + bool check_path_ext(const std::string& path, const char* ext) + { + return fs::path(path).extension() == ext; + } + + std::string to_lower(std::string s) + { + std::transform( + s.begin(), + s.end(), + s.begin(), + [](unsigned char c) + { + return std::tolower(c); + }); + return s; + } + + // PointCloud::timestamps / LocalTrajectoryNode::timestamps.first are stored + // in NANOSECONDS: lidar_odometry.cpp writes both the scan_lio_*.laz gps_time + // field and the trajectory_lio_*.csv "timestamp_nanoseconds" column as + // `seconds * 1e9`, and both are read back verbatim (no /1e9). McapPoint / + // McapImuSample / McapTransform all expect absolute seconds, so every + // session-sourced timestamp is converted here, once. + constexpr double kNanosecondsToSeconds = 1e-9; + + // Session point clouds carry no per-point ring/laser_id, so PointCloud2's + // Generic layout (the only one that needs them) still round-trips fine -- + // both fields just come out zero. + std::vector to_mcap_points(const PointCloud& pc) + { + std::vector out; + out.reserve(pc.points_local.size()); + for (size_t i = 0; i < pc.points_local.size(); ++i) + { + const double ts_ns = (i < pc.timestamps.size()) ? pc.timestamps[i] : 0.0; + if (ts_ns == 0.0) // sentinel for "no timestamp", same convention as save_all_to_las's skip_ts_0 + continue; + + const Eigen::Vector3d world = pc.m_pose * pc.points_local[i]; + + rosbags::McapPoint mp{}; + mp.x = static_cast(world.x()); + mp.y = static_cast(world.y()); + mp.z = static_cast(world.z()); + mp.intensity = (i < pc.intensities.size()) ? static_cast(pc.intensities[i]) : 0.0f; + mp.timestamp = ts_ns * kNanosecondsToSeconds; + out.push_back(mp); + } + return out; + } + + void sort_points_by_timestamp(std::vector& points) + { + std::sort( + points.begin(), + points.end(), + [](const auto& a, const auto& b) + { + return a.timestamp < b.timestamp; + }); + } + + rosbags::McapTransform to_mcap_transform(double timestamp_s, const Eigen::Affine3d& T) + { + rosbags::McapTransform t{}; + t.timestamp = timestamp_s; + t.tx = T.translation().x(); + t.ty = T.translation().y(); + t.tz = T.translation().z(); + Eigen::Quaterniond q(T.linear()); + q.normalize(); + t.qx = q.x(); + t.qy = q.y(); + t.qz = q.z(); + t.qw = q.w(); + return t; + } + + // T_map_lidar per node = pc.m_pose * node.m_pose: local_trajectory poses are + // stored relative to the chunk's own first pose (lidar_odometry.cpp writes + // `intermediate_trajectory[0].inverse() * intermediate_trajectory[j]`), the + // same convention points_local uses -- see the file header comment. + std::vector to_mcap_transforms(const PointCloud& pc) + { + std::vector out; + if (!pc.local_trajectory.empty()) + { + out.reserve(pc.local_trajectory.size()); + for (const auto& node : pc.local_trajectory) + out.push_back(to_mcap_transform(node.timestamps.first * kNanosecondsToSeconds, pc.m_pose * node.m_pose)); + return out; + } + + // Older session without a trajectory_lio_*.csv: one sample for the whole + // chunk, stamped at its first valid (non-sentinel) point timestamp. + double ts_ns = 0.0; + for (double t : pc.timestamps) + { + if (t != 0.0) + { + ts_ns = t; + break; + } + } + out.push_back(to_mcap_transform(ts_ns * kNanosecondsToSeconds, pc.m_pose)); + return out; + } + + // Cuts a session chunk's points into one PointCloud2 message per 1/msg_hz + // seconds, so the bag replays at a lidar-like rate instead of one huge + // message per chunk. Mirrors laz_to_mcap.cpp's MessageSplitter exactly + // (absolute time-bin grid, pending points carry over a chunk boundary); + // duplicated rather than shared since that one buffers Point3Di and this one + // already-converted McapPoint. + class MessageSplitter + { + public: + MessageSplitter(rosbags::McapFileWriter& writer, double msg_hz) + : writer_(writer) + , msg_hz_(msg_hz) + { + } + + // `points` must be sorted by timestamp, and successive calls must be in + // timestamp order too (session chunks are processed in container order). + void add(const std::vector& points) + { + if (msg_hz_ <= 0.0) + { + write(points); + return; + } + for (const auto& p : points) + { + const int64_t bin = static_cast(std::floor(p.timestamp * msg_hz_)); + if (!pending_.empty() && bin != current_bin_) + flush(); + current_bin_ = bin; + pending_.push_back(p); + } + } + + void flush() + { + write(pending_); + pending_.clear(); + } + + size_t messages_written() const + { + return messages_; + } + + private: + void write(const std::vector& points) + { + if (points.empty()) + return; + const uint64_t stamp_ns = static_cast(points.front().timestamp * 1e9); + writer_.writePointCloud(stamp_ns, points); + ++messages_; + } + + rosbags::McapFileWriter& writer_; + double msg_hz_; + std::vector pending_; + int64_t current_bin_ = 0; + size_t messages_ = 0; + }; + + // load_imu() reads a single sensor's stream out of an imuNNNN.csv (see its doc + // comment in lidar_odometry_utils.h): on a rig with more than one IMU, rows + // carry an optional "imuId" column and only rows matching this id are kept. + // session_to_mcap has no per-sensor calibration to resolve which id is "the" + // IMU (a session keeps no raw IMU/calibration provenance at all), so it + // always reads id 0 and instead warns when a file actually contains more + // than one id -- see distinct_imu_ids() below. + constexpr int kImuIdToUse = 0; + + // Splits a line the same way load_imu()'s CSVFormat does (space/comma/tab + // delimited), just for peeking at the header/imuId column below. + std::vector split_csv_line(const std::string& line) + { + std::vector out; + std::string cur; + for (char c : line) + { + if (c == ' ' || c == ',' || c == '\t') + { + if (!cur.empty()) + { + out.push_back(cur); + cur.clear(); + } + } + else + cur.push_back(c); + } + if (!cur.empty()) + out.push_back(cur); + return out; + } + + // Returns every distinct "imuId" value in a modern-format (named-column) + // IMU csv, purely to warn when a file mixes more than one IMU. Empty for a + // file with no imuId column (a single-IMU recording -- id 0 covers it, no + // warning needed) or for the legacy headerless format load_imu() also + // accepts (not inspected here; load_imu() itself still reads it correctly). + std::set distinct_imu_ids(const std::string& csv_path) + { + std::set ids; + std::ifstream file(csv_path); + std::string header_line; + if (!file.is_open() || !std::getline(file, header_line)) + return ids; + + const auto header = split_csv_line(header_line); + const auto it = std::find(header.begin(), header.end(), "imuId"); + if (it == header.end()) + return ids; + const auto imu_id_index = static_cast(std::distance(header.begin(), it)); + + std::string line; + while (std::getline(file, line)) + { + const auto row = split_csv_line(line); + if (imu_id_index >= row.size()) + continue; + try + { + ids.insert(std::stoi(row[imu_id_index])); + } catch (const std::exception&) + { + } + } + return ids; + } + + // Reads every imu*.csv in raw_dir (mandeye's imuNNNN.csv chunk convention) + // and merges them into one timestamp-sorted stream, always using IMU id 0 + // (warning first if any file actually carries more than one IMU id). + std::vector load_all_imu(const fs::path& raw_dir) + { + std::vector csvs; + for (const auto& entry : fs::directory_iterator(raw_dir)) + { + if (!entry.is_regular_file()) + continue; + if (to_lower(entry.path().extension().string()) != ".csv") + continue; + if (!to_lower(entry.path().stem().string()).starts_with("imu")) + continue; + csvs.push_back(entry.path().string()); + } + std::sort(csvs.begin(), csvs.end()); + + std::set all_ids; + for (const auto& csv : csvs) + { + const auto ids = distinct_imu_ids(csv); + all_ids.insert(ids.begin(), ids.end()); + } + if (all_ids.size() > 1) + { + std::string ids_str; + for (int id : all_ids) + ids_str += (ids_str.empty() ? "" : ", ") + std::to_string(id); + spdlog::warn( + "{} carries more than one IMU (ids: {}) - session_to_mcap always reads id {}", raw_dir.string(), ids_str, kImuIdToUse); + } + + std::vector out; + for (const auto& csv : csvs) + { + const auto imu_data = load_imu(csv, kImuIdToUse); + for (const auto& [ts, gyr, acc] : imu_data) + { + rosbags::McapImuSample s{}; + s.timestamp = ts.first; + s.gyro_x = gyr.x(); + s.gyro_y = gyr.y(); + s.gyro_z = gyr.z(); + s.acc_x = acc.x(); + s.acc_y = acc.y(); + s.acc_z = acc.z(); + out.push_back(s); + } + } + std::sort( + out.begin(), + out.end(), + [](const auto& a, const auto& b) + { + return a.timestamp < b.timestamp; + }); + return out; + } + + void print_usage(const char* argv0) + { + spdlog::error("Usage: {} [options]", argv0); + spdlog::error(" session.json a lidar_odometry_step_1 session; its point clouds are already"); + spdlog::error(" undistorted and are written straight into the map frame, plus a"); + spdlog::error(" /tf stream of map->lidar samples taken from each chunk's trajectory"); + spdlog::error("Options:"); + spdlog::error(" --raw-dir original mandeye recording directory (imuNNNN.csv files);"); + spdlog::error(" a session keeps no raw IMU samples, so this is the only way"); + spdlog::error(" to include /imu. Omitted: lidar + tf only, no /imu channel is written."); + spdlog::error(" Always reads IMU id 0; warns if a file carries more than one IMU id."); + spdlog::error(" --lidar-topic lidar PointCloud2 topic (default: /lidar_points)"); + spdlog::error(" --imu-topic IMU topic (default: /imu)"); + spdlog::error(" --tf-topic tf topic (default: /tf)"); + spdlog::error(" --map-frame tf parent frame / PointCloud2 frame_id (default: map)"); + spdlog::error(" --lidar-frame tf child frame / Imu frame_id (default: lidar)"); + spdlog::error(" --lidar-type PointCloud2 field layout: generic|velodyne|ouster|hesai (default: generic)"); + spdlog::error(" --msg_hz message rate: points are split into one PointCloud2 per"); + spdlog::error(" 1/hz seconds (default: 10; 0 = one message per session chunk)"); + } + +} // namespace + +int main(const int argc, const char** argv) +{ + if (argc < 3) + { + print_usage(argv[0]); + return EXIT_FAILURE; + } + + const std::string session_path = argv[1]; + const std::string mcap_path = argv[2]; + std::string raw_dir; + rosbags::McapWriterOptions options; + options.frame_id = "lidar"; + options.pointcloud_frame_id = "map"; + options.map_frame = "map"; + double msg_hz = 10.0; + + for (int i = 3; i < argc; ++i) + { + const std::string arg = argv[i]; + const bool hasValue = i + 1 < argc; + + if (arg == "--raw-dir" && hasValue) + raw_dir = argv[++i]; + else if (arg == "--lidar-topic" && hasValue) + options.lidar_topic = argv[++i]; + else if (arg == "--imu-topic" && hasValue) + options.imu_topic = argv[++i]; + else if (arg == "--tf-topic" && hasValue) + options.tf_topic = argv[++i]; + else if (arg == "--map-frame" && hasValue) + { + const std::string value = argv[++i]; + options.pointcloud_frame_id = value; + options.map_frame = value; + } + else if (arg == "--lidar-frame" && hasValue) + options.frame_id = argv[++i]; + else if (arg == "--msg_hz" && hasValue) + { + const std::string value = argv[++i]; + try + { + msg_hz = std::stod(value); + } catch (const std::exception&) + { + spdlog::error("Invalid --msg_hz '{}' (expected a number)", value); + return EXIT_FAILURE; + } + if (!std::isfinite(msg_hz) || msg_hz < 0.0) + { + spdlog::error("Invalid --msg_hz '{}' (expected >= 0; 0 = one message per session chunk)", value); + return EXIT_FAILURE; + } + } + else if (arg == "--lidar-type" && hasValue) + { + const std::string type = argv[++i]; + if (type == "generic") + options.lidar_layout = rosbags::PointCloudLayout::Generic; + else if (type == "velodyne") + options.lidar_layout = rosbags::PointCloudLayout::Velodyne; + else if (type == "ouster") + options.lidar_layout = rosbags::PointCloudLayout::Ouster; + else if (type == "hesai") + options.lidar_layout = rosbags::PointCloudLayout::Hesai; + else + { + spdlog::error("Unknown --lidar-type '{}' (expected generic|velodyne|ouster|hesai)", type); + return EXIT_FAILURE; + } + } + else + { + spdlog::error("Unrecognized argument '{}'", arg); + print_usage(argv[0]); + return EXIT_FAILURE; + } + } + + if (!check_path_ext(mcap_path, ".mcap")) + { + spdlog::error("Invalid extension for output file {} - expected .mcap", mcap_path); + return EXIT_FAILURE; + } + if (!fs::exists(session_path)) + { + spdlog::error("Session file {} does not exist", session_path); + return EXIT_FAILURE; + } + Session session; + if (!session.load(session_path, /*is_decimate=*/false, 0, 0, 0, /*calculate_offset=*/false)) + { + spdlog::error("Failed to load session '{}'", session_path); + return EXIT_FAILURE; + } + + rosbags::McapFileWriter writer(mcap_path, options); + if (!writer.isOpen()) + { + spdlog::error("Failed to open output mcap file {}", mcap_path); + return EXIT_FAILURE; + } + + const auto& clouds = session.point_clouds_container.point_clouds; + spdlog::info("Loaded session with {} chunk(s) from {}", clouds.size(), session_path); + + size_t total_points = 0; + std::vector all_tf; + MessageSplitter splitter(writer, msg_hz); + for (size_t idx = 0; idx < clouds.size(); ++idx) + { + const auto& pc = clouds[idx]; + if (!pc.visible) + { + spdlog::info("[{}/{}] {}: skipped (not visible)", idx + 1, clouds.size(), pc.file_name); + continue; + } + + auto points = to_mcap_points(pc); + sort_points_by_timestamp(points); + total_points += points.size(); + splitter.add(points); + spdlog::info("[{}/{}] {}: {} points", idx + 1, clouds.size(), pc.file_name, points.size()); + + const auto tf = to_mcap_transforms(pc); + all_tf.insert(all_tf.end(), tf.begin(), tf.end()); + } + splitter.flush(); + spdlog::info( + "Loaded {} points across {} chunk(s), wrote {} point cloud message(s)", total_points, clouds.size(), splitter.messages_written()); + + std::sort( + all_tf.begin(), + all_tf.end(), + [](const auto& a, const auto& b) + { + return a.timestamp < b.timestamp; + }); + writer.writeTf(all_tf); + spdlog::info("Wrote {} tf sample(s)", all_tf.size()); + + if (!raw_dir.empty()) + { + if (!fs::exists(raw_dir) || !fs::is_directory(raw_dir)) + { + spdlog::error("--raw-dir {} does not exist or is not a directory - no /imu written", raw_dir); + } + else + { + const auto imu = load_all_imu(raw_dir); + if (!imu.empty()) + { + writer.writeImu(imu); + spdlog::info("Loaded {} IMU sample(s) from {}", imu.size(), raw_dir); + } + else + { + spdlog::warn("No imu*.csv samples found in {} - no /imu written", raw_dir); + } + } + } + + spdlog::info("Wrote {}", mcap_path); + return EXIT_SUCCESS; +} diff --git a/apps/manual_color/CMakeLists.txt b/apps/manual_color/CMakeLists.txt deleted file mode 100644 index 0b946882..00000000 --- a/apps/manual_color/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -cmake_minimum_required(VERSION 4.0.0) - -project(mandeye_with_360_camera_manual_coloring) - -add_executable(mandeye_with_360_camera_manual_coloring manual_color.cpp) - -target_include_directories( - mandeye_with_360_camera_manual_coloring - PRIVATE include - ${REPOSITORY_DIRECTORY}/core/include - ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${THIRDPARTY_DIRECTORY} - ${THIRDPARTY_DIRECTORY}/glm - ${EIGEN3_INCLUDE_DIR} - ${THIRDPARTY_DIRECTORY}/imgui - ${THIRDPARTY_DIRECTORY}/imgui/backends - ${THIRDPARTY_DIRECTORY}/ImGuizmo - ${THIRDPARTY_DIRECTORY}/json/include - ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master - ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/glew-cmake/include - ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${FREEGLUT_INCLUDE_DIR}) - -target_link_libraries( - mandeye_with_360_camera_manual_coloring - PRIVATE ${FREEGLUT_LIBRARY} - ${OPENGL_gl_LIBRARY} - OpenGL::GLU - ${PLATFORM_LASZIP_LIB} - ${PLATFORM_MISCELLANEOUS_LIBS} - ${CORE_LIBRARIES} - ${GUI_LIBRARIES}) - -if(WIN32) - add_custom_command( - TARGET mandeye_with_360_camera_manual_coloring - POST_BUILD - COMMAND - ${CMAKE_COMMAND} -E copy - $ - $ - COMMAND_EXPAND_LISTS) -endif() - -if (MSVC) - target_compile_options(mandeye_with_360_camera_manual_coloring PRIVATE /bigobj) -endif() - -hdmapping_install_app(mandeye_with_360_camera_manual_coloring) \ No newline at end of file diff --git a/apps/manual_color/manual_color.cpp b/apps/manual_color/manual_color.cpp deleted file mode 100644 index 2c187642..00000000 --- a/apps/manual_color/manual_color.cpp +++ /dev/null @@ -1,1474 +0,0 @@ -#include -#include - -#include "imgui.h" -#include "imgui_impl_glut.h" -#include "imgui_impl_opengl2.h" - -#define STB_IMAGE_IMPLEMENTATION -#include "stb_image.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include - -double fx = 2141.3412300023847; -double fy = 2141.3412300023847; -double cx = 1982.4503600047012; -double cy = 1472.7228631802407; -double k1 = -0.00042559601894193817; -double k2 = 0.003534402929232146; -double k3 = -0.0022518302398800826; -double k4 = 0.0001842010188374431; -double alpha = 0; - -bool color = false; - -GLuint tex1; - -GLUquadric* sphere; - -GLuint make_tex(const std::string& fn) -{ - GLuint tex; - glGenTextures(1, &tex); - glBindTexture(GL_TEXTURE_2D, tex); - // set the texture wrapping/filtering options (on the currently bound texture object) - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); - // glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - // load and generate the texture - int width, height, nrChannels; - unsigned char* data = stbi_load(fn.c_str(), &width, &height, &nrChannels, 0); - - std::cout << "width: " << width << " height: " << height << " nrChannels: " << nrChannels << std::endl; - - if (data) - { - if (nrChannels == 1) - { - unsigned char* data3 = (unsigned char*)malloc(width * height * 3); - - int counter = 0; - for (int wh = 0; wh < width * height; wh++) - { - data3[counter++] = data[wh]; - data3[counter++] = data[wh]; - data3[counter++] = data[wh]; - } - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, data3); - stbi_image_free(data3); - } - else if (nrChannels == 4) - { - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width, height, 0, GL_RGBA, GL_UNSIGNED_BYTE, data); - } - else - { - glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, data); - } - - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); - // glGenerateMipmap(GL_TEXTURE_2D); - } - stbi_image_free(data); - return tex; -} - -float rot = 0; -float width = 34; -float height = 71; -const uint32_t window_width = 500; -const uint32_t window_height = 400; -int mouse_old_x, mouse_old_y; -int mouse_buttons = 0; -float rotate_x = 0.0, rotate_y = 0.0; -float translate_z = -90.0; -float translate_x, translate_y = 0.0; -bool gui_mouse_down{ false }; - -void display(); -void reshape(int w, int h); -void mouse(int glut_button, int state, int x, int y); -void motion(int x, int y); -bool initGL(int* argc, char** argv); - -float imgui_co_size{ 1000.0f }; -bool imgui_draw_co{ true }; - -double CameraRotationZ = 0; -double CameraHeight = 0; - -namespace SystemData -{ - std::vector points; - std::pair clickedRay; - int closestPointIndex{ -1 }; - std::vector pointPickedImage; - std::vector pointPickedPointCloud; - - unsigned char* imageData; - int imageWidth, imageHeight, imageNrChannels; - - Eigen::Affine3d camera_pose = Eigen::Affine3d::Identity(); - - int point_size = 1; -} // namespace SystemData - -int main(int argc, char* argv[]) -{ - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - // pose.om = M_PI * 0.5; - // pose.fi = 0; - // pose.ka = M_PI * 0.5; - // pose.px = 0; - // pose.py = 0; - // pose.pz = 0; //-0.25; - pose.om = M_PI * 0.5; - pose.fi = 0; - pose.ka = M_PI * 0.5; - pose.px = 0.055; - pose.py = 0.13; - pose.pz = 0.085; - - SystemData::camera_pose = affine_matrix_from_pose_tait_bryan(pose); - - initGL(&argc, argv); - glutDisplayFunc(display); - glutMouseFunc(mouse); - glutMotionFunc(motion); - glutMainLoop(); -} - -void imagePicker( - const std::string& name, ImTextureID tex1, std::vector& point_picked, const std::vector& point_pickedInPointcloud) -{ - ImGuiIO& io = ImGui::GetIO(); - static float zoom = 0.1f; - const int Tex_width = 5000; - const int Tex_height = 2500; - - float speed = io.KeyShift ? 10.f : 1.f; - int transX = 0; - int transY = 0; - - if (ImGui::IsKeyDown(ImGuiKey_UpArrow)) - transY = -10 * speed; - - if (ImGui::IsKeyDown(ImGuiKey_DownArrow)) - transY = 10 * speed; - - if (ImGui::IsKeyDown(ImGuiKey_LeftArrow)) - transX = -10 * speed; - - if (ImGui::IsKeyDown(ImGuiKey_RightArrow)) - transX = 10 * speed; - - if (ImGui::IsKeyDown(ImGuiKey_PageUp)) - zoom *= 1.0f + 0.01f * speed; - - if (ImGui::IsKeyDown(ImGuiKey_PageDown)) - zoom /= 1.00f + 0.01f * speed; - - ImVec2 uv_min = ImVec2(0.0f, 0.0f); // Top-left - ImVec2 uv_max = ImVec2(1.0f, 1.0f); // Lower-right - ImVec4 tint_col = ImVec4(1.0f, 1.0f, 1.0f, 1.0f); // No tint - ImVec4 border_col = ImVec4(1.0f, 1.0f, 1.0f, 0.5f); // 50% opaque white - - ImGuiWindowFlags window_flags = ImGuiWindowFlags_HorizontalScrollbar | ImGuiWindowFlags_NoScrollWithMouse; - - ImGui::InputFloat("zoom", &zoom, 0.1f, 0.5f); - float my_tex_w = Tex_width * zoom; - float my_tex_h = Tex_height * zoom; - const ImVec2 child_size{ ImGui::GetWindowWidth() * 1.0f, ImGui::GetWindowHeight() * 0.5f }; - - ImGui::Checkbox("color", &color); - - struct point_pair - { - ImVec2 p1; - bool visible1; - ImVec2 p2; - bool visible2; - }; - - auto draw_zoom_pick_point = [my_tex_w, my_tex_h, tint_col, border_col](const ImTextureID& tex, std::vector& point_picked) - { - ImVec2 img_start = ImGui::GetItemRectMin(); - ImGuiIO& io = ImGui::GetIO(); - ImGui::BeginTooltip(); - float region_sz = 32.0f; - float region_x = io.MousePos.x - img_start.x - region_sz * 0.5f; - float region_y = io.MousePos.y - img_start.y - region_sz * 0.5f; - - // add point - if (io.MouseClicked[2] && io.KeyShift) - { - ImVec2 picked_point{ (io.MousePos.x - img_start.x) / my_tex_w, (io.MousePos.y - img_start.y) / my_tex_h }; - point_picked.push_back(picked_point); - } - - // remove last point - if (io.MouseClicked[1] && io.KeyShift && point_picked.size() > 0) - { - point_picked.pop_back(); - } - float local_zoom = 4.0f; - if (region_x < 0.0f) - { - region_x = 0.0f; - } - else if (region_x > my_tex_w - region_sz) - { - region_x = my_tex_w - region_sz; - } - if (region_y < 0.0f) - { - region_y = 0.0f; - } - else if (region_y > my_tex_h - region_sz) - { - region_y = my_tex_h - region_sz; - } - ImVec2 uv0 = ImVec2((region_x) / my_tex_w, (region_y) / my_tex_h); - ImVec2 uv1 = ImVec2((region_x + region_sz) / my_tex_w, (region_y + region_sz) / my_tex_h); - ImGui::Image(tex, ImVec2(region_sz * local_zoom, region_sz * local_zoom), uv0, uv1, tint_col, border_col); - ImVec2 img_start_loc = ImGui::GetItemRectMin(); - ImVec2 img_sz = { ImGui::GetItemRectMax().x - ImGui::GetItemRectMin().x, ImGui::GetItemRectMax().y - ImGui::GetItemRectMin().y }; - ImVec2 window_center = ImVec2(img_start_loc.x + img_sz.x * 0.5f, img_start_loc.y + img_sz.y * 0.5f); - ImGui::GetForegroundDrawList()->AddLine( - { window_center.x - 10, window_center.y }, { window_center.x + 10, window_center.y }, IM_COL32(0, 255, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddLine( - { window_center.x, window_center.y - 10 }, { window_center.x, window_center.y + 10 }, IM_COL32(0, 255, 0, 200), 1); - ImGui::EndTooltip(); - }; - - ImGui::BeginChild((name + "_child1").c_str(), child_size, false, window_flags); - { - ImGui::Image(tex1, ImVec2(my_tex_w, my_tex_h), uv_min, uv_max, tint_col, border_col); - const ImVec2 view_port_start = ImGui::GetWindowPos(); - const ImVec2 view_port_end{ view_port_start.x + ImGui::GetWindowWidth(), view_port_start.y + ImGui::GetWindowHeight() }; - ImVec2 img_start = ImGui::GetItemRectMin(); - for (int i = 0; i < point_picked.size(); i++) - { - const auto& p = point_picked[i]; - - ImVec2 center{ img_start.x + p.x * my_tex_w, img_start.y + p.y * my_tex_h }; - - if (center.x > view_port_start.x && center.x < view_port_end.x && center.y > view_port_start.y && center.y < view_port_end.y) - { - char data[16]; - snprintf(data, 16, "%d", i); - - ImGui::GetForegroundDrawList()->AddLine( - { center.x - 10, center.y }, { center.x + 10, center.y }, IM_COL32(255, 255, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddLine( - { center.x, center.y - 10 }, { center.x, center.y + 10 }, IM_COL32(255, 255, 0, 200), 1); - ImGui::GetForegroundDrawList()->AddText({ center.x, center.y + 10 }, IM_COL32(255, 0, 0, 200), data); - if (i < point_pickedInPointcloud.size()) - { - ImGui::GetForegroundDrawList()->AddLine( - { center.x, center.y }, point_pickedInPointcloud.at(i), IM_COL32(255, 0, 0, 200), 1); - - ImGui::GetForegroundDrawList()->AddText({ point_pickedInPointcloud.at(i) }, IM_COL32(255, 0, 0, 200), data); - } - } - } - if (ImGui::IsItemHovered()) - { - draw_zoom_pick_point(tex1, point_picked); - } - } - ImGui::SetScrollX(ImGui::GetScrollX() + transX); - ImGui::SetScrollY(ImGui::GetScrollY() + transY); - ImGui::EndChild(); -} - -ImVec2 UnprojectPoint(const Eigen::Vector3d& point) -{ - GLint viewport[4]; - GLdouble modelview[16]; - GLdouble projection[16]; - - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); - glGetDoublev(GL_PROJECTION_MATRIX, projection); - glGetIntegerv(GL_VIEWPORT, viewport); - - GLdouble winX, winY, winZ; - gluProject(point[0], point[1], point[2], modelview, projection, viewport, &winX, &winY, &winZ); - return { static_cast(winX), static_cast(viewport[3] - winY) }; -} - -std::pair GetRay(int x, int y) -{ - GLint viewport[4]; - GLdouble modelview[16]; - GLdouble projection[16]; - GLfloat winX, winY, winZ; - GLdouble posXnear, posYnear, posZnear; - GLdouble posXfar, posYfar, posZfar; - - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); - glGetDoublev(GL_PROJECTION_MATRIX, projection); - glGetIntegerv(GL_VIEWPORT, viewport); - - winX = (float)x; - winY = (float)viewport[3] - (float)y; - - Eigen::Vector3d position; - Eigen::Vector3d direction; - gluUnProject(winX, winY, 0, modelview, projection, viewport, &posXnear, &posYnear, &posZnear); - gluUnProject(winX, winY, -1000, modelview, projection, viewport, &posXfar, &posYfar, &posZfar); - - position.x() = posXnear; - position.y() = posYnear; - position.z() = posZnear; - - direction.x() = posXfar - posXnear; - direction.y() = posYfar - posYnear; - direction.z() = posZfar - posZnear; - - direction.normalize(); - - return { position, direction }; -} - -double GetDistanceToRay(const Eigen::Vector3d& qureyPoint, const std::pair& ray) -{ - return ray.second.cross(qureyPoint - ray.first).norm(); -} - -std::vector ApplyColorToPointcloud( - const std::vector& pointsRGB, - const unsigned char* imageData, - int imageWidth, - int imageHeight, - int nrChannels, - const Eigen::Affine3d& transfom) -{ - std::vector newCloud(pointsRGB.size()); - std::transform( -#if USE_EXECUTION_PAR_UNSEQ - std::execution::par_unseq, -#endif - pointsRGB.begin(), - pointsRGB.end(), - newCloud.begin(), - [&](mandeye::PointRGB p) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transfom); - double du, dv; - equrectangular_camera_colinearity_tait_bryan_wc( - du, - dv, - imageHeight, - imageWidth, - M_PI, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - p.point.x(), - p.point.y(), - p.point.z()); - int u = std::round(du); - int v = std::round(dv); - if (u > 0 && v > 0 && u < imageWidth && v < imageHeight) - { - int index = (v * imageWidth + u) * nrChannels; - unsigned char red = imageData[index]; - unsigned char green = imageData[index + 1]; - unsigned char blue = imageData[index + 2]; - p.rgb = { 1.f * red / 256.f, 1.f * green / 256.f, 1.f * blue / 256.f, 1.f }; - } - return p; - }); - return newCloud; -} - -std::vector ApplyColorToPointcloudFishEye( - const std::vector& pointsRGB, - const unsigned char* imageData, - int imageWidth, - int imageHeight, - int nrChannels, - const Eigen::Affine3d& transfom) -{ - std::vector newCloud(pointsRGB.size()); - std::transform( -#if USE_EXECUTION_PAR_UNSEQ - std::execution::par_unseq, -#endif - pointsRGB.begin(), - pointsRGB.end(), - newCloud.begin(), - [&](mandeye::PointRGB p) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transfom); - double du, dv; - // equrectangular_camera_colinearity_tait_bryan_wc(du,dv, imageHeight, imageWidth, - // M_PI, pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - // p.point.x(), - // p.point.y(), - // p.point.z()); - - projection_fisheye_camera_tait_bryan_wc( - du, - dv, - fx, - fy, - cx, - cy, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - p.point.x(), - p.point.y(), - p.point.z(), - k1, - k2, - k3, - k4, - alpha); - - int u = std::round(du); - int v = std::round(dv); - if (u > 0 && v > 0 && u < imageWidth && v < imageHeight) - { - int index = (v * imageWidth + u) * nrChannels; - unsigned char red = imageData[index]; - unsigned char green = imageData[index + 1]; - unsigned char blue = imageData[index + 2]; - p.rgb = { 1.f * red / 256.f, 1.f * green / 256.f, 1.f * blue / 256.f, 1.f }; - } - return p; - }); - return newCloud; -} - -uint32_t fromGrayCode(uint32_t gray) -{ - uint32_t num = 0; - for (; gray; gray >>= 1) - { - num ^= gray; - } - return num; -} - -uint32_t packBoolsToUint32(const std::array& bools) -{ - uint32_t result = 0; - for (size_t i = 0; i < bools.size(); ++i) - { - if (bools[i]) - { - result |= (1U << i); - } - } - return result; -} - -void TimeStampCount() -{ - namespace SD = SystemData; - - static bool show_terminal = false; - static char diode_input[256] = ""; - static uint32_t packedValue = 0; - static uint64_t decodedValue = 0; - static std::array diodeBits = {}; - static uint64_t fullTimestamp = 0; - static const std::array newOrder = { 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 0, 1, 2, 3, 4, 5, 6, 7 }; - static uint64_t sessionStart = 0; - - static const std::array diodePositions = { ImVec2(400, 3375), ImVec2(840, 3375), ImVec2(1250, 3350), ImVec2(1600, 3330), - ImVec2(2000, 3350), ImVec2(2400, 3400), ImVec2(2850, 3450), ImVec2(3370, 3470), - ImVec2(3870, 3460), ImVec2(4400, 3450), ImVec2(4900, 3400), ImVec2(5300, 3350), - ImVec2(5600, 3300), ImVec2(6000, 3300), ImVec2(6400, 3330), ImVec2(6800, 3350), - ImVec2(7200, 3375), ImVec2(7600, 3375) }; - - if (ImGui::Button("Manual Calculate Timestamp")) - { - show_terminal = true; - } - - if (show_terminal) - { - ImGui::Begin("Timestamp Terminal", &show_terminal); - - float scale = 1.0f; - ImVec2 imgPos = ImGui::GetCursorScreenPos(); - - if (SD::imageData != nullptr) - { - ImVec2 windowSize = ImGui::GetContentRegionAvail(); - float aspectRatio = (float)SD::imageWidth / (float)SD::imageHeight; - float scaleX = windowSize.x / SD::imageWidth; - float scaleY = windowSize.y / SD::imageHeight; - scale = std::min(scaleX, scaleY); - - float scaledWidth = SD::imageWidth * scale; - float scaledHeight = SD::imageHeight * scale; - - ImGui::Text("Loaded image:"); - imgPos = ImGui::GetCursorScreenPos(); - ImGui::Image(reinterpret_cast(static_cast(tex1)), ImVec2(scaledWidth, scaledHeight)); - - auto* drawList = ImGui::GetWindowDrawList(); - ImGui::SetWindowFontScale(1.25f); - for (int i = 0; i < 18; ++i) - { - ImVec2 pos = diodePositions[i]; - pos.x = imgPos.x + pos.x * scale; - pos.y = imgPos.y + pos.y * scale; - ImU32 color = diodeBits[i] ? IM_COL32(255, 0, 0, 255) : IM_COL32(0, 0, 0, 255); - drawList->AddText(pos, color, std::to_string(i).c_str()); - } - } - else - { - ImGui::Text("No image loaded."); - } - - ImGui::SetWindowFontScale(1.0f); - ImGui::Text("Click on bits:"); - for (int i = 0; i < 18; ++i) - { - if (diodeBits[i]) - { - ImGui::PushStyleColor(ImGuiCol_Button, IM_COL32(255, 0, 0, 255)); - ImGui::PushStyleColor(ImGuiCol_ButtonHovered, IM_COL32(200, 0, 0, 255)); - ImGui::PushStyleColor(ImGuiCol_ButtonActive, IM_COL32(150, 0, 0, 255)); - } - else - { - ImGui::PushStyleColor(ImGuiCol_Button, IM_COL32(50, 50, 50, 255)); - ImGui::PushStyleColor(ImGuiCol_ButtonHovered, IM_COL32(80, 80, 80, 255)); - ImGui::PushStyleColor(ImGuiCol_ButtonActive, IM_COL32(100, 100, 100, 255)); - } - - std::string label = std::to_string(i) + ": " + (diodeBits[i] ? "1" : "0"); - if (ImGui::SmallButton(label.c_str())) - { - diodeBits[i] = !diodeBits[i]; - } - - ImGui::PopStyleColor(3); - - if (i != 17) - ImGui::SameLine(); - } - ImGui::NewLine(); - - if (ImGui::Button("Load status.json")) - { - const auto input_file_names = mandeye::fd::OpenFileDialog("Choose json", mandeye::fd::Session_filter, false); - if (!input_file_names.empty()) - { - std::ifstream file(input_file_names.front()); - if (file) - { - try - { - nlohmann::json j; - file >> j; - - if (j.contains("livox") && j["livox"].contains("LivoxLidarInfo") && - j["livox"]["LivoxLidarInfo"].contains("m_sessionStart")) - { - sessionStart = j["livox"]["LivoxLidarInfo"]["m_sessionStart"]; - std::cout << "m_sessionStart: " << sessionStart << std::endl; - - fullTimestamp = sessionStart + decodedValue; - } - else - { - std::cerr << "Invalid JSON or missing 'm_sessionStart' key\n"; - } - } catch (const std::exception& e) - { - std::cerr << "JSON read error: " << e.what() << "\n"; - } - } - } - } - - if (ImGui::Button("Calculate timestamp")) - { - std::array reorderedBits = {}; - for (int i = 0; i < 18; ++i) - { - reorderedBits[i] = diodeBits[newOrder[i]]; - } - - packedValue = packBoolsToUint32(reorderedBits); - decodedValue = static_cast(fromGrayCode(packedValue)) * 10'000'000; - if (sessionStart != 0) - { - fullTimestamp = sessionStart + decodedValue; - } - } - - ImGui::Text("Packed uint32_t: %u", packedValue); - ImGui::Text("Decoded timestamp: %llu", decodedValue); - ImGui::Text("Timestamp: %llu", fullTimestamp); - double timestampSeconds = static_cast(fullTimestamp) / 1'000'000'000.0; - ImGui::Text("Timestamp (formatted): %.6f", timestampSeconds); - - ImGui::End(); - } -} - -void ImGuiLoadSaveButtons() -{ - namespace SD = SystemData; - if (ImGui::Button("Load Image")) - { - const auto input_file_names = mandeye::fd::OpenFileDialog("Choose Image", mandeye::fd::ImageFilter, false); - if (input_file_names.size()) - { - tex1 = make_tex(input_file_names.front()); - SD::imageData = stbi_load(input_file_names.front().c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); - } - - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); - } - ImGui::SameLine(); - if (ImGui::Button("Load Poincloud")) - { - const auto input_file_names = mandeye::fd::OpenFileDialog("Choose Pointcloud", mandeye::fd::LazFilter, false); - if (!input_file_names.empty()) - { - auto points = mandeye::load(input_file_names.front()); - SystemData::points.resize(points.size()); - std::transform( - points.begin(), - points.end(), - SystemData::points.begin(), - [&](const mandeye::Point& p) - { - return p; - }); - } - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); - } - ImGui::SameLine(); - if (ImGui::Button("Save Pointcloud")) - { - const auto input_file_names = mandeye::fd::SaveFileDialog("Choose Pointcloud", mandeye::fd::LazFilter); - if (!input_file_names.empty()) - { - mandeye::saveLaz(input_file_names, SD::points); - } - } - ImGui::SameLine(); -} - -void optimize() -{ - if (SystemData::pointPickedPointCloud.size() == SystemData::pointPickedImage.size() && SystemData::pointPickedPointCloud.size() >= 5) - { - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - for (int i = 0; i < SystemData::pointPickedImage.size(); i++) - { - Eigen::Matrix delta; - observation_equation_equrectangular_camera_colinearity_tait_bryan_wc( - delta, - SystemData::imageHeight, - SystemData::imageWidth, - M_PI, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - SystemData::pointPickedImage[i].x * SystemData::imageWidth, - SystemData::pointPickedImage[i].y * SystemData::imageHeight); - - Eigen::Matrix jacobian; - observation_equation_equrectangular_camera_colinearity_tait_bryan_wc_jacobian( - jacobian, - SystemData::imageHeight, - SystemData::imageWidth, - M_PI, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - SystemData::pointPickedImage[i].x, - SystemData::pointPickedImage[i].y); - - int ir = tripletListB.size(); - int ic_camera = 0; - - tripletListA.emplace_back(ir, ic_camera, -jacobian(0, 0)); - tripletListA.emplace_back(ir, ic_camera + 1, -jacobian(0, 1)); - tripletListA.emplace_back(ir, ic_camera + 2, -jacobian(0, 2)); - tripletListA.emplace_back(ir, ic_camera + 3, -jacobian(0, 3)); - tripletListA.emplace_back(ir, ic_camera + 4, -jacobian(0, 4)); - tripletListA.emplace_back(ir, ic_camera + 5, -jacobian(0, 5)); - tripletListA.emplace_back(ir + 1, ic_camera, -jacobian(1, 0)); - tripletListA.emplace_back(ir + 1, ic_camera + 1, -jacobian(1, 1)); - tripletListA.emplace_back(ir + 1, ic_camera + 2, -jacobian(1, 2)); - tripletListA.emplace_back(ir + 1, ic_camera + 3, -jacobian(1, 3)); - tripletListA.emplace_back(ir + 1, ic_camera + 4, -jacobian(1, 4)); - tripletListA.emplace_back(ir + 1, ic_camera + 5, -jacobian(1, 5)); - tripletListP.emplace_back(ir, ir, cauchy(delta(0, 0), 1)); - tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1, 0), 1)); - tripletListB.emplace_back(ir, 0, delta(0, 0)); - tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); - } - - Eigen::SparseMatrix matA(tripletListB.size(), 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(6, 6); - Eigen::SparseMatrix AtPB(6, 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = (AtP)*matA; - AtPB = (AtP)*matB; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - - Eigen::SimplicialCholesky> solver(AtPA); - - Eigen::SparseMatrix x = solver.solve(AtPB); - - std::vector h_x; - - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - h_x.push_back(it.value()); - } - } - - if (h_x.size() == 6) - { - int counter = 0; - pose.px += h_x[counter++] * 0.1; - pose.py += h_x[counter++] * 0.1; - pose.pz += h_x[counter++] * 0.1; - pose.om += h_x[counter++] * 0.1; - pose.fi += h_x[counter++] * 0.1; - pose.ka += h_x[counter++] * 0.1; - - SystemData::camera_pose = affine_matrix_from_pose_tait_bryan(pose); - } - else - { - std::cout << "AtPA=AtPB FAILED" << std::endl; - } - } - else - { - std::cout << "Please mark at least 5 proper image to cloud correspondances" << std::endl; - } -} - -void optimize_fish_eye() -{ - if (SystemData::pointPickedPointCloud.size() == SystemData::pointPickedImage.size() && SystemData::pointPickedPointCloud.size() >= 5) - { - std::vector> tripletListA; - std::vector> tripletListP; - std::vector> tripletListB; - - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - for (int i = 0; i < SystemData::pointPickedImage.size(); i++) - { - Eigen::Matrix delta; - // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc(delta, SystemData::imageHeight, SystemData::imageWidth, - // M_PI, - // pose.px, pose.py, pose.pz, pose.om, pose.fi, pose.ka, - // SystemData::pointPickedPointCloud[i].x(), - // SystemData::pointPickedPointCloud[i].y(), - // SystemData::pointPickedPointCloud[i].z(), - // SystemData::pointPickedImage[i].x * - // SystemData::imageWidth, - // SystemData::pointPickedImage[i].y * - // SystemData::imageHeight); - - observation_equation_fisheye_camera_tait_bryan_wc( - delta, - fx, - fy, - cx, - cy, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - SystemData::pointPickedImage[i].x * SystemData::imageWidth, - SystemData::pointPickedImage[i].y * SystemData::imageHeight, - k1, - k2, - k3, - k4, - alpha); - - Eigen::Matrix jacobian; - // observation_equation_equrectangular_camera_colinearity_tait_bryan_wc_jacobian(jacobian, SystemData::imageHeight, - // SystemData::imageWidth, M_PI, - // pose.px, pose.py, pose.pz, pose.om, pose.fi, - // pose.ka, - // SystemData::pointPickedPointCloud[i].x(), - // SystemData::pointPickedPointCloud[i].y(), - // SystemData::pointPickedPointCloud[i].z(), - // SystemData::pointPickedImage[i].x, - // SystemData::pointPickedImage[i].y); - - observation_equation_fisheye_camera_tait_bryan_wc_jacobian( - jacobian, - fx, - fy, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - SystemData::pointPickedPointCloud[i].x(), - SystemData::pointPickedPointCloud[i].y(), - SystemData::pointPickedPointCloud[i].z(), - k1, - k2, - k3, - k4, - alpha); - - int ir = tripletListB.size(); - int ic_camera = 0; - - tripletListA.emplace_back(ir, ic_camera, -jacobian(0, 0)); - tripletListA.emplace_back(ir, ic_camera + 1, -jacobian(0, 1)); - tripletListA.emplace_back(ir, ic_camera + 2, -jacobian(0, 2)); - tripletListA.emplace_back(ir, ic_camera + 3, -jacobian(0, 3)); - tripletListA.emplace_back(ir, ic_camera + 4, -jacobian(0, 4)); - tripletListA.emplace_back(ir, ic_camera + 5, -jacobian(0, 5)); - tripletListA.emplace_back(ir + 1, ic_camera, -jacobian(1, 0)); - tripletListA.emplace_back(ir + 1, ic_camera + 1, -jacobian(1, 1)); - tripletListA.emplace_back(ir + 1, ic_camera + 2, -jacobian(1, 2)); - tripletListA.emplace_back(ir + 1, ic_camera + 3, -jacobian(1, 3)); - tripletListA.emplace_back(ir + 1, ic_camera + 4, -jacobian(1, 4)); - tripletListA.emplace_back(ir + 1, ic_camera + 5, -jacobian(1, 5)); - - tripletListP.emplace_back(ir, ir, cauchy(delta(0, 0), 1)); - tripletListP.emplace_back(ir + 1, ir + 1, cauchy(delta(1, 0), 1)); - - tripletListB.emplace_back(ir, 0, delta(0, 0)); - tripletListB.emplace_back(ir + 1, 0, delta(1, 0)); - } - - Eigen::SparseMatrix matA(tripletListB.size(), 6); - Eigen::SparseMatrix matP(tripletListB.size(), tripletListB.size()); - Eigen::SparseMatrix matB(tripletListB.size(), 1); - - matA.setFromTriplets(tripletListA.begin(), tripletListA.end()); - matP.setFromTriplets(tripletListP.begin(), tripletListP.end()); - matB.setFromTriplets(tripletListB.begin(), tripletListB.end()); - - Eigen::SparseMatrix AtPA(6, 6); - Eigen::SparseMatrix AtPB(6, 1); - - { - Eigen::SparseMatrix AtP = matA.transpose() * matP; - AtPA = (AtP)*matA; - AtPB = (AtP)*matB; - } - - tripletListA.clear(); - tripletListP.clear(); - tripletListB.clear(); - - Eigen::SimplicialCholesky> solver(AtPA); - - Eigen::SparseMatrix x = solver.solve(AtPB); - - std::vector h_x; - - for (int k = 0; k < x.outerSize(); ++k) - { - for (Eigen::SparseMatrix::InnerIterator it(x, k); it; ++it) - { - h_x.push_back(it.value()); - } - } - - if (h_x.size() == 6) - { - int counter = 0; - pose.px += h_x[counter++] * 0.1; - pose.py += h_x[counter++] * 0.1; - pose.pz += h_x[counter++] * 0.1; - pose.om += h_x[counter++] * 0.1; - pose.fi += h_x[counter++] * 0.1; - pose.ka += h_x[counter++] * 0.1; - - SystemData::camera_pose = affine_matrix_from_pose_tait_bryan(pose); - } - else - { - std::cout << "AtPA=AtPB FAILED" << std::endl; - } - } - else - { - std::cout << "Please mark at least 5 proper image to cloud correspondances" << std::endl; - } -} - -void display() -{ - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - glTranslatef(translate_x, translate_y, translate_z); - glRotatef(rotate_x, 1.0, 0.0, 0.0); - glRotatef(rotate_y, 0.0, 0.0, 1.0); - - ////////// - // glColor3f(p.rgb.data()); - glPointSize(SystemData::point_size); - glBegin(GL_POINTS); - for (const auto& p : SystemData::points) - { - if (color) - { - glColor3fv(p.rgb.data()); - } - else - { - glColor3f(p.intensity - 100, p.intensity - 100, p.intensity - 100); - // p.intensity - } - - glVertex3dv(p.point.data()); - } - glEnd(); - ////////////////////////////////// - glPointSize(10); - glBegin(GL_POINTS); - for (const auto& p : SystemData::pointPickedPointCloud) - { - glColor3f(1.f, 0.f, 0.f); - glVertex3dv(p.data()); - } - glEnd(); - - if (imgui_draw_co) - { - glLineWidth(5); - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(imgui_co_size, 0.0f, 0.0f); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, imgui_co_size, 0.0f); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, imgui_co_size); - glEnd(); - } - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); - - std::vector picked3DPoints(SystemData::pointPickedPointCloud.size()); - std::transform( - SystemData::pointPickedPointCloud.begin(), SystemData::pointPickedPointCloud.end(), picked3DPoints.begin(), UnprojectPoint); - - ImGui::Begin("Image"); - ImGuiLoadSaveButtons(); - TimeStampCount(); - // if (ImGui::Button("apply color to PC (fishEye)")) - //{ - // SystemData::points = ApplyColorToPointcloudFishEye(SystemData::points, SystemData::imageData, SystemData::imageWidth, - // SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); - //} - - if (ImGui::Button("Optimize")) - { - optimize(); - - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - std::cout << "pose" << std::endl; - std::cout << "px " << pose.px << std::endl; - std::cout << "py " << pose.py << std::endl; - std::cout << "pz " << pose.pz << std::endl; - std::cout << "om " << pose.om << std::endl; - std::cout << "fi " << pose.fi << std::endl; - std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); - } - ImGui::SameLine(); - - /*if (ImGui::Button("Optimize(fisheye)")) - { - optimize_fish_eye(); - - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - std::cout << "pose" << std::endl; - std::cout << "px " << pose.px << std::endl; - std::cout << "py " << pose.py << std::endl; - std::cout << "pz " << pose.pz << std::endl; - std::cout << "om " << pose.om << std::endl; - std::cout << "fi " << pose.fi << std::endl; - std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloudFishEye(SystemData::points, SystemData::imageData, SystemData::imageWidth, - SystemData::imageHeight, SystemData::imageNrChannels, SystemData::camera_pose); - }*/ - ImGui::SameLine(); - - if (ImGui::Button("Optimize x 100")) - { - for (int i = 0; i < 100; i++) - { - optimize(); - if (i % 10 == 0) - { - std::cout << "iteration: " << i << " of 100" << std::endl; - } - } - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - std::cout << "pose" << std::endl; - std::cout << "px " << pose.px << std::endl; - std::cout << "py " << pose.py << std::endl; - std::cout << "pz " << pose.pz << std::endl; - std::cout << "om " << pose.om << std::endl; - std::cout << "fi " << pose.fi << std::endl; - std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); - } - ImGui::InputInt("point_size", &SystemData::point_size); - - if (SystemData::point_size < 1) - { - SystemData::point_size = 1; - } - - if (ImGui::Button("save camera to lidar relative pose (*.reg)")) - { - auto output_file_name = mandeye::fd::SaveFileDialog("Save RESSO file", mandeye::fd::Resso_filter /*mandeye::fd::reg_filter*/, ""); - std::cout << "RESSO file to save: '" << output_file_name << "'" << std::endl; - - std::ofstream outfile; - outfile.open(output_file_name); - if (!outfile.good()) - { - std::cout << "can not save file: " << output_file_name << std::endl; - - return; - } - - outfile << 1 << std::endl; - outfile << "camera_to_lidar_relative_pose" << std::endl; - outfile << SystemData::camera_pose(0, 0) << " " << SystemData::camera_pose(0, 1) << " " << SystemData::camera_pose(0, 2) << " " - << SystemData::camera_pose(0, 3) << std::endl; - outfile << SystemData::camera_pose(1, 0) << " " << SystemData::camera_pose(1, 1) << " " << SystemData::camera_pose(1, 2) << " " - << SystemData::camera_pose(1, 3) << std::endl; - outfile << SystemData::camera_pose(2, 0) << " " << SystemData::camera_pose(2, 1) << " " << SystemData::camera_pose(2, 2) << " " - << SystemData::camera_pose(2, 3) << std::endl; - outfile << "0 0 0 1" << std::endl; - - outfile.close(); - } - - ImGui::SameLine(); - - if (ImGui::Button("load camera to lidar relative pose (*.reg)")) - { - std::string input_file_name = ""; - input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO", mandeye::fd::Resso_filter); - std::cout << "resso file: '" << input_file_name << "'" << std::endl; - - std::ifstream infile(input_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << input_file_name << "'" << std::endl; - return; - } - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - - int num_scans; - iss >> num_scans; - - std::cout << "number of scans: " << num_scans << std::endl; - size_t sum_points_before_decimation = 0; - size_t sum_points_after_decimation = 0; - - for (size_t i = 0; i < num_scans; i++) - { - std::getline(infile, line); - std::istringstream iss(line); - std::string point_cloud_file_name; - iss >> point_cloud_file_name; - - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss3(line); - iss3 >> r31 >> r32 >> r33 >> t34; - - std::getline(infile, line); - - // PointCloud pc; - // pc.file_name = point_cloud_file_name; - SystemData::camera_pose = Eigen::Affine3d::Identity(); - SystemData::camera_pose(0, 0) = r11; - SystemData::camera_pose(0, 1) = r12; - SystemData::camera_pose(0, 2) = r13; - SystemData::camera_pose(1, 0) = r21; - SystemData::camera_pose(1, 1) = r22; - SystemData::camera_pose(1, 2) = r23; - SystemData::camera_pose(2, 0) = r31; - SystemData::camera_pose(2, 1) = r32; - SystemData::camera_pose(2, 2) = r33; - SystemData::camera_pose(0, 3) = t14; - SystemData::camera_pose(1, 3) = t24; - SystemData::camera_pose(2, 3) = t34; - } - infile.close(); - - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(SystemData::camera_pose); - std::cout << "pose" << std::endl; - std::cout << "px " << pose.px << std::endl; - std::cout << "py " << pose.py << std::endl; - std::cout << "pz " << pose.pz << std::endl; - std::cout << "om " << pose.om << std::endl; - std::cout << "fi " << pose.fi << std::endl; - std::cout << "ka " << pose.ka << std::endl; - SystemData::points = ApplyColorToPointcloud( - SystemData::points, - SystemData::imageData, - SystemData::imageWidth, - SystemData::imageHeight, - SystemData::imageNrChannels, - SystemData::camera_pose); - } - - imagePicker("ImagePicker", (ImTextureID)tex1, SystemData::pointPickedImage, picked3DPoints); - - ImGui::Text("!!! SELECT at least 5 image <--> point cloud pairs !!!"); - ImGui::Text("To pick image: press shift and middle mouse button (cursor on image)"); - ImGui::Text("To pick point in 3D: press shift and middle mouse button (cursor on point cloud)"); - ImGui::Text("page up: zoom in"); - ImGui::Text("page down: zoom out"); - ImGui::Text("arrows: move image"); - - // 2D Points Picked - ImGui::BeginChild("2D", ImVec2(300, 0), true); - ImGui::Text("2D:"); - for (auto it = SystemData::pointPickedImage.begin(); it != SystemData::pointPickedImage.end(); it++) - { - auto index = std::distance(SystemData::pointPickedImage.begin(), it); - const auto& p = *it; - ImGui::Text("%d : %.1f,%.1f", index, p.x, p.y); - ImGui::SameLine(); - const auto label = std::string("-##2s") + std::to_string(index); - if (ImGui::Button(label.c_str())) - { - SystemData::pointPickedImage.erase(it); - break; - } - } - - ImGui::EndChild(); - ImGui::SameLine(); - - // 3D Points Picked - ImGui::BeginChild("3D", ImVec2(300, 0), true); - ImGui::Text("3D:"); - for (auto it = SystemData::pointPickedPointCloud.begin(); it != SystemData::pointPickedPointCloud.end(); it++) - { - const auto& p = *it; - const auto index = std::distance(SystemData::pointPickedPointCloud.begin(), it); - auto prev = it != SystemData::pointPickedPointCloud.begin() ? it - 1 : SystemData::pointPickedPointCloud.end(); - auto next = it + 1 != SystemData::pointPickedPointCloud.end() ? it + 1 : SystemData::pointPickedPointCloud.end(); - - const auto label = std::string("-##2s") + std::to_string(index); - - if (ImGui::Button(label.c_str())) - { - SystemData::pointPickedPointCloud.erase(it); - break; - } - const auto labelUp = std::string("U##2s") + std::to_string(index); - const auto labelDn = std::string("D##2s") + std::to_string(index); - if (prev != SystemData::pointPickedPointCloud.end()) - { - ImGui::SameLine(); - if (ImGui::Button(labelUp.c_str())) - { - std::swap(*it, *prev); - break; - } - } - if (next != SystemData::pointPickedPointCloud.end()) - { - ImGui::SameLine(); - if (ImGui::Button(labelDn.c_str())) - { - std::swap(*it, *next); - break; - } - } - ImGui::SameLine(); - ImGui::Text("%ld: %.1f,%.1f,%.1f", index, p.x(), p.y(), p.z()); - } - ImGui::EndChild(); - - ImGui::End(); - - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - glutSwapBuffers(); - glutPostRedisplay(); -} - -void mouse(int glut_button, int state, int x, int y) -{ - ImGui_ImplGLUT_MouseFunc(glut_button, state, x, y); - ImGuiIO& io = ImGui::GetIO(); - int button = -1; - if (glut_button == GLUT_LEFT_BUTTON) - button = 0; - if (glut_button == GLUT_RIGHT_BUTTON) - button = 1; - if (glut_button == GLUT_MIDDLE_BUTTON) - button = 2; - - if (!io.WantCaptureMouse) - { - if (state == GLUT_DOWN) - { - mouse_buttons |= 1 << glut_button; - } - else if (state == GLUT_UP) - { - mouse_buttons = 0; - } - mouse_old_x = x; - mouse_old_y = y; - - if (state == GLUT_DOWN) - { - if (glut_button == GLUT_MIDDLE_BUTTON && io.KeyShift) - { - SystemData::clickedRay = GetRay(x, y); - - std::mutex mtx; - std::pair distanceIndexPair{ std::numeric_limits::max(), -1 }; - - std::for_each( -#if USE_EXECUTION_PAR_UNSEQ - std::execution::par_unseq, -#endif - SystemData::points.begin(), - SystemData::points.end(), - [&](const mandeye::PointRGB& p) - { - double D = GetDistanceToRay(p.point, SystemData::clickedRay); - std::scoped_lock guard(mtx); - if (D < distanceIndexPair.first) - { - // Assume that SystemData::point is an array-like type implementation, naked pointer arithmetic ahead: - const int index = &p - &SystemData::points.front(); - assert(index >= 0); - assert(index < SystemData::points.size()); - distanceIndexPair = { D, index }; - } - }); - - if (distanceIndexPair.second > 0) - { - const auto& [distance, index] = distanceIndexPair; - std::cout << "Closest point found, distance " << distance << std::endl; - SystemData::closestPointIndex = distanceIndexPair.second; - SystemData::pointPickedPointCloud.push_back(SystemData::points.at(index).point); - } - } - if (glut_button == GLUT_RIGHT_BUTTON && io.KeyShift) - { - if (SystemData::pointPickedPointCloud.size() > 0) - { - SystemData::pointPickedPointCloud.pop_back(); - } - } - } - } -} - -void motion(int x, int y) -{ - ImGui_ImplGLUT_MotionFunc(x, y); - ImGuiIO& io = ImGui::GetIO(); - - if (!io.WantCaptureMouse) - { - float dx, dy; - dx = (float)(x - mouse_old_x); - dy = (float)(y - mouse_old_y); - gui_mouse_down = mouse_buttons > 0; - if (mouse_buttons & 1) - { - rotate_x += dy * 0.2f; - rotate_y += dx * 0.2f; - } - else if (mouse_buttons & 4) - { - translate_z += dy * 0.05f; - } - else if (mouse_buttons & 3) - { - translate_x += dx * 0.05f; - translate_y -= dy * 0.05f; - } - mouse_old_x = x; - mouse_old_y = y; - } - glutPostRedisplay(); -} - -void reshape(int w, int h) -{ - glViewport(0, 0, (GLsizei)w, (GLsizei)h); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - gluPerspective(60.0, (GLfloat)w / (GLfloat)h, 0.01, 10000.0); - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); -} - -bool initGL(int* argc, char** argv) -{ - glutInit(argc, argv); - glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE); - glutInitWindowSize(window_width, window_height); - glutCreateWindow("MANDEYE with 360 camera manual coloring " HDMAPPING_VERSION_STRING); - glutDisplayFunc(display); - glutMotionFunc(motion); - - // default initialization - glClearColor(1.0, 1.0, 1.0, 1.0); - // glEnable(GL_DEPTH_TEST); - - glViewport(0, 0, window_width, window_height); - - // projection - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 1000.0); - glutReshapeFunc(reshape); - ImGui::CreateContext(); - ImGuiIO& io = ImGui::GetIO(); - (void)io; - - ImGui::StyleColorsDark(); - ImGui_ImplGLUT_Init(); - ImGui_ImplGLUT_InstallFuncs(); - ImGui_ImplOpenGL2_Init(); - - return true; -} \ No newline at end of file diff --git a/apps/single_session_manual_coloring/CMakeLists.txt b/apps/single_session_manual_coloring/CMakeLists.txt deleted file mode 100644 index 12a28435..00000000 --- a/apps/single_session_manual_coloring/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -cmake_minimum_required(VERSION 4.0.0) - -project(single_session_manual_coloring) - -add_executable(single_session_manual_coloring single_session_manual_coloring.cpp) - -target_include_directories( - single_session_manual_coloring - PRIVATE include - ${REPOSITORY_DIRECTORY}/core/include - ${REPOSITORY_DIRECTORY}/core_hd_mapping/include - ${THIRDPARTY_DIRECTORY} - ${THIRDPARTY_DIRECTORY}/glm - ${EIGEN3_INCLUDE_DIR} - ${THIRDPARTY_DIRECTORY}/imgui - ${THIRDPARTY_DIRECTORY}/imgui/backends - ${THIRDPARTY_DIRECTORY}/ImGuizmo - ${THIRDPARTY_DIRECTORY}/json/include - ${THIRDPARTY_DIRECTORY}/portable-file-dialogs-master - ${LASZIP_INCLUDE_DIR}/LASzip/include - ${THIRDPARTY_DIRECTORY}/glew-cmake/include - ${THIRDPARTY_DIRECTORY}/observation_equations/codes - ${FREEGLUT_INCLUDE_DIR}) - -target_compile_definitions(single_session_manual_coloring PRIVATE WITH_GUI=1) - -target_link_libraries( - single_session_manual_coloring - PRIVATE ${FREEGLUT_LIBRARY} - ${OPENGL_gl_LIBRARY} - OpenGL::GLU - ${PLATFORM_LASZIP_LIB} - ${PLATFORM_MISCELLANEOUS_LIBS} - ${CORE_LIBRARIES} - ${GUI_LIBRARIES}) - -if(WIN32) - add_custom_command( - TARGET single_session_manual_coloring - POST_BUILD - COMMAND - ${CMAKE_COMMAND} -E copy - $ - $ - COMMAND_EXPAND_LISTS) -endif() - -if (MSVC) - target_compile_options(single_session_manual_coloring PRIVATE /bigobj) -endif() - -hdmapping_install_app(single_session_manual_coloring) \ No newline at end of file diff --git a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp b/apps/single_session_manual_coloring/single_session_manual_coloring.cpp deleted file mode 100644 index 06a14f51..00000000 --- a/apps/single_session_manual_coloring/single_session_manual_coloring.cpp +++ /dev/null @@ -1,1096 +0,0 @@ - -#include - -#include "imgui.h" -#include "imgui_impl_glut.h" -#include "imgui_impl_opengl2.h" - -#define STB_IMAGE_IMPLEMENTATION -#include "stb_image.h" - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include -#include - -const uint32_t window_width = 800; -const uint32_t window_height = 600; -double camera_ortho_xy_view_zoom = 10; -double camera_ortho_xy_view_shift_x = 0.0; -double camera_ortho_xy_view_shift_y = 0.0; -double camera_mode_ortho_z_center_h = 0.0; -double camera_ortho_xy_view_rotation_angle_deg = 0; -bool is_ortho = false; -bool show_axes = true; -ImVec4 clear_color = ImVec4(0.8f, 0.8f, 0.8f, 1.00f); -ImVec4 pc_neigbouring_color = ImVec4(0.5f, 0.5f, 0.5f, 1.0f); -ImVec4 pc_color2 = ImVec4(0.0f, 0.0f, 1.0f, 1.0f); - -Eigen::Vector3f rotation_center = Eigen::Vector3f::Zero(); -float translate_x, translate_y = 0.0; -float translate_z = -20.0; -float rotate_x = 0.0, rotate_y = 0.0; -int mouse_old_x, mouse_old_y; -int mouse_buttons = 0; -bool gui_mouse_down{ false }; -float mouse_sensitivity = 1.0; - -float m_ortho_projection[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - -float m_ortho_gizmo_view[] = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 }; - -int decimation_step = 1000; -Session session; - -std::vector corresponding_images; -std::vector offsets; -std::vector images_file_names; - -namespace fs = std::filesystem; - -namespace SystemData -{ - std::vector points; - std::pair clickedRay; - int closestPointIndex{ -1 }; - std::vector pointPickedImage; - std::vector pointPickedPointCloud; - - unsigned char* imageData; - int imageWidth, imageHeight, imageNrChannels; - - Eigen::Affine3d camera_pose = Eigen::Affine3d::Identity(); - - int point_size = 1; -} // namespace SystemData - -void reshape(int w, int h) -{ - glViewport(0, 0, (GLsizei)w, (GLsizei)h); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - if (!is_ortho) - { - gluPerspective(60.0, (GLfloat)w / (GLfloat)h, 0.01, 10000.0); - } - else - { - ImGuiIO& io = ImGui::GetIO(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - glOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); - // glOrtho(-translate_z, translate_z, -translate_z * (float)h / float(w), translate_z * float(h) / float(w), -10000, 10000); - } - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); -} - -void motion(int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - - if (!io.WantCaptureMouse) - { - float dx, dy; - dx = (float)(x - mouse_old_x); - dy = (float)(y - mouse_old_y); - - if (is_ortho) - { - if (mouse_buttons & 1) - { - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - Eigen::Vector3d v( - dx * (camera_ortho_xy_view_zoom / (GLsizei)io.DisplaySize.x * 2), - dy * (camera_ortho_xy_view_zoom / (GLsizei)io.DisplaySize.y * 2 / ratio), - 0); - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = camera_ortho_xy_view_rotation_angle_deg * M_PI / 180.0; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - Eigen::Vector3d v_t = m * v; - camera_ortho_xy_view_shift_x += v_t.x(); - camera_ortho_xy_view_shift_y += v_t.y(); - } - } - else - { - gui_mouse_down = mouse_buttons > 0; - if (mouse_buttons & 1) - { - rotate_x += dy * 0.2f; // * mouse_sensitivity; - rotate_y += dx * 0.2f; // * mouse_sensitivity; - } - if (mouse_buttons & 4) - { - translate_x += dx * 0.05f * mouse_sensitivity; - translate_y -= dy * 0.05f * mouse_sensitivity; - } - } - - mouse_old_x = x; - mouse_old_y = y; - } - glutPostRedisplay(); -} - -std::vector ApplyColorToPointcloud( - const std::vector& pointsRGB, - const unsigned char* imageData, - int imageWidth, - int imageHeight, - int nrChannels, - const Eigen::Affine3d& transfom) -{ - std::vector newCloud(pointsRGB.size()); - std::transform( -#if USE_EXECUTION_PAR_UNSEQ - std::execution::par_unseq, -#endif - pointsRGB.begin(), - pointsRGB.end(), - newCloud.begin(), - [&](mandeye::PointRGB p) - { - TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(transfom); - double du, dv; - equrectangular_camera_colinearity_tait_bryan_wc( - du, - dv, - imageHeight, - imageWidth, - M_PI, - pose.px, - pose.py, - pose.pz, - pose.om, - pose.fi, - pose.ka, - p.point.x(), - p.point.y(), - p.point.z()); - int u = std::round(du); - int v = std::round(dv); - if (u > 0 && v > 0 && u < imageWidth && v < imageHeight) - { - int index = (v * imageWidth + u) * nrChannels; - unsigned char red = imageData[index]; - unsigned char green = imageData[index + 1]; - unsigned char blue = imageData[index + 2]; - p.rgb = { 1.f * red / 256.f, 1.f * green / 256.f, 1.f * blue / 256.f, 1.f }; - } - else - { - p.rgb = { 0.f, 0.f, 0.f, 1.f }; - } - return p; - }); - return newCloud; -} - -void project_gui() -{ - if (ImGui::Begin("single_session_manual_coloring")) - { - ImGui::ColorEdit3("clear color", (float*)&clear_color); - ImGui::Checkbox("show_axes", &show_axes); - ImGui::InputInt("point cloud decimation_step", &decimation_step); - if (decimation_step < 1) - { - decimation_step = 1; - } - - ImGui::InputInt("point_size", &SystemData::point_size); - - if (SystemData::point_size < 1) - { - SystemData::point_size = 1; - } - - if (ImGui::Button("load session")) - { - corresponding_images.clear(); - offsets.clear(); - - std::string input_file_name = ""; - input_file_name = mandeye::fd::OpenFileDialogOneFile("Load session file", mandeye::fd::Session_filter); - std::cout << "Session file: '" << input_file_name << "'" << std::endl; - - if (input_file_name.size() > 0) - { - session.load(fs::path(input_file_name).string(), false, 0.0, 0.0, 0.0, false); - } - - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - corresponding_images.push_back(0); - offsets.push_back(0); - } - } - - ImGui::SameLine(); - if (ImGui::Button("save colored pointcloud")) - { - const auto input_file_names = mandeye::fd::SaveFileDialog("Colored point cloud", mandeye::fd::LazFilter); - if (!input_file_names.empty()) - { - std::vector pointsRGB; - - for (const auto& p : session.point_clouds_container.point_clouds) - { - if (p.visible) - { - for (int index = 0; index < p.points_local.size(); index++) - { - mandeye::PointRGB out; - out.point = p.m_pose * p.points_local[index]; - out.intensity = p.intensities[index]; - out.rgb[0] = p.colors[index].x(); - out.rgb[1] = p.colors[index].y(); - out.rgb[2] = p.colors[index].z(); - out.rgb[3] = 1.0; - pointsRGB.push_back(out); - } - } - } - - mandeye::saveLaz(input_file_names, pointsRGB); - } - } - - if (ImGui::Button("load equirectangular images")) - { - images_file_names.clear(); - std::vector input_file_names; - - input_file_names = mandeye::fd::OpenFileDialog("Load all files", mandeye::fd::ImageFilter, true); - - std::cout << "input_images list begin" << std::endl; - std::cout << "----------------------" << std::endl; - for (const auto& fn : input_file_names) - { - std::cout << "'" << fn << "'" << std::endl; - images_file_names.push_back(fn); - } - std::cout << "----------------------" << std::endl; - std::cout << "input_images list end" << std::endl; - } - - if (ImGui::Button("load camera to lidar extrinsic calibration (*.reg)")) - { - std::string input_file_name = ""; - input_file_name = mandeye::fd::OpenFileDialogOneFile("Load RESSO", mandeye::fd::Resso_filter); - std::cout << "resso file: '" << input_file_name << "'" << std::endl; - - std::ifstream infile(input_file_name); - if (!infile.good()) - { - std::cout << "problem with file: '" << input_file_name << "'" << std::endl; - return; - } - std::string line; - std::getline(infile, line); - std::istringstream iss(line); - - int num_scans; - iss >> num_scans; - - std::cout << "number of scans: " << num_scans << std::endl; - size_t sum_points_before_decimation = 0; - size_t sum_points_after_decimation = 0; - - for (size_t i = 0; i < num_scans; i++) - { - std::getline(infile, line); - std::istringstream iss(line); - std::string point_cloud_file_name; - iss >> point_cloud_file_name; - - double r11, r12, r13, r21, r22, r23, r31, r32, r33; - double t14, t24, t34; - - std::getline(infile, line); - std::istringstream iss1(line); - iss1 >> r11 >> r12 >> r13 >> t14; - - std::getline(infile, line); - std::istringstream iss2(line); - iss2 >> r21 >> r22 >> r23 >> t24; - - std::getline(infile, line); - std::istringstream iss3(line); - iss3 >> r31 >> r32 >> r33 >> t34; - - std::getline(infile, line); - - SystemData::camera_pose = Eigen::Affine3d::Identity(); - SystemData::camera_pose(0, 0) = r11; - SystemData::camera_pose(0, 1) = r12; - SystemData::camera_pose(0, 2) = r13; - SystemData::camera_pose(1, 0) = r21; - SystemData::camera_pose(1, 1) = r22; - SystemData::camera_pose(1, 2) = r23; - SystemData::camera_pose(2, 0) = r31; - SystemData::camera_pose(2, 1) = r32; - SystemData::camera_pose(2, 2) = r33; - SystemData::camera_pose(0, 3) = t14; - SystemData::camera_pose(1, 3) = t24; - SystemData::camera_pose(2, 3) = t34; - } - infile.close(); - } - - if (ImGui::Button("select all")) - { - for (auto& pc : session.point_clouds_container.point_clouds) - { - pc.visible = true; - } - } - - ImGui::SameLine(); - - if (ImGui::Button("unselect all")) - { - for (auto& pc : session.point_clouds_container.point_clouds) - { - pc.visible = false; - } - } - - for (int i = 0; i < session.point_clouds_container.point_clouds.size(); - i++ /*auto &pc : session.point_clouds_container.point_clouds*/) - { - auto& pc = session.point_clouds_container.point_clouds[i]; - - ImGui::Text("----------------------------"); - ImGui::Checkbox(pc.file_name.c_str(), &pc.visible); - - if (pc.visible) - { - if (session.point_clouds_container.point_clouds.size() == corresponding_images.size() && images_file_names.size() > 0) - { - int prev = corresponding_images[i]; - - ImGui::InputInt((std::string("image[") + std::to_string(i) + std::string("]")).c_str(), &corresponding_images[i]); - if (corresponding_images[i] < 0) - { - corresponding_images[i] = 0; - } - if (corresponding_images[i] >= images_file_names.size() - 1) - { - corresponding_images[i] = images_file_names.size() - 1; - } - - if (prev != corresponding_images[i]) - { - namespace SD = SystemData; - SD::imageData = stbi_load( - images_file_names[corresponding_images[i]].c_str(), &SD::imageWidth, &SD::imageHeight, &SD::imageNrChannels, 0); - std::cout << "imageWidth: " << SD::imageWidth << std::endl; - std::cout << "imageHeight: " << SD::imageHeight << std::endl; - std::cout << "imageNrChannels: " << SD::imageNrChannels << std::endl; - - /////////////// - // std::vector newCloud(session.point_clouds_container.point_clouds[i]..size()); - - session.point_clouds_container.point_clouds[i].colors.resize( - session.point_clouds_container.point_clouds[i].points_local.size()); - for (auto& c : session.point_clouds_container.point_clouds[i].colors) - { - c.x() = c.y() = c.z() = 0.0; - } - - Eigen::Affine3d transfom = SystemData::camera_pose; // * pc.local_trajectory[offsets[i]].m_pose.inverse(); - - std::vector pointsRGB; - - for (int p = 0; p < session.point_clouds_container.point_clouds[i].points_local.size(); p++) - { - mandeye::PointRGB point; - point.point = session.point_clouds_container.point_clouds[i].points_local[p]; - // point.point = pc.local_trajectory[0].m_pose.inverse() * point.point; - // point.point = pc.local_trajectory[offsets[i]].m_pose * point.point; - point.rgb = { 0.f, 0.f, 0.f, 1.f }; - pointsRGB.push_back(point); - } - - std::vector pc = ApplyColorToPointcloud( - pointsRGB, SD::imageData, SD::imageWidth, SD::imageHeight, SD::imageNrChannels, transfom); - - for (int color_idx = 0; color_idx < pc.size(); color_idx++) - { - session.point_clouds_container.point_clouds[i].colors[color_idx].x() = pc[color_idx].rgb.x(); - session.point_clouds_container.point_clouds[i].colors[color_idx].y() = pc[color_idx].rgb.y(); - session.point_clouds_container.point_clouds[i].colors[color_idx].z() = pc[color_idx].rgb.z(); - } - - session.point_clouds_container.point_clouds[i].show_color = true; - } - - // ImGui::InputInt((std::string("trajectory offset[") + std::to_string(i) + std::string("]")).c_str(), &offsets[i]); - // if (offsets[i] < 0) - //{ - // offsets[i] = 0; - //} - // if (offsets[i] >= pc.local_trajectory.size() - 1) - //{ - // offsets[i] = pc.local_trajectory.size() - 1; - //} - - /* - if (ImGui::Button(("colorize with '" + images_file_names[corresponding_images[i]] + "'").c_str())) - { - // - // std::vector points_local; - // std::vector normal_vectors_local; - // std::vector colors; - // - - namespace SD = SystemData; - SD::imageData = stbi_load(images_file_names[corresponding_images[i]].c_str(), &SD::imageWidth, &SD::imageHeight, - &SD::imageNrChannels, 0); std::cout << "imageWidth: " << SD::imageWidth << std::endl; std::cout << "imageHeight: " << - SD::imageHeight << std::endl; std::cout << "imageNrChannels: " << SD::imageNrChannels << std::endl; - - /////////////// - // std::vector newCloud(session.point_clouds_container.point_clouds[i]..size()); - - session.point_clouds_container.point_clouds[i].colors.resize(session.point_clouds_container.point_clouds[i].points_local.size()); - for (auto &c : session.point_clouds_container.point_clouds[i].colors) - { - c.x() = c.y() = c.z() = 0.0; - } - - Eigen::Affine3d transfom = SystemData::camera_pose;// * pc.local_trajectory[offsets[i]].m_pose.inverse(); - - std::vector pointsRGB; - - for (int p = 0; p < session.point_clouds_container.point_clouds[i].points_local.size(); p++) - { - mandeye::PointRGB point; - point.point = session.point_clouds_container.point_clouds[i].points_local[p]; - //point.point = pc.local_trajectory[0].m_pose.inverse() * point.point; - //point.point = pc.local_trajectory[offsets[i]].m_pose * point.point; - point.rgb = {0.f, 0.f, 0.f, 1.f}; - pointsRGB.push_back(point); - } - - std::vector - pc = ApplyColorToPointcloud(pointsRGB, SD::imageData, SD::imageWidth, SD::imageHeight, SD::imageNrChannels, - transfom); - - for (int color_idx = 0; color_idx < pc.size(); color_idx++) - { - session.point_clouds_container.point_clouds[i].colors[color_idx].x() = pc[color_idx].rgb.x(); - session.point_clouds_container.point_clouds[i].colors[color_idx].y() = pc[color_idx].rgb.y(); - session.point_clouds_container.point_clouds[i].colors[color_idx].z() = pc[color_idx].rgb.z(); - } - - session.point_clouds_container.point_clouds[i].show_color = true; - // return newCloud; - /////////////// - }*/ - } - } - - // ImGui::Checkbox(session.point_clouds_container.point_clouds[i].file_name.c_str(), - // &session.point_clouds_container.point_clouds[i].visible); -#if 0 - for (size_t i = 0; i < session.point_clouds_container.point_clouds.size(); i++) - { - ImGui::Separator(); - ImGui::Checkbox(session.point_clouds_container.point_clouds[i].file_name.c_str(), &session.point_clouds_container.point_clouds[i].visible); - // ImGui::SameLine(); - ImGui::Text("--"); - ImGui::SameLine(); - ImGui::Checkbox((std::string("gizmo_") + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].gizmo); - ImGui::SameLine(); - ImGui::Checkbox((std::string("fixed_") + std::to_string(i)).c_str(), &session.point_clouds_container.point_clouds[i].fixed); - ImGui::SameLine(); - ImGui::PushButtonRepeat(true); - float spacing = ImGui::GetStyle().ItemInnerSpacing.x; - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##left").c_str(), ImGuiDir_Left)) - { - (session.point_clouds_container.point_clouds[i].point_size)--; - } - ImGui::SameLine(0.0f, spacing); - if (ImGui::ArrowButton(("[" + std::to_string(i) + "] ##right").c_str(), ImGuiDir_Right)) - { - (session.point_clouds_container.point_clouds[i].point_size)++; - } - ImGui::PopButtonRepeat(); - ImGui::SameLine(); - ImGui::Text("point size %d", session.point_clouds_container.point_clouds[i].point_size); - if (session.point_clouds_container.point_clouds[i].point_size < 1) - { - session.point_clouds_container.point_clouds[i].point_size = 1; - } - - ImGui::SameLine(); - if (ImGui::Button(std::string("#" + std::to_string(i) + " save scan(global reference frame)").c_str())) - { - const auto output_file_name = mandeye::fd::SaveFileDialog("Choose folder", {}); - std::cout << "Scan file to save: '" << output_file_name << "'" << std::endl; - if (output_file_name.size() > 0) - { - session.point_clouds_container.point_clouds[i].save_as_global(output_file_name); - } - } - ImGui::SameLine(); - if (ImGui::Button(std::string("#" + std::to_string(i) + " shift points to center").c_str())) - { - session.point_clouds_container.point_clouds[i].shift_to_center(); - } - if (session.point_clouds_container.point_clouds[i].gizmo) - { - for (size_t j = 0; j < session.point_clouds_container.point_clouds.size(); j++) - { - if (i != j) - { - session.point_clouds_container.point_clouds[j].gizmo = false; - } - } - m_gizmo[0] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 0); - m_gizmo[1] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 0); - m_gizmo[2] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 0); - m_gizmo[3] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 0); - m_gizmo[4] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 1); - m_gizmo[5] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 1); - m_gizmo[6] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 1); - m_gizmo[7] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 1); - m_gizmo[8] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 2); - m_gizmo[9] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 2); - m_gizmo[10] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 2); - m_gizmo[11] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 2); - m_gizmo[12] = (float)session.point_clouds_container.point_clouds[i].m_pose(0, 3); - m_gizmo[13] = (float)session.point_clouds_container.point_clouds[i].m_pose(1, 3); - m_gizmo[14] = (float)session.point_clouds_container.point_clouds[i].m_pose(2, 3); - m_gizmo[15] = (float)session.point_clouds_container.point_clouds[i].m_pose(3, 3); - } - - if (session.point_clouds_container.point_clouds[i].visible) - { - ImGui::Text("--"); - ImGui::SameLine(); - ImGui::Checkbox(std::string(std::to_string(i) + ": show_color").c_str(), &session.point_clouds_container.point_clouds[i].show_color); // - - if (!session.point_clouds_container.point_clouds[i].show_color) - { - ImGui::SameLine(); - ImGui::ColorEdit3(std::string(std::to_string(i) + ": pc_color").c_str(), session.point_clouds_container.point_clouds[i].render_color); - } - - ImGui::SameLine(); - if (ImGui::Button(std::string("#" + std::to_string(i) + "_ICP").c_str())) - { - size_t index_target = i; - PointClouds pcs; - for (size_t k = 0; k < index_target; k++) - { - if (session.point_clouds_container.point_clouds[k].visible) - { - pcs.point_clouds.push_back(session.point_clouds_container.point_clouds[k]); - } - } - - if (pcs.point_clouds.size() > 0) - { - for (size_t k = 0; k < pcs.point_clouds.size(); k++) - { - pcs.point_clouds[k].fixed = true; - } - } - pcs.point_clouds.push_back(session.point_clouds_container.point_clouds[index_target]); - pcs.point_clouds[pcs.point_clouds.size() - 1].fixed = false; - - ICP icp; - icp.search_radious = 0.3; // ToDo move to params - for (auto &pc : pcs.point_clouds) - { - pc.rgd_params.resolution_X = icp.search_radious; - pc.rgd_params.resolution_Y = icp.search_radious; - pc.rgd_params.resolution_Z = icp.search_radious; - - pc.build_rgd(); - pc.cout_rgd(); - pc.compute_normal_vectors(0.5); - } - - icp.number_of_threads = std::thread::hardware_concurrency(); - - icp.number_of_iterations = 10; - icp.is_adaptive_robust_kernel = false; - - icp.is_ballanced_horizontal_vs_vertical = false; - icp.is_fix_first_node = false; - icp.is_gauss_newton = true; - icp.is_levenberg_marguardt = false; - icp.is_cw = false; - icp.is_wc = true; - icp.is_tait_bryan_angles = true; - icp.is_quaternion = false; - icp.is_rodrigues = false; - std::cout << "optimization_point_to_point_source_to_target" << std::endl; - - icp.optimization_point_to_point_source_to_target(pcs); - - std::cout << "pose before: " << session.point_clouds_container.point_clouds[index_target].m_pose.matrix() << std::endl; - - std::vector all_m_poses; - for (int j = 0; j < session.point_clouds_container.point_clouds.size(); j++) - { - all_m_poses.push_back(session.point_clouds_container.point_clouds[j].m_pose); - } - - session.point_clouds_container.point_clouds[index_target].m_pose = pcs.point_clouds[pcs.point_clouds.size() - 1].m_pose; - - std::cout << "pose after ICP: " << session.point_clouds_container.point_clouds[index_target].m_pose.matrix() << std::endl; - - // like gizmo - if (!manipulate_only_marked_gizmo) - { - std::cout << "update all poses after current pose" << std::endl; - - Eigen::Affine3d curr_m_pose = session.point_clouds_container.point_clouds[index_target].m_pose; - for (int j = index_target + 1; j < session.point_clouds_container.point_clouds.size(); j++) - { - curr_m_pose = curr_m_pose * (all_m_poses[j - 1].inverse() * all_m_poses[j]); - session.point_clouds_container.point_clouds[j].m_pose = curr_m_pose; - } - } - } - } - ImGui::SameLine(); - if (ImGui::Button(std::string("#" + std::to_string(i) + " print frame to console").c_str())) - { - std::cout << session.point_clouds_container.point_clouds[i].m_pose.matrix() << std::endl; - } - - ImGui::SameLine(); - ImGui::Checkbox(std::string("#" + std::to_string(i) + " fuse inclination from IMU").c_str(), &session.point_clouds_container.point_clouds[i].fuse_inclination_from_IMU); - - } -#endif - } - - /* - ImGui::InputInt("point_size", &point_size); - if (point_size < 1) - { - point_size = 1; - } - - - - if (session.point_clouds_container.point_clouds.size() > 0) - { - ImGui::InputFloat("offset_intensity", &offset_intensity, 0.01, 0.1); - if (offset_intensity < 0) - { - offset_intensity = 0; - } - if (offset_intensity > 1) - { - offset_intensity = 1; - } - - ImGui::Checkbox("show_neighbouring_scans", &show_neighbouring_scans); - - if (show_neighbouring_scans) - { - ImGui::ColorEdit3("pc_neigbouring_color", (float *)&pc_neigbouring_color); - } - - ImGui::Text("----------- navigate with index_rendered_points_local ---------"); - - ImGui::InputInt("index_rendered_points_local", &index_rendered_points_local, 1, 10); - if (index_rendered_points_local < 0) - { - index_rendered_points_local = 0; - } - if (index_rendered_points_local >= session.point_clouds_container.point_clouds.size() - 1) - { - index_rendered_points_local = session.point_clouds_container.point_clouds.size() - 1; - } - - ImGui::Text(session.point_clouds_container.point_clouds[index_rendered_points_local].file_name.c_str()); - - double ts = session.point_clouds_container.point_clouds[index_rendered_points_local].timestamps[0] / 1e9; - ImGui::Text((std::string("ts: ") + std::to_string(ts)).c_str()); - } - */ - ImGui::End(); - } - return; -} - -void display() -{ - ImGuiIO& io = ImGui::GetIO(); - glViewport(0, 0, (GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - float ratio = float(io.DisplaySize.x) / float(io.DisplaySize.y); - - if (is_ortho) - { - glOrtho( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100000, - 100000); - - glm::mat4 proj = glm::orthoLH_ZO( - -camera_ortho_xy_view_zoom, - camera_ortho_xy_view_zoom, - -camera_ortho_xy_view_zoom / ratio, - camera_ortho_xy_view_zoom / ratio, - -100, - 100); - - std::copy(&proj[0][0], &proj[3][3], m_ortho_projection); - - Eigen::Vector3d v_eye_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h + 10); - Eigen::Vector3d v_center_t(-camera_ortho_xy_view_shift_x, camera_ortho_xy_view_shift_y, camera_mode_ortho_z_center_h); - Eigen::Vector3d v(0, 1, 0); - - TaitBryanPose pose_tb; - pose_tb.px = 0.0; - pose_tb.py = 0.0; - pose_tb.pz = 0.0; - pose_tb.om = 0.0; - pose_tb.fi = 0.0; - pose_tb.ka = -camera_ortho_xy_view_rotation_angle_deg * M_PI / 180.0; - auto m = affine_matrix_from_pose_tait_bryan(pose_tb); - - Eigen::Vector3d v_t = m * v; - - gluLookAt(v_eye_t.x(), v_eye_t.y(), v_eye_t.z(), v_center_t.x(), v_center_t.y(), v_center_t.z(), v_t.x(), v_t.y(), v_t.z()); - glm::mat4 lookat = glm::lookAt( - glm::vec3(v_eye_t.x(), v_eye_t.y(), v_eye_t.z()), - glm::vec3(v_center_t.x(), v_center_t.y(), v_center_t.z()), - glm::vec3(v_t.x(), v_t.y(), v_t.z())); - std::copy(&lookat[0][0], &lookat[3][3], m_ortho_gizmo_view); - } - - glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - glEnable(GL_DEPTH_TEST); - - if (!is_ortho) - { - reshape((GLsizei)io.DisplaySize.x, (GLsizei)io.DisplaySize.y); - - Eigen::Affine3f viewTranslation = Eigen::Affine3f::Identity(); - viewTranslation.translate(rotation_center); - Eigen::Affine3f viewLocal = Eigen::Affine3f::Identity(); - viewLocal.translate(Eigen::Vector3f(translate_x, translate_y, translate_z)); - viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_x / 180.f, Eigen::Vector3f::UnitX())); - viewLocal.rotate(Eigen::AngleAxisf(M_PI * rotate_y / 180.f, Eigen::Vector3f::UnitZ())); - - Eigen::Affine3f viewTranslation2 = Eigen::Affine3f::Identity(); - viewTranslation2.translate(-rotation_center); - - Eigen::Affine3f result = viewTranslation * viewLocal * viewTranslation2; - - glLoadMatrixf(result.matrix().data()); - /* glTranslatef(translate_x, translate_y, translate_z); - glRotatef(rotate_x, 1.0, 0.0, 0.0); - glRotatef(rotate_y, 0.0, 0.0, 1.0);*/ - } - else - { - glMatrixMode(GL_MODELVIEW); - glLoadIdentity(); - } - - if (show_axes) - { - glBegin(GL_LINES); - glColor3f(1.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(10, 0.0f, 0.0f); - - glColor3f(0.0f, 1.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 10, 0.0f); - - glColor3f(0.0f, 0.0f, 1.0f); - glVertex3f(0.0f, 0.0f, 0.0f); - glVertex3f(0.0f, 0.0f, 10); - glEnd(); - } - - ObservationPicking observation_picking; - observation_picking.point_size = SystemData::point_size; - - for (int k = 0; k < session.point_clouds_container.point_clouds.size(); k++) - { - session.point_clouds_container.point_clouds[k].point_size = SystemData::point_size; - } - - session.point_clouds_container.render(observation_picking, decimation_step, 1); - // session.ground_control_points.render(session.point_clouds_container); - - /*session.point_clouds_container.render({}, {}, session.point_clouds_container.xz_intersection, - session.point_clouds_container.yz_intersection, session.point_clouds_container.xy_intersection, - session.point_clouds_container.xz_grid_10x10, session.point_clouds_container.xz_grid_1x1, - session.point_clouds_container.xz_grid_01x01, session.point_clouds_container.yz_grid_10x10, - session.point_clouds_container.yz_grid_1x1, session.point_clouds_container.yz_grid_01x01, - session.point_clouds_container.xy_grid_10x10, session.point_clouds_container.xy_grid_1x1, - session.point_clouds_container.xy_grid_01x01, session.point_clouds_container.intersection_width);*/ - - /*if (ImGui::GetIO().KeyCtrl) - { - glBegin(GL_LINES); - glColor3f(1.f, 1.f, 1.f); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x() + 1.f, rotation_center.y(), rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x() - 1.f, rotation_center.y(), rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y() - 1.f, rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y() + 1.f, rotation_center.z()); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() - 1.f); - glVertex3fv(rotation_center.data()); - glVertex3f(rotation_center.x(), rotation_center.y(), rotation_center.z() + 1.f); - glEnd(); - } - - */ - -#if 0 - if (index_rendered_points_local >= 0 && index_rendered_points_local < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size()) - { - double max_intensity = 0.0; - for (int i = 0; i < session.point_clouds_container.point_clouds[index_rendered_points_local].intensities.size(); i++) - { - if (session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i] > max_intensity) - { - max_intensity = session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i]; - } - } - - Eigen::Affine3d pose = session.point_clouds_container.point_clouds[index_rendered_points_local].m_pose; - pose(0, 3) = 0.0; - pose(1, 3) = 0.0; - pose(2, 3) = 0.0; - - glBegin(GL_POINTS); - for (int i = 0; i < session.point_clouds_container.point_clouds[index_rendered_points_local].points_local.size(); i++) - { - glColor3f(session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i] / max_intensity + offset_intensity, 0.0, 1.0 - session.point_clouds_container.point_clouds[index_rendered_points_local].intensities[i] / max_intensity + offset_intensity); - - Eigen::Vector3d p(session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[i].x(), - session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[i].y(), - session.point_clouds_container.point_clouds[index_rendered_points_local].points_local[i].z()); - p = pose * p; - glVertex3f(p.x(), p.y(), p.z()); - } - glEnd(); - - if (show_neighbouring_scans) - { - // pc_neigbouring_color - - glColor3f(pc_neigbouring_color.x, pc_neigbouring_color.y, pc_neigbouring_color.z); - - glBegin(GL_POINTS); - for (int index = index_rendered_points_local - 20; index <= index_rendered_points_local + 20; index += 5) - { - if (index != index_rendered_points_local) - { - if (index >= 0 && index < session.point_clouds_container.point_clouds.size()) - { - Eigen::Affine3d pose = session.point_clouds_container.point_clouds[index].m_pose; - Eigen::Affine3d pose_offset = session.point_clouds_container.point_clouds[index_rendered_points_local].m_pose; - - pose(0, 3) -= pose_offset(0, 3); - pose(1, 3) -= pose_offset(1, 3); - pose(2, 3) -= pose_offset(2, 3); - - for (int i = 0; i < session.point_clouds_container.point_clouds[index].points_local.size(); i++) - { - Eigen::Vector3d p(session.point_clouds_container.point_clouds[index].points_local[i].x(), - session.point_clouds_container.point_clouds[index].points_local[i].y(), - session.point_clouds_container.point_clouds[index].points_local[i].z()); - p = pose * p; - glVertex3f(p.x(), p.y(), p.z()); - } - } - } - } - glEnd(); - } - } -#endif - - ImGui_ImplOpenGL2_NewFrame(); - ImGui_ImplGLUT_NewFrame(); - ImGui::NewFrame(); - - project_gui(); - - ImGui::Render(); - ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); - - glutSwapBuffers(); - glutPostRedisplay(); -} - -void wheel(int button, int dir, int x, int y) -{ - if (dir > 0) - { - if (is_ortho) - { - camera_ortho_xy_view_zoom -= 0.1f * camera_ortho_xy_view_zoom; - - if (camera_ortho_xy_view_zoom < 0.1) - { - camera_ortho_xy_view_zoom = 0.1; - } - } - else - { - translate_z -= 0.05f * translate_z; - } - } - else - { - if (is_ortho) - { - camera_ortho_xy_view_zoom += 0.1 * camera_ortho_xy_view_zoom; - } - else - { - translate_z += 0.05f * translate_z; - } - } - - return; -} - -void mouse(int glut_button, int state, int x, int y) -{ - ImGuiIO& io = ImGui::GetIO(); - io.MousePos = ImVec2((float)x, (float)y); - int button = -1; - if (glut_button == GLUT_LEFT_BUTTON) - button = 0; - if (glut_button == GLUT_RIGHT_BUTTON) - button = 1; - if (glut_button == GLUT_MIDDLE_BUTTON) - button = 2; - if (button != -1 && state == GLUT_DOWN) - io.MouseDown[button] = true; - if (button != -1 && state == GLUT_UP) - io.MouseDown[button] = false; - - static int glutMajorVersion = glutGet(GLUT_VERSION) / 10000; - if (state == GLUT_DOWN && (glut_button == 3 || glut_button == 4) && glutMajorVersion < 3) - { - wheel(glut_button, glut_button == 3 ? 1 : -1, x, y); - } - - if (!io.WantCaptureMouse) - { - if (glut_button == GLUT_MIDDLE_BUTTON && state == GLUT_DOWN && io.KeyCtrl) - { - } - - if (state == GLUT_DOWN) - { - mouse_buttons |= 1 << glut_button; - } - else if (state == GLUT_UP) - { - mouse_buttons = 0; - } - mouse_old_x = x; - mouse_old_y = y; - } -} - -bool initGL(int* argc, char** argv) -{ - glutInit(argc, argv); - glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); - glutInitWindowSize(window_width, window_height); - glutCreateWindow("single_session_manual_coloring " HDMAPPING_VERSION_STRING); - glutDisplayFunc(display); - glutMotionFunc(motion); - - // default initialization - glClearColor(0.0, 0.0, 0.0, 1.0); - glEnable(GL_DEPTH_TEST); - - // viewport - glViewport(0, 0, window_width, window_height); - - // projection - glMatrixMode(GL_PROJECTION); - glLoadIdentity(); - gluPerspective(60.0, (GLfloat)window_width / (GLfloat)window_height, 0.01, 10000.0); - glutReshapeFunc(reshape); - ImGui::CreateContext(); - ImGuiIO& io = ImGui::GetIO(); - (void)io; - // io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls - - ImGui::StyleColorsDark(); - ImGui_ImplGLUT_Init(); - ImGui_ImplGLUT_InstallFuncs(); - ImGui_ImplOpenGL2_Init(); - return true; -} - -int main(int argc, char* argv[]) -{ - initGL(&argc, argv); - glutDisplayFunc(display); - glutMouseFunc(mouse); - glutMotionFunc(motion); - glutMouseWheelFunc(wheel); - glutMainLoop(); - - ImGui_ImplOpenGL2_Shutdown(); - ImGui_ImplGLUT_Shutdown(); - - ImGui::DestroyContext(); - return 0; -} \ No newline at end of file diff --git a/calib_core/CMakeLists.txt b/calib_core/CMakeLists.txt index 99397ebc..4a98a14a 100644 --- a/calib_core/CMakeLists.txt +++ b/calib_core/CMakeLists.txt @@ -18,12 +18,30 @@ project(calib_core) # calib_core and core/core_raylib in the same binary. add_library(calib_core STATIC src/Camera.cpp + src/CameraInfoYaml.cpp src/PointCloud.cpp src/Trajectory.cpp src/CliArgs.cpp src/CameraCalibrationSolver.cpp + src/CameraCalibrationSolverCeres.cpp ) +# ── Optional Ceres-based Mei/fisheye extrinsics solver ──────────────────────── +# OFF by default: HDMapping otherwise depends on nothing but Eigen for its +# optimization. Built without it, solveExtrinsicsCeres() returns false and +# explains why, so callers need no #ifdef of their own. Enable with +# cmake -DCALIB_ENABLE_CERES=ON (needs libceres-dev or equivalent). +option(CALIB_ENABLE_CERES "Enable the Ceres-based extrinsics solver for the Mei and fisheye camera models" OFF) +if(CALIB_ENABLE_CERES) + find_package(Ceres REQUIRED) + # PUBLIC so calib_core_tests can #ifdef on it to build the real-solve + # test only when it can actually run. + target_compile_definitions(calib_core PUBLIC CALIB_ENABLE_CERES) + message(STATUS "calib_core Mei/fisheye Ceres solver: ENABLED") +else() + message(STATUS "calib_core Mei/fisheye Ceres solver: disabled (set -DCALIB_ENABLE_CERES=ON to enable)") +endif() + target_include_directories(calib_core PUBLIC include ) @@ -50,10 +68,19 @@ target_include_directories(calib_core PRIVATE # doesn't violate calib_core's no-raylib/imgui/OpenCV rule above, and # nothing here links the core/core_math library, just includes headers. ${REPOSITORY_DIRECTORY}/core/include + # PointCloud.cpp uses nlohmann::json for metadata I/O; header-only, same + # bundled copy core/CMakeLists.txt already exposes to its own targets. + ${THIRDPARTY_DIRECTORY}/json/include ) target_link_libraries(calib_core PUBLIC ${PLATFORM_LASZIP_LIB}) +if(CALIB_ENABLE_CERES) + # PRIVATE: CameraCalibrationSolver.h never exposes a Ceres type, so + # consumers need the symbols at link time but not Ceres' include dirs. + target_link_libraries(calib_core PRIVATE Ceres::ceres) +endif() + if(MSVC) target_compile_options(calib_core PRIVATE /W4) target_compile_definitions(calib_core PRIVATE _USE_MATH_DEFINES LASZIP_API_VERSION) diff --git a/calib_core/include/CalibCore/Camera.h b/calib_core/include/CalibCore/Camera.h index 65d3de88..997121d6 100644 --- a/calib_core/include/CalibCore/Camera.h +++ b/calib_core/include/CalibCore/Camera.h @@ -2,103 +2,230 @@ #include #include #include +#include +#include namespace calib { + //! Which projection @ref projectPoint applies. Selected by a "model" key + //! in the calibration JSON; absent, it is Pinhole. + enum class CameraModel + { + Pinhole, // fx/fy/cx/cy + the rational distortion coefficients below + Mei, // Insta 360 + Fisheye // OpenCV cv::fisheye (equidistant); fx/fy/cx/cy + k1..k4 + }; + struct Intrinsics { + CameraModel model = CameraModel::Pinhole; float fx = 800.f, fy = 800.f; float cx = 640.f, cy = 360.f; - // OpenCV rational distortion model: - // radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) + //! OpenCV rational distortion model (CameraModel::Pinhole): + //! radial = (1 + k1 r² + k2 r⁴ + k3 r⁶) / (1 + k4 r² + k5 r⁴ + k6 r⁶) + //! @note CameraModel::Mei reuses k1/k2/k3 and p1/p2 for its own + //! (non-rational) polynomial and leaves k4/k5/k6 unused -- it has + //! no rational denominator. + //! @note CameraModel::Fisheye reuses k1..k4 as OpenCV's fisheye + //! coefficients on the incidence angle theta: + //! theta_d = theta (1 + k1 theta² + k2 theta⁴ + k3 theta⁶ + k4 theta⁸). + //! k5/k6 and p1/p2 are unused. float k1 = 0.f, k2 = 0.f, k3 = 0.f; float k4 = 0.f, k5 = 0.f, k6 = 0.f; - // tangential + //! Tangential distortion. float p1 = 0.f, p2 = 0.f; + //! Unified-sphere mirror parameter, CameraModel::Mei only. + //! @see loadCameraInfoYaml + float xi = 0.f; + //! Image size in pixels. Not read by @ref projectPoint; set by + //! @ref loadCameraInfoYaml and scaled by @ref scaleIntrinsics. + int width = 0, height = 0; + }; + + + struct CameraIdentity + { + + std::string serial; + std::string frameId; + std::string model; + std::string firmware; + bool empty() const + { + return serial.empty() && frameId.empty(); + } }; - // Minimum distance (degrees) fi is kept away from the om/fi/ka - // parameterization's gimbal-lock points (fi = +/-90 deg), where om and - // ka become individually non-unique (only om+ka, or om-ka, is - // determined) and CameraCalibrationSolver's normal equations go - // rank-deficient in that 2x2 block. Used by UI code that edits fi - // interactively (see apps/camera_lidar_calibration/UI.cpp's - // avoidGimbalLock) so a manual drag can't land exactly on the - // singularity. Extrinsics' own default (below) no longer needs this -- - // see kCameraLidarAxisOffset -- but it's kept as a cheap safety net for - // whatever fi a user or a loaded file lands on. + //! Name of a camera model, as written to the calibration JSON's "model" key. + //! @param m model to name + //! @return one of "pinhole", "mei", "fisheye" + const char* modelToString(CameraModel m); + + //! Camera model named by a calibration JSON's "model" key. + //! @param s model name, as written by @ref modelToString; "equidistant" + //! (the ROS/Kalibr name) is accepted for CameraModel::Fisheye too + //! @return the named model, or CameraModel::Pinhole for anything + //! unrecognized (including an absent key) + CameraModel modelFromString(const std::string& s); + + //! Largest incidence angle (radians from the optical axis) at which + //! CameraModel::Fisheye's theta -> theta_d polynomial is still increasing. + //! Past it the image radius shrinks again and far-off-axis directions + //! would fold back onto valid pixels, so @ref projectPoint rejects them. + //! @param K intrinsics; only k1..k4 are read + //! @return an angle in (0, pi]; pi when the polynomial is monotonic over + //! the whole sphere + float fisheyeMaxTheta(const Intrinsics& K); + + //! Minimum distance (degrees) fi is kept away from the om/fi/ka + //! parameterization's gimbal-lock points (fi = +/-90 deg), where om and ka + //! become individually non-unique (only om+ka, or om-ka, is determined) + //! and CameraCalibrationSolver's normal equations go rank-deficient in + //! that 2x2 block. Used by UI code that edits fi interactively so a manual + //! drag can't land exactly on the singularity. + //! @note @ref Extrinsics' own default no longer needs this -- see + //! kCameraLidarAxisOffset -- but it is kept as a cheap safety net + //! for whatever fi a user or a loaded file lands on. constexpr float kGimbalLockEpsilonDeg = 0.1f; - // Nudges fi_deg off the nearest gimbal-lock point (+/-90 deg) if it's - // within kGimbalLockEpsilonDeg of one, in place. A no-op otherwise. - // Safe to call unconditionally every frame after any edit to fi (manual - // slider drag, typed value, or loaded from a file) -- idempotent. + //! Nudges fi_deg off the nearest gimbal-lock point (+/-90 deg) if it is + //! within @ref kGimbalLockEpsilonDeg of one. A no-op otherwise. + //! @param fi_deg angle to adjust, in place + //! @note Idempotent, so it is safe to call unconditionally every frame + //! after any edit to fi (slider drag, typed value, or file load). void avoidGimbalLock(float& fi_deg); - // Fixed rotation baked into Extrinsics' om/fi/ka (see below): the - // "camera axes vs LiDAR axes" alignment -- camera X=right, Y=down, - // Z=forward matched to LiDAR X=forward, Y=left, Z=up. This is a - // constant coordinate-convention twist that has nothing to do with the - // actual calibration being solved for, so it's factored out as a fixed - // offset rather than folded into om/fi/ka: om=fi=ka=0 is then already - // the correct nominal alignment (Extrinsics' literal default), and - // om/fi/ka become exactly "how far off nominal the real mount is" -- - // normally a few degrees at most, so nowhere near the om/fi/ka - // parameterization's gimbal-lock points (fi=+/-90 deg) in practice, - // unlike the old scheme where fi had to carry this entire 90-degree - // twist directly and sat right on top of the singularity by default. + //! Fixed rotation baked into @ref Extrinsics' om/fi/ka: the "camera axes + //! vs LiDAR axes" alignment -- camera X=right, Y=down, Z=forward matched + //! to LiDAR X=forward, Y=left, Z=up. + //! @note This constant coordinate-convention twist has nothing to do with + //! the calibration being solved for, so it is factored out rather + //! than folded into om/fi/ka. om=fi=ka=0 is then already the correct + //! nominal alignment, and om/fi/ka become exactly "how far off + //! nominal the real mount is" -- a few degrees at most, so nowhere + //! near fi=+/-90 deg in practice, unlike the old scheme where fi + //! carried the whole 90-degree twist and sat on the singularity. inline const Eigen::Matrix3f kCameraLidarAxisOffset = (Eigen::Matrix3f() << 0.f, 0.f, 1.f, -1.f, 0.f, 0.f, 0.f, -1.f, 0.f).finished(); struct Extrinsics { - // Camera position in LiDAR/world frame + //! Camera position in the LiDAR/world frame. float tx = 0.f, ty = 0.f, tz = 0.f; - // Camera orientation in LiDAR/world frame, as a SMALL deviation from - // the fixed kCameraLidarAxisOffset alignment: R_wc = - // kCameraLidarAxisOffset * Rx(om) * Ry(fi) * Rz(ka). om/fi/ka are - // degrees, Tait-Bryan, matching CameraCalibrationSolver's own - // parameterization (om/fi/ka feed the vendored observation - // equations directly there too -- see CameraCalibrationSolver.cpp - // for how the offset is threaded through the solve without - // modifying those equations). - // Default: om=fi=ka=0, i.e. exactly the nominal alignment -- a - // real calibration only needs to move these by however far the - // actual camera mount deviates from nominal, typically a few - // degrees, so "0,0,0" is already a good initial guess, not just a - // mathematically convenient one. + //! Camera orientation in the LiDAR/world frame, as a SMALL deviation + //! from the fixed kCameraLidarAxisOffset alignment: + //! R_wc = kCameraLidarAxisOffset * Rx(om) * Ry(fi) * Rz(ka). Degrees, + //! Tait-Bryan, matching CameraCalibrationSolver's parameterization. + //! @note Default om=fi=ka=0 is exactly the nominal alignment, so a real + //! calibration only moves these by however far the mount deviates + //! from nominal -- typically a few degrees. "0,0,0" is therefore + //! a good initial guess, not just a convenient one. float om = 0.f, fi = 0.f, ka = 0.f; }; - // Rectangular region of interest, in full-resolution image pixels. - // When enabled, only pixels inside [x, x+w) x [y, y+h) are considered valid - // (e.g. for coloring a point cloud); everything outside is ignored. + //! Rectangular region of interest, in full-resolution image pixels. + //! When enabled, only pixels inside [x, x+w) x [y, y+h) are considered + //! valid (e.g. for coloring a point cloud); everything outside is ignored. struct Roi { bool enabled = false; int x = 0, y = 0, w = 0, h = 0; }; - // R = kCameraLidarAxisOffset * Rx * Ry * Rz (Tait-Bryan om/fi/ka, - // degrees → rotation matrix). Matches Extrinsics' own om/fi/ka - // convention above -- om=fi=ka=0 returns kCameraLidarAxisOffset exactly. + //! R = kCameraLidarAxisOffset * Rx * Ry * Rz (Tait-Bryan om/fi/ka). + //! Matches @ref Extrinsics' own om/fi/ka convention. + //! @param om_deg,fi_deg,ka_deg Tait-Bryan angles in degrees + //! @return the rotation matrix; om=fi=ka=0 returns kCameraLidarAxisOffset + //! exactly Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg); - // Inverse of omFiKaToMat3: decomposes kCameraLidarAxisOffset^T * R - // assuming that equals Rx(om)*Ry(fi)*Rz(ka), for reading a rotation - // matrix (e.g. from a saved calibration file) back into Extrinsics' - // om/fi/ka fields. Calibration files store the rotation as a plain - // matrix (convention-independent, portable to any external tool, and - // knows nothing about kCameraLidarAxisOffset), while the app's own - // UI/solver work in om/fi/ka, so this conversion is needed at the file - // -I/O boundary either way. Result is passed through avoidGimbalLock. + //! Inverse of @ref omFiKaToMat3: decomposes kCameraLidarAxisOffset^T * R + //! assuming that equals Rx(om)*Ry(fi)*Rz(ka), for reading a rotation + //! matrix (e.g. from a saved calibration file) back into @ref Extrinsics' + //! om/fi/ka fields. + //! @param R rotation matrix to decompose + //! @param om_deg,fi_deg,ka_deg receive the Tait-Bryan angles, in degrees, + //! passed through @ref avoidGimbalLock + //! @note Calibration files store the rotation as a plain matrix + //! (convention-independent, portable, and knowing nothing about + //! kCameraLidarAxisOffset) while the UI and solver work in om/fi/ka, + //! so this conversion is needed at the file-I/O boundary either way. void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, float& ka_deg); - // Project a point from LiDAR frame to image pixel (u, v). - // R_wc = camera orientation in world, t = camera position in world. - // depth = z component in camera frame (positive = in front). - // Returns false if depth <= 0 (behind camera). + //! Load intrinsics from a camera_info.yaml in the flat layout + //! insta360-to-images and insta360-test-calib write: top-level `width`, + //! `height`, `fx`, `fy`, `cx`, `cy`, [`xi`,] and a `distortion` flow + //! sequence whose order `distortion_model` sets: + //! - insta360_mei_v2: CameraModel::Mei, (k1, k2, k3, p1, p2), plus `xi` + //! - equidistant or fisheye: CameraModel::Fisheye, (k1, k2, k3, k4) + //! - plumb_bob: CameraModel::Pinhole, (k1, k2, p1, p2, k3) + //! - rational_polynomial: CameraModel::Pinhole, (k1, k2, p1, p2, k3, k4, k5, k6) + //! @param path file to read + //! @param K overwritten with the loaded intrinsics on success, untouched + //! on failure + //! @return false on a missing file, a missing required field, an unknown + //! distortion_model, or an equidistant file without exactly 4 + //! coefficients + //! @warning Mei's order (k1, k2, k3, p1, p2) is NOT OpenCV's pinhole order + //! (k1, k2, p1, p2, k3). The two are easy to mix up, both being + //! five numbers in a row, and doing so produces a + //! plausible-looking but badly wrong reprojection with no crash. + //! @note A file with `xi` and no distortion_model is taken as Mei, and any + //! distortion_model containing "mei" is read as insta360_mei_v2, + //! both with a warning. Failures and warnings go to stderr rather + //! than being thrown -- a malformed file should degrade the app to + //! "no reprojection available", not crash it. + bool loadCameraInfoYaml(const std::string& path, Intrinsics& K); + + //! Reads a camera_info.yaml-shaped file's `serial`, `frame_id` and + //! `model` fields. Opens and scans the file independently of + //! @ref loadCameraInfoYaml -- + //! identity and intrinsics are unrelated concerns read by separate + //! functions, not two jobs of the same one. + //! @param path file to read + //! @param id overwritten on success (cleared first, so a field the file + //! does not name comes back empty rather than kept from a + //! previous load), untouched on failure + //! @return false if the file cannot be opened + bool loadCameraIdentity(const std::string& path, CameraIdentity& id); + + //! The same camera after its images are resampled, so a downscaled image + //! projects with the same geometry. Distortion terms are dimensionless and + //! carry over unchanged. + //! @param K intrinsics at the original resolution + //! @param s resample factor (0.5 = half size) + //! @return intrinsics valid for the resampled image + Intrinsics scaleIntrinsics(const Intrinsics& K, float s); + + //! The same rectangle on a resampled image, so a ROI -- given in + //! full-resolution pixels, see @ref Roi -- can be tested against a + //! downscaled copy. + //! @param r rectangle in full-resolution pixels + //! @param s resample factor (0.5 = half size) + //! @return the scaled rectangle + //! @note An unset (w/h == 0) ROI comes back unchanged, and a set one never + //! collapses to empty, which callers would read as "no ROI". + Roi scaleRoi(const Roi& r, float s); + + //! Project a point from the LiDAR frame to an image pixel, applying + //! whichever model K.model selects. + //! @param px,py,pz point in the LiDAR frame + //! @param K camera intrinsics; K.model picks the projection + //! @param R_wc camera orientation in world + //! @param t camera position in world + //! @param u,v receive the image pixel + //! @param depth receives the camera-frame z for Pinhole, range from the + //! camera for Mei and Fisheye + //! @return false when the point does not project: behind the camera for + //! Pinhole, at the camera itself or past the fold-back angle + //! (where the projection stops being injective) for Mei and + //! Fisheye -- @ref fisheyeMaxTheta gives Fisheye's + //! @note Fisheye takes theta from atan2, not OpenCV's atan(r), so it + //! matches cv::fisheye::projectPoints in front of the camera and + //! still projects directions past 90 deg for lenses wider than 180. + //! @note The caller owns rounding to integer pixels, bounds checking and + //! any ROI test. bool projectPoint( float px, float py, @@ -110,4 +237,12 @@ namespace calib float& v, float& depth); + //! Reads the `FRAME_WALL_CLOCK` field (nanoseconds since epoch) from an + //! image's `.meta.json` sidecar. + //! @param path the image file, e.g. ".../cam0_123.jpg"; the sidecar is + //! the same basename with its extension replaced by ".meta.json" + //! (".../cam0_123.meta.json") + //! @return the timestamp in nanoseconds, or nullopt if the sidecar is + //! missing, unreadable, or has no FRAME_WALL_CLOCK field + std::optional LoadTimestampFromSideCar(const std::string& path); } // namespace calib diff --git a/calib_core/include/CalibCore/CameraCalibrationSolver.h b/calib_core/include/CalibCore/CameraCalibrationSolver.h index d476dad7..34448426 100644 --- a/calib_core/include/CalibCore/CameraCalibrationSolver.h +++ b/calib_core/include/CalibCore/CameraCalibrationSolver.h @@ -1,42 +1,46 @@ #pragma once #include "Camera.h" #include +#include #include namespace calib { - // A single manually-picked correspondence: a 3D point in the LiDAR/world - // frame paired with the pixel it should project to in the camera image. - // Pixel coordinates are expected in the *undistorted* (ideal pinhole) - // frame -- i.e. picked from the rectified image display, see - // solveExtrinsicsFromCorrespondences() below. + //! A single manually-picked correspondence: a 3D point in the LiDAR/world + //! frame paired with the pixel it should project to in the camera image. + //! @note Pixel coordinates are expected in whatever frame the displayed + //! image is in: undistorted/ideal-pinhole for CameraModel::Pinhole + //! (picked from the rectified display), raw/distorted for + //! CameraModel::Mei and CameraModel::Fisheye, whose images are never + //! rectified. struct PointPixelCorrespondence { + //! Point in the LiDAR/world frame. Eigen::Vector3d p; + //! Pixel it should project to. double u = 0.0, v = 0.0; }; - // Solves for the extrinsics (camera position + orientation) that best - // explain the given LiDAR-point <-> image-pixel correspondences via - // damped Gauss-Newton (Levenberg-Marquardt) on the reused observation - // equations. Intrinsics (fx, fy, cx, cy) are held fixed at their - // current values in K. extrinsicsInOut is used as the initial guess and - // is overwritten with the solved result. Pixel coordinates in - // `correspondences` must be in the undistorted/ideal-pinhole frame -- - // i.e. picked from the rectified image display (calib::Intrinsics's - // distortion terms are ignored here). - // - // fixTranslation=true blocks tx/ty/tz from being solved for -- they - // stay pinned at extrinsicsInOut's initial values and only orientation - // (3-DOF) is optimized. Useful when the camera position relative to the - // LiDAR is already known precisely (e.g. measured by hand) and only - // orientation needs refining from the picked pairs. - // - // Returns false (leaving extrinsicsInOut unchanged) if there are fewer - // than 3 correspondences, or fewer than the number of free parameters - // (3 with fixTranslation, else 6), or the normal-equations system is - // singular. + //! Solve for the extrinsics (camera position + orientation) that best + //! explain the given LiDAR-point <-> image-pixel correspondences, via + //! damped Gauss-Newton (Levenberg-Marquardt) on the reused observation + //! equations. + //! @param correspondences picked pairs; pixel coordinates must be in the + //! undistorted/ideal-pinhole frame, i.e. picked from the rectified + //! image display (@ref Intrinsics' distortion terms are ignored) + //! @param K intrinsics, held fixed at fx/fy/cx/cy + //! @param extrinsicsInOut initial guess in, solved result out + //! @param outRmsPixels optionally receives the RMS reprojection error + //! @param fixTranslation pin tx/ty/tz at their initial values and optimize + //! orientation only (3-DOF) -- useful when the camera position + //! relative to the LiDAR is already known precisely + //! @return false, leaving extrinsicsInOut unchanged, for fewer than 3 + //! correspondences, fewer correspondences than free parameters + //! (3 with fixTranslation, else 6), or a singular system + //! @note Pinhole only: the reused observation equations are a pure + //! rectilinear perspective projection, with no distortion and no + //! unified-sphere term. @see solveExtrinsicsCeres bool solveExtrinsicsFromCorrespondences( const std::vector& correspondences, const Intrinsics& K, @@ -44,4 +48,32 @@ namespace calib double* outRmsPixels = nullptr, bool fixTranslation = false); + //! CameraModel::Mei and CameraModel::Fisheye counterpart to + //! @ref solveExtrinsicsFromCorrespondences. No vendored analytic Jacobian + //! exists for either model, so this minimizes reprojection error with + //! Ceres' automatic differentiation, solving the same (tx,ty,tz,om,fi,ka) + //! Extrinsics. + //! @param correspondences picked pairs; pixel coordinates are in the raw + //! (distorted) frame, since neither model's image is rectified + //! @param K intrinsics, held fixed; K.model must be Mei or Fisheye + //! @param extrinsicsInOut initial guess in, solved result out + //! @param errorMessage set on failure, left untouched on success. Required + //! rather than defaulted -- hence its position ahead of the + //! optional parameters -- so a failure reason is never silently + //! dropped + //! @param outRmsPixels optionally receives the RMS reprojection error + //! @param fixTranslation as in @ref solveExtrinsicsFromCorrespondences + //! @return false on failure, with the reason in errorMessage -- including + //! any other K.model + //! @note Needs -DCALIB_ENABLE_CERES=ON (OFF by default). Built without it + //! this always returns false and says so, so callers never need an + //! \#ifdef of their own. + bool solveExtrinsicsCeres( + const std::vector& correspondences, + const Intrinsics& K, + Extrinsics& extrinsicsInOut, + std::string& errorMessage, + double* outRmsPixels = nullptr, + bool fixTranslation = false); + } // namespace calib diff --git a/calib_core/include/CalibCore/CliArgs.h b/calib_core/include/CalibCore/CliArgs.h index 6afeeb42..df48433e 100644 --- a/calib_core/include/CalibCore/CliArgs.h +++ b/calib_core/include/CalibCore/CliArgs.h @@ -5,43 +5,55 @@ namespace calib { -// Shared command-line parsing for all CalibrationApp tools. -// -// Flags are stored generically in a multimap (key = flag name without the -// leading "--"), so the same parser serves every tool and new flags need no -// parser changes. Each tool just reads the keys it cares about and ignores the -// rest. Recognised conventions: -// -// --mjs session manifest file; the session directory is -// its parent folder (parent_path) -// --camera_dir directory of CAMERA_0 images -// --laz [b.laz ...] one or more point clouds (.laz / .las). May be -// repeated; consecutive non-flag tokens after a -// --laz are all taken as clouds. -// -h, --help print usage and exit -// -// A flag may take several values (each consecutive non-flag token becomes its -// own multimap entry) or none (stored once with an empty value). Tokens that -// don't follow a flag are collected into `positional`, preserving the old -// extension/drag-and-drop behaviour. +//! Shared command-line parsing for all CalibrationApp tools. +//! +//! Flags are stored generically in a multimap (key = flag name without the +//! leading "--"), so the same parser serves every tool and new flags need no +//! parser changes. Each tool reads the keys it cares about and ignores the +//! rest. Recognised conventions: +//! +//! --mjs session manifest file; the session +//! directory is its parent folder +//! --camera_dir directory of CAMERA_0 images +//! --laz [b.laz ...] one or more point clouds (.laz / .las); +//! may be repeated +//! -h, --help print usage and exit +//! +//! @note A flag may take several values -- each consecutive non-flag token +//! becomes its own multimap entry -- or none, in which case it is stored +//! once with an empty value. Tokens that don't follow a flag are +//! collected into @ref positional, preserving the old +//! extension/drag-and-drop behaviour. struct CliArgs { - std::multimap opts; // flag -> value(s) - std::vector positional; // non-flag arguments, in order + //! Flag name (without "--") to value(s). + std::multimap opts; + //! Non-flag arguments, in the order given. + std::vector positional; - bool help = false; // -h / --help was given - bool valid = true; // false on a malformed argument - std::string error; // message describing why valid == false + //! -h / --help was given. + bool help = false; + //! False on a malformed argument; see @ref error. + bool valid = true; + //! Message describing why @ref valid is false. + std::string error; - // True if the flag was present at all (even with an empty value). + //! Whether the flag was present at all, even with an empty value. + //! @param key flag name, without the leading "--" + //! @return true when present bool has(const std::string& key) const { return opts.find(key) != opts.end(); } - // First value for `key`, or `def` if absent. + //! First value given for a flag. + //! @param key flag name, without the leading "--" + //! @param def returned when the flag is absent + //! @return the first value, or `def` std::string get(const std::string& key, const std::string& def = {}) const { auto it = opts.find(key); return it == opts.end() ? def : it->second; } - // All values for `key`, in the order given on the command line. + //! Every value given for a flag, in command-line order. + //! @param key flag name, without the leading "--" + //! @return the values, empty when the flag is absent std::vector getAll(const std::string& key) const { std::vector v; auto range = opts.equal_range(key); @@ -50,13 +62,16 @@ struct CliArgs { } }; -// Parse argv. Never terminates the process — the caller inspects `help` and -// `valid` and decides what to do. +//! Parse argv. +//! @param argc,argv as received by main() +//! @return the parsed arguments +//! @note Never terminates the process -- the caller inspects +//! @ref CliArgs::help and @ref CliArgs::valid and decides what to do. CliArgs parseArgs(int argc, char* argv[]); -// Pre-formatted help lines for the shared flags, so every tool describes the -// same flag the same way. An app passes the subset it actually honours to -// printUsage(); the -h/--help line is always added automatically. +//! Pre-formatted help lines for the shared flags, so every tool describes the +//! same flag the same way. An app passes the subset it actually honours to +//! @ref printUsage; the -h/--help line is always added automatically. namespace cliopt { inline constexpr const char* MJS = " --mjs session manifest file; the session\n" @@ -69,9 +84,12 @@ inline constexpr const char* LAZ = " --laz [b.laz ...] one or more point clouds (.laz/.las); may repeat"; } // namespace cliopt -// Print usage for `appName` listing only `options` (e.g. {cliopt::MJS, ...}). -// `desc` is a one-line summary of the tool. Goes to stdout, or stderr when -// reporting an error (toStderr = true). +//! Print usage for one tool. +//! @param appName name to print +//! @param desc one-line summary of the tool +//! @param options the flag lines to list, e.g. {cliopt::MJS, cliopt::LAZ}; +//! the -h/--help line is added automatically +//! @param toStderr print to stderr rather than stdout, for error reporting void printUsage(const char* appName, const char* desc, const std::vector& options, bool toStderr = false); diff --git a/calib_core/include/CalibCore/PointCloud.h b/calib_core/include/CalibCore/PointCloud.h index 2e1a8e2c..e83b9f2f 100644 --- a/calib_core/include/CalibCore/PointCloud.h +++ b/calib_core/include/CalibCore/PointCloud.h @@ -23,4 +23,6 @@ struct PointCloud { bool empty() const { return points.empty(); } }; +std::string GetLidarSerial(const char* path); + } // namespace calib diff --git a/calib_core/include/CalibCore/Trajectory.h b/calib_core/include/CalibCore/Trajectory.h index eb880df7..9688a194 100644 --- a/calib_core/include/CalibCore/Trajectory.h +++ b/calib_core/include/CalibCore/Trajectory.h @@ -6,24 +6,39 @@ namespace calib { -// One LiDAR pose from the trajectory CSV. -// T = T_world_lidar: p_world = T * p_lidar +//! One LiDAR pose from the trajectory CSV. struct TrajPose { + //! Timestamp, nanoseconds. int64_t ts_ns = 0; + //! T_world_lidar, i.e. p_world = T * p_lidar. Eigen::Affine3f T = Eigen::Affine3f::Identity(); }; +//! A LiDAR trajectory: poses over time, loaded from Mandeye's CSV. struct Trajectory { + //! The poses. Kept in whatever order they were loaded until @ref sort. std::vector poses; - // Load one trajectory_lio_N.csv. Appends to poses. - // If mrp != nullptr it is applied to every pose: T_corrected = *mrp * T_pose. + //! Load one trajectory_lio_N.csv, appending to @ref poses. + //! @param path CSV to read + //! @param mrp optional correction applied to every pose loaded, + //! T_corrected = *mrp * T_pose; ignored when null + //! @return false if the file could not be opened bool loadCSV(const std::string& path, const Eigen::Affine3f* mrp = nullptr); + //! Sort @ref poses by ascending timestamp. Call after loading, and before + //! @ref nearest, which relies on the ordering. void sort(); + //! Pose closest in time to `ts_ns`. + //! @param ts_ns timestamp to look up, nanoseconds + //! @return the nearest pose, clamped to the first or last one when `ts_ns` + //! falls outside the trajectory, or nullptr when it is empty + //! @warning Assumes @ref poses is sorted by timestamp -- it binary-searches. + //! Call @ref sort first, or the result is arbitrary. const TrajPose* nearest(int64_t ts_ns) const; + //! True when no poses have been loaded. bool empty() const { return poses.empty(); } }; diff --git a/calib_core/src/Camera.cpp b/calib_core/src/Camera.cpp index c2fbabd0..a2b0c1df 100644 --- a/calib_core/src/Camera.cpp +++ b/calib_core/src/Camera.cpp @@ -1,5 +1,11 @@ #include +#include +#include +#include + +#include + // Reuses (does not duplicate) core's own om/fi/ka<->matrix conversion -- // header-only, pulls in nothing but Eigen/std (see structures.h), so this // doesn't violate calib_core's no-raylib/imgui/OpenCV design (see @@ -8,6 +14,99 @@ namespace calib { +namespace +{ + std::string trim(std::string s) + { + const char* ws = " \t\r\n"; + const auto b = s.find_first_not_of(ws); + if (b == std::string::npos) + return {}; + return s.substr(b, s.find_last_not_of(ws) - b + 1); + } + + std::string unquote(std::string s) + { + if (s.size() >= 2 && (s.front() == '"' || s.front() == '\'') && s.back() == s.front()) + return s.substr(1, s.size() - 2); + return s; + } +} // namespace + +// Unrelated to loadCameraInfoYaml (CameraInfoYaml.cpp) -- opens and reads the +// file on its own rather than sharing a file handle or result with it, +// since parsing intrinsics and reading identity fields are two different +// jobs. Works on any flat `key: value` yaml, not just a Mei camera_info.yaml. +bool loadCameraIdentity(const std::string& path, CameraIdentity& id) +{ + std::ifstream f(path); + if (!f) + return false; + + // Cleared rather than merged, so a file naming no camera comes back + // empty instead of keeping whatever was loaded before it. + CameraIdentity next; + std::string line; + while (std::getline(f, line)) + { + const auto hash = line.find('#'); + if (hash != std::string::npos) + line = line.substr(0, hash); + const auto colon = line.find(':'); + if (colon == std::string::npos) + continue; + const std::string key = trim(line.substr(0, colon)); + const std::string value = unquote(trim(line.substr(colon + 1))); + if (key == "serial") + next.serial = value; + else if (key == "frame_id") + next.frameId = value; + else if (key == "model") + next.model = value; + } + id = next; + + return true; +} + +std::optional LoadTimestampFromSideCar(const std::string& path) +{ + const auto dot = path.rfind('.'); + const std::string sidecar = (dot != std::string::npos ? path.substr(0, dot) : path) + ".meta.json"; + + std::ifstream f(sidecar); + if (!f) + return std::nullopt; + + nlohmann::json j; + try + { + f >> j; + } catch (const nlohmann::json::exception&) + { + return std::nullopt; + } + + const auto it = j.find("FRAME_WALL_CLOCK"); + if (it == j.end()) + return std::nullopt; + + // FRAME_WALL_CLOCK is nanoseconds since epoch, as a number or a numeric + // string -- returned as-is, matching the filename timestamps. + if (it->is_string()) + { + try + { + return std::stod(it->get()); + } catch (const std::exception&) + { + return std::nullopt; + } + } + if (it->is_number()) + return it->get(); + return std::nullopt; +} Eigen::Matrix3f omFiKaToMat3(float om_deg, float fi_deg, float ka_deg) { TaitBryanPose pose; @@ -27,6 +126,151 @@ void omFiKaFromMat3(const Eigen::Matrix3f& R, float& om_deg, float& fi_deg, floa ka_deg = static_cast(rad2deg(pose.ka)); } +// No `default:` case on purpose: -Wswitch then flags a future CameraModel +// enumerator added without a matching string here, instead of it silently +// falling through to "pinhole". +const char* modelToString(CameraModel m) +{ + switch (m) + { + case CameraModel::Pinhole: + return "pinhole"; + case CameraModel::Mei: + return "mei"; + case CameraModel::Fisheye: + return "fisheye"; + } + return "pinhole"; +} + +CameraModel modelFromString(const std::string& s) +{ + if (s == "mei") + return CameraModel::Mei; + if (s == "fisheye" || s == "equidistant") + return CameraModel::Fisheye; + return CameraModel::Pinhole; +} + +float fisheyeMaxTheta(const Intrinsics& K) +{ + const double k1 = K.k1, k2 = K.k2, k3 = K.k3, k4 = K.k4; + auto thetaD = [&](double t) + { + const double t2 = t * t; + return t * (1.0 + t2 * (k1 + t2 * (k2 + t2 * (k3 + t2 * k4)))); + }; + // Scanned numerically, like maxValidRadiusSq below: a 9th-order + // polynomial's first turning point has no useful closed form. + const double kStep = 1e-3; + double prev = 0.0; + for (double t = kStep; t <= M_PI; t += kStep) + { + const double cur = thetaD(t); + if (cur <= prev) + return static_cast(t - kStep); + prev = cur; + } + return static_cast(M_PI); +} + +// Memoized for the same reason as cachedMaxValidRadiusSq below. +static float cachedFisheyeMaxTheta(const Intrinsics& K) +{ + thread_local float lastK[4] = { 0.f, 0.f, 0.f, 0.f }; + thread_local float lastResult = -1.f; + if (lastResult >= 0.f && lastK[0] == K.k1 && lastK[1] == K.k2 && lastK[2] == K.k3 && lastK[3] == K.k4) + return lastResult; + lastResult = fisheyeMaxTheta(K); + lastK[0] = K.k1; + lastK[1] = K.k2; + lastK[2] = K.k3; + lastK[3] = K.k4; + return lastResult; +} + +// Radius (in normalized camera coords, squared) past which the rational distortion model +// stops being usable. r -> r*radial(r) is only injective up to its turning point; beyond it +// the model folds, so directions far outside the lens' actual field of view map back onto +// valid pixel coordinates -- painting whatever is at the centre of the frame onto geometry +// the camera never saw. The projection alone cannot tell such a fold-back from a genuine +// hit, so find the turning point once and reject everything past it. Scanned numerically -- +// the turning point of a 6th-order rational function has no useful closed form. It always +// lies outside the image itself (otherwise the calibration could not reach its own corners), +// so no legitimate pixel is lost. Ported from the equivalent fix applied directly in +// TrajectoryViewer.cpp's (now-removed) inline distortion code -- see upstream commit +// "Fix colorization for calibration for invalid points" (#527) -- but placed here so every +// caller of projectPoint() gets it, not just that one call site. +static float maxValidRadiusSq(float k1, float k2, float k3, float k4, float k5, float k6) { + auto g = [&](float r) { + float r2 = r * r; + float den = 1.f + (k4 + (k5 + k6 * r2) * r2) * r2; + if (std::fabs(den) < 1e-9f) + return -1.f; // pole -- certainly past the turning point + return r * (1.f + (k1 + (k2 + k3 * r2) * r2) * r2) / den; + }; + // 8.0 == tan(83 deg), wider than any lens this app sees. A distortion-free model is + // monotonic everywhere and so keeps the whole range, i.e. no behaviour change. + const float kLimit = 8.f, kStep = 0.005f; + float prev = 0.f; + for (float r = kStep; r <= kLimit; r += kStep) { + float cur = g(r); + if (cur <= prev) + return (r - kStep) * (r - kStep); + prev = cur; + } + return kLimit * kLimit; +} + +// projectPoint() is called per-point -- potentially millions of times per colorize pass -- +// with the SAME Intrinsics each time, so re-running the numeric scan above on every call +// would be a severe perf regression. Memoize on the six coefficients actually scanned; exact +// float equality is fine here since it's detecting "same Intrinsics as last call", not +// comparing independently-derived values. +static float cachedMaxValidRadiusSq(float k1, float k2, float k3, float k4, float k5, float k6) { + thread_local float lastK[6] = { 0.f, 0.f, 0.f, 0.f, 0.f, 0.f }; + thread_local float lastResult = -1.f; + if (lastResult >= 0.f && lastK[0] == k1 && lastK[1] == k2 && lastK[2] == k3 && + lastK[3] == k4 && lastK[4] == k5 && lastK[5] == k6) { + return lastResult; + } + lastResult = maxValidRadiusSq(k1, k2, k3, k4, k5, k6); + lastK[0] = k1; lastK[1] = k2; lastK[2] = k3; lastK[3] = k4; lastK[4] = k5; lastK[5] = k6; + return lastResult; +} + +Intrinsics scaleIntrinsics(const Intrinsics& K, float s) { + Intrinsics out = K; + out.fx *= s; + out.fy *= s; + out.cx *= s; + out.cy *= s; + out.width = static_cast(std::lround(K.width * s)); + out.height = static_cast(std::lround(K.height * s)); + return out; +} + +Roi scaleRoi(const Roi& r, float s) { + Roi out = r; + if (r.w <= 0 || r.h <= 0) { + return out; // w/h == 0 is the "no ROI set" sentinel; leave it alone + } + const int x0 = static_cast(std::lround(r.x * s)); + const int y0 = static_cast(std::lround(r.y * s)); + const int x1 = static_cast(std::lround((r.x + r.w) * s)); + const int y1 = static_cast(std::lround((r.y + r.h) * s)); + out.x = x0; + out.y = y0; + // Both edges are rounded and then subtracted, rather than the width being + // scaled on its own, so two abutting rectangles cannot come back + // overlapping. The clamp keeps a rectangle too small to survive the scale + // at one pixel: collapsing it to w/h == 0 would read as "no ROI" and + // silently pass everything the ROI was there to reject. + out.w = std::max(1, x1 - x0); + out.h = std::max(1, y1 - y0); + return out; +} + bool projectPoint(float px, float py, float pz, const Intrinsics& K, const Eigen::Matrix3f& R_wc, @@ -35,13 +279,75 @@ bool projectPoint(float px, float py, float pz, // p_cam = R_wc^T * (p_lidar - C) Eigen::Vector3f pc = R_wc.transpose() * (Eigen::Vector3f(px, py, pz) - t); + if (K.model == CameraModel::Mei) { + depth = pc.norm(); + if (depth < 1e-4f) return false; // point sits on the camera itself + + // Validity domain. r(theta) = sin/(cos+xi) is only injective up to + // its turning point at cos(theta) = -1/xi; past it the radius shrinks + // again and far-off-axis directions FOLD BACK onto valid pixels -- + // at theta = 180 deg exactly onto (cx, cy). For xi <= 1 the + // denominator blows up first, so "Xs.z + xi > 0" is the limit there. + // xi <= 1: Xs.z > -xi (reduces to Pinhole's pc.z > 0 at xi = 0) + // xi > 1: Xs.z > -1/xi + const float zMin = (K.xi > 1.f) ? -1.f / K.xi : -K.xi; + if (pc.z() / depth <= zMin) return false; + + // Unified sphere, then a plain (non-rational) radial/tangential + // polynomial. Computed in double: the xi denominator gets small near + // the edge of the valid dome, where float loses too much. + const Eigen::Vector3d Xs = pc.cast().normalized(); + const double den = Xs.z() + K.xi; + const double x = Xs.x() / den, y = Xs.y() / den; + const double r2 = x*x + y*y; + const double radial = 1.0 + K.k1*r2 + K.k2*r2*r2 + K.k3*r2*r2*r2; + const double xd = x*radial + 2*K.p1*x*y + K.p2*(r2 + 2*x*x); + const double yd = y*radial + K.p1*(r2 + 2*y*y) + 2*K.p2*x*y; + + u = static_cast(K.fx * xd + K.cx); + v = static_cast(K.fy * yd + K.cy); + return true; + } + + if (K.model == CameraModel::Fisheye) { + depth = pc.norm(); + if (depth < 1e-4f) return false; // point sits on the camera itself + + // Equidistant: image radius grows with the incidence angle theta, not + // tan(theta), so there is no z > 0 requirement -- only the fold-back + // limit. + const double x = pc.x(), y = pc.y(), z = pc.z(); + const double r = std::hypot(x, y); + const double theta = std::atan2(r, z); + if (theta >= cachedFisheyeMaxTheta(K)) return false; + // Directly behind has no direction to push the point out along, so + // s below would drop it on the principal point. Checked on its own: + // a limit of pi, rounded to float, lies just above the double pi. + if (r == 0.0 && z < 0.0) return false; + + const double t2 = theta * theta; + const double thetaD = theta * (1.0 + t2*(K.k1 + t2*(K.k2 + t2*(K.k3 + t2*K.k4)))); + // r == 0 is the optical axis, which lands on the principal point. + const double s = r > 0.0 ? thetaD / r : 0.0; + + u = static_cast(K.fx * x * s + K.cx); + v = static_cast(K.fy * y * s + K.cy); + return true; + } + depth = pc.z(); if (depth <= 1e-4f) return false; float xn = pc.x() / depth; float yn = pc.y() / depth; + // Off-axis cutoff: beyond the rational distortion model's turning point, the projection + // folds back and would paint frame-centre content onto geometry the camera never saw. + // See maxValidRadiusSq() above. float r2 = xn*xn + yn*yn; + if (r2 > cachedMaxValidRadiusSq(K.k1, K.k2, K.k3, K.k4, K.k5, K.k6)) + return false; + float r4 = r2 * r2; float r6 = r4 * r2; float radial = (1.f + K.k1*r2 + K.k2*r4 + K.k3*r6) diff --git a/calib_core/src/CameraCalibrationSolverCeres.cpp b/calib_core/src/CameraCalibrationSolverCeres.cpp new file mode 100644 index 00000000..122bf00c --- /dev/null +++ b/calib_core/src/CameraCalibrationSolverCeres.cpp @@ -0,0 +1,235 @@ +#include + +// Always compiled; the #ifdef below picks between the real Ceres +// implementation and a stub that explains why it isn't available, so callers +// check solveExtrinsicsCeres's return value rather than an #ifdef. +#ifdef CALIB_ENABLE_CERES + +#include + +#include + +namespace calib +{ + namespace + { + // Ceres::Jet-compatible equivalent of Camera.cpp's omFiKaToMat3: + // R = kCameraLidarAxisOffset * Rx(om)*Ry(fi)*Rz(ka), om/fi/ka in + // RADIANS (Extrinsics stores degrees; solve() converts). The Rx*Ry*Rz + // part mirrors Core/transformations.h's + // affine_matrix_from_pose_tait_bryan. kCameraLidarAxisOffset's entries + // are only {0, +-1}, so it is applied by permuting/negating Rdelta's + // rows rather than a general 3x3 product: offset = + // [[0,0,1],[-1,0,0],[0,-1,0]], so row 0 of R is row 2 of Rdelta, + // row 1 is -(row 0), row 2 is -(row 1). + template + void rotationMatrix(const T& om, const T& fi, const T& ka, T R[3][3]) + { + const T sx = sin(om), cx = cos(om); + const T sy = sin(fi), cy = cos(fi); + const T sz = sin(ka), cz = cos(ka); + + T Rdelta[3][3]; + Rdelta[0][0] = cy * cz; + Rdelta[1][0] = cz * sx * sy + cx * sz; + Rdelta[2][0] = -cx * cz * sy + sx * sz; + Rdelta[0][1] = -cy * sz; + Rdelta[1][1] = cx * cz - sx * sy * sz; + Rdelta[2][1] = cz * sx + cx * sy * sz; + Rdelta[0][2] = sy; + Rdelta[1][2] = -cy * sx; + Rdelta[2][2] = cx * cy; + + for (int c = 0; c < 3; ++c) + { + R[0][c] = Rdelta[2][c]; + R[1][c] = -Rdelta[0][c]; + R[2][c] = -Rdelta[1][c]; + } + } + + // Templated equivalent of calib::projectPoint's Mei branch, for + // Ceres autodiff -- + // same formula. Intrinsics stay plain doubles (fixed, not solved + // for); only pc is the Jet-typed variable. + template + void projectMei( + const T pc[3], + double fx, + double fy, + double cx, + double cy, + double xi, + double k1, + double k2, + double k3, + double p1, + double p2, + T& u, + T& v) + { + const T n = sqrt(pc[0] * pc[0] + pc[1] * pc[1] + pc[2] * pc[2]); + const T Xx = pc[0] / n, Xy = pc[1] / n, Xz = pc[2] / n; + const T denom = Xz + T(xi); + const T x = Xx / denom, y = Xy / denom; + const T r2 = x * x + y * y; + const T radial = T(1.0) + T(k1) * r2 + T(k2) * r2 * r2 + T(k3) * r2 * r2 * r2; + const T xd = x * radial + T(2.0 * p1) * x * y + T(p2) * (r2 + T(2.0) * x * x); + const T yd = y * radial + T(p1) * (r2 + T(2.0) * y * y) + T(2.0 * p2) * x * y; + u = T(fx) * xd + T(cx); + v = T(fy) * yd + T(cy); + } + + // Templated equivalent of calib::projectPoint's Fisheye branch, for + // Ceres autodiff. + template + void projectFisheye(const T pc[3], double fx, double fy, double cx, double cy, double k1, double k2, double k3, double k4, T& u, T& v) + { + const T r2 = pc[0] * pc[0] + pc[1] * pc[1]; + // theta_d / r, the factor that scales (x, y) onto the image plane. + // sqrt's derivative is infinite at r = 0, so on the optical axis + // use its limit 1/z instead. + T s; + if (r2 > T(1e-18)) + { + const T r = sqrt(r2); + const T theta = atan2(r, pc[2]); + const T t2 = theta * theta; + s = theta * (T(1.0) + t2 * (T(k1) + t2 * (T(k2) + t2 * (T(k3) + t2 * T(k4))))) / r; + } + else + { + s = T(1.0) / pc[2]; + } + u = T(fx) * pc[0] * s + T(cx); + v = T(fy) * pc[1] * s + T(cy); + } + + // Reprojection residual for one correspondence: predicted (u, v) + // minus the picked pixel, like the Pinhole solver's observation + // equation but autodiff'd, no vendored Mei or fisheye Jacobian + // existing. K.model must be Mei or Fisheye. + struct ReprojectionResidual + { + ReprojectionResidual(const Eigen::Vector3d& p, double u_kp, double v_kp, const Intrinsics& K) + : p_(p), u_kp_(u_kp), v_kp_(v_kp), K_(K) + { + } + + template + bool operator()(const T* const tx_ty_tz, const T* const om_fi_ka, T* residual) const + { + T R[3][3]; + rotationMatrix(om_fi_ka[0], om_fi_ka[1], om_fi_ka[2], R); + + const T d[3] = { T(p_.x()) - tx_ty_tz[0], T(p_.y()) - tx_ty_tz[1], T(p_.z()) - tx_ty_tz[2] }; + // p_cam = R_wc^T * (p_world - C) + const T pc[3] = { + R[0][0] * d[0] + R[1][0] * d[1] + R[2][0] * d[2], + R[0][1] * d[0] + R[1][1] * d[1] + R[2][1] * d[2], + R[0][2] * d[0] + R[1][2] * d[1] + R[2][2] * d[2], + }; + + T u, v; + if (K_.model == CameraModel::Fisheye) + projectFisheye(pc, K_.fx, K_.fy, K_.cx, K_.cy, K_.k1, K_.k2, K_.k3, K_.k4, u, v); + else + projectMei(pc, K_.fx, K_.fy, K_.cx, K_.cy, K_.xi, K_.k1, K_.k2, K_.k3, K_.p1, K_.p2, u, v); + residual[0] = u - T(u_kp_); + residual[1] = v - T(v_kp_); + return true; + } + + const Eigen::Vector3d p_; + const double u_kp_, v_kp_; + const Intrinsics K_; + }; + } // namespace + + bool solveExtrinsicsCeres( + const std::vector& correspondences, + const Intrinsics& K, + Extrinsics& extrinsicsInOut, + std::string& errorMessage, + double* outRmsPixels, + bool fixTranslation) + { + if (K.model != CameraModel::Mei && K.model != CameraModel::Fisheye) + { + errorMessage = std::string("The Ceres solver handles the mei and fisheye models only, not ") + modelToString(K.model); + return false; + } + + const int nParams = fixTranslation ? 3 : 6; + if (static_cast(correspondences.size()) < 3 || static_cast(correspondences.size()) * 2 < nParams) + { + errorMessage = "Need at least 3 correspondences"; + return false; + } + + const double d2r = M_PI / 180.0; + double txyz[3] = { extrinsicsInOut.tx, extrinsicsInOut.ty, extrinsicsInOut.tz }; + double omfika[3] = { extrinsicsInOut.om * d2r, extrinsicsInOut.fi * d2r, extrinsicsInOut.ka * d2r }; + + ceres::Problem problem; + for (const auto& c : correspondences) + { + auto* cost = new ceres::AutoDiffCostFunction(new ReprojectionResidual(c.p, c.u, c.v, K)); + problem.AddResidualBlock(cost, nullptr, txyz, omfika); + } + if (fixTranslation) + problem.SetParameterBlockConstant(txyz); + + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_QR; + options.max_num_iterations = 100; + options.logging_type = ceres::SILENT; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + if (!summary.IsSolutionUsable()) + { + errorMessage = "Ceres solve failed: " + summary.BriefReport(); + return false; + } + + extrinsicsInOut.tx = static_cast(txyz[0]); + extrinsicsInOut.ty = static_cast(txyz[1]); + extrinsicsInOut.tz = static_cast(txyz[2]); + extrinsicsInOut.om = static_cast(omfika[0] / d2r); + extrinsicsInOut.fi = static_cast(omfika[1] / d2r); + extrinsicsInOut.ka = static_cast(omfika[2] / d2r); + + // final_cost is 0.5*sum(residual^2) over every SCALAR residual (2 per + // correspondence), so the Pinhole solver's rms formula + // sqrt(sum(du^2+dv^2) / (2*N)) simplifies to sqrt(final_cost/N). + if (outRmsPixels) + *outRmsPixels = std::sqrt(summary.final_cost / static_cast(correspondences.size())); + + return true; + } +} // namespace calib + +#else // !CALIB_ENABLE_CERES + +#include + +namespace calib +{ + bool solveExtrinsicsCeres( + const std::vector&, + const Intrinsics&, + Extrinsics&, + std::string& errorMessage, + double*, + bool) + { + errorMessage = + "Mei/fisheye extrinsics solving needs calib_core built with -DCALIB_ENABLE_CERES=ON (see calib_core/CMakeLists.txt)"; + std::cerr << errorMessage << std::endl; + return false; + } +} // namespace calib + +#endif \ No newline at end of file diff --git a/calib_core/src/CameraInfoYaml.cpp b/calib_core/src/CameraInfoYaml.cpp new file mode 100644 index 00000000..e10ac4ed --- /dev/null +++ b/calib_core/src/CameraInfoYaml.cpp @@ -0,0 +1,196 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace calib +{ + +namespace +{ + std::string trim(std::string s) + { + const char* ws = " \t\r\n"; + const auto b = s.find_first_not_of(ws); + if (b == std::string::npos) + return {}; + return s.substr(b, s.find_last_not_of(ws) - b + 1); + } + + // A flat camera_info.yaml is a mapping of `key: value` scalars plus a + // `distortion: [a, b, c, d, e]` flow sequence -- no nesting, no anchors, + // no block sequences. Parsed here rather than with a YAML library so + // calib_core keeps depending on nothing but Eigen/LASzip/std. + std::map readFlatYaml(std::istream& in) + { + std::map kv; + std::string line; + while (std::getline(in, line)) + { + const auto hash = line.find('#'); + if (hash != std::string::npos) + line = line.substr(0, hash); + const auto colon = line.find(':'); + if (colon == std::string::npos) + continue; + std::string key = trim(line.substr(0, colon)); + if (!key.empty()) + kv[key] = trim(line.substr(colon + 1)); + } + return kv; + } + + std::vector parseArray(const std::string& v) + { + std::vector out; + std::string inner = trim(v); + if (inner.size() >= 2 && inner.front() == '[' && inner.back() == ']') + inner = inner.substr(1, inner.size() - 2); + std::stringstream ss(inner); + std::string tok; + while (std::getline(ss, tok, ',')) + { + tok = trim(tok); + if (!tok.empty()) + out.push_back(std::strtod(tok.c_str(), nullptr)); + } + return out; + } + + //! How one distortion_model maps onto Intrinsics. + struct FlatModel + { + const char* name; //!< distortion_model, lower-case + CameraModel model; + std::vector order; //!< the field each `distortion` entry goes to + }; + + const std::vector& flatModels() + { + using I = Intrinsics; + static const std::vector models = { + { "insta360_mei_v2", CameraModel::Mei, { &I::k1, &I::k2, &I::k3, &I::p1, &I::p2 } }, + { "equidistant", CameraModel::Fisheye, { &I::k1, &I::k2, &I::k3, &I::k4 } }, + { "plumb_bob", CameraModel::Pinhole, { &I::k1, &I::k2, &I::p1, &I::p2, &I::k3 } }, + { "rational_polynomial", CameraModel::Pinhole, { &I::k1, &I::k2, &I::p1, &I::p2, &I::k3, &I::k4, &I::k5, &I::k6 } }, + }; + return models; + } +} // namespace + +bool loadCameraInfoYaml(const std::string& path, Intrinsics& K) +{ + std::ifstream f(path); + if (!f) + { + std::fprintf(stderr, "calib_core: failed to open '%s'\n", path.c_str()); + return false; + } + const std::map kv = readFlatYaml(f); + + const auto unquote = [](std::string s) + { + if (s.size() >= 2 && (s.front() == '"' || s.front() == '\'') && s.back() == s.front()) + return s.substr(1, s.size() - 2); + return s; + }; + const auto modelIt = kv.find("distortion_model"); + std::string distortionModel = modelIt != kv.end() ? unquote(modelIt->second) : ""; + std::transform( + distortionModel.begin(), + distortionModel.end(), + distortionModel.begin(), + [](unsigned char c) + { + return static_cast(std::tolower(c)); + }); + + const FlatModel* layout = nullptr; + const std::string name = distortionModel == "fisheye" ? "equidistant" : distortionModel; + for (const FlatModel& m : flatModels()) + if (name == m.name) + layout = &m; + // Files the Mei-only loader this replaced used to accept. + if (!layout && (distortionModel.find("mei") != std::string::npos || (distortionModel.empty() && kv.count("xi")))) + { + std::fprintf( + stderr, + "calib_core: WARNING '%s' has distortion_model='%s', reading it as insta360_mei_v2 " + "(results will be wrong if the model differs)\n", + path.c_str(), + distortionModel.c_str()); + layout = &flatModels().front(); + } + if (!layout) + { + std::string known; + for (const FlatModel& m : flatModels()) + known += std::string(known.empty() ? "" : ", ") + m.name; + std::fprintf( + stderr, "calib_core: '%s' has distortion_model='%s', expected one of %s\n", path.c_str(), distortionModel.c_str(), known.c_str()); + return false; + } + + // Every numeric field is required: a calibration silently defaulting one + // of these to 0 reprojects wrongly with no visible failure. + std::vector required = { "width", "height", "fx", "fy", "cx", "cy", "distortion" }; + if (layout->model == CameraModel::Mei) + required.push_back("xi"); + for (const char* key : required) + { + if (kv.find(key) == kv.end()) + { + std::fprintf(stderr, "calib_core: '%s' is missing required field '%s'\n", path.c_str(), key); + return false; + } + } + + const std::vector d = parseArray(kv.at("distortion")); + const size_t n = layout->order.size(); + if (d.size() != n) + { + // Four coefficients are all a fisheye has, so any other count means + // the file is not the model it names. + if (layout->model == CameraModel::Fisheye) + { + std::fprintf( + stderr, "calib_core: '%s' distortion has %zu elements, equidistant needs exactly 4 (k1,k2,k3,k4)\n", path.c_str(), d.size()); + return false; + } + std::fprintf( + stderr, + "calib_core: WARNING '%s' distortion has %zu elements, expected %zu for %s " + "-- missing ones default to 0, extras are ignored\n", + path.c_str(), + d.size(), + n, + layout->name); + } + + auto num = [&](const char* key) + { + return std::strtod(kv.at(key).c_str(), nullptr); + }; + K = Intrinsics{}; + K.model = layout->model; + K.width = static_cast(num("width")); + K.height = static_cast(num("height")); + K.fx = static_cast(num("fx")); + K.fy = static_cast(num("fy")); + K.cx = static_cast(num("cx")); + K.cy = static_cast(num("cy")); + if (layout->model == CameraModel::Mei) + K.xi = static_cast(num("xi")); + for (size_t i = 0; i < n; ++i) + K.*(layout->order[i]) = i < d.size() ? static_cast(d[i]) : 0.f; + return true; +} + +} // namespace calib \ No newline at end of file diff --git a/calib_core/src/PointCloud.cpp b/calib_core/src/PointCloud.cpp index c44fc9a3..05e6f328 100644 --- a/calib_core/src/PointCloud.cpp +++ b/calib_core/src/PointCloud.cpp @@ -2,7 +2,8 @@ #include #include #include - +#include +#include namespace calib { void PointCloud::clear() { @@ -85,4 +86,32 @@ bool PointCloud::load(const std::string& path) { } +std::string GetLidarSerial(const char* path) +{ + static constexpr const char* kUnknownLidarSerial = "unknown"; + + const std::string spath(path); + const std::regex lidarPattern(R"(lidar(\d+)\.laz$)"); + std::smatch match; + + if (!std::regex_search(spath, match, lidarPattern)) + return kUnknownLidarSerial; + + std::string statusPath = std::regex_replace(spath, lidarPattern, "status$1.json"); + + std::ifstream f(statusPath); + if (!f) + { + return kUnknownLidarSerial; + } + try + { + nlohmann::json j; + f >> j; + return j["lidar"]["LivoxLidarInfo"]["sn"].get(); + } catch (...) + { + } + return kUnknownLidarSerial; +} } // namespace calib diff --git a/calib_core/src/Trajectory.cpp b/calib_core/src/Trajectory.cpp index a5328521..69466544 100644 --- a/calib_core/src/Trajectory.cpp +++ b/calib_core/src/Trajectory.cpp @@ -2,6 +2,7 @@ #include #include #include +#include namespace calib { @@ -17,7 +18,12 @@ bool Trajectory::loadCSV(const std::string& path, const Eigen::Affine3f* mrp) { std::istringstream ss(line); TrajPose p; float raw[12]; - ss >> p.ts_ns; + // lidar_odometry_step_1 writes the timestamp as a double (seconds * 1e9), + // so it can carry a fraction ("548348730189.99993896"); reading it + // straight into an int64 would stop at the '.' and shift every column. + double ts_ns = 0.0; + ss >> ts_ns; + p.ts_ns = std::llround(ts_ns); for (int i = 0; i < 12; i++) ss >> raw[i]; if (!ss) continue; diff --git a/calib_core/tests/CMakeLists.txt b/calib_core/tests/CMakeLists.txt new file mode 100644 index 00000000..8a73af43 --- /dev/null +++ b/calib_core/tests/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 4.0.0) + +project(calib_core_tests) + +# Unit tests for calib_core's camera models and solvers. calib_core pulls in +# no raylib/imgui/GL, so its projection math is testable without a GL context +# -- the reason the Mei and fisheye models live there, not in an app. +# Uses doctest, like shared/tests. test_camera.cpp owns doctest's main() +# (DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN); test_solver.cpp adds more TEST_CASEs +# to the same registry. +add_executable(calib_core_tests + test_camera.cpp + test_solver.cpp + test_trajectory.cpp +) + +target_link_libraries(calib_core_tests PRIVATE calib_core) + +# calib_core's Eigen include is PRIVATE, so it isn't inherited by linking. +target_include_directories(calib_core_tests PRIVATE + ${THIRDPARTY_DIRECTORY}/doctest + ${EIGEN3_INCLUDE_DIR} +) + +if (MSVC) + target_compile_definitions(calib_core_tests PRIVATE _USE_MATH_DEFINES) +endif() + +include(CTest) +add_test(NAME calib_core_tests COMMAND calib_core_tests) diff --git a/calib_core/tests/test_camera.cpp b/calib_core/tests/test_camera.cpp new file mode 100644 index 00000000..28a2bd08 --- /dev/null +++ b/calib_core/tests/test_camera.cpp @@ -0,0 +1,858 @@ +#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN +#include + +#include + +#include +#include +#include +#include +#include +#include + +using namespace calib; + +namespace +{ + // A representative Mei/unified-sphere fisheye, values in the shape + // insta360_mei_v2 calibrations take rather than a real calibrated camera. + Intrinsics mei() + { + Intrinsics K; + K.model = CameraModel::Mei; + K.fx = 300.f; K.fy = 300.f; + K.cx = 320.f; K.cy = 240.f; + K.xi = 1.2f; + K.k1 = -0.15f; K.k2 = 0.02f; K.k3 = -0.001f; + K.p1 = 0.001f; K.p2 = -0.0005f; + K.width = 640; K.height = 480; + return K; + } + + // Coefficients in the range a real ~180 deg OpenCV fisheye calibration + // produces, not a specific camera. Their theta -> theta_d polynomial is + // increasing all the way to pi. + Intrinsics fisheye() + { + Intrinsics K; + K.model = CameraModel::Fisheye; + K.fx = 285.f; K.fy = 286.f; + K.cx = 322.f; K.cy = 238.f; + K.k1 = -0.0075f; K.k2 = 0.0435f; K.k3 = -0.0414f; K.k4 = 0.0077f; + K.width = 640; K.height = 480; + return K; + } + + // Direction at `deg` from the optical axis, in the plane y = 0. The + // explicit return type matters: `auto` would deduce an Eigen expression + // template holding a reference to the temporary, and dangle. + Eigen::Vector3f offAxis(float deg) + { + const float r = deg * float(M_PI) / 180.f; + return Eigen::Vector3f(std::sin(r), 0.f, std::cos(r)) * 10.f; + } + + // Identity pose: p_cam == p_lidar, so test points can be written directly + // in camera axes (X = right, Y = down, Z = forward). + const Eigen::Matrix3f kIdentity = Eigen::Matrix3f::Identity(); + const Eigen::Vector3f kOrigin = Eigen::Vector3f::Zero(); + + // Convenience wrapper: projects and returns the pixel, CHECKing success. + struct Px + { + float u, v, depth; + }; + + Px project(const Intrinsics& K, const Eigen::Vector3f& p, const Eigen::Matrix3f& R_wc = kIdentity, const Eigen::Vector3f& t = kOrigin) + { + Px r{ 0, 0, 0 }; + REQUIRE(projectPoint(p.x(), p.y(), p.z(), K, R_wc, t, r.u, r.v, r.depth)); + return r; + } +} // namespace + +// ── Mei ───────────────────────────────────────────────────────────────────── + +TEST_CASE("mei: forward is the image centre, depth is range") +{ + const Intrinsics K = mei(); + + Px r = project(K, { 0, 0, 10 }); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + CHECK(r.depth == doctest::Approx(10.0)); // range, not z -- see below + + Px oblique = project(K, { 3, 0, 4 }); + CHECK(oblique.depth == doctest::Approx(5.0)); // a pinhole camera would report 4 +} + +TEST_CASE("mei: unified-sphere projection matches known-good reference values") +{ + // Pins the unified-sphere + polynomial math against values captured from + // the implementation, so a change to the formula has to be deliberate. + // A Mei camera has no closed-form check as simple as the pinhole one, and + // these were cross-checked against the rig's own reprojection. + const Intrinsics K = mei(); + struct Ref + { + Eigen::Vector3f p; + double u, v, depth; + }; + const Ref refs[] = { + { { 0.3f, -0.2f, 0.9f }, 363.3981018, 211.0740356, 0.9695359 }, + { { -1.5f, 0.8f, 2.0f }, 233.9576416, 285.9132385, 2.6248810 }, + { { 0.05f, 0.02f, 1.0f }, 326.8120728, 242.7250366, 1.0014490 }, + { { -0.6f, -1.1f, 0.8f }, 252.7274475, 116.8022079, 1.4866068 }, + }; + + for (const auto& r : refs) + { + Px got = project(K, r.p); + CHECK(got.u == doctest::Approx(r.u).epsilon(1e-6)); + CHECK(got.v == doctest::Approx(r.v).epsilon(1e-6)); + CHECK(got.depth == doctest::Approx(r.depth).epsilon(1e-6)); + } +} + +TEST_CASE("mei: a point on the optical axis lands on the principal point") +{ + const Intrinsics K = mei(); + Px r = project(K, { 0.f, 0.f, 1.f }); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + CHECK(r.depth == doctest::Approx(1.0)); +} + +TEST_CASE("mei: a point on the camera itself is rejected") +{ + const Intrinsics K = mei(); + float u, v, depth; + CHECK_FALSE(projectPoint(0, 0, 0, K, kIdentity, kOrigin, u, v, depth)); +} + +TEST_CASE("mei: a point behind the camera is rejected, not silently mis-projected") +{ + // The projection has no domain guard of its own, and past the valid + // dome the projection is not injective -- it folds far-off-axis + // directions back onto real pixels instead of pushing them out of frame. + float u, v, depth; + + const auto at = offAxis; + auto projects = [&](const Intrinsics& K, const Eigen::Vector3f& p) + { return projectPoint(p.x(), p.y(), p.z(), K, kIdentity, kOrigin, u, v, depth); }; + + SUBCASE("xi > 1: the limit is the fold-back angle, acos(-1/xi)") + { + const Intrinsics K = mei(); // xi = 1.2 -> 146.44 deg + CHECK(projects(K, at(0.f))); + CHECK(projects(K, at(145.f))); + CHECK_FALSE(projects(K, at(148.f))); + // Straight behind used to land on (cx, cy) -- the whole point of the guard. + CHECK_FALSE(projects(K, at(180.f))); + } + + SUBCASE("xi <= 1: the limit is where the denominator blows up, acos(-xi)") + { + Intrinsics K = mei(); + K.xi = 0.5f; // -> 120 deg + CHECK(projects(K, at(0.f))); + CHECK(projects(K, at(119.f))); + CHECK_FALSE(projects(K, at(121.f))); + CHECK_FALSE(projects(K, at(180.f))); + } + + SUBCASE("xi = 0 reduces to the pinhole half-space") + { + Intrinsics K = mei(); + K.xi = 0.f; + CHECK(projects(K, at(89.f))); + CHECK_FALSE(projects(K, at(91.f))); + } +} + +TEST_CASE("mei: respects the extrinsics") +{ + const Intrinsics K = mei(); + + // om=fi=ka=0 is the nominal camera-vs-LiDAR alignment, so LiDAR forward + // (+X) should come out as camera forward, i.e. the image centre. + const Eigen::Matrix3f R_wc = kCameraLidarAxisOffset; + + Px r = project(K, { 10, 0, 0 }, R_wc); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + + // The camera position is subtracted: one metre in front of an offset + // camera reprojects the same as one metre in front of the origin. + const Eigen::Vector3f C(1.f, 2.f, 3.f); + Px offset = project(K, C + Eigen::Vector3f(1.f, 0.f, 0.f), R_wc, C); + // p_lidar - C = LiDAR +X, which R_wc's transpose turns into camera +Z + // (camera-forward) -- same axis remap as the centre check above. + // On-axis, so it lands on the principal point, as the centre check above. + CHECK(offset.u == doctest::Approx(K.cx)); + CHECK(offset.v == doctest::Approx(K.cy)); + CHECK(offset.depth == doctest::Approx(1.0)); +} + +// ── Fisheye ─────────────────────────────────────────────────────────────────── + +TEST_CASE("fisheye: matches cv::fisheye::projectPoints") +{ + // Captured from OpenCV 4.6's cv::fisheye::projectPoints with an identity + // pose and the same K/D -- the model this one has to agree with, so that + // an OpenCV fisheye calibration can be loaded verbatim. + const Intrinsics K = fisheye(); + struct Ref + { + Eigen::Vector3f p; + double u, v; + }; + const Ref refs[] = { + { { 0.3f, -0.2f, 0.9f }, 412.3305153, 177.5683570 }, + { { -1.5f, 0.8f, 2.0f }, 144.4155107, 333.0440495 }, + { { 0.05f, 0.02f, 1.0f }, 336.2359451, 243.7143583 }, + { { -0.6f, -1.1f, 0.8f }, 184.8716725, -14.2840458 }, + { { 2.0f, 1.0f, 0.3f }, 668.3977375, 411.8065841 }, + }; + + for (const auto& r : refs) + { + Px got = project(K, r.p); + CHECK(got.u == doctest::Approx(r.u).epsilon(1e-5)); + CHECK(got.v == doctest::Approx(r.v).epsilon(1e-5)); + CHECK(got.depth == doctest::Approx(r.p.norm())); + } +} + +TEST_CASE("fisheye: a point on the optical axis lands on the principal point") +{ + const Intrinsics K = fisheye(); + Px r = project(K, { 0.f, 0.f, 4.f }); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + CHECK(r.depth == doctest::Approx(4.0)); +} + +TEST_CASE("fisheye: without distortion the image radius is f * theta, past 90 deg too") +{ + Intrinsics K = fisheye(); + K.k1 = K.k2 = K.k3 = K.k4 = 0.f; + const double pi = M_PI; + + Px side = project(K, offAxis(90.f)); + CHECK(side.u == doctest::Approx(K.cx + K.fx * pi / 2)); + CHECK(side.v == doctest::Approx(K.cy)); + + // Behind the image plane, which a pinhole camera cannot see at all. + Px behind = project(K, offAxis(120.f)); + CHECK(behind.u == doctest::Approx(K.cx + K.fx * 2 * pi / 3)); + CHECK(behind.depth == doctest::Approx(10.0)); // range, not z (which is negative) + + Intrinsics P; // Pinhole + float u, v, depth; + const Eigen::Vector3f p = offAxis(120.f); + CHECK_FALSE(projectPoint(p.x(), p.y(), p.z(), P, kIdentity, kOrigin, u, v, depth)); +} + +TEST_CASE("fisheye: directions past the fold-back angle are rejected") +{ + float u, v, depth; + auto projects = [&](const Intrinsics& K, const Eigen::Vector3f& p) + { return projectPoint(p.x(), p.y(), p.z(), K, kIdentity, kOrigin, u, v, depth); }; + + SUBCASE("a turning point limits the field of view") + { + // theta_d = theta - 0.3 theta^3 peaks at theta = sqrt(1/0.9) (60.4 + // deg) and falls back to 0 -- the principal point -- at 104.6 deg. + Intrinsics K = fisheye(); + K.k1 = -0.3f; + K.k2 = K.k3 = K.k4 = 0.f; + CHECK(fisheyeMaxTheta(K) == doctest::Approx(std::sqrt(1.0 / 0.9)).epsilon(2e-3)); + CHECK(projects(K, offAxis(55.f))); + CHECK_FALSE(projects(K, offAxis(65.f))); + CHECK_FALSE(projects(K, offAxis(104.6f))); + } + + SUBCASE("a monotonic polynomial keeps everything short of straight behind") + { + const Intrinsics K = fisheye(); + CHECK(fisheyeMaxTheta(K) == doctest::Approx(M_PI)); + CHECK(projects(K, offAxis(170.f))); + CHECK_FALSE(projects(K, { 0.f, 0.f, -10.f })); + } + + SUBCASE("a point on the camera itself") + { + CHECK_FALSE(projects(fisheye(), { 0.f, 0.f, 0.f })); + } +} + +TEST_CASE("fisheye: respects the extrinsics") +{ + const Intrinsics K = fisheye(); + const Eigen::Matrix3f R_wc = kCameraLidarAxisOffset; + + // LiDAR forward is camera forward at om=fi=ka=0. + Px r = project(K, { 10, 0, 0 }, R_wc); + CHECK(r.u == doctest::Approx(K.cx)); + CHECK(r.v == doctest::Approx(K.cy)); + + const Eigen::Vector3f C(1.f, 2.f, 3.f); + Px offset = project(K, C + Eigen::Vector3f(1.f, 0.f, 0.f), R_wc, C); + CHECK(offset.u == doctest::Approx(K.cx)); + CHECK(offset.v == doctest::Approx(K.cy)); + CHECK(offset.depth == doctest::Approx(1.0)); +} + +// ── modelToString / modelFromString ─────────────────────────────────────────── + +TEST_CASE("modelFromString reads back every name modelToString writes") +{ + for (CameraModel m : { CameraModel::Pinhole, CameraModel::Mei, CameraModel::Fisheye }) + { + CAPTURE(modelToString(m)); + CHECK(modelFromString(modelToString(m)) == m); + } + CHECK(modelFromString("equidistant") == CameraModel::Fisheye); // the ROS/Kalibr name + CHECK(modelFromString("") == CameraModel::Pinhole); +} + +// ── Pinhole (regression: this path must not change) ─────────────────────────── + +TEST_CASE("pinhole is the default model") +{ + CHECK(Intrinsics{}.model == CameraModel::Pinhole); +} + +TEST_CASE("pinhole: projection matches hand-computed values") +{ + Intrinsics K; // fx = fy = 800, cx = 640, cy = 360 + + SUBCASE("undistorted") + { + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(840.0)); + CHECK(r.v == doctest::Approx(760.0)); + CHECK(r.depth == doctest::Approx(4.0)); + } + SUBCASE("radial numerator") + { + K.k1 = 0.1f; + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(846.25)); + CHECK(r.v == doctest::Approx(772.5)); + } + SUBCASE("rational denominator") + { + K.k1 = 0.1f; + K.k4 = 0.2f; + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(834.117647)); + CHECK(r.v == doctest::Approx(748.235294)); + } + SUBCASE("tangential") + { + K.p1 = 0.01f; + K.p2 = 0.02f; + Px r = project(K, { 1, 2, 4 }); + CHECK(r.u == doctest::Approx(849.0)); + CHECK(r.v == doctest::Approx(770.5)); + } +} + +TEST_CASE("pinhole: rejects points at or behind the camera plane") +{ + Intrinsics K; + float u, v, depth; + CHECK_FALSE(projectPoint(1, 2, -4, K, kIdentity, kOrigin, u, v, depth)); + CHECK_FALSE(projectPoint(1, 2, 0, K, kIdentity, kOrigin, u, v, depth)); +} + +// ── scaleRoi ────────────────────────────────────────────────────────────────── + +TEST_CASE("scaleRoi: a half-size image halves the rectangle") +{ + Roi r{ true, 100, 200, 40, 60 }; + Roi h = scaleRoi(r, 0.5f); + CHECK(h.enabled); + CHECK(h.x == 50); + CHECK(h.y == 100); + CHECK(h.w == 20); + CHECK(h.h == 30); +} + +TEST_CASE("scaleRoi: abutting rectangles stay abutting") +{ + // Scaling the width on its own would give both of these w == 2 and make + // them overlap at x == 2; rounding the two edges and subtracting cannot. + Roi a{ true, 1, 1, 3, 3 }; + Roi b{ true, 4, 4, 3, 3 }; + Roi as = scaleRoi(a, 0.5f); + Roi bs = scaleRoi(b, 0.5f); + CHECK(as.x + as.w == bs.x); + CHECK(as.y + as.h == bs.y); +} + +TEST_CASE("scaleRoi: a non-empty rectangle never scales down to empty") +{ + // w/h == 0 reads as "no ROI set", i.e. accept everything -- the exact + // opposite of what a ROI this small is asking for. + Roi tiny{ true, 10, 10, 2, 2 }; + Roi s = scaleRoi(tiny, 0.1f); + CHECK(s.w >= 1); + CHECK(s.h >= 1); +} + +TEST_CASE("scaleRoi: an unset rectangle is left alone") +{ + Roi none; + Roi s = scaleRoi(none, 0.5f); + CHECK_FALSE(s.enabled); + CHECK(s.w == 0); + CHECK(s.h == 0); +} + +// ── scaleIntrinsics ─────────────────────────────────────────────────────────── + +TEST_CASE("scaleIntrinsics: a half-size image projects to half the pixel") +{ + SUBCASE("pinhole") + { + Intrinsics K; + Intrinsics H = scaleIntrinsics(K, 0.5f); + CHECK(H.model == CameraModel::Pinhole); + CHECK(H.fx == doctest::Approx(400.0)); + CHECK(H.cx == doctest::Approx(320.0)); + + Px full = project(K, { 1, 2, 4 }); + Px half = project(H, { 1, 2, 4 }); + CHECK(half.u == doctest::Approx(full.u * 0.5)); + CHECK(half.v == doctest::Approx(full.v * 0.5)); + } + SUBCASE("distortion and model are carried over unchanged") + { + Intrinsics K; + K.k1 = 0.1f; + K.p2 = 0.02f; + Intrinsics H = scaleIntrinsics(K, 0.25f); + CHECK(H.k1 == doctest::Approx(0.1)); + CHECK(H.p2 == doctest::Approx(0.02)); + } + SUBCASE("mei") + { + Intrinsics K = mei(); + Intrinsics H = scaleIntrinsics(K, 0.5f); + CHECK(H.model == CameraModel::Mei); + CHECK(H.fx == doctest::Approx(K.fx * 0.5)); + CHECK(H.cx == doctest::Approx(K.cx * 0.5)); + CHECK(H.width == K.width / 2); + CHECK(H.height == K.height / 2); + // xi and the k*/p* polynomial are dimensionless, carried over as-is. + CHECK(H.xi == doctest::Approx(K.xi)); + CHECK(H.k1 == doctest::Approx(K.k1)); + CHECK(H.p2 == doctest::Approx(K.p2)); + + Px full = project(K, { 0.3f, -0.2f, 0.9f }); + Px half = project(H, { 0.3f, -0.2f, 0.9f }); + CHECK(half.u == doctest::Approx(full.u * 0.5)); + CHECK(half.v == doctest::Approx(full.v * 0.5)); + } + SUBCASE("fisheye") + { + Intrinsics K = fisheye(); + Intrinsics H = scaleIntrinsics(K, 0.5f); + CHECK(H.model == CameraModel::Fisheye); + CHECK(H.k4 == doctest::Approx(K.k4)); // theta coefficients are dimensionless + + Px full = project(K, { 2.0f, 1.0f, 0.3f }); + Px half = project(H, { 2.0f, 1.0f, 0.3f }); + CHECK(half.u == doctest::Approx(full.u * 0.5)); + CHECK(half.v == doctest::Approx(full.v * 0.5)); + } +} +// ── CameraIdentity::empty ───────────────────────────────────────────────────── + +TEST_CASE("CameraIdentity::empty: a default-constructed identity is empty") +{ + CHECK(CameraIdentity{}.empty()); +} + +TEST_CASE("CameraIdentity::empty: a serial alone makes it non-empty") +{ + CameraIdentity id; + id.serial = "SN-1"; + CHECK_FALSE(id.empty()); +} + +TEST_CASE("CameraIdentity::empty: a frame_id alone makes it non-empty") +{ + CameraIdentity id; + id.frameId = "camera_front"; + CHECK_FALSE(id.empty()); +} + +TEST_CASE("CameraIdentity::empty: model/firmware alone do not count") +{ + // Only serial/frameId identify a physical camera; model and firmware are + // descriptive metadata that can be present without either. + CameraIdentity id; + id.model = "Insta360 X4"; + id.firmware = "1.2.3"; + CHECK(id.empty()); +} + +// ── loadCameraInfoYaml ──────────────────────────────────────────────────────── + +namespace +{ + // Writes `body` to a temp file and loads it into K, so the parser is + // exercised through its real file-reading path. + bool loadIntoFromString(const std::string& body, Intrinsics& K) + { + const std::string path = (std::filesystem::temp_directory_path() / "calib_core_test_camera_info.yaml").string(); + { + std::ofstream f(path); + f << body; + } + const bool ok = loadCameraInfoYaml(path, K); + std::filesystem::remove(path); + return ok; + } + + // As above, into a fresh Intrinsics. Returns nullopt when the load failed. + std::optional loadFromString(const std::string& body) + { + Intrinsics K; + return loadIntoFromString(body, K) ? std::optional(K) : std::nullopt; + } + + // As above, for loadCameraIdentity. `id` is only meaningful when this + // returns true. + bool loadIdentityFromString(const std::string& body, CameraIdentity& id) + { + const std::string path = (std::filesystem::temp_directory_path() / "calib_core_test_camera_info.yaml").string(); + { + std::ofstream f(path); + f << body; + } + const bool ok = loadCameraIdentity(path, id); + std::filesystem::remove(path); + return ok; + } + + const char* kSample = R"(# this rig's camera_info.yaml +frame_id: camera_front +distortion_model: insta360_mei_v2 +width: 3840 +height: 1920 +fx: 620.5 +fy: 621.25 +cx: 959.5 +cy: 539.5 +xi: 1.234 +distortion: [-0.0123, 0.0045, -0.0007, 0.0011, -0.0002] +)"; + + // Verbatim from insta360-test-calib's SaveCamera, comment line included. + const char* kFisheyeSample = + R"(# calib_app: intrinsics re-estimated from equidistant, 5 views, 422 corners: rms 0.556 px (was 6.787), 40 iterations; started from a generic guess +width: 2880 +height: 2880 +distortion_model: equidistant +fx: 634.3157288 +fy: 633.5613727 +cx: 1454.934903 +cy: 1429.728826 +distortion: [0.1417399868, -0.01614857261, 0.01912948205, -0.006671667431] +k: [634.3157288, 0, 1454.934903, 0, 633.5613727, 1429.728826, 0, 0, 1] +r: [1, 0, 0, 0, 1, 0, 0, 0, 1] +p: [634.3157288, 0, 1454.934903, 0, 0, 633.5613727, 1429.728826, 0, 0, 0, 1, 0] +)"; + + // The same file with `distortion_model` and `distortion` replaced. + std::string withModel(const std::string& model, const std::string& distortion) + { + std::string body = kFisheyeSample; + auto replaceLine = [&](const std::string& key, const std::string& value) + { + const auto at = body.find("\n" + key + ": "); + REQUIRE(at != std::string::npos); + const auto end = body.find('\n', at + 1); + body.replace(at + 1, end - at - 1, key + ": " + value); + }; + replaceLine("distortion_model", model); + replaceLine("distortion", distortion); + return body; + } +} // namespace + +TEST_CASE("loadCameraInfoYaml: reads this rig's flat camera_info.yaml") +{ + const auto K = loadFromString(kSample); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Mei); + CHECK(K->width == 3840); + CHECK(K->height == 1920); + CHECK(K->fx == doctest::Approx(620.5)); + CHECK(K->cy == doctest::Approx(539.5)); + CHECK(K->xi == doctest::Approx(1.234)); + // distortion is (k1, k2, k3, p1, p2) -- NOT OpenCV's pinhole order. + CHECK(K->k1 == doctest::Approx(-0.0123)); + CHECK(K->k2 == doctest::Approx(0.0045)); + CHECK(K->k3 == doctest::Approx(-0.0007)); + CHECK(K->p1 == doctest::Approx(0.0011)); + CHECK(K->p2 == doctest::Approx(-0.0002)); + // The Mei polynomial has no rational denominator. + CHECK(K->k4 == 0.f); + CHECK(K->k5 == 0.f); + CHECK(K->k6 == 0.f); +} + +TEST_CASE("loadCameraInfoYaml: quotes and trailing comments are not taken literally") +{ + std::string body = kSample; + body += "\nxi: 0.75 # trailing comment\n"; + const auto K = loadFromString(body); + REQUIRE(K.has_value()); + CHECK(K->xi == doctest::Approx(0.75)); +} + +TEST_CASE("loadCameraInfoYaml: a missing field fails instead of defaulting to 0") +{ + // A calibration that silently reads xi as 0 reprojects wrongly with no + // visible failure, so the load has to reject it outright. + std::string body = kSample; + const auto at = body.find("xi: 1.234\n"); + REQUIRE(at != std::string::npos); + body.erase(at, std::string("xi: 1.234\n").size()); + + CHECK_FALSE(loadFromString(body).has_value()); +} + +TEST_CASE("loadCameraInfoYaml: a missing file fails cleanly, and leaves K alone") +{ + Intrinsics K = mei(); + const Intrinsics before = K; + CHECK_FALSE(loadCameraInfoYaml("/nonexistent/camera_info.yaml", K)); + CHECK(K.fx == before.fx); + CHECK(K.xi == before.xi); +} + +TEST_CASE("loadCameraInfoYaml: reads insta360-test-calib's equidistant file") +{ + const auto K = loadFromString(kFisheyeSample); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Fisheye); + CHECK(K->width == 2880); + CHECK(K->height == 2880); + CHECK(K->fx == doctest::Approx(634.3157288)); + CHECK(K->fy == doctest::Approx(633.5613727)); + CHECK(K->cx == doctest::Approx(1454.934903)); + CHECK(K->cy == doctest::Approx(1429.728826)); + CHECK(K->k1 == doctest::Approx(0.1417399868)); + CHECK(K->k2 == doctest::Approx(-0.01614857261)); + CHECK(K->k3 == doctest::Approx(0.01912948205)); + CHECK(K->k4 == doctest::Approx(-0.006671667431)); + CHECK(K->p1 == 0.f); + CHECK(K->p2 == 0.f); + CHECK(K->xi == 0.f); + + Px r = project(*K, { 0.f, 0.f, 3.f }); + CHECK(r.u == doctest::Approx(K->cx)); + CHECK(r.v == doctest::Approx(K->cy)); +} + +TEST_CASE("loadCameraInfoYaml: distortion_model fisheye is read as equidistant") +{ + const auto K = loadFromString(withModel("fisheye", "[0.1, 0.01, 0, 0]")); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Fisheye); + CHECK(K->k1 == doctest::Approx(0.1)); +} + +TEST_CASE("loadCameraInfoYaml: equidistant needs exactly four coefficients, and a failure leaves K alone") +{ + Intrinsics K = mei(); + const Intrinsics before = K; + CHECK_FALSE(loadIntoFromString(withModel("equidistant", "[0.1, 0.01, 0, 0, 0.5]"), K)); + CHECK(K.model == before.model); + CHECK(K.fx == before.fx); + CHECK(K.k1 == before.k1); +} + +TEST_CASE("loadCameraInfoYaml: the pinhole models take OpenCV's coefficient order") +{ + // Unlike Mei's (k1, k2, k3, p1, p2), p1/p2 come before k3. + SUBCASE("plumb_bob") + { + const auto K = loadFromString(withModel("plumb_bob", "[0.1, -0.2, 0.001, 0.002, 0.05]")); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Pinhole); + CHECK(K->k1 == doctest::Approx(0.1)); + CHECK(K->k2 == doctest::Approx(-0.2)); + CHECK(K->p1 == doctest::Approx(0.001)); + CHECK(K->p2 == doctest::Approx(0.002)); + CHECK(K->k3 == doctest::Approx(0.05)); + CHECK(K->k4 == 0.f); + } + SUBCASE("rational_polynomial") + { + const auto K = loadFromString(withModel("rational_polynomial", "[0.4, -0.05, 0.0006, -0.0004, 0.001, 0.7, -0.02, 0.005]")); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Pinhole); + CHECK(K->p1 == doctest::Approx(0.0006)); + CHECK(K->k3 == doctest::Approx(0.001)); + CHECK(K->k4 == doctest::Approx(0.7)); + CHECK(K->k5 == doctest::Approx(-0.02)); + CHECK(K->k6 == doctest::Approx(0.005)); + } +} + +TEST_CASE("loadCameraInfoYaml: an unknown distortion_model fails rather than falling back to pinhole") +{ + CHECK_FALSE(loadFromString(withModel("scaramuzza", "[0, 0, 0, 0]")).has_value()); +} + +TEST_CASE("loadCameraInfoYaml: xi with no distortion_model is read as Mei") +{ + std::string body = kSample; + const auto at = body.find("distortion_model: insta360_mei_v2\n"); + REQUIRE(at != std::string::npos); + body.erase(at, std::string("distortion_model: insta360_mei_v2\n").size()); + + const auto K = loadFromString(body); + REQUIRE(K.has_value()); + CHECK(K->model == CameraModel::Mei); + CHECK(K->xi == doctest::Approx(1.234)); +} + +// ── loadCameraIdentity ──────────────────────────────────────────────────────── +// Independent of loadCameraInfoYaml -- opens the same kind of file again on +// its own and only ever looks at `serial`/`frame_id`/`model`, so these tests +// don't depend on the intrinsics fields being present or valid at all. + +TEST_CASE("loadCameraIdentity: reads serial, frame_id and model when all are present") +{ + std::string body = kSample; + body += "\nserial: SN-12345\nmodel: Insta360 X4\n"; + + CameraIdentity id; + CHECK(loadIdentityFromString(body, id)); + CHECK(id.serial == "SN-12345"); + CHECK(id.frameId == "camera_front"); + CHECK(id.model == "Insta360 X4"); +} + +TEST_CASE("loadCameraIdentity: a field the file does not name comes back empty") +{ + // kSample has frame_id but no serial. + CameraIdentity id; + CHECK(loadIdentityFromString(kSample, id)); + CHECK(id.serial.empty()); + CHECK(id.frameId == "camera_front"); +} + +TEST_CASE("loadCameraIdentity: a successful load clears a previously-populated id") +{ + // Loading a file that names no camera must drop the previous identity + // rather than leave it attached to a different one. + CameraIdentity id; + id.serial = "stale-serial"; + id.model = "stale-model"; + id.firmware = "stale-firmware"; + + CHECK(loadIdentityFromString(kSample, id)); + CHECK(id.serial.empty()); + CHECK(id.model.empty()); + CHECK(id.firmware.empty()); + CHECK(id.frameId == "camera_front"); +} + +TEST_CASE("loadCameraIdentity: quotes around a value are not taken literally") +{ + std::string body = kSample; + body += "\nserial: \"SN-12345\"\n"; + + CameraIdentity id; + CHECK(loadIdentityFromString(body, id)); + CHECK(id.serial == "SN-12345"); +} + +TEST_CASE("loadCameraIdentity: neither field present comes back empty, not a failure") +{ + // Unlike loadCameraInfoYaml, no field here is required -- a file that + // simply doesn't name a camera is a valid, successful "no identity". + std::string body = "distortion_model: insta360_mei_v2\nwidth: 640\n"; + CameraIdentity id; + CHECK(loadIdentityFromString(body, id)); + CHECK(id.empty()); +} + +TEST_CASE("loadCameraIdentity: a missing file fails cleanly, and leaves id alone") +{ + CameraIdentity id; + id.serial = "untouched"; + + CHECK_FALSE(loadCameraIdentity("/nonexistent/camera_info.yaml", id)); + CHECK(id.serial == "untouched"); +} + +// ── LoadTimestampFromSideCar ──────────────────────────────────────────────── + +namespace +{ + // Real-world sample, trimmed from a libcamera-style .meta.json sidecar + // next to a captured frame -- FRAME_WALL_CLOCK is a quoted nanosecond + // epoch string, not a bare JSON number. + const char* kMetaSample = R"({ + "AE_STATE": "2", + "ANALOGUE_GAIN": "1.000000", + "EXPOSURE_TIME": 6.34, + "FRAME_DURATION": 16.68, + "FRAME_WALL_CLOCK": "1789125060554994432", + "LUX": "580.969055" +})"; + + // Writes `metaBody` to "/.meta.json" and calls + // LoadTimestampFromSideCar on "/." (a file that need not + // itself exist -- only the sidecar is read). + std::optional loadTimestampForStem(const std::string& stem, const std::string& ext, const std::string& metaBody) + { + const auto dir = std::filesystem::temp_directory_path(); + const std::string sidecar = (dir / (stem + ".meta.json")).string(); + { + std::ofstream f(sidecar); + f << metaBody; + } + const auto result = LoadTimestampFromSideCar((dir / (stem + "." + ext)).string()); + std::filesystem::remove(sidecar); + return result; + } +} // namespace + +TEST_CASE("LoadTimestampFromSideCar: reads FRAME_WALL_CLOCK from the image's .meta.json") +{ + const auto ts = loadTimestampForStem("calib_core_test_cam0_frame", "jpg", kMetaSample); + REQUIRE(ts.has_value()); + CHECK(*ts == doctest::Approx(1789125060554994432.0)); +} + +TEST_CASE("LoadTimestampFromSideCar: a missing sidecar returns nullopt") +{ + const auto dir = std::filesystem::temp_directory_path(); + const auto missing = (dir / "calib_core_test_no_such_frame.jpg").string(); + CHECK_FALSE(LoadTimestampFromSideCar(missing).has_value()); +} + +TEST_CASE("LoadTimestampFromSideCar: a sidecar with no FRAME_WALL_CLOCK returns nullopt") +{ + const auto ts = loadTimestampForStem("calib_core_test_cam0_nofield", "jpg", R"({"LUX": "580.969055"})"); + CHECK_FALSE(ts.has_value()); +} + +TEST_CASE("LoadTimestampFromSideCar: an unquoted numeric value is read too") +{ + const auto ts = loadTimestampForStem("calib_core_test_cam0_unquoted", "jpg", R"({"FRAME_WALL_CLOCK": 1789125060554994432})"); + REQUIRE(ts.has_value()); + CHECK(*ts == doctest::Approx(1789125060554994432.0)); +} diff --git a/calib_core/tests/test_solver.cpp b/calib_core/tests/test_solver.cpp new file mode 100644 index 00000000..095236be --- /dev/null +++ b/calib_core/tests/test_solver.cpp @@ -0,0 +1,160 @@ +// Solver tests, split out of test_camera.cpp (which owns doctest's +// DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN / main()) -- this file just registers +// more TEST_CASEs into the same executable/registry, same multi-TU doctest +// setup shared/tests uses. +#include + +#include + +#include + +using namespace calib; + +namespace +{ + Intrinsics meiIntrinsics() + { + Intrinsics K; + K.model = CameraModel::Mei; + K.fx = 300.f; K.fy = 300.f; + K.cx = 320.f; K.cy = 240.f; + K.xi = 1.2f; + K.k1 = -0.15f; K.k2 = 0.02f; K.k3 = -0.001f; + K.p1 = 0.001f; K.p2 = -0.0005f; + return K; + } +} // namespace + +#ifndef CALIB_ENABLE_CERES + +TEST_CASE("solveExtrinsicsCeres: stub explains the build flag when Ceres is disabled") +{ + std::vector corr(3); // content doesn't matter -- fails before using it + Extrinsics E; + std::string err; + CHECK_FALSE(solveExtrinsicsCeres(corr, meiIntrinsics(), E, err)); + CHECK(err.find("CALIB_ENABLE_CERES") != std::string::npos); +} + +#else // CALIB_ENABLE_CERES + +namespace +{ + // Coefficients in the range a real ~180 deg OpenCV fisheye calibration + // produces, not a specific camera. + Intrinsics fisheyeIntrinsics() + { + Intrinsics K; + K.model = CameraModel::Fisheye; + K.fx = 285.f; K.fy = 286.f; + K.cx = 322.f; K.cy = 238.f; + K.k1 = -0.0075f; K.k2 = 0.0435f; K.k3 = -0.0414f; K.k4 = 0.0077f; + return K; + } + + // Ground truth this test solves for, expressed the same way + // AppState::saveCalibration/loadCalibration do: camera position + a + // small om/fi/ka deviation from kCameraLidarAxisOffset. + Extrinsics groundTruthExtrinsics() + { + Extrinsics E; + E.tx = 1.5f; E.ty = -0.3f; E.tz = 0.8f; + E.om = 4.f; E.fi = -6.f; E.ka = 2.f; + return E; + } + + // A handful of LiDAR-frame points spread across the field of view, + // roughly in front of groundTruthExtrinsics()'s camera. The last one is + // ~100 deg off-axis, past where a pinhole model could see it. + const Eigen::Vector3f kLidarPoints[] = { + { 3.f, 0.f, 0.f }, { 4.f, 1.5f, 0.5f }, { 5.f, -1.f, -0.5f }, { 3.5f, 0.8f, -0.8f }, + { 6.f, -1.8f, 1.f }, { 4.5f, 0.3f, 1.2f }, { 3.f, -0.6f, 0.4f }, { 1.f, 3.f, 0.5f }, + }; + + std::vector syntheticCorrespondences(const Intrinsics& K, const Extrinsics& truth) + { + const Eigen::Matrix3f R_wc = omFiKaToMat3(truth.om, truth.fi, truth.ka); + const Eigen::Vector3f C(truth.tx, truth.ty, truth.tz); + + std::vector corr; + for (const auto& p : kLidarPoints) + { + float u, v, depth; + REQUIRE(projectPoint(p.x(), p.y(), p.z(), K, R_wc, C, u, v, depth)); + PointPixelCorrespondence c; + c.p = p.cast(); + c.u = u; + c.v = v; + corr.push_back(c); + } + return corr; + } +} // namespace + +TEST_CASE("solveExtrinsicsCeres: recovers known extrinsics from synthetic correspondences") +{ + for (const Intrinsics& K : { meiIntrinsics(), fisheyeIntrinsics() }) + { + CAPTURE(modelToString(K.model)); + const Extrinsics truth = groundTruthExtrinsics(); + const auto corr = syntheticCorrespondences(K, truth); + + // Perturbed initial guess -- a solver that just echoed its input back + // unchanged (e.g. Ceres silently failing to run and IsSolutionUsable() + // being CHECK_FALSE'd elsewhere) would not pass this. + Extrinsics guess = truth; + guess.tx += 0.3f; guess.ty -= 0.2f; guess.tz += 0.15f; + guess.om += 2.f; guess.fi -= 1.5f; guess.ka += 1.f; + + double rms = -1.0; + std::string err; + REQUIRE(solveExtrinsicsCeres(corr, K, guess, err, &rms, false)); + + CHECK(rms < 0.5); // px -- points are noise-free, should fit almost exactly + CHECK(guess.tx == doctest::Approx(truth.tx).epsilon(1e-3)); + CHECK(guess.ty == doctest::Approx(truth.ty).epsilon(1e-3)); + CHECK(guess.tz == doctest::Approx(truth.tz).epsilon(1e-3)); + CHECK(guess.om == doctest::Approx(truth.om).epsilon(1e-2)); + CHECK(guess.fi == doctest::Approx(truth.fi).epsilon(1e-2)); + CHECK(guess.ka == doctest::Approx(truth.ka).epsilon(1e-2)); + } +} + +TEST_CASE("solveExtrinsicsCeres: fixTranslation leaves tx/ty/tz untouched") +{ + for (const Intrinsics& K : { meiIntrinsics(), fisheyeIntrinsics() }) + { + CAPTURE(modelToString(K.model)); + const Extrinsics truth = groundTruthExtrinsics(); + const auto corr = syntheticCorrespondences(K, truth); + + Extrinsics guess = truth; + guess.om += 3.f; guess.fi -= 2.f; guess.ka += 1.5f; + const float lockedTx = guess.tx, lockedTy = guess.ty, lockedTz = guess.tz; + + double rms = -1.0; + std::string err; + REQUIRE(solveExtrinsicsCeres(corr, K, guess, err, &rms, /*fixTranslation=*/true)); + + CHECK(guess.tx == doctest::Approx(lockedTx)); + CHECK(guess.ty == doctest::Approx(lockedTy)); + CHECK(guess.tz == doctest::Approx(lockedTz)); + CHECK(guess.om == doctest::Approx(truth.om).epsilon(1e-2)); + CHECK(guess.fi == doctest::Approx(truth.fi).epsilon(1e-2)); + CHECK(guess.ka == doctest::Approx(truth.ka).epsilon(1e-2)); + } +} + +TEST_CASE("solveExtrinsicsCeres: a model it has no residual for fails and says why") +{ + // Routing a pinhole camera here by mistake would otherwise solve it as Mei. + Intrinsics K; // Pinhole + std::vector corr(3); + Extrinsics E = groundTruthExtrinsics(); + std::string err; + CHECK_FALSE(solveExtrinsicsCeres(corr, K, E, err)); + CHECK(err.find("pinhole") != std::string::npos); + CHECK(E.tx == groundTruthExtrinsics().tx); +} + +#endif // CALIB_ENABLE_CERES \ No newline at end of file diff --git a/calib_core/tests/test_trajectory.cpp b/calib_core/tests/test_trajectory.cpp new file mode 100644 index 00000000..74d8755c --- /dev/null +++ b/calib_core/tests/test_trajectory.cpp @@ -0,0 +1,32 @@ +// Trajectory CSV loading tests; registers into test_camera.cpp's doctest main(). +#include + +#include + +#include +#include + +using namespace calib; + +TEST_CASE("Trajectory::loadCSV: fractional timestamp does not shift the pose columns") +{ + // lidar_odometry_step_1 writes seconds * 1e9 as a double, so a row can + // carry a fraction like this one (seen in a real session). + const auto path = std::filesystem::temp_directory_path() / "calib_core_test_trajectory_lio_0.csv"; + { + std::ofstream f(path); + f << "timestamp_nanoseconds pose00 pose01 pose02 pose03 pose10 pose11 pose12 pose13 pose20 pose21 pose22 pose23 " + "timestampUnix_nanoseconds om_rad fi_rad ka_rad\n"; + f << "548348730180 1 0 0 1 0 1 0 2 0 0 1 3 0 0 0 0\n"; + f << "548348730189.99993896 1 0 0 4 0 1 0 5 0 0 1 6 0 0 0 0\n"; + } + + Trajectory t; + REQUIRE(t.loadCSV(path.string())); + std::filesystem::remove(path); + + REQUIRE(t.poses.size() == 2); + CHECK(t.poses[1].ts_ns == 548348730190); + CHECK(t.poses[1].T.linear().isIdentity(1e-6f)); + CHECK(t.poses[1].T.translation().isApprox(Eigen::Vector3f(4.f, 5.f, 6.f))); +} diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt index 3c353cbc..5e38ab80 100644 --- a/core/CMakeLists.txt +++ b/core/CMakeLists.txt @@ -116,8 +116,8 @@ function(add_core_target target_name with_gui) endif() endfunction() -# core_no_gui is used by python bindings and lidar odometry test subprojects only - if they are not build just do not waste time building core_no_gui -if(BUILD_TESTING OR BUILD_WITH_PYBIND) +# core_no_gui is used by python bindings, lidar odometry test subprojects and session_to_mcap (console tools) only - if they are not build just do not waste time building core_no_gui +if(BUILD_TESTING OR BUILD_WITH_PYBIND OR BUILD_WITH_CLI_TOOLS) add_core_target(core_no_gui FALSE) target_precompile_headers(core_no_gui diff --git a/deploy_mandeye.bat b/deploy_mandeye.bat index ab24d95c..74fef193 100644 --- a/deploy_mandeye.bat +++ b/deploy_mandeye.bat @@ -60,7 +60,7 @@ rem This list has to be kept in sync by hand with the hdmapping_install_app() rem calls in apps/*/CMakeLists.txt. hd_mapper is deliberately absent: it is rem gated behind BUILD_WITH_HD_MAPPER_APPLICATION, which defaults to OFF, so rem listing it would warn on every normal build. -set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe mandeye_with_360_camera_manual_coloring.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_session_registration_step_3_legacy.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe single_session_manual_coloring.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll" +set "EXPECTED=camera_lidar_calibration.exe camera_lidar_intrinsics_calib.exe camera_lidar_trajectory_viewer.exe concatenate_multi_livox.exe drag_folder_with_mandeye_data_and_drop_here-precision_forestry.exe laz_to_mcap.exe laz_to_pcd.exe laz_to_ply.exe laz_to_txt.exe lidar_odometry_step_1.exe livox_mid_360_intrinsic_calibration.exe mandeye_compare_trajectories.exe mandeye_mission_recorder_calibration.exe mandeye_raw_data_viewer.exe mandeye_single_session_viewer.exe matrix_mul.exe mcap_to_laz.exe multiply_timestamps_session_point_cloud_laz.exe multiply_timestamps_session_trajectory_csv.exe multi_session_registration_step_3.exe multi_session_registration_step_3_legacy.exe multi_view_tls_registration_step_2.exe multi_view_tls_registration_step_2_legacy.exe pcd_to_laz.exe precision_forestry_tools.exe session_to_mcap.exe split_multi_livox.exe freeglut.dll laszip3.dll opencv_world4130.dll proj_9_3.dll tbb12.dll z.dll" set "MISSING=" for %%F in (%EXPECTED%) do ( diff --git a/rosbags/McapWriter.cpp b/rosbags/McapWriter.cpp index 93b05e76..64fdaa1f 100644 --- a/rosbags/McapWriter.cpp +++ b/rosbags/McapWriter.cpp @@ -50,6 +50,37 @@ uint8 FLOAT64=8 static constexpr const char* kStringSchema = R"(string data )"; +static constexpr const char* kTfMessageSchema = R"(geometry_msgs/TransformStamped[] transforms +================================================================================ +MSG: geometry_msgs/TransformStamped +std_msgs/Header header +string child_frame_id +geometry_msgs/Transform transform +================================================================================ +MSG: std_msgs/Header +builtin_interfaces/Time stamp +string frame_id +================================================================================ +MSG: builtin_interfaces/Time +int32 sec +uint32 nanosec +================================================================================ +MSG: geometry_msgs/Transform +geometry_msgs/Vector3 translation +geometry_msgs/Quaternion rotation +================================================================================ +MSG: geometry_msgs/Vector3 +float64 x +float64 y +float64 z +================================================================================ +MSG: geometry_msgs/Quaternion +float64 x +float64 y +float64 z +float64 w +)"; + static constexpr const char* kImuSchema = R"(std_msgs/Header header geometry_msgs/Quaternion orientation float64[9] orientation_covariance @@ -302,6 +333,32 @@ static std::vector serializeImu(uint64_t timestamp_ns, const McapImuSam return w.data(); } +// tf2_msgs/msg/TFMessage carrying a single TransformStamped, matching how a +// real /tf topic publishes one changed transform per message. +static std::vector serializeTf( + uint64_t timestamp_ns, const McapTransform& t, const std::string& parent_frame, const std::string& child_frame) +{ + CdrWriter w; + + w.write_u32(1); // transforms[] sequence length + + writeHeader(w, timestamp_ns, parent_frame); // TransformStamped.header + w.write_string(child_frame); + + // transform.translation + w.write_f64(t.tx); + w.write_f64(t.ty); + w.write_f64(t.tz); + + // transform.rotation + w.write_f64(t.qx); + w.write_f64(t.qy); + w.write_f64(t.qz); + w.write_f64(t.qw); + + return w.data(); +} + // --------------------------------------------------------------------------- // Impl // --------------------------------------------------------------------------- @@ -312,9 +369,11 @@ struct McapFileWriter::Impl mcap::ChannelId lidarChannelId{0}; mcap::ChannelId imuChannelId{0}; mcap::ChannelId snChannelId{0}; + mcap::ChannelId tfChannelId{0}; uint32_t lidarSequence{0}; uint32_t imuSequence{0}; uint32_t snSequence{0}; + uint32_t tfSequence{0}; McapWriterOptions options; bool open{false}; }; @@ -369,6 +428,16 @@ McapFileWriter::McapFileWriter(const std::filesystem::path& path, const McapWrit impl_->writer.addChannel(snChannel); impl_->snChannelId = snChannel.id; + // Register tf2_msgs/msg/TFMessage schema + /tf channel + mcap::Schema tfSchema("tf2_msgs/msg/TFMessage", "ros2msg", + {reinterpret_cast(kTfMessageSchema), + reinterpret_cast(kTfMessageSchema) + std::strlen(kTfMessageSchema)}); + impl_->writer.addSchema(tfSchema); + + mcap::Channel tfChannel(impl_->options.tf_topic, "cdr", tfSchema.id); + impl_->writer.addChannel(tfChannel); + impl_->tfChannelId = tfChannel.id; + impl_->open = true; } @@ -409,8 +478,8 @@ void McapFileWriter::writePointCloud(uint64_t timestamp_ns, const std::vectoroptions.frame_id, impl_->options.lidar_layout); + const std::string& frame_id = impl_->options.pointcloud_frame_id.empty() ? impl_->options.frame_id : impl_->options.pointcloud_frame_id; + auto payload = serializePointCloud2(timestamp_ns, points, frame_id, impl_->options.lidar_layout); mcap::Message msg; msg.channelId = impl_->lidarChannelId; @@ -452,4 +521,31 @@ void McapFileWriter::writeImu(const std::vector& imu) writeImuSample(sample); } +void McapFileWriter::writeTfSample(const McapTransform& transform) +{ + if(!isOpen()) + return; + + const uint64_t ts = static_cast(transform.timestamp * 1e9); + auto payload = serializeTf(ts, transform, impl_->options.map_frame, impl_->options.frame_id); + + mcap::Message msg; + msg.channelId = impl_->tfChannelId; + msg.sequence = impl_->tfSequence++; + msg.publishTime = ts; + msg.logTime = ts; + msg.data = reinterpret_cast(payload.data()); + msg.dataSize = payload.size(); + + auto s = impl_->writer.write(msg); + if(!s.ok()) + std::cerr << "McapWriter: tf write error: " << s.message << "\n"; +} + +void McapFileWriter::writeTf(const std::vector& transforms) +{ + for(const auto& t : transforms) + writeTfSample(t); +} + } // namespace rosbags \ No newline at end of file diff --git a/rosbags/McapWriter.h b/rosbags/McapWriter.h index 1291feee..d083b63e 100644 --- a/rosbags/McapWriter.h +++ b/rosbags/McapWriter.h @@ -55,6 +55,23 @@ struct McapImuSample float acc_z{}; }; +// One rigid transform sample (parent -> child), written as a single-element +// tf2_msgs/msg/TFMessage -- one message per sample, matching how a real /tf +// topic carries one changed transform per publish. `timestamp` is an +// absolute timestamp in seconds. Rotation must be a unit quaternion; the +// default is identity. +struct McapTransform +{ + double timestamp{}; + double tx{}; + double ty{}; + double tz{}; + double qx{}; + double qy{}; + double qz{}; + double qw{1.0}; +}; + // Selects the sensor_msgs/msg/PointCloud2 field layout the lidar channel is // written with. The message type is always PointCloud2 -- only the `fields` // array/point_step (and thus which McapPoint members get written) changes, @@ -70,9 +87,18 @@ enum class PointCloudLayout struct McapWriterOptions { std::string frame_id = "lidar"; + // Overrides frame_id for PointCloud2 headers only (Imu headers and the + // /tf child_frame_id keep using frame_id). Left empty, PointCloud2 also + // uses frame_id -- unchanged default behavior. Set this to distinguish a + // point cloud published in a fixed frame (e.g. "map", already + // motion-compensated) from a sensor's own moving frame. + std::string pointcloud_frame_id; + // Parent frame written into /tf's TransformStamped.header.frame_id. + std::string map_frame = "map"; std::string lidar_topic = "/lidar_points"; std::string imu_topic = "/imu"; std::string sn_topic = "/lidar_sn"; + std::string tf_topic = "/tf"; PointCloudLayout lidar_layout = PointCloudLayout::Generic; }; @@ -84,6 +110,7 @@ struct McapWriterOptions // /lidar_points — sensor_msgs/msg/PointCloud2 (field layout per options().lidar_layout) // /imu — sensor_msgs/msg/Imu // /lidar_sn — std_msgs/msg/String +// /tf — tf2_msgs/msg/TFMessage (one TransformStamped per message) // // PointCloud2 field layouts (see PointCloudLayout): // Generic (point_step = 28): @@ -140,6 +167,13 @@ class McapFileWriter // Write a string to /lidar_sn (std_msgs/msg/String). void writeSn(uint64_t timestamp_ns, const std::string& data); + // Write a single transform as its own tf2_msgs/msg/TFMessage (one + // TransformStamped, parent = options().map_frame, child = options().frame_id). + void writeTfSample(const McapTransform& transform); + + // Write a batch of transforms, one /tf message per sample. + void writeTf(const std::vector& transforms); + bool isOpen() const; private: diff --git a/rosbags/cdr_serializer.hpp b/rosbags/cdr_serializer.hpp index 8a17e4d0..a4ab196d 100644 --- a/rosbags/cdr_serializer.hpp +++ b/rosbags/cdr_serializer.hpp @@ -788,4 +788,34 @@ inline std::string decodeSn(const uint8_t* data, size_t size) return r.ok() ? s : std::string{}; } +// tf2_msgs/msg/TFMessage → McapTransform (first TransformStamped only; McapWriter +// never writes more than one) +inline std::optional decodeTf(const uint8_t* data, size_t size) +{ + CdrReader r(data, size); + + const uint32_t n = r.read_u32(); // transforms[] sequence length + if(n == 0) + return std::nullopt; + + const int32_t stamp_sec = r.read_i32(); + const uint32_t stamp_nsec = r.read_u32(); + r.read_string(); // frame_id (parent) + r.read_string(); // child_frame_id + + McapTransform t{}; + t.timestamp = static_cast(stamp_sec) + static_cast(stamp_nsec) * 1e-9; + t.tx = r.read_f64(); + t.ty = r.read_f64(); + t.tz = r.read_f64(); + t.qx = r.read_f64(); + t.qy = r.read_f64(); + t.qz = r.read_f64(); + t.qw = r.read_f64(); + + if(!r.ok()) + return std::nullopt; + return t; +} + } // namespace rosbags \ No newline at end of file diff --git a/rosbags/tests/test_mcap_reader.cpp b/rosbags/tests/test_mcap_reader.cpp index 8ce96fb5..e2efc861 100644 --- a/rosbags/tests/test_mcap_reader.cpp +++ b/rosbags/tests/test_mcap_reader.cpp @@ -261,7 +261,7 @@ TEST_CASE("McapFileReader: topic resolution") CHECK(reader.topics().lidar == "/custom/points"); CHECK(reader.topics().imu == "/custom/imu"); CHECK(reader.topics().sn == "/custom/sn"); - CHECK(reader.channels().size() == 3); + CHECK(reader.channels().size() == 4); // lidar, imu, sn, tf are always registered } SUBCASE("an explicit topic is honored") @@ -280,7 +280,7 @@ TEST_CASE("McapFileReader: topic resolution") rosbags::McapFileReader reader(path, options); CHECK_FALSE(reader.isOpen()); CHECK(reader.error().find("/does/not/exist") != std::string::npos); - CHECK(reader.channels().size() == 3); // --list still works on an unresolvable file + CHECK(reader.channels().size() == 4); // --list still works on an unresolvable file } fs::remove(path); diff --git a/rosbags/tests/test_mcap_writer.cpp b/rosbags/tests/test_mcap_writer.cpp index 428c54d8..0dcc798d 100644 --- a/rosbags/tests/test_mcap_writer.cpp +++ b/rosbags/tests/test_mcap_writer.cpp @@ -262,6 +262,54 @@ TEST_CASE("McapFileWriter: IMU round-trip") fs::remove(path); } +TEST_CASE("McapFileWriter: TF round-trip") +{ + const auto path = tempMcapPath("hdmapping_test_tf.mcap"); + + std::vector transforms; + for (int i = 0; i < 5; ++i) + { + rosbags::McapTransform t{}; + t.timestamp = 6000.0 + i * 0.1; + t.tx = 1.0 * i; + t.ty = -2.0 * i; + t.tz = 0.5; + t.qx = 0.0; + t.qy = 0.0; + t.qz = 0.0; + t.qw = 1.0; + transforms.push_back(t); + } + + { + rosbags::McapWriterOptions options; + options.frame_id = "lidar"; + options.map_frame = "map"; + rosbags::McapFileWriter writer(path, options); + REQUIRE(writer.isOpen()); + writer.writeTf(transforms); + } + + const auto decoded = readTopic>( + path, "/tf", [](const uint8_t* d, size_t n) + { + return rosbags::decodeTf(d, n); + }); + + REQUIRE(decoded.size() == transforms.size()); + for (size_t i = 0; i < transforms.size(); ++i) + { + REQUIRE(decoded[i].has_value()); + CHECK(decoded[i]->tx == doctest::Approx(transforms[i].tx)); + CHECK(decoded[i]->ty == doctest::Approx(transforms[i].ty)); + CHECK(decoded[i]->tz == doctest::Approx(transforms[i].tz)); + CHECK(decoded[i]->qw == doctest::Approx(transforms[i].qw)); + CHECK(decoded[i]->timestamp == doctest::Approx(transforms[i].timestamp).epsilon(1e-6)); + } + + fs::remove(path); +} + TEST_CASE("McapFileWriter: custom topic names are honored") { const auto path = tempMcapPath("hdmapping_test_topics.mcap");