/* given a value in radians, returns a string parsed into either
hh:mm:ss.sss or (-)dd:mm:ss.sss format, as determined by setting
scale equal to either 24 or 360, respectively */
int xms2str (char* xms_str, unsigned xms_strlen, char sign,
double radians, double scale, unsigned places)
{
unsigned field_width[3] = {2, 2, 2};
unsigned field_precision[3] = {0, 0, 0};
double field_scale[3] = {0.0, 60.0, 60.0};
field_precision[2] = places;
field_scale[0] = scale;
radians /= 2.0 * M_PI;
return unit2str (xms_str, xms_strlen, sign,
3, field_width, field_precision,
field_scale, ':', radians);
}
int main(int argc, char **argv) {
if (argc > 3) {
std::cout << "Only the path of a SVO or a InitParams file can be passed in arg." << std::endl;
return -1;
}
// Quick check input arguments
bool readSVO = false;
std::string SVOName;
bool loadParams = false;
std::string ParamsName;
if (argc > 1) {
std::string _arg;
for (int i = 1; i < argc; i++) {
_arg = argv[i];
if (_arg.find(".svo") != std::string::npos) {
// If a SVO is given we save its name
readSVO = true;
SVOName = _arg;
}
if (_arg.find(".ZEDinitParam") != std::string::npos) {
// If a parameter file is given we save its name
loadParams = true;
ParamsName = _arg;
}
}
}
sl::zed::Camera* zed;
if (!readSVO) // Live Mode
zed = new sl::zed::Camera(sl::zed::HD720);
else // SVO playback mode
zed = new sl::zed::Camera(SVOName);
// Define a struct of parameters for the initialization
sl::zed::InitParams params;
if (loadParams) // A parameters file was given in argument, we load it
params.load(ParamsName);
// Enables verbosity in the console
params.verbose = true;
sl::zed::ERRCODE err = zed->init(params);
std::cout << "Error code : " << sl::zed::errcode2str(err) << std::endl;
if (err != sl::zed::SUCCESS) {
// Exit if an error occurred
delete zed;
return 1;
}
// Save the initialization parameters
// The file can be used later in any zed based application
params.save("MyParam");
char key = ' ';
int viewID = 0;
int confidenceThres = 100;
bool displayDisp = true;
bool displayConfidenceMap = false;
int width = zed->getImageSize().width;
int height = zed->getImageSize().height;
cv::Mat disp(height, width, CV_8UC4);
cv::Mat anaglyph(height, width, CV_8UC4);
cv::Mat confidencemap(height, width, CV_8UC4);
cv::Size displaySize(720, 404);
cv::Mat dispDisplay(displaySize, CV_8UC4);
cv::Mat anaglyphDisplay(displaySize, CV_8UC4);
cv::Mat confidencemapDisplay(displaySize, CV_8UC4);
sl::zed::SENSING_MODE dm_type = sl::zed::STANDARD;
// Mouse callback initialization
sl::zed::Mat depth;
zed->grab(dm_type);
depth = zed->retrieveMeasure(sl::zed::MEASURE::DEPTH); // Get the pointer
// Set the structure
mouseStruct._image = cv::Size(width, height);
mouseStruct._resize = displaySize;
mouseStruct.data = (float*) depth.data;
mouseStruct.step = depth.step;
mouseStruct.name = "DEPTH";
mouseStruct.unit = unit2str(params.unit);
// The depth is limited to 20 METERS, as defined in zed::init()
zed->setDepthClampValue(10000);
// Create OpenCV Windows
// NOTE: You may encounter an issue with OpenGL support, to solve it either
// use the default rendering by removing ' | cv::WINDOW_OPENGL' from the flags
// or recompile OpenCV with OpenGL support (you may also need the gtk OpenGL Extension
// on Linux, provided by the packages libgtkglext1 libgtkglext1-dev)
cv::namedWindow(mouseStruct.name, cv::WINDOW_AUTOSIZE | cv::WINDOW_OPENGL);
cv::setMouseCallback(mouseStruct.name, onMouseCallback, (void*) &mouseStruct);
cv::namedWindow("VIEW", cv::WINDOW_AUTOSIZE | cv::WINDOW_OPENGL);
std::cout << "Press 'q' to exit" << std::endl;
// Jetson only. Execute the calling thread on core 2
sl::zed::Camera::sticktoCPUCore(2);
sl::zed::ZED_SELF_CALIBRATION_STATUS old_self_calibration_status = sl::zed::SELF_CALIBRATION_NOT_CALLED;
// Loop until 'q' is pressed
while (key != 'q') {
// Disparity Map filtering
zed->setConfidenceThreshold(confidenceThres);
// Get frames and launch the computation
bool res = zed->grab(dm_type);
if (!res) {
if (old_self_calibration_status != zed->getSelfCalibrationStatus()) {
std::cout << "Self Calibration Status : " << sl::zed::statuscode2str(zed->getSelfCalibrationStatus()) << std::endl;
old_self_calibration_status = zed->getSelfCalibrationStatus();
}
depth = zed->retrieveMeasure(sl::zed::MEASURE::DEPTH); // Get the pointer
// The following is the best way to retrieve a disparity map / image / confidence map in OpenCV Mat.
// If the buffer is not duplicated, it will be replaced by a next retrieve (retrieveImage, normalizeMeasure, getView...)
// Disparity, depth, confidence are 32F buffer by default and 8UC4 buffer in normalized format (displayable grayscale)
// -- The next part is about displaying the data --
// Normalize the disparity / depth map in order to use the full color range of gray level image
if (displayDisp)
slMat2cvMat(zed->normalizeMeasure(sl::zed::MEASURE::DISPARITY)).copyTo(disp);
else
slMat2cvMat(zed->normalizeMeasure(sl::zed::MEASURE::DEPTH)).copyTo(disp);
// To get the depth at a given position, click on the disparity / depth map image
cv::resize(disp, dispDisplay, displaySize);
imshow(mouseStruct.name, dispDisplay);
if (displayConfidenceMap) {
slMat2cvMat(zed->normalizeMeasure(sl::zed::MEASURE::CONFIDENCE)).copyTo(confidencemap);
cv::resize(confidencemap, confidencemapDisplay, displaySize);
imshow("confidence", confidencemapDisplay);
}
// 'viewID' can be 'SIDE mode' or 'VIEW mode'
if (viewID >= sl::zed::LEFT && viewID < sl::zed::LAST_SIDE)
slMat2cvMat(zed->retrieveImage(static_cast<sl::zed::SIDE> (viewID))).copyTo(anaglyph);
else
slMat2cvMat(zed->getView(static_cast<sl::zed::VIEW_MODE> (viewID - (int) sl::zed::LAST_SIDE))).copyTo(anaglyph);
cv::resize(anaglyph, anaglyphDisplay, displaySize);
imshow("VIEW", anaglyphDisplay);
key = cv::waitKey(5);
// Keyboard shortcuts
switch (key) {
case 'b':
if (confidenceThres >= 10)
confidenceThres -= 10;
break;
case 'n':
if (confidenceThres <= 90)
confidenceThres += 10;
break;
// From 'SIDE' enum
case '0': // Left
viewID = 0;
std::cout << "Current View switched to Left (rectified/aligned)" << std::endl;
break;
case '1': // Right
viewID = 1;
std::cout << "Current View switched to Right (rectified/aligned)" << std::endl;
break;
// From 'VIEW' enum
case '2': // Side by Side
viewID = 10;
std::cout << "Current View switched to Side by Side mode" << std::endl;
break;
case '3': // Overlay
viewID = 11;
std::cout << "Current View switched to Overlay mode" << std::endl;
break;
case '4': // Difference
viewID = 9;
std::cout << "Current View switched to Difference mode" << std::endl;
break;
case '5': // Anaglyph
viewID = 8;
std::cout << "Current View switched to Anaglyph mode" << std::endl;
break;
case 'c':
displayConfidenceMap = !displayConfidenceMap;
break;
case 's':
dm_type = (dm_type == sl::zed::SENSING_MODE::STANDARD) ? sl::zed::SENSING_MODE::FILL : sl::zed::SENSING_MODE::STANDARD;
std::cout << "SENSING_MODE " << sensing_mode2str(dm_type) << std::endl;
break;
case 'd':
displayDisp = !displayDisp;
break;
}
} else key = cv::waitKey(5);
}
delete zed;
return 0;
}
