option to export poses in kalibr_camera_calibration, move report gen …

…functions to CameraUtils file

.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "random": "cpp"
+    }
+}

aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras

@@ -110,7 +110,8 @@ def parseArgs():
     outputSettings.add_argument('--show-extraction', action='store_true', dest='showextraction', help='Show the calibration target extraction. (disables plots)')
     outputSettings.add_argument('--plot', action='store_true', dest='plot', help='Plot during calibration (this could be slow).')
     outputSettings.add_argument('--dont-show-report', action='store_true', dest='dontShowReport', help='Do not show the report on screen after calibration.')
-
+    outputSettings.add_argument('--export-poses', action='store_true', dest='exportPoses', help='Export the optimized poses into a CSV (time_ns, position, quaterion)')
+
     #print help if no argument is specified
     if len(sys.argv)==1:
         parser.print_help()
@@ -273,7 +274,7 @@ def main():
                     est_baselines.append( sm.Transformation(baseline.T()) )
                 T_tc_guess = graph.getTargetPoseGuess(timestamp, cameraList, est_baselines)
 
-                success = calibrator.addTargetView(obs_tuple, T_tc_guess)
+                success = calibrator.addTargetView(timestamp, obs_tuple, T_tc_guess)
 
                 #display process
                 if (verbose or (view_id % 25) == 0) and calibrator.estimator.getNumBatches()>0 and view_id>1:
@@ -399,6 +400,7 @@ def main():
             print("")
             print("Processed {0} images with {1} images used".format(numViews, calibrator.estimator.getNumBatches()))
             kcc.printParameters(calibrator)
+            print("")
 
             if parsed.verbose and len(calibrator.baselines)>1:
                 f=pl.figure(100006)
@@ -409,20 +411,33 @@ def main():
             bagtag = os.path.splitext(parsed.bagfile)[0]
             resultFile = bagtag + "-camchain.yaml"
             kcc.saveChainParametersYaml(calibrator, resultFile, graph)
-            print("Results written to file: {0}".format(resultFile))
+            print("Results written to:")
+            print("  Saving camera chain calibration to file: {0}".format(resultFile))
 
             #save results to file
             resultFileTxt = bagtag + "-results-cam.txt"
             kcc.saveResultTxt(calibrator, filename=resultFileTxt)
             print("  Detailed results written to file: {0}".format(resultFileTxt))
 
             #generate report
+            print("")
+            print("Generating result report...")
             reportFile = bagtag + "-report-cam.pdf"
             G=None; 
             if numCams>1: 
                 G=graph
             kcc.generateReport(calibrator, reportFile, showOnScreen=not parsed.dontShowReport, graph=G, removedOutlierCorners=removedOutlierCorners);
+            print("  Report written to {0}".format(reportFile))
+            print("")
 
+            #generate trajectory file
+            if parsed.exportPoses:
+                print("Exporting poses...")
+                posesFile = bagtag + "-poses-cam0.csv"
+                kcc.exportPoses(calibrator, filename=posesFile)
+                print("  Poses written to {0}".format(posesFile))
+                print("")
+
         except kcc.OptimizationDiverged:
             restartAttempts-=1
             sm.logWarn("Optimization diverged possibly due to a bad initialization. (Do the models fit the lenses well?)")

aslam_offline_calibration/kalibr/python/kalibr_calibrate_imu_camera

@@ -102,7 +102,7 @@ def parseArgs():
     outputSettings.add_argument('--extraction-stepping', action='store_true', dest='extractionstepping', help='Show each image during calibration target extraction  (disables plots)', required=False)
     outputSettings.add_argument('--verbose', action='store_true', dest='verbose', help='Verbose output (disables plots)')
     outputSettings.add_argument('--dont-show-report', action='store_true', dest='dontShowReport', help='Do not show the report on screen after calibration.')
-    outputSettings.add_argument('--export-poses', action='store_true', dest='exportPoses', help='Export the optimized poses (time_ns, position, quaterion)')
+    outputSettings.add_argument('--export-poses', action='store_true', dest='exportPoses', help='Export the optimized poses into a CSV (time_ns, position, quaterion)')
 
     #print help if no argument is specified
     if len(sys.argv)==1:
@@ -213,33 +213,34 @@ def main():
     print("==================")
     util.printErrorStatistics(iCal)
     util.printResults(iCal, withCov=parsed.recover_cov)
-
     print("")
+
     bagtag = os.path.splitext(parsed.bagfile[0])[0]
     yamlFilename = bagtag + "-camchain-imucam.yaml"
     iCal.saveCamChainParametersYaml(yamlFilename)
+    print("Results written to:")
     print("  Saving camera chain calibration to file: {0}".format(yamlFilename))
 
-    print("")
     yamlFilename = bagtag + "-imu.yaml"
     iCal.saveImuSetParametersYaml(yamlFilename)
     print("  Saving imu calibration to file: {0}".format(yamlFilename))
 
     resultFileTxt = bagtag + "-results-imucam.txt"
     util.saveResultTxt(iCal, filename=resultFileTxt)
     print("  Detailed results written to file: {0}".format(resultFileTxt))
-
-    print("Generating result report...")    
+
+    print("")
+    print("Generating result report...")
     reportFile = bagtag + "-report-imucam.pdf"
     util.generateReport(iCal, filename=reportFile, showOnScreen=not parsed.dontShowReport)
     print("  Report written to {0}".format(reportFile))
     print("")
-    
+
     if parsed.exportPoses:
         print("Exporting poses...")
         posesFile = bagtag + "-poses-imucam-imu0.csv"
         util.exportPoses(iCal, filename=posesFile)
-        print("Poses written to {0}".format(posesFile))
+        print("  Poses written to {0}".format(posesFile))
         print("")
 
 if __name__ == "__main__":