From cd465b80b18228e6f9ef9109319c731e4ca61142 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Tue, 9 Jul 2024 14:33:53 -0400 Subject: [PATCH 01/12] Initial stereo-disparity depth map --- core/components/CMakeLists.txt | 7 +- .../code/mtsAtracsysFusionTrack.cpp | 496 +++++++++++++----- core/components/code/mtsAtracsysStereo.cpp | 296 +++++++++++ .../mtsAtracsysFusionTrack.h | 13 + .../mtsAtracsysStereo.h | 108 ++++ ros/CMakeLists.txt | 5 +- ros/src/atracsys.cpp | 18 +- ros/src/mts_ros_crtk_atracsys_bridge.cpp | 1 + 8 files changed, 808 insertions(+), 136 deletions(-) create mode 100644 core/components/code/mtsAtracsysStereo.cpp create mode 100644 core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h diff --git a/core/components/CMakeLists.txt b/core/components/CMakeLists.txt index 4834176..862d036 100755 --- a/core/components/CMakeLists.txt +++ b/core/components/CMakeLists.txt @@ -48,6 +48,8 @@ if (cisst_FOUND_AS_REQUIRED) endif () endif () + find_package(OpenCV REQUIRED) + # catkin/ROS paths cisst_set_output_path () @@ -89,12 +91,15 @@ if (cisst_FOUND_AS_REQUIRED) add_library (sawAtracsysFusionTrack ${IS_SHARED} ${sawAtracsysFusionTrack_HEADER_DIR}/sawAtracsysFusionTrackExport.h code/mtsAtracsysFusionTrack.cpp - ${sawAtracsysFusionTrack_HEADER_DIR}/mtsAtracsysFusionTrack.h) + code/mtsAtracsysStereo.cpp + ${sawAtracsysFusionTrack_HEADER_DIR}/mtsAtracsysFusionTrack.h + ${sawAtracsysFusionTrack_HEADER_DIR}/mtsAtracsysStereo.h) set_target_properties (sawAtracsysFusionTrack PROPERTIES VERSION ${sawAtracsysFusionTrack_VERSION} FOLDER "sawAtracsysFusionTrack") cisst_target_link_libraries (sawAtracsysFusionTrack ${REQUIRED_CISST_LIBRARIES}) target_link_libraries (sawAtracsysFusionTrack ${AtracsysSDK_LIBRARIES}) + target_link_libraries (sawAtracsysFusionTrack ${OpenCV_LIBS}) configure_file ( "${sawAtracsysFusionTrack_SOURCE_DIR}/code/sawAtracsysFusionTrackConfig.h.in" diff --git a/core/components/code/mtsAtracsysFusionTrack.cpp b/core/components/code/mtsAtracsysFusionTrack.cpp index b0c2590..ec31d4b 100644 --- a/core/components/code/mtsAtracsysFusionTrack.cpp +++ b/core/components/code/mtsAtracsysFusionTrack.cpp @@ -16,13 +16,17 @@ no warranty. The complete license can be found in license.txt and --- end cisst license --- */ +#include + #include + #include #include #include + #include #include -#include + CMN_IMPLEMENT_SERVICES_DERIVED_ONEARG(mtsAtracsysFusionTrack, mtsTaskContinuous, mtsTaskContinuousConstructorArg); @@ -221,7 +225,9 @@ class mtsAtracsysFusionTrackInternals mtsAtracsysFusionTrackInternals(): m_library(0), m_sn(0), - m_frame_query(0) + m_frame_query(nullptr), + m_configured(false), + m_images_enabled(false) {} void SetupFrameQuery(const size_t number_of_tools, @@ -230,7 +236,6 @@ class mtsAtracsysFusionTrackInternals delete m_frame_query; } m_frame_query = ftkCreateFrame(); - // memset(m_frame_query, 0, sizeof(ftkFrameQuery)); // lead to error 4 with SDK 4.4.1 // tools, aka fT Markers m_tools = new ftkMarker[number_of_tools]; @@ -241,8 +246,7 @@ class mtsAtracsysFusionTrackInternals m_frame_query->threeDFiducials = m_stray_markers; m_frame_query->threeDFiducialsVersionSize.ReservedSize = sizeof(ftk3DFiducial) * number_of_stray_markers; - ftkError err(ftkSetFrameOptions(false, false, 16u, 16u, - // 0u, 16u, + ftkError err(ftkSetFrameOptions(m_images_enabled, false, 16u, 16u, number_of_stray_markers, number_of_tools, m_frame_query)); if (err != FTK_ERROR_NS::FTK_OK) { @@ -252,6 +256,170 @@ class mtsAtracsysFusionTrackInternals } } + std::string QueryDeviceType() + { + ftkBuffer buffer; + ftkError status = ftkGetData(m_library, m_sn, option_id_sTk_device_type, &buffer); + if (status != ftkError::FTK_OK) return ""; + + std::string device_type(buffer.data); + + return device_type; + } + + ftkError ConfigureImageSending(bool enabled) + { + ftkBuffer buffer; + buffer.reset(); + std::string frame_pattern = "S"; + std::memcpy(buffer.uData, frame_pattern.c_str(), frame_pattern.length()); + buffer.size = frame_pattern.length(); + + ftkError status = ftkSetData(m_library, m_sn, option_id_image_pattern, &buffer); + if (status != ftkError::FTK_OK) { + return status; + } + + int32_t enabled_value = enabled ? 1 : 0; + status = ftkSetInt32(m_library, m_sn, option_id_enable_images_sending, enabled_value); + if (status == ftkError::FTK_OK) { + m_images_enabled = enabled; + } else { + return status; + } + + status = ftkSetInt32(m_library, m_sn, option_id_vis_integration_time, 16000); + if (status != ftkError::FTK_OK) { + return status; + } + + status = ftkSetInt32(m_library, m_sn, option_id_sl_integration_time, 16000); + if (status != ftkError::FTK_OK) { + return status; + } + + status = ftkSetInt32(m_library, m_sn, option_id_num_enabled_dot_projector, 3); + if (status != ftkError::FTK_OK) { + return status; + } + + status = ftkSetInt32(m_library, m_sn, option_id_dot_projector_strobe_time, 16000); + if (status != ftkError::FTK_OK) { + return status; + } + + return ftkError::FTK_OK; + } + + void ExtractImage(uint8_t *pixels, prmImageFrame& dest) + { + dest.Width() = m_frame_query->imageHeader->width; + dest.Height() = m_frame_query->imageHeader->height; + dest.Channels() = 1; + + const size_t size = dest.Width() * dest.Height(); + dest.Data().resize(size); + std::copy(pixels, pixels + size, dest.Data().begin()); + } + + void ExtractLeftImage(prmImageFrame& dest) + { + ExtractImage(m_frame_query->imageLeftPixels, dest); + } + + void ExtractRightImage(prmImageFrame& dest) + { + ExtractImage(m_frame_query->imageRightPixels, dest); + } + + // Convert ftkCameraParameters to standard 3x3 intrinsic matrix representation + void CameraParamsToIntrinsicMatrix(ftkCameraParameters& params, vct3x3& intrinsic_out) + { + double fu = params.FocalLength[0]; + double fv = params.FocalLength[1]; + + intrinsic_out.at(0, 0) = fu; + intrinsic_out.at(1, 1) = fv; + intrinsic_out.at(0, 1) = fu * params.Skew; + intrinsic_out.at(0, 2) = params.OpticalCentre[0]; + intrinsic_out.at(1, 2) = params.OpticalCentre[1]; + intrinsic_out.at(2, 2) = 1.0f; + } + + // Retrieve stereo camera calibration from device, return true/false for success/error + bool RetrieveStereoCameraCalibration(prmCameraInfo& left, prmCameraInfo& right, vct3& rotation, vct3& translation) + { + ftkError status; + + status = ftkSetInt32(m_library, m_sn, option_id_export_calibration, 1); + if (status != ftkError::FTK_OK) { + + CMN_LOG_RUN_ERROR << "Error enabling stereo calibration export: " + << static_cast(status) << std::endl; + return false; + } + + // need initialized frame query to retrieve camera calibration + if (!m_frame_query) { + SetupFrameQuery(0, 0); + } + + int attempts = 0; + int max_attempts = 20; + + do { + status = ftkGetLastFrame(m_library, m_sn, m_frame_query, 100u); + } while (status != ftkError::FTK_OK && attempts++ < max_attempts); + + if (status != ftkError::FTK_OK) { + CMN_LOG_RUN_ERROR << "Error retrieving stereo calibration frame: " + << static_cast(status) << std::endl; + return false; + } + + status = ftkSetInt32(m_library, m_sn, option_id_export_calibration, 0); + if (status != ftkError::FTK_OK) { + CMN_LOG_RUN_ERROR << "Error disabling stereo calibration export: " + << static_cast(status) << std::endl; + return false; + } + + ftkFrameInfoData frame_info; + frame_info.WantedInformation = ftkInformationType::CalibrationParameters; + + status = ftkExtractFrameInfo(m_frame_query, &frame_info); + if (status != ftkError::FTK_OK) { + CMN_LOG_RUN_ERROR << "Error extracting frame info for stereo calibration: " + << static_cast(status) << std::endl; + return false; + } + + ftkStereoParameters stereo_params = frame_info.Calibration; + ftkCameraParameters left_params = stereo_params.LeftCamera; + ftkCameraParameters right_params = stereo_params.RightCamera; + + auto header = m_frame_query->imageHeader; + left.Width() = right.Width() = header->width; + left.Height() = right.Height() = header->height; + + left.DistortionParameters().SetSize(5); + right.DistortionParameters().SetSize(5); + for (size_t idx = 0; idx < 5; idx++) { + left.DistortionParameters()[idx] = left_params.Distorsions[idx]; + right.DistortionParameters()[idx] = right_params.Distorsions[idx]; + } + + CameraParamsToIntrinsicMatrix(left_params, left.Intrinsic()); + CameraParamsToIntrinsicMatrix(right_params, right.Intrinsic()); + + for (size_t idx = 0; idx < 3; idx++) { + rotation[idx] = stereo_params.Rotation[idx]; + translation[idx] = stereo_params.Translation[idx] * cmn_mm; + } + + return true; + } + ftkLibrary m_library; uint64 m_sn; ftkFrameQuery * m_frame_query; @@ -259,10 +427,21 @@ class mtsAtracsysFusionTrackInternals ftk3DFiducial * m_stray_markers; bool m_configured; + bool m_images_enabled; + typedef std::map GeometryIdToToolMap; GeometryIdToToolMap GeometryIdToTool; -}; +private: + const uint32_t option_id_sTk_device_type = 221; + const uint32_t option_id_enable_images_sending = 6003; + const uint32_t option_id_image_pattern = 199; + const uint32_t option_id_vis_integration_time = 193; + const uint32_t option_id_sl_integration_time = 194; + const uint32_t option_id_num_enabled_dot_projector = 110; + const uint32_t option_id_dot_projector_strobe_time = 197; + const uint32_t option_id_export_calibration = 181; +}; static void mtsAtracsysFusionTrackDeviceEnum(uint64 device, void * user, ftkDeviceType CMN_UNUSED(type)) { @@ -272,7 +451,6 @@ static void mtsAtracsysFusionTrackDeviceEnum(uint64 device, void * user, ftkDevi } } - void mtsAtracsysFusionTrack::Init(void) { m_stray_markers_max = 10; @@ -283,7 +461,14 @@ void mtsAtracsysFusionTrack::Init(void) AddStateTable(&m_configuration_state_table); m_configuration_state_table.AddData(m_crtk_interfaces_provided, "crtk_interfaces_provided"); + m_configuration_state_table.AddData(m_left_camera_info, "left_camera_info"); + m_configuration_state_table.AddData(m_right_camera_info, "right_camera_info"); + m_configuration_state_table.AddData(m_camera_rotation, "camera_rotation"); + m_configuration_state_table.AddData(m_camera_translation, "camera_translation"); + StateTable.AddData(m_measured_cp_array, "measured_cp_array"); + StateTable.AddData(m_left_image_raw, "left_image"); + StateTable.AddData(m_right_image_raw, "right_image"); m_controller_interface = AddInterfaceProvided("Controller"); if (m_controller_interface) { @@ -297,14 +482,17 @@ void mtsAtracsysFusionTrack::Init(void) "crtk_interfaces_provided"); m_controller_interface->AddEventVoid(m_crtk_interfaces_provided_updated, "crtk_interfaces_provided_updated"); } -} -#if 0 -static void OptionEnumerator(uint64 sn, void *CMN_UNUSED(user), ftkOptionsInfo *oi) -{ - CMN_LOG_INIT_VERBOSE << "Option " << oi->id << " " << oi->name << std::endl; + m_stereo_raw_interface = AddInterfaceProvided("StereoRaw"); + if (m_stereo_raw_interface) { + m_stereo_raw_interface->AddCommandReadState(StateTable, m_left_image_raw, "left/image_raw"); + m_stereo_raw_interface->AddCommandReadState(StateTable, m_right_image_raw, "right/image_raw"); + m_stereo_raw_interface->AddCommandReadState(m_configuration_state_table, m_left_camera_info, "left/camera_info"); + m_stereo_raw_interface->AddCommandReadState(m_configuration_state_table, m_right_camera_info, "right/camera_info"); + m_stereo_raw_interface->AddCommandReadState(m_configuration_state_table, m_camera_rotation, "camera_rotation"); + m_stereo_raw_interface->AddCommandReadState(m_configuration_state_table, m_camera_translation, "camera_translation"); + } } -#endif void mtsAtracsysFusionTrack::Configure(const std::string & filename) { @@ -315,9 +503,6 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) CMN_LOG_CLASS_INIT_VERBOSE << "Atracsys SDK Version " << buffer.data << std::endl; #if 0 - if (ftkEnumerateOptions(m_internals->m_library, 0LL, OptionEnumerator, NULL) != SDK_FTK_OK) - CMN_LOG_CLASS_INIT_WARNING << "Configure: failed to enumerate Atracsys options" << std::endl; - if (ftkGetData(m_internals->m_library, 0LL, FTK_OPT_DRIVER_VER, &buffer) != SDK_FTK_OK) CMN_LOG_CLASS_INIT_WARNING << "Configure: failed to get Atracsys Driver version" << std::endl; #endif @@ -345,6 +530,7 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) break; } } + if (m_internals->m_sn == 0LL) { CMN_LOG_CLASS_INIT_ERROR << "Configure: no device connected (" << this->GetName() << ")" << std::endl; @@ -455,7 +641,7 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) << toolName << " and configuration file: " << filename << ", reference: " << reference << std::endl; if (!AddTool(toolName, fullname, isJson, reference)) { - exit(EXIT_FAILURE); + exit(EXIT_FAILURE); } } else { CMN_LOG_CLASS_INIT_ERROR << "Configure: configuration file \"" << toolFile @@ -466,6 +652,22 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) } } + // if stereo is configured, configure Atracsys to send images + bool has_stereo_config = jsonConfig.isMember("stereo"); + if (has_stereo_config) { + std::string device_type = m_internals->QueryDeviceType(); + if (device_type == "") { + CMN_LOG_CLASS_INIT_ERROR << "failed to read device type" << std::endl; + } else { + CMN_LOG_CLASS_INIT_VERBOSE << "device type: " << device_type << std::endl; + } + + ftkError status = m_internals->ConfigureImageSending(true); + if (status != ftkError::FTK_OK) { + CMN_LOG_CLASS_INIT_ERROR << "failed to enabled image sending: " << static_cast(status) << std::endl; + } + } + // finally, prepare frame query CMN_LOG_CLASS_INIT_VERBOSE << "Configure: calling SetupFrameQuery for " << m_tools.size() << " tool(s) and up to " << m_stray_markers_max << " stray marker(s)" << std::endl; @@ -476,17 +678,24 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) void mtsAtracsysFusionTrack::Startup(void) { - CMN_LOG_CLASS_RUN_ERROR << "Startup" << std::endl; // trigger event so connected component can bridge crtk provided interface as needed m_configuration_state_table.Start(); m_crtk_interfaces_provided.push_back(mtsDescriptionInterfaceFullName("localhost", this->Name, "Controller")); - m_configuration_state_table.Advance(); m_crtk_interfaces_provided_updated(); // reports system found m_controller_interface->SendStatus(this->GetName() + ": found device SN " + std::to_string(m_internals->m_sn)); // set reference frame for measured_cp_array m_measured_cp_array.ReferenceFrame() = this->Name; + + bool ok = m_internals->RetrieveStereoCameraCalibration( + m_left_camera_info, m_right_camera_info, + m_camera_rotation, m_camera_translation + ); + m_left_camera_info.Valid() = ok; + m_right_camera_info.Valid() = ok; + + m_configuration_state_table.Advance(); } @@ -495,136 +704,48 @@ void mtsAtracsysFusionTrack::Run(void) // process mts commands ProcessQueuedCommands(); + if (m_internals->m_library == 0LL) { + return; + } + // get latest frame from fusion track library/device ftkError status = ftkGetLastFrame(m_internals->m_library, m_internals->m_sn, m_internals->m_frame_query, 100u); + // negative error codes are warnings m_measured_cp_array.SetValid(true); if (status != FTK_ERROR_NS::FTK_OK) { if (static_cast(status) < 0) { - // std::cerr << "Warning: " << status << std::endl; + CMN_LOG_CLASS_RUN_WARNING << "Warning: " << static_cast(status) << std::endl; } else { m_measured_cp_array.SetValid(false); - std::cerr << "Error: " << static_cast(status) << std::endl; + CMN_LOG_CLASS_RUN_ERROR << "Error: " << static_cast(status) << std::endl; return; } } - // check results of last frame - switch (m_internals->m_frame_query->markersStat) { - case FTK_QS_NS::QS_WAR_SKIPPED: - // CMN_LOG_CLASS_RUN_ERROR << "Run: marker fields in the frame are not set correctly" << std::endl; - break; - case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: - // CMN_LOG_CLASS_RUN_ERROR << "Run: frame.markersVersionSize is invalid" << std::endl; - break; - default: - // CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; - break; - case FTK_QS_NS::QS_OK: - break; - } - - // make sure we're not getting more markers than allocated - size_t count = m_internals->m_frame_query->markersCount; - if (count > m_tools.size()) { - CMN_LOG_CLASS_RUN_WARNING << "Run: marker overflow, please increase number of markers. Only the first " - << m_tools.size() << " marker(s) will processed." << std::endl; - count = m_tools.size(); - } - - // initialize all tools - const ToolsType::iterator end = m_tools.end(); - ToolsType::iterator iter; - for (iter = m_tools.begin(); iter != end; ++iter) { - iter->second->m_state_table.Start(); - iter->second->m_local_measured_cp.SetValid(false); - iter->second->m_measured_cp.SetValid(false); - } - - // for each marker, get the data and populate corresponding tool - for (size_t index = 0; index < count; ++index) { - ftkMarker & ft_tool = m_internals->m_tools[index]; - - // find the appropriate tool - uint32 id = ft_tool.geometryId; - if (m_internals->GeometryIdToTool.find(id) == m_internals->GeometryIdToTool.end()) { - CMN_LOG_CLASS_RUN_WARNING << "Run: found a geometry Id (" - << id << ") not registered using AddTool, this marker will be ignored (" - << this->GetName() << ")" << std::endl; - } - else { - mtsAtracsysFusionTrackTool * tool = m_internals->GeometryIdToTool.at(id); - tool->m_local_measured_cp.SetValid(true); - tool->m_local_measured_cp.Position().Translation().Assign(ft_tool.translationMM[0] * cmn_mm, - ft_tool.translationMM[1] * cmn_mm, - ft_tool.translationMM[2] * cmn_mm); - for (size_t row = 0; row < 3; ++row) { - for (size_t col = 0; col < 3; ++col) { - tool->m_local_measured_cp.Position().Rotation().Element(row, col) = ft_tool.rotation[row][col]; - } - } - tool->m_registration_error = ft_tool.registrationErrorMM * cmn_mm; - } - } - - // finalize all tools - for (iter = m_tools.begin(); iter != end; ++iter) { - auto reference = iter->second->m_reference_measured_cp; - if (reference == nullptr) { - iter->second->m_measured_cp = iter->second->m_local_measured_cp; - } else { - // valid if both valid - iter->second->m_measured_cp.Valid() - = iter->second->m_local_measured_cp.Valid() && reference->Valid(); - // use reference pose - iter->second->m_measured_cp.Position() - = reference->Position().Inverse() * iter->second->m_local_measured_cp.Position(); - } - iter->second->m_state_table.Advance(); - } - - // ---- 3D Fiducials, aka stray markers --- - switch (m_internals->m_frame_query->threeDFiducialsStat) { - case FTK_QS_NS::QS_WAR_SKIPPED: - CMN_LOG_CLASS_RUN_ERROR << "Run: 3D status fields in the frame is not set correctly" << std::endl; - break; - case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: - CMN_LOG_CLASS_RUN_ERROR << "Run: frame.threeDFiducialsVersionSize is invalid" << std::endl; - break; - default: - // CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; - break; - case FTK_QS_NS::QS_OK: - break; - } - - size_t stray_count = m_internals->m_frame_query->threeDFiducialsCount; - m_measured_cp_array.Positions().resize(stray_count); - - //printf("3D fiducials:\n"); - for (uint32 m = 0; m < stray_count; m++) { -#if 0 - printf("\tINDEXES (%u %u)\t XYZ (%.2f %.2f %.2f)\n\t\tEPI_ERR: %.2f\tTRI_ERR: %.2f\tPROB: %.2f\n", - m_internals->m_stray_markers[m].leftIndex, - m_internals->m_stray_markers[m].rightIndex, - m_internals->m_stray_markers[m].positionMM.x, - m_internals->m_stray_markers[m].positionMM.y, - m_internals->m_stray_markers[m].positionMM.z, - m_internals->m_stray_markers[m].epipolarErrorPixels, - m_internals->m_stray_markers[m].triangulationErrorMM, - m_internals->m_stray_markers[m].probability); -#endif - m_measured_cp_array.Positions().at(m).Translation().Assign(m_internals->m_stray_markers[m].positionMM.x * cmn_mm, - m_internals->m_stray_markers[m].positionMM.y * cmn_mm, - m_internals->m_stray_markers[m].positionMM.z * cmn_mm); + ftkPixelFormat frame_format = m_internals->m_frame_query->imageHeader->format; + switch (frame_format) + { + case ftkPixelFormat::GRAY8: + case ftkPixelFormat::GRAY16: + ProcessIRTrackingFrame(); + break; + case ftkPixelFormat::GRAY8_VIS: + case ftkPixelFormat::GRAY16_VIS: + case ftkPixelFormat::GRAY8_SL: + case ftkPixelFormat::GRAY16_SL: + ProcessRGBStereoFrame(); + break; + default: + CMN_LOG_CLASS_RUN_WARNING << "Warning: unknown frame format: " << static_cast(frame_format) << std::endl; + break; } - // maybe this helps with error 13? - // Sleep(50.0 * cmn_ms); // no needed with 4.4.1? } + void mtsAtracsysFusionTrack::Cleanup(void) { ftkClose(&m_internals->m_library); @@ -766,3 +887,116 @@ std::string mtsAtracsysFusionTrack::GetToolName(const size_t index) const } return toolIterator->first; } + +void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() { + // check results of last frame + switch (m_internals->m_frame_query->markersStat) { + case FTK_QS_NS::QS_WAR_SKIPPED: + CMN_LOG_CLASS_RUN_ERROR << "Run: marker fields in the frame are not set correctly" << std::endl; + break; + case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: + CMN_LOG_CLASS_RUN_ERROR << "Run: frame.markersVersionSize is invalid" << std::endl; + break; + default: + CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; + break; + case FTK_QS_NS::QS_OK: + break; + } + + // make sure we're not getting more markers than allocated + size_t count = m_internals->m_frame_query->markersCount; + if (count > m_tools.size()) { + CMN_LOG_CLASS_RUN_WARNING << "Run: marker overflow, please increase number of markers. Only the first " + << m_tools.size() << " marker(s) will processed." << std::endl; + count = m_tools.size(); + } + + // initialize all tools + const ToolsType::iterator end = m_tools.end(); + ToolsType::iterator iter; + for (iter = m_tools.begin(); iter != end; ++iter) { + iter->second->m_state_table.Start(); + iter->second->m_local_measured_cp.SetValid(false); + iter->second->m_measured_cp.SetValid(false); + } + + // for each marker, get the data and populate corresponding tool + for (size_t index = 0; index < count; ++index) { + ftkMarker & ft_tool = m_internals->m_tools[index]; + + // find the appropriate tool + uint32 id = ft_tool.geometryId; + if (m_internals->GeometryIdToTool.find(id) == m_internals->GeometryIdToTool.end()) { + CMN_LOG_CLASS_RUN_WARNING << "Run: found a geometry Id (" + << id << ") not registered using AddTool, this marker will be ignored (" + << this->GetName() << ")" << std::endl; + } + else { + mtsAtracsysFusionTrackTool * tool = m_internals->GeometryIdToTool.at(id); + tool->m_local_measured_cp.SetValid(true); + tool->m_local_measured_cp.Position().Translation().Assign(ft_tool.translationMM[0] * cmn_mm, + ft_tool.translationMM[1] * cmn_mm, + ft_tool.translationMM[2] * cmn_mm); + for (size_t row = 0; row < 3; ++row) { + for (size_t col = 0; col < 3; ++col) { + tool->m_local_measured_cp.Position().Rotation().Element(row, col) = ft_tool.rotation[row][col]; + } + } + tool->m_registration_error = ft_tool.registrationErrorMM * cmn_mm; + } + } + + // finalize all tools + for (iter = m_tools.begin(); iter != end; ++iter) { + auto reference = iter->second->m_reference_measured_cp; + if (reference == nullptr) { + iter->second->m_measured_cp = iter->second->m_local_measured_cp; + } else { + // valid if both valid + iter->second->m_measured_cp.Valid() + = iter->second->m_local_measured_cp.Valid() && reference->Valid(); + // use reference pose + iter->second->m_measured_cp.Position() + = reference->Position().Inverse() * iter->second->m_local_measured_cp.Position(); + } + iter->second->m_state_table.Advance(); + } + + // ---- 3D Fiducials, aka stray markers --- + switch (m_internals->m_frame_query->threeDFiducialsStat) { + case FTK_QS_NS::QS_WAR_SKIPPED: + //CMN_LOG_CLASS_RUN_ERROR << "Run: 3D status fields in the frame is not set correctly" << std::endl; + break; + case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: + CMN_LOG_CLASS_RUN_ERROR << "Run: frame.threeDFiducialsVersionSize is invalid" << std::endl; + break; + default: + // CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; + break; + case FTK_QS_NS::QS_OK: + break; + } + + size_t stray_count = m_internals->m_frame_query->threeDFiducialsCount; + m_measured_cp_array.Positions().resize(stray_count); + + for (uint32 m = 0; m < stray_count; m++) { + m_measured_cp_array.Positions().at(m).Translation().Assign(m_internals->m_stray_markers[m].positionMM.x * cmn_mm, + m_internals->m_stray_markers[m].positionMM.y * cmn_mm, + m_internals->m_stray_markers[m].positionMM.z * cmn_mm); + } +} + + +void mtsAtracsysFusionTrack::ProcessRGBStereoFrame() { + if (!m_internals->m_images_enabled) { + return; + } + + // Image frames + m_internals->ExtractLeftImage(m_left_image_raw); + m_internals->ExtractRightImage(m_right_image_raw); + m_left_image_raw.Valid() = true; + m_right_image_raw.Valid() = true; +} diff --git a/core/components/code/mtsAtracsysStereo.cpp b/core/components/code/mtsAtracsysStereo.cpp new file mode 100644 index 0000000..44fa91f --- /dev/null +++ b/core/components/code/mtsAtracsysStereo.cpp @@ -0,0 +1,296 @@ +/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ +/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ + +/* + Author(s): Anton Deguet + Created on: 2014-07-17 + + (C) Copyright 2014-2024 Johns Hopkins University (JHU), All Rights Reserved. + +--- begin cisst license - do not edit --- + +This software is provided "as is" under an open source license, with +no warranty. The complete license can be found in license.txt and +http://www.cisst.org/cisst/license.txt. + +--- end cisst license --- +*/ + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + + +CMN_IMPLEMENT_SERVICES_DERIVED_ONEARG(mtsAtracsysStereo, mtsTaskContinuous, mtsTaskContinuousConstructorArg); + +void mtsAtracsysStereo::Init(void) +{ + StateTable.AddData(m_left_image, "left_image"); + StateTable.AddData(m_right_image, "right_image"); + StateTable.AddData(m_left_camera_info, "left_camera_info"); + StateTable.AddData(m_right_camera_info, "right_camera_info"); + StateTable.AddData(m_depth, "depth"); + + m_stereo_interface = AddInterfaceProvided("stereo"); + if (m_stereo_interface) { + m_stereo_interface->AddCommandReadState(StateTable, m_left_image, "left/image_rect_color"); + m_stereo_interface->AddCommandReadState(StateTable, m_left_camera_info, "left/camera_info"); + m_stereo_interface->AddCommandReadState(StateTable, m_right_image, "right/image_rect_color"); + m_stereo_interface->AddCommandReadState(StateTable, m_right_camera_info, "right/camera_info"); + m_stereo_interface->AddCommandReadState(StateTable, m_depth, "depth"); + + m_stereo_interface->AddCommandReadState(StateTable, StateTable.PeriodStats, "period_statistics"); + } + + m_stereo_raw_interface = AddInterfaceRequired("StereoRaw"); + if (m_stereo_raw_interface) { + m_stereo_raw_interface->AddFunction("left/image_raw", m_get_left_image_raw); + m_stereo_raw_interface->AddFunction("right/image_raw", m_get_right_image_raw); + m_stereo_raw_interface->AddFunction("left/camera_info", m_get_left_camera_info); + m_stereo_raw_interface->AddFunction("right/camera_info", m_get_right_camera_info); + m_stereo_raw_interface->AddFunction("camera_rotation", m_get_camera_rotation); + m_stereo_raw_interface->AddFunction("camera_translation", m_get_camera_translation); + } + + m_pipline_initialized = false; + m_video_enabled = false; + m_depth_enabled = false; + + m_local_block_matching = false; + m_filter_depth_map = false; +} + +void mtsAtracsysStereo::InitStereoPipeline() +{ + m_pipline_initialized = false; + + mtsExecutionResult result; + + result = m_get_left_camera_info(m_left_camera_info); + if (!result.IsOK() || !m_left_camera_info.Valid()) { + return; + } + + result = m_get_right_camera_info(m_right_camera_info); + if (!result.IsOK() || !m_right_camera_info.Valid()) { + return; + } + + vct3 rotation, translation; + + result = m_get_camera_rotation(rotation); + if (!result.IsOK()) { + return; + } + + result = m_get_camera_translation(translation); + if (!result.IsOK()) { + return; + } + + cv::Mat left_intrinsic = cv::Mat(3, 3, CV_64F, m_left_camera_info.Intrinsic().Pointer()); + cv::Mat right_intrinsic = cv::Mat(3, 3, CV_64F, m_right_camera_info.Intrinsic().Pointer()); + cv::Mat left_distortion = cv::Mat(5, 1, CV_64F, m_left_camera_info.DistortionParameters().Pointer()); + cv::Mat right_distortion = cv::Mat(5, 1, CV_64F, m_right_camera_info.DistortionParameters().Pointer()); + + cv::Size original_image_size = cv::Size(m_left_camera_info.Width(), m_left_camera_info.Height()); + + cv::Mat R = cv::Mat(3, 1, CV_64F, rotation.Pointer()); + cv::Mat T = cv::Mat(3, 1, CV_64F, translation.Pointer()); + + // stereo rectification output matrices + cv::Mat left_rect, right_rect; + cv::Mat left_proj, right_proj; + + cv::stereoRectify( + left_intrinsic, left_distortion, + right_intrinsic, right_distortion, + original_image_size, R, T, + left_rect, right_rect, left_proj, right_proj, + disparity_to_depth + ); + + cv::initUndistortRectifyMap( + left_intrinsic, left_distortion, + left_rect, left_proj, + original_image_size, + CV_32FC(1), + m_left_undistort_map_x, m_left_undistort_map_y + ); + + cv::initUndistortRectifyMap( + right_intrinsic, right_distortion, + right_rect, right_proj, + original_image_size, + CV_32FC(1), + m_right_undistort_map_x, m_right_undistort_map_y + ); + + std::copy(left_rect.begin(), left_rect.end(), m_left_camera_info.Rectification().begin()); + std::copy(right_rect.begin(), right_rect.end(), m_right_camera_info.Rectification().begin()); + std::copy(left_proj.begin(), left_proj.end(), m_left_camera_info.Projection().begin()); + std::copy(right_proj.begin(), right_proj.end(), m_right_camera_info.Projection().begin()); + + if (m_local_block_matching) { + m_left_stereo_matcher = cv::StereoBM::create(384, 7); + } else { + m_left_stereo_matcher = cv::StereoSGBM::create(0, 384, 7); + } + + m_right_stereo_matcher = cv::ximgproc::createRightMatcher(m_left_stereo_matcher); + m_wls_filter = cv::ximgproc::createDisparityWLSFilter(m_left_stereo_matcher); + m_wls_filter->setLambda(8000.0); + m_wls_filter->setSigmaColor(1.0); + + m_pipline_initialized = true; +} + +void mtsAtracsysStereo::Rectify(prmImageFrame& raw, cv::Mat& rectified_mat, prmImageFrame& rectified, const cv::Mat& undistort_x, const cv::Mat& undistort_y) +{ + cv::Mat raw_bayer = cv::Mat(raw.Height(), raw.Width(), CV_8U, raw.Data().Pointer()); + cv::Mat raw_rgb; + cv::cvtColor(raw_bayer, raw_rgb, cv::COLOR_BayerGR2BGR_EA); + + cv::remap(raw_rgb, rectified_mat, undistort_x, undistort_y, cv::INTER_LINEAR); + + rectified.Width() = rectified_mat.cols; + rectified.Height() = rectified_mat.rows; + rectified.Channels() = rectified_mat.channels(); + + size_t size = rectified_mat.cols * rectified_mat.rows * rectified_mat.channels(); + rectified.Data().resize(size); + std::copy(rectified_mat.data, rectified_mat.data + size, rectified.Data().begin()); +} + +void mtsAtracsysStereo::ComputeDepth() +{ + cv::Mat left_gray, right_gray; + cv::cvtColor(m_left_image_mat, left_gray, cv::COLOR_RGB2GRAY); + cv::cvtColor(m_right_image_mat, right_gray, cv::COLOR_RGB2GRAY); + + cv::Mat left_disparity, right_disparity; + m_left_stereo_matcher->compute(left_gray, right_gray, left_disparity); + + cv::Mat disparity; + if (m_filter_depth_map) { + m_right_stereo_matcher->compute(right_gray, left_gray, right_disparity); + m_wls_filter->filter(left_disparity, m_left_image_mat, disparity, right_disparity, cv::Rect(), m_right_image_mat); + } else { + disparity = left_disparity; + } + + disparity.convertTo(disparity, CV_32F, 1.0f/16.0, 0.0f); + + cv::Mat depth; + cv::reprojectImageTo3D(disparity, depth, disparity_to_depth, true, -1); + + m_depth.Width() = depth.cols; + m_depth.Height() = depth.rows; + + size_t size = depth.cols * depth.rows * depth.channels(); + + m_depth.Points().resize(size); + std::copy(reinterpret_cast(depth.data), reinterpret_cast(depth.data) + size, m_depth.Points().begin()); + + m_depth.Color().resize(m_left_image_mat.cols * m_left_image_mat.rows * m_left_image_mat.channels()); + std::copy(m_left_image_mat.data, m_left_image_mat.data + size, m_depth.Color().begin()); +} + +void mtsAtracsysStereo::Configure(const std::string & filename) +{ + CMN_LOG_CLASS_INIT_VERBOSE << "Configure: using " << filename << std::endl; + + cmnPath m_config_path; + m_config_path.Add(cmnPath::GetWorkingDirectory()); + m_config_path.Add(std::string(sawAtracsysFusionTrack_SOURCE_DIR) + "/../share", cmnPath::TAIL); + + std::string config_fullname = m_config_path.Find(filename); + if (!cmnPath::Exists(config_fullname)) { + CMN_LOG_CLASS_INIT_ERROR << "Configure: configuration file \"" << filename + << "\" not found in path (" << m_config_path << ")" << std::endl; + exit(EXIT_FAILURE); + } + + // read JSON file passed as param, see configAtracsysFusionTrack.json for an example + std::ifstream jsonStream; + jsonStream.open(config_fullname.c_str()); + + Json::Value jsonConfig; + Json::Reader jsonReader; + if (!jsonReader.parse(jsonStream, jsonConfig)) { + CMN_LOG_CLASS_INIT_ERROR << "Configure: failed to parse configuration" << std::endl + << jsonReader.getFormattedErrorMessages(); + exit(EXIT_FAILURE); + return; + } + + if (jsonConfig.isMember("stereo")) { + const Json::Value stereo = jsonConfig["stereo"]; + const Json::Value video = stereo["video"]; + m_video_enabled = stereo.isMember("video") && video.isBool() && video.asBool(); + const Json::Value depth = stereo["depth"]; + m_depth_enabled = stereo.isMember("depth") && depth.isBool() && depth.asBool(); + // video processing is required for depth map computation + m_video_enabled = m_depth_enabled || m_video_enabled; + + const Json::Value local_bm = stereo["local_block_matching"]; + m_local_block_matching = stereo.isMember("local_block_matching") && local_bm.isBool() && local_bm.asBool(); + + const Json::Value filter_depth = stereo["filter_depth_map"]; + m_filter_depth_map = stereo.isMember("filter_depth_map") && filter_depth.isBool() && filter_depth.asBool(); + } +} + +void mtsAtracsysStereo::Startup(void) { } + +void mtsAtracsysStereo::Run(void) +{ + // process mts commands + ProcessQueuedCommands(); + + if (!m_depth_enabled && !m_video_enabled) { + return; + } + + if (!m_pipline_initialized) { + InitStereoPipeline(); + return; + } + + mtsExecutionResult result; + prmImageFrame left_raw, right_raw; + + result = m_get_left_image_raw(left_raw); + if (!result.IsOK() || !left_raw.Valid()) { + return; + } + + result = m_get_right_image_raw(right_raw); + if (!result.IsOK() || !right_raw.Valid()) { + return; + } + + Rectify(left_raw, m_left_image_mat, m_left_image, m_left_undistort_map_x, m_left_undistort_map_y); + Rectify(right_raw, m_right_image_mat, m_right_image, m_right_undistort_map_x, m_right_undistort_map_y); + m_left_image.Valid() = true; + m_right_image.Valid() = true; + + if (!m_depth_enabled) { + return; + } + + ComputeDepth(); + m_depth.Valid() = true; + m_depth.ReferenceFrame() = m_reference_frame; +} + +void mtsAtracsysStereo::Cleanup() {} diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h index 1ff7e72..cd79167 100644 --- a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h @@ -22,6 +22,8 @@ no warranty. The complete license can be found in license.txt and #include #include #include +#include +#include #include // in order to read config file @@ -102,6 +104,9 @@ class CISST_EXPORT mtsAtracsysFusionTrack: public mtsTaskContinuous /*! Code called by all constructors. */ void Init(void); + void ProcessIRTrackingFrame(); + void ProcessRGBStereoFrame(); + /*! Search path for configuration files */ cmnPath m_path; @@ -110,6 +115,8 @@ class CISST_EXPORT mtsAtracsysFusionTrack: public mtsTaskContinuous /*! Controller interface */ mtsInterfaceProvided * m_controller_interface = nullptr; + + mtsInterfaceProvided * m_stereo_raw_interface = nullptr; mtsAtracsysFusionTrackInternals * m_internals = nullptr; typedef std::map ToolsType; @@ -120,9 +127,15 @@ class CISST_EXPORT mtsAtracsysFusionTrack: public mtsTaskContinuous prmPositionCartesianArrayGet m_measured_cp_array; + prmCameraInfo m_left_camera_info, m_right_camera_info; + vct3 m_camera_rotation, m_camera_translation; + prmImageFrame m_left_image_raw, m_right_image_raw; + int m_num_enabled_dot_projectors = 0; + /*! CRTK related methods */ mtsFunctionVoid m_crtk_interfaces_provided_updated; std::vector m_crtk_interfaces_provided; + }; CMN_DECLARE_SERVICES_INSTANTIATION(mtsAtracsysFusionTrack); diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h new file mode 100644 index 0000000..3e93a0b --- /dev/null +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h @@ -0,0 +1,108 @@ +/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */ +/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */ + +/* + Author(s): Brendan Burkhart + Created on: 2024-07-01 + + (C) Copyright 2024 Johns Hopkins University (JHU), All Rights Reserved. + +--- begin cisst license - do not edit --- + +This software is provided "as is" under an open source license, with +no warranty. The complete license can be found in license.txt and +http://www.cisst.org/cisst/license.txt. + +--- end cisst license --- +*/ + +#ifndef _mtsAtracsysStereo_h +#define _mtsAtracsysStereo_h + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include // always include last + +class CISST_EXPORT mtsAtracsysStereo: public mtsTaskContinuous +{ + CMN_DECLARE_SERVICES(CMN_DYNAMIC_CREATION_ONEARG, CMN_LOG_ALLOW_DEFAULT); + + public: + inline mtsAtracsysStereo(const std::string & componentName, std::string reference_frame): + mtsTaskContinuous(componentName, 100), + m_reference_frame(reference_frame) { + Init(); + } + + inline mtsAtracsysStereo(const mtsTaskContinuousConstructorArg & arg): + mtsTaskContinuous(arg), + m_reference_frame(GetName()) { + Init(); + } + + ~mtsAtracsysStereo(void) {}; + + void Configure(const std::string & filename = "") override; + + void Startup(void) override; + + void Run(void) override; + + void Cleanup(void) override; + +protected: + void Init(void); // shared initialization by all constructors + void InitStereoPipeline(); // one-time pipeline resource initialization + void Rectify(prmImageFrame& raw, cv::Mat& rectified_mat, prmImageFrame& rectified, const cv::Mat& undistort_x, const cv::Mat& undistort_y); + void ComputeDepth(); + + std::string m_reference_frame; + + mtsInterfaceProvided * m_stereo_interface = nullptr; + mtsInterfaceRequired * m_stereo_raw_interface = nullptr; + + prmCameraInfo m_left_camera_info; + prmImageFrame m_left_image; + prmCameraInfo m_right_camera_info; + prmImageFrame m_right_image; + prmDepthMap m_depth; + + mtsFunctionRead m_get_left_image_raw; + mtsFunctionRead m_get_right_image_raw; + mtsFunctionRead m_get_left_camera_info; + mtsFunctionRead m_get_right_camera_info; + mtsFunctionRead m_get_camera_rotation; + mtsFunctionRead m_get_camera_translation; + + cv::Mat m_left_image_mat, m_right_image_mat; + + bool m_pipline_initialized; + bool m_video_enabled; + bool m_depth_enabled; + + bool m_local_block_matching; + bool m_filter_depth_map; + + cv::Mat m_left_undistort_map_x, m_left_undistort_map_y; + cv::Mat m_right_undistort_map_x, m_right_undistort_map_y; + cv::Mat disparity_to_depth; + + cv::Ptr m_left_stereo_matcher, m_right_stereo_matcher; + cv::Ptr m_wls_filter; +}; + +CMN_DECLARE_SERVICES_INSTANTIATION(mtsAtracsysStereo); + +#endif // _mtsAtracsysStereo_h diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 89f13e8..3609b38 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -70,6 +70,8 @@ if (cisst_FOUND_AS_REQUIRED) # sawAtracsysFusionTrack has been compiled within cisst, we should find it automatically find_package (sawAtracsysFusionTrack 1.1.0) + find_package(OpenCV REQUIRED) + if (sawAtracsysFusionTrack_FOUND) # sawAtracsysFusionTrack configuration @@ -79,7 +81,8 @@ if (cisst_FOUND_AS_REQUIRED) add_executable (atracsys src/atracsys.cpp src/mts_ros_crtk_atracsys_bridge.cpp src/mts_ros_crtk_atracsys_bridge.h) target_link_libraries (atracsys ${sawAtracsysFusionTrack_LIBRARIES} - ${catkin_LIBRARIES}) + ${catkin_LIBRARIES} + ${OpenCV_LIBS}) cisst_target_link_libraries (atracsys ${REQUIRED_CISST_LIBRARIES}) install (TARGETS atracsys diff --git a/ros/src/atracsys.cpp b/ros/src/atracsys.cpp index 5ebc293..6e9582e 100644 --- a/ros/src/atracsys.cpp +++ b/ros/src/atracsys.cpp @@ -23,6 +23,7 @@ no warranty. The complete license can be found in license.txt and #include #include #include +#include #include #include "mts_ros_crtk_atracsys_bridge.h" @@ -38,6 +39,7 @@ int main(int argc, char * argv[]) cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL); cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL); cmnLogger::SetMaskClassMatching("mtsAtracsysFusionTrack", CMN_LOG_ALLOW_ALL); + cmnLogger::SetMaskClassMatching("mtsAtracsysStereo", CMN_LOG_ALLOW_ALL); cmnLogger::AddChannel(std::cerr, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS); // create ROS node handle @@ -79,18 +81,28 @@ int main(int argc, char * argv[]) mtsAtracsysFusionTrack * tracker = new mtsAtracsysFusionTrack("atracsys"); tracker->Configure(jsonConfigFile); + mtsAtracsysStereo * stereo = new mtsAtracsysStereo("stereo", tracker->GetName()); + stereo->Configure(jsonConfigFile); + // add the components to the component manager mtsManagerLocal * componentManager = mtsComponentManager::GetInstance(); componentManager->AddComponent(tracker); + componentManager->AddComponent(stereo); + componentManager->Connect(tracker->GetName(), "StereoRaw", + stereo->GetName(), "StereoRaw"); // ROS CRTK bridge mts_ros_crtk_atracsys_bridge * crtk_bridge = new mts_ros_crtk_atracsys_bridge("atracsys_crtk_bridge", rosNode); - crtk_bridge->add_factory_source("atracsys", "Controller", rosPeriod, tfPeriod); + + crtk_bridge->bridge(tracker->GetName(), "Controller", rosPeriod, tfPeriod); + std::string stereo_namespace = tracker->GetName() + "/stereo"; + cisst_ral::clean_namespace(stereo_namespace); + crtk_bridge->bridge_interface_provided(stereo->GetName(), "stereo", stereo_namespace, rosPeriod, tfPeriod); auto num_tools = tracker->GetNumberOfTools(); for (size_t i = 0; i < num_tools; ++i) { - crtk_bridge->bridge_tool_error("atracsys", tracker->GetToolName(i)); + crtk_bridge->bridge_tool_error(tracker->GetName(), tracker->GetToolName(i)); } componentManager->AddComponent(crtk_bridge); @@ -119,7 +131,7 @@ int main(int argc, char * argv[]) prmPositionCartesianGetQtWidgetFactory * positionQtWidgetFactory = new prmPositionCartesianGetQtWidgetFactory("positionQtWidgetFactory"); positionQtWidgetFactory->SetPrismaticRevoluteFactors(1.0 / cmn_mm, cmn180_PI); // to display values in mm and degrees - positionQtWidgetFactory->AddFactorySource("atracsys", "Controller"); + positionQtWidgetFactory->AddFactorySource(tracker->GetName(), "Controller"); componentManager->AddComponent(positionQtWidgetFactory); positionQtWidgetFactory->Connect(); tabWidget->addTab(positionQtWidgetFactory, "Tools"); diff --git a/ros/src/mts_ros_crtk_atracsys_bridge.cpp b/ros/src/mts_ros_crtk_atracsys_bridge.cpp index 52b9c68..7596ecb 100644 --- a/ros/src/mts_ros_crtk_atracsys_bridge.cpp +++ b/ros/src/mts_ros_crtk_atracsys_bridge.cpp @@ -52,6 +52,7 @@ void mts_ros_crtk_atracsys_bridge::bridge(const std::string & _component_name, if (!_clean_namespace.empty()) { _clean_namespace.append("/"); } + // required interfaces specific to this component to bridge const std::string _required_interface_name = _component_name + "_using_" + _interface_name; From 20d6e9697bfee22691a8996da1b20b14cd7573ee Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Fri, 12 Jul 2024 14:47:30 -0400 Subject: [PATCH 02/12] Exit if device cannot be initialized --- .../code/mtsAtracsysFusionTrack.cpp | 20 +++++++++++++------ .../mtsAtracsysFusionTrack.h | 2 ++ ros/src/atracsys.cpp | 5 +++++ 3 files changed, 21 insertions(+), 6 deletions(-) diff --git a/core/components/code/mtsAtracsysFusionTrack.cpp b/core/components/code/mtsAtracsysFusionTrack.cpp index ec31d4b..15001ac 100644 --- a/core/components/code/mtsAtracsysFusionTrack.cpp +++ b/core/components/code/mtsAtracsysFusionTrack.cpp @@ -422,6 +422,7 @@ class mtsAtracsysFusionTrackInternals ftkLibrary m_library; uint64 m_sn; + ftkDeviceType m_device_type; ftkFrameQuery * m_frame_query; ftkMarker * m_tools; ftk3DFiducial * m_stray_markers; @@ -443,11 +444,12 @@ class mtsAtracsysFusionTrackInternals const uint32_t option_id_export_calibration = 181; }; -static void mtsAtracsysFusionTrackDeviceEnum(uint64 device, void * user, ftkDeviceType CMN_UNUSED(type)) +static void mtsAtracsysFusionTrackDeviceEnum(uint64 device, void * user, ftkDeviceType type) { - uint64 * lastDevice = reinterpret_cast(user); - if (lastDevice) { - *lastDevice = device; + mtsAtracsysFusionTrackInternals* internals = reinterpret_cast(user); + if (internals) { + internals->m_sn = device; + internals->m_device_type = type; } } @@ -519,7 +521,7 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) // search for devices ftkError error = ftkEnumerateDevices(m_internals->m_library, mtsAtracsysFusionTrackDeviceEnum, - &(m_internals->m_sn)); + m_internals); if (error != FTK_ERROR_NS::FTK_OK) { CMN_LOG_CLASS_INIT_ERROR << "Configure: unable to enumerate devices (" << this->GetName() << ")" << std::endl; @@ -538,8 +540,11 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) return; } + CMN_LOG_CLASS_INIT_VERBOSE << "Configure: found device SN " << m_internals->m_sn << std::endl; + if (filename == "") { m_internals->SetupFrameQuery(m_tools.size(), m_stray_markers_max); + m_internals->m_configured = true; return; } @@ -757,7 +762,6 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, const bool isJson, const std::string & referenceName) { - // check if this tool already exists auto tool_iterator = m_tools.find(toolName); if (tool_iterator != m_tools.end()) { @@ -888,6 +892,10 @@ std::string mtsAtracsysFusionTrack::GetToolName(const size_t index) const return toolIterator->first; } +bool mtsAtracsysFusionTrack::HardwareInitialized() const { + return m_internals->m_configured; +} + void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() { // check results of last frame switch (m_internals->m_frame_query->markersStat) { diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h index cd79167..37d0c8c 100644 --- a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h @@ -99,6 +99,8 @@ class CISST_EXPORT mtsAtracsysFusionTrack: public mtsTaskContinuous std::string GetToolName(const size_t index) const; + bool HardwareInitialized() const; + protected: /*! Code called by all constructors. */ diff --git a/ros/src/atracsys.cpp b/ros/src/atracsys.cpp index 6e9582e..0bde858 100644 --- a/ros/src/atracsys.cpp +++ b/ros/src/atracsys.cpp @@ -81,6 +81,11 @@ int main(int argc, char * argv[]) mtsAtracsysFusionTrack * tracker = new mtsAtracsysFusionTrack("atracsys"); tracker->Configure(jsonConfigFile); + if (!tracker->HardwareInitialized()) { + CMN_LOG_INIT_ERROR << "Failed to initialize Atracsys hardware --- is device connected and powered on?" << std::endl; + return -1; + } + mtsAtracsysStereo * stereo = new mtsAtracsysStereo("stereo", tracker->GetName()); stereo->Configure(jsonConfigFile); From a4a42c9d27281cf343d68b0f3b5e5e87147fa283 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Fri, 12 Jul 2024 14:52:03 -0400 Subject: [PATCH 03/12] Prevent re-bridging tool or controller components The ctrk factory source was causing all components to be re-bridged every update, producing excessive warnings. --- .../code/mtsAtracsysFusionTrack.cpp | 5 +- ros/src/atracsys.cpp | 9 +-- ros/src/mts_ros_crtk_atracsys_bridge.cpp | 61 ++++++++++--------- ros/src/mts_ros_crtk_atracsys_bridge.h | 9 +++ 4 files changed, 43 insertions(+), 41 deletions(-) diff --git a/core/components/code/mtsAtracsysFusionTrack.cpp b/core/components/code/mtsAtracsysFusionTrack.cpp index 15001ac..cb3a028 100644 --- a/core/components/code/mtsAtracsysFusionTrack.cpp +++ b/core/components/code/mtsAtracsysFusionTrack.cpp @@ -683,10 +683,7 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) void mtsAtracsysFusionTrack::Startup(void) { - // trigger event so connected component can bridge crtk provided interface as needed m_configuration_state_table.Start(); - m_crtk_interfaces_provided.push_back(mtsDescriptionInterfaceFullName("localhost", this->Name, "Controller")); - m_crtk_interfaces_provided_updated(); // reports system found m_controller_interface->SendStatus(this->GetName() + ": found device SN " + std::to_string(m_internals->m_sn)); @@ -701,6 +698,8 @@ void mtsAtracsysFusionTrack::Startup(void) m_right_camera_info.Valid() = ok; m_configuration_state_table.Advance(); + + m_crtk_interfaces_provided_updated(); } diff --git a/ros/src/atracsys.cpp b/ros/src/atracsys.cpp index 0bde858..c664652 100644 --- a/ros/src/atracsys.cpp +++ b/ros/src/atracsys.cpp @@ -101,14 +101,7 @@ int main(int argc, char * argv[]) = new mts_ros_crtk_atracsys_bridge("atracsys_crtk_bridge", rosNode); crtk_bridge->bridge(tracker->GetName(), "Controller", rosPeriod, tfPeriod); - std::string stereo_namespace = tracker->GetName() + "/stereo"; - cisst_ral::clean_namespace(stereo_namespace); - crtk_bridge->bridge_interface_provided(stereo->GetName(), "stereo", stereo_namespace, rosPeriod, tfPeriod); - - auto num_tools = tracker->GetNumberOfTools(); - for (size_t i = 0; i < num_tools; ++i) { - crtk_bridge->bridge_tool_error(tracker->GetName(), tracker->GetToolName(i)); - } + crtk_bridge->bridge_interface_provided(stereo->GetName(), "stereo", tracker->GetName() + "/stereo", rosPeriod, tfPeriod); componentManager->AddComponent(crtk_bridge); crtk_bridge->Connect(); diff --git a/ros/src/mts_ros_crtk_atracsys_bridge.cpp b/ros/src/mts_ros_crtk_atracsys_bridge.cpp index 7596ecb..64fe326 100644 --- a/ros/src/mts_ros_crtk_atracsys_bridge.cpp +++ b/ros/src/mts_ros_crtk_atracsys_bridge.cpp @@ -16,11 +16,12 @@ no warranty. The complete license can be found in license.txt and --- end cisst license --- */ -// system include +#include "mts_ros_crtk_atracsys_bridge.h" + +#include #include // cisst -#include "mts_ros_crtk_atracsys_bridge.h" #include CMN_IMPLEMENT_SERVICES(mts_ros_crtk_atracsys_bridge); @@ -35,51 +36,51 @@ void mts_ros_crtk_atracsys_bridge::bridge(const std::string & _component_name, cisst_ral::clean_namespace(_clean_namespace); // create factory to bridge tool as they get created - this->add_factory_source(_component_name, + add_factory_source(_component_name, _interface_name, _publish_period_in_seconds, _tf_period_in_seconds); // controller specific topics, some might be CRTK compliant - this->bridge_interface_provided(_component_name, + bridge_interface_provided(_component_name, _interface_name, _clean_namespace, _publish_period_in_seconds, _tf_period_in_seconds); +} - // non CRTK topics - // add trailing / for clean namespace - if (!_clean_namespace.empty()) { - _clean_namespace.append("/"); - } - - // required interfaces specific to this component to bridge - const std::string _required_interface_name = _component_name + "_using_" + _interface_name; - - // connections - m_connections.Add(m_subscribers_bridge->GetName(), _required_interface_name, - _component_name, _interface_name); - m_connections.Add(m_events_bridge->GetName(), _required_interface_name, - _component_name, _interface_name); +void mts_ros_crtk_atracsys_bridge::bridge_interface_provided(const std::string & _component_name, + const std::string & _interface_name, + const double _publish_period_in_seconds, + const double _tf_period_in_seconds) { + // bridge CRTK topics + mts_ros_crtk_bridge::bridge_interface_provided(_component_name, _interface_name, + _publish_period_in_seconds, _tf_period_in_seconds); + // and also the registration_error command (if present) + bridge_tool_error(_component_name, _interface_name); } void mts_ros_crtk_atracsys_bridge::bridge_tool_error(const std::string & _component_name, const std::string & _interface_name) { - std::string _clean_namespace = _component_name; - cisst_ral::clean_namespace(_clean_namespace); + mtsManagerLocal * _component_manager = mtsComponentManager::GetInstance(); + mtsComponent * _component = _component_manager->GetComponent(_component_name); + if (!_component) { + CMN_LOG_CLASS_RUN_ERROR << "bridge_tool_error: unable to find component \"" + << _component_name << "\"" << std::endl; + return; + } + // then try to find the interface + mtsInterfaceProvided * _interface_provided = _component->GetInterfaceProvided(_interface_name); - // add trailing / for clean namespace - if (!_clean_namespace.empty()) { - _clean_namespace.append("/"); + // check if interface provides tool registration error + const auto& read_commands = _interface_provided->GetNamesOfCommandsRead(); + if (std::find(read_commands.begin(), read_commands.end(), "registration_error") == read_commands.end()) { + return; } - // required interfaces specific to this component to bridge - const std::string _required_interface_name = _component_name + "_using_" + _interface_name; - // non CRTK topics - m_subscribers_bridge->AddPublisherFromCommandRead - (_required_interface_name, "registration_error", _component_name + "/" + _interface_name + "/registration_error"); + std::string ros_topic = _component_name + "/" + _interface_name + "/registration_error"; - m_connections.Add(m_subscribers_bridge->GetName(), _required_interface_name, - _component_name, _interface_name); + m_subscribers_bridge->AddPublisherFromCommandRead + (_interface_name, "registration_error", ros_topic); } diff --git a/ros/src/mts_ros_crtk_atracsys_bridge.h b/ros/src/mts_ros_crtk_atracsys_bridge.h index fc46637..6567666 100644 --- a/ros/src/mts_ros_crtk_atracsys_bridge.h +++ b/ros/src/mts_ros_crtk_atracsys_bridge.h @@ -40,6 +40,15 @@ class mts_ros_crtk_atracsys_bridge: public mts_ros_crtk_bridge const double _publish_period_in_seconds, const double _tf_period_in_seconds); + using mts_ros_crtk_bridge::bridge_interface_provided; // bring all overloads into scope before overriding + + virtual void bridge_interface_provided(const std::string & _component_name, + const std::string & _interface_name, + const double _publish_period_in_seconds + = cisst_ros_crtk::bridge_provided_default_publish_period, + const double _tf_period_in_seconds + = cisst_ros_crtk::bridge_provided_default_tf_period) override; + void bridge_tool_error(const std::string & _component_name, const std::string & _interface_name); }; From eb99d639ad3bd334e5a465d508bd9a69da1e8870 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Fri, 12 Jul 2024 18:20:57 -0400 Subject: [PATCH 04/12] Retrieve device option list during init User manual states option ids associated with each device option name may change between SDK versions, so option list is queried. When attempting to set an option, we now first check that it is supported, and for integer-value options also check whether it is within the valid range for that option. --- .../code/mtsAtracsysFusionTrack.cpp | 333 +++++++++++------- 1 file changed, 215 insertions(+), 118 deletions(-) diff --git a/core/components/code/mtsAtracsysFusionTrack.cpp b/core/components/code/mtsAtracsysFusionTrack.cpp index cb3a028..a311aaa 100644 --- a/core/components/code/mtsAtracsysFusionTrack.cpp +++ b/core/components/code/mtsAtracsysFusionTrack.cpp @@ -223,15 +223,70 @@ class mtsAtracsysFusionTrackInternals { public: mtsAtracsysFusionTrackInternals(): - m_library(0), + m_library(nullptr), m_sn(0), m_frame_query(nullptr), m_configured(false), m_images_enabled(false) {} + ~mtsAtracsysFusionTrackInternals() { + if (m_frame_query != nullptr) { + delete m_frame_query; + } + + if (m_library != nullptr) { + ftkClose(&m_library); + } + } + + bool Initialize() + { + ftkBuffer buffer; + ftkVersion(&buffer); + CMN_LOG_INIT_VERBOSE << "Atracsys SDK Version " << buffer.data << std::endl; + + // initialize SDK + m_library = ftkInit(); + if (m_library == nullptr) { + CMN_LOG_INIT_ERROR << "Configure: unable to initialize Atracsys SDK" << std::endl; + return false; + } + + size_t max_attempts = 10; + for (size_t attempt = 0; attempt < max_attempts; attempt++) { + // search for devices + ftkError error = ftkEnumerateDevices(m_library, DeviceEnumerateCallback, this); + if (error != FTK_ERROR_NS::FTK_OK) { + CMN_LOG_INIT_ERROR << "Configure: unable to enumerate Atracsys devices" << std::endl; + } + + if (m_sn != 0) { + break; + } + } + + if (m_sn == 0) { + CMN_LOG_INIT_ERROR << "Configure: no Atracsys device connected" << std::endl; + return false; + } + + ftkError error = ftkEnumerateOptions(m_library, 0, OptionEnumerateCallback, this); + if (error != FTK_ERROR_NS::FTK_OK && error != ftkError::FTK_WAR_OPT_GLOBAL_ONLY) { + CMN_LOG_INIT_ERROR << "Configure: unable to enumerate supported global library options" << std::endl; + } + + error = ftkEnumerateOptions(m_library, m_sn, OptionEnumerateCallback, this); + if (error != FTK_ERROR_NS::FTK_OK) { + CMN_LOG_INIT_ERROR << "Configure: unable to enumerate supported device options" << std::endl; + } + + return true; + } + void SetupFrameQuery(const size_t number_of_tools, - const size_t number_of_stray_markers) { + const size_t number_of_stray_markers) + { if (m_frame_query) { delete m_frame_query; } @@ -253,13 +308,17 @@ class mtsAtracsysFusionTrackInternals CMN_LOG_INIT_ERROR << "mtsAtracsysFusionTrackInternals: ftkSetFrameOptions failed" << std::endl; } else { CMN_LOG_INIT_VERBOSE << "mtsAtracsysFusionTrackInternals: ftkSetFrameOptions ok" << std::endl; + m_configured = true; } } std::string QueryDeviceType() { + auto option = m_device_options.find(option_sTk_device_type); + if (option == m_device_options.end()) { return ""; } + ftkBuffer buffer; - ftkError status = ftkGetData(m_library, m_sn, option_id_sTk_device_type, &buffer); + ftkError status = ftkGetData(m_library, m_sn, option->second.id, &buffer); if (status != ftkError::FTK_OK) return ""; std::string device_type(buffer.data); @@ -267,48 +326,42 @@ class mtsAtracsysFusionTrackInternals return device_type; } - ftkError ConfigureImageSending(bool enabled) + bool ConfigureImageSending(bool enabled, int sl_period, int dots) { - ftkBuffer buffer; - buffer.reset(); - std::string frame_pattern = "S"; - std::memcpy(buffer.uData, frame_pattern.c_str(), frame_pattern.length()); - buffer.size = frame_pattern.length(); + bool ok; - ftkError status = ftkSetData(m_library, m_sn, option_id_image_pattern, &buffer); - if (status != ftkError::FTK_OK) { - return status; + // No structured light, IR frames only + if (sl_period <= 0) { + ok = SetStringOption(option_image_pattern, "I"); + // One structured light frame per period, others are IR + } else { + std::string pattern = std::string(sl_period - 1, 'I') + "S"; + ok = SetStringOption(option_image_pattern, pattern); } + if (!ok) { return false; } + int32_t enabled_value = enabled ? 1 : 0; - status = ftkSetInt32(m_library, m_sn, option_id_enable_images_sending, enabled_value); - if (status == ftkError::FTK_OK) { + ok = SetIntOption(option_enable_images_sending, enabled_value); + if (ok) { m_images_enabled = enabled; } else { - return status; + return false; } - status = ftkSetInt32(m_library, m_sn, option_id_vis_integration_time, 16000); - if (status != ftkError::FTK_OK) { - return status; - } + ok = SetIntOption(option_vis_integration_time, 16000); + if (!ok) { return false; } - status = ftkSetInt32(m_library, m_sn, option_id_sl_integration_time, 16000); - if (status != ftkError::FTK_OK) { - return status; - } + ok = SetIntOption(option_sl_integration_time, 16000); + if (!ok) { return false; } - status = ftkSetInt32(m_library, m_sn, option_id_num_enabled_dot_projector, 3); - if (status != ftkError::FTK_OK) { - return status; - } + ok = SetIntOption(option_num_enabled_dot_projector, dots); + if (!ok) { return false; } - status = ftkSetInt32(m_library, m_sn, option_id_dot_projector_strobe_time, 16000); - if (status != ftkError::FTK_OK) { - return status; - } + ok = SetIntOption(option_dot_projector_strobe_time, 16000); + if (!ok) { return false; } - return ftkError::FTK_OK; + return true; } void ExtractImage(uint8_t *pixels, prmImageFrame& dest) @@ -349,15 +402,8 @@ class mtsAtracsysFusionTrackInternals // Retrieve stereo camera calibration from device, return true/false for success/error bool RetrieveStereoCameraCalibration(prmCameraInfo& left, prmCameraInfo& right, vct3& rotation, vct3& translation) { - ftkError status; - - status = ftkSetInt32(m_library, m_sn, option_id_export_calibration, 1); - if (status != ftkError::FTK_OK) { - - CMN_LOG_RUN_ERROR << "Error enabling stereo calibration export: " - << static_cast(status) << std::endl; - return false; - } + bool ok = SetIntOption(option_export_calibration, 1); + if (!ok) { return false; } // need initialized frame query to retrieve camera calibration if (!m_frame_query) { @@ -366,6 +412,7 @@ class mtsAtracsysFusionTrackInternals int attempts = 0; int max_attempts = 20; + ftkError status; do { status = ftkGetLastFrame(m_library, m_sn, m_frame_query, 100u); @@ -373,16 +420,12 @@ class mtsAtracsysFusionTrackInternals if (status != ftkError::FTK_OK) { CMN_LOG_RUN_ERROR << "Error retrieving stereo calibration frame: " - << static_cast(status) << std::endl; + << static_cast(status) << std::endl; return false; } - status = ftkSetInt32(m_library, m_sn, option_id_export_calibration, 0); - if (status != ftkError::FTK_OK) { - CMN_LOG_RUN_ERROR << "Error disabling stereo calibration export: " - << static_cast(status) << std::endl; - return false; - } + ok = SetIntOption(option_export_calibration, 0); + if (!ok) { return false; } ftkFrameInfoData frame_info; frame_info.WantedInformation = ftkInformationType::CalibrationParameters; @@ -390,7 +433,7 @@ class mtsAtracsysFusionTrackInternals status = ftkExtractFrameInfo(m_frame_query, &frame_info); if (status != ftkError::FTK_OK) { CMN_LOG_RUN_ERROR << "Error extracting frame info for stereo calibration: " - << static_cast(status) << std::endl; + << static_cast(status) << std::endl; return false; } @@ -434,24 +477,99 @@ class mtsAtracsysFusionTrackInternals GeometryIdToToolMap GeometryIdToTool; private: - const uint32_t option_id_sTk_device_type = 221; - const uint32_t option_id_enable_images_sending = 6003; - const uint32_t option_id_image_pattern = 199; - const uint32_t option_id_vis_integration_time = 193; - const uint32_t option_id_sl_integration_time = 194; - const uint32_t option_id_num_enabled_dot_projector = 110; - const uint32_t option_id_dot_projector_strobe_time = 197; - const uint32_t option_id_export_calibration = 181; -}; + std::map m_device_options; + + const std::string option_sTk_device_type = "sTk device type"; + const std::string option_enable_images_sending = "Enable images sending"; + const std::string option_image_pattern = "Image Scheduler Pattern";; + const std::string option_vis_integration_time = "Image Integration Time for VIS frames"; + const std::string option_sl_integration_time = "Image Integration Time for SL frames"; + const std::string option_num_enabled_dot_projector = "Enable dot projectors"; + const std::string option_dot_projector_strobe_time = "Dot projectors Strobe Time for SL frames"; + const std::string option_export_calibration = "Calibration export"; + + bool SetIntOption(const std::string& option_name, int32_t value) { + auto option = m_device_options.find(option_name); + if (option == m_device_options.end()) { + CMN_LOG_RUN_ERROR << "option \"" << option_name << "\" not supported by device" << std::endl; + return false; + } + + ftkError status; + auto id = option->second.id; + int32_t min, max; + status = ftkGetInt32(m_library, m_sn, id, &min, ftkOptionGetter::FTK_MIN_VAL); + if (status != ftkError::FTK_OK) { + CMN_LOG_INIT_ERROR << "error getting min for device option \"" << option_name << "\"" + << ": " << static_cast(status) << std::endl; + return false; + } + + status = ftkGetInt32(m_library, m_sn, id, &max, ftkOptionGetter::FTK_MAX_VAL); + if (status != ftkError::FTK_OK) { + CMN_LOG_INIT_ERROR << "error getting max for device option \"" << option_name << "\"" + << ": " << static_cast(status) << std::endl; + return false; + } + + if (value < min || value > max) { + CMN_LOG_RUN_ERROR << "value " << value + << " out of range [" << min << ".." << max << "]" + << " for device option \"" << option_name << "\"" << std::endl; + return false; + } + + status = ftkSetInt32(m_library, m_sn, id, value); + if (status != ftkError::FTK_OK) { + CMN_LOG_INIT_ERROR << "error setting device option \"" << option_name << "\"" + << ": " << static_cast(status) << std::endl; + return false; + } + + return true; + } + + bool SetStringOption(const std::string& option_name, std::string value) { + auto option = m_device_options.find(option_name); + if (option == m_device_options.end()) { + CMN_LOG_RUN_ERROR << "option \"" << option_name << "\" not supported by device" << std::endl; + return false; + } + + auto id = option->second.id; + + ftkBuffer buffer; + buffer.reset(); + std::memcpy(buffer.uData, value.c_str(), value.length()); + buffer.size = value.length(); + + ftkError status = ftkSetData(m_library, m_sn, id, &buffer); + if (status != ftkError::FTK_OK) { + CMN_LOG_INIT_ERROR << "error setting device option \"" << option_name << "\"" + << ": " << static_cast(status) << std::endl; + return false; + } + + return true; + } + + static void DeviceEnumerateCallback(uint64 device, void* user, ftkDeviceType type) + { + mtsAtracsysFusionTrackInternals* internals = reinterpret_cast(user); + if (!internals) { return; } -static void mtsAtracsysFusionTrackDeviceEnum(uint64 device, void * user, ftkDeviceType type) -{ - mtsAtracsysFusionTrackInternals* internals = reinterpret_cast(user); - if (internals) { internals->m_sn = device; internals->m_device_type = type; } -} + + static void OptionEnumerateCallback(uint64 CMN_UNUSED(sn), void* user, ftkOptionsInfo* oi) + { + mtsAtracsysFusionTrackInternals* internals = reinterpret_cast(user); + if (!internals) { return; } + + internals->m_device_options[oi->name] = *oi; + } +}; void mtsAtracsysFusionTrack::Init(void) { @@ -499,52 +617,17 @@ void mtsAtracsysFusionTrack::Init(void) void mtsAtracsysFusionTrack::Configure(const std::string & filename) { CMN_LOG_CLASS_INIT_VERBOSE << "Configure: using " << filename << std::endl; - ftkBuffer buffer; - - ftkVersion(&buffer); - CMN_LOG_CLASS_INIT_VERBOSE << "Atracsys SDK Version " << buffer.data << std::endl; - -#if 0 - if (ftkGetData(m_internals->m_library, 0LL, FTK_OPT_DRIVER_VER, &buffer) != SDK_FTK_OK) - CMN_LOG_CLASS_INIT_WARNING << "Configure: failed to get Atracsys Driver version" << std::endl; -#endif - - // initialize fusion track library - m_internals->m_library = ftkInit(); - if (!m_internals->m_library) { - CMN_LOG_CLASS_INIT_ERROR << "Configure: unable to initialize (" - << this->GetName() << ")" << std::endl; - return; - } - - for (size_t i = 0; i < 10; i++) { - // search for devices - ftkError error = ftkEnumerateDevices(m_internals->m_library, - mtsAtracsysFusionTrackDeviceEnum, - m_internals); - if (error != FTK_ERROR_NS::FTK_OK) { - CMN_LOG_CLASS_INIT_ERROR << "Configure: unable to enumerate devices (" - << this->GetName() << ")" << std::endl; - ftkClose(&m_internals->m_library); - } - - if (m_internals->m_sn != 0LL) { - break; - } - } - if (m_internals->m_sn == 0LL) { - CMN_LOG_CLASS_INIT_ERROR << "Configure: no device connected (" + bool ok = m_internals->Initialize(); + if (!ok) { + CMN_LOG_INIT_ERROR << "Configure: no device connected (" << this->GetName() << ")" << std::endl; - ftkClose(&m_internals->m_library); - return; } CMN_LOG_CLASS_INIT_VERBOSE << "Configure: found device SN " << m_internals->m_sn << std::endl; if (filename == "") { m_internals->SetupFrameQuery(m_tools.size(), m_stray_markers_max); - m_internals->m_configured = true; return; } @@ -623,11 +706,11 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) const Json::Value iniTool = jsonValue["ini-file"]; const Json::Value jsonTool = jsonValue["json-file"]; if (iniTool.empty() && jsonTool.empty()) { - CMN_LOG_CLASS_INIT_ERROR << "Configure: you need to define either \"ini-file\" or \"json-file\" for the tool \"" + CMN_LOG_INIT_ERROR << "Configure: you need to define either \"ini-file\" or \"json-file\" for the tool \"" << toolName << "\" in configuration file \"" << filename << "\", neither was found" << std::endl; } else if (!iniTool.empty() && !jsonTool.empty()) { - CMN_LOG_CLASS_INIT_ERROR << "Configure: you need to define either \"ini-file\" or \"json-file\" for the tool \"" + CMN_LOG_INIT_ERROR << "Configure: you need to define either \"ini-file\" or \"json-file\" for the tool \"" << toolName << "\" in configuration file \"" << filename << "\", both were found" << std::endl; exit(EXIT_FAILURE); @@ -667,9 +750,27 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) CMN_LOG_CLASS_INIT_VERBOSE << "device type: " << device_type << std::endl; } - ftkError status = m_internals->ConfigureImageSending(true); - if (status != ftkError::FTK_OK) { - CMN_LOG_CLASS_INIT_ERROR << "failed to enabled image sending: " << static_cast(status) << std::endl; + const Json::Value stereo_config = jsonConfig["stereo"]; + + const Json::Value video = stereo_config["video"]; + bool video_enabled = stereo_config.isMember("video") && video.isBool() && video.asBool(); + const Json::Value depth = stereo_config["depth"]; + bool depth_enabled = stereo_config.isMember("depth") && depth.isBool() && depth.asBool(); + // video processing is required for depth map computation + video_enabled = depth_enabled || video_enabled; + + const Json::Value dots_config = stereo_config["num_dot_projectors"]; + bool dots_configured = stereo_config.isMember("num_dot_projectors") && dots_config.isInt(); + int enabled_dot_projectors = dots_configured ? dots_config.asInt() : 1; + if (!depth_enabled) { + enabled_dot_projectors = 0; + } + + int sl_period = depth_enabled ? 5 : 0; + + bool ok = m_internals->ConfigureImageSending(video_enabled, sl_period, enabled_dot_projectors); + if (!ok) { + CMN_LOG_CLASS_INIT_ERROR << "failed to enabled image sending" << std::endl; } } @@ -677,7 +778,6 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) CMN_LOG_CLASS_INIT_VERBOSE << "Configure: calling SetupFrameQuery for " << m_tools.size() << " tool(s) and up to " << m_stray_markers_max << " stray marker(s)" << std::endl; m_internals->SetupFrameQuery(m_tools.size(), m_stray_markers_max); - m_internals->m_configured = true; } @@ -708,7 +808,7 @@ void mtsAtracsysFusionTrack::Run(void) // process mts commands ProcessQueuedCommands(); - if (m_internals->m_library == 0LL) { + if (!HardwareInitialized()) { return; } @@ -750,10 +850,7 @@ void mtsAtracsysFusionTrack::Run(void) } -void mtsAtracsysFusionTrack::Cleanup(void) -{ - ftkClose(&m_internals->m_library); -} +void mtsAtracsysFusionTrack::Cleanup(void) {} bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, @@ -764,7 +861,7 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, // check if this tool already exists auto tool_iterator = m_tools.find(toolName); if (tool_iterator != m_tools.end()) { - CMN_LOG_CLASS_INIT_ERROR << "AddTool: " << toolName << " already exists" << std::endl; + CMN_LOG_INIT_ERROR << "AddTool: " << toolName << " already exists" << std::endl; return false; } @@ -773,7 +870,7 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, if (_referenceName != "") { referenceIterator = m_tools.find(_referenceName); if (referenceIterator == m_tools.end()) { - CMN_LOG_CLASS_INIT_ERROR << "AddTool: can't find reference \"" << _referenceName + CMN_LOG_INIT_ERROR << "AddTool: can't find reference \"" << _referenceName << "\" for tool \"" << toolName << "\". Make sure reference frames/tools are created earlier in the config file." << std::endl; @@ -802,14 +899,14 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, << geometry.positions[index].z << ")" << std::endl; } } else { - CMN_LOG_CLASS_INIT_ERROR << "AddTool: failed to parse geometry file \"" + CMN_LOG_INIT_ERROR << "AddTool: failed to parse geometry file \"" << fileName << "\" for tool \"" << toolName << "\"" << std::endl; return false; } ftkError error = ftkSetGeometry(m_internals->m_library, m_internals->m_sn, &geometry); if (error != FTK_ERROR_NS::FTK_OK) { - CMN_LOG_CLASS_INIT_ERROR << "AddTool: unable to set geometry for tool \"" << toolName + CMN_LOG_INIT_ERROR << "AddTool: unable to set geometry for tool \"" << toolName << "\" using geometry file \"" << fileName << "\" (" << this->GetName() << ")" << std::endl; return false; @@ -825,7 +922,7 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, const mtsAtracsysFusionTrackInternals::GeometryIdToToolMap::const_iterator toolIterator = m_internals->GeometryIdToTool.find(geometry.geometryId); if (toolIterator != m_internals->GeometryIdToTool.end()) { - CMN_LOG_CLASS_INIT_ERROR << "AddTool: error, found an existing tool with the same Id " + CMN_LOG_INIT_ERROR << "AddTool: error, found an existing tool with the same Id " << geometry.geometryId << " for tool \"" << toolName << "\"" << std::endl; return false; } @@ -836,7 +933,7 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, // create an interface for tool tool->m_interface = AddInterfaceProvided(toolName); if (!tool->m_interface) { - CMN_LOG_CLASS_INIT_ERROR << "AddTool: " << tool->m_name << " already exists" << std::endl; + CMN_LOG_INIT_ERROR << "AddTool: " << tool->m_name << " already exists" << std::endl; delete tool; return false; } From 1e6b3aab50b66900d2594b233d844f0cb583b3dc Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Sat, 13 Jul 2024 15:42:33 -0400 Subject: [PATCH 05/12] Rename mts_ros_crtk_atracsys_bridge -> atracsys_bridge Bridge handles more than just CRTK topics, and the longer name is a little unwieldy --- ros/CMakeLists.txt | 2 +- ros/src/atracsys.cpp | 13 ++++++------- ...rtk_atracsys_bridge.cpp => atracsys_bridge.cpp} | 10 +++++----- ...os_crtk_atracsys_bridge.h => atracsys_bridge.h} | 14 +++++++------- 4 files changed, 19 insertions(+), 20 deletions(-) rename ros/src/{mts_ros_crtk_atracsys_bridge.cpp => atracsys_bridge.cpp} (90%) rename ros/src/{mts_ros_crtk_atracsys_bridge.h => atracsys_bridge.h} (84%) diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 3609b38..25dd15c 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -78,7 +78,7 @@ if (cisst_FOUND_AS_REQUIRED) include_directories (${CATKIN_DEVEL_PREFIX}/include ${sawAtracsysFusionTrack_INCLUDE_DIR}) link_directories (${sawAtracsysFusionTrack_LIBRARY_DIR}) - add_executable (atracsys src/atracsys.cpp src/mts_ros_crtk_atracsys_bridge.cpp src/mts_ros_crtk_atracsys_bridge.h) + add_executable (atracsys src/atracsys.cpp src/atracsys_bridge.cpp src/atracsys_bridge.h) target_link_libraries (atracsys ${sawAtracsysFusionTrack_LIBRARIES} ${catkin_LIBRARIES} diff --git a/ros/src/atracsys.cpp b/ros/src/atracsys.cpp index c664652..5903327 100644 --- a/ros/src/atracsys.cpp +++ b/ros/src/atracsys.cpp @@ -26,7 +26,7 @@ no warranty. The complete license can be found in license.txt and #include #include -#include "mts_ros_crtk_atracsys_bridge.h" +#include "atracsys_bridge.h" #include #include @@ -97,14 +97,13 @@ int main(int argc, char * argv[]) stereo->GetName(), "StereoRaw"); // ROS CRTK bridge - mts_ros_crtk_atracsys_bridge * crtk_bridge - = new mts_ros_crtk_atracsys_bridge("atracsys_crtk_bridge", rosNode); + atracsys_bridge * bridge = new atracsys_bridge("atracsys_bridge", rosNode); - crtk_bridge->bridge(tracker->GetName(), "Controller", rosPeriod, tfPeriod); - crtk_bridge->bridge_interface_provided(stereo->GetName(), "stereo", tracker->GetName() + "/stereo", rosPeriod, tfPeriod); + bridge->bridge(tracker->GetName(), "Controller", rosPeriod, tfPeriod); + bridge->bridge_interface_provided(stereo->GetName(), "stereo", tracker->GetName() + "/stereo", rosPeriod, tfPeriod); - componentManager->AddComponent(crtk_bridge); - crtk_bridge->Connect(); + componentManager->AddComponent(bridge); + bridge->Connect(); // create a Qt user interface QApplication application(argc, argv); diff --git a/ros/src/mts_ros_crtk_atracsys_bridge.cpp b/ros/src/atracsys_bridge.cpp similarity index 90% rename from ros/src/mts_ros_crtk_atracsys_bridge.cpp rename to ros/src/atracsys_bridge.cpp index 64fe326..56da790 100644 --- a/ros/src/mts_ros_crtk_atracsys_bridge.cpp +++ b/ros/src/atracsys_bridge.cpp @@ -16,7 +16,7 @@ no warranty. The complete license can be found in license.txt and --- end cisst license --- */ -#include "mts_ros_crtk_atracsys_bridge.h" +#include "atracsys_bridge.h" #include #include @@ -24,9 +24,9 @@ no warranty. The complete license can be found in license.txt and // cisst #include -CMN_IMPLEMENT_SERVICES(mts_ros_crtk_atracsys_bridge); +CMN_IMPLEMENT_SERVICES(atracsys_bridge); -void mts_ros_crtk_atracsys_bridge::bridge(const std::string & _component_name, +void atracsys_bridge::bridge(const std::string & _component_name, const std::string & _interface_name, const double _publish_period_in_seconds, const double _tf_period_in_seconds) @@ -49,7 +49,7 @@ void mts_ros_crtk_atracsys_bridge::bridge(const std::string & _component_name, _tf_period_in_seconds); } -void mts_ros_crtk_atracsys_bridge::bridge_interface_provided(const std::string & _component_name, +void atracsys_bridge::bridge_interface_provided(const std::string & _component_name, const std::string & _interface_name, const double _publish_period_in_seconds, const double _tf_period_in_seconds) { @@ -60,7 +60,7 @@ void mts_ros_crtk_atracsys_bridge::bridge_interface_provided(const std::string & bridge_tool_error(_component_name, _interface_name); } -void mts_ros_crtk_atracsys_bridge::bridge_tool_error(const std::string & _component_name, +void atracsys_bridge::bridge_tool_error(const std::string & _component_name, const std::string & _interface_name) { mtsManagerLocal * _component_manager = mtsComponentManager::GetInstance(); diff --git a/ros/src/mts_ros_crtk_atracsys_bridge.h b/ros/src/atracsys_bridge.h similarity index 84% rename from ros/src/mts_ros_crtk_atracsys_bridge.h rename to ros/src/atracsys_bridge.h index 6567666..41bfa24 100644 --- a/ros/src/mts_ros_crtk_atracsys_bridge.h +++ b/ros/src/atracsys_bridge.h @@ -16,23 +16,23 @@ no warranty. The complete license can be found in license.txt and --- end cisst license --- */ -#ifndef _mts_ros_crtk_atracsys_bridge_h -#define _mts_ros_crtk_atracsys_bridge_h +#ifndef _atracsys_bridge_h +#define _atracsys_bridge_h #include -class mts_ros_crtk_atracsys_bridge: public mts_ros_crtk_bridge +class atracsys_bridge: public mts_ros_crtk_bridge { CMN_DECLARE_SERVICES(CMN_NO_DYNAMIC_CREATION, CMN_LOG_ALLOW_DEFAULT); public: - inline mts_ros_crtk_atracsys_bridge(const std::string & _component_name, + inline atracsys_bridge(const std::string & _component_name, cisst_ral::node_ptr_t _node_handle, const double _period_in_seconds = 5.0 * cmn_ms): mts_ros_crtk_bridge(_component_name, _node_handle, _period_in_seconds) {} - inline ~mts_ros_crtk_atracsys_bridge() {} + inline ~atracsys_bridge() {} /*! Everything needed to bridge the sawAtracsysTracker component */ void bridge(const std::string & _component_name, @@ -53,6 +53,6 @@ class mts_ros_crtk_atracsys_bridge: public mts_ros_crtk_bridge const std::string & _interface_name); }; -CMN_DECLARE_SERVICES_INSTANTIATION(mts_ros_crtk_atracsys_bridge); +CMN_DECLARE_SERVICES_INSTANTIATION(atracsys_bridge); -#endif // _mts_ros_crtk_atracsys_bridge_h +#endif // _atracsys_bridge_h From 9edf705f8905784b627ee75a744bc9a24b34ab40 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Sat, 13 Jul 2024 20:33:03 -0400 Subject: [PATCH 06/12] Refactor Atracsys ROS bridge --- ros/src/atracsys.cpp | 8 +- ros/src/atracsys_bridge.cpp | 148 ++++++++++++++++++++++++------------ ros/src/atracsys_bridge.h | 45 +++++------ 3 files changed, 127 insertions(+), 74 deletions(-) diff --git a/ros/src/atracsys.cpp b/ros/src/atracsys.cpp index 5903327..c94079a 100644 --- a/ros/src/atracsys.cpp +++ b/ros/src/atracsys.cpp @@ -97,12 +97,12 @@ int main(int argc, char * argv[]) stereo->GetName(), "StereoRaw"); // ROS CRTK bridge - atracsys_bridge * bridge = new atracsys_bridge("atracsys_bridge", rosNode); + atracsys_bridge * bridge = new atracsys_bridge("atracsys_bridge", rosNode, rosPeriod, tfPeriod); + componentManager->AddComponent(bridge); - bridge->bridge(tracker->GetName(), "Controller", rosPeriod, tfPeriod); - bridge->bridge_interface_provided(stereo->GetName(), "stereo", tracker->GetName() + "/stereo", rosPeriod, tfPeriod); + bridge->bridge_controller(tracker->GetName(), "Controller"); + bridge->bridge_stereo(stereo->GetName(), "stereo", tracker->GetName() + "/stereo"); - componentManager->AddComponent(bridge); bridge->Connect(); // create a Qt user interface diff --git a/ros/src/atracsys_bridge.cpp b/ros/src/atracsys_bridge.cpp index 56da790..01d902e 100644 --- a/ros/src/atracsys_bridge.cpp +++ b/ros/src/atracsys_bridge.cpp @@ -21,66 +21,118 @@ no warranty. The complete license can be found in license.txt and #include #include -// cisst +#include #include +#include CMN_IMPLEMENT_SERVICES(atracsys_bridge); -void atracsys_bridge::bridge(const std::string & _component_name, - const std::string & _interface_name, - const double _publish_period_in_seconds, - const double _tf_period_in_seconds) +atracsys_bridge::atracsys_bridge(const std::string& _component_name, + cisst_ral::node_ptr_t _node_handle, + double ros_period, + double tf_period): + mts_ros_crtk_bridge_provided(_component_name, _node_handle, ros_period), + m_ros_period(ros_period), + m_tf_period(tf_period) { - // clean ROS namespace - std::string _clean_namespace = _component_name; - cisst_ral::clean_namespace(_clean_namespace); - - // create factory to bridge tool as they get created - add_factory_source(_component_name, - _interface_name, - _publish_period_in_seconds, - _tf_period_in_seconds); - - // controller specific topics, some might be CRTK compliant - bridge_interface_provided(_component_name, - _interface_name, - _clean_namespace, - _publish_period_in_seconds, - _tf_period_in_seconds); -} + mtsManagerLocal* componentManager = mtsManagerLocal::GetInstance(); + + std::string m_bridge_name = GetName() + "_pub"; + cisst_ral::clean_namespace(m_bridge_name); + + m_pub_bridge = new mtsROSBridge(m_bridge_name, ros_period, _node_handle); + m_pub_bridge->AddIntervalStatisticsInterface(); + componentManager->AddComponent(m_pub_bridge); -void atracsys_bridge::bridge_interface_provided(const std::string & _component_name, - const std::string & _interface_name, - const double _publish_period_in_seconds, - const double _tf_period_in_seconds) { - // bridge CRTK topics - mts_ros_crtk_bridge::bridge_interface_provided(_component_name, _interface_name, - _publish_period_in_seconds, _tf_period_in_seconds); - // and also the registration_error command (if present) - bridge_tool_error(_component_name, _interface_name); + // add pub bridge stats to the stats bridge from the base class + stats_bridge().AddIntervalStatisticsPublisher("publishers", m_pub_bridge->GetName()); } -void atracsys_bridge::bridge_tool_error(const std::string & _component_name, - const std::string & _interface_name) +void atracsys_bridge::bridge_controller(const std::string & _component_name, + const std::string & _interface_name) { - mtsManagerLocal * _component_manager = mtsComponentManager::GetInstance(); - mtsComponent * _component = _component_manager->GetComponent(_component_name); - if (!_component) { - CMN_LOG_CLASS_RUN_ERROR << "bridge_tool_error: unable to find component \"" - << _component_name << "\"" << std::endl; - return; - } - // then try to find the interface - mtsInterfaceProvided * _interface_provided = _component->GetInterfaceProvided(_interface_name); + std::string required_interface_name = GetName() + "_" + _component_name + "_using_" + _interface_name + "_factory"; + mtsInterfaceRequired* required_interface = AddInterfaceRequired(required_interface_name); - // check if interface provides tool registration error - const auto& read_commands = _interface_provided->GetNamesOfCommandsRead(); - if (std::find(read_commands.begin(), read_commands.end(), "registration_error") == read_commands.end()) { - return; + if (!required_interface) { + CMN_LOG_CLASS_INIT_ERROR << "bridge_controller: couldn't add required interface for factory source" << std::endl; } - std::string ros_topic = _component_name + "/" + _interface_name + "/registration_error"; + required_interface->AddFunction("crtk_interfaces_provided", m_get_factory_sources); + required_interface->AddEventHandlerVoid(&atracsys_bridge::sources_update_handler, this, + "crtk_interfaces_provided_updated"); + + mtsManagerLocal* componentManager = mtsManagerLocal::GetInstance(); + componentManager->Connect(GetName(), required_interface_name, _component_name, _interface_name); + + // clean ROS namespace + std::string clean_namespace = _component_name; + cisst_ral::clean_namespace(clean_namespace); + + // bridge all CRTK-compatible topics from the controller + bridge_interface_provided(_component_name, _interface_name, clean_namespace, + m_ros_period, m_tf_period); +} + +void atracsys_bridge::bridge_stereo(const std::string & _component_name, + const std::string & _interface_name, + const std::string & _ros_namespace) +{ + std::string clean_namespace = _ros_namespace; + cisst_ral::clean_namespace(clean_namespace); + + m_pub_bridge->AddPublisherFromCommandRead + (_interface_name, "left/image_rect_color", clean_namespace + "/left/image_rect_color"); - m_subscribers_bridge->AddPublisherFromCommandRead + m_pub_bridge->AddPublisherFromCommandRead + (_interface_name, "right/image_rect_color", clean_namespace + "/right/image_rect_color"); + + m_pub_bridge->AddPublisherFromCommandRead + (_interface_name, "left/camera_info", clean_namespace + "/left/camera_info"); + + m_pub_bridge->AddPublisherFromCommandRead + (_interface_name, "right/camera_info", clean_namespace + "/right/camera_info"); + + m_pub_bridge->AddPublisherFromCommandRead + (_interface_name, "depth", clean_namespace + "/depth"); + + mtsManagerLocal* componentManager = mtsManagerLocal::GetInstance(); + componentManager->Connect(m_pub_bridge->GetName(), _interface_name, _component_name, _interface_name); +} + +void atracsys_bridge::bridge_tool(const std::string & _component_name, + const std::string & _interface_name, + const std::string & _ros_namespace) +{ + std::string clean_namespace = _ros_namespace; + cisst_ral::clean_namespace(clean_namespace); + std::string ros_topic = clean_namespace + "/registration_error"; + + m_pub_bridge->AddPublisherFromCommandRead (_interface_name, "registration_error", ros_topic); + + mtsManagerLocal* componentManager = mtsManagerLocal::GetInstance(); + componentManager->Connect(m_pub_bridge->GetName(), _interface_name, _component_name, _interface_name); +} + +void atracsys_bridge::sources_update_handler() +{ + std::vector sources; + m_get_factory_sources(sources); + for (const auto & source : sources) { + // skip any sources bridged previously + if (m_bridged_sources.find(source) != m_bridged_sources.end()) { + continue; + } + + std::string ros_namespace = source.ComponentName + "/" + source.InterfaceName; + cisst_ral::clean_namespace(ros_namespace); + bridge_interface_provided(source.ComponentName, + source.InterfaceName, + ros_namespace, + m_ros_period, m_tf_period); + bridge_tool(source.ComponentName, source.InterfaceName, ros_namespace); + Connect(); + m_bridged_sources.insert(source); + } } diff --git a/ros/src/atracsys_bridge.h b/ros/src/atracsys_bridge.h index 41bfa24..2bd4176 100644 --- a/ros/src/atracsys_bridge.h +++ b/ros/src/atracsys_bridge.h @@ -19,38 +19,39 @@ no warranty. The complete license can be found in license.txt and #ifndef _atracsys_bridge_h #define _atracsys_bridge_h -#include +#include -class atracsys_bridge: public mts_ros_crtk_bridge +class atracsys_bridge: public mts_ros_crtk_bridge_provided { CMN_DECLARE_SERVICES(CMN_NO_DYNAMIC_CREATION, CMN_LOG_ALLOW_DEFAULT); - public: - inline atracsys_bridge(const std::string & _component_name, - cisst_ral::node_ptr_t _node_handle, - const double _period_in_seconds = 5.0 * cmn_ms): - mts_ros_crtk_bridge(_component_name, _node_handle, _period_in_seconds) - {} +public: + atracsys_bridge(const std::string& component_name, + cisst_ral::node_ptr_t node_handle, + double ros_period = 5.0 * cmn_ms, + double tf_period = 5.0 * cmn_ms); - inline ~atracsys_bridge() {} + void bridge_controller(const std::string& _component_name, + const std::string& _interface_name); - /*! Everything needed to bridge the sawAtracsysTracker component */ - void bridge(const std::string & _component_name, + void bridge_stereo(const std::string & _component_name, const std::string & _interface_name, - const double _publish_period_in_seconds, - const double _tf_period_in_seconds); + const std::string & _ros_namespace); - using mts_ros_crtk_bridge::bridge_interface_provided; // bring all overloads into scope before overriding +protected: + mtsROSBridge* m_pub_bridge; - virtual void bridge_interface_provided(const std::string & _component_name, - const std::string & _interface_name, - const double _publish_period_in_seconds - = cisst_ros_crtk::bridge_provided_default_publish_period, - const double _tf_period_in_seconds - = cisst_ros_crtk::bridge_provided_default_tf_period) override; + const double m_ros_period; + const double m_tf_period; - void bridge_tool_error(const std::string & _component_name, - const std::string & _interface_name); + // source factory for tools + mtsFunctionRead m_get_factory_sources; + std::set m_bridged_sources; + void sources_update_handler(); + + void bridge_tool(const std::string & _component_name, + const std::string & _interface_name, + const std::string & _ros_namespace); }; CMN_DECLARE_SERVICES_INSTANTIATION(atracsys_bridge); From 173b3d92176b2495c0bc1b8459e18ab4c9a25712 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Mon, 15 Jul 2024 16:49:07 -0400 Subject: [PATCH 07/12] Brief documentation of stereo processing config options --- README.md | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/README.md b/README.md index e632870..073e1a2 100644 --- a/README.md +++ b/README.md @@ -73,6 +73,33 @@ If you also want ROS topics corresponding to the tracked tools, try: rosrun atracsys atracsys -j config003.json ``` +### Configuration file format + +The JSON configuration files consists of a list of tools to track, and optionally configuration for the stereo image processing (for the spryTrack RGB devices). The `definition-path` property can optionally be set to a list of file paths to search tool definitions for --- by default, the search path for tool definition files is the current working directory, and the directory containing the configuration file. + +```json +{ + definition-path: ["additional tool definition search path", "another search path"], + tools: [ + { + { + "name": "Marker3", // tool name, used in GUI display and in ROS topics for that tool + // provided tool definition as either json or ini + "json-file": "geometry003.json", // json tool definition file + "ini-file": "geometry003.ini" // or ini tool definition file + }, + } + ], + stereo: { + "video": true, // (default false) provide rectified video streams + "depth": true, // (default false) compute 3D point cloud from stereo images + "filter_depth_map": true, // post-process depth map - improves quality at cost of some additional computation + "global_block_matching": true, // (default false) use (semi)global block matching algorithm instead of default local block matching + "num_dot_projectors": 3 // (default 0 if depth = false, 1 if depth = true) how many dot projector pairs to turn on - dot projectors add texture to images, increasing quality of stereo correspondence matching + } +} +``` + # Unable to find shared object file `libdevice64.so` When using ROS, we copy the SDK libraries in the ROS build tree so you shouldn't have to edit your LD_LIBRARY path. If you still get some error messages re. missing libraries, you need to locate the libraries and edit your `LD_LIBRARY_PATH`. Something like: @@ -134,6 +161,14 @@ transform: --- ``` +# Stereo processing + +For devices such as the spryTrack 300 RGB which provide RGB images and have dot projectors, stereo processing is provided to compute a dense 3D point cloud. This can be enabled via the configuration file as described above. + +Enabling the global block matching option may improve depth map quality, however it will often slow down processing significantly + +Enabling the depth map filtering algorithm will perform left-right consisteny checks to reject bad readings, and apply an edge-aware filtering algorithm to help reduce noise. This increases the computational cost of the stereo processing, particularly when the global block matching is enabled. + # Known issues, features to add * Support for SDK 4 while preserving SDK 3 support for older tracking system From d65275e45ddcbd854a7284cd34eef1d7bc94c66e Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Mon, 15 Jul 2024 17:03:25 -0400 Subject: [PATCH 08/12] Minor refactoring --- .../code/mtsAtracsysFusionTrack.cpp | 80 ++++++++++++------- core/components/code/mtsAtracsysStereo.cpp | 35 ++++++-- .../mtsAtracsysFusionTrack.h | 3 + .../mtsAtracsysStereo.h | 2 +- 4 files changed, 80 insertions(+), 40 deletions(-) diff --git a/core/components/code/mtsAtracsysFusionTrack.cpp b/core/components/code/mtsAtracsysFusionTrack.cpp index a311aaa..dd99f6e 100644 --- a/core/components/code/mtsAtracsysFusionTrack.cpp +++ b/core/components/code/mtsAtracsysFusionTrack.cpp @@ -349,16 +349,13 @@ class mtsAtracsysFusionTrackInternals return false; } - ok = SetIntOption(option_vis_integration_time, 16000); - if (!ok) { return false; } - ok = SetIntOption(option_sl_integration_time, 16000); if (!ok) { return false; } ok = SetIntOption(option_num_enabled_dot_projector, dots); if (!ok) { return false; } - ok = SetIntOption(option_dot_projector_strobe_time, 16000); + ok = SetIntOption(option_dot_projector_strobe_time, 8000); if (!ok) { return false; } return true; @@ -786,7 +783,9 @@ void mtsAtracsysFusionTrack::Startup(void) m_configuration_state_table.Start(); // reports system found - m_controller_interface->SendStatus(this->GetName() + ": found device SN " + std::to_string(m_internals->m_sn)); + std::string status_message = this->GetName() + ": found device SN " + std::to_string(m_internals->m_sn) + + ", type " + m_internals->QueryDeviceType(); + m_controller_interface->SendStatus(status_message); // set reference frame for measured_cp_array m_measured_cp_array.ReferenceFrame() = this->Name; @@ -819,7 +818,6 @@ void mtsAtracsysFusionTrack::Run(void) 100u); // negative error codes are warnings - m_measured_cp_array.SetValid(true); if (status != FTK_ERROR_NS::FTK_OK) { if (static_cast(status) < 0) { CMN_LOG_CLASS_RUN_WARNING << "Warning: " << static_cast(status) << std::endl; @@ -830,6 +828,8 @@ void mtsAtracsysFusionTrack::Run(void) } } + m_measured_cp_array.SetValid(true); + ftkPixelFormat frame_format = m_internals->m_frame_query->imageHeader->format; switch (frame_format) { @@ -975,6 +975,7 @@ bool mtsAtracsysFusionTrack::AddTool(const std::string & toolName, return true; } + std::string mtsAtracsysFusionTrack::GetToolName(const size_t index) const { ToolsType::const_iterator toolIterator = m_tools.begin(); @@ -988,15 +989,40 @@ std::string mtsAtracsysFusionTrack::GetToolName(const size_t index) const return toolIterator->first; } -bool mtsAtracsysFusionTrack::HardwareInitialized() const { + +bool mtsAtracsysFusionTrack::HardwareInitialized() const +{ return m_internals->m_configured; } -void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() { + +void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() +{ + ProcessTools(); + ProcessStrayMarkers(); +} + + +void mtsAtracsysFusionTrack::ProcessRGBStereoFrame() +{ + if (!m_internals->m_images_enabled) { + return; + } + + // Image frames + m_internals->ExtractLeftImage(m_left_image_raw); + m_internals->ExtractRightImage(m_right_image_raw); + m_left_image_raw.Valid() = true; + m_right_image_raw.Valid() = true; +} + + +void mtsAtracsysFusionTrack::ProcessTools() +{ // check results of last frame switch (m_internals->m_frame_query->markersStat) { case FTK_QS_NS::QS_WAR_SKIPPED: - CMN_LOG_CLASS_RUN_ERROR << "Run: marker fields in the frame are not set correctly" << std::endl; + CMN_LOG_CLASS_RUN_ERROR << "Run: marker field is not written" << std::endl; break; case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: CMN_LOG_CLASS_RUN_ERROR << "Run: frame.markersVersionSize is invalid" << std::endl; @@ -1051,7 +1077,7 @@ void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() { } } - // finalize all tools + // make pose relative to reference frame and finalize all tools for (iter = m_tools.begin(); iter != end; ++iter) { auto reference = iter->second->m_reference_measured_cp; if (reference == nullptr) { @@ -1066,19 +1092,23 @@ void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() { } iter->second->m_state_table.Advance(); } +} + +void mtsAtracsysFusionTrack::ProcessStrayMarkers() +{ // ---- 3D Fiducials, aka stray markers --- switch (m_internals->m_frame_query->threeDFiducialsStat) { case FTK_QS_NS::QS_WAR_SKIPPED: - //CMN_LOG_CLASS_RUN_ERROR << "Run: 3D status fields in the frame is not set correctly" << std::endl; - break; + CMN_LOG_CLASS_RUN_ERROR << "Run: 3D fiducials fields not written" << std::endl; + return; case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: CMN_LOG_CLASS_RUN_ERROR << "Run: frame.threeDFiducialsVersionSize is invalid" << std::endl; + return; + case FTK_QS_NS::QS_OK: break; default: - // CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; - break; - case FTK_QS_NS::QS_OK: + CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; break; } @@ -1086,21 +1116,9 @@ void mtsAtracsysFusionTrack::ProcessIRTrackingFrame() { m_measured_cp_array.Positions().resize(stray_count); for (uint32 m = 0; m < stray_count; m++) { - m_measured_cp_array.Positions().at(m).Translation().Assign(m_internals->m_stray_markers[m].positionMM.x * cmn_mm, - m_internals->m_stray_markers[m].positionMM.y * cmn_mm, - m_internals->m_stray_markers[m].positionMM.z * cmn_mm); + auto& marker_translation = m_measured_cp_array.Positions().at(m).Translation(); + marker_translation.Assign(m_internals->m_stray_markers[m].positionMM.x * cmn_mm, + m_internals->m_stray_markers[m].positionMM.y * cmn_mm, + m_internals->m_stray_markers[m].positionMM.z * cmn_mm); } } - - -void mtsAtracsysFusionTrack::ProcessRGBStereoFrame() { - if (!m_internals->m_images_enabled) { - return; - } - - // Image frames - m_internals->ExtractLeftImage(m_left_image_raw); - m_internals->ExtractRightImage(m_right_image_raw); - m_left_image_raw.Valid() = true; - m_right_image_raw.Valid() = true; -} diff --git a/core/components/code/mtsAtracsysStereo.cpp b/core/components/code/mtsAtracsysStereo.cpp index 44fa91f..cd9bbab 100644 --- a/core/components/code/mtsAtracsysStereo.cpp +++ b/core/components/code/mtsAtracsysStereo.cpp @@ -65,7 +65,7 @@ void mtsAtracsysStereo::Init(void) m_video_enabled = false; m_depth_enabled = false; - m_local_block_matching = false; + m_global_block_matching = false; m_filter_depth_map = false; } @@ -75,6 +75,8 @@ void mtsAtracsysStereo::InitStereoPipeline() mtsExecutionResult result; + /// Retrieve stereo calibration + result = m_get_left_camera_info(m_left_camera_info); if (!result.IsOK() || !m_left_camera_info.Valid()) { return; @@ -97,6 +99,8 @@ void mtsAtracsysStereo::InitStereoPipeline() return; } + /// Convert to OpenCV calibration format + cv::Mat left_intrinsic = cv::Mat(3, 3, CV_64F, m_left_camera_info.Intrinsic().Pointer()); cv::Mat right_intrinsic = cv::Mat(3, 3, CV_64F, m_right_camera_info.Intrinsic().Pointer()); cv::Mat left_distortion = cv::Mat(5, 1, CV_64F, m_left_camera_info.DistortionParameters().Pointer()); @@ -111,6 +115,8 @@ void mtsAtracsysStereo::InitStereoPipeline() cv::Mat left_rect, right_rect; cv::Mat left_proj, right_proj; + // Compute rectification maps and projection matrices + cv::stereoRectify( left_intrinsic, left_distortion, right_intrinsic, right_distortion, @@ -135,37 +141,44 @@ void mtsAtracsysStereo::InitStereoPipeline() m_right_undistort_map_x, m_right_undistort_map_y ); + /// Convert results to CISST data types + std::copy(left_rect.begin(), left_rect.end(), m_left_camera_info.Rectification().begin()); std::copy(right_rect.begin(), right_rect.end(), m_right_camera_info.Rectification().begin()); std::copy(left_proj.begin(), left_proj.end(), m_left_camera_info.Projection().begin()); std::copy(right_proj.begin(), right_proj.end(), m_right_camera_info.Projection().begin()); - if (m_local_block_matching) { - m_left_stereo_matcher = cv::StereoBM::create(384, 7); + /// Create stereo matcher(s) and depth map filters + + if (!m_global_block_matching) { + m_left_stereo_matcher = cv::StereoBM::create(784, 7); } else { - m_left_stereo_matcher = cv::StereoSGBM::create(0, 384, 7); + m_left_stereo_matcher = cv::StereoSGBM::create(0, 784, 7); } m_right_stereo_matcher = cv::ximgproc::createRightMatcher(m_left_stereo_matcher); m_wls_filter = cv::ximgproc::createDisparityWLSFilter(m_left_stereo_matcher); m_wls_filter->setLambda(8000.0); - m_wls_filter->setSigmaColor(1.0); + m_wls_filter->setSigmaColor(1.4); m_pipline_initialized = true; } void mtsAtracsysStereo::Rectify(prmImageFrame& raw, cv::Mat& rectified_mat, prmImageFrame& rectified, const cv::Mat& undistort_x, const cv::Mat& undistort_y) { + /// De-bayer image cv::Mat raw_bayer = cv::Mat(raw.Height(), raw.Width(), CV_8U, raw.Data().Pointer()); cv::Mat raw_rgb; cv::cvtColor(raw_bayer, raw_rgb, cv::COLOR_BayerGR2BGR_EA); + // Rectify cv::remap(raw_rgb, rectified_mat, undistort_x, undistort_y, cv::INTER_LINEAR); rectified.Width() = rectified_mat.cols; rectified.Height() = rectified_mat.rows; rectified.Channels() = rectified_mat.channels(); + // Convert to prmImageFrame size_t size = rectified_mat.cols * rectified_mat.rows * rectified_mat.channels(); rectified.Data().resize(size); std::copy(rectified_mat.data, rectified_mat.data + size, rectified.Data().begin()); @@ -173,13 +186,17 @@ void mtsAtracsysStereo::Rectify(prmImageFrame& raw, cv::Mat& rectified_mat, prmI void mtsAtracsysStereo::ComputeDepth() { + // Convert to gray-scale cv::Mat left_gray, right_gray; cv::cvtColor(m_left_image_mat, left_gray, cv::COLOR_RGB2GRAY); cv::cvtColor(m_right_image_mat, right_gray, cv::COLOR_RGB2GRAY); + // Compute left-to-right stereo correspondence cv::Mat left_disparity, right_disparity; m_left_stereo_matcher->compute(left_gray, right_gray, left_disparity); + // If filtering is enabled, compute the right-to-left correspondence + // and run WLS filter cv::Mat disparity; if (m_filter_depth_map) { m_right_stereo_matcher->compute(right_gray, left_gray, right_disparity); @@ -188,8 +205,10 @@ void mtsAtracsysStereo::ComputeDepth() disparity = left_disparity; } + // Disparity maps use fixed-point format, need to rescale by 1/16 to convert to floating point disparity.convertTo(disparity, CV_32F, 1.0f/16.0, 0.0f); + // Use stereo calibration to convert from disparity to depth values cv::Mat depth; cv::reprojectImageTo3D(disparity, depth, disparity_to_depth, true, -1); @@ -220,7 +239,6 @@ void mtsAtracsysStereo::Configure(const std::string & filename) exit(EXIT_FAILURE); } - // read JSON file passed as param, see configAtracsysFusionTrack.json for an example std::ifstream jsonStream; jsonStream.open(config_fullname.c_str()); @@ -239,11 +257,12 @@ void mtsAtracsysStereo::Configure(const std::string & filename) m_video_enabled = stereo.isMember("video") && video.isBool() && video.asBool(); const Json::Value depth = stereo["depth"]; m_depth_enabled = stereo.isMember("depth") && depth.isBool() && depth.asBool(); + // video processing is required for depth map computation m_video_enabled = m_depth_enabled || m_video_enabled; - const Json::Value local_bm = stereo["local_block_matching"]; - m_local_block_matching = stereo.isMember("local_block_matching") && local_bm.isBool() && local_bm.asBool(); + const Json::Value global_bm = stereo["global_block_matching"]; + m_global_block_matching = stereo.isMember("global_block_matching") && global_bm.isBool() && global_bm.asBool(); const Json::Value filter_depth = stereo["filter_depth_map"]; m_filter_depth_map = stereo.isMember("filter_depth_map") && filter_depth.isBool() && filter_depth.asBool(); diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h index 37d0c8c..37454db 100644 --- a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysFusionTrack.h @@ -109,6 +109,9 @@ class CISST_EXPORT mtsAtracsysFusionTrack: public mtsTaskContinuous void ProcessIRTrackingFrame(); void ProcessRGBStereoFrame(); + void ProcessTools(); + void ProcessStrayMarkers(); + /*! Search path for configuration files */ cmnPath m_path; diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h index 3e93a0b..62502f2 100644 --- a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h @@ -92,7 +92,7 @@ class CISST_EXPORT mtsAtracsysStereo: public mtsTaskContinuous bool m_video_enabled; bool m_depth_enabled; - bool m_local_block_matching; + bool m_global_block_matching; bool m_filter_depth_map; cv::Mat m_left_undistort_map_x, m_left_undistort_map_y; From 85e4cbac0b13100f170fdceec0fbb5900b660225 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Mon, 15 Jul 2024 17:03:43 -0400 Subject: [PATCH 09/12] Bridge crtk topics from stereo component, i.e. period_statistics --- ros/src/atracsys_bridge.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ros/src/atracsys_bridge.cpp b/ros/src/atracsys_bridge.cpp index 01d902e..0cdd32c 100644 --- a/ros/src/atracsys_bridge.cpp +++ b/ros/src/atracsys_bridge.cpp @@ -81,6 +81,10 @@ void atracsys_bridge::bridge_stereo(const std::string & _component_name, std::string clean_namespace = _ros_namespace; cisst_ral::clean_namespace(clean_namespace); + // bridge all CRTK-compatible topics from the controller + bridge_interface_provided(_component_name, _interface_name, clean_namespace, + m_ros_period, m_tf_period); + m_pub_bridge->AddPublisherFromCommandRead (_interface_name, "left/image_rect_color", clean_namespace + "/left/image_rect_color"); From 4ace9e3d9a271384257c75af1b0e0b92289a3125 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Wed, 17 Jul 2024 17:31:33 -0400 Subject: [PATCH 10/12] Make pointcloud color optional Only add colors to pointclouds if enabled in configuration, this helps speed up processing speed quite a bit in some circumstances. Also enables speckle filtering to clean up semi-global matching disparity, and replaces invalid values in the depth map with NaN. --- README.md | 17 ++++++--- core/components/code/mtsAtracsysStereo.cpp | 37 ++++++++++++++----- .../mtsAtracsysStereo.h | 1 + ros/src/atracsys_bridge.cpp | 2 +- 4 files changed, 41 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index 073e1a2..bd3d6f6 100644 --- a/README.md +++ b/README.md @@ -93,9 +93,10 @@ The JSON configuration files consists of a list of tools to track, and optionall stereo: { "video": true, // (default false) provide rectified video streams "depth": true, // (default false) compute 3D point cloud from stereo images - "filter_depth_map": true, // post-process depth map - improves quality at cost of some additional computation - "global_block_matching": true, // (default false) use (semi)global block matching algorithm instead of default local block matching - "num_dot_projectors": 3 // (default 0 if depth = false, 1 if depth = true) how many dot projector pairs to turn on - dot projectors add texture to images, increasing quality of stereo correspondence matching + "color_pointcloud": false, // (default false) add color to pointcloud output - enabling can slow down ROS publishing quite a bit + "filter_depth_map": false, // post-process depth map - can remove some noise, but may over-smooth regions + "global_block_matching": false, // (default false) use (semi)global block matching algorithm instead of default local block matching + "num_dot_projectors": 3 // (default 0 if depth = false, 1 if depth = true) how many dot projector pairs to turn on - dot projectors add texture to images, increasing quality of stereo correspondence matching. Valid range may depend on device model, but is 0 to 3 inclusive for the spryTrack 300. } } ``` @@ -163,11 +164,15 @@ transform: # Stereo processing -For devices such as the spryTrack 300 RGB which provide RGB images and have dot projectors, stereo processing is provided to compute a dense 3D point cloud. This can be enabled via the configuration file as described above. +For devices such as the spryTrack 300 RGB which provide RGB images and have dot projectors, stereo processing is provided to compute a dense 3D point cloud. This can be enabled via the configuration file as described above, which at minimum requires adding `"stereo": { "depth": true }` to the configuration file. -Enabling the global block matching option may improve depth map quality, however it will often slow down processing significantly +A good default configuration is to enabled depth map filtering, but keep global block matching disabled, this should provide a fairly good point cloud. The colored point cloud option can be enabled to make it easier to visualize the point cloud, but should otherwise be disabled since it degrades performance. -Enabling the depth map filtering algorithm will perform left-right consisteny checks to reject bad readings, and apply an edge-aware filtering algorithm to help reduce noise. This increases the computational cost of the stereo processing, particularly when the global block matching is enabled. +Enabling the global block matching option may improve depth map quality, however it will often slow down processing significantly. + +Enabling the depth map filtering algorithm will perform left-right consisteny checks to reject bad readings, and apply an edge-aware filtering algorithm to help reduce noise. This may help smooth the pointcloud in some situations, but the smoothing can also be detrimental so this option is disabled by default. Enabling this option will somewhat increase the computational cost of the stereo processing, particularly when the global block matching is enabled. + +Enabling the depth map filtering may also help to fill in holes in the point cloud. # Known issues, features to add diff --git a/core/components/code/mtsAtracsysStereo.cpp b/core/components/code/mtsAtracsysStereo.cpp index cd9bbab..324fc37 100644 --- a/core/components/code/mtsAtracsysStereo.cpp +++ b/core/components/code/mtsAtracsysStereo.cpp @@ -19,6 +19,8 @@ no warranty. The complete license can be found in license.txt and #include #include +#include + #include #include @@ -38,7 +40,7 @@ void mtsAtracsysStereo::Init(void) StateTable.AddData(m_right_image, "right_image"); StateTable.AddData(m_left_camera_info, "left_camera_info"); StateTable.AddData(m_right_camera_info, "right_camera_info"); - StateTable.AddData(m_depth, "depth"); + StateTable.AddData(m_depth, "pointcloud"); m_stereo_interface = AddInterfaceProvided("stereo"); if (m_stereo_interface) { @@ -46,7 +48,7 @@ void mtsAtracsysStereo::Init(void) m_stereo_interface->AddCommandReadState(StateTable, m_left_camera_info, "left/camera_info"); m_stereo_interface->AddCommandReadState(StateTable, m_right_image, "right/image_rect_color"); m_stereo_interface->AddCommandReadState(StateTable, m_right_camera_info, "right/camera_info"); - m_stereo_interface->AddCommandReadState(StateTable, m_depth, "depth"); + m_stereo_interface->AddCommandReadState(StateTable, m_depth, "pointcloud"); m_stereo_interface->AddCommandReadState(StateTable, StateTable.PeriodStats, "period_statistics"); } @@ -64,6 +66,7 @@ void mtsAtracsysStereo::Init(void) m_pipline_initialized = false; m_video_enabled = false; m_depth_enabled = false; + m_color_pointcloud = false; m_global_block_matching = false; m_filter_depth_map = false; @@ -150,16 +153,18 @@ void mtsAtracsysStereo::InitStereoPipeline() /// Create stereo matcher(s) and depth map filters + int block_size = 25; + int block_area = block_size * block_size; if (!m_global_block_matching) { - m_left_stereo_matcher = cv::StereoBM::create(784, 7); + m_left_stereo_matcher = cv::StereoBM::create(384, block_size); } else { - m_left_stereo_matcher = cv::StereoSGBM::create(0, 784, 7); + m_left_stereo_matcher = cv::StereoSGBM::create(0, 384, block_size, 8 * block_area, 28 * block_area, 0, 0, 10, 100, 1, cv::StereoSGBM::MODE_SGBM); } m_right_stereo_matcher = cv::ximgproc::createRightMatcher(m_left_stereo_matcher); m_wls_filter = cv::ximgproc::createDisparityWLSFilter(m_left_stereo_matcher); m_wls_filter->setLambda(8000.0); - m_wls_filter->setSigmaColor(1.4); + m_wls_filter->setSigmaColor(1.0); m_pipline_initialized = true; } @@ -205,13 +210,20 @@ void mtsAtracsysStereo::ComputeDepth() disparity = left_disparity; } - // Disparity maps use fixed-point format, need to rescale by 1/16 to convert to floating point - disparity.convertTo(disparity, CV_32F, 1.0f/16.0, 0.0f); + // Disparity maps use fixed-point format, need to rescale by 1/DISP_SCALE (1/16) to convert to floating point + disparity.convertTo(disparity, CV_32F, 1.0f/cv::StereoMatcher::DISP_SCALE, 0.0f); // Use stereo calibration to convert from disparity to depth values cv::Mat depth; cv::reprojectImageTo3D(disparity, depth, disparity_to_depth, true, -1); + float opencv_bigz = 10000.0f; + float invalid = std::numeric_limits::quiet_NaN(); + cv::Mat z; + cv::extractChannel(depth, z, 2); + cv::Mat mask = (z<=0.0f) | (z==opencv_bigz); + depth.setTo(cv::Scalar(invalid, invalid, invalid), mask); + m_depth.Width() = depth.cols; m_depth.Height() = depth.rows; @@ -220,8 +232,12 @@ void mtsAtracsysStereo::ComputeDepth() m_depth.Points().resize(size); std::copy(reinterpret_cast(depth.data), reinterpret_cast(depth.data) + size, m_depth.Points().begin()); - m_depth.Color().resize(m_left_image_mat.cols * m_left_image_mat.rows * m_left_image_mat.channels()); - std::copy(m_left_image_mat.data, m_left_image_mat.data + size, m_depth.Color().begin()); + if (m_color_pointcloud) { + m_depth.Color().resize(m_left_image_mat.cols * m_left_image_mat.rows * m_left_image_mat.channels()); + std::copy(m_left_image_mat.data, m_left_image_mat.data + size, m_depth.Color().begin()); + } else { + m_depth.Color().resize(0); + } } void mtsAtracsysStereo::Configure(const std::string & filename) @@ -261,6 +277,9 @@ void mtsAtracsysStereo::Configure(const std::string & filename) // video processing is required for depth map computation m_video_enabled = m_depth_enabled || m_video_enabled; + const Json::Value color_pointcloud = stereo["color_pointcloud"]; + m_color_pointcloud = stereo.isMember("color_pointcloud") && color_pointcloud.isBool() && color_pointcloud.asBool(); + const Json::Value global_bm = stereo["global_block_matching"]; m_global_block_matching = stereo.isMember("global_block_matching") && global_bm.isBool() && global_bm.asBool(); diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h index 62502f2..1b7630e 100644 --- a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h @@ -91,6 +91,7 @@ class CISST_EXPORT mtsAtracsysStereo: public mtsTaskContinuous bool m_pipline_initialized; bool m_video_enabled; bool m_depth_enabled; + bool m_color_pointcloud; bool m_global_block_matching; bool m_filter_depth_map; diff --git a/ros/src/atracsys_bridge.cpp b/ros/src/atracsys_bridge.cpp index 0cdd32c..c09eaf9 100644 --- a/ros/src/atracsys_bridge.cpp +++ b/ros/src/atracsys_bridge.cpp @@ -98,7 +98,7 @@ void atracsys_bridge::bridge_stereo(const std::string & _component_name, (_interface_name, "right/camera_info", clean_namespace + "/right/camera_info"); m_pub_bridge->AddPublisherFromCommandRead - (_interface_name, "depth", clean_namespace + "/depth"); + (_interface_name, "pointcloud", clean_namespace + "/pointcloud"); mtsManagerLocal* componentManager = mtsManagerLocal::GetInstance(); componentManager->Connect(m_pub_bridge->GetName(), _interface_name, _component_name, _interface_name); From 344ba11303f45f7cf4e8b26f6dc4952d1c1ae9ed Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Fri, 19 Jul 2024 17:36:29 -0400 Subject: [PATCH 11/12] Add config option for minimum point cloud depth --- README.md | 3 ++- .../code/mtsAtracsysFusionTrack.cpp | 15 ++++++++----- core/components/code/mtsAtracsysStereo.cpp | 22 ++++++++++++++----- .../mtsAtracsysStereo.h | 1 + 4 files changed, 30 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index bd3d6f6..a032628 100644 --- a/README.md +++ b/README.md @@ -96,7 +96,8 @@ The JSON configuration files consists of a list of tools to track, and optionall "color_pointcloud": false, // (default false) add color to pointcloud output - enabling can slow down ROS publishing quite a bit "filter_depth_map": false, // post-process depth map - can remove some noise, but may over-smooth regions "global_block_matching": false, // (default false) use (semi)global block matching algorithm instead of default local block matching - "num_dot_projectors": 3 // (default 0 if depth = false, 1 if depth = true) how many dot projector pairs to turn on - dot projectors add texture to images, increasing quality of stereo correspondence matching. Valid range may depend on device model, but is 0 to 3 inclusive for the spryTrack 300. + "num_dot_projectors": 3, // (default 0 if depth = false, 1 if depth = true) how many dot projector pairs to turn on - dot projectors add texture to images, increasing quality of stereo correspondence matching. Valid range may depend on device model, but is 0 to 3 inclusive for the spryTrack 300. + "min_depth": 1.0 // (default 1.35 meters) minimum depth (from camera) that will be measured. Due to stereo geometry, measuring closer distances results in reduced viewport. } } ``` diff --git a/core/components/code/mtsAtracsysFusionTrack.cpp b/core/components/code/mtsAtracsysFusionTrack.cpp index dd99f6e..4264973 100644 --- a/core/components/code/mtsAtracsysFusionTrack.cpp +++ b/core/components/code/mtsAtracsysFusionTrack.cpp @@ -267,7 +267,7 @@ class mtsAtracsysFusionTrackInternals } if (m_sn == 0) { - CMN_LOG_INIT_ERROR << "Configure: no Atracsys device connected" << std::endl; + CMN_LOG_INIT_ERROR << "Configure: no Atracsys devices found" << std::endl; return false; } @@ -617,8 +617,9 @@ void mtsAtracsysFusionTrack::Configure(const std::string & filename) bool ok = m_internals->Initialize(); if (!ok) { - CMN_LOG_INIT_ERROR << "Configure: no device connected (" + CMN_LOG_INIT_ERROR << "Configure: failed to initialize device (" << this->GetName() << ")" << std::endl; + return; } CMN_LOG_CLASS_INIT_VERBOSE << "Configure: found device SN " << m_internals->m_sn << std::endl; @@ -1100,15 +1101,19 @@ void mtsAtracsysFusionTrack::ProcessStrayMarkers() // ---- 3D Fiducials, aka stray markers --- switch (m_internals->m_frame_query->threeDFiducialsStat) { case FTK_QS_NS::QS_WAR_SKIPPED: - CMN_LOG_CLASS_RUN_ERROR << "Run: 3D fiducials fields not written" << std::endl; + CMN_LOG_CLASS_RUN_ERROR << "Stray markers: 3D fiducials fields not written" << std::endl; return; case FTK_QS_NS::QS_ERR_INVALID_RESERVED_SIZE: - CMN_LOG_CLASS_RUN_ERROR << "Run: frame.threeDFiducialsVersionSize is invalid" << std::endl; + CMN_LOG_CLASS_RUN_ERROR << "Stray markers: frame.threeDFiducialsVersionSize must be multiple of fiducial type size" << std::endl; return; + case FTK_QS_NS::QS_ERR_OVERFLOW: + CMN_LOG_CLASS_RUN_ERROR << "Stray markers: too many stray markers detected --- try covering metallic/reflective surfaces" << std::endl; + break; case FTK_QS_NS::QS_OK: break; default: - CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status" << std::endl; + int status = static_cast(m_internals->m_frame_query->threeDFiducialsStat); + CMN_LOG_CLASS_RUN_ERROR << "Run: invalid status " << status << std::endl; break; } diff --git a/core/components/code/mtsAtracsysStereo.cpp b/core/components/code/mtsAtracsysStereo.cpp index 324fc37..860a21e 100644 --- a/core/components/code/mtsAtracsysStereo.cpp +++ b/core/components/code/mtsAtracsysStereo.cpp @@ -153,12 +153,21 @@ void mtsAtracsysStereo::InitStereoPipeline() /// Create stereo matcher(s) and depth map filters + double fx = m_left_camera_info.Intrinsic().at(0, 0); + double b = translation.Norm(); + int max_disparity = std::ceil(fx*b/m_min_depth); + max_disparity = max_disparity - (max_disparity % 16); // required to be multiple of 16 + + // See OpenCV docs for parameter description int block_size = 25; int block_area = block_size * block_size; if (!m_global_block_matching) { - m_left_stereo_matcher = cv::StereoBM::create(384, block_size); + m_left_stereo_matcher = cv::StereoBM::create(max_disparity, block_size); } else { - m_left_stereo_matcher = cv::StereoSGBM::create(0, 384, block_size, 8 * block_area, 28 * block_area, 0, 0, 10, 100, 1, cv::StereoSGBM::MODE_SGBM); + m_left_stereo_matcher = cv::StereoSGBM::create(0, max_disparity, block_size, + 8*block_area, 32*block_area, + 0, 0, 20, + 50, 1, cv::StereoSGBM::MODE_SGBM); } m_right_stereo_matcher = cv::ximgproc::createRightMatcher(m_left_stereo_matcher); @@ -217,12 +226,12 @@ void mtsAtracsysStereo::ComputeDepth() cv::Mat depth; cv::reprojectImageTo3D(disparity, depth, disparity_to_depth, true, -1); - float opencv_bigz = 10000.0f; + float opencv_bigz = 10000.0f; // defined arbitrarily in OpenCV source code, unforunately not exposed float invalid = std::numeric_limits::quiet_NaN(); cv::Mat z; cv::extractChannel(depth, z, 2); - cv::Mat mask = (z<=0.0f) | (z==opencv_bigz); - depth.setTo(cv::Scalar(invalid, invalid, invalid), mask); + cv::Mat invalid_mask = (z<=0.0f) | (z==opencv_bigz); + depth.setTo(cv::Scalar(invalid, invalid, invalid), invalid_mask); m_depth.Width() = depth.cols; m_depth.Height() = depth.rows; @@ -285,6 +294,9 @@ void mtsAtracsysStereo::Configure(const std::string & filename) const Json::Value filter_depth = stereo["filter_depth_map"]; m_filter_depth_map = stereo.isMember("filter_depth_map") && filter_depth.isBool() && filter_depth.asBool(); + + const Json::Value min_depth = stereo["min_depth"]; + m_min_depth = (stereo.isMember("min_depth") && min_depth.isDouble()) ? min_depth.asDouble() : 1.35; } } diff --git a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h index 1b7630e..83fcb6a 100644 --- a/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h +++ b/core/components/include/sawAtracsysFusionTrack/mtsAtracsysStereo.h @@ -95,6 +95,7 @@ class CISST_EXPORT mtsAtracsysStereo: public mtsTaskContinuous bool m_global_block_matching; bool m_filter_depth_map; + double m_min_depth; cv::Mat m_left_undistort_map_x, m_left_undistort_map_y; cv::Mat m_right_undistort_map_x, m_right_undistort_map_y; From 44b81dda833517c0451137f89daf07f673a45f55 Mon Sep 17 00:00:00 2001 From: Brendan Burkhart Date: Fri, 19 Jul 2024 17:37:15 -0400 Subject: [PATCH 12/12] Make OpenCV dependency optional --- core/components/CMakeLists.txt | 22 +++++++++++++++++----- ros/CMakeLists.txt | 12 ++++++++---- ros/src/atracsys.cpp | 21 ++++++++++++++++----- 3 files changed, 41 insertions(+), 14 deletions(-) diff --git a/core/components/CMakeLists.txt b/core/components/CMakeLists.txt index 862d036..62e9f24 100755 --- a/core/components/CMakeLists.txt +++ b/core/components/CMakeLists.txt @@ -48,8 +48,6 @@ if (cisst_FOUND_AS_REQUIRED) endif () endif () - find_package(OpenCV REQUIRED) - # catkin/ROS paths cisst_set_output_path () @@ -88,17 +86,31 @@ if (cisst_FOUND_AS_REQUIRED) cisst_add_config_files (sawAtracsysFusionTrack) - add_library (sawAtracsysFusionTrack ${IS_SHARED} + set (sawAtracsysFusionTrack_SOURCES ${sawAtracsysFusionTrack_HEADER_DIR}/sawAtracsysFusionTrackExport.h code/mtsAtracsysFusionTrack.cpp - code/mtsAtracsysStereo.cpp ${sawAtracsysFusionTrack_HEADER_DIR}/mtsAtracsysFusionTrack.h - ${sawAtracsysFusionTrack_HEADER_DIR}/mtsAtracsysStereo.h) + ) + + find_package(OpenCV) + if (OpenCV_FOUND) + set (sawAtracsysFusionTrack_SOURCES ${sawAtracsysFusionTrack_SOURCES} + code/mtsAtracsysStereo.cpp + ${sawAtracsysFusionTrack_HEADER_DIR}/mtsAtracsysStereo.h + ) + endif (OpenCV_FOUND) + + add_library (sawAtracsysFusionTrack ${IS_SHARED} ${sawAtracsysFusionTrack_SOURCES}) set_target_properties (sawAtracsysFusionTrack PROPERTIES VERSION ${sawAtracsysFusionTrack_VERSION} FOLDER "sawAtracsysFusionTrack") cisst_target_link_libraries (sawAtracsysFusionTrack ${REQUIRED_CISST_LIBRARIES}) target_link_libraries (sawAtracsysFusionTrack ${AtracsysSDK_LIBRARIES}) + + if (OpenCV_FOUND) + target_link_libraries (sawAtracsysFusionTrack ${OpenCV_LIBS}) + endif (OpenCV_FOUND) + target_link_libraries (sawAtracsysFusionTrack ${OpenCV_LIBS}) configure_file ( diff --git a/ros/CMakeLists.txt b/ros/CMakeLists.txt index 25dd15c..eec6d7d 100644 --- a/ros/CMakeLists.txt +++ b/ros/CMakeLists.txt @@ -70,8 +70,6 @@ if (cisst_FOUND_AS_REQUIRED) # sawAtracsysFusionTrack has been compiled within cisst, we should find it automatically find_package (sawAtracsysFusionTrack 1.1.0) - find_package(OpenCV REQUIRED) - if (sawAtracsysFusionTrack_FOUND) # sawAtracsysFusionTrack configuration @@ -81,8 +79,14 @@ if (cisst_FOUND_AS_REQUIRED) add_executable (atracsys src/atracsys.cpp src/atracsys_bridge.cpp src/atracsys_bridge.h) target_link_libraries (atracsys ${sawAtracsysFusionTrack_LIBRARIES} - ${catkin_LIBRARIES} - ${OpenCV_LIBS}) + ${catkin_LIBRARIES}) + + find_package(OpenCV) + if (OpenCV_FOUND) + target_link_libraries (atracsys ${OpenCV_LIBS}) + endif (OpenCV_FOUND) + target_compile_definitions(atracsys PRIVATE OpenCV_AVAILABLE=${OpenCV_FOUND}) + cisst_target_link_libraries (atracsys ${REQUIRED_CISST_LIBRARIES}) install (TARGETS atracsys diff --git a/ros/src/atracsys.cpp b/ros/src/atracsys.cpp index c94079a..978370e 100644 --- a/ros/src/atracsys.cpp +++ b/ros/src/atracsys.cpp @@ -23,9 +23,12 @@ no warranty. The complete license can be found in license.txt and #include #include #include -#include #include +#if OpenCV_AVAILABLE +#include +#endif + #include "atracsys_bridge.h" #include @@ -39,7 +42,7 @@ int main(int argc, char * argv[]) cmnLogger::SetMaskFunction(CMN_LOG_ALLOW_ALL); cmnLogger::SetMaskDefaultLog(CMN_LOG_ALLOW_ALL); cmnLogger::SetMaskClassMatching("mtsAtracsysFusionTrack", CMN_LOG_ALLOW_ALL); - cmnLogger::SetMaskClassMatching("mtsAtracsysStereo", CMN_LOG_ALLOW_ALL); + cmnLogger::SetMaskClassMatching("mtsAtracsysSteasdsreo", CMN_LOG_ALLOW_ALL); cmnLogger::AddChannel(std::cerr, CMN_LOG_ALLOW_ERRORS_AND_WARNINGS); // create ROS node handle @@ -86,22 +89,30 @@ int main(int argc, char * argv[]) return -1; } - mtsAtracsysStereo * stereo = new mtsAtracsysStereo("stereo", tracker->GetName()); - stereo->Configure(jsonConfigFile); - // add the components to the component manager mtsManagerLocal * componentManager = mtsComponentManager::GetInstance(); componentManager->AddComponent(tracker); + +#if OpenCV_AVAILABLE + mtsAtracsysStereo * stereo = new mtsAtracsysStereo("stereo", tracker->GetName()); + stereo->Configure(jsonConfigFile); + componentManager->AddComponent(stereo); componentManager->Connect(tracker->GetName(), "StereoRaw", stereo->GetName(), "StereoRaw"); +#else + std::cout << "OpenCV not available, so no stereo processing" << std::endl; +#endif // ROS CRTK bridge atracsys_bridge * bridge = new atracsys_bridge("atracsys_bridge", rosNode, rosPeriod, tfPeriod); componentManager->AddComponent(bridge); bridge->bridge_controller(tracker->GetName(), "Controller"); + +#if OpenCV_AVAILABLE bridge->bridge_stereo(stereo->GetName(), "stereo", tracker->GetName() + "/stereo"); +#endif bridge->Connect();