diff --git a/apps/calibration/intrinsic/calibration-helper.hpp b/apps/calibration/intrinsic/calibration-helper.hpp index 63db49ee19..6617b1ff6e 100644 --- a/apps/calibration/intrinsic/calibration-helper.hpp +++ b/apps/calibration/intrinsic/calibration-helper.hpp @@ -42,16 +42,45 @@ #include #include #include +#include #ifndef DOXYGEN_SHOULD_SKIP_THIS +#if defined(ENABLE_VISP_NAMESPACE) +using namespace VISP_NAMESPACE_NAME; +#endif + namespace calib_helper { + +// Adapted from: +// https://stackoverflow.com/questions/58881746/c-how-to-cout-and-write-at-the-same-time/58881939#58881939 +class Tee +{ +private: + std::ostream &os; + std::ofstream &file; + +public: + Tee(std::ostream &os_, std::ofstream &file_) : os(os_), file(file_) { } + + template + Tee &operator<<(const T &thing) + { + os << thing; + if (file.is_open()) { + file << thing; + } + return *this; + } +}; + class Settings { public: - Settings() - : boardSize(), calibrationPattern(UNDEFINED), squareSize(0.), input(), tempo(0.), goodInput(false), patternToUse() + Settings(Tee &tee_) + : boardSize(), calibrationPattern(UNDEFINED), squareSize(0.), input(), tempo(0.), goodInput(false), patternToUse(), + tee(tee_) { boardSize = cv::Size(0, 0); calibrationPattern = UNDEFINED; @@ -65,21 +94,21 @@ class Settings bool read(const std::string &filename) // Read the parameters { // reading configuration file - if (!VISP_NAMESPACE_ADDRESSING vpIoTools::loadConfigFile(filename)) + if (!vpIoTools::loadConfigFile(filename)) return false; - VISP_NAMESPACE_ADDRESSING vpIoTools::readConfigVar("BoardSize_Width:", boardSize.width); - VISP_NAMESPACE_ADDRESSING vpIoTools::readConfigVar("BoardSize_Height:", boardSize.height); - VISP_NAMESPACE_ADDRESSING vpIoTools::readConfigVar("Square_Size:", squareSize); - VISP_NAMESPACE_ADDRESSING vpIoTools::readConfigVar("Calibrate_Pattern:", patternToUse); - VISP_NAMESPACE_ADDRESSING vpIoTools::readConfigVar("Input:", input); - VISP_NAMESPACE_ADDRESSING vpIoTools::readConfigVar("Tempo:", tempo); - - std::cout << "grid width : " << boardSize.width << std::endl; - std::cout << "grid height: " << boardSize.height << std::endl; - std::cout << "square size: " << squareSize << std::endl; - std::cout << "pattern : " << patternToUse << std::endl; - std::cout << "input seq : " << input << std::endl; - std::cout << "tempo : " << tempo << std::endl; + vpIoTools::readConfigVar("BoardSize_Width:", boardSize.width); + vpIoTools::readConfigVar("BoardSize_Height:", boardSize.height); + vpIoTools::readConfigVar("Square_Size:", squareSize); + vpIoTools::readConfigVar("Calibrate_Pattern:", patternToUse); + vpIoTools::readConfigVar("Input:", input); + vpIoTools::readConfigVar("Tempo:", tempo); + + tee << "grid width : " << boardSize.width << "\n"; + tee << "grid height: " << boardSize.height << "\n"; + tee << "square size: " << squareSize << "\n"; + tee << "pattern : " << patternToUse << "\n"; + tee << "input seq : " << input << "\n"; + tee << "tempo : " << tempo << "\n"; interprate(); return true; } @@ -88,11 +117,11 @@ class Settings { goodInput = true; if (boardSize.width <= 0 || boardSize.height <= 0) { - std::cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << std::endl; + tee << "Invalid Board size: " << boardSize.width << " " << boardSize.height << "\n"; goodInput = false; } if (squareSize <= 10e-6) { - std::cerr << "Invalid square size " << squareSize << std::endl; + tee << "Invalid square size " << squareSize << "\n"; goodInput = false; } @@ -105,7 +134,7 @@ class Settings else if (patternToUse.compare("CIRCLES_GRID") == 0) calibrationPattern = CIRCLES_GRID; if (calibrationPattern == UNDEFINED) { - std::cerr << " Inexistent camera calibration mode: " << patternToUse << std::endl; + tee << " Inexistent camera calibration mode: " << patternToUse << "\n"; goodInput = false; } } @@ -124,39 +153,39 @@ class Settings private: std::string patternToUse; + Tee &tee; }; struct CalibInfo { - CalibInfo(const VISP_NAMESPACE_ADDRESSING vpImage &img, const std::vector &points, - const std::vector &imPts, const std::string &frame_name) + CalibInfo(const vpImage &img, const std::vector &points, + const std::vector &imPts, const std::string &frame_name) : m_img(img), m_points(points), m_imPts(imPts), m_frame_name(frame_name) { } - VISP_NAMESPACE_ADDRESSING vpImage m_img; - std::vector m_points; - std::vector m_imPts; + vpImage m_img; + std::vector m_points; + std::vector m_imPts; std::string m_frame_name; }; -void drawCalibrationOccupancy(VISP_NAMESPACE_ADDRESSING vpImage &I, const std::vector &calib_info, - unsigned int patternW) +void drawCalibrationOccupancy(vpImage &I, const std::vector &calib_info, unsigned int patternW) { I = 0u; unsigned char pixel_value = static_cast(255.0 / calib_info.size()); for (size_t idx = 0; idx < calib_info.size(); idx++) { const CalibInfo &calib = calib_info[idx]; - std::vector corners; + std::vector corners; corners.push_back(calib.m_imPts.front()); corners.push_back(*(calib.m_imPts.begin() + patternW - 1)); corners.push_back(calib.m_imPts.back()); corners.push_back(*(calib.m_imPts.end() - patternW)); - VISP_NAMESPACE_ADDRESSING vpPolygon poly(corners); + vpPolygon poly(corners); for (unsigned int i = 0; i < I.getHeight(); i++) { for (unsigned int j = 0; j < I.getWidth(); j++) { - if (poly.isInside(VISP_NAMESPACE_ADDRESSING vpImagePoint(i, j))) { + if (poly.isInside(vpImagePoint(i, j))) { I[i][j] += pixel_value; } } @@ -164,17 +193,17 @@ void drawCalibrationOccupancy(VISP_NAMESPACE_ADDRESSING vpImage & } } -std::vector undistort(const VISP_NAMESPACE_ADDRESSING vpCameraParameters &cam_dist, const std::vector &imPts) +std::vector undistort(const vpCameraParameters &cam_dist, const std::vector &imPts) { - std::vector imPts_undist; + std::vector imPts_undist; - VISP_NAMESPACE_ADDRESSING vpCameraParameters cam(cam_dist.get_px(), cam_dist.get_py(), cam_dist.get_u0(), cam_dist.get_v0()); + vpCameraParameters cam(cam_dist.get_px(), cam_dist.get_py(), cam_dist.get_u0(), cam_dist.get_v0()); for (size_t i = 0; i < imPts.size(); i++) { double x = 0, y = 0; - VISP_NAMESPACE_ADDRESSING vpPixelMeterConversion::convertPoint(cam_dist, imPts[i], x, y); + vpPixelMeterConversion::convertPoint(cam_dist, imPts[i], x, y); - VISP_NAMESPACE_ADDRESSING vpImagePoint imPt; - VISP_NAMESPACE_ADDRESSING vpMeterPixelConversion::convertPoint(cam, x, y, imPt); + vpImagePoint imPt; + vpMeterPixelConversion::convertPoint(cam, x, y, imPt); imPts_undist.push_back(imPt); } @@ -205,7 +234,7 @@ bool extractCalibrationPoints(const Settings &s, const cv::Mat &cvI, std::vector if (found) // If done with success, { - std::vector data; + std::vector data; if (s.calibrationPattern == Settings::CHESSBOARD) { // improve the found corners' coordinate accuracy for chessboard @@ -221,6 +250,70 @@ bool extractCalibrationPoints(const Settings &s, const cv::Mat &cvI, std::vector return found; } +void computeDistortionDisplacementMap(const vpCameraParameters &cam_dist, vpImage &I_color) +{ + vpImage displacement_map(I_color.getHeight(), I_color.getWidth()); + + double min_displ = 1e12, max_displ = 0; + for (unsigned int i = 0; i < I_color.getHeight(); i++) { + for (unsigned int j = 0; j < I_color.getWidth(); j++) { + double x = 0, y = 0, id = 0, jd = 0; + vpPixelMeterConversion::convertPointWithoutDistortion(cam_dist, j, i, x, y); + vpMeterPixelConversion::convertPoint(cam_dist, x, y, jd, id); + + double erri = id - i; + double errj = jd - j; + double displ = std::sqrt(erri*erri + errj*errj); + displacement_map[i][j] = displ; + min_displ = displ < min_displ ? displ : min_displ; + max_displ = displ > max_displ ? displ : max_displ; + } + } + + double a = 255 / (max_displ - min_displ); + double b = (-255 * min_displ) / (max_displ - min_displ); + + vpImage I_gray(I_color.getHeight(), I_color.getWidth()); + for (unsigned int i = 0; i < displacement_map.getHeight(); i++) { + for (unsigned int j = 0; j < displacement_map.getWidth(); j++) { + I_gray[i][j] = static_cast(vpMath::clamp(a * displacement_map[i][j] + b, 0.0, 255.0)); + } + } + + vpColormap colormap(vpColormap::COLORMAP_TURBO); + colormap.convert(I_gray, I_color); +} + +void createMosaic(const std::vector> &list_imgs, std::vector> &list_mosaics) +{ + const unsigned int nb_rows = 4, nb_cols = 6; + const unsigned int nb_totals = nb_rows*nb_cols; + if (list_imgs.empty()) { + return; + } + + const unsigned int img_h = list_imgs[0].getHeight(); + const unsigned int img_w = list_imgs[0].getWidth(); + vpImage mosaic(img_h*nb_rows, img_w*nb_cols); + for (size_t i = 0; i < list_imgs.size(); i += nb_totals) { + mosaic = vpRGBa(0, 0, 0); + + for (size_t j = 0; j < nb_totals; j++) { + const size_t idx = i + j; + const unsigned int pos_mod = idx % nb_totals; + if (idx >= list_imgs.size()) { + break; + } + + const unsigned int pos_row = pos_mod / nb_cols; + vpImagePoint top_left(pos_row*img_h, (pos_mod - pos_row*nb_cols)*img_w); + mosaic.insert(list_imgs[idx], top_left); + } + + list_mosaics.push_back(mosaic); + } +} + } // namespace calib_helper #endif // DOXYGEN_SHOULD_SKIP_THIS diff --git a/apps/calibration/intrinsic/visp-calibrate-camera.cpp b/apps/calibration/intrinsic/visp-calibrate-camera.cpp index 9cd4c0fd75..ff999c176a 100644 --- a/apps/calibration/intrinsic/visp-calibrate-camera.cpp +++ b/apps/calibration/intrinsic/visp-calibrate-camera.cpp @@ -81,6 +81,7 @@ void usage(const char *argv[], int error) << " [--init-from-xml ]" << " [--camera-name ]" << " [--aspect-ratio ]" + << " [--save]" << " [--output ]" << " [--help, -h]" << std::endl << std::endl; @@ -98,9 +99,12 @@ void usage(const char *argv[], int error) << std::endl << " --aspect-ratio " << std::endl << " Pixel aspect ratio. To estimate px = py, use \"--aspect-ratio 1\" option. Set to -1" << std::endl - << " to unset any constraint for px and py parameters. " << std::endl + << " to unset any constraint for px and py parameters." << std::endl << " Default: -1." << std::endl << std::endl + << " --save" << std::endl + << " Flag to automatically save the image processing results in a new directory." << std::endl + << std::endl << " --output " << std::endl << " XML file containing estimated camera parameters." << std::endl << " Default: \"camera.xml\"." << std::endl @@ -131,11 +135,11 @@ int main(int argc, const char *argv[]) return EXIT_SUCCESS; } std::string opt_output_file_name = "camera.xml"; - Settings s; const std::string opt_inputSettingsFile = argc > 1 ? argv[1] : "default.cfg"; std::string opt_init_camera_xml_file; std::string opt_camera_name = "Camera"; double opt_aspect_ratio = -1; // Not used + bool save_results = false; for (int i = 2; i < argc; i++) { if (std::string(argv[i]) == "--init-from-xml" && i + 1 < argc) { @@ -150,6 +154,9 @@ int main(int argc, const char *argv[]) else if (std::string(argv[i]) == "--aspect-ratio" && i + 1 < argc) { opt_aspect_ratio = std::atof(argv[++i]); } + else if (std::string(argv[i]) == "--save") { + save_results = true; + } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { usage(argv, 0); return EXIT_SUCCESS; @@ -160,30 +167,46 @@ int main(int argc, const char *argv[]) } } - std::cout << "Settings from config file: " << argv[1] << std::endl; + // Results folder + log file + std::string save_results_folder = ""; + vpImage I_save_results; + std::ofstream log_file; + Tee tee(std::cout, log_file); + if (save_results) { + save_results_folder = vpTime::getDateTime("calib_results_%Y-%m-%d_%H.%M.%S"); + vpIoTools::makeDirectory(save_results_folder); + + std::string output_log_file_name = vpIoTools::createFilePath(save_results_folder, "calibration_log.txt"); + log_file.open(output_log_file_name); + tee << "Create output results folder: " << save_results_folder << "\n"; + tee << "Save calibration log into: " << output_log_file_name << "\n"; + } + + tee << "Settings from config file: " << argv[1] << "\n"; + Settings s(tee); if (!s.read(opt_inputSettingsFile)) { - std::cout << "Could not open the configuration file: \"" << opt_inputSettingsFile << "\"" << std::endl; + tee << "Could not open the configuration file: \"" << opt_inputSettingsFile << "\"\n"; usage(argv, 0); return EXIT_FAILURE; } if (!s.goodInput) { - std::cout << "Invalid input detected. Application stopping. " << std::endl; + tee << "Invalid input detected. Application stopping. " << "\n"; return EXIT_FAILURE; } - std::cout << "\nSettings from command line options: " << std::endl; + tee << "\nSettings from command line options: " << "\n"; if (!opt_init_camera_xml_file.empty()) { - std::cout << "Init parameters: " << opt_init_camera_xml_file << std::endl; + tee << "Init parameters: " << opt_init_camera_xml_file << "\n"; } - std::cout << "Ouput xml file : " << opt_output_file_name << std::endl; - std::cout << "Camera name : " << opt_camera_name << std::endl; + tee << "Ouput xml file : " << opt_output_file_name << "\n"; + tee << "Camera name : " << opt_camera_name << "\n"; // Check if output file name exists if (vpIoTools::checkFilename(opt_output_file_name)) { - std::cout << "\nOutput file name " << opt_output_file_name << " already exists." << std::endl; - std::cout << "Remove this file or change output file name using [--output ] command line option." - << std::endl; + tee << "\nOutput file name " << opt_output_file_name << " already exists." << "\n"; + tee << "Remove this file or change output file name using [--output ] command line option." + << "\n"; return EXIT_SUCCESS; } @@ -195,24 +218,24 @@ int main(int argc, const char *argv[]) reader.open(I); } catch (const vpException &e) { - std::cout << "Catch an exception: " << e.getStringMessage() << std::endl; - std::cout << "Check if input images name \"" << s.input << "\" set in " << opt_inputSettingsFile - << " config file is correct..." << std::endl; + tee << "Catch an exception: " << e.getStringMessage() << "\n"; + tee << "Check if input images name \"" << s.input << "\" set in " << opt_inputSettingsFile + << " config file is correct..." << "\n"; return EXIT_FAILURE; } #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) - std::shared_ptr d = vpDisplayFactory::createDisplay(I, vpDisplay::SCALE_AUTO); + std::shared_ptr d = vpDisplayFactory::createDisplay(I, -1, -1, "", vpDisplay::SCALE_AUTO); #else - d = vpDisplayFactory::allocateDisplay(I, vpDisplay::SCALE_AUTO); + d = vpDisplayFactory::allocateDisplay(I, -1, -1, "", vpDisplay::SCALE_AUTO); #endif vpCameraParameters cam_init; bool init_from_xml = false; if (!opt_init_camera_xml_file.empty()) { if (!vpIoTools::checkFilename(opt_init_camera_xml_file)) { - std::cout << "Input camera file \"" << opt_init_camera_xml_file << "\" doesn't exist!" << std::endl; - std::cout << "Modify [--init-from-xml ] option value" << std::endl; + tee << "Input camera file \"" << opt_init_camera_xml_file << "\" doesn't exist!" << "\n"; + tee << "Modify [--init-from-xml ] option value" << "\n"; #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) if (display != nullptr) { delete display; @@ -223,13 +246,13 @@ int main(int argc, const char *argv[]) init_from_xml = true; } if (init_from_xml) { - std::cout << "Initialize camera parameters from xml file: " << opt_init_camera_xml_file << std::endl; + tee << "Initialize camera parameters from xml file: " << opt_init_camera_xml_file << "\n"; vpXmlParserCamera parser; if (parser.parse(cam_init, opt_init_camera_xml_file, opt_camera_name, vpCameraParameters::perspectiveProjWithoutDistortion) != vpXmlParserCamera::SEQUENCE_OK) { - std::cout << "Unable to find camera with name \"" << opt_camera_name - << "\" in file: " << opt_init_camera_xml_file << std::endl; - std::cout << "Modify [--camera-name ] option value" << std::endl; + tee << "Unable to find camera with name \"" << opt_camera_name + << "\" in file: " << opt_init_camera_xml_file << "\n"; + tee << "Modify [--camera-name ] option value" << "\n"; #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) if (display != nullptr) { delete display; @@ -239,7 +262,7 @@ int main(int argc, const char *argv[]) } } else { - std::cout << "Initialize camera parameters with default values " << std::endl; + tee << "Initialize camera parameters with default values " << "\n"; // Initialize camera parameters double px = cam_init.get_px(); double py = cam_init.get_py(); @@ -249,7 +272,7 @@ int main(int argc, const char *argv[]) cam_init.initPersProjWithoutDistortion(px, py, u0, v0); } - std::cout << "Camera parameters used for initialization:\n" << cam_init << std::endl; + tee << "Camera parameters used for initialization:\n" << cam_init << "\n"; std::vector model; std::vector calibrator; @@ -265,6 +288,7 @@ int main(int argc, const char *argv[]) map_cam_sorted.insert(std::make_pair(1000, cam_init)); + double start_time = vpTime::measureTimeMs(); do { reader.acquire(I); long frame_index = reader.getFrameIndex(); @@ -277,14 +301,14 @@ int main(int argc, const char *argv[]) std::vector pointBuf; vpImageConvert::convert(I, cvI); - std::cout << "Process frame: " << frame_name << std::flush; + tee << "Process frame: " << frame_name; bool found = extractCalibrationPoints(s, cvI, pointBuf); - std::cout << ", grid detection status: " << found; + tee << ", grid detection status: " << found; if (!found) - std::cout << ", image rejected" << std::endl; + tee << ", image rejected" << "\n"; else - std::cout << ", image used as input data" << std::endl; + tee << ", image used as input data" << "\n"; if (found) { // If image processing done with success vpDisplay::setTitle(I, frame_name); @@ -324,8 +348,7 @@ int main(int argc, const char *argv[]) } } if (!calib_status) { - std::cout << "frame: " << frame_name << ", unable to calibrate from single image, image rejected" - << std::endl; + tee << "frame: " << frame_name << ", unable to calibrate from single image, image rejected" << "\n"; found = false; } } @@ -335,7 +358,7 @@ int main(int argc, const char *argv[]) "Image processing succeed", vpColor::green); else vpDisplay::displayText(I, 15 * vpDisplay::getDownScalingFactor(I), 15 * vpDisplay::getDownScalingFactor(I), - "Image processing failed", vpColor::green); + "Image processing failed", vpColor::red); if (s.tempo > 10.f) { vpDisplay::displayText(I, 35 * vpDisplay::getDownScalingFactor(I), 15 * vpDisplay::getDownScalingFactor(I), @@ -347,12 +370,19 @@ int main(int argc, const char *argv[]) vpDisplay::flush(I); vpTime::wait(s.tempo * 1000); } + + if (save_results) { + vpDisplay::getImage(I, I_save_results); + std::ostringstream oss; + oss << save_results_folder << "/" << vpIoTools::getNameWE(reader.getFrameName()) << ".png"; + vpImageIo::write(I_save_results, oss.str()); + } } while (!reader.end()); // Now we consider the multi image calibration // Calibrate by a non linear method based on virtual visual servoing if (calibrator.empty()) { - std::cerr << "Unable to calibrate. Image processing failed !" << std::endl; + tee << "Unable to calibrate. Image processing failed !" << "\n"; #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) if (display != nullptr) { delete display; @@ -403,6 +433,14 @@ int main(int argc, const char *argv[]) vpTime::wait(s.tempo * 1000); } + std::vector> list_img_reproj, list_img_reproj_dist, list_img_undist; + if (save_results) { + vpDisplay::getImage(I_color, I_save_results); + std::ostringstream oss; + oss << save_results_folder << "/calibration_pattern_occupancy.png"; + vpImageIo::write(I_save_results, oss.str()); + } + d->close(I_color); std::stringstream ss_additional_info; @@ -431,10 +469,10 @@ int main(int argc, const char *argv[]) double error; // Initialize with camera parameter that has the lowest residual vpCameraParameters cam = map_cam_sorted.begin()->second; - std::cout << "\nCalibration without distortion in progress on " << calibrator.size() << " images..." << std::endl; + tee << "\nCalibration without distortion in progress on " << calibrator.size() << " images..." << "\n"; if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS, calibrator, cam, error, false) == EXIT_SUCCESS) { - std::cout << cam << std::endl; + tee << cam << "\n"; d->init(I); vpDisplay::setTitle(I, "Without distortion results"); @@ -443,7 +481,7 @@ int main(int argc, const char *argv[]) double reproj_error = sqrt(calibrator[i].getResidual() / calibrator[i].get_npt()); const CalibInfo &calib = calib_info[i]; - std::cout << "Image " << calib.m_frame_name << " reprojection error: " << reproj_error << std::endl; + tee << "Image " << calib.m_frame_name << " reprojection error: " << reproj_error << "\n"; ss_additional_info << "" << reproj_error << ""; I = calib.m_img; @@ -480,9 +518,18 @@ int main(int argc, const char *argv[]) vpDisplay::flush(I); vpTime::wait(s.tempo * 1000); } + + if (save_results) { + vpDisplay::getImage(I, I_save_results); + list_img_reproj.push_back(I_save_results); + std::ostringstream oss; + oss << save_results_folder << "/reproj_err_without_dist_" << vpIoTools::getNameWE(calib.m_frame_name) + << ".png"; + vpImageIo::write(I_save_results, oss.str()); + } } - std::cout << "\nGlobal reprojection error: " << error << std::endl; + tee << "\nGlobal reprojection error: " << error << "\n"; ss_additional_info << "" << error << ""; ss_additional_info << ""; @@ -490,18 +537,18 @@ int main(int argc, const char *argv[]) if (xml.save(cam, opt_output_file_name.c_str(), opt_camera_name, I.getWidth(), I.getHeight()) == vpXmlParserCamera::SEQUENCE_OK) - std::cout << "Camera parameters without distortion successfully saved in \"" << opt_output_file_name << "\"" - << std::endl; + tee << "Camera parameters without distortion successfully saved in \"" << opt_output_file_name << "\"" + << "\n"; else { - std::cout << "Failed to save the camera parameters without distortion in \"" << opt_output_file_name << "\"" - << std::endl; - std::cout << "A file with the same name exists. Remove it to be able " + tee << "Failed to save the camera parameters without distortion in \"" << opt_output_file_name << "\"" + << "\n"; + tee << "A file with the same name exists. Remove it to be able " "to save the parameters..." - << std::endl; + << "\n"; } } else { - std::cout << "Calibration without distortion failed." << std::endl; + tee << "Calibration without distortion failed." << "\n"; #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) if (display != nullptr) { delete display; @@ -512,10 +559,10 @@ int main(int argc, const char *argv[]) vpCameraParameters cam_without_dist = cam; std::vector calibrator_without_dist = calibrator; - std::cout << "\n\nCalibration with distortion in progress on " << calibrator.size() << " images..." << std::endl; + tee << "\n\nCalibration with distortion in progress on " << calibrator.size() << " images..." << "\n"; if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS_DIST, calibrator, cam, error, false) == EXIT_SUCCESS) { - std::cout << cam << std::endl; + tee << cam << "\n"; vpDisplay::setTitle(I, "With distortion results"); ss_additional_info << ""; @@ -523,7 +570,7 @@ int main(int argc, const char *argv[]) double reproj_error = sqrt(calibrator[i].getResidual_dist() / calibrator[i].get_npt()); const CalibInfo &calib = calib_info[i]; - std::cout << "Image " << calib.m_frame_name << " reprojection error: " << reproj_error << std::endl; + tee << "Image " << calib.m_frame_name << " reprojection error: " << reproj_error << "\n"; ss_additional_info << "" << reproj_error << ""; I = calib.m_img; vpDisplay::display(I); @@ -559,21 +606,29 @@ int main(int argc, const char *argv[]) vpDisplay::flush(I); vpTime::wait(s.tempo * 1000); } + + if (save_results) { + vpDisplay::getImage(I, I_save_results); + list_img_reproj_dist.push_back(I_save_results); + std::ostringstream oss; + oss << save_results_folder << "/reproj_err_with_dist_" << vpIoTools::getNameWE(calib.m_frame_name) << ".png"; + vpImageIo::write(I_save_results, oss.str()); + } } - std::cout << "\nGlobal reprojection error: " << error << std::endl; + tee << "\nGlobal reprojection error: " << error << "\n"; ss_additional_info << "" << error << ""; - ss_additional_info << ""; + // Display side-by-side original and undistorted images, draw lines from start and end calib pattern points vpImage I_undist; vpImage I_dist_undist(I.getHeight(), 2 * I.getWidth()); d->close(I); d->init(I_dist_undist, 0, 0, "Straight lines have to be straight - distorted image / undistorted image"); for (size_t idx = 0; idx < calib_info.size(); idx++) { - std::cout << "\nThis tool computes the line fitting error (mean distance error) on image points extracted from " + tee << "\nThis tool computes the line fitting error (mean distance error) on image points extracted from " "the raw distorted image." - << std::endl; + << "\n"; I = calib_info[idx].m_img; vpImageTools::undistort(I, cam, I_undist); @@ -597,18 +652,17 @@ int main(int argc, const char *argv[]) double a = 0, b = 0, c = 0; double line_fitting_error = vpMath::lineFitting(current_line, a, b, c); double line_fitting_error_undist = vpMath::lineFitting(current_line_undist, a, b, c); - std::cout << calib_info[idx].m_frame_name << " line " << i + 1 + tee << calib_info[idx].m_frame_name << " line " << i + 1 << " fitting error on distorted points: " << line_fitting_error - << " ; on undistorted points: " << line_fitting_error_undist << std::endl; + << " ; on undistorted points: " << line_fitting_error_undist << "\n"; vpImagePoint ip1 = current_line.front(); vpImagePoint ip2 = current_line.back(); vpDisplay::displayLine(I_dist_undist, ip1, ip2, vpColor::red); } - std::cout << "\nThis tool computes the line fitting error (mean distance error) on image points extracted from " - "the undistorted image" - << " (vpImageTools::undistort())." << std::endl; + tee << "\nThis tool computes the line fitting error (mean distance error) on image points extracted from " + "the undistorted image" << " (vpImageTools::undistort())." << "\n"; cv::Mat cvI; std::vector pointBuf; vpImageConvert::convert(I_undist, cvI); @@ -630,8 +684,8 @@ int main(int argc, const char *argv[]) double a = 0, b = 0, c = 0; double line_fitting_error = vpMath::lineFitting(current_line, a, b, c); - std::cout << calib_info[idx].m_frame_name << " undistorted image, line " << i + 1 - << " fitting error: " << line_fitting_error << std::endl; + tee << calib_info[idx].m_frame_name << " undistorted image, line " << i + 1 + << " fitting error: " << line_fitting_error << "\n"; vpImagePoint ip1 = current_line.front() + vpImagePoint(0, I.getWidth()); vpImagePoint ip2 = current_line.back() + vpImagePoint(0, I.getWidth()); @@ -640,13 +694,13 @@ int main(int argc, const char *argv[]) } else { std::string msg("Unable to detect grid on undistorted image"); - std::cout << msg << std::endl; - std::cout << "Check that the grid is not too close to the image edges" << std::endl; + tee << msg << "\n"; + tee << "Check that the grid is not too close to the image edges" << "\n"; vpDisplay::displayText(I_dist_undist, 15 * vpDisplay::getDownScalingFactor(I_dist_undist), - 15 * vpDisplay::getDownScalingFactor(I_dist_undist), + I.getWidth() + 15 * vpDisplay::getDownScalingFactor(I_dist_undist), calib_info[idx].m_frame_name + std::string(" undistorted"), vpColor::red); vpDisplay::displayText(I_dist_undist, 30 * vpDisplay::getDownScalingFactor(I_dist_undist), - 15 * vpDisplay::getDownScalingFactor(I_dist_undist), msg, vpColor::red); + I.getWidth() + 15 * vpDisplay::getDownScalingFactor(I_dist_undist), msg, vpColor::red); } if (s.tempo > 10.f) { @@ -660,9 +714,142 @@ int main(int argc, const char *argv[]) vpDisplay::flush(I_dist_undist); vpTime::wait(s.tempo * 1000); } + + if (save_results) { + vpDisplay::getImage(I_dist_undist, I_save_results); + std::ostringstream oss; + oss << save_results_folder << "/dist_vs_undist_" << vpIoTools::getNameWE(calib_info[idx].m_frame_name) + << ".png"; + vpImage I_right; + vpImageTools::crop(I_save_results, vpRect(I_save_results.getWidth()/2, 0, + I_save_results.getWidth()/2, I_save_results.getHeight()), I_right); + list_img_undist.push_back(I_right); + vpImageIo::write(I_save_results, oss.str()); + } + } + tee << "\n"; + + // Display pseudo distortion displacement map + d->close(I_dist_undist); + vpImage I_dist_map(I.getHeight(), I.getWidth()); + d->init(I_dist_map, 0, 0, "Distortion displacement map"); + calib_helper::computeDistortionDisplacementMap(cam, I_dist_map); + d->display(I_dist_map); + + const float ref_img_width = 640; + const unsigned int step = std::max(20u, static_cast((I_dist_undist.getWidth()/ref_img_width)*20)); + for (unsigned int i = 0; i < I_dist_map.getHeight(); i += step) { + for (unsigned int j = 0; j < I_dist_map.getWidth(); j += step) { + vpImagePoint imPt(i, j); + double x = 0, y = 0; + vpPixelMeterConversion::convertPointWithoutDistortion(cam, imPt, x, y); + vpImagePoint imPt_dist; + vpMeterPixelConversion::convertPoint(cam, x, y, imPt_dist); + + vpDisplay::displayArrow(I_dist_map, imPt, imPt_dist, vpColor::red, 2); + } + + unsigned int remainder_col = (I_dist_map.getWidth()-1) - (I_dist_map.getWidth() / step)*step; + if (remainder_col >= step/2) { + unsigned int j = I_dist_map.getWidth() - 1; + vpImagePoint imPt(i, j); + double x = 0, y = 0; + vpPixelMeterConversion::convertPointWithoutDistortion(cam, imPt, x, y); + vpImagePoint imPt_dist; + vpMeterPixelConversion::convertPoint(cam, x, y, imPt_dist); + + vpDisplay::displayArrow(I_dist_map, imPt, imPt_dist, vpColor::red, 2); + } + } + + // Last row / column + unsigned int remainder_row = (I_dist_map.getHeight()-1) - (I_dist_map.getHeight() / step)*step; + if (remainder_row >= step/2) { + unsigned int i = I_dist_map.getHeight() - 1; + + for (unsigned int j = 0; j < I_dist_map.getWidth(); j += step) { + vpImagePoint imPt(i, j); + double x = 0, y = 0; + vpPixelMeterConversion::convertPointWithoutDistortion(cam, imPt, x, y); + vpImagePoint imPt_dist; + vpMeterPixelConversion::convertPoint(cam, x, y, imPt_dist); + + vpDisplay::displayArrow(I_dist_map, imPt, imPt_dist, vpColor::red, 2); + } + + unsigned int remainder_col = (I_dist_map.getWidth()-1) - (I_dist_map.getWidth() / step)*step; + if (remainder_col >= step/2) { + unsigned int j = I_dist_map.getWidth() - 1; + vpImagePoint imPt(i, j); + double x = 0, y = 0; + vpPixelMeterConversion::convertPointWithoutDistortion(cam, imPt, x, y); + vpImagePoint imPt_dist; + vpMeterPixelConversion::convertPoint(cam, x, y, imPt_dist); + + vpDisplay::displayArrow(I_dist_map, imPt, imPt_dist, vpColor::red, 2); + } + } + + // Image center and camera principal point + vpDisplay::displayCross(I_dist_map, vpImagePoint((I_dist_map.getHeight()-1)/2, (I_dist_map.getWidth()-1)/2), + 10*vpDisplay::getDownScalingFactor(I_dist_map), vpColor::green, 2); + vpDisplay::displayCircle(I_dist_map, vpImagePoint(cam.get_v0(), cam.get_u0()), + 8*vpDisplay::getDownScalingFactor(I_dist_map), vpColor::red); + + if (s.tempo > 10.f) { + vpDisplay::displayText( + I_dist_map, I_dist_map.getHeight() - 20 * vpDisplay::getDownScalingFactor(I_dist_map), + 15 * vpDisplay::getDownScalingFactor(I_dist_map), "Click to continue...", vpColor::red); + vpDisplay::flush(I_dist_map); + vpDisplay::getClick(I_dist_map); + } + else { + vpDisplay::flush(I_dist_map); + vpTime::wait(s.tempo * 1000); + } + + if (save_results) { + vpDisplay::getImage(I_dist_map, I_save_results); + std::ostringstream oss; + oss << save_results_folder << "/distortion_displacement_map.png"; + vpImageIo::write(I_save_results, oss.str()); + + // Save mosaic images + { + std::vector> mosaics; + createMosaic(list_img_reproj, mosaics); + for (size_t i = 0; i < mosaics.size(); i++) { + oss.str(""); + oss << save_results_folder << "/mosaics_reproj_err_without_dist_%04d.png"; + char name[FILENAME_MAX]; + snprintf(name, FILENAME_MAX, oss.str().c_str(), i); + vpImageIo::write(mosaics[i], name); + } + } + { + std::vector> mosaics; + createMosaic(list_img_reproj_dist, mosaics); + for (size_t i = 0; i < mosaics.size(); i++) { + oss.str(""); + oss << save_results_folder << "/mosaics_reproj_err_with_dist_%04d.png"; + char name[FILENAME_MAX]; + snprintf(name, FILENAME_MAX, oss.str().c_str(), i); + vpImageIo::write(mosaics[i], name); + } + } + { + std::vector> mosaics; + createMosaic(list_img_undist, mosaics); + for (size_t i = 0; i < mosaics.size(); i++) { + oss.str(""); + oss << save_results_folder << "/mosaics_undist_%04d.png"; + char name[FILENAME_MAX]; + snprintf(name, FILENAME_MAX, oss.str().c_str(), i); + vpImageIo::write(mosaics[i], name); + } + } } - std::cout << std::endl; vpXmlParserCamera xml; // Camera poses @@ -679,25 +866,25 @@ int main(int argc, const char *argv[]) if (xml.save(cam, opt_output_file_name.c_str(), opt_camera_name, I.getWidth(), I.getHeight(), ss_additional_info.str()) == vpXmlParserCamera::SEQUENCE_OK) - std::cout << "Camera parameters without distortion successfully saved in \"" << opt_output_file_name << "\"" - << std::endl; + tee << "Camera parameters without distortion successfully saved in \"" << opt_output_file_name << "\"" + << "\n"; else { - std::cout << "Failed to save the camera parameters without distortion in \"" << opt_output_file_name << "\"" - << std::endl; - std::cout << "A file with the same name exists. Remove it to be able " + tee << "Failed to save the camera parameters without distortion in \"" << opt_output_file_name << "\"" + << "\n"; + tee << "A file with the same name exists. Remove it to be able " "to save the parameters..." - << std::endl; + << "\n"; } - std::cout << std::endl; - std::cout << "Estimated pose using vpPoseVector format: [tx ty tz tux tuy tuz] with translation in meter and " + tee << "\n"; + tee << "Estimated pose using vpPoseVector format: [tx ty tz tux tuy tuz] with translation in meter and " "rotation in rad" - << std::endl; + << "\n"; for (unsigned int i = 0; i < calibrator.size(); i++) - std::cout << "Estimated pose on input data extracted from " << calib_info[i].m_frame_name << ": " - << vpPoseVector(calibrator[i].cMo_dist).t() << std::endl; + tee << "Estimated pose on input data extracted from " << calib_info[i].m_frame_name << ": " + << vpPoseVector(calibrator[i].cMo_dist).t() << "\n"; } else { - std::cout << "Calibration with distortion failed." << std::endl; + tee << "Calibration with distortion failed." << "\n"; #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) if (display != nullptr) { delete display; @@ -706,7 +893,16 @@ int main(int argc, const char *argv[]) return EXIT_FAILURE; } - std::cout << "\nCamera calibration succeeded. Results are saved in " << "\"" << opt_output_file_name << "\"" << std::endl; + double end_time = vpTime::measureTimeMs(); + int milli = (end_time-start_time); + int seconds = milli / 1000; + milli %= 1000; + + int minutes = seconds / 60; + seconds %= 60; + + tee << "\nCamera calibration succeeded. Results are saved in " << "\"" << opt_output_file_name + << "\". Total time: " << minutes << " min " << seconds << " sec " << milli << " ms." << "\n"; #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11) if (display != nullptr) { delete display; diff --git a/doc/image/tutorial/calibration/img-chessboard-distortion-displacement-map.jpg b/doc/image/tutorial/calibration/img-chessboard-distortion-displacement-map.jpg new file mode 100644 index 0000000000..bfbc808c10 Binary files /dev/null and b/doc/image/tutorial/calibration/img-chessboard-distortion-displacement-map.jpg differ diff --git a/doc/tutorial/calibration/tutorial-calibration-intrinsic.dox b/doc/tutorial/calibration/tutorial-calibration-intrinsic.dox index 89f2a702ea..2b4276b150 100644 --- a/doc/tutorial/calibration/tutorial-calibration-intrinsic.dox +++ b/doc/tutorial/calibration/tutorial-calibration-intrinsic.dox @@ -133,8 +133,9 @@ $ ./tutorial-grabber-opencv --seqname chessboard-%02d.jpg --record 1 \section calibration 4. Calibration \subsection calibration_source_code 4.1. Source code -Note that all the material (source code) described in this tutorial is part of ViSP source code -(in `example/calibration` folder) and could be found in https://github.com/lagadic/visp/tree/master/example/calibration. +Note that all the material (source code) described in this tutorial is part of the ViSP source code +(in the `apps/calibration/intrinsic` folder) and could also be found on Github at +https://github.com/lagadic/visp/tree/master/apps/calibration/intrinsic url. The calibration tool is available in `visp-calibrate-camera.cpp` located in `apps/calibration/intrinsic` folder. @@ -144,7 +145,7 @@ We will not describe in detail the source, but just mention that: - the calibration tool takes as input a configuration file that allows to precise the kind of pattern used in the images (chessboard or circles grid), and the location of the images used as input. If `libjpeg` and `libpng` 3rd party libraries are installed and detected during ViSP configuration, you may consider .pgm, .ppm, .jpg, .png images. - Default configuration files are provided in \c example/calibration folder; + Default configuration files are provided in \c apps/calibration/intrinsic folder; - the resulting parameters are saved in \c camera.xml file. \subsection calibration_chessboard 4.2. With a chessboard @@ -465,7 +466,16 @@ chessboard-03.jpg undistorted image, line 5 fitting error: 0.08680507551 chessboard-03.jpg undistorted image, line 6 fitting error: 0.06309999164 \endcode -\subsubsection calibration_tools_poses 4.4.4. Camera poses +\subsubsection calibration_tools_poses 4.4.4. Camera lens distortion +A pseudo "distortion displacement map" is also displayed: + +\image html img-chessboard-distortion-displacement-map.jpg + +The effect of the lens distortion is visible with hot colors corresponding to much stronger effect of the distortion +compared to cold colors (typically in the center of the image). With barrel distortion, this effect tends to "push" the +pixels at the extremities to the image toward the center of the image. + +\subsubsection calibration_tools_poses 4.4.5. Camera poses There is `camera_calibration_show_extrinsics.py` script that allows to display camera poses. Prior to use that script, you need to install `scipy`: \code{.sh}