Skip to content

Commit

Permalink
Update for ZED SDK 2.2
Browse files Browse the repository at this point in the history
  • Loading branch information
@nesnes
nesnes committed Nov 3, 2017
1 parent 1de2960 commit c6c3894
Show file tree
Hide file tree
Showing 8 changed files with 46 additions and 68 deletions.
1 change: 1 addition & 0 deletions positional tracking/include/TrackingViewer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ class TrackBallCamera {
};

class TrackingViewer {
const std::string str_tracking = "POSITIONAL TRACKING : ";
public:

TrackingViewer();
Expand Down
12 changes: 6 additions & 6 deletions positional tracking/src/TrackingViewer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -321,7 +321,8 @@ void TrackingViewer::idle() {

void TrackingViewer::exit() {
run = false;
glutLeaveMainLoop();
if (isInit)
glutLeaveMainLoop();
}

void TrackingViewer::mouse(int button, int state, int x, int y) {
Expand Down Expand Up @@ -471,18 +472,18 @@ void TrackingViewer::printText() {
int start_w = 20;
int start_h = h_wnd - 40;

bool trackingIsOK = trackState == sl::TRACKING_STATE_OK;

trackingIsOK ? glColor3f(0.2f, 0.65f, 0.2f) : glColor3f(0.85f, 0.2f, 0.2f);
(trackState == sl::TRACKING_STATE_OK) ? glColor3f(0.2f, 0.65f, 0.2f) : glColor3f(0.85f, 0.2f, 0.2f);
glRasterPos2i(start_w, start_h);
safe_glutBitmapString(GLUT_BITMAP_HELVETICA_18, sl::trackingState2str(trackState).c_str());
std::string track_str = (str_tracking + sl::toString(trackState).c_str());
safe_glutBitmapString(GLUT_BITMAP_HELVETICA_18, track_str.c_str());

glColor3f(0.9255f, 0.9412f, 0.9451f);
glRasterPos2i(start_w, start_h - 25);
safe_glutBitmapString(GLUT_BITMAP_HELVETICA_18, "Translation (m) :");

glColor3f(0.4980f, 0.5490f, 0.5529f);
glRasterPos2i(155, start_h - 25);

safe_glutBitmapString(GLUT_BITMAP_HELVETICA_18, txtT.c_str());

glColor3f(0.9255f, 0.9412f, 0.9451f);
Expand All @@ -501,7 +502,6 @@ void TrackingViewer::printText() {
}

void TrackingViewer::updateText(std::string stringT, std::string stringR, sl::TRACKING_STATE state) {

if (!getViewerState())
return;

Expand Down
21 changes: 12 additions & 9 deletions positional tracking/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,9 @@ int main(int argc, char **argv) {
initParameters.coordinate_units = UNIT_METER;
initParameters.coordinate_system = COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP;

if (argc > 1 && std::string(argv[1]).find(".svo"))
initParameters.svo_input_filename.set(argv[1]);

// Open the camera
ERROR_CODE err = zed.open(initParameters);
if (err != sl::SUCCESS) {
Expand All @@ -77,7 +80,7 @@ int main(int argc, char **argv) {
// Set positional tracking parameters
TrackingParameters trackingParameters;
trackingParameters.initial_world_transform = sl::Transform::identity();
trackingParameters.enable_spatial_memory = true; // Enable Spatial memory
trackingParameters.enable_spatial_memory = true;

// Start motion tracking
zed.enableTracking(trackingParameters);
Expand Down Expand Up @@ -123,14 +126,14 @@ void run() {
// Create a CSV file to log motion tracking data
std::ofstream outputFile;
std::string csvName = "Motion_data";
outputFile.open(csvName + ".csv");
if (!outputFile.is_open())
cout << "WARNING: Can't create CSV file. Run the application with administrator rights." << endl;
else
outputFile << "Timestamp(ns);Rotation_X(rad);Rotation_Y(rad);Rotation_Z(rad);Position_X(m);Position_Y(m);Position_Z(m);" << endl;

while (!quit) {
outputFile.open(csvName + ".csv");
if (!outputFile.is_open())
cout << "WARNING: Can't create CSV file. Run the application with administrator rights." << endl;
else
outputFile << "Timestamp(ns);Rotation_X(rad);Rotation_Y(rad);Rotation_Z(rad);Position_X(m);Position_Y(m);Position_Z(m);" << endl;


while (!quit && zed.getSVOPosition() != zed.getSVONumberOfFrames() - 1) {
if (zed.grab() == SUCCESS) {
// Get the position of the camera in a fixed reference frame (the World Frame)
TRACKING_STATE tracking_state = zed.getPosition(camera_pose, sl::REFERENCE_FRAME_WORLD);
Expand Down
39 changes: 22 additions & 17 deletions spatial mapping/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,13 +121,13 @@ int main(int argc, char** argv) {
glutCreateWindow("ZED Spatial Mapping");

// Configure Spatial Mapping and filtering parameters
spatial_mapping_params.range_meter.second = sl::SpatialMappingParameters::get(sl::SpatialMappingParameters::RANGE_FAR);
spatial_mapping_params.resolution_meter = sl::SpatialMappingParameters::get(sl::SpatialMappingParameters::RESOLUTION_LOW);
spatial_mapping_params.range_meter = sl::SpatialMappingParameters::get(sl::SpatialMappingParameters::MAPPING_RANGE_FAR);
spatial_mapping_params.resolution_meter = sl::SpatialMappingParameters::get(sl::SpatialMappingParameters::MAPPING_RESOLUTION_LOW);
spatial_mapping_params.save_texture = false;
spatial_mapping_params.max_memory_usage = 512;
spatial_mapping_params.keep_mesh_consistent = !USE_CHUNKS; // If we use chunks we do not need to keep the mesh consistent
spatial_mapping_params.use_chunk_only = USE_CHUNKS; // If we use chunks we do not need to keep the mesh consistent

filter_params.set(sl::MeshFilterParameters::FILTER_LOW);
filter_params.set(sl::MeshFilterParameters::MESH_FILTER_LOW);

// Initialize OpenGL
int res = initGL();
Expand Down Expand Up @@ -189,7 +189,6 @@ void startMapping() {

mapping_is_started = true;
std::cout << "** Spatial Mapping is started ... **" << std::endl;
return;
}

/**
Expand All @@ -206,7 +205,7 @@ void stopMapping() {
std::cout << ">> Mesh has been extracted..." << std::endl;

// Filter the extracted mesh
wholeMesh.filter(filter_params, !USE_CHUNKS);
wholeMesh.filter(filter_params, USE_CHUNKS);
std::cout << ">> Mesh has been filtered..." << std::endl;

// If textures have been saved during spatial mapping, apply them to the mesh
Expand All @@ -222,11 +221,14 @@ void stopMapping() {
else std::cout << ">> Failed to save the mesh under " << saveName << std::endl;

// Update the displayed Mesh
#if USE_CHUNKS
mesh_object.clear();
mesh_object.resize(wholeMesh.chunks.size());
for (int c = 0; c < wholeMesh.chunks.size(); c++)
mesh_object[c].updateMesh(wholeMesh.chunks[c].vertices, wholeMesh.chunks[c].triangles);
return;
#else
mesh_object[0].updateMesh(wholeMesh.vertices, wholeMesh.triangles);
#endif
}

/**
Expand Down Expand Up @@ -258,7 +260,6 @@ void run() {
}

if (zed.getMeshRequestStatusAsync() == sl::SUCCESS) {

// Get the current mesh generated and send it to opengl
if (zed.retrieveMeshAsync(mesh) == sl::SUCCESS) {
#if USE_CHUNKS
Expand Down Expand Up @@ -436,7 +437,7 @@ void drawGL() {
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

#if USE_CHUNKS
for (int c = 0; c < mesh.chunks.size(); c++)
for (int c = 0; c < mesh_object.size(); c++)
mesh_object[c].draw(GL_TRIANGLES);
#else
mesh_object[0].draw(GL_TRIANGLES);
Expand Down Expand Up @@ -466,29 +467,33 @@ void drawGL() {
// Show actions
glColor4f(0.25f, 0.99f, 0.25f, 1.f);
if (!mapping_is_started)
printGL(-0.99f, 0.9f, "* Press Space Bar to activate Spatial Mapping.");
printGL(-0.99f, 0.9f, "Press Space Bar to activate Spatial Mapping.");
else
printGL(-0.99f, 0.9f, "* Press Space Bar to stop spatial mapping.");
printGL(-0.99f, 0.9f, "Press Space Bar to stop spatial mapping.");

std::string positional_tracking_state_str("POSITIONAL TRACKING STATE : ");
std::string spatial_mapping_state_str("SPATIAL MAPPING STATE : ");
std::string state_str;
// Show mapping state
if ((tracking_state == sl::TRACKING_STATE_OK)) {
sl::SPATIAL_MAPPING_STATE state = zed.getSpatialMappingState();
if (state == sl::SPATIAL_MAPPING_STATE_OK || state == sl::SPATIAL_MAPPING_STATE_INITIALIZING)
sl::SPATIAL_MAPPING_STATE spatial_mapping_state = zed.getSpatialMappingState();
if (spatial_mapping_state == sl::SPATIAL_MAPPING_STATE_OK || spatial_mapping_state == sl::SPATIAL_MAPPING_STATE_INITIALIZING)
glColor4f(0.25f, 0.99f, 0.25f, 1.f);
else if (state == sl::SPATIAL_MAPPING_STATE_NOT_ENABLED)
else if (spatial_mapping_state == sl::SPATIAL_MAPPING_STATE_NOT_ENABLED)
glColor4f(0.55f, 0.65f, 0.55f, 1.f);
else
glColor4f(0.95f, 0.25f, 0.25f, 1.f);
printGL(-0.99f, 0.83f, (std::string("** ") + sl::spatialMappingState2str(state)).c_str());
state_str = spatial_mapping_state_str + sl::toString(spatial_mapping_state).c_str();
} else {
if (mapping_is_started) {
glColor4f(0.95f, 0.25f, 0.25f, 1.f);
printGL(-0.99f, 0.83f, (std::string("** ") + sl::trackingState2str(tracking_state)).c_str());
state_str = positional_tracking_state_str + sl::toString(tracking_state).c_str();
} else {
glColor4f(0.55f, 0.65f, 0.55f, 1.f);
printGL(-0.99f, 0.83f, (std::string("** ") + sl::spatialMappingState2str(sl::SPATIAL_MAPPING_STATE_NOT_ENABLED)).c_str());
state_str = spatial_mapping_state_str + sl::toString(sl::SPATIAL_MAPPING_STATE_NOT_ENABLED).c_str();
}
}
printGL(-0.99f, 0.83f, state_str.c_str());

// Swap buffers
glutSwapBuffers();
Expand Down
2 changes: 1 addition & 1 deletion svo recording/export/include/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ void ProgressBar(float ratio, unsigned int w) {
}

// If the current project uses openCV
#if defined (__OPENCV_ALL_HPP__) or defined(OPENCV_ALL_HPP)
#if defined (__OPENCV_ALL_HPP__) || defined(OPENCV_ALL_HPP)
// Conversion function between sl::Mat and cv::Mat
cv::Mat slMat2cvMat(sl::Mat &input) {
int cv_type = -1;
Expand Down
2 changes: 1 addition & 1 deletion svo recording/playback/include/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ void ProgressBar(float ratio, unsigned int w) {
}

// If the current project uses openCV
#if defined (__OPENCV_ALL_HPP__) or defined(OPENCV_ALL_HPP)
#if defined (__OPENCV_ALL_HPP__) || defined(OPENCV_ALL_HPP)
// Conversion function between sl::Mat and cv::Mat
cv::Mat slMat2cvMat(sl::Mat &input) {
int cv_type = -1;
Expand Down
33 changes: 1 addition & 32 deletions svo recording/recording/include/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,35 +45,4 @@ void SetCtrlHandler() {
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
#endif
}

// Display progress bar
void ProgressBar(float ratio, unsigned int w) {
int c = ratio * w;
for (int x = 0; x < c; x++) std::cout << "=";
for (int x = c; x < w; x++) std::cout << " ";
std::cout << (int) (ratio * 100) << "% ";
std::cout << "\r" << std::flush;
}

// If the current project uses openCV
#ifdef __OPENCV_ALL_HPP__
// Conversion function between sl::Mat and cv::Mat
cv::Mat slMat2cvMat(sl::Mat &input) {
int cv_type = -1;
switch (input.getDataType()) {
case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
default: break;
}
// Since cv::Mat data requires a uchar* pointer, we get the uchar1 pointer from sl::Mat (getPtr<T>())
// cv::Mat and sl::Mat will share a single memory structure
return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
}
#endif
}
4 changes: 2 additions & 2 deletions tutorials/tutorial 5 - spatial mapping/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ int main(int argc, char **argv) {
int i = 0;
sl::Mesh mesh; // Create a mesh object
while (i < 500) {
// For each new grab, mesh data is updated
// For each new grab, mesh data is updated
if (zed.grab() == SUCCESS) {
// In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh
sl::SPATIAL_MAPPING_STATE mapping_state = zed.getSpatialMappingState();
Expand All @@ -71,7 +71,7 @@ int main(int argc, char **argv) {
printf("Extracting Mesh...\n");
zed.extractWholeMesh(mesh); // Extract the whole mesh
printf("Filtering Mesh...\n");
mesh.filter(sl::MeshFilterParameters::FILTER_LOW); // Filter the mesh (remove unnecessary vertices and faces)
mesh.filter(sl::MeshFilterParameters::MESH_FILTER_LOW); // Filter the mesh (remove unnecessary vertices and faces)
printf("Saving Mesh...\n");
mesh.save("mesh.obj"); // Save the mesh in an obj file

Expand Down

0 comments on commit c6c3894

Please sign in to comment.