{"id":29867589,"url":"https://github.com/dkogan/camera-lidar-calibration","last_synced_at":"2025-07-30T13:38:37.759Z","repository":{"id":300889836,"uuid":"1007482803","full_name":"dkogan/camera-lidar-calibration","owner":"dkogan","description":null,"archived":false,"fork":false,"pushed_at":"2025-07-08T23:16:51.000Z","size":1813,"stargazers_count":1,"open_issues_count":0,"forks_count":0,"subscribers_count":0,"default_branch":"master","last_synced_at":"2025-07-09T00:28:35.180Z","etag":null,"topics":[],"latest_commit_sha":null,"homepage":null,"language":"C","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":"apache-2.0","status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/dkogan.png","metadata":{"files":{"readme":"README.org","changelog":null,"contributing":null,"funding":null,"license":"LICENSE","code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null,"zenodo":null}},"created_at":"2025-06-24T04:30:36.000Z","updated_at":"2025-07-08T23:16:54.000Z","dependencies_parsed_at":"2025-06-24T05:33:16.932Z","dependency_job_id":"2c508f96-1359-45f4-b40b-aa08eaf851df","html_url":"https://github.com/dkogan/camera-lidar-calibration","commit_stats":null,"previous_names":["dkogan/camera-lidar-calibration"],"tags_count":0,"template":false,"template_full_name":null,"purl":"pkg:github/dkogan/camera-lidar-calibration","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/dkogan%2Fcamera-lidar-calibration","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/dkogan%2Fcamera-lidar-calibration/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/dkogan%2Fcamera-lidar-calibration/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/dkogan%2Fcamera-lidar-calibration/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/dkogan","download_url":"https://codeload.github.com/dkogan/camera-lidar-calibration/tar.gz/refs/heads/master","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/dkogan%2Fcamera-lidar-calibration/sbom","host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":267875960,"owners_count":24158784,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2022-07-04T15:15:14.044Z","status":"online","status_checked_at":"2025-07-30T02:00:09.044Z","response_time":70,"last_error":null,"robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":true,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":[],"created_at":"2025-07-30T13:38:37.044Z","updated_at":"2025-07-30T13:38:37.743Z","avatar_url":"https://github.com/dkogan.png","language":"C","funding_links":[],"categories":[],"sub_categories":[],"readme":"This is a LIDAR-camera calibration toolkit using sets of stationary chessboard\nobservations to calibrate the two sets of sensors together.\n\n* Build\nThe bulk of this is C code. To build you need to\n\n#+begin_src sh\nsudo apt install \\\n  libmrgingham-dev \\\n  libdogleg-dev \\\n  libopencv-dev \\\n  libpython3-dev \\\n  libmrcal-dev \\\n  python3-mrcal\n#+end_src\n\nNote that you need bleeding-edge mrcal packages. The v2.5 series is\nrecent-enough.\n\nOnce the deependencies are installed, a plain =make= will build clc. To run the\ntool from logged data, run =./fit.py ...=\n\n* Overview\nCLC is a geometric calibration algorithm to align some number of rigidly-mounted\ncameras (=Ncameras= may be 0) and some number of rigidly mounted LIDAR units\n(=Nlidars= must be at least 1). A common use case is a ground vehicle equipped\nwith some number of these sensors.\n\nIt is assumed that the camera intrinsics have already been calibrated (with a\ntool such as [[https://mrcal.secretsauce.net][mrcal]]), and that the LIDAR angular spacings are known. With those\nassumptions, the only parameters this calibration problem needs to estimate are\nthe poses of all of the sensors.\n\nArbitrarily, CLC computes all the geometry in the coordinate system of the first\nLIDAR unit (=lidar0=). Thus it needs to solve for =Nlidars-1 + Ncameras=\ntransformations, which is an optimization problem of =6*(Nlidars-1 + Ncameras)=\nvariables. As with all optimization problems, detecting issues and quantifying\nthe quality of the result is a crucial part of the computation, and CLC provides\nthis feedback by quantifying the solve uncertainty: the sensitivity of\ntransforming a 3D point from one sensor's frame to another in respect to\nexisting noise in the calibration inputs. This is inspired by, and is very\nsimilar to the logic in [[https://mrcal.secretsauce.net][mrcal]], for quantifying the uncertainty of camera\ncalibrations. The resulting algorithm is good at identifying issues in the\ncalibration data, and will tell you where the result is or is not reliable.\n\n* Calibration process\nIn order to compute a calibration, it is assumed that the set of sensors is\nmounted rigidly, and does not move. A user moves a /calibration object/ over the\nfield of view of the sensors to capture the calibration input data. The sensors\ncapture a set of /snapshots/: synchronized observations of the object by the\nsensors. In order to be useful, each snapshot must be observed by all the\nsensors. It is /not/ required for /every/ snapshot to contain observations from\n/all/ the sensors, but the full set of sensors /must/ be transitively connected.\nFor instance, if snapshot 0 has observations from LIDAR 0 and LIDAR 1, and\nsnapshot 1 has observations from LIDAR 1 and LIDAR 2, this is sufficient to\ncompute a solution for LIDARs 0, 1 and 2.\n\nThe calibration object must be observable by all the sensors, so we use a big\nchessboard target, the same one used to calibrate cameras. The cameras see the\nchessboard grid. The LIDAR units cannot see the chessboard grid, but they do see\na flat object. Thus all the sensors /can/ observe this object, and we can feed\nthose observations to the CLC solver.\n\n** Camera observations\nThe cameras observe the chessboard grid, and we employ the same chessboard\ndetector used by the camera calibration routine ([[https://github.com/dkogan/mrgingham][=mrgingham=]]). The chessboard\ncorner pixel coordinates are input to the solver.\n\nThis routine is available standalone in =clc_camera_chessboard_detection()=\n\n** LIDAR segmentation\nEach LIDAR contains a set of lasers that are rigidly mounted on a spindle\nrotating around the LIDAR z axis. Each laser has a full 360deg view all around\nit. The chessboard is located somewhere in space, in an location unknown prior\nto running the solve. So in order to input the LIDAR observations to the solver,\nwe must find the chessboard in the point cloud. We look for a flat object of\nroughly the known size at a reasonable distance away from the sensor. These\nconditions aren't very discrimintating, so this /LIDAR segmentation/ routine is\nchallenging to get right. There /are/ extra conditions we're not yet employing,\nlike throwing out stationary objects over time, and this will likely be done at\nsome point.\n\nThese routines are available standalone in\n- =clc_lidar_segmentation_unsorted()=\n- =clc_lidar_segmentation_sorted()=\n\n** Solve\nOnce we have sets of observation snapshots, we can attempt the solve. We are\ntrying to find a set of geomeric transformations $\\left\\{ T_\\mathrm{ref,lidar_i}\n\\right\\}$ and $\\left\\{ T_\\mathrm{ref,camera_j} \\right\\}$ and $\\left\\{\nT_\\mathrm{ref,board_k} \\right\\}$ that best explain our observations. The full\nset of transformations is parametrized as [[mrcal =rt= transforms][mrcal =rt= transforms]] in a /state\nvector/ $\\vec b$. For any hypothesis $\\vec b$ I can compute where those\nhypothetical LIDARs and cameras would have observed the hypothetical board. The\ndifference between these hypothetical observations and the actual observations\nis encoded in the /measurement vector/ $\\vec x$. I then compute an error\nfunction $E \\equiv \\left \\Vert \\vec x \\left(\\vec b\\right)\\right \\Vert ^2$, and I\nmove the LIDARs and cameras and boards in order to minimize $E$.\n\nArbitrarily, CLC uses the frame of the first LIDAR (called the =lidar0= frame\nfrom now on) as the reference frame: it is the $\\mathrm{ref}$ in the\ntransformations $T$ above, and its pose does /not/ appear in $\\vec b$.\n\n*** Seed\nWe're solving a nonlinear least-squares optimization problem. We employ a\ntraditional iterative method, so starting the search from a good initial\nestimate of the solution (a /seed/) is essential to ensure convergence of the\nsolve. We compute the seed using a non-iterative global method, solving a\nsimplified version of our full optimization problem. The simplifications are\nrequired for the global method to work. This is implemented in =fit_seed()=. The\napproach is very similar to what [[https://mrcal.secretsauce.net][mrcal]] does:\n\n1. We traverse a graph of sensors with overlapping observations. We start with\n   those with the most overlapping observations, and eventually we cover /all/\n   the sensors. For each pair of sensors visited, we can use a simple [[https://mrcal.secretsauce.net/mrcal-python-api-reference.html#-align_procrustes_points_Rt01][Procrustes\n   fit]] to compute a transformation relating that pair of sensors. And we can\n   then use one path through the graph to estimate the transform between each\n   sensor and =lidar0=.\n\n2. We now have an estimate of the pose of each sensor, and we can use this to\n   estimate the pose of the chessboard for each snapshot. If any cameras observe\n   the chessboard, I use a PnP solve from one of those cameras to estimate the\n   board pose. Otherwise I use the LIDAR points to reconstruct the board pose\n   with arbitrary origin point and yaw (because the LIDARs only see a plane, and\n   don't have a sense of the rotation of the board, or where its edges are).\n\n*** Cost function\nWe now have the input data and an initial estimate of the solution. We can feed\nthe solver. As with [[https://mrcal.secretsauce.net][mrcal]], [[https://github.com/dkogan/libdogleg][libdogleg]] is used to solve the least-squares problem.\n\nWe are minimizing $E \\equiv \\left \\Vert \\vec x \\left(\\vec b\\right)\\right \\Vert\n^2$. The measurement vector $\\vec x$ contains\n\n- $\\vec x_\\mathrm{lidar}$: discrepancies between the hypothetical LIDAR\n  observations from our hypothetical poses and the actual observations\n- $\\vec x_\\mathrm{camera}$: discrepancies between the hypothetical chessboard\n  corner observation and the actual ones observed by our cameras\n- $\\vec x_\\mathrm{regularization}$: small regularization terms\n\nTwo different ways to define the LIDAR errors are implemented:\n\n**** LIDAR errors: perpendicular distance off the plane\nThis is a simplified cost function. I observed that it converges better than the\nfull cost function below, so I use this as another pre-solve.\n\nThe pose of the board is Rt_lidar0_board. The board is at z=0 in board coords so\nthe normal to the plane is nlidar0 = R_lidar0_board[:,2] = R_lidar0_board [0 0\n1]t. I define the board as an infinite plane:\n\n#+begin_example\n  all x where inner(nlidar0,xlidar0) = d\n#+end_example\n\nSo the normal distance from the sensor to the board plane at\nRt_lidar0_board is\n\n#+begin_example\n  d1 = inner(nlidar0, R_lidar0_board xboard0 + t_lidar0_board) =\n     = [0 0 1] R_lidar0_board_t R_lidar0_board xboard0 + [0 0 1] R_lidar0_board_t t_lidar0_board)\n     = inner(nlidar0, t_lidar0_board)\n#+end_example\n\nFor any lidar-observed point p I can compute its perpendicular\ndistance to the board plane:\n\n#+begin_example\n  d2 = inner(nlidar0, Rt_lidar0_lidar p)\n     = inner(nlidar0, R_lidar0_lidar p + t_lidar0_lidar)\n     = inner(nlidar0, v)\n#+end_example\n\nwhere v = R_lidar0_lidar p + t_lidar0_lidar. So\n\n#+begin_example\n  err = d1 - d2 =\n      = inner(nlidar0, t_lidar0_board - v)\n#+end_example\n\nElements of the measurement vector $\\vec x$ in the least-squares problem are the\nindividual =err= quantities above.\n\n**** LIDAR errors: range difference\nThe previous derivation is aphysical. I want my optimization to produce a\nmaximum-likelihood estimate of the solution. This requires the errors in the\nmeasurement vector $\\vec x$ to be independent and homoscedactic. With enough\ndata, the measurement vector will track the noise in the input observations,\nwhich /are/ independent, and can be scaled to be homoscedactic. Thus I want the\nmeasurement vector $\\vec x$ to contain discrepancies in the input observations:\n\n- LIDAR ranges\n- Pixel coordinates from the chessboard corners\n\n#+begin_example\nA plane is zboard = 0\nA lidar point plidar = vlidar dlidar\n\npboard = Rbl plidar + tbl\n       = T_b_l0 T_l0_l plidar\n0 = zboard = pboard[2] = inner(Rbl[2,:],plidar) + tbl[2]\n-\u003e inner(Rbl[2,:],vlidar)*dlidar = -tbl[2]\n-\u003e dlidar = -tbl[2] / inner(Rbl[2,:],vlidar)\n          = -tbl[2] / (inner(Rbl[2,:],plidar) / mag(plidar))\n          = -tbl[2] mag(plidar) / inner(Rbl[2,:],plidar)\n\nAnd the error is\n\n  err = dlidar_observed - dlidar\n      = mag(plidar) - dlidar\n      = mag(plidar) + tbl[2] mag(plidar) / inner(Rbl[2,:],plidar)\n      = mag(plidar) * (1 + tbl[2] / inner(Rbl[2,:],plidar) )\n\nRbl[2,:] = Rlb[:,2] = R_lidar_board z = R_lidar_lidar0 nlidar0\n\ntbl[2]   = (R_board_lidar0 t_lidar0_lidar + t_board_lidar0)[2]\n         = R_board_lidar0[2,:] t_lidar0_lidar + t_board_lidar0[2]\n         = R_lidar0_board[:,2] t_lidar0_lidar + t_board_lidar0[2]\n         = inner(nlidar0,t_lidar0_lidar) + t_board_lidar0[2]\n\nR_lidar0_board pb + t_lidar0_board = pl0\n-\u003e pb = R_board_lidar0 pl0 - R_board_lidar0 t_lidar0_board\n-\u003e t_board_lidar0 = - R_board_lidar0 t_lidar0_board\n-\u003e t_board_lidar0[2] = - R_board_lidar0[2,:] t_lidar0_board\n                     = - R_lidar0_board[:,2] t_lidar0_board\n                     = - inner(nlidar0, t_lidar0_board)\n                     = -d1 (the same d1 as in the crude solve above)\n#+end_example\n\n**** Camera errors\nThe camera discrepancies are done exacly in the same way as with [[https://mrcal.secretsauce.net][mrcal]]: each\nobserved chessboard corner produces two values in $\\vec x$: an error in the $x$\nand $y$ pixel coordinates.\n\n**** Regularization terms\nFor snapshots observed only by LIDARs, the above error expression is ambiguous.\nSince we're considering an infinite plane, the board pose representation\n=rt_lidar0_board= is free to translate and yaw within the plane. We resolve this\nambiguity with regularization terms, extra terms in the measurement vector $\\vec\nx$ that *lightly* pull every element of =rt_lidar0_board= towards zero.\n\n**** Scaling\nAs with [[https://mrcal.secretsauce.net][mrcal]], we're solving a least squares problem, and we want to produce a\nmaximum-likelihood estimate of the optimal solution $\\vec b$. For that to\nhappen, the noise on the measurements should be\n\n- normally distributed\n- independent\n- mean-0\n- homoscedactic (the noise on /every/ measurement should have the same variance)\n\nAll of those requirements are reasonable, but we have to do a bit of work to get\nhomoscedasticity. If we had just one type of measurement in $\\vec x$ (only LIDAR\ndata, say) then we'd have consistent noise in all of those measurements, and the\nhomoscedasticity condition would be met. However, we have LIDAR /and/ camera\ndata here, and we must balance them against each other. At this time, clc is\ngiven the expected noise levels for LIDAR and camera data, and it scales the\nmeasurement errors to produce unitless quantities in $\\vec x$ with consistent\nnoise in each element: $\\sigma = 1$. Oh a high level, =clc.c= has this:\n\n#+begin_src c\n#define SCALE_MEASUREMENT_PX 0.15   /* expected noise levels */\n#define SCALE_MEASUREMENT_M  0.03   /* expected noise levels */\n\nstatic void cost(...)\n{\n    for(...)\n    {\n        ...\n        // LIDAR error\n        x[iMeasurement] =\n          (dlidar_observed - dlidar) / SCALE_MEASUREMENT_M;\n        ...\n        // camera error\n        x[iMeasurement] =\n          (q_observed.xy[k] - q.xy[k]) / SCALE_MEASUREMENT_PX;\n        ...\n    }\n}\n#+end_src\n\nThis works /if/ we have a good estimate of =SCALE_MEASUREMENT_PX= /and/\n=SCALE_MEASUREMENT_M= a priori. In advance we can only estimate them. However,\nsince the optimization residuals approach the input noise levels with enough\ndata, we can\n\n1. Roughly estimate the scalings\n2. Solve\n3. Look at the residuals the get the true scaling\n4. Re-solve with the corrected scalings\n\nToday clc does not do this, and just uses the hard-coded-at-compile-time\nscalings in =clc.c=. This creates a bias in the solution, but likely not\nbig-enough to care about. We can improve this later.\n\n*** Outliers\nCurrently no outlier rejection is implemented. This should be done, it just\nisn't implemented /yet/. Today the residuals can be visualized, and a human can\nvisually evaluate whether outliers are a problem or not. This should be\nautomated.\n\n* Usage details\nCurrently several interfaces are provided:\n\n- A C API to access all the core functionality, as a /library/\n\n- A Python API to access the core functions provided by the C API\n\n- Commandline tools to run the sensor calibration routines without writing any\n  code. These are written in Python, and utilize the Python API\n\n** C API\nThe CLC core is implemented in C, using mrcal for the core geometric types and\ncamera models. The API is defined in =clc.h=.\n\nThe input data is provided as a set of synchronized /snapshots/ in one of these\nstructures:\n\n- =clc_sensor_snapshot_unsorted_t=: the LIDAR data is not assumed to be ordered\n  in any way, and may contain invalid points (0,0,0). The images are given as\n  images\n- =clc_sensor_snapshot_segmented_t=: the LIDAR point clouds have been segmented\n  by =clc_lidar_segmentation_unsorted()= or =clc_lidar_segmentation_sorted()=.\n  The segmented points are stored as indices into the original =points= array.\n  The images are still given as images\n- =clc_sensor_snapshot_sorted_t=: the LIDAR data has been process by\n  =clc_lidar_preprocess()=: the points have been sorted by ring and azimuth, and\n  invalid entries have been removed. The images were processed with a chessboard\n  detector, and the chessboard corners are stored instead of the source images\n- =clc_sensor_snapshot_segmented_dense_t=. The LIDAR point clouds have been\n  segmented. The =points= array contains /only/ the segmented points. The images\n  were processed with a chessboard detector, and the chessboard corners are\n  stored instead of the source images\n\nThe available functions are:\n\n*** =clc_lidar_preprocess()=\nSort the input LIDAR points by ring and azimuth, and remove the invalid entries.\nUsually there's no reason to call this explicitly:\n=fit(sensor_snapshots_unsorted)= does this for you. If calling this function\nourselves, we can call =fit(sensor_snapshots_sorted)= instead.\n\n*** =clc_lidar_segmentation_default_context()=\nSeveral functions in the C API invoke the LIDAR segmentation routine. This\nroutine has a number of parameters that affect its operation, given in the\n=const clc_lidar_segmentation_context_t* ctx= argument. Most often, we would set\nthe default parameters, make small adjustments, and then invoke clc:\n\n#+begin_src c\nclc_lidar_segmentation_context_t ctx;\nclc_lidar_segmentation_default_context(\u0026ctx);\nctx.threshold_max_plane_size = ...; // segmentation parameter tweaks here\n...;\nclc(..., \u0026ctx, ...);\n#+end_src\n\nThe available parameters, a description of their operation and their default\nvalues are given in =clc.h= in the =CLC_LIDAR_SEGMENTATION_LIST_CONTEXT= macro.\n\n*** =clc_lidar_segmentation_unsorted()=, =clc_lidar_segmentation_sorted()=\nInvoke the LIDAR segmentation routine in isolation. Usually there's no reason to\ncall this explicitly: =fit(sensor_snapshots_unsorted)= and\n=fit(sensor_snapshots_sorted)= does this for us. If calling this function /and/\n=clc_camera_chessboard_detection()= ourselves, we can call\n=fit(sensor_snapshots_segmented)= or =fit(sensor_snapshots_segmented_dense)=\ninstead.\n\n*** =clc_camera_chessboard_detection()=\nInvoke the chessboard detector in isolation, to find the chessboard in images\nfrom the camera. Usually there's no reason to call this explicitly:\n=fit(sensor_snapshots_unsorted)= and =fit(sensor_snapshots_sorted)= does this\nfor us. If calling this function /and/ =clc_lidar_segmentation_...()= ourselves,\nwe can call =fit(sensor_snapshots_segmented)= or\n=fit(sensor_snapshots_segmented_dense)= instead.\n\n*** =clc()=\nInvoke the full calibration routine. As with the other functions, each argument\nis documented in =clc.h=. The outputs are given first, and most can be =NULL= if\nwe aren't interested in those specific outputs. To run uncertainty computations,\nthe covariance output is needed, so set =Var_rt_lidar0_sensor= to non-=NULL=.\n\nIt's often helpful to be able to to re-run a solve for testing different\nconfigurations. The =buf_inputs_dump= argument can be used to store a solve\ndump, which can then be replayed by calling =clc_fit_from_inputs_dump()=\n\n*** =clc_fit_from_inputs_dump()=\nIt's often helpful to be able to to re-run a solve for testing different\nconfigurations. The =buf_inputs_dump= argument to =fit()= can be used to store a\nsolve dump, which can then be replayed by calling =clc_fit_from_inputs_dump()=\n\n** Python API\nThe Python API provides Python access to all the core functionality provided by\nthe C API. This Python access is then used by all the commandline tools, which\nare also written in Python. The public Python API lives in the =clc= module, and\nall the functions are documented thoroughly in their respective docstrings.\n\nA summary of all the available functions:\n\n- =lidar_segmentation()=\nFind the calibration plane in a LIDAR point cloud\n\n- =calibrate()=\nInvoke the full clc calibration routine\n\n- =clc.fit_from_inputs_dump()=\nRe-run a previously-dumped calibration\n\n- =clc.lidar_segmentation_parameters()=\nReports the metadata for ALL of the lidar segmentation parameters\n\n- =clc.lidar_segmentation_default_context()=\nReports the default values for ALL of the lidar segmentation parameters\n\n- =color_sequence_rgb()=\nReturn the default color sequence for gnuplot objects. Useful for complex\nplotting\n\n- =plot()=\nWrapper for gnuplotlib.plot(), reporting the hardcopy output to the console\n\n- =pointcloud_plot_tuples()=\nHelper function for visualizing LIDAR data in a common frame\n\n- =sensor_forward_vectors_plot_tuples()=\nHelper function for visualizing sensor poses in geometric plots\n\n*** Input data format\nThe Python API (and thus the commandline tools) can read logged data. As of\ntoday, clc uses ROS bags as its input data storage format. clc does NOT actually\nuse ROS, and it is NOT required to be installed; instead it uses the \"rosbags\"\nlibrary to read the data.\n\nA rosbag may contain multiple data streams with a \"topic\" string identifying\neach one. The data stream for any given topic is a series of messages of\nidentical data type. clc reads lidar scans (msgtype\n'sensor_msgs/msg/PointCloud2') and images (msgtype 'sensor_msgs/msg/Image').\n\nWe want to get a set of time-synchronized \"snapshots\" from the data, reporting\nobservations of a moving calibration object by a set of stationary sensors. Each\nsnapshot should report observations from a single instant in time.\n\nThere are two ways to capture such data:\n\n- Move the chessboard between stationary poses; capture a small rosbag from each\n  sensor at each stationary pose. Each bag provides one snapshot. This works\n  well, but takes more work from the people capturing the data. Therefore, most\n  people prefer the next method\n\n- Move the chessboard; slowly. Continuously capture the data into a single bag.\n  Subdivide the bag into time periods of length =decimation_period_s=. Each\n  decimation period produces one snapshot. This method has risks of motion blur\n  and synchronization issues, so the motions need to be slow, and the tooling\n  needs to enforce tight timings, and it is highly desireable to have an outlier\n  rejection method.\n\nThe tooling supports both methods. The functions and tools that accept a\n\"decimation period\" will use the one-snapshot-per-bag scheme if the decimation\nperiod is omitted, and the one-big-bag scheme if the decimation period is given.\n\nThe various utilities for reading the input data are in the =clc.bag_interface=\nmodule, with each function's docstring providing detailed documentation.\n\n** Commandline tools\nclc also provides a number of commandline tools, intended to make the most\ncommon applications of the tool available directly to the users, without\nrequiring any code to be written. These are written in Python, using the Python\nAPI. The manpages for each available tool follow\n\n*** fit-from-inputs-dump.py\n#+begin_example\nNAME\n    fit-from-inputs-dump.py - Calibrate from a binary input dump\n\nSYNOPSIS\n      ./fit-from-inputs-dump.py xxx\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      context            .pickle file from fit.py --dump or buf_inputs_dump from\n                         the clc_...() C functions\n\n  OPTIONAL ARGUMENTS\n      -h, --help         show this help message and exit\n      --inject-noise     If given, we add expected noise to the observations. If\n                         --inject-noise and no --fit-seed then I will fit() from\n                         the previous fit() result, NOT from the previous\n                         fit_seed() result.\n      --fit-seed         If given, we fit_seed() and then fit(). By default (no\n                         --fit-seed), we only call fit(): from the previous\n                         fit_seed() result if no --inject-noise or from the\n                         previous fit() result if --inject-noise\n      --exclude EXCLUDE  Optional comma-separated list of integers \u003e= 0. If given,\n                         exclude these snapshots\n      --verbose          Report details about the solve\n      --dump DUMP        Write solver diagnostics into the given .pickle file.\n                         Primarily to feed show-transformation-uncertainty.py\n\n#+end_example\n\n*** fit.py\n#+begin_example\nNAME\n    fit.py - Calibrate a set of cameras and LIDARs into a common coordinate\n    system\n\nSYNOPSIS\n      $ lidars=(/lidar/vl_points_0)\n      $ cameras=(/front/multisense/{{left,right}/image_mono_throttle,aux/image_color_throttle})\n      $ sensors=($lidars $cameras)\n\n      $ ./fit.py \\\n          --topics ${(j:,:)sensors} \\\n          --bag 'camera-lidar-*.bag'      \\\n          intrinsics/{left,right,aux}_camera/camera-0-OPENCV8.cameramodel\n\n      ....\n      clc.c(3362) fit(): Finished full solve\n      clc.c(3387) fit(): RMS fit error: 0.43 normalized units\n      clc.c(3404) fit(): RMS fit error (camera): 0.71 pixels\n      clc.c(3410) fit(): RMS fit error (lidar): 0.013 m\n      clc.c(3415) fit(): norm2(error_regularization)/norm2(error): 0.00\n      clc.c(2695) plot_residuals(): Wrote '/tmp/residuals.gp'\n      clc.c(2727) plot_residuals(): Wrote '/tmp/residuals-histogram-lidar.gp'\n      clc.c(3020) plot_geometry(): Wrote '/tmp/geometry.gp'\n      clc.c(3020) plot_geometry(): Wrote '/tmp/geometry-onlyaxes.gp'\n\n      [ The tool chugs for a bit, and in the end produces diagnostics and the aligned ]\n      [ models                                                                        ]\n\nDESCRIPTION\n    This tool computes a geometry-only calibration. It is assumed that the\n    camera intrinsics have already been computed. The results are computed\n    in the coordinate system of the first LIDAR. All the sensors must\n    overlap each other transitively: every sensor doesn't need to overlap\n    every other sensor, but there must be an overlapping path between each\n    pair of sensors.\n\n    The data comes from a set of ROS bags. Each bag is assumed to have\n    captured a single frame (one set of images, LIDAR revolutions) of a\n    stationary scene\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      models                Camera models for the optical calibration. Only the\n                            intrinsics are used. The number of models given must\n                            match the number of camera --topics arguments EXACTLY\n\n  OPTIONAL ARGUMENTS\n      -h, --help            show this help message and exit\n      --topics TOPICS       Which lidar(s) and camera(s) we're talking to. This is\n                            a comma-separated list of topics. Any Nlidars \u003e= 1 and\n                            Ncameras \u003e= 0 is supported\n      --bag BAG             Glob for the rosbag that contains the lidar and camera\n                            data. This can match multiple files\n      --exclude-bag EXCLUDE_BAG\n                            Bags to exclude from the processing. These are a regex\n                            match against the bag paths\n      --decimation-period DECIMATION_PERIOD\n                            If given, we expect ONE bag, and rather than taking\n                            the first message from each bag, we take all the\n                            messages from THIS bag, spaced out with a period given\n                            by this argument, in seconds\n      --after AFTER         If given, start reading the bags at this time. Could\n                            be an integer (s since epoch or ns since epoch), a\n                            float (s since the epoch) or a string, to be parsed\n                            with dateutil.parser.parse()\n      --before BEFORE       If given, stop reading the bags at this time. Could be\n                            an integer (s since epoch or ns since epoch), a float\n                            (s since the epoch) or a string, to be parsed with\n                            dateutil.parser.parse()\n      --exclude-time-period EXCLUDE_TIME_PERIOD\n                            If given, a comma-separated pair of time periods to be\n                            excluded from the data. Each could be an integer (s\n                            since epoch or ns since epoch), a float (s since the\n                            epoch) or a string, to be parsed with\n                            dateutil.parser.parse(). May be given multiple times\n      --dump DUMP           Write solver diagnostics into the given .pickle file\n      --Nsectors NSECTORS   Used in the uncertainty quantification. We report the\n                            uncertainty in radial sectors around the vehicle\n                            origin. If omitted, we use 36 sectors\n      --threshold-valid-lidar-range THRESHOLD_VALID_LIDAR_RANGE\n                            Used in the uncertainty quantification. Lidar returns\n                            closer than this are classified as \"occluded\". This is\n                            used to determine the lidar field-of-view in the\n                            uncertainty reporting. If omitted, we set this to 1.0m\n      --threshold-valid-lidar-Npoints THRESHOLD_VALID_LIDAR_NPOINTS\n                            Used in the uncertainty quantification. We require at\n                            least this many unoccluded lidar returns in a sector\n                            to classify it as \"visible\". If omitted, we require\n                            100 returns\n      --uncertainty-quantification-range UNCERTAINTY_QUANTIFICATION_RANGE\n                            Used in the uncertainty quantification. We report the\n                            uncertainty in radial sectors around the vehicle\n                            origin. In each sector we look at a point this\n                            distance away from the origin. If omitted, we look 10m\n                            ahead\n      --max-time-spread-s MAX_TIME_SPREAD_S\n                            The maximum time spread of observations in a snapshot.\n                            Any snapshot that contains sensor observations with a\n                            bigger time differences than this are thrown out; the\n                            WHOLE snapshot\n      --rt-vehicle-lidar0 RT_VEHICLE_LIDAR0\n                            Used in the uncertainty quantification. The vehicle-\n                            lidar0 transform. The solve is always done in lidar0\n                            coordinates, but we the uncertainty quantification\n                            operates in a different \"vehicle\" frame. This argument\n                            specifies the relationship between those frames. If\n                            omitted, we assume an identity transform: the vehicle\n                            frame is the lidar0 frame\n      --verbose             Report details about the solve\n      --debug-iring DEBUG_IRING\n                            stage1: report diagnostic information on stderr, ONLY\n                            for this ring\n      --debug-xmin DEBUG_XMIN\n                            report diagnostic information on stderr, ONLY for the\n                            region within the given xy bounds\n      --debug-xmax DEBUG_XMAX\n                            report diagnostic information on stderr, ONLY for the\n                            region within the given xy bounds\n      --debug-ymin DEBUG_YMIN\n                            report diagnostic information on stderr, ONLY for the\n                            region within the given xy bounds\n      --debug-ymax DEBUG_YMAX\n                            report diagnostic information on stderr, ONLY for the\n                            region within the given xy bounds\n      --threshold-min-Npoints-in-segment THRESHOLD_MIN_NPOINTS_IN_SEGMENT\n                            stage1: segments are accepted only if they contain at\n                            least this many points\n      --threshold-max-Npoints-invalid-segment THRESHOLD_MAX_NPOINTS_INVALID_SEGMENT\n                            stage1: segments are accepted only if they contain at\n                            most this many invalid points\n      --threshold-max-range THRESHOLD_MAX_RANGE\n                            stage2: discard all segment clusters that lie\n                            COMPLETELY past the given range\n      --threshold-distance-adjacent-points-cross-segment THRESHOLD_DISTANCE_ADJACENT_POINTS_CROSS_SEGMENT\n                            stage2: adjacent cross-segment points in the same ring\n                            must be at most this far apart\n      --threshold-min-cos-angle-error-same-direction-intra-ring THRESHOLD_MIN_COS_ANGLE_ERROR_SAME_DIRECTION_INTRA_RING\n                            stage2: cos threshold used to accumulate a segment to\n                            an adjacent one in the same ring\n      --threshold-max-plane-size THRESHOLD_MAX_PLANE_SIZE\n                            Post-processing: high limit on the linear size of the\n                            reported plane. In a square board this is roughly\n                            compared to the side length\n      --threshold-max-rms-fit-error THRESHOLD_MAX_RMS_FIT_ERROR\n                            Post-processing: high limit on the RMS plane fit\n                            residual. Lower values will demand flatter planes\n      --threshold-min-rms-point-cloud-2nd-dimension--multiple-max-plane-size THRESHOLD_MIN_RMS_POINT_CLOUD_2ND_DIMENSION__MULTIPLE_MAX_PLANE_SIZE\n                            Post-processing: low limit on the short length of the\n                            found plane. Too-skinny planes are rejected Given as a\n                            multiple of the max_plane_size\n      --Npoints-per-rotation NPOINTS_PER_ROTATION\n                            How many points are reported by the LIDAR in a\n                            rotation. This is hardware-dependent, and needs to be\n                            set each for LIDAR unit. Defaults to -1, in which case\n                            clc_lidar_preprocess() will try to estimate this\n      --Npoints-per-segment NPOINTS_PER_SEGMENT\n                            stage1: length of segments we're looking for\n      --threshold-max-Ngap THRESHOLD_MAX_NGAP\n                            The maximum number of consecutive missing points in a\n                            ring\n      --threshold-max-deviation-off-segment-line THRESHOLD_MAX_DEVIATION_OFF_SEGMENT_LINE\n                            stage1: maximum allowed deviation off a segment line\n                            fit. If any points violate this, the entire segment is\n                            rejected\n      --threshold-max-distance-across-rings THRESHOLD_MAX_DISTANCE_ACROSS_RINGS\n                            stage2: max ring-ring distance allowed to join two\n                            segments into a cluster\n      --threshold-max-cos-angle-error-normal THRESHOLD_MAX_COS_ANGLE_ERROR_NORMAL\n                            stage2: cos(v,n) threshold to accept a segment (and\n                            its direction v) into an existing cluster (and its\n                            normal n)\n      --threshold-min-cos-angle-error-same-direction-cross-ring THRESHOLD_MIN_COS_ANGLE_ERROR_SAME_DIRECTION_CROSS_RING\n                            stage2: cos threshold used to construct a cluster from\n                            two cross-ring segments. Non fitting pairs are not\n                            used to create a new cluster\n      --threshold-max-plane-point-error-stage2 THRESHOLD_MAX_PLANE_POINT_ERROR_STAGE2\n                            stage2: distance threshold to make sure each segment\n                            center lies in plane Non-fitting segments are not\n                            added to the cluster\n      --threshold-min-cos-plane-tilt-stage2 THRESHOLD_MIN_COS_PLANE_TILT_STAGE2\n                            stage2: the 'tilt' is the off-head-on orientation\n      --threshold-max-plane-point-error-stage3 THRESHOLD_MAX_PLANE_POINT_ERROR_STAGE3\n                            stage3: distance threshold to make sure each point\n                            lies in the plane Non-fitting points are culled from\n                            the reported plane\n      --threshold-min-plane-point-error-isolation THRESHOLD_MIN_PLANE_POINT_ERROR_ISOLATION\n                            stage3: points just off the edge of the detected board\n                            must fit AT LEAST this badly\n      --threshold-min-points-per-ring--multiple-Npoints-per-segment THRESHOLD_MIN_POINTS_PER_RING__MULTIPLE_NPOINTS_PER_SEGMENT\n                            stage3: minimum number of points in EACH ring in the\n                            cluster; a multiple of Npoints_per_segment\n      --threshold-max-Nsegments-in-cluster THRESHOLD_MAX_NSEGMENTS_IN_CLUSTER\n                            stage2: clusters with more than this many segments are\n                            rejected\n      --threshold-min-Nsegments-in-cluster THRESHOLD_MIN_NSEGMENTS_IN_CLUSTER\n                            stage2: clusters with fewer than this many segments\n                            are rejected\n      --threshold-min-Nrings-in-cluster THRESHOLD_MIN_NRINGS_IN_CLUSTER\n                            stage2: clusters with date from fewer than this many\n                            rings are rejected\n      --threshold-max-gap-Npoints THRESHOLD_MAX_GAP_NPOINTS\n                            stage3: moving from the center, we stop accumulating\n                            points when we encounter an angular gap at least this\n                            large\n\n#+end_example\n\n*** format-geometry-for-ros.py\n#+begin_example\nNAME\n    format-geometry-for-ros.py - Format the output calibration geometry for\n    ROS\n\nSYNOPSIS\n      $ lidars=(/lidar/velodyne_0/points \\\n                /lidar/velodyne_1/points \\\n                /lidar/velodyne_2/points);\n\n      $ ./fit.py                   \\\n        --topics ${(j:,:)lidars}   \\\n        --bag $BAG\n\n      ....\n      Wrote '/tmp/lidar0-mounted.cameramodel' and a symlink '/tmp/sensor0-mounted.cameramodel'\n      Wrote '/tmp/lidar1-mounted.cameramodel' and a symlink '/tmp/sensor1-mounted.cameramodel'\n      Wrote '/tmp/lidar2-mounted.cameramodel' and a symlink '/tmp/sensor2-mounted.cameramodel'\n\n      $ ./format-geometry-for-ros.py \\\n        --topics ${(j:,:)lidars}     \\\n        $BAG\n\n      stamp:\n          nsec: 1749145667549590272\n      transforms:\n          velodyne_0:\n              parent_frame: base_link\n              translation:\n                  x:  ...\n                  y:  ...\n                  z:  ...\n              rotation:\n                  x: ...\n                  y: ...\n                  z: ...\n                  w: ...\n          velodyne_1:\n      .....\n\nDESCRIPTION\n    The solve computes everything in the frame of lidar0. To communicate the\n    solution to ROS, I want to base everything off the \"base_link\" frame. I\n    read the \"/tf_static\" topic to get transform between the lidar0 and the\n    base_link. I then use this transform to shift the solution, and output\n    the solution in some yaml thing that's palatable to ROS.\n\n    There's some funkyness in that each sensor is identified both by its\n    \"topic\" and by its \"name\". The name is what appears in /tf_static and\n    that's what I use in the output. I can't find any consistent way to map\n    topics to/from names. So I assume that each topic is\n    \"/xxx/xxx/xxx/NAME/xxx/xxx/xxx\". I.e. there's lots of unknowable cruft,\n    with the sensor name in the middle somewhere. I use the names in\n    /tf_static to infer which topic component has the name, and I then use\n    that.\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      bag              The bag we read the /tf_static transform from\n\n  OPTIONAL ARGUMENTS\n      -h, --help       show this help message and exit\n      --topics TOPICS  Which lidar(s) and camera(s) we're talking to. This is a comma-separated list of topics. Any Nlidars \u003e= 1 and Ncameras \u003e=\n                       0 is supported\n\n#+end_example\n\n*** infer-lidar-spacing.py\n#+begin_example\nNAME\n    infer-lidar-spacing.py - Report the az and el layout present in a LIDAR\n    dataset\n\nSYNOPSIS\n      $ ./infer-lidar-spacing.py \\\n          /points0 \\\n          camera-lidar0.bag\n\n      Nrings=128\n      Npoints_per_rotation=512\n\n      ....\n\nDESCRIPTION\n    These parameters are used by the lidar segmentation routine\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      lidar-topic       The LIDAR topic we're looking at\n      bag               The rosbag that contain the lidar data.\n\n  OPTIONAL ARGUMENTS\n      -h, --help        show this help message and exit\n      --show-histogram  If given, we display the histogram of potential Npoints_per_rotation values. Hopefully we see a sharp peak at the \"right\"\n                        value and that it is a power of 2\n\n#+end_example\n\n*** lidar-segmentation.py\n#+begin_example\nNAME\n    lidar-segmentation.py - Find the board in a point cloud\n\nSYNOPSIS\n      $ ./lidar-segmentation.py     \\\n          /lidar/vl_points_1     \\\n          'camera-lidar*.bag'\n\nDESCRIPTION\n    This tool is primarily for developing and debugging C code that\n    interacts with the LIDAR data. This tool makes various assumptions. Read\n    the code before blindly using this\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      lidar-topic           The LIDAR topic we're looking at\n      bag                   The rosbag that contain the lidar data.\n\n  OPTIONAL ARGUMENTS\n      -h, --help            show this help message and exit\n      --debug DEBUG DEBUG DEBUG DEBUG DEBUG\n                            If given, overrides --debug-iring, --debug-xmin, --debug-ymin, --debug-xmax, --debug-ymax\n      --after AFTER         If given, start reading the bag at this time. Could be an integer (s since epoch or ns since epoch), a float (s since\n                            the epoch) or a string, to be parsed with dateutil.parser.parse()\n      --dump                if true, diagnostic detector data meant for plotting is output on stdout. The intended use is ./lidar-segmentation-\n                            test.py --dump TOPIC BAG | | feedgnuplot \\ --style label 'with labels' \\ --style ACCEPTED \"with points pt 2 ps 2 lw 2\n                            lc \\\"red\\\"\"\\ --tuplesize label 4 \\ --style all 'with points pt 7 ps 0.5' \\ --style stage1-segment \"with vectors lc\n                            \\\"green\\\"\"\\ --style plane-normal \"with vectors lc \\\"black\\\"\"\\ --tuplesize stage1-segment,plane-normal 6\\ --3d \\\n                            --domain \\ --dataid \\ --square \\ --points \\ --tuplesizeall 3 \\ --autolegend \\ --xlabel x \\ --ylabel y \\ --zlabel z\\\n      --debug-iring DEBUG_IRING\n                            stage1: report diagnostic information on stderr, ONLY for this ring\n      --debug-xmin DEBUG_XMIN\n                            report diagnostic information on stderr, ONLY for the region within the given xy bounds\n      --debug-xmax DEBUG_XMAX\n                            report diagnostic information on stderr, ONLY for the region within the given xy bounds\n      --debug-ymin DEBUG_YMIN\n                            report diagnostic information on stderr, ONLY for the region within the given xy bounds\n      --debug-ymax DEBUG_YMAX\n                            report diagnostic information on stderr, ONLY for the region within the given xy bounds\n      --threshold-min-Npoints-in-segment THRESHOLD_MIN_NPOINTS_IN_SEGMENT\n                            stage1: segments are accepted only if they contain at least this many points\n      --threshold-max-Npoints-invalid-segment THRESHOLD_MAX_NPOINTS_INVALID_SEGMENT\n                            stage1: segments are accepted only if they contain at most this many invalid points\n      --threshold-max-range THRESHOLD_MAX_RANGE\n                            stage2: discard all segment clusters that lie COMPLETELY past the given range\n      --threshold-distance-adjacent-points-cross-segment THRESHOLD_DISTANCE_ADJACENT_POINTS_CROSS_SEGMENT\n                            stage2: adjacent cross-segment points in the same ring must be at most this far apart\n      --threshold-min-cos-angle-error-same-direction-intra-ring THRESHOLD_MIN_COS_ANGLE_ERROR_SAME_DIRECTION_INTRA_RING\n                            stage2: cos threshold used to accumulate a segment to an adjacent one in the same ring\n      --threshold-max-plane-size THRESHOLD_MAX_PLANE_SIZE\n                            Post-processing: high limit on the linear size of the reported plane. In a square board this is roughly compared to\n                            the side length\n      --threshold-max-rms-fit-error THRESHOLD_MAX_RMS_FIT_ERROR\n                            Post-processing: high limit on the RMS plane fit residual. Lower values will demand flatter planes\n      --threshold-min-rms-point-cloud-2nd-dimension--multiple-max-plane-size THRESHOLD_MIN_RMS_POINT_CLOUD_2ND_DIMENSION__MULTIPLE_MAX_PLANE_SIZE\n                            Post-processing: low limit on the short length of the found plane. Too-skinny planes are rejected Given as a multiple\n                            of the max_plane_size\n      --Npoints-per-rotation NPOINTS_PER_ROTATION\n                            How many points are reported by the LIDAR in a rotation. This is hardware-dependent, and needs to be set each for\n                            LIDAR unit. Defaults to -1, in which case clc_lidar_preprocess() will try to estimate this\n      --Npoints-per-segment NPOINTS_PER_SEGMENT\n                            stage1: length of segments we're looking for\n      --threshold-max-Ngap THRESHOLD_MAX_NGAP\n                            The maximum number of consecutive missing points in a ring\n      --threshold-max-deviation-off-segment-line THRESHOLD_MAX_DEVIATION_OFF_SEGMENT_LINE\n                            stage1: maximum allowed deviation off a segment line fit. If any points violate this, the entire segment is rejected\n      --threshold-max-distance-across-rings THRESHOLD_MAX_DISTANCE_ACROSS_RINGS\n                            stage2: max ring-ring distance allowed to join two segments into a cluster\n      --threshold-max-cos-angle-error-normal THRESHOLD_MAX_COS_ANGLE_ERROR_NORMAL\n                            stage2: cos(v,n) threshold to accept a segment (and its direction v) into an existing cluster (and its normal n)\n      --threshold-min-cos-angle-error-same-direction-cross-ring THRESHOLD_MIN_COS_ANGLE_ERROR_SAME_DIRECTION_CROSS_RING\n                            stage2: cos threshold used to construct a cluster from two cross-ring segments. Non fitting pairs are not used to\n                            create a new cluster\n      --threshold-max-plane-point-error-stage2 THRESHOLD_MAX_PLANE_POINT_ERROR_STAGE2\n                            stage2: distance threshold to make sure each segment center lies in plane Non-fitting segments are not added to the\n                            cluster\n      --threshold-min-cos-plane-tilt-stage2 THRESHOLD_MIN_COS_PLANE_TILT_STAGE2\n                            stage2: the 'tilt' is the off-head-on orientation\n      --threshold-max-plane-point-error-stage3 THRESHOLD_MAX_PLANE_POINT_ERROR_STAGE3\n                            stage3: distance threshold to make sure each point lies in the plane Non-fitting points are culled from the reported\n                            plane\n      --threshold-min-plane-point-error-isolation THRESHOLD_MIN_PLANE_POINT_ERROR_ISOLATION\n                            stage3: points just off the edge of the detected board must fit AT LEAST this badly\n      --threshold-min-points-per-ring--multiple-Npoints-per-segment THRESHOLD_MIN_POINTS_PER_RING__MULTIPLE_NPOINTS_PER_SEGMENT\n                            stage3: minimum number of points in EACH ring in the cluster; a multiple of Npoints_per_segment\n      --threshold-max-Nsegments-in-cluster THRESHOLD_MAX_NSEGMENTS_IN_CLUSTER\n                            stage2: clusters with more than this many segments are rejected\n      --threshold-min-Nsegments-in-cluster THRESHOLD_MIN_NSEGMENTS_IN_CLUSTER\n                            stage2: clusters with fewer than this many segments are rejected\n      --threshold-min-Nrings-in-cluster THRESHOLD_MIN_NRINGS_IN_CLUSTER\n                            stage2: clusters with date from fewer than this many rings are rejected\n      --threshold-max-gap-Npoints THRESHOLD_MAX_GAP_NPOINTS\n                            stage3: moving from the center, we stop accumulating points when we encounter an angular gap at least this large\n\n#+end_example\n\n*** show-aligned-lidar-pointclouds.py\n#+begin_example\nNAME\n    show-aligned-lidar-pointclouds.py - Display a set of LIDAR point clouds\n    in the aligned coordinate system\n\nSYNOPSIS\n      $ ./show-aligned-lidar-pointclouds.py                   \\\n          --bag camera-lidar.bag                              \\\n          --topic /lidar/vl_points_0,/lidar/vl_points_1 \\\n          /tmp/lidar[01]-mounted.cameramodel\n        [plot pops up to show the aligned points]\n\nDESCRIPTION\n    Displays the point clouds in the lidar0 coord system\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      lidar-models          The .cameramodel for the lidars in question. Must correspond to the set in --topic.\n\n  OPTIONAL ARGUMENTS\n      -h, --help            show this help message and exit\n      --topics TOPICS       The LIDAR topics to visualize. This is a comma-separated list of topics\n      --bag BAG             The one bag we're visualizing\n      --after AFTER         If given, start reading the bags at this time. Could be an integer (s since epoch or ns since epoch), a float (s\n                            since the epoch) or a string, to be parsed with dateutil.parser.parse()\n      --threshold-range THRESHOLD_RANGE\n                            Max distance where we cut the plot\n      --title TITLE         Title string for the plot. Overrides the default title\n      --hardcopy HARDCOPY   Write the output to disk, instead of an interactive plot\n      --terminal TERMINAL   gnuplotlib terminal. The default is good almost always, so most people don't need this option\n      --set SET             Extra 'set' directives to gnuplotlib. Can be given multiple times\n      --unset UNSET         Extra 'unset' directives to gnuplotlib. Can be given multiple times\n\n#+end_example\n\n*** show-bag.py\n#+begin_example\nNAME\n    show-bag.py - Display the LIDAR scans from a rosbag\n\nSYNOPSIS\n      $ ./show-bag.py             \\\n          'camera-lidar*.bag'\n\n      Bag 'camera-lidar-0.bag':\n        /points0\n        /points1\n      Bag 'camera-lidar-1.bag':\n        /points0\n        /points1\n        /image0\n      ....\n\n      $ ./show-bag.py             \\\n          --period 2              \\\n          --set \"view 60,30,2\"    \\\n          --set \"xrange [-20:20]\" \\\n          --set \"yrange [-20:20]\" \\\n          --set \"zrange [-2:2]\"   \\\n          --topic /points0 \\\n          'camera-lidar*.bag'\n\n        [A plot pops up showing the LIDAR scans from the bags, updating every 2 sec]\n\n      $ ./show-bag.py     \\\n          --topic /image0 \\\n          'camera-lidar*.bag'\n\n        [A plot pops up showing the images in the bags. Updated with the next bag\n        when the user closes the plot]\n\nDESCRIPTION\n    This is a display tool to show the contents of a ros bag. This is\n    roughly similar to the \"rosbag\" and \"ros2 bag\" and \"rviz\" tools, but\n    uses the \"rosbags\" Python package, and does NOT require ROS to be\n    installed\n\nOPTIONS\n  POSITIONAL ARGUMENTS\n      bags                  Glob(s) for the rosbags that contain the lidar data. Each of these will be sorted alphanumerically\n\n  OPTIONAL ARGUMENTS\n      -h, --help            show this help message and exit\n      --maxrange MAXRANGE   Applies to LIDAR data. If given, cut off the points at this range\n      --with WITH           Applies to LIDAR data. If given, uses the requested style to plot the lidar points. The default is \"dots\". If there\n                            aren't many points to show, this can be illegible, and \"points\" works better\n      --no-intensity        Applies to LIDAR data. By default we color-code each point by intensity. With --no-intensity, we plot all the points\n                            with the same color. Improves legibility in some cases\n      --ring RING           Applies to LIDAR data. If given, show ONLY data from this ring. Otherwise, display all of them\n      --xy                  Applies to LIDAR data. If given, I make a 2D plot, ignoring the z axis\n      --extract-images      Applies to camera data. If given, I write the images to the directory given in this argument. Exactly one bag and one\n                            topic is expected. I write to the directory in --outdir ('/tmp' by default) and I write at most --extract-images-\n                            count (all images by default)\n      --extract-images-count EXTRACT_IMAGES_COUNT\n                            If --extract-images is given I write this many images at most. By default, I write ALL the images\n      --outdir OUTDIR       The directory --extract-.... will write to. The default is \"/tmp/\"\n      --period PERIOD       How much time to wait between moving to the next bag; if \u003c= 0 (the default), we wait until each window is manually\n                            closed\n      --hardcopy HARDCOPY   Write the output to disk, instead of an interactive plot\n      --terminal TERMINAL   gnuplotlib terminal. The default is good almost always, so most people don't need this option\n      --set SET             Extra 'set' directives to gnuplotlib. Can be given multiple times\n      --unset UNSET         Extra 'unset' directives to gnuplotlib. Can be given multiple times\n      --topic TOPIC         The topic we're visualizing. Can select LIDAR or camera data. If omitted, we report the topics present in the bag,\n                            and we exit. If --timeline, this is a ,-separated list of topics to visualize\n      --decimation-period DECIMATION_PERIOD\n                            If given, we expect ONE bag, and rather than taking the first message from each bag, we take all the messages from\n                            THIS bag, spaced out with a period given by this argument, in seconds\n      --after AFTER         If given, start reading the bags at this time. Could be an integer (s since epoch or ns since epoch), a float (s\n                            since the epoch) or a string, to be parsed with dateutil.parser.parse()\n      --before BEFORE       If given, stop reading the bags at this time. Could be an integer (s since epoch or ns since epoch), a float (s since\n                            the epoch) or a string, to be parsed with dateutil.parser.parse()\n      --timeline TIMELINE   If given, we plot time message timeline from the ONE give bag. Multiple bags not allowed. If no --topic, we report\n                            ALL the topics. Takes one argument: the duration (in seconds) of the requested plot. If \u003c= 0, we plot the whole bag\n      --time-header-ns      If given, we use the time_header_ns for --timeline. This is the time the data was SENT, not the time it was recorded.\n                            All the log replay code uses time_ns\n\n#+end_example\n\n*** show-transformation-uncertainty.py\n#+begin_example\nNAME\n    show-transformation-uncertainty.py - Visualize transformation\n    uncertainty between a pair of sensors\n\nSYNOPSIS\n      $ ./fit.py ... --dump /tmp/clc-context.pickle\n\n      $ ./show-transformation-uncertainty.py                  \\\n          --bag camera-lidar.bag                              \\\n          --topic /lidar/vl_points_0,/lidar/vl_points_1       \\\n          --context /tmp/clc-context.pickle\n        [plots pop up to show the uncertainty]\n\nDESCRIPTION\n    Displays uncertainties of transformations between pairs of sensors\n\nOPTIONS\n  OPTIONAL ARGUMENTS\n      -h, --help            show this help message and exit\n      --gridn GRIDN         How densely we should sample the space. We use a\n                            square grid with gridn cells on each side. By default\n                            gridn=25\n      --radius RADIUS       How far we should sample the space. We use a square\n                            grid, 2*radius m per side. By default radius=20\n      --ellipsoids          By default we plot the transformation uncertainty,\n                            which is derived from uncertainty ellipsoids. It is\n                            sometimes useful to see the ellipsoids themselves,\n                            usually for debugging. Pass --ellipsoids to do that\n      --topics TOPICS       The topics to visualize. This is a comma-separated\n                            list of topics\n      --bag BAG             The one bag we're visualizing. Required if\n                            --ellipsoids\n      --after AFTER         If given, start reading the bags at this time. Could\n                            be an integer (s since epoch or ns since epoch), a\n                            float (s since the epoch) or a string, to be parsed\n                            with dateutil.parser.parse()\n      --context CONTEXT     .pickle file from fit.py\n      --threshold THRESHOLD\n                            Max distance where we cut the plot\n      --cbmax CBMAX         If given, we use this cbmax in the uncertainty plots\n      --hardcopy HARDCOPY   If given, plot to this file instead of making an\n                            interactive plot. If no --ellipsoids, we make multiple\n                            plots, one per topic. The sanitized topic name will be\n                            appended to the end of this given filename\n\n#+end_example\n\n* Interpretation of the results\nAs [[https://mrcal.secretsauce.net][mrcal]], clc tries hard to provide deep feedback to the user to enable them to\nclearly see if the calibration results are correct and reliable. The techniques\nare similar as with mrcal:\n\n1. Various visualizations are available to check for errors in the data and to\n   check the final fit\n2. The uncertainty in the input observations is propagated to the output\n   transforms to see how much and where in space we have confidence in our\n   solution\n\n** Visualization of the solution\nA nominal run of clc looks like this:\n\n#+begin_example\n$ lidars=(/lidar/vl_points_0)\n$ cameras=(/front/multisense/{{left,right}/image_mono_throttle,aux/image_color_throttle})\n$ sensors=($lidars $cameras)\n\n$ ./fit.py \\\n    --topics ${(j:,:)sensors} \\\n    --bag 'camera-lidar-*.bag'      \\\n    intrinsics/{left,right,aux}_camera/camera-0-OPENCV8.cameramodel\n\n....\nclc.c(3362) fit(): Finished full solve\nclc.c(3387) fit(): RMS fit error: 0.43 normalized units\nclc.c(3404) fit(): RMS fit error (camera): 0.71 pixels\nclc.c(3410) fit(): RMS fit error (lidar): 0.013 m\nclc.c(3415) fit(): norm2(error_regularization)/norm2(error): 0.00\nclc.c(2695) plot_residuals(): Wrote '/tmp/residuals.gp'\nclc.c(2727) plot_residuals(): Wrote '/tmp/residuals-histogram-lidar.gp'\nclc.c(3020) plot_geometry(): Wrote '/tmp/geometry.gp'\nclc.c(3020) plot_geometry(): Wrote '/tmp/geometry-onlyaxes.gp'\n#+end_example\n\nThe sample tells us that:\n\n- The RMS of the full $\\vec x$ vector is 0.43 normalized units (see [[*Scaling][above for\n  scaling notes]])\n- We have 0.71 pixels RMS of error in the camera data and 0.013m RMS error in\n  the LIDAR data\n- The regularization terms have a /very/ small contribution to the total cost\n  (as intended; these are for tie-breakers only)\n\nThe =.gp= files are executable, and produce diagnostic plots.\n\nThe =residuals= plots visualize the optimized measurement vector $\\vec x$. This\nis an estimate of the input noise, so as noted [[*Scaling][above]], we want to see\n\n- Normally distributed noise. The histogram plot displays this\n- Independent noise. There should be no discernible patterns in the residuals.\n  If there are, there's something likely wrong in the data collection process\n- Mean-0 noise. This will bias the solution, but will not show up in any clear\n  way in the plots\n- Homoscedactic noise: the noise on /every/ measurement should have the same\n  variance. Since we [[*Scaling][rescaled]] the measurements, the observed variance of the\n  camera and lidar measurements should be 1.0. Disparate lidar/camera variances\n  will produce a suboptimal solve. Variances significantly off from 1.0 will\n  produce errors in the uncertainty reporting: that code currently assumes\n  variances of 1.0\n\nThe =geometry= plots display a 3D view of the sensor layout in the solution,\nwith or without the solved board geometry. The LIDAR xyz axes are front-left-up.\nThe camera xyz axes are right-down-forward.\n\n** Uncertainty\nclc can propagate the uncertainty in the input noise the point transformations\nthat use the calibrated geometry. This is important because this noise is\n/always/ present: it cannot be eliminated, so we make sure to be robust to it. A\npoor uncertainty generally means that we didn't gather enough of the right kind\nof data: we want a good distribution of positions and orientations of the\nchessboard. For instance, board tilt is important: if the board was only\npresented vertically, then the solver doesn't have enough information to compute\nthe vertical LIDAR position. /Some/ position will still be reported, but it\nwould be selected primarily based on the input noise, and the uncertainty\nreporting will tell us that the solve is unreliable.\n\nThe computation is done in two steps:\n\n=fit()= reports a covariance of the solution in =Var_rt_lidar0_sensor=. This is\na large, symmetric matrix. If we're calibrating $N$ sensors, we're computing\n$N-1$ poses (one sensor is the reference), and we have $6\\left( N-1 \\right)$\noptimization variables, and the covariance matrix thus has dimensions $\\left(\n6\\left( N-1 \\right), 6\\left( N-1 \\right) \\right)$. This covariance of the state\nvector is done [[https://mrcal.secretsauce.net/docs-2.4/uncertainty.html#org1461ff3][/exactly/ as in mrcal]], except we rescaled our measurements]], and\nthus the noise in the inputs $\\sigma$ is assumed to be 1.0\n\nWe propagate this covariance. For any function $\\vec F \\left(\\vec b\\right)$ we\nhave $\\mathrm{Var}\\left( \\vec F \\right) = \\frac{\\partial \\vec F}{\\partial \\vec\nb} \\mathrm{Var}\\left( \\vec b \\right) \\frac{\\partial \\vec F}{\\partial \\vec b}^T$.\nWe can thus take an arbitrary point $\\vec p_\\mathrm{i}$ in the coordinate frame\nof sensor $\\mathrm{i}$, transform it to the frame of sensor $\\mathrm{j}$. This\ncomputation is a function of the transformations in $\\vec b$. We can thus\ncompute $\\frac{\\partial \\vec p_\\mathrm{j}}{\\partial \\vec b}$, and compute\n$\\mathrm{Var}\\left( \\vec p_\\mathrm{j} \\right)$ to see how reliable that transform is. We\nwill discover that this reliability varies for different sensor combinations and\ndifferent locations in space, and we can use that as a gauge of whether our\ncalibration is good-enough.\n\n** Auxillary tools\nA number of commandline tools are available to visualize various things.\n\n*** =show-aligned-lidar-pointclouds.py=\nThis tool displays point clouds from different LIDARs, transformed by the solved\nsensor geometry. It is a good check of how well we did, and should follow the\nuncertainty predictions.\n\n*** =show-bag.py=\nUsed to determine which topics are available in the bag, and to visualize\nand/our export the data in various ways. Much of this can be done just as well\nwith ROS tools (=rostopic=, =rviz=, etc), but clc does not use ROS.\n\n*** =show-transformation-uncertainty.py=\nVisualize the solved uncertainty.\n\n** Sector-based feedback\n=clc()= produces some extra feedback to support the common case of\nground-vehicles and horizontally-oriented sensors. The ground plane is\nsubdivided into =Nsectors= slices, with some diagnostic reporting for each\nslice. The vehicle frame is defined by =rt_vehicle_lidar0= and the sector count\nby =Nsectors=, both arguments to =clc()=.\n\nThe reported feedback is all returned in arguments to =clc()=:\n\n- =observations_per_sector=: reports how well-covered a given sector is, to find\n  cases where the chessboard wasn't placed in all the necessary locations. This\n  isn't needed because the uncertainty reporting will tell you if the data\n  coverage is insufficient\n\n- =isvisible_per_sensor_per_sector=: reports which areas are invisible to the\n  sensors, due to the sensors arrangement or occlusions. This is somewhat\n  poorly-defined (because the observable area is a 3D region, not a 2D pie\n  slice), but could be useful.\n\n- =stdev_worst_per_sector=: the uncertainty report for each sector. Meant to\n  give the user a quick sense of the quality of the solve, and to identify areas\n  with issues. We look through every pair of sensors, and report the uncertainty\n  of the worst pair\n\n- =isensors_pair_stdev_worst=: which pair of sensors produced the uncertainty in\n  =stdev_worst_per_sector=.\n\n- =isector_of_last_snapshot=: which sector contained the most-recent chessboard\n  observation\n\nThese diagnostics are controlled by a few parameters, also arguments to =clc()=:\n\n- =threshold_valid_lidar_range=, =threshold_valid_lidar_Npoints=: used for\n  =isvisible_per_sensor_per_sector=. For a sector to be deemed \"visible\" by a\n  LIDAR, we need to have seen at least this many points beyond a given range.\n  When mounted to a vehicle, the vehicle body will occlude some of the LIDAR\n  view, and we need to ask for a distance beyond those occlusions\n\n- =uncertainty_quantification_range=: used for the visibility and uncertainty\n  reporting. For each sector, we actually evaluate a single point on the ground\n  ($z=0$), this far away\n\n* LICENSE AND COPYRIGHT\n\nCopyright (c) 2023-2025 California Institute of Technology (\"Caltech\"). U.S.\nGovernment sponsorship acknowledged. All rights reserved.\n\nLicensed under the Apache License, Version 2.0 (the \"License\");\nYou may obtain a copy of the License at\n\n    http://www.apache.org/licenses/LICENSE-2.0\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fdkogan%2Fcamera-lidar-calibration","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fdkogan%2Fcamera-lidar-calibration","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fdkogan%2Fcamera-lidar-calibration/lists"}