diff --git a/apps/camera_lidar_calibration/App.cpp b/apps/camera_lidar_calibration/App.cpp index ba4a1c40..35c65f38 100644 --- a/apps/camera_lidar_calibration/App.cpp +++ b/apps/camera_lidar_calibration/App.cpp @@ -618,6 +618,10 @@ void App::update() // counterpart to the image view's zoom loupe: a precision aid the user sees // before committing a click. Must be called inside BeginMode3D/EndMode3D. // +// The markers are wireframe spheres (not solid ones): a solid marker hides +// the very point it marks, which defeats the purpose when the user is +// aiming at a specific point in the cloud. +// // Index labels for the markers are 2D text (DrawText), which must NOT be // called while inside BeginMode3D/EndMode3D -- while a 3D projection matrix // is active, DrawText's screen-pixel quads would get warped through it @@ -633,7 +637,7 @@ static void updateAndDrawPicking3D( { const auto& p = state.pairs[i]; Vector3 wp = { p.px, p.pz, -p.py }; - DrawSphere(wp, 0.05f, YELLOW); + DrawSphereWires(wp, 0.05f, 6, 8, YELLOW); DrawLine3D(Vector3{ wp.x - 0.1f, wp.y, wp.z }, Vector3{ wp.x + 0.1f, wp.y, wp.z }, YELLOW); DrawLine3D(Vector3{ wp.x, wp.y - 0.1f, wp.z }, Vector3{ wp.x, wp.y + 0.1f, wp.z }, YELLOW); @@ -661,7 +665,7 @@ static void updateAndDrawPicking3D( if (state.pendingHasCloud) { Vector3 wp = { state.pendingPair.px, state.pendingPair.pz, -state.pendingPair.py }; - DrawSphere(wp, 0.07f, ORANGE); + DrawSphereWires(wp, 0.07f, 6, 8, ORANGE); } if (!shiftHeld() || ImGui::GetIO().WantCaptureMouse) @@ -686,7 +690,7 @@ static void updateAndDrawPicking3D( { const auto& hp = state.cloud.points[hitIndex]; Vector3 wp = state.cloudPointsRaylib[hitIndex]; - DrawSphere(wp, 0.08f, LIME); + DrawSphereWires(wp, 0.08f, 6, 8, LIME); if (IsMouseButtonPressed(MOUSE_BUTTON_LEFT)) state.setPendingCloudPoint(hp.x, hp.y, hp.z); diff --git a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp index 299701e2..d3f83b92 100644 --- a/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp +++ b/apps/camera_lidar_intrinsics_calib/IntrinsicsCalib.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include @@ -72,7 +71,7 @@ struct State cv::Size imageSize; // output - char outPath[512] = "intrinsics.json"; + char outPath[512] = "intrinsics.yaml"; std::string statusMsg; // background detection @@ -238,29 +237,35 @@ static void runCalibration(State& s) s.statusMsg = "RMS: " + std::to_string(s.rmsError).substr(0, 5) + " px (" + std::to_string(allObj.size()) + " images)"; } -static void saveJson(const State& s) +// Saved with cv::FileStorage, so K/D go out in OpenCV's own camera_info +// YAML (!!opencv-matrix) -- readable by cv::FileStorage, ROS and +// camera_lidar_calibration's intrinsics loader. Format follows the +// extension, so outPath must end in .yaml/.yml. +static void saveYaml(State& s) { if (!s.calibrated) return; - double fx = s.K.at(0, 0); - double fy = s.K.at(1, 1); - double cx = s.K.at(0, 2); - double cy = s.K.at(1, 2); - // CALIB_RATIONAL_MODEL dist order: k1 k2 p1 p2 k3 k4 k5 k6 - auto d = [&](int i) + + cv::FileStorage fs(s.outPath, cv::FileStorage::WRITE); + if (!fs.isOpened()) { - return i < s.D.rows ? s.D.at(i) : 0.0; - }; - - nlohmann::json j; - j["intrinsics"] = { { "fx", fx }, { "fy", fy }, { "cx", cx }, { "cy", cy }, { "k1", d(0) }, { "k2", d(1) }, - { "p1", d(2) }, { "p2", d(3) }, { "k3", d(4) }, { "k4", d(5) }, { "k5", d(6) }, { "k6", d(7) } }; - j["image_size"] = { s.imageSize.width, s.imageSize.height }; - j["rms_error"] = s.rmsError; - - std::ofstream f(s.outPath); - if (f) - f << j.dump(4); + s.statusMsg = std::string("Cannot write: ") + s.outPath; + return; + } + fs << "image_width" << s.imageSize.width; + fs << "image_height" << s.imageSize.height; + fs << "camera_name" + << "camera"; + fs << "camera_matrix" << s.K; + // ROS's name for the 8-coefficient model CALIB_RATIONAL_MODEL solves + // for; D's order is OpenCV's distCoeffs order k1 k2 p1 p2 k3 k4 k5 k6. + fs << "distortion_model" + << "rational_polynomial"; + fs << "distortion_coefficients" << s.D; + fs << "avg_reprojection_error" << s.rmsError; + fs.release(); + + s.statusMsg = std::string("Saved: ") + s.outPath; } // Upload current image (with corners drawn) to a raylib texture. @@ -511,13 +516,11 @@ int main(int argc, char* argv[]) setBuf( state.outPath, sizeof(state.outPath), - mandeye::fd::SaveFileDialog("Save intrinsics JSON", mandeye::fd::json_filter, ".json", defaultName)); - } - if (ImGui::Button("Save JSON", ImVec2(-1, 0))) - { - saveJson(state); - state.statusMsg = std::string("Saved: ") + state.outPath; + mandeye::fd::SaveFileDialog("Save intrinsics YAML", mandeye::fd::IntrinsicsFilter, ".yaml", defaultName)); } + if (ImGui::Button("Save YAML", ImVec2(-1, 0))) + saveYaml(state); + ImGui::PopItemWidth(); } }