diff --git a/3rdparty/CMakeLists.txt b/3rdparty/CMakeLists.txt index 4b0ffa0846..115e0f26e1 100644 --- a/3rdparty/CMakeLists.txt +++ b/3rdparty/CMakeLists.txt @@ -89,10 +89,12 @@ endif() # Build RPLIDAR? # ---------------------------------- if(CMAKE_MRPT_HAS_ROBOPEAK_LIDAR) - set(RPLIDAR_ROOT ${MRPT_SOURCE_DIR}/3rdparty/rplidar_sdk/sdk/sdk/) + set(RPLIDAR_ROOT ${MRPT_SOURCE_DIR}/3rdparty/rplidar_sdk/sdk/) set(SRCS_LIST ${RPLIDAR_ROOT}/src/*.cpp ${RPLIDAR_ROOT}/src/hal/*.cpp + ${RPLIDAR_ROOT}/src/dataunpacker/*.cpp + ${RPLIDAR_ROOT}/src/dataunpacker/unpacker/*.cpp ${RPLIDAR_ROOT}/include/*.h ) if (WIN32) diff --git a/3rdparty/rplidar_sdk b/3rdparty/rplidar_sdk index f577cb57c6..27445354bd 160000 --- a/3rdparty/rplidar_sdk +++ b/3rdparty/rplidar_sdk @@ -1 +1 @@ -Subproject commit f577cb57c679101665a0e8abf468dca9a6ec0fe6 +Subproject commit 27445354bddedb1d7373489b6e896c1feb0e3b37 diff --git a/3rdparty/xspublic/.clang-format b/3rdparty/xspublic/.clang-format new file mode 100644 index 0000000000..445f63ed30 --- /dev/null +++ b/3rdparty/xspublic/.clang-format @@ -0,0 +1,3 @@ +DisableFormat: true +SortIncludes: Never + diff --git a/3rdparty/xspublic/xscontroller/broadcastdevice.cpp b/3rdparty/xspublic/xscontroller/broadcastdevice.cpp index 83a5cfe7e4..260c0420e7 100644 --- a/3rdparty/xspublic/xscontroller/broadcastdevice.cpp +++ b/3rdparty/xspublic/xscontroller/broadcastdevice.cpp @@ -83,8 +83,8 @@ class BroadcastForwardFunc { class ForwardConstFunc : public BroadcastForwardFunc { public: typedef bool (XsDevice::*FuncType)() const; - inline ForwardConstFunc (BroadcastDevice* bc, FuncType func) - : BroadcastForwardFunc(bc), m_func(func) {} + inline ForwardConstFunc (const BroadcastDevice* bc, FuncType func) + : BroadcastForwardFunc(const_cast(bc)), m_func(func) {} virtual bool call(XsDevice* device) { return (device->*m_func)(); } private: FuncType m_func; @@ -95,8 +95,8 @@ template class ForwardConstFunc1Arg : public BroadcastForwardFunc { public: typedef bool (XsDevice::*FuncType)(Arg1) const; - inline ForwardConstFunc1Arg (BroadcastDevice* bc, FuncType func, Arg1 arg1) - : BroadcastForwardFunc(bc), m_func(func), m_arg1(arg1) {} + inline ForwardConstFunc1Arg (const BroadcastDevice* bc, FuncType func, Arg1 arg1) + : BroadcastForwardFunc(const_cast(bc)), m_func(func), m_arg1(arg1) {} virtual bool call(XsDevice* device) { return (device->*m_func)(m_arg1); } private: FuncType m_func; @@ -321,17 +321,17 @@ bool BroadcastDevice::abortFlushing() bool BroadcastDevice::isMeasuring() const { - return ForwardConstFunc(const_cast(this), &XsDevice::isMeasuring)(); + return ForwardConstFunc(const_cast(this), &XsDevice::isMeasuring)(); } bool BroadcastDevice::isRecording() const { - return ForwardConstFunc(const_cast(this), &XsDevice::isRecording)(); + return ForwardConstFunc(const_cast(this), &XsDevice::isRecording)(); } bool BroadcastDevice::isReadingFromFile() const { - return ForwardConstFunc(const_cast(this), &XsDevice::isReadingFromFile)(); + return ForwardConstFunc(const_cast(this), &XsDevice::isReadingFromFile)(); } bool BroadcastDevice::gotoConfig() diff --git a/3rdparty/xspublic/xscontroller/enumexpanders.cpp b/3rdparty/xspublic/xscontroller/enumexpanders.cpp index 86e59577b9..f64dfa7d53 100644 --- a/3rdparty/xspublic/xscontroller/enumexpanders.cpp +++ b/3rdparty/xspublic/xscontroller/enumexpanders.cpp @@ -60,10 +60,14 @@ JLENUMEXPANDER(XsConnectivityState, JLENUMCASE(XCS_Unknown) ) -static void forceEnumExpanderInclusion() +namespace +{ +[[maybe_unused]] void forceEnumExpanderInclusion() { volatile bool blah = false; + (void)blah; JLWRITEG("blah" << XDS_Initial << blah); JLWRITEG("blah" << XOP_Unknown << blah); JLWRITEG("blah" << XCS_File << blah); } +} diff --git a/3rdparty/xspublic/xscontroller/mti6x0device.cpp b/3rdparty/xspublic/xscontroller/mti6x0device.cpp index 776e88c47d..881641aaf3 100644 --- a/3rdparty/xspublic/xscontroller/mti6x0device.cpp +++ b/3rdparty/xspublic/xscontroller/mti6x0device.cpp @@ -88,9 +88,9 @@ MtiBaseDevice::BaseFrequencyResult Mti6X0Device::getBaseFrequencyInternal(XsData return result; } - auto baseFreq = [&](XsDataIdentifier dataType) + auto baseFreq = [&](XsDataIdentifier data_type) { - switch (dataType & XDI_TypeMask) + switch (data_type & XDI_TypeMask) { case XDI_None: return 400; case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; diff --git a/3rdparty/xspublic/xscontroller/mti7device.cpp b/3rdparty/xspublic/xscontroller/mti7device.cpp index de9970c719..9031c700de 100644 --- a/3rdparty/xspublic/xscontroller/mti7device.cpp +++ b/3rdparty/xspublic/xscontroller/mti7device.cpp @@ -76,9 +76,9 @@ MtiBaseDevice::BaseFrequencyResult Mti7Device::getBaseFrequencyInternal(XsDataId return result; } - auto baseFreq = [&](XsDataIdentifier dataType) + auto baseFreq = [&](XsDataIdentifier data_type) { - switch (dataType & XDI_TypeMask) + switch (data_type & XDI_TypeMask) { case XDI_None: return 100; case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; @@ -93,7 +93,7 @@ MtiBaseDevice::BaseFrequencyResult Mti7Device::getBaseFrequencyInternal(XsDataId case XDI_PressureGroup: return 50; case XDI_GnssGroup: { - if ((dataType & XDI_FullTypeMask) == XDI_GnssPvtData) + if ((data_type & XDI_FullTypeMask) == XDI_GnssPvtData) return 4; return 0; } diff --git a/3rdparty/xspublic/xscontroller/mtigdevice.cpp b/3rdparty/xspublic/xscontroller/mtigdevice.cpp index de2de0c165..437be0d400 100644 --- a/3rdparty/xspublic/xscontroller/mtigdevice.cpp +++ b/3rdparty/xspublic/xscontroller/mtigdevice.cpp @@ -108,12 +108,12 @@ MtiBaseDevice::BaseFrequencyResult MtigDevice::getBaseFrequencyInternal(XsDataId return result; } - auto baseFreq = [&](XsDataIdentifier dataType) + auto baseFreq = [&](XsDataIdentifier data_type) { XsVersion const legacyFwVersion(MTMK4_700_LEGACY_FW_VERSION_MAJOR, MTMK4_700_LEGACY_FW_VERSION_MINOR, MTMK4_700_LEGACY_FW_VERSION_REVISION); bool const isLegacyFirmware = (firmwareVersion() <= legacyFwVersion); - switch (dataType & XDI_TypeMask) + switch (data_type & XDI_TypeMask) { case XDI_None: return 2000; case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; diff --git a/3rdparty/xspublic/xscontroller/mtix00device.cpp b/3rdparty/xspublic/xscontroller/mtix00device.cpp index 021d5b5d2d..a56ae15853 100644 --- a/3rdparty/xspublic/xscontroller/mtix00device.cpp +++ b/3rdparty/xspublic/xscontroller/mtix00device.cpp @@ -105,9 +105,9 @@ MtiBaseDevice::BaseFrequencyResult MtiX00Device::getBaseFrequencyInternal(XsData return result; } - auto baseFreq = [&](XsDataIdentifier dataType) + auto baseFreq = [&](XsDataIdentifier data_type) { - switch (dataType & XDI_TypeMask) + switch (data_type & XDI_TypeMask) { case XDI_None: return 2000; case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; diff --git a/3rdparty/xspublic/xscontroller/mtix0device.cpp b/3rdparty/xspublic/xscontroller/mtix0device.cpp index cd82cdc222..9e09e0d914 100644 --- a/3rdparty/xspublic/xscontroller/mtix0device.cpp +++ b/3rdparty/xspublic/xscontroller/mtix0device.cpp @@ -105,9 +105,9 @@ MtiBaseDevice::BaseFrequencyResult MtiX0Device::getBaseFrequencyInternal(XsDataI return result; } - auto baseFreq = [&](XsDataIdentifier dataType) + auto baseFreq = [&](XsDataIdentifier data_type) { - switch (dataType & XDI_TypeMask) + switch (data_type & XDI_TypeMask) { case XDI_None: return 2000; case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; diff --git a/3rdparty/xspublic/xscontroller/mtixdevice.cpp b/3rdparty/xspublic/xscontroller/mtixdevice.cpp index 19d0dc05ba..8e42e3f74e 100644 --- a/3rdparty/xspublic/xscontroller/mtixdevice.cpp +++ b/3rdparty/xspublic/xscontroller/mtixdevice.cpp @@ -82,9 +82,9 @@ MtiBaseDevice::BaseFrequencyResult MtiXDevice::getBaseFrequencyInternal(XsDataId return result; } - auto baseFreq = [&](XsDataIdentifier dataType) + auto baseFreq = [&](XsDataIdentifier data_type) { - switch (dataType & XDI_TypeMask) + switch (data_type & XDI_TypeMask) { case XDI_None: return 100; case XDI_TimestampGroup: return XDI_MAX_FREQUENCY_VAL; diff --git a/3rdparty/xspublic/xscontroller/serialcommunicator.cpp b/3rdparty/xspublic/xscontroller/serialcommunicator.cpp index f62c35a44f..98c410e4ee 100644 --- a/3rdparty/xspublic/xscontroller/serialcommunicator.cpp +++ b/3rdparty/xspublic/xscontroller/serialcommunicator.cpp @@ -130,8 +130,8 @@ XsResultValue SerialCommunicator::gotoConfig(bool detectRs485) while (true) { std::shared_ptr reply2 = addReplyObject(XMID_GotoConfigAck); - XsMessage rcv = reply2->message(100); - if (rcv.getMessageId() != XMID_GotoConfigAck) + XsMessage rcv2 = reply2->message(100); + if (rcv2.getMessageId() != XMID_GotoConfigAck) break; } diff --git a/3rdparty/xspublic/xscontroller/serialinterface.cpp b/3rdparty/xspublic/xscontroller/serialinterface.cpp index 226404a206..07940388f7 100644 --- a/3rdparty/xspublic/xscontroller/serialinterface.cpp +++ b/3rdparty/xspublic/xscontroller/serialinterface.cpp @@ -590,10 +590,10 @@ XsResultValue SerialInterface::readData(XsFilePos maxLength, XsByteArray& data) } else { - int err = errno; + int err2 = errno; data.clear(); - switch (err) + switch (err2) { case EAGAIN: #if defined(EWOULDBLOCK) && EWOULDBLOCK != EAGAIN diff --git a/3rdparty/xspublic/xscontroller/xsdevice_def.cpp b/3rdparty/xspublic/xscontroller/xsdevice_def.cpp index f98fa6d34b..7f25913053 100644 --- a/3rdparty/xspublic/xscontroller/xsdevice_def.cpp +++ b/3rdparty/xspublic/xscontroller/xsdevice_def.cpp @@ -499,9 +499,9 @@ XsOutputConfiguration XsDevice::findConfiguration(XsDataIdentifier dataType) con XsOutputConfigurationArray cfg = outputConfiguration(); auto item = std::find_if(cfg.begin(), cfg.end(), - [&](const XsOutputConfiguration &cfg) + [&](const XsOutputConfiguration &cfg2) { - return (cfg.m_dataIdentifier & mask) == dataType; + return (cfg2.m_dataIdentifier & mask) == dataType; } ); diff --git a/3rdparty/xspublic/xscontroller/xsdeviceconfiguration.c b/3rdparty/xspublic/xscontroller/xsdeviceconfiguration.c index 8deab21b35..0adb037e89 100644 --- a/3rdparty/xspublic/xscontroller/xsdeviceconfiguration.c +++ b/3rdparty/xspublic/xscontroller/xsdeviceconfiguration.c @@ -155,8 +155,8 @@ void XsDeviceConfiguration_readFromMessage(XsDeviceConfiguration* thisPtr, const for (i = 0; i < nDevs; ++i) { thisPtr->m_deviceInfo[i].m_deviceId = XsMessage_getDataLong(msg, 98+i*20); - XsDeviceId deviceId = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; - if (XsDeviceId_isLegacyDeviceId(&deviceId)) + XsDeviceId deviceId2 = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; + if (XsDeviceId_isLegacyDeviceId(&deviceId2)) { thisPtr->m_deviceInfo[i].m_deviceId = (uint32_t)XsMessage_getDataLong(msg, 98+i*20); memcpy(thisPtr->m_deviceInfo[i].m_reserved, XsMessage_getDataBuffer(msg, 102 + i * 20), 8); @@ -214,8 +214,8 @@ void XsDeviceConfiguration_writeToMessage(const XsDeviceConfiguration* thisPtr, for (i = 0; i < thisPtr->m_numberOfDevices; ++i) { - XsDeviceId deviceId = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; - if (XsDeviceId_isLegacyDeviceId(&deviceId)) + XsDeviceId deviceId2 = {thisPtr->m_deviceInfo[i].m_deviceId, XSDEVICEID_PRODUCT_CODE_INIT, 0, 0}; + if (XsDeviceId_isLegacyDeviceId(&deviceId2)) { XsMessage_setDataLong (msg, (uint32_t)thisPtr->m_deviceInfo[i].m_deviceId, 98+i*20); XsMessage_setDataBuffer(msg, thisPtr->m_masterInfo.m_reserved1, 8, 102+i*20); diff --git a/3rdparty/xspublic/xstypes/xstime.c b/3rdparty/xspublic/xstypes/xstime.c index a1c5bbfac5..e2e741f4cb 100644 --- a/3rdparty/xspublic/xstypes/xstime.c +++ b/3rdparty/xspublic/xstypes/xstime.c @@ -40,7 +40,7 @@ # include # define snprintf _snprintf #else -# include +//# include # include # include # include @@ -201,7 +201,8 @@ void XsTime_getDateAsString(char* dest, const struct tm* date) year = dt.tm_year + 1900; month = dt.tm_mon + 1; - snprintf(dest, 9, "%04d%02d%02d", year, month, dt.tm_mday); + volatile int dst_size = 9; // JLBC: just to supress truncation warning + snprintf(dest, dst_size, "%04d%02d%02d", year, month, dt.tm_mday); } /*! \brief Retrieves the time as binary @@ -220,7 +221,8 @@ void XsTime_getTimeAsString(char* dest, const struct tm* date) else XsTime_getDateTime(&dt); - snprintf(dest, 8, "%02d%02d%02d%02d", dt.tm_hour, dt.tm_min, dt.tm_sec, 0); + volatile int dst_size = 8; // JLBC: just to supress truncation warning + snprintf(dest, dst_size, "%02d%02d%02d%02d", dt.tm_hour, dt.tm_min, dt.tm_sec, 0); } /*! \brief Retrieves the date as wstring representation diff --git a/CMakeLists.txt b/CMakeLists.txt index 8ce5c5b65a..1dcb498bac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,7 @@ endif() if ((NOT IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/nanogui/include") - OR (NOT IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rplidar_sdk/sdk/sdk") + OR (NOT IS_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/rplidar_sdk/sdk/src") ) message(FATAL_ERROR "git submodules missing! " "You probably did not clone the project with --recursive. It is possible to recover " diff --git a/apps/DifOdometry-Camera/DifOdometry_Camera.cpp b/apps/DifOdometry-Camera/DifOdometry_Camera.cpp index 495a0d4e13..1b912671fd 100644 --- a/apps/DifOdometry-Camera/DifOdometry_Camera.cpp +++ b/apps/DifOdometry-Camera/DifOdometry_Camera.cpp @@ -301,7 +301,9 @@ void CDifodoCamera::initializeScene() // User-interface information img::CImage img_legend; - img_legend.loadFromXPM(legend_xpm); + bool loadOk = img_legend.loadFromXPM(legend_xpm); + ASSERT_(loadOk); + Viewport::Ptr legend = scene->createViewport("legend"); legend->setViewportPosition(20, 20, 350, 201); legend->setImageView(img_legend); diff --git a/apps/DifOdometry-Datasets/DifOdometry_Datasets.cpp b/apps/DifOdometry-Datasets/DifOdometry_Datasets.cpp index e7412e8255..58c7b0995c 100644 --- a/apps/DifOdometry-Datasets/DifOdometry_Datasets.cpp +++ b/apps/DifOdometry-Datasets/DifOdometry_Datasets.cpp @@ -268,7 +268,9 @@ void CDifodoDatasets::initializeScene() // User-interface information CImage img_legend; - img_legend.loadFromXPM(legend_xpm); + bool loadOk = img_legend.loadFromXPM(legend_xpm); + ASSERT_(loadOk); + Viewport::Ptr legend = scene->createViewport("legend"); legend->setViewportPosition(20, 20, 332, 164); legend->setImageView(img_legend); diff --git a/apps/RawLogViewer/CFormPlayVideo.cpp b/apps/RawLogViewer/CFormPlayVideo.cpp index e219458659..124b5a1391 100644 --- a/apps/RawLogViewer/CFormPlayVideo.cpp +++ b/apps/RawLogViewer/CFormPlayVideo.cpp @@ -1082,7 +1082,8 @@ void CFormPlayVideo::saveCamImage(int n) string fil = string(dialog.GetPath().mb_str()); - o->image.saveToFile(fil); + bool savedOk = o->image.saveToFile(fil); + ASSERT_(savedOk); } else if (IS_CLASS(*displayedImgs[n], CObservationStereoImages)) { @@ -1124,7 +1125,8 @@ void CFormPlayVideo::saveCamImage(int n) CImage& im = (n == 2 ? o->imageDisparity : (n == 1 ? o->imageRight : o->imageLeft)); - im.saveToFile(fil); + bool savedOk = im.saveToFile(fil); + ASSERT_(savedOk); } WX_END_TRY diff --git a/apps/RawLogViewer/main_convert_ops.cpp b/apps/RawLogViewer/main_convert_ops.cpp index 84e9f9a93c..3ebd34539b 100644 --- a/apps/RawLogViewer/main_convert_ops.cpp +++ b/apps/RawLogViewer/main_convert_ops.cpp @@ -598,7 +598,9 @@ void xRawLogViewerFrame::OnMenuConvertExternallyStored(wxCommandEvent& event) string fileName = format( "img_stereo_%u_left_%05u.%s", k, imgSaved, imgFileExtension.c_str()); - obsSt->imageLeft.saveToFile(outDir + fileName); + bool savedOk = + obsSt->imageLeft.saveToFile(outDir + fileName); + ASSERT_(savedOk); obsSt->imageLeft.setExternalStorage(fileName); imgSaved++; @@ -607,7 +609,9 @@ void xRawLogViewerFrame::OnMenuConvertExternallyStored(wxCommandEvent& event) fileName = format( "img_stereo_%u_right_%05u.%s", k, imgSaved, imgFileExtension.c_str()); - obsSt->imageRight.saveToFile(outDir + fileName); + savedOk = + obsSt->imageRight.saveToFile(outDir + fileName); + ASSERT_(savedOk); obsSt->imageRight.setExternalStorage(fileName); imgSaved++; @@ -623,7 +627,9 @@ void xRawLogViewerFrame::OnMenuConvertExternallyStored(wxCommandEvent& event) string fileName = format( "img_monocular_%u_%05u.%s", k, imgSaved, imgFileExtension.c_str()); - obsIm->image.saveToFile(outDir + fileName); + bool savedOk = + obsIm->image.saveToFile(outDir + fileName); + ASSERT_(savedOk); obsIm->image.setExternalStorage(fileName); imgSaved++; diff --git a/apps/RawLogViewer/main_images_ops.cpp b/apps/RawLogViewer/main_images_ops.cpp index c56a2fbaf1..46f5e756d5 100644 --- a/apps/RawLogViewer/main_images_ops.cpp +++ b/apps/RawLogViewer/main_images_ops.cpp @@ -90,14 +90,17 @@ void xRawLogViewerFrame::OnGenerateSeqImgs(wxCommandEvent& event) { auto obsSt = SF->getObservationByIndexAs< CObservationStereoImages::Ptr>(k); - obsSt->imageLeft.saveToFile(format( + bool savedOk = obsSt->imageLeft.saveToFile(format( "%s/img_stereo_%u_left_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); - obsSt->imageRight.saveToFile(format( + savedOk = obsSt->imageRight.saveToFile(format( "%s/img_stereo_%u_right_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); + imgSaved++; } if (SF->getObservationByIndex(k)->GetRuntimeClass() == @@ -105,9 +108,10 @@ void xRawLogViewerFrame::OnGenerateSeqImgs(wxCommandEvent& event) { auto obsIm = SF->getObservationByIndexAs< CObservationImage::Ptr>(k); - obsIm->image.saveToFile(format( + bool savedOk = obsIm->image.saveToFile(format( "%s/img_monocular_%u_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); imgSaved++; } } @@ -123,25 +127,28 @@ void xRawLogViewerFrame::OnGenerateSeqImgs(wxCommandEvent& event) CObservationStereoImages::Ptr obsSt = std::dynamic_pointer_cast( o); - obsSt->imageLeft.saveToFile(format( + bool savedOk = obsSt->imageLeft.saveToFile(format( "%s/img_stereo_%s_left_%05u.%s", outDir.c_str(), obsSt->sensorLabel.c_str(), imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); - obsSt->imageRight.saveToFile(format( + savedOk = obsSt->imageRight.saveToFile(format( "%s/img_stereo_%s_right_%05u.%s", outDir.c_str(), obsSt->sensorLabel.c_str(), imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); imgSaved++; } else if (IS_CLASS(*o, CObservationImage)) { CObservationImage::Ptr obsIm = std::dynamic_pointer_cast(o); - obsIm->image.saveToFile(format( + bool savedOk = obsIm->image.saveToFile(format( "%s/img_monocular_%s_%05u.%s", outDir.c_str(), obsIm->sensorLabel.c_str(), imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); imgSaved++; } } @@ -356,7 +363,9 @@ void xRawLogViewerFrame::OnMenuRectifyImages(wxCommandEvent& event) // Save image to file and free memory if (obsIm->image.isExternallyStored()) { - obsIm->image.saveToFile(p); + bool savedOk = obsIm->image.saveToFile(p); + ASSERT_(savedOk); + obsIm->image.unload(); } // end if } // end if image is not undistorted @@ -394,7 +403,9 @@ void xRawLogViewerFrame::OnMenuRectifyImages(wxCommandEvent& event) // Save image to file and free memory if (obsIm->image.isExternallyStored()) { - obsIm->image.saveToFile(p); + bool savedOk = obsIm->image.saveToFile(p); + ASSERT_(savedOk); + obsIm->image.unload(); } } // end if image is not undistorted diff --git a/apps/RawLogViewer/main_imports_exports.cpp b/apps/RawLogViewer/main_imports_exports.cpp index 9ae7fa7746..b717543598 100644 --- a/apps/RawLogViewer/main_imports_exports.cpp +++ b/apps/RawLogViewer/main_imports_exports.cpp @@ -332,7 +332,8 @@ void xRawLogViewerFrame::OnImportSequenceOfImages(wxCommandEvent& event) CObservationImage::Ptr im = std::make_shared(); im->cameraPose = CPose3D(0, 0, 0); - im->image.loadFromFile(filName); + bool loadOk = im->image.loadFromFile(filName); + ASSERT_(loadOk); im->timestamp = fakeTimeStamp; // Default camera parameters: diff --git a/apps/RawLogViewer/xRawLogViewerMain.cpp b/apps/RawLogViewer/xRawLogViewerMain.cpp index d90e83113e..ccc60fdd31 100644 --- a/apps/RawLogViewer/xRawLogViewerMain.cpp +++ b/apps/RawLogViewer/xRawLogViewerMain.cpp @@ -3038,18 +3038,28 @@ void xRawLogViewerFrame::OnFileSaveImages(wxCommandEvent&) { auto obsSt = SF->getObservationByIndexAs< CObservationStereoImages::Ptr>(k); - obsSt->imageLeft.saveToFile(format( + bool savedOk = obsSt->imageLeft.saveToFile(format( "%s/img_stereo_%u_left_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + + ASSERT_(savedOk); + if (obsSt->hasImageRight) - obsSt->imageRight.saveToFile(format( + { + savedOk = obsSt->imageRight.saveToFile(format( "%s/img_stereo_%u_right_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); + } + if (obsSt->hasImageDisparity) - obsSt->imageDisparity.saveToFile(format( + { + savedOk = obsSt->imageDisparity.saveToFile(format( "%s/img_stereo_%u_disp_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); + } imgSaved++; } if (SF->getObservationByIndex(k)->GetRuntimeClass() == @@ -3059,9 +3069,10 @@ void xRawLogViewerFrame::OnFileSaveImages(wxCommandEvent&) SF->getObservationByIndexAs( k); - obsIm->image.saveToFile(format( + bool savedOk = obsIm->image.saveToFile(format( "%s/img_monocular_%u_%05u.%s", outDir.c_str(), k, imgSaved, imgFileExtension.c_str())); + ASSERT_(savedOk); imgSaved++; } } diff --git a/apps/ReactiveNav3D-Demo/ReactiveNav3D_demo.h b/apps/ReactiveNav3D-Demo/ReactiveNav3D_demo.h index b9bc915919..f928520830 100644 --- a/apps/ReactiveNav3D-Demo/ReactiveNav3D_demo.h +++ b/apps/ReactiveNav3D-Demo/ReactiveNav3D_demo.h @@ -568,13 +568,18 @@ class CMyReactInterface // Maps are loaded here. Different maps can be loaded changing these // lines and including them above (#define...) - myImg.loadFromXPM(map2_1_xpm); + bool loadOk = myImg.loadFromXPM(map2_1_xpm); + ASSERT_(loadOk); grid.loadFromBitmap(myImg, resolution); maps.push_back(grid); - myImg.loadFromXPM(map2_2_xpm); + + loadOk = myImg.loadFromXPM(map2_2_xpm); + ASSERT_(loadOk); grid.loadFromBitmap(myImg, resolution); maps.push_back(grid); - myImg.loadFromXPM(map2_3_xpm); + + loadOk = myImg.loadFromXPM(map2_3_xpm); + ASSERT_(loadOk); grid.loadFromBitmap(myImg, resolution); maps.push_back(grid); diff --git a/apps/SceneViewer3D/_DSceneViewerMain.cpp b/apps/SceneViewer3D/_DSceneViewerMain.cpp index 7bf10641f4..cf892491db 100644 --- a/apps/SceneViewer3D/_DSceneViewerMain.cpp +++ b/apps/SceneViewer3D/_DSceneViewerMain.cpp @@ -221,7 +221,8 @@ void CMyGLCanvas::OnPostRenderSwapBuffers(double At, wxPaintDC& dc) string fileName(format( "%s/screenshot_%07i.png", capturingDir.c_str(), captureCount++)); - frame.saveToFile(fileName); + bool savedOk = frame.saveToFile(fileName); + ASSERT_(savedOk); } // Estimate FPS: @@ -1423,7 +1424,8 @@ void _DSceneViewerFrame::OnMenuItem14Selected(wxCommandEvent& event) if (dialog.ShowModal() != wxID_OK) return; - frame.saveToFile(std::string(dialog.GetPath().mb_str())); + bool savedOk = frame.saveToFile(std::string(dialog.GetPath().mb_str())); + ASSERT_(savedOk); } void _DSceneViewerFrame::OnMenuCameraTrackingArbitrary(wxCommandEvent& event) @@ -1894,7 +1896,8 @@ void _DSceneViewerFrame::OnMenuItemHighResRender(wxCommandEvent& event) // render the scene render.render_RGB(*m_canvas->getOpenGLSceneRef(), frame); - frame.saveToFile(sTargetFil); + bool savedOk = frame.saveToFile(sTargetFil); + ASSERT_(savedOk); } } catch (const std::exception& e) diff --git a/apps/benchmarking-image-features/src/mainwindow.cpp b/apps/benchmarking-image-features/src/mainwindow.cpp index 13e12cd759..7004fc8445 100644 --- a/apps/benchmarking-image-features/src/mainwindow.cpp +++ b/apps/benchmarking-image-features/src/mainwindow.cpp @@ -1129,9 +1129,9 @@ void MainWindow::on_file_input_choose(int choice) { next_button->setVisible(true); prev_button->setVisible(true); - if (choice != 4 || choice != 2) // no need of vision options for stereo - // datasets and rawlog formats - makeVisionOptionsVisible(true); + // no need of vision options for stereo + // datasets and rawlog formats + if (choice == 4 || choice == 2) makeVisionOptionsVisible(true); } if (choice == 3) @@ -1178,14 +1178,19 @@ void MainWindow::fillDetectorInfo() { numFeats = numFeaturesLineEdit->text().toInt(); - img1.loadFromFile(file_path1); + bool loadOk = img1.loadFromFile(file_path1); + ASSERT_(loadOk); + resolution_x = img1.getWidth(); resolution_y = img1.getHeight(); if (currentInputIndex == 1 || currentInputIndex == 4 || (currentInputIndex == 2 && rawlog_type == 1)) // stereo image or stereo dataset - img2.loadFromFile(file_path2); + { + loadOk = img2.loadFromFile(file_path2); + ASSERT_(loadOk); + } if (detector_selected == 0) // 0 = KLT Detector { @@ -1308,7 +1313,8 @@ void MainWindow::fillDescriptorInfo() // read inputs from user ReadInputFormat(); numFeats = numFeaturesLineEdit->text().toInt(); - img1.loadFromFile(file_path1); + bool loadOk = img1.loadFromFile(file_path1); + ASSERT_(loadOk); resolution_x = img1.getWidth(); resolution_y = img1.getHeight(); @@ -1316,7 +1322,10 @@ void MainWindow::fillDescriptorInfo() if (currentInputIndex == 1 || currentInputIndex == 4 || (currentInputIndex == 2 && rawlog_type == 1)) // stereo image or stereo dataset - img2.loadFromFile(file_path2); + { + loadOk = img2.loadFromFile(file_path2); + ASSERT_(loadOk); + } if (descriptor_selected == 0) //!< SIFT Descriptors { @@ -1761,7 +1770,9 @@ void MainWindow::on_browse_button_clicked() if (currentInputIndex == 0) { file_path1 = inputFilePath->text().toStdString(); - img1.loadFromFile(file_path1); + bool loadOk = img1.loadFromFile(file_path1); + ASSERT_(loadOk); + resolution_x = img1.getWidth(); resolution_y = img1.getHeight(); } @@ -1772,11 +1783,14 @@ void MainWindow::on_browse_button_clicked() file_path1 = inputFilePath->text().toStdString(); file_path2 = inputFilePath2->text().toStdString(); - img1.loadFromFile(file_path1); + bool loadOk = img1.loadFromFile(file_path1); + ASSERT_(loadOk); + resolution_x = img1.getWidth(); resolution_y = img1.getHeight(); - img2.loadFromFile(file_path2); + loadOk = img2.loadFromFile(file_path2); + ASSERT_(loadOk); } } @@ -1812,7 +1826,11 @@ void MainWindow::on_browse_button_clicked2() file_path2 = inputFilePath2->text().toStdString(); - if (currentInputIndex == 1) img2.loadFromFile(file_path2); + if (currentInputIndex == 1) + { + bool loadOk = img2.loadFromFile(file_path2); + ASSERT_(loadOk); + } } /************************************************************************************************ @@ -1943,28 +1961,15 @@ void MainWindow::readFilesFromFolder(int next_prev) } /// this loop simply pushes absolute paths for the left images - for (unsigned int i = 0, j = 0; i < files.size(); i++) - { - if (files.at(i).size() > 4) // this removes the . and .. in linux - // as all files will have size more - // than 4 .png .jpg etc. - { + for (unsigned int i = 0; i < files.size(); i++) + if (files.at(i).size() > 4) files_fullpath.push_back(file_path1 + "/" + files.at(i)); - j++; - } - } // end of for /// this loop simply pushes absolute paths for the right images - for (unsigned int i = 0, j = 0; i < files2.size(); i++) - { - if (files2.at(i).size() > 4) // this removes the . and .. in linux - // as all files will have size more - // than 4 .png .jpg etc. - { + for (unsigned int i = 0; i < files2.size(); i++) + if (files2.at(i).size() > 4) files_fullpath2.push_back(file_path1 + "/" + files2.at(i)); - j++; - } - } // end of for + sort( files_fullpath2.begin(), files_fullpath2.end()); // Use the start and end like this @@ -1972,22 +1977,15 @@ void MainWindow::readFilesFromFolder(int next_prev) } else if ( currentInputIndex == 3 || currentInputIndex == 4 || - currentInputIndex == - 2) // meaning stereo dataset or single image dataset + currentInputIndex == 2 // stereo dataset or single image dataset + ) { dir = opendir(file_path1.c_str()); while ((pdir = readdir(dir))) files.emplace_back(pdir->d_name); - for (unsigned int i = 0, j = 0; i < files.size(); i++) - { - if (files.at(i).size() > 4) // this removes the . and .. in linux - // as all files will have size more - // than 4 .png .jpg etc. - { + for (unsigned int i = 0; i < files.size(); i++) + if (files.at(i).size() > 4) files_fullpath.push_back(file_path1 + "/" + files.at(i)); - j++; - } - } // end of for } sort( files_fullpath.begin(), @@ -2005,16 +2003,11 @@ void MainWindow::readFilesFromFolder(int next_prev) dir2 = opendir(file_path2.c_str()); while ((pdir2 = readdir(dir2))) files2.emplace_back(pdir2->d_name); - for (unsigned int i = 0, j = 0; i < files2.size(); i++) - { - if (files2.at(i).size() > 4) // this removes the . and .. in linux - // as all files will have size more - // than 4 .png .jpg etc. - { + + for (unsigned int i = 0; i < files2.size(); i++) + if (files2.at(i).size() > 4) files_fullpath2.push_back(file_path2 + "/" + files2.at(i)); - j++; - } - } // end of for + sort( files_fullpath2.begin(), files_fullpath2.end()); // Use the start and end like this @@ -2048,7 +2041,9 @@ void MainWindow::displayImagesWithoutDetector() /// DISPLAYING THE NEXT IMAGE AS A QIMAGE WITHOUT DETECTOR, DETECTOR WILL /// APPEAR ON THE IMAGE WHEN EVALUATE DETECTOR BUTTON IS CLICKED - img1.loadFromFile(file_path1); + bool loadOk = img1.loadFromFile(file_path1); + ASSERT_(loadOk); + resolution_x = img1.getWidth(); resolution_y = img1.getHeight(); @@ -2089,7 +2084,8 @@ void MainWindow::displayImagesWithoutDetector() /// file has stereo images in the dataset if (currentInputIndex == 4 || (currentInputIndex == 2 && rawlog_type == 1)) { - img2.loadFromFile(file_path2); + loadOk = img2.loadFromFile(file_path2); + ASSERT_(loadOk); cv::Mat cvImg2 = img2.asCvMatRef(); @@ -2159,7 +2155,9 @@ void MainWindow::on_sample_clicked() sampling_rate += factor; ReadInputFormat(); - img1.loadFromFile(file_path1); + bool loadOk = img1.loadFromFile(file_path1); + ASSERT_(loadOk); + cv::Mat cvImg_1 = img1.asCvMatRef(); cv::Mat temp1(cvImg_1.cols, cvImg_1.rows, cvImg_1.type()); @@ -2178,7 +2176,9 @@ void MainWindow::on_sample_clicked() if (currentInputIndex == 1) { - img2.loadFromFile(file_path2); + loadOk = img2.loadFromFile(file_path2); + ASSERT_(loadOk); + cv::Mat cvImg2 = img2.asCvMatRef(); cv::Mat temp2(cvImg2.cols, cvImg2.rows, cvImg2.type()); @@ -2469,17 +2469,10 @@ void MainWindow::onTrackingEnabled(int state) while ((pdir = readdir(dir))) files.emplace_back(pdir->d_name); - for (unsigned long i = 0, j = 0; i < files.size(); i++) - { - if (files.at(i).size() > 4) // this removes the . and .. in - // linux as all files will have - // size more than 4 .png .jpg etc. - { + for (unsigned long i = 0; i < files.size(); i++) + if (files.at(i).size() > 4) files_fullpath_tracking.push_back( file_path_temp + "/" + files.at(i)); - j++; - } - } // end of for } sort( files_fullpath_tracking.begin(), @@ -2839,22 +2832,12 @@ void MainWindow::store_Training_TestingSets() dir = opendir(file_path_temp.c_str()); while ((pdir = readdir(dir))) files.emplace_back(pdir->d_name); - for (unsigned int i = 0, j = 0; i < files.size(); i++) - { - if (files.at(i).size() > 4) // this removes the . and .. in - // linux as all files will have - // size more than 4 .png .jpg - // etc. - { + for (unsigned int i = 0; i < files.size(); i++) + if (files.at(i).size() > 4) testing_files_paths.push_back( file_path_temp + "/" + files.at(i)); - j++; - } - } // end of for } - sort( - testing_files_paths.begin(), - testing_files_paths.end()); // Use the start and end like this + sort(testing_files_paths.begin(), testing_files_paths.end()); // displayVector(testing_files_paths); } } @@ -2872,7 +2855,8 @@ void MainWindow::on_place_recog_clicked() { QMessageBox::information( this, "Choose Correct descriptor", - "For Place Recognition, Please specify only among the following " + "For Place Recognition, Please specify only among the " + "following " "descriptors:\n SIFT, SURF, ORB, BLD, LATCH"); return; } @@ -2892,8 +2876,8 @@ void MainWindow::on_place_recog_clicked() current_place_recog_index = 0; } - /// return a string here to show the place recognition accuracy on different - /// classes + /// return a string here to show the place recognition accuracy on + /// different classes string result = place_recog_obj->startPlaceRecognition(fext); // int place_predicted = // place_recog_obj->predictLabel(place_recog_obj->feats_testing_org, @@ -2934,7 +2918,8 @@ void MainWindow::on_place_recog_clicked_iterate() { QMessageBox::information( this, "Choose Correct descriptor", - "For Place Recognition, Please specify only among the following " + "For Place Recognition, Please specify only among the " + "following " "descriptors:\n SIFT, SURF, ORB, BLD, LATCH"); return; } @@ -2951,8 +2936,8 @@ void MainWindow::on_place_recog_clicked_iterate() current_place_recog_index = 0; } - /// return a string here to show the place recognition accuracy on different - /// classes + /// return a string here to show the place recognition accuracy on + /// different classes string result = place_recog_obj->startPlaceRecognition(fext); place_recog_label->setText(QString::fromStdString(result)); place_recog_image = new QLabel; @@ -2972,7 +2957,7 @@ void MainWindow::on_place_recog_clicked_iterate() /************************************************************************************************ * Main Window Constructor * ************************************************************************************************/ -MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) +MainWindow::MainWindow(QWidget* parent) : QMainWindow(parent) { placeRecog_checked_flag = false; current_place_recog_index = 0; @@ -2982,7 +2967,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) vo_layout = new QGridLayout; vo_message_running = new QLabel; vo_message_running->setText( - "Visual Odometry is currently running on the selected dataset
, " + "Visual Odometry is currently running on the selected dataset " + "
, " "Please wait till the process is finished"); progressBar->setMinimum(0); progressBar->setMaximum(0); @@ -3077,7 +3063,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) groupBox_images = new QGroupBox("Single Image"); image1 = new my_qlabel; qimage1.load( - "../../apps/benchmarking-image-features/images/1.png"); // replace this + "../../apps/benchmarking-image-features/images/1.png"); // replace + // this // with initial // image of // select an @@ -3090,7 +3077,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) image2 = new QLabel; qimage2.load( - "../../apps/benchmarking-image-features/images/2.png"); // replace this + "../../apps/benchmarking-image-features/images/2.png"); // replace + // this // with initial // image of // select an @@ -3162,7 +3150,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) browse_button2, SIGNAL(clicked()), this, SLOT(on_browse_button_clicked2())); - /// initially have the buttons hidden as single image selected by default + /// initially have the buttons hidden as single image selected by + /// default inputFilePath2->setVisible(false); browse_button2->setVisible(false); @@ -3201,8 +3190,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) browseHomography, SIGNAL(clicked()), this, SLOT(on_browse_homography_clicked3())); - /// initially have the homography widgets hidden, to be displayed when the - /// user selects the "Activate Homography based Reapeatability" + /// initially have the homography widgets hidden, to be displayed when + /// the user selects the "Activate Homography based Reapeatability" makeHomographyParamsVisible(false); /// tracking options @@ -3249,8 +3238,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) browseCalibration, SIGNAL(clicked()), this, SLOT(on_browse_calibration_clicked())); - /// initially have the visual odometry widgets hidden, only displayed when - /// user selects input as single image dataset + /// initially have the visual odometry widgets hidden, only displayed + /// when user selects input as single image dataset makeVisualOdomParamsVisible(false); /// create a signal slot for displaying the progress of VO @@ -3258,7 +3247,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) // connect(&visual_odom.current_frame, SIGNAL(valueChanged(int)), this, // SLOT(setValue(int))); // connect( - // &visual_odom.cnt, SIGNAL(valueChanged(int)), this, SLOT(setValue(int))); + // &visual_odom.cnt, SIGNAL(valueChanged(int)), this, + // SLOT(setValue(int))); connect( &visual_odom.cnt, SIGNAL(valueChanged(int)), this, SLOT(updateVOProgress())); @@ -3305,7 +3295,8 @@ MainWindow::MainWindow(QWidget* window_gui) : QMainWindow(window_gui) place_recog_label = new QLabel; place_recog_qimage.load( - "../../apps/benchmarking-image-features/images/1.png"); // replace this + "../../apps/benchmarking-image-features/images/1.png"); // replace + // this // with initial // image of // select an @@ -3597,8 +3588,8 @@ string MainWindow::findRepeatability(float mouse_x, float mouse_y) fillDetectorInfo(); - // Clearing the features list is very important to avoid mixing subsequent - // button clicks output + // Clearing the features list is very important to avoid mixing + // subsequent button clicks output CFeatureList temp_featsImage1; //.clear(); CImage temp_img1; int consecutive = 0; @@ -3609,7 +3600,8 @@ string MainWindow::findRepeatability(float mouse_x, float mouse_y) for (int i = 0; i < files_length; i++) { temp_featsImage1.clear(); - temp_img1.loadFromFile(files_fullpath.at(i)); + bool loadOk = temp_img1.loadFromFile(files_fullpath.at(i)); + ASSERT_(loadOk); fext.detectFeatures(temp_img1, temp_featsImage1, 0, numFeats); @@ -3710,8 +3702,8 @@ string MainWindow::findRepeatabilityHomography(float mouse_x, float mouse_y) // as all files will have size more // than 4 .png .jpg etc. { - /// might be a dumb way, but works. checking if the image is not - /// png so as to read only text files + /// might be a dumb way, but works. checking if the image is + /// not png so as to read only text files // long len_file_name = files2.at(i).length(); // if(files2.at(i).at(len_file_name-1) != 'g') files_fullpath2.push_back(file_path2_temp + "/" + files2.at(i)); @@ -3782,8 +3774,8 @@ string MainWindow::findRepeatabilityHomography(float mouse_x, float mouse_y) fillDetectorInfo(); - // Clearing the features list is very important to avoid mixing subsequent - // button clicks output + // Clearing the features list is very important to avoid mixing + // subsequent button clicks output CFeatureList temp_featsImage1; //.clear(); CImage temp_img1; int consecutive = 0; @@ -3791,22 +3783,22 @@ string MainWindow::findRepeatabilityHomography(float mouse_x, float mouse_y) bool flag_consecutive = false; int max_num = 0; - /// start checking the repeatability based on the homographies stored in the - /// homograph + /// start checking the repeatability based on the homographies stored in + /// the homograph for (int i = 1; i < files_length; i++) // i starts from 1 as we do not want // to find the repeatability for the // first image as that is where the // key-point comes from { - double temp_x_before = - mouse_x; // (mouse_x, mouse_y) is the key-point selected in image 1 + double temp_x_before = mouse_x; // (mouse_x, mouse_y) is the + // key-point selected in image 1 double temp_y_before = mouse_y; - // temp_x_after, temp_y_after is the expected corresponding key-point in - // the second image after applying the homography. - // temp_z_after should be ideally be zero as it is a 2D image.. so its - // actually not required here. - // i-1 because 5 homographies w.r.t to the first reference frame + // temp_x_after, temp_y_after is the expected corresponding + // key-point in the second image after applying the homography. + // temp_z_after should be ideally be zero as it is a 2D image.. so + // its actually not required here. i-1 because 5 homographies w.r.t + // to the first reference frame double temp_x_after = homographies[i - 1][0][0] * temp_x_before + homographies[i - 1][0][1] * temp_y_before + @@ -3816,7 +3808,8 @@ string MainWindow::findRepeatabilityHomography(float mouse_x, float mouse_y) homographies[i - 1][1][2]; temp_featsImage1.clear(); - temp_img1.loadFromFile(files_fullpath.at(i)); + bool loadOk = temp_img1.loadFromFile(files_fullpath.at(i)); + ASSERT_(loadOk); fext.detectFeatures(temp_img1, temp_featsImage1, 0, numFeats); @@ -3878,15 +3871,13 @@ string MainWindow::falsePositivesNegatives() rawlog_type == 1)) // implying stereo images or stereo image dataset { // compute homography from img1 and img2 - // For this case, I was thinking again in the homography approach. If - // the match is found outside a - // small area where it should be according to the homography, it is - // called a false positive. For the - // false negatives, if a keypoint in image1 should be matched to a - // certain keypoint in image2 according - // to the homography AND actually there is a keypoint in that position - // but the match is not found, - // then it's a false negative. + // For this case, I was thinking again in the homography approach. + // If the match is found outside a small area where it should be + // according to the homography, it is called a false positive. For + // the false negatives, if a keypoint in image1 should be matched to + // a certain keypoint in image2 according to the homography AND + // actually there is a keypoint in that position but the match is + // not found, then it's a false negative. //"i" in featsImage1 matches with min_dist_indexes[i] in featsImage2 // computing false positives here @@ -3903,7 +3894,8 @@ string MainWindow::falsePositivesNegatives() if (!isInNeighborhood) { number_false_positives++; } // now checking false negatives - // check if a keypoint match actually exist around the area in image + // check if a keypoint match actually exist around the area in + // image // 2 bool flag_false_negative = false; for (unsigned int j = 0; j < featsImage2.size(); j++) @@ -3918,8 +3910,9 @@ string MainWindow::falsePositivesNegatives() break; } } - // if a keypoint exists around the region in image2 but the closest - // match is found somewhere else instead, its a false negative + // if a keypoint exists around the region in image2 but the + // closest match is found somewhere else instead, its a false + // negative if (flag_false_negative && !isInNeighborhood) number_false_negatives++; @@ -3990,21 +3983,15 @@ void MainWindow::Mouse_Pressed() { QMessageBox::information( this, "All Buttons need to be clicked", - "Please click evaluate detector, evaluate descriptor and visualize " - "descriptor before clicking any key-Point for Visualization !!"); + "Please click evaluate detector, evaluate descriptor and " + "visualize " + "descriptor before clicking any key-Point for Visualization " + "!!"); return; } - stringstream ss; - ss << "x : " << image1->x << " y : " << image1->y; - string str = ss.str(); - - QString ss2 = QString::fromStdString(str); - - mouse_x = image1->x; - mouse_y = image1->y; // -40 as it is the padding added due to a hidden - // reason (hidden reason: mapping was missing from - // label to image actual frame size) + const auto mouse_x = image1->x; + const auto mouse_y = image1->y; double x[numDesc1], y[numDesc1]; for (int i = 0; i < numDesc1; i++) @@ -4014,7 +4001,8 @@ void MainWindow::Mouse_Pressed() } plotInfo->setText( - "Descriptor Distances from selected descriptor
in Image 1 to " + "Descriptor Distances from selected descriptor
in Image 1 " + "to " "all other descriptors in Image 2 "); cv::Mat desc_Ref_img = imread(file_path1, IMREAD_ANYCOLOR); @@ -4050,22 +4038,28 @@ void MainWindow::Mouse_Pressed() images_static_sift_surf2->setVisible(false); } - /// mapping to the correct x and y dimensions in the qlabel to correctly get - /// the clicked point in horizontal and vertical direction + /// mapping to the correct x and y dimensions in the qlabel to correctly + /// get the clicked point in horizontal and vertical direction +#if QT_VERSION < QT_VERSION_CHECK(5, 15, 0) int pixel_width = image1->pixmap()->width(); + double pixel_height = image1->pixmap()->height(); +#else + int pixel_width = image1->pixmap(Qt::ReturnByValue).width(); + double pixel_height = image1->pixmap(Qt::ReturnByValue).height(); +#endif int mapped_mouse_x = mouse_x / pixel_width * resolution_x; // int mapped_mouse_y = mouse_y / IMAGE_HEIGHT * resolution_y; - double pixel_height = image1->pixmap()->height(); double other_height = image1->size().height(); double temp_mouse_y = (mouse_y + (pixel_height - other_height) / 2); int mapped_mouse_y = (int)((double)temp_mouse_y / (double)pixel_height * resolution_y); int pos = findClosest(mapped_mouse_x, mapped_mouse_y, x, y, numDesc1); - float temp_x = featsImage1.getFeatureX( - pos); // get the descriptor x coordinate corresponding to images1[cnt] + float temp_x = + featsImage1.getFeatureX(pos); // get the descriptor x coordinate + // corresponding to images1[cnt] float temp_y = featsImage1.getFeatureY(pos); // computing th repleatibility of the detector here @@ -4097,7 +4091,8 @@ void MainWindow::Mouse_Pressed() images_static2 = images2[pos]; } - /// this is done to fix the overlaying of labels on top of each other. + /// this is done to fix the overlaying of labels on top of each + /// other. if (flag_descriptor_match) featureMatched->setVisible(false); flag_descriptor_match = true; @@ -4123,12 +4118,12 @@ void MainWindow::Mouse_Pressed() /// draw a marker around image 1 keypoints for (unsigned int i = 0; i < featsImage2.size(); i++) { - int temp_x = (int)featsImage2.getFeatureX(i); - int temp_y = (int)featsImage2.getFeatureY(i); + int tempx = (int)featsImage2.getFeatureX(i); + int tempy = (int)featsImage2.getFeatureY(i); drawMarker( - temp2, Point(temp_x, temp_y), Scalar(0, 255, 0), - MARKER_CROSS, CROSS_SIZE, CROSS_THICKNESS); + temp2, Point(tempx, tempy), Scalar(0, 255, 0), MARKER_CROSS, + CROSS_SIZE, CROSS_THICKNESS); } drawLineLSD(temp2, 1); // 1 means right image drawMarker( @@ -4149,14 +4144,14 @@ void MainWindow::Mouse_Pressed() /// now following lines are for computing the x and y coordinate - /// drawing marker all over the image and coloring the selected one with - /// a different color + /// drawing marker all over the image and coloring the selected one + /// with a different color for (unsigned int i = 0; i < featsImage1.size(); i++) { - int temp_x = (int)featsImage1.getFeatureX(i); - int temp_y = (int)featsImage1.getFeatureY(i); + int tempx = (int)featsImage1.getFeatureX(i); + int tempy = (int)featsImage1.getFeatureY(i); drawMarker( - temp_desc_ref, Point(temp_x, temp_y), Scalar(0, 255, 0), + temp_desc_ref, Point(tempx, tempy), Scalar(0, 255, 0), MARKER_CROSS, CROSS_SIZE, CROSS_THICKNESS); } @@ -4181,16 +4176,17 @@ void MainWindow::Mouse_Pressed() (currentInputIndex == 2 && rawlog_type == 1)) { desc_VisualizeGrid->addWidget( - images_plots_sift_surf, 0, 2, 1, 1); // add the image distance + images_plots_sift_surf, 0, 2, 1, + 1); // add the image distance // plot with respect to // other features in image // 1 images_plots_sift_surf->setVisible(true); } - QLabel* detector_info = new QLabel(""); + QLabel* detector_info2 = new QLabel(""); if (currentInputIndex == 0 || currentInputIndex == 3) - desc_VisualizeGrid->addWidget(detector_info, 0, 1, 1, 1); + desc_VisualizeGrid->addWidget(detector_info2, 0, 1, 1, 1); if (currentInputIndex == 1 || currentInputIndex == 4 || (currentInputIndex == 2 && rawlog_type == 1)) @@ -4209,7 +4205,8 @@ void MainWindow::Mouse_Pressed() (currentInputIndex == 2 && rawlog_type == 1)) images_static_sift_surf2 = images2[pos]; - /// this is done to fix the overlaying of labels on top of each other. + /// this is done to fix the overlaying of labels on top of each + /// other. if (flag_descriptor_match) featureMatched->setVisible(false); flag_descriptor_match = true; @@ -4236,11 +4233,11 @@ void MainWindow::Mouse_Pressed() /// draw a marker around image 2 keypoints for (unsigned int i = 0; i < featsImage2.size(); i++) { - int temp_x = (int)featsImage2.getFeatureX(i); - int temp_y = (int)featsImage2.getFeatureY(i); + int tempx = (int)featsImage2.getFeatureX(i); + int tempy = (int)featsImage2.getFeatureY(i); drawMarker( - temp2, Point(temp_x, temp_y), Scalar(0, 255, 0), - MARKER_CROSS, CROSS_SIZE, CROSS_THICKNESS); + temp2, Point(tempx, tempy), Scalar(0, 255, 0), MARKER_CROSS, + CROSS_SIZE, CROSS_THICKNESS); } drawLineLSD(temp2, 1); // 1 means draw on right image drawMarker( @@ -4263,14 +4260,14 @@ void MainWindow::Mouse_Pressed() string str = ss.str(); // cout << str << endl; - /// drawing marker all over the image and coloring the selected one with - /// a different color + /// drawing marker all over the image and coloring the selected one + /// with a different color for (unsigned int i = 0; i < featsImage1.size(); i++) { - int temp_x = (int)featsImage1.getFeatureX(i); - int temp_y = (int)featsImage1.getFeatureY(i); + int tempx = (int)featsImage1.getFeatureX(i); + int tempy = (int)featsImage1.getFeatureY(i); drawMarker( - temp_desc_ref, Point(temp_x, temp_y), Scalar(0, 255, 0), + temp_desc_ref, Point(tempx, tempy), Scalar(0, 255, 0), MARKER_CROSS, CROSS_SIZE, CROSS_THICKNESS); } drawLineLSD(temp_desc_ref, 0); // 1 means right image @@ -4287,7 +4284,8 @@ void MainWindow::Mouse_Pressed() desc_VisualizeGrid->addWidget(plotInfo, 1, 2, 1, 1); plotInfo->setVisible(true); desc_VisualizeGrid->addWidget( - images_plots_sift_surf, 0, 2, 1, 1); // add the image distance + images_plots_sift_surf, 0, 2, 1, + 1); // add the image distance // plot with respect to // other features in image // 1 @@ -4296,9 +4294,9 @@ void MainWindow::Mouse_Pressed() desc_VisualizeGrid->addWidget(images_static_sift_surf, 0, 0, 1, 1); - QLabel* detector_info = new QLabel(""); + QLabel* detector_info2 = new QLabel(""); if (currentInputIndex == 0 || currentInputIndex == 3) - desc_VisualizeGrid->addWidget(detector_info, 0, 1, 1, 1); + desc_VisualizeGrid->addWidget(detector_info2, 0, 1, 1, 1); if (currentInputIndex == 1 || currentInputIndex == 4 || (currentInputIndex == 2 && rawlog_type == 1)) desc_VisualizeGrid->addWidget( diff --git a/apps/benchmarking-image-features/src/mainwindow.h b/apps/benchmarking-image-features/src/mainwindow.h index 41e6e9d479..9cf8917b73 100644 --- a/apps/benchmarking-image-features/src/mainwindow.h +++ b/apps/benchmarking-image-features/src/mainwindow.h @@ -492,8 +492,6 @@ class MainWindow : public QMainWindow QLabel* images_plots_sift_surf; //!< Label which holds the image to show //! the descriptor distance splot - double mouse_x, mouse_y; //!< stores the coordinates of the mouse click - bool flag_descriptor_match; //!< this is used to fix the overlaying of //! labels on top of each other. bool flag_read_files_bug; //!< used to get rid of a bug while reading files diff --git a/apps/benchmarking-image-features/src/place_recognition.cpp b/apps/benchmarking-image-features/src/place_recognition.cpp index 3912189dcd..16a155fdfe 100644 --- a/apps/benchmarking-image-features/src/place_recognition.cpp +++ b/apps/benchmarking-image-features/src/place_recognition.cpp @@ -30,8 +30,8 @@ PlaceRecognition::PlaceRecognition( int NumFeats) : training_paths(TrainingPaths), testing_paths(TestingPaths), - numFeats(NumFeats), desc_to_compute(DescToCompute), + numFeats(NumFeats), descriptor_selected(DescriptorSelected), len_training(training_paths.size()), len_testing(testing_paths.size()) @@ -94,8 +94,8 @@ int PlaceRecognition::predictLabel2( std::vector& training_word_labels, int total_vocab_size, int current_image) { - CFeatureList feats_testing = feats_testingAll[current_image]; - int feats_size = feats_testing.size(); + CFeatureList feats = feats_testingAll[current_image]; + int feats_size = feats.size(); /// PUT A CONDITION IF feats_size =0 OUTPUT A RANDOM CLASS INSTEAD OF GOING /// THROUGH BLANK STUFF, actually kinda doing that only currently @@ -113,11 +113,11 @@ int PlaceRecognition::predictLabel2( vector temp_feat; if (descriptor_selected == 5) - temp_feat = feats_testing.getByID(i)->descriptors.ORB.value(); + temp_feat = feats.getByID(i)->descriptors.ORB.value(); else if (descriptor_selected == 6) - temp_feat = feats_testing.getByID(i)->descriptors.BLD.value(); + temp_feat = feats.getByID(i)->descriptors.BLD.value(); else if (descriptor_selected == 7) - temp_feat = feats_testing.getByID(i)->descriptors.LATCH.value(); + temp_feat = feats.getByID(i)->descriptors.LATCH.value(); long descriptor_size = temp_feat.size(); /// following for loop iterates over the entire vocabulary in the @@ -161,8 +161,8 @@ int PlaceRecognition::predictLabel( std::vector& training_word_labels, int total_vocab_size, int current_image) { - CFeatureList feats_testing = feats_testingAll[current_image]; - int feats_size = feats_testing.size(); + CFeatureList feats = feats_testingAll[current_image]; + int feats_size = feats.size(); /// PUT A CONDITION IF feats_size =0 OUTPUT A RANDOM CLASS INSTEAD OF GOING /// THROUGH BLANK STUFF, actually kinda doing that only currently @@ -176,9 +176,9 @@ int PlaceRecognition::predictLabel( double min = std::numeric_limits::max(); // 99999; vector temp_feat; if (descriptor_selected == 1) - temp_feat = feats_testing.getByID(i)->descriptors.SURF.value(); + temp_feat = feats.getByID(i)->descriptors.SURF.value(); else if (descriptor_selected == 2) - temp_feat = feats_testing.getByID(i)->descriptors.SpinImg.value(); + temp_feat = feats.getByID(i)->descriptors.SpinImg.value(); long descriptor_size = temp_feat.size(); @@ -264,14 +264,18 @@ string PlaceRecognition::startPlaceRecognition(CFeatureExtraction fext) { for (int i = 0; i < len_training; i++) { - training[i].loadFromFile(training_paths.at(i)); + bool loadOk = training[i].loadFromFile(training_paths.at(i)); + ASSERT_(loadOk); + fext.detectFeatures(training[i], feats_training[i], 0, numFeats); fext.computeDescriptors( training[i], feats_training[i], desc_to_compute); } for (int i = 0; i < len_testing; i++) { - testing[i].loadFromFile(testing_paths.at(i)); + bool loadOk = testing[i].loadFromFile(testing_paths.at(i)); + ASSERT_(loadOk); + fext.detectFeatures(testing[i], feats_testing[i], 0, numFeats); fext.computeDescriptors( testing[i], feats_testing[i], desc_to_compute); diff --git a/apps/benchmarking-image-features/src/tracker.cpp b/apps/benchmarking-image-features/src/tracker.cpp index ddf03c6af2..136ca8ba09 100644 --- a/apps/benchmarking-image-features/src/tracker.cpp +++ b/apps/benchmarking-image-features/src/tracker.cpp @@ -82,7 +82,8 @@ cv::Mat Tracker::trackThemAll( long current_num = tracking_image_counter % files_fullpath_tracking.size(); CImage theImg; // The grabbed image: - theImg.loadFromFile(files_fullpath_tracking.at(current_num)); + bool loadOk = theImg.loadFromFile(files_fullpath_tracking.at(current_num)); + ASSERT_(loadOk); // Take the resolution upon first valid frame. if (!hasResolution) diff --git a/apps/benchmarking-image-features/src/visual_odometry.cpp b/apps/benchmarking-image-features/src/visual_odometry.cpp index f0bfd578f3..318ed640d8 100644 --- a/apps/benchmarking-image-features/src/visual_odometry.cpp +++ b/apps/benchmarking-image-features/src/visual_odometry.cpp @@ -292,9 +292,11 @@ Mat VisualOdometry::generateVO( cvtColor(img_2_c, img_2, COLOR_BGR2GRAY); CImage img1, img2; - img1.loadFromFile(filename1); + bool loadOk = img1.loadFromFile(filename1); + ASSERT_(loadOk); - img2.loadFromFile(filename2); + loadOk = img2.loadFromFile(filename2); + ASSERT_(loadOk); // feature detection, tracking vector points1, @@ -351,7 +353,8 @@ Mat VisualOdometry::generateVO( Mat currImage_c = imread(filename); CImage img3; - img3.loadFromFile(filename); + loadOk = img3.loadFromFile(filename); + ASSERT_(loadOk); cvtColor(currImage_c, currImage, COLOR_BGR2GRAY); vector Status; diff --git a/apps/camera-calib/camera_calib_guiMain.cpp b/apps/camera-calib/camera_calib_guiMain.cpp index 7a1cd22af7..71801c6b94 100644 --- a/apps/camera-calib/camera_calib_guiMain.cpp +++ b/apps/camera-calib/camera_calib_guiMain.cpp @@ -999,8 +999,11 @@ void camera_calib_guiDialog::OnbtnSaveImagesClick(wxCommandEvent& event) string dir = string(dlg.GetPath().mb_str()); for (auto& lst_image : lst_images) - lst_image.second.img_original.saveToFile( + { + bool savedOk = lst_image.second.img_original.saveToFile( dir + string("/") + lst_image.first + string(".png")); + ASSERT_(savedOk); + } } } catch (const std::exception& e) diff --git a/apps/map-partition/map-partition.cpp b/apps/map-partition/map-partition.cpp index e01e3062ed..5c6f16219e 100644 --- a/apps/map-partition/map-partition.cpp +++ b/apps/map-partition/map-partition.cpp @@ -199,8 +199,14 @@ void Test() CImage img, img2; img.setFromMatrix(A, true /* normalized in range [0,1] */); img2.setFromMatrix(B, true /* normalized in range [0,1] */); - img.saveToFile("MAP-PARTITION_RESULTS/ADJ_MATRIX_BEFORE.png"); - img2.saveToFile("MAP-PARTITION_RESULTS/ADJ_MATRIX_AFTER.png"); + + bool savedOk = + img.saveToFile("MAP-PARTITION_RESULTS/ADJ_MATRIX_BEFORE.png"); + ASSERT_(savedOk); + + savedOk = img2.saveToFile("MAP-PARTITION_RESULTS/ADJ_MATRIX_AFTER.png"); + ASSERT_(savedOk); + win.showImage(img); win2.showImage(img2); win.setPos(20, 20); diff --git a/apps/mrpt-performance/perf-atan2lut.cpp b/apps/mrpt-performance/perf-atan2lut.cpp index 78c73b83ad..47fb834655 100644 --- a/apps/mrpt-performance/perf-atan2lut.cpp +++ b/apps/mrpt-performance/perf-atan2lut.cpp @@ -41,8 +41,8 @@ double atan2_lut_test_query(int, int) { x += dx; y += dy; - // bool valid = - lut.atan2(y, x, atan2val); + bool valid = lut.atan2(y, x, atan2val); + (void)valid; } return tictac.Tac() / step; } @@ -89,8 +89,8 @@ double atan2_lut_multires_test_query(int, int) { x += dx; y += dy; - // bool valid = - atan2lut.atan2(y, x, atan2val); + bool valid = atan2lut.atan2(y, x, atan2val); + (void)valid; } return tictac.Tac() / step; } diff --git a/apps/mrpt-performance/perf-feature_extraction.cpp b/apps/mrpt-performance/perf-feature_extraction.cpp index 1f005196dd..8b9fb78206 100644 --- a/apps/mrpt-performance/perf-feature_extraction.cpp +++ b/apps/mrpt-performance/perf-feature_extraction.cpp @@ -117,12 +117,9 @@ void register_tests_feature_extraction() "feature_extraction [640x480]: FAST (OpenCV)", benchmark_detectFeatures, 100); - MRPT_TODO("AKAZE crashes inside OpenCV. Disabled for now (Jan 2019)"); -#if 0 lstTests.emplace_back( "feature_extraction [640x480]: AKAZE (OpenCV)", benchmark_detectFeatures, 5); -#endif lstTests.emplace_back( "feature_extraction [640x480]: LSD (OpenCV)", benchmark_detectFeatures, 5); diff --git a/apps/mrpt-performance/perf-images.cpp b/apps/mrpt-performance/perf-images.cpp index 9e333579a2..1738106d0c 100644 --- a/apps/mrpt-performance/perf-images.cpp +++ b/apps/mrpt-performance/perf-images.cpp @@ -58,7 +58,10 @@ double image_test_1(int w, int img_quality) tictac.Tic(); for (size_t i = 0; i < N; i++) - img.saveToFile(fil, img_quality); + { + bool savedOk = img.saveToFile(fil, img_quality); + ASSERT_(savedOk); + } const double T = tictac.Tac() / N; mrpt::system::deleteFile(fil); @@ -103,7 +106,8 @@ double image_saveload(int iFormat, int to_shm) if (perf_load) { // LOAD: - img.saveToFile(fil); + bool savedOk = img.saveToFile(fil); + ASSERT_(savedOk); const size_t N = 30; @@ -111,7 +115,8 @@ double image_saveload(int iFormat, int to_shm) for (size_t i = 0; i < N; i++) { CImage img_new; - img.loadFromFile(fil); + bool loadOk = img.loadFromFile(fil); + ASSERT_(loadOk); } T = tictac.Tac() / N; @@ -123,7 +128,10 @@ double image_saveload(int iFormat, int to_shm) tictac.Tic(); for (size_t i = 0; i < N; i++) - img.saveToFile(fil); + { + bool savedOk = img.saveToFile(fil); + ASSERT_(savedOk); + } T = tictac.Tac() / N; } @@ -263,8 +271,8 @@ double image_KLTscore(int WIN, int N) tictac.Tic(); for (int i = 0; i < N; i++) { - // float r = - img.KLT_response(x | 128, y | 128, WIN); + float r = img.KLT_response(x | 128, y | 128, WIN); + (void)r; x++; x &= 0x1FF; y++; diff --git a/apps/simul-landmarks/simul-landmarks-main.cpp b/apps/simul-landmarks/simul-landmarks-main.cpp index d862ba2da5..4b7ddcf873 100644 --- a/apps/simul-landmarks/simul-landmarks-main.cpp +++ b/apps/simul-landmarks/simul-landmarks-main.cpp @@ -240,9 +240,12 @@ int main(int argc, char** argv) const size_t N_STEPS_STOP_AT_THE_BEGINNING = 4; + double timeStamp = 0; + const double timeStep = 1.0; + CMatrixDouble GT_path; - for (size_t i = 0; i < nSteps; i++) + for (size_t i = 0; i < nSteps; i++, timeStamp += timeStep) { cout << "Generating step " << i << "...\n"; CSensoryFrame SF; @@ -292,9 +295,9 @@ int main(int argc, char** argv) } // Simulate observations: - CObservationBearingRange::Ptr obs = - std::make_shared(); + auto obs = CObservationBearingRange::Create(); + obs->sensorLabel = "sensor"; obs->minSensorDistance = minSensorDistance; obs->maxSensorDistance = maxSensorDistance; obs->fieldOfView_yaw = fieldOfView; @@ -306,6 +309,9 @@ int main(int argc, char** argv) sensorDetectsIDs, // wheter to identy landmarks stdRange, stdYaw, stdPitch); + // Overwrite the time generated in the time above: + obs->timestamp = mrpt::Clock::fromDouble(timeStamp); + // Keep the GT of the robot pose: GT_path.setSize(i + 1, 6); for (size_t k = 0; k < 6; k++) diff --git a/appveyor.yml b/appveyor.yml index a0be2c0307..ded29e6c7f 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 2.12.0-{branch}-build{build} +version: 2.12.1-{branch}-build{build} os: Visual Studio 2019 diff --git a/cmakemodules/script_assimp.cmake b/cmakemodules/script_assimp.cmake index e07885ba47..f74bcf9054 100644 --- a/cmakemodules/script_assimp.cmake +++ b/cmakemodules/script_assimp.cmake @@ -42,8 +42,8 @@ if (NOT ASSIMP_FOUND) include(ExternalProject) # download from GH ExternalProject_Add(EP_assimp - URL "https://github.com/assimp/assimp/archive/v4.1.0.tar.gz" - URL_MD5 "83b53a10c38d964bd1e69da0606e2727" + URL "https://github.com/assimp/assimp/archive/v5.3.1.tar.gz" + URL_MD5 "835357be3c1e70120fe2d5a52542ce55" SOURCE_DIR "${MRPT_BINARY_DIR}/3rdparty/assimp/" CMAKE_ARGS -DASSIMP_BUILD_ASSIMP_TOOLS=OFF diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index cd4818952f..718662a65b 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,5 +1,24 @@ \page changelog Change Log +# Version 2.12.1: Released April 4th, 2024 +- Changes in apps: + - simul-landmarks: Fix correct generation of sensorLabel and timestamps in observations. +- Changes in libraries: + - \ref mrpt_obs_grp: + - mrpt::obs::CObservationBearingRange now implements the `exportTxt*()` virtual interface. + - \ref mrpt_opengl_grp: + - mrpt::opengl::CAssimpModel now can read embedded textures in model files. + - Update embedded Assimp lib version 4.1.0 -> 5.3.1 (when built as ExternalProject) + - \ref mrpt_poses_grp: + - New methods: + - mrpt::poses::CPose2DGridTemplate::data(), mrpt::poses::CPose3DGridTemplate::data() + - mrpt::poses::CPose2DGridTemplate::absidx2idx(), mrpt::poses::CPose3DGridTemplate::absidx2idx() + - mrpt::poses::CPose2DGridTemplate::idx2absidx(), mrpt::poses::CPose3DGridTemplate::idx2absidx() + - Fix const-correctness of mrpt::poses::CPose2DGridTemplate::getAsMatrix() + - rplidar_skd: Update to the latest upstream version, and fix all build warnings. + - xsens library: Fix all build warnings. + + # Version 2.12.0: Released March 17th, 2024 - Changes in libraries: - \ref mrpt_obs_grp: diff --git a/libs/apps/include/mrpt/apps/CRawlogProcessor.h b/libs/apps/include/mrpt/apps/CRawlogProcessor.h index a3b7ff210c..fe5383b3f8 100644 --- a/libs/apps/include/mrpt/apps/CRawlogProcessor.h +++ b/libs/apps/include/mrpt/apps/CRawlogProcessor.h @@ -176,23 +176,23 @@ class CRawlogProcessorOnEachObservation : public CRawlogProcessor // within a "SF": for (size_t idxObs = 0; true; idxObs++) { - mrpt::obs::CObservation::Ptr* obs_indiv = nullptr; + mrpt::obs::CObservation::Ptr obs_indiv; if (obs) { if (idxObs > 0) break; - obs_indiv = &obs; + obs_indiv = obs; } else if (SF) { if (idxObs >= SF->size()) break; - obs_indiv = &SF->getObservationByIndex(idxObs); + obs_indiv = SF->getObservationByIndex(idxObs); } else break; // shouldn't... // Process "obs_indiv": ASSERT_(obs_indiv); - if (!processOneObservation(*obs_indiv)) return false; + if (!processOneObservation(obs_indiv)) return false; } if (actions) diff --git a/libs/apps/src/CGridMapAlignerApp.cpp b/libs/apps/src/CGridMapAlignerApp.cpp index 455161be35..38e315a729 100644 --- a/libs/apps/src/CGridMapAlignerApp.cpp +++ b/libs/apps/src/CGridMapAlignerApp.cpp @@ -371,7 +371,9 @@ void CGridMapAlignerApp::run() format("%s/map1_LM.png", RESULTS_DIR.c_str()), &lm1, true); CImage img; grid1->getAsImageFiltered(img); - img.saveToFile(format("%s/map1_filt.png", RESULTS_DIR.c_str())); + bool savedOk = + img.saveToFile(format("%s/map1_filt.png", RESULTS_DIR.c_str())); + ASSERT_(savedOk); } { @@ -465,9 +467,10 @@ void CGridMapAlignerApp::run() &lm2, true); CImage img; grid2->getAsImageFiltered(img); - img.saveToFile(format( + bool savedOk = img.saveToFile(format( "%s/map2_filt_noise_%f.png", RESULTS_DIR.c_str(), STD_NOISE_XY)); + ASSERT_(savedOk); } // Only if the case of "save-corr-dists" we can do NOT align the @@ -635,9 +638,10 @@ void CGridMapAlignerApp::run() imgGrid1LY - 1 - grid1->y2idx(pp1.y), TColor::black()); - imgCanvas.saveToFile(format( + bool savedOk = imgCanvas.saveToFile(format( "%s/_OVERLAP_MAPS_SOG_MODE_%04u.png", RESULTS_DIR.c_str(), (unsigned int)nNode)); + ASSERT_(savedOk); // Save as 3D scene: Scene scene; diff --git a/libs/apps/src/KFSLAMApp.cpp b/libs/apps/src/KFSLAMApp.cpp index 8327a378d1..566e2069e8 100644 --- a/libs/apps/src/KFSLAMApp.cpp +++ b/libs/apps/src/KFSLAMApp.cpp @@ -166,7 +166,9 @@ struct kfslam_traits CMatrixF H2(H); CImage imgF; imgF.setFromMatrix(H2, false /*it's not normalized*/); - imgF.saveToFile(OUT_DIR + string("/information_matrix_final.png")); + bool savedOk = imgF.saveToFile( + OUT_DIR + string("/information_matrix_final.png")); + ASSERT_(savedOk); // ---------------------------------------- // Compute the "approximation error factor" E: diff --git a/libs/apps/src/RBPF_SLAM_App.cpp b/libs/apps/src/RBPF_SLAM_App.cpp index fe3f2f8e98..44978aa350 100644 --- a/libs/apps/src/RBPF_SLAM_App.cpp +++ b/libs/apps/src/RBPF_SLAM_App.cpp @@ -396,8 +396,9 @@ void RBPF_SLAM_App_Base::run() { mrpt::img::CImage img; mapBuilder->drawCurrentEstimationToImage(&img); - img.saveToFile( + bool savedOk = img.saveToFile( format("%s/mapping_%05u.png", OUT_DIR, step)); + ASSERT_(savedOk); } } diff --git a/libs/apps/src/RBPF_SLAM_App_unittest.cpp b/libs/apps/src/RBPF_SLAM_App_unittest.cpp index 761f4f5cc0..e3c31a2cbe 100644 --- a/libs/apps/src/RBPF_SLAM_App_unittest.cpp +++ b/libs/apps/src/RBPF_SLAM_App_unittest.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -81,13 +82,23 @@ static auto tester_for_ROSLAM_demo = [](mrpt::apps::RBPF_SLAM_App_Base& o) { EXPECT_EQ(o.out_estimated_path.size(), 99U); const auto p = mrpt::poses::CPose3D(o.out_estimated_path.rbegin()->second); const auto p_gt = mrpt::poses::CPose3D::FromString( - "[1.938686 3.352273 0.000000 114.993417 0.000000 0.000000]"); + "[2.009005 3.330509 0.000000 115.004460 0.000000 0.000000]"); EXPECT_LT(mrpt::poses::Lie::SE<3>::log(p - p_gt).norm(), 1.0) << "actual pose =" << p.asString() << "\nexpected pose=" << p_gt.asString(); - MRPT_TODO("Stricter unit tests: check for estimated landmark positions"); + const auto& maps = o.mapBuilder->getCurrentlyBuiltMetricMap().maps; + ASSERT_EQUAL_(maps.size(), 1U); + auto beaconMap = + std::dynamic_pointer_cast(maps.at(0)); + ASSERT_(beaconMap); + + // TODO(jlbc): check why actual LM mean positions seem not to match GT? + // beaconMap->saveMetricMapRepresentationToFile("beaconMap"); + + // there are 14 beacons: + EXPECT_EQ(beaconMap->size(), 14UL); }; TEST(RBPF_SLAM_App, MapFromRawlog_Lidar2D_optimal_sampling) diff --git a/libs/apps/src/rawlog-edit_deexternalize.cpp b/libs/apps/src/rawlog-edit_deexternalize.cpp index abe50132cc..e5e04bf2a5 100644 --- a/libs/apps/src/rawlog-edit_deexternalize.cpp +++ b/libs/apps/src/rawlog-edit_deexternalize.cpp @@ -63,8 +63,9 @@ DECLARE_OP_FUNCTION(op_deexternalize) if (obsSt->imageLeft.isExternallyStored()) { - obsSt->imageLeft.loadFromFile( + bool loadOk = obsSt->imageLeft.loadFromFile( obsSt->imageLeft.getExternalStorageFileAbsolutePath()); + ASSERT_(loadOk); entries_converted++; } else @@ -72,8 +73,9 @@ DECLARE_OP_FUNCTION(op_deexternalize) if (obsSt->imageRight.isExternallyStored()) { - obsSt->imageRight.loadFromFile( + bool loadOk = obsSt->imageRight.loadFromFile( obsSt->imageRight.getExternalStorageFileAbsolutePath()); + ASSERT_(loadOk); entries_converted++; } else @@ -85,8 +87,9 @@ DECLARE_OP_FUNCTION(op_deexternalize) if (obsIm->image.isExternallyStored()) { - obsIm->image.loadFromFile( + bool loadOk = obsIm->image.loadFromFile( obsIm->image.getExternalStorageFileAbsolutePath()); + ASSERT_(loadOk); entries_converted++; } else @@ -124,18 +127,20 @@ DECLARE_OP_FUNCTION(op_deexternalize) if (obs3D->hasIntensityImage && obs3D->intensityImage.isExternallyStored()) { - obs3D->intensityImage.loadFromFile( + bool loadOk = obs3D->intensityImage.loadFromFile( obs3D->intensityImage .getExternalStorageFileAbsolutePath()); + ASSERT_(loadOk); } // Confidence channel: if (obs3D->hasConfidenceImage && obs3D->confidenceImage.isExternallyStored()) { - obs3D->confidenceImage.loadFromFile( + bool loadOk = obs3D->confidenceImage.loadFromFile( obs3D->confidenceImage .getExternalStorageFileAbsolutePath()); + ASSERT_(loadOk); } // Range image & 3D points: diff --git a/libs/apps/src/rawlog-edit_externalize.cpp b/libs/apps/src/rawlog-edit_externalize.cpp index 87441ca217..36642438b0 100644 --- a/libs/apps/src/rawlog-edit_externalize.cpp +++ b/libs/apps/src/rawlog-edit_externalize.cpp @@ -107,7 +107,10 @@ DECLARE_OP_FUNCTION(op_externalize) { const string fileName = "img_"s + label_time + "_left."s + imgFileExtension; - obsSt->imageLeft.saveToFile(outDir + fileName); + bool savedOk = + obsSt->imageLeft.saveToFile(outDir + fileName); + ASSERT_(savedOk); + obsSt->imageLeft.setExternalStorage(fileName); entries_converted++; } @@ -118,7 +121,10 @@ DECLARE_OP_FUNCTION(op_externalize) { const string fileName = "img_"s + label_time + "_right."s + imgFileExtension; - obsSt->imageRight.saveToFile(outDir + fileName); + bool savedOk = + obsSt->imageRight.saveToFile(outDir + fileName); + ASSERT_(savedOk); + obsSt->imageRight.setExternalStorage(fileName); entries_converted++; } @@ -134,7 +140,9 @@ DECLARE_OP_FUNCTION(op_externalize) { const string fileName = "img_"s + label_time + "."s + imgFileExtension; - obsIm->image.saveToFile(outDir + fileName); + bool savedOk = obsIm->image.saveToFile(outDir + fileName); + ASSERT_(savedOk); + obsIm->image.setExternalStorage(fileName); entries_converted++; } @@ -199,7 +207,10 @@ DECLARE_OP_FUNCTION(op_externalize) { const string fileName = "3DCAM_"s + label_time + "_INT."s + imgFileExtension; - obs3D->intensityImage.saveToFile(outDir + fileName); + bool savedOk = + obs3D->intensityImage.saveToFile(outDir + fileName); + ASSERT_(savedOk); + obs3D->intensityImage.setExternalStorage(fileName); entries_converted++; } @@ -212,7 +223,10 @@ DECLARE_OP_FUNCTION(op_externalize) { const string fileName = "3DCAM_"s + label_time + "_CONF."s + imgFileExtension; - obs3D->confidenceImage.saveToFile(outDir + fileName); + bool savedOk = + obs3D->confidenceImage.saveToFile(outDir + fileName); + ASSERT_(savedOk); + obs3D->confidenceImage.setExternalStorage(fileName); entries_converted++; } diff --git a/libs/apps/src/rawlog-edit_stereo-rectify.cpp b/libs/apps/src/rawlog-edit_stereo-rectify.cpp index c148ea7a77..3bc590c85f 100644 --- a/libs/apps/src/rawlog-edit_stereo-rectify.cpp +++ b/libs/apps/src/rawlog-edit_stereo-rectify.cpp @@ -155,8 +155,8 @@ DECLARE_OP_FUNCTION(op_stereo_rectify) // This is needed to raise an exception of the correct // type that reveal any missing external file: - o->imageLeft.getWidth(); - o->imageRight.getWidth(); + [[maybe_unused]] auto wL = o->imageLeft.getWidth(); + [[maybe_unused]] auto wR = o->imageRight.getWidth(); // This call rectifies the images in-place and also // updates @@ -170,14 +170,18 @@ DECLARE_OP_FUNCTION(op_stereo_rectify) const string fileName = string("img_") + label_time + string("_left.") + imgFileExtension; - o->imageLeft.saveToFile(outDir + fileName); + bool savedOk = + o->imageLeft.saveToFile(outDir + fileName); + ASSERT_(savedOk); o->imageLeft.setExternalStorage(fileName); } { const string fileName = string("img_") + label_time + string("_right.") + imgFileExtension; - o->imageRight.saveToFile(outDir + fileName); + bool savedOk = + o->imageRight.saveToFile(outDir + fileName); + ASSERT_(savedOk); o->imageRight.setExternalStorage(fileName); } m_changedCams++; diff --git a/libs/graphs/include/mrpt/graphs/CNetworkOfPoses.h b/libs/graphs/include/mrpt/graphs/CNetworkOfPoses.h index fda2ad2987..2abe539e6b 100644 --- a/libs/graphs/include/mrpt/graphs/CNetworkOfPoses.h +++ b/libs/graphs/include/mrpt/graphs/CNetworkOfPoses.h @@ -290,7 +290,10 @@ class CNetworkOfPoses { detail::graph_ops::load_graph_of_poses_from_text_file( this, fileName); - if (collapse_dup_edges) this->collapseDuplicatedEdges(); + if (collapse_dup_edges) + { + [[maybe_unused]] auto n = this->collapseDuplicatedEdges(); + } } /** Reads as text in the format used by TORO, HoG-man, G2O. diff --git a/libs/graphslam/src/graph_slam_levmarq_unittest.cpp b/libs/graphslam/src/graph_slam_levmarq_unittest.cpp index edb55129a7..7f13372086 100644 --- a/libs/graphslam/src/graph_slam_levmarq_unittest.cpp +++ b/libs/graphslam/src/graph_slam_levmarq_unittest.cpp @@ -174,7 +174,9 @@ class GraphTester : public GraphSlamLevMarqTest, my_graph_t graph, graph_good; graph.loadFromTextFile(in_f); + graph_good.loadFromTextFile(good_f); + ASSERT_(graph.nodeCount() > 1); ASSERT_EQ(graph.nodeCount(), graph_good.nodeCount()); ASSERT_EQ(graph.edgeCount(), graph_good.edgeCount()); diff --git a/libs/hwdrivers/src/CVelodyneScanner.cpp b/libs/hwdrivers/src/CVelodyneScanner.cpp index 7eb0a51958..994eb9dcff 100644 --- a/libs/hwdrivers/src/CVelodyneScanner.cpp +++ b/libs/hwdrivers/src/CVelodyneScanner.cpp @@ -177,7 +177,11 @@ void CVelodyneScanner::loadConfig_sensorSpecific( std::string calibration_file; MRPT_LOAD_CONFIG_VAR(calibration_file, string, cfg, sect); - if (!calibration_file.empty()) this->loadCalibrationFile(calibration_file); + if (!calibration_file.empty()) + { + bool calibLoadOk = loadCalibrationFile(calibration_file); + ASSERT_(calibLoadOk); + } // Check validity: const model_properties_list_t& lstModels = TModelPropertiesFactory::get(); diff --git a/libs/maps/include/mrpt/maps/CMultiMetricMap.h b/libs/maps/include/mrpt/maps/CMultiMetricMap.h index dc1c00148d..ce980cab5c 100644 --- a/libs/maps/include/mrpt/maps/CMultiMetricMap.h +++ b/libs/maps/include/mrpt/maps/CMultiMetricMap.h @@ -220,12 +220,7 @@ class CMultiMetricMap : public mrpt::maps::CMetricMap const mrpt::maps::CSimplePointsMap* getAsSimplePointsMap() const override; /** Returns a short description of the map. */ - std::string asString() const override - { - return mrpt::format( - "Multi-map with %u children.", - static_cast(maps.size())); - } + std::string asString() const override; protected: // See base class docs: diff --git a/libs/maps/src/maps/CBeaconMap.cpp b/libs/maps/src/maps/CBeaconMap.cpp index daf711b1ae..c10743faad 100644 --- a/libs/maps/src/maps/CBeaconMap.cpp +++ b/libs/maps/src/maps/CBeaconMap.cpp @@ -1236,6 +1236,8 @@ void CBeaconMap::saveToTextFile(const string& fil) const MRPT_START FILE* f = os::fopen(fil.c_str(), "wt"); ASSERT_(f != nullptr); + os::fprintf( + f, "%% ID X Y Z C(0,0) C(1,1) C(2,2) D2 |C| C(0,1) C(1,2) C(1,1)\n"); for (const auto& m_beacon : m_beacons) { diff --git a/libs/maps/src/maps/CMultiMetricMap.cpp b/libs/maps/src/maps/CMultiMetricMap.cpp index 8231dfb54a..078f4d1969 100644 --- a/libs/maps/src/maps/CMultiMetricMap.cpp +++ b/libs/maps/src/maps/CMultiMetricMap.cpp @@ -315,6 +315,16 @@ const CSimplePointsMap* CMultiMetricMap::getAsSimplePointsMap() const MRPT_END } +std::string CMultiMetricMap::asString() const +{ + std::stringstream ss; + ss << "Multi-map with " << maps.size() << " children maps: "; + for (size_t i = 0; i < maps.size(); i++) + ss << "[" << i << "] " << maps[i]->asString() << ", "; + + return ss.str(); +} + mrpt::maps::CMetricMap::Ptr CMultiMetricMap::mapByIndex(size_t idx) const { MRPT_START diff --git a/libs/obs/include/mrpt/obs/CObservationBearingRange.h b/libs/obs/include/mrpt/obs/CObservationBearingRange.h index 5dcf906a9f..11c32a8fbd 100644 --- a/libs/obs/include/mrpt/obs/CObservationBearingRange.h +++ b/libs/obs/include/mrpt/obs/CObservationBearingRange.h @@ -105,6 +105,11 @@ class CObservationBearingRange : public CObservation } void getDescriptionAsText(std::ostream& o) const override; + // See base class docs: + bool exportTxtSupported() const override { return true; } + std::string exportTxtHeader() const override; + std::string exportTxtDataRow() const override; + }; // End of class def. } // namespace mrpt::obs diff --git a/libs/obs/src/CObservationBearingRange.cpp b/libs/obs/src/CObservationBearingRange.cpp index 75e0a839c7..47da60ed55 100644 --- a/libs/obs/src/CObservationBearingRange.cpp +++ b/libs/obs/src/CObservationBearingRange.cpp @@ -201,3 +201,32 @@ void CObservationBearingRange::getDescriptionAsText(std::ostream& o) const o << " (N/A)\n"; } } + +std::string CObservationBearingRange::exportTxtHeader() const +{ + return "[LANDMARK_ID RANGE YAW PITCH " + "SENSOR_XYZ_YAWPITCH_ROLL_ON_ROBOT] x N \n"; +} + +std::string CObservationBearingRange::exportTxtDataRow() const +{ + std::stringstream o; + + for (size_t i = 0; i < sensedData.size(); i++) + { + const auto& d = sensedData[i]; + if (i != 0) // to fit the format for rawlog-edit --export-txt + o << mrpt::format("%16.6f ", mrpt::Clock::toDouble(timestamp)); + + o << format( + " %i %.04f %.04f %.04f %.03f %.03f %.03f " + "%.03f %.03f %.03f", + (int)d.landmarkID, d.range, d.yaw, d.pitch, + sensorLocationOnRobot.x(), sensorLocationOnRobot.y(), + sensorLocationOnRobot.z(), sensorLocationOnRobot.yaw(), + sensorLocationOnRobot.pitch(), sensorLocationOnRobot.roll()); + + if (i + 1 != sensedData.size()) o << "\n"; + } + return o.str(); +} diff --git a/libs/opengl/CMakeLists.txt b/libs/opengl/CMakeLists.txt index 25f1ec8e6f..af09bf071e 100644 --- a/libs/opengl/CMakeLists.txt +++ b/libs/opengl/CMakeLists.txt @@ -70,3 +70,7 @@ endif () if (MINGW) target_link_libraries(opengl PRIVATE opengl32 GlU32) endif() +if(CMAKE_MRPT_HAS_OPENCV) + target_link_libraries(opengl PRIVATE imp_opencv) +endif() + diff --git a/libs/opengl/src/CAssimpModel.cpp b/libs/opengl/src/CAssimpModel.cpp index 5ffef24b76..1a3c70bafe 100644 --- a/libs/opengl/src/CAssimpModel.cpp +++ b/libs/opengl/src/CAssimpModel.cpp @@ -37,6 +37,11 @@ #include #include +// Universal include for all versions of OpenCV +#if MRPT_HAS_OPENCV +#include +#endif + #include #include @@ -67,7 +72,13 @@ class TexturesCache }; CachedTexturesInfo& get( - const CAssimpModel::filepath_t& texturePath, bool verboseLoad) + const CAssimpModel::filepath_t& texturePath, bool verboseLoad, +#if MRPT_HAS_ASSIMP + const aiScene* scene +#else + const void* scene +#endif + ) { using namespace std::string_literals; @@ -96,7 +107,46 @@ class TexturesCache std::cout << "[CAssimpModel] Loaded texture: " << texturePath << "\n"; } - else +#if MRPT_HAS_ASSIMP && MRPT_HAS_OPENCV + else if (scene->HasTextures()) + { + // Embedded texture? + auto aiTex = scene->GetEmbeddedTexture(texturePath.c_str()); + if (aiTex) + { + const auto texW = aiTex->mWidth; + const auto texH = aiTex->mHeight; + + if (texH == 0) + { + // compressed format: + // aiTex->achFormatHint; + + const cv::Mat data(texW, 1, CV_8UC1, aiTex->pcData); + + const cv::Mat im = cv::imdecode(data, cv::IMREAD_UNCHANGED); + + if (!im.empty()) + { + // load ok: + entry.load_ok = true; + + entry.img_rgb = + mrpt::img::CImage(im, mrpt::img::DEEP_COPY); + } + } + else + { + // uncompressed format: + THROW_EXCEPTION( + "Support for uncompressed embedded textures not " + "implemented yet for mrpt::opengl::CAssimpModel."); + } + } + } +#endif + + if (!entry.load_ok) { /* Error occured */ const std::string sError = mrpt::format( @@ -693,12 +743,6 @@ void CAssimpModel::process_textures(const aiScene* scene) { using namespace std::string_literals; - if (scene->HasTextures()) - THROW_EXCEPTION( - "Support for meshes with *embedded* textures is not implemented. " - "Please, use external texture files or contribute a PR to mrpt " - "with this feature."); - m_textureIdMap.clear(); m_texturedObjects.clear(); @@ -746,7 +790,7 @@ void CAssimpModel::process_textures(const aiScene* scene) // Query textureCache: auto& cache = internal::TexturesCache::Instance(); - auto& tc = cache.get(fileloc, m_verboseLoad); + auto& tc = cache.get(fileloc, m_verboseLoad, scene); if (tc.load_ok) { diff --git a/libs/poses/include/mrpt/poses/CPose2DGridTemplate.h b/libs/poses/include/mrpt/poses/CPose2DGridTemplate.h index 726eff00bd..d466dc4fdd 100644 --- a/libs/poses/include/mrpt/poses/CPose2DGridTemplate.h +++ b/libs/poses/include/mrpt/poses/CPose2DGridTemplate.h @@ -12,6 +12,8 @@ #include // for round() #include +#include + namespace mrpt::poses { /** This is a template class for storing a 3D (2D+heading) grid containing any @@ -35,11 +37,12 @@ class CPose2DGridTemplate */ int m_idxLeftX, m_idxLeftY, m_idxLeftPhi; - /** The data: - */ + /** The data */ std::vector m_data; public: + const std::vector& data() const { return m_data; } + /** Returns "indexes" from coordinates: */ size_t x2idx(double x) const @@ -159,12 +162,29 @@ class CPose2DGridTemplate return getByIndex(x2idx(x), y2idx(y), phi2idx(phi)); } + /** (x,y,phi) indices to absolute index in raw data container */ + size_t idx2absidx(size_t cx, size_t cy, size_t cPhi) const + { + return cPhi * m_sizeXY + cy * m_sizeX + cx; + } + + /** absolute index to (x,y,phi) indices */ + std::tuple absidx2idx(size_t absIdx) const + { + const auto cPhi = absIdx / m_sizeXY; + const auto r = absIdx % m_sizeXY; + const auto cy = r / m_sizeX; + const auto cx = r % m_sizeX; + + return {cx, cy, cPhi}; + } + /** Reads the contents of a cell */ const T* getByIndex(size_t x, size_t y, size_t phi) const { ASSERT_(x < m_sizeX && y < m_sizeY && phi < m_sizePhi); - return &m_data[phi * m_sizeXY + y * m_sizeX + x]; + return &m_data[idx2absidx(x, y, phi)]; } /** Reads the contents of a cell @@ -179,7 +199,7 @@ class CPose2DGridTemplate * each row contains values for a fixed "y". */ template - void getAsMatrix(double phi, MATRIXLIKE& outMat) + void getAsMatrix(double phi, MATRIXLIKE& outMat) const { MRPT_START outMat.setSize(m_sizeY, m_sizeX); diff --git a/libs/poses/include/mrpt/poses/CPose3DGridTemplate.h b/libs/poses/include/mrpt/poses/CPose3DGridTemplate.h index 2ec8b5fb6c..48b075656f 100644 --- a/libs/poses/include/mrpt/poses/CPose3DGridTemplate.h +++ b/libs/poses/include/mrpt/poses/CPose3DGridTemplate.h @@ -13,6 +13,8 @@ #include #include +#include + namespace mrpt::poses { /** This is a template class for storing a 6-dimensional grid, with components @@ -64,6 +66,8 @@ class CPose3DGridTemplate std::vector m_data; public: + const std::vector& data() const { return m_data; } + /** Default constructor: */ CPose3DGridTemplate( @@ -250,6 +254,37 @@ class CPose3DGridTemplate cx, cy, cz, cY, cP, cR)); } + /** (x,y,z,yaw,pitch,roll) indices to absolute index in raw data container + */ + size_t idx2absidx( + size_t cx, size_t cy, size_t cz, size_t cYaw, size_t cPitch, + size_t cRoll) const + { + return cx + m_sizeX * cy + m_size_xy * cz + m_size_xyz * cYaw + + m_size_xyzY * cPitch + m_size_xyzYP * cRoll; + } + + /** absolute index to (x,y,z,yaw,pitch,roll) indices */ + std::tuple absidx2idx( + size_t absIdx) const + { + const size_t cR = absIdx / m_size_xyzYP; + const size_t r1 = absIdx % m_size_xyzYP; + + const size_t cP = r1 / m_size_xyzY; + const size_t r2 = r1 % m_size_xyzY; + + const size_t cY = r2 / m_size_xyz; + const size_t r3 = r2 % m_size_xyz; + + const size_t cz = r3 / m_size_xy; + const size_t r4 = r3 % m_size_xy; + const size_t cy = r4 / m_sizeX; + const size_t cx = r4 % m_sizeX; + + return {cx, cy, cz, cY, cP, cR}; + } + /** Returns a XY slice of the grid, for given constant z,yaw, pitch and * roll. */ diff --git a/libs/ros2bridge/src/stereo_image.cpp b/libs/ros2bridge/src/stereo_image.cpp index 647bd48f6c..f3856de0bc 100644 --- a/libs/ros2bridge/src/stereo_image.cpp +++ b/libs/ros2bridge/src/stereo_image.cpp @@ -16,7 +16,7 @@ #include #include -#if CV_BRIDGE_VERSION <= 0x030400 +#if CV_BRIDGE_VERSION < 0x030400 #include #else #include diff --git a/libs/slam/src/slam/CGridMapAligner.cpp b/libs/slam/src/slam/CGridMapAligner.cpp index 4334c9574e..2b7a4ebfcf 100644 --- a/libs/slam/src/slam/CGridMapAligner.cpp +++ b/libs/slam/src/slam/CGridMapAligner.cpp @@ -297,7 +297,8 @@ CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch( 10 + FEAT_W, 5 + j * (FEAT_H + 5), im2); } fil += ".png"; - img_compose.saveToFile(fil); + bool savedOk = img_compose.saveToFile(fil); + ASSERT_(savedOk); } // end for map } diff --git a/libs/slam/src/slam/CIncrementalMapPartitioner.cpp b/libs/slam/src/slam/CIncrementalMapPartitioner.cpp index 7473569d46..80a4fde218 100644 --- a/libs/slam/src/slam/CIncrementalMapPartitioner.cpp +++ b/libs/slam/src/slam/CIncrementalMapPartitioner.cpp @@ -79,7 +79,6 @@ void CIncrementalMapPartitioner::TOptions::loadFromConfigFile( mrpt::config::CConfigFilePrefixer cfp( source, section + std::string("."), ""); metricmap.loadFromConfigFile(cfp, "metricmap"); - MRPT_TODO("Add link to example INI file"); MRPT_END } diff --git a/libs/slam/src/slam/CMetricMapBuilderRBPF.cpp b/libs/slam/src/slam/CMetricMapBuilderRBPF.cpp index 6692e26ea2..c6f94448b3 100644 --- a/libs/slam/src/slam/CMetricMapBuilderRBPF.cpp +++ b/libs/slam/src/slam/CMetricMapBuilderRBPF.cpp @@ -470,7 +470,8 @@ void CMetricMapBuilderRBPF::saveCurrentEstimationToImage( { CImage img(1, 1, CH_GRAY); drawCurrentEstimationToImage(&img); - img.saveToFile(file); + bool savedOk = img.saveToFile(file); + ASSERT_(savedOk); } MRPT_END diff --git a/package.xml b/package.xml index c97b6623e6..0cad7b296d 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ mrpt2 - 2.12.0 + 2.12.1 Mobile Robot Programming Toolkit (MRPT) version 2.x Jose-Luis Blanco-Claraco diff --git a/python/src/mrpt/math/CAtan2LookUpTable.cpp b/python/src/mrpt/math/CAtan2LookUpTable.cpp index 87cd6cd82d..e6e5f2f224 100644 --- a/python/src/mrpt/math/CAtan2LookUpTable.cpp +++ b/python/src/mrpt/math/CAtan2LookUpTable.cpp @@ -40,7 +40,7 @@ void bind_mrpt_math_CAtan2LookUpTable(std::function< pybind11::module &(std::str cl.def("getSizeX", (size_t (mrpt::math::CAtan2LookUpTable::*)() const) &mrpt::math::CAtan2LookUpTable::getSizeX, "C++: mrpt::math::CAtan2LookUpTable::getSizeX() const --> size_t"); cl.def("getSizeY", (size_t (mrpt::math::CAtan2LookUpTable::*)() const) &mrpt::math::CAtan2LookUpTable::getSizeY, "C++: mrpt::math::CAtan2LookUpTable::getSizeY() const --> size_t"); } - { // mrpt::math::CAtan2LookUpTableMultiRes file:mrpt/math/CAtan2LookUpTable.h line:83 + { // mrpt::math::CAtan2LookUpTableMultiRes file:mrpt/math/CAtan2LookUpTable.h line:87 pybind11::class_> cl(M("mrpt::math"), "CAtan2LookUpTableMultiRes", "Like CAtan2LookUpTable but with a multiresolution grid for increasingly\n better accuracy in points nearer to the origin.\n Example of usage:\n \n\n\n\n\n\n\n\n\n\n\n \n\n "); cl.def( pybind11::init( [](){ return new mrpt::math::CAtan2LookUpTableMultiRes(); } ) ); cl.def( pybind11::init &>(), pybind11::arg("lst_resolutions2extensions") ); diff --git a/python/src/mrpt/obs/CObservationBearingRange.cpp b/python/src/mrpt/obs/CObservationBearingRange.cpp index 5d81ef511f..8f76d2ce93 100644 --- a/python/src/mrpt/obs/CObservationBearingRange.cpp +++ b/python/src/mrpt/obs/CObservationBearingRange.cpp @@ -146,23 +146,22 @@ struct PyCallBack_mrpt_obs_CObservationBearingRange : public mrpt::obs::CObserva } return CObservationBearingRange::setSensorPose(a0); } - using _binder_ret_0 = mrpt::Clock::time_point; - _binder_ret_0 getOriginalReceivedTimeStamp() const override { + bool exportTxtSupported() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "getOriginalReceivedTimeStamp"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "exportTxtSupported"); if (overload) { auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference<_binder_ret_0>::value) { - static pybind11::detail::override_caster_t<_binder_ret_0> caster; - return pybind11::detail::cast_ref<_binder_ret_0>(std::move(o), caster); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); } - else return pybind11::detail::cast_safe<_binder_ret_0>(std::move(o)); + else return pybind11::detail::cast_safe(std::move(o)); } - return CObservation::getOriginalReceivedTimeStamp(); + return CObservationBearingRange::exportTxtSupported(); } - std::string asString() const override { + std::string exportTxtHeader() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "asString"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "exportTxtHeader"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -171,37 +170,38 @@ struct PyCallBack_mrpt_obs_CObservationBearingRange : public mrpt::obs::CObserva } else return pybind11::detail::cast_safe(std::move(o)); } - return CObservation::asString(); + return CObservationBearingRange::exportTxtHeader(); } - bool exportTxtSupported() const override { + std::string exportTxtDataRow() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "exportTxtSupported"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "exportTxtDataRow"); if (overload) { auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); } - else return pybind11::detail::cast_safe(std::move(o)); + else return pybind11::detail::cast_safe(std::move(o)); } - return CObservation::exportTxtSupported(); + return CObservationBearingRange::exportTxtDataRow(); } - std::string exportTxtHeader() const override { + using _binder_ret_0 = mrpt::Clock::time_point; + _binder_ret_0 getOriginalReceivedTimeStamp() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "exportTxtHeader"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "getOriginalReceivedTimeStamp"); if (overload) { auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); + if (pybind11::detail::cast_is_temporary_value_reference<_binder_ret_0>::value) { + static pybind11::detail::override_caster_t<_binder_ret_0> caster; + return pybind11::detail::cast_ref<_binder_ret_0>(std::move(o), caster); } - else return pybind11::detail::cast_safe(std::move(o)); + else return pybind11::detail::cast_safe<_binder_ret_0>(std::move(o)); } - return CObservation::exportTxtHeader(); + return CObservation::getOriginalReceivedTimeStamp(); } - std::string exportTxtDataRow() const override { + std::string asString() const override { pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "exportTxtDataRow"); + pybind11::function overload = pybind11::get_overload(static_cast(this), "asString"); if (overload) { auto o = overload.operator()(); if (pybind11::detail::cast_is_temporary_value_reference::value) { @@ -210,7 +210,7 @@ struct PyCallBack_mrpt_obs_CObservationBearingRange : public mrpt::obs::CObserva } else return pybind11::detail::cast_safe(std::move(o)); } - return CObservation::exportTxtDataRow(); + return CObservation::asString(); } void unload() const override { pybind11::gil_scoped_acquire gil; @@ -831,6 +831,9 @@ void bind_mrpt_obs_CObservationBearingRange(std::function< pybind11::module &(st cl.def("debugPrintOut", (void (mrpt::obs::CObservationBearingRange::*)()) &mrpt::obs::CObservationBearingRange::debugPrintOut, "Prints out the contents of the object.\n\nC++: mrpt::obs::CObservationBearingRange::debugPrintOut() --> void"); cl.def("getSensorPose", (void (mrpt::obs::CObservationBearingRange::*)(class mrpt::poses::CPose3D &) const) &mrpt::obs::CObservationBearingRange::getSensorPose, "C++: mrpt::obs::CObservationBearingRange::getSensorPose(class mrpt::poses::CPose3D &) const --> void", pybind11::arg("out_sensorPose")); cl.def("setSensorPose", (void (mrpt::obs::CObservationBearingRange::*)(const class mrpt::poses::CPose3D &)) &mrpt::obs::CObservationBearingRange::setSensorPose, "C++: mrpt::obs::CObservationBearingRange::setSensorPose(const class mrpt::poses::CPose3D &) --> void", pybind11::arg("newSensorPose")); + cl.def("exportTxtSupported", (bool (mrpt::obs::CObservationBearingRange::*)() const) &mrpt::obs::CObservationBearingRange::exportTxtSupported, "C++: mrpt::obs::CObservationBearingRange::exportTxtSupported() const --> bool"); + cl.def("exportTxtHeader", (std::string (mrpt::obs::CObservationBearingRange::*)() const) &mrpt::obs::CObservationBearingRange::exportTxtHeader, "C++: mrpt::obs::CObservationBearingRange::exportTxtHeader() const --> std::string"); + cl.def("exportTxtDataRow", (std::string (mrpt::obs::CObservationBearingRange::*)() const) &mrpt::obs::CObservationBearingRange::exportTxtDataRow, "C++: mrpt::obs::CObservationBearingRange::exportTxtDataRow() const --> std::string"); cl.def("assign", (class mrpt::obs::CObservationBearingRange & (mrpt::obs::CObservationBearingRange::*)(const class mrpt::obs::CObservationBearingRange &)) &mrpt::obs::CObservationBearingRange::operator=, "C++: mrpt::obs::CObservationBearingRange::operator=(const class mrpt::obs::CObservationBearingRange &) --> class mrpt::obs::CObservationBearingRange &", pybind11::return_value_policy::automatic, pybind11::arg("")); { // mrpt::obs::CObservationBearingRange::TMeasurement file:mrpt/obs/CObservationBearingRange.h line:51 diff --git a/python/src/mrpt/poses/CPose2DGridTemplate.cpp b/python/src/mrpt/poses/CPose2DGridTemplate.cpp index 3a695248a7..07652cd9bd 100644 --- a/python/src/mrpt/poses/CPose2DGridTemplate.cpp +++ b/python/src/mrpt/poses/CPose2DGridTemplate.cpp @@ -11,6 +11,7 @@ #include #include // __str__ #include +#include #include #include @@ -28,7 +29,7 @@ void bind_mrpt_poses_CPose2DGridTemplate(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::poses::CPose2DGridTemplate file:mrpt/poses/CPose2DGridTemplate.h line:22 + { // mrpt::poses::CPose2DGridTemplate file:mrpt/poses/CPose2DGridTemplate.h line:24 pybind11::class_, std::shared_ptr>> cl(M("mrpt::poses"), "CPose2DGridTemplate_double_t", ""); cl.def( pybind11::init( [](){ return new mrpt::poses::CPose2DGridTemplate(); } ), "doc" ); cl.def( pybind11::init( [](double const & a0){ return new mrpt::poses::CPose2DGridTemplate(a0); } ), "doc" , pybind11::arg("xMin")); @@ -51,6 +52,8 @@ void bind_mrpt_poses_CPose2DGridTemplate(std::function< pybind11::module &(std:: cl.def("setSize", [](mrpt::poses::CPose2DGridTemplate &o, double const & a0, double const & a1, double const & a2, double const & a3, double const & a4, double const & a5, double const & a6) -> void { return o.setSize(a0, a1, a2, a3, a4, a5, a6); }, "", pybind11::arg("xMin"), pybind11::arg("xMax"), pybind11::arg("yMin"), pybind11::arg("yMax"), pybind11::arg("resolutionXY"), pybind11::arg("resolutionPhi"), pybind11::arg("phiMin")); cl.def("setSize", (void (mrpt::poses::CPose2DGridTemplate::*)(double, double, double, double, double, double, double, double)) &mrpt::poses::CPose2DGridTemplate::setSize, "C++: mrpt::poses::CPose2DGridTemplate::setSize(double, double, double, double, double, double, double, double) --> void", pybind11::arg("xMin"), pybind11::arg("xMax"), pybind11::arg("yMin"), pybind11::arg("yMax"), pybind11::arg("resolutionXY"), pybind11::arg("resolutionPhi"), pybind11::arg("phiMin"), pybind11::arg("phiMax")); cl.def("getByPos", (double * (mrpt::poses::CPose2DGridTemplate::*)(double, double, double)) &mrpt::poses::CPose2DGridTemplate::getByPos, "C++: mrpt::poses::CPose2DGridTemplate::getByPos(double, double, double) --> double *", pybind11::return_value_policy::automatic, pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("phi")); + cl.def("idx2absidx", (size_t (mrpt::poses::CPose2DGridTemplate::*)(size_t, size_t, size_t) const) &mrpt::poses::CPose2DGridTemplate::idx2absidx, "C++: mrpt::poses::CPose2DGridTemplate::idx2absidx(size_t, size_t, size_t) const --> size_t", pybind11::arg("cx"), pybind11::arg("cy"), pybind11::arg("cPhi")); + cl.def("absidx2idx", (class std::tuple (mrpt::poses::CPose2DGridTemplate::*)(size_t) const) &mrpt::poses::CPose2DGridTemplate::absidx2idx, "C++: mrpt::poses::CPose2DGridTemplate::absidx2idx(size_t) const --> class std::tuple", pybind11::arg("absIdx")); cl.def("getByIndex", (double * (mrpt::poses::CPose2DGridTemplate::*)(size_t, size_t, size_t)) &mrpt::poses::CPose2DGridTemplate::getByIndex, "C++: mrpt::poses::CPose2DGridTemplate::getByIndex(size_t, size_t, size_t) --> double *", pybind11::return_value_policy::automatic, pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("phi")); cl.def("getXMin", (double (mrpt::poses::CPose2DGridTemplate::*)() const) &mrpt::poses::CPose2DGridTemplate::getXMin, "C++: mrpt::poses::CPose2DGridTemplate::getXMin() const --> double"); cl.def("getXMax", (double (mrpt::poses::CPose2DGridTemplate::*)() const) &mrpt::poses::CPose2DGridTemplate::getXMax, "C++: mrpt::poses::CPose2DGridTemplate::getXMax() const --> double"); @@ -65,7 +68,7 @@ void bind_mrpt_poses_CPose2DGridTemplate(std::function< pybind11::module &(std:: cl.def("getSizePhi", (size_t (mrpt::poses::CPose2DGridTemplate::*)() const) &mrpt::poses::CPose2DGridTemplate::getSizePhi, "C++: mrpt::poses::CPose2DGridTemplate::getSizePhi() const --> size_t"); cl.def("assign", (class mrpt::poses::CPose2DGridTemplate & (mrpt::poses::CPose2DGridTemplate::*)(const class mrpt::poses::CPose2DGridTemplate &)) &mrpt::poses::CPose2DGridTemplate::operator=, "C++: mrpt::poses::CPose2DGridTemplate::operator=(const class mrpt::poses::CPose2DGridTemplate &) --> class mrpt::poses::CPose2DGridTemplate &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::poses::CPose3DGridTemplate file:mrpt/poses/CPose3DGridTemplate.h line:23 + { // mrpt::poses::CPose3DGridTemplate file:mrpt/poses/CPose3DGridTemplate.h line:25 pybind11::class_, std::shared_ptr>> cl(M("mrpt::poses"), "CPose3DGridTemplate_double_t", ""); cl.def( pybind11::init( [](){ return new mrpt::poses::CPose3DGridTemplate(); } ), "doc" ); cl.def( pybind11::init( [](const struct mrpt::math::TPose3D & a0){ return new mrpt::poses::CPose3DGridTemplate(a0); } ), "doc" , pybind11::arg("bb_min")); @@ -90,6 +93,8 @@ void bind_mrpt_poses_CPose2DGridTemplate(std::function< pybind11::module &(std:: cl.def("getByPos", (double * (mrpt::poses::CPose3DGridTemplate::*)(double, double, double, double, double, double)) &mrpt::poses::CPose3DGridTemplate::getByPos, "C++: mrpt::poses::CPose3DGridTemplate::getByPos(double, double, double, double, double, double) --> double *", pybind11::return_value_policy::automatic, pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("yaw"), pybind11::arg("pitch"), pybind11::arg("roll")); cl.def("getByPos", (double * (mrpt::poses::CPose3DGridTemplate::*)(const struct mrpt::math::TPose3D &)) &mrpt::poses::CPose3DGridTemplate::getByPos, "C++: mrpt::poses::CPose3DGridTemplate::getByPos(const struct mrpt::math::TPose3D &) --> double *", pybind11::return_value_policy::automatic, pybind11::arg("p")); cl.def("getByIndex", (double * (mrpt::poses::CPose3DGridTemplate::*)(int, int, int, int, int, int)) &mrpt::poses::CPose3DGridTemplate::getByIndex, "C++: mrpt::poses::CPose3DGridTemplate::getByIndex(int, int, int, int, int, int) --> double *", pybind11::return_value_policy::automatic, pybind11::arg("cx"), pybind11::arg("cy"), pybind11::arg("cz"), pybind11::arg("cY"), pybind11::arg("cP"), pybind11::arg("cR")); + cl.def("idx2absidx", (size_t (mrpt::poses::CPose3DGridTemplate::*)(size_t, size_t, size_t, size_t, size_t, size_t) const) &mrpt::poses::CPose3DGridTemplate::idx2absidx, "C++: mrpt::poses::CPose3DGridTemplate::idx2absidx(size_t, size_t, size_t, size_t, size_t, size_t) const --> size_t", pybind11::arg("cx"), pybind11::arg("cy"), pybind11::arg("cz"), pybind11::arg("cYaw"), pybind11::arg("cPitch"), pybind11::arg("cRoll")); + cl.def("absidx2idx", (class std::tuple (mrpt::poses::CPose3DGridTemplate::*)(size_t) const) &mrpt::poses::CPose3DGridTemplate::absidx2idx, "C++: mrpt::poses::CPose3DGridTemplate::absidx2idx(size_t) const --> class std::tuple", pybind11::arg("absIdx")); cl.def("getMinBoundingBox", (struct mrpt::math::TPose3D (mrpt::poses::CPose3DGridTemplate::*)() const) &mrpt::poses::CPose3DGridTemplate::getMinBoundingBox, "C++: mrpt::poses::CPose3DGridTemplate::getMinBoundingBox() const --> struct mrpt::math::TPose3D"); cl.def("getMaxBoundingBox", (struct mrpt::math::TPose3D (mrpt::poses::CPose3DGridTemplate::*)() const) &mrpt::poses::CPose3DGridTemplate::getMaxBoundingBox, "C++: mrpt::poses::CPose3DGridTemplate::getMaxBoundingBox() const --> struct mrpt::math::TPose3D"); cl.def("getResolutionXYZ", (double (mrpt::poses::CPose3DGridTemplate::*)() const) &mrpt::poses::CPose3DGridTemplate::getResolutionXYZ, "C++: mrpt::poses::CPose3DGridTemplate::getResolutionXYZ() const --> double"); diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi index 6ce3829aaa..fcd15722eb 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/obs/__init__.pyi @@ -833,6 +833,12 @@ class CObservationBearingRange(CObservation): def debugPrintOut(self) -> None: ... @overload def debugPrintOut() -> void: ... + def exportTxtDataRow(self) -> str: ... + def exportTxtHeader(self) -> str: ... + @overload + def exportTxtSupported(self) -> bool: ... + @overload + def exportTxtSupported() -> bool: ... @overload def getSensorPose(self, out_sensorPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> None: ... @overload diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi index 938120a808..e3a7808426 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/poses/__init__.pyi @@ -674,6 +674,7 @@ class CPose2DGridTemplate_double_t: def __init__(self, xMin: float, xMax: float, yMin: float, yMax: float, resolutionXY: float, resolutionPhi: float, phiMin: float, phiMax: float) -> None: ... @overload def __init__(self, arg0: CPose2DGridTemplate_double_t) -> None: ... + def absidx2idx(self, absIdx: int) -> Tuple[int,int,int]: ... def assign(self) -> CPose2DGridTemplate_double_t: ... def getByIndex(self, x: int, y: int, phi: int) -> float: ... def getByPos(self, x: float, y: float, phi: float) -> float: ... @@ -721,6 +722,7 @@ class CPose2DGridTemplate_double_t: def getYMin(self) -> float: ... @overload def getYMin() -> double: ... + def idx2absidx(self, cx: int, cy: int, cPhi: int) -> int: ... @overload def idx2phi(self, phi: int) -> float: ... @overload @@ -929,6 +931,7 @@ class CPose3DGridTemplate_double_t: def __init__(self, bb_min: mrpt.pymrpt.mrpt.math.TPose3D, bb_max: mrpt.pymrpt.mrpt.math.TPose3D, resolution_XYZ: float, resolution_YPR: float) -> None: ... @overload def __init__(self, arg0: CPose3DGridTemplate_double_t) -> None: ... + def absidx2idx(self, absIdx: int) -> Tuple[int,int,int,int,int,int]: ... def assign(self) -> CPose3DGridTemplate_double_t: ... def fill(self, val: float) -> None: ... def getByIndex(self, cx: int, cy: int, cz: int, cY: int, cP: int, cR: int) -> float: ... @@ -974,6 +977,7 @@ class CPose3DGridTemplate_double_t: def getTotalVoxelCount(self) -> int: ... @overload def getTotalVoxelCount() -> uint32_t: ... + def idx2absidx(self, cx: int, cy: int, cz: int, cYaw: int, cPitch: int, cRoll: int) -> int: ... @overload def idx2pitch(self, cP: int) -> float: ... @overload diff --git a/samples/gui_capture_render_to_img_example/test.cpp b/samples/gui_capture_render_to_img_example/test.cpp index 38e39e4a28..359d5aa50b 100644 --- a/samples/gui_capture_render_to_img_example/test.cpp +++ b/samples/gui_capture_render_to_img_example/test.cpp @@ -156,7 +156,8 @@ void TestDisplay3D() { static int i = 0; const string s = format("GRAB_%06i.png", ++i); - img->saveToFile(s); + bool savedOk = img->saveToFile(s); + ASSERT_(savedOk); printf("Saved frame image to: %s \r", s.c_str()); // "\ r" is to // overwrite the // same line over diff --git a/samples/gui_display3D_custom_render/test.cpp b/samples/gui_display3D_custom_render/test.cpp index f546c34ce0..a55c69d94b 100644 --- a/samples/gui_display3D_custom_render/test.cpp +++ b/samples/gui_display3D_custom_render/test.cpp @@ -123,9 +123,9 @@ void TestDisplay3D() while (!end && win.isOpen()) { // Move the scene: - Scene::Ptr& theScene = win.get3DSceneAndLock(); + Scene::Ptr& scene = win.get3DSceneAndLock(); - opengl::CRenderizable::Ptr obj1 = theScene->getByName("ball_1"); + opengl::CRenderizable::Ptr obj1 = scene->getByName("ball_1"); const double t = timer.Tac(); const double R = 8; const double W = 5.0, Q = 3.3; diff --git a/samples/hwdrivers_camera_capture_dialog/test.cpp b/samples/hwdrivers_camera_capture_dialog/test.cpp index c62640d345..58a01335ff 100644 --- a/samples/hwdrivers_camera_capture_dialog/test.cpp +++ b/samples/hwdrivers_camera_capture_dialog/test.cpp @@ -93,7 +93,8 @@ void TestCameraCaptureAsk() const std::string sFile = mrpt::format("frame%05i.png", cnt++); cout << "Saving frame to: " << sFile << endl; - img->saveToFile(sFile); + bool savedOk = img->saveToFile(sFile); + ASSERT_(savedOk); } break; default: break; diff --git a/samples/hwdrivers_ntrip_client_example/test.cpp b/samples/hwdrivers_ntrip_client_example/test.cpp index a22f9dbdde..7eab82a4b3 100644 --- a/samples/hwdrivers_ntrip_client_example/test.cpp +++ b/samples/hwdrivers_ntrip_client_example/test.cpp @@ -8,6 +8,7 @@ +------------------------------------------------------------------------+ */ #include +#include #include #include @@ -21,6 +22,9 @@ using namespace mrpt; using namespace mrpt::hwdrivers; using namespace mrpt::system; +std::string ntrip_user = mrpt::get_env("NTRIP_USER"); +std::string ntrip_pass = mrpt::get_env("NTRIP_PASS"); + // ------------------------------------------------------ // TestNTRIP // ------------------------------------------------------ @@ -72,8 +76,14 @@ void TestNTRIP() params.server = server; params.port = server_port; - params.user = ""; - params.password = ""; + cout << "Using user: " << ntrip_user << endl; + cout << "Using pass: " << ntrip_pass << endl; + cout << "(You can change them with env variables NTRIP_USER and " + "NTRIP_PASS" + << endl; + + params.user = ntrip_user; + params.password = ntrip_pass; string msgerr; diff --git a/samples/hwdrivers_sick_serial_example/test.cpp b/samples/hwdrivers_sick_serial_example/test.cpp index f208b28756..99a232b254 100644 --- a/samples/hwdrivers_sick_serial_example/test.cpp +++ b/samples/hwdrivers_sick_serial_example/test.cpp @@ -109,7 +109,8 @@ void TestPLS() std::this_thread::sleep_for(15ms); }; - laser.turnOff(); + bool ok = laser.turnOff(); + (void)ok; } int main(int argc, char** argv) diff --git a/samples/img_basic_example/test.cpp b/samples/img_basic_example/test.cpp index 9e1da194f8..67de6f8ed5 100644 --- a/samples/img_basic_example/test.cpp +++ b/samples/img_basic_example/test.cpp @@ -116,10 +116,12 @@ void TestImageConversion() win4.waitForKey(); tictac.Tic(); - imgGray.saveToFile("frame_out.jpg"); + bool savedOk = imgGray.saveToFile("frame_out.jpg"); + ASSERT_(savedOk); printf("jpeg file saved in %.03fms\n", 1000.0 * tictac.Tac()); - imgSmall2.saveToFile("frame_out_small.png"); + savedOk = imgSmall2.saveToFile("frame_out_small.png"); + ASSERT_(savedOk); return; } diff --git a/samples/img_convolution_fft/test.cpp b/samples/img_convolution_fft/test.cpp index f1d674ec8f..4d47f3f6f0 100644 --- a/samples/img_convolution_fft/test.cpp +++ b/samples/img_convolution_fft/test.cpp @@ -82,11 +82,11 @@ void TestImageConvolutionFFT() { float r1 = I1_R(y, x); float r2 = I2_R(y, x); - float i1 = I1_I(y, x); - float i2 = I2_I(y, x); + float im1 = I1_I(y, x); + float im2 = I2_I(y, x); - I2_R(y, x) = r1 * r2 - i1 * i2; - I2_I(y, x) = r2 * i1 + r1 * i2; + I2_R(y, x) = r1 * r2 - im1 * im2; + I2_I(y, x) = r2 * im1 + r1 * im2; } // IFFT: diff --git a/samples/img_fft_example/test.cpp b/samples/img_fft_example/test.cpp index 6ddc2c154e..8a4024068e 100644 --- a/samples/img_fft_example/test.cpp +++ b/samples/img_fft_example/test.cpp @@ -116,10 +116,13 @@ void TestImageFFT() CImage IM1, IM2; CMatrixF imgCorr; - IM1.loadFromFile( + bool loadOk = IM1.loadFromFile( myDataDir + string("fft2_test_image_patch.jpg"), 0); // "Patch" - IM2.loadFromFile( + ASSERT_(loadOk); + + loadOk = IM2.loadFromFile( myDataDir + string("fft2_test_image.jpg"), 0); // Ref. image + ASSERT_(loadOk); printf("Computing images correlation..."); tictac.Tic(); diff --git a/samples/img_gauss_filtering_example/test.cpp b/samples/img_gauss_filtering_example/test.cpp index 4de4ac1fd5..d563a7f852 100644 --- a/samples/img_gauss_filtering_example/test.cpp +++ b/samples/img_gauss_filtering_example/test.cpp @@ -30,7 +30,8 @@ void Test_GaussWindows() CTicTac tictac; CImage inImg, outImg; - inImg.loadFromFile(myDataDir + "test_in.jpg"); + bool loadOk = inImg.loadFromFile(myDataDir + "test_in.jpg"); + ASSERT_(loadOk); // Smoothed image: // --------------------------- diff --git a/samples/math_csparse_example/test.cpp b/samples/math_csparse_example/test.cpp index 9f16959aca..646bf6da34 100644 --- a/samples/math_csparse_example/test.cpp +++ b/samples/math_csparse_example/test.cpp @@ -35,14 +35,16 @@ void ExampleCSparse() cout << "M (as dense):\n" << M; cout << "Saving to sparse_demo1.txt...\n"; - SM.saveToTextFile_sparse("sparse_demo1.txt"); + bool savedOk = SM.saveToTextFile_sparse("sparse_demo1.txt"); + ASSERT_(savedOk); // Compress from the triplet to the column-compressed form: cout << "Compressing as CCS...\n"; SM.compressFromTriplet(); cout << "Saving to sparse_demo2.txt...\n"; - SM.saveToTextFile_sparse("sparse_demo2.txt"); + savedOk = SM.saveToTextFile_sparse("sparse_demo2.txt"); + ASSERT_(savedOk); // Compute the Cholesky decomposition: CSparseMatrix::CholeskyDecomp Chol(SM); diff --git a/samples/math_slerp_example/test.cpp b/samples/math_slerp_example/test.cpp index 9d12f5049f..3998572cd3 100644 --- a/samples/math_slerp_example/test.cpp +++ b/samples/math_slerp_example/test.cpp @@ -99,9 +99,9 @@ void TestSLERP() mrpt::math::slerp(pose_a, pose_b, t, pose_interp); // Move the scene: - Scene::Ptr& theScene = win.get3DSceneAndLock(); + Scene::Ptr& scene = win.get3DSceneAndLock(); - auto obj1 = theScene->getByName("slerp_obj"); + auto obj1 = scene->getByName("slerp_obj"); obj1->setPose(pose_interp); // Show text: diff --git a/samples/nav_circ_robot_path_planning/test.cpp b/samples/nav_circ_robot_path_planning/test.cpp index 159e85146d..17db932090 100644 --- a/samples/nav_circ_robot_path_planning/test.cpp +++ b/samples/nav_circ_robot_path_planning/test.cpp @@ -107,7 +107,8 @@ void TestPathPlanning() const std::string dest = "path_planning.png"; cout << "Saving output to: " << dest << endl; - img.saveToFile(dest); + bool savedOk = img.saveToFile(dest); + ASSERT_(savedOk); printf("Done\n"); #if MRPT_HAS_WXWIDGETS diff --git a/samples/opengl_offscreen_render_example/test.cpp b/samples/opengl_offscreen_render_example/test.cpp index e4754eec40..72bb3ac489 100644 --- a/samples/opengl_offscreen_render_example/test.cpp +++ b/samples/opengl_offscreen_render_example/test.cpp @@ -60,7 +60,8 @@ int TestOffscreenRender(int argc, char* argv[]) // render the scene render.render_RGB(scene, frame); - frame.saveToFile(sOut); + bool savedOk = frame.saveToFile(sOut); + ASSERT_(savedOk); return 0; } diff --git a/samples/rgbd_dataset2rawlog/test.cpp b/samples/rgbd_dataset2rawlog/test.cpp index da11edcac9..cd9cbb0fb6 100644 --- a/samples/rgbd_dataset2rawlog/test.cpp +++ b/samples/rgbd_dataset2rawlog/test.cpp @@ -195,12 +195,16 @@ void rgbd2rawlog(const string& src_path, const string& out_name) // RGB img: obs.hasIntensityImage = true; - obs.intensityImage.loadFromFile( + bool loadOk = obs.intensityImage.loadFromFile( src_path + string("/") + it_list_rgb->second); + ASSERT_(loadOk); + const string sRGBfile = mrpt::format("%.06f_rgb.png", avrg_time); - obs.intensityImage.saveToFile( + bool savedOk = obs.intensityImage.saveToFile( out_img_dir + string("/") + sRGBfile); + ASSERT_(savedOk); + obs.intensityImage.setExternalStorage(sRGBfile); // Depth: @@ -307,10 +311,14 @@ void rgbd2rawlog(const string& src_path, const string& out_name) // RGB img: obs.hasIntensityImage = true; - obs.intensityImage.loadFromFile( + bool loadOk = obs.intensityImage.loadFromFile( src_path + string("/") + it_list_rgb->second); + ASSERT_(loadOk); + const string sRGBfile = mrpt::format("%.06f_rgb.png", avrg_time); - obs.intensityImage.saveToFile(out_img_dir + string("/") + sRGBfile); + bool savedOk = obs.intensityImage.saveToFile( + out_img_dir + string("/") + sRGBfile); + ASSERT_(savedOk); obs.intensityImage.setExternalStorage(sRGBfile); // save: diff --git a/samples/vision_feature_extraction/test.cpp b/samples/vision_feature_extraction/test.cpp index 4526255ff0..fabb558640 100644 --- a/samples/vision_feature_extraction/test.cpp +++ b/samples/vision_feature_extraction/test.cpp @@ -257,8 +257,11 @@ void TestMatchingComparative() string("imgs/imR_p01.jpg"); // Right image CImage im1, im2; - im1.loadFromFile(imgL); - im2.loadFromFile(imgR); + bool loadOk = im1.loadFromFile(imgL); + ASSERT_(loadOk); + + loadOk = im2.loadFromFile(imgR); + ASSERT_(loadOk); size_t imW = im1.getWidth(); size_t imH = im1.getHeight(); diff --git a/samples/vision_keypoint_matching_example/test.cpp b/samples/vision_keypoint_matching_example/test.cpp index 21fd864701..3d7411333e 100644 --- a/samples/vision_keypoint_matching_example/test.cpp +++ b/samples/vision_keypoint_matching_example/test.cpp @@ -276,8 +276,11 @@ void TestMatchingComparative() string imgR = myDataDir + string("imR_p01.jpg"); // Right image CImage im1, im2; - im1.loadFromFile(imgL); - im2.loadFromFile(imgR); + bool loadOk = im1.loadFromFile(imgL); + ASSERT_(loadOk); + + loadOk = im2.loadFromFile(imgR); + ASSERT_(loadOk); size_t imW = im1.getWidth(); size_t imH = im1.getHeight(); diff --git a/share/mrpt/config_files/simul-beacons/example_simul-beacons.ini b/share/mrpt/config_files/simul-beacons/example_simul-beacons.ini index a938c10639..09ac542125 100644 --- a/share/mrpt/config_files/simul-beacons/example_simul-beacons.ini +++ b/share/mrpt/config_files/simul-beacons/example_simul-beacons.ini @@ -4,9 +4,9 @@ #------------------------------------------------------ [Params] -nBeacons=4 // Number of beacons -nSteps=1000 // Number of steps to simulate -circularPath=0 // Circular/Square paths +nBeacons=14 // Number of beacons +nSteps=100 // Number of steps to simulate +circularPath=1 // Circular/Square paths squarePathLength=20 // "Steps" or length of the square path (integer) min_x=-4 diff --git a/share/mrpt/datasets/RO-SLAM_demo.rawlog b/share/mrpt/datasets/RO-SLAM_demo.rawlog index bfb3fbfaf6..cb9e3ece12 100644 Binary files a/share/mrpt/datasets/RO-SLAM_demo.rawlog and b/share/mrpt/datasets/RO-SLAM_demo.rawlog differ diff --git a/share/mrpt/datasets/RO-SLAM_demo_ground_truth.txt b/share/mrpt/datasets/RO-SLAM_demo_ground_truth.txt index 3107383109..4d07faada4 100644 --- a/share/mrpt/datasets/RO-SLAM_demo_ground_truth.txt +++ b/share/mrpt/datasets/RO-SLAM_demo_ground_truth.txt @@ -1,15 +1,15 @@ -0 -0.456624 3.616688 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -1 -0.068401 -0.778283 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -2 -3.653335 3.325381 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -3 3.494198 6.217460 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -4 4.636356 2.677605 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -5 -0.185889 2.381078 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -6 5.920600 4.529777 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -7 1.493879 7.689046 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -8 0.131672 1.307529 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -9 -3.070301 -3.740870 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -10 -3.549153 -0.197495 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -11 1.218614 3.734104 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -12 -2.031066 4.080770 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -13 2.521068 -0.425033 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 -14 3.753711 -3.788677 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +% ID X Y Z C(0,0) C(1,1) C(2,2) D2 |C| C(0,1) C(1,2) C(1,1) +0 -3.150245 -0.324123 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +1 2.703717 -1.159713 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +2 0.282258 -1.790852 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +3 -1.161647 3.591730 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +4 -2.909660 3.713527 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +5 -1.789188 -1.355850 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +6 2.213303 -1.472485 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +7 -2.602169 -0.838980 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +8 0.133487 -3.978425 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +9 -2.204788 3.109222 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +10 3.388271 -3.234335 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +11 2.649972 0.784999 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +12 1.103943 -0.337452 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 +13 2.827560 -2.350020 0.000000 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 diff --git a/share/mrpt/datasets/kf-slam_demo.rawlog b/share/mrpt/datasets/kf-slam_demo.rawlog index 3feb5ee260..a1e0ec232c 100644 Binary files a/share/mrpt/datasets/kf-slam_demo.rawlog and b/share/mrpt/datasets/kf-slam_demo.rawlog differ diff --git a/version_prefix.txt b/version_prefix.txt index 038ecfc570..59691f2a48 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.12.0 +2.12.1 # IMPORTANT: This file is parsed by CMake, don't add any comment to # the first line. # This file is used in both Windows and Linux scripts to automatically