From 143defcfc1ee163bfeb8fb5dca1d329ab6bcfed6 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 28 Sep 2023 22:58:51 -0700 Subject: [PATCH 1/2] fixed the order of translation multiplication --- src/device/CalibrationHandler.cpp | 42 +++++++++++++++---------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 9e0e1a22a..2291878c3 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -419,24 +419,24 @@ std::vector> CalibrationHandler::getImuToCameraExtrinsics(Cam throw std::runtime_error("There is no Camera data available corresponding to the requested source cameraId"); } - std::vector> transformationMatrix = eepromData.imuExtrinsics.rotationMatrix; + std::vector> currTransformationMatrixImu = eepromData.imuExtrinsics.rotationMatrix; if(useSpecTranslation) { - transformationMatrix[0].push_back(eepromData.imuExtrinsics.specTranslation.x); - transformationMatrix[1].push_back(eepromData.imuExtrinsics.specTranslation.y); - transformationMatrix[2].push_back(eepromData.imuExtrinsics.specTranslation.z); + currTransformationMatrixImu[0].push_back(eepromData.imuExtrinsics.specTranslation.x); + currTransformationMatrixImu[1].push_back(eepromData.imuExtrinsics.specTranslation.y); + currTransformationMatrixImu[2].push_back(eepromData.imuExtrinsics.specTranslation.z); } else { - transformationMatrix[0].push_back(eepromData.imuExtrinsics.translation.x); - transformationMatrix[1].push_back(eepromData.imuExtrinsics.translation.y); - transformationMatrix[2].push_back(eepromData.imuExtrinsics.translation.z); + currTransformationMatrixImu[0].push_back(eepromData.imuExtrinsics.translation.x); + currTransformationMatrixImu[1].push_back(eepromData.imuExtrinsics.translation.y); + currTransformationMatrixImu[2].push_back(eepromData.imuExtrinsics.translation.z); } std::vector homogeneous_vector = {0, 0, 0, 1}; - transformationMatrix.push_back(homogeneous_vector); + currTransformationMatrixImu.push_back(homogeneous_vector); if(eepromData.imuExtrinsics.toCameraSocket == cameraId) { - return transformationMatrix; + return currTransformationMatrixImu; } else { - std::vector> localTransformationMatrix = getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation); - return matMul(transformationMatrix, localTransformationMatrix); + std::vector> destTransformationMatrixCurr = getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation); + return matMul(destTransformationMatrixCurr, currTransformationMatrixImu); } } @@ -506,25 +506,25 @@ std::vector> CalibrationHandler::computeExtrinsicMatrix(Camer transformationMatrix.push_back(homogeneous_vector); return transformationMatrix; } else { - std::vector> futureTransformationMatrix = + std::vector> destTransformationMatrixCurr = computeExtrinsicMatrix(eepromData.cameraData.at(srcCamera).extrinsics.toCameraSocket, dstCamera, useSpecTranslation); - std::vector> currTransformationMatrix = eepromData.cameraData.at(srcCamera).extrinsics.rotationMatrix; + std::vector> currTransformationMatrixSrc = eepromData.cameraData.at(srcCamera).extrinsics.rotationMatrix; if(useSpecTranslation) { const dai::Point3f& mTrans = eepromData.cameraData.at(srcCamera).extrinsics.specTranslation; if(mTrans.x == 0 && mTrans.y == 0 && mTrans.z == 0) { throw std::runtime_error("Cannot use useSpecTranslation argument since specTranslation has {0, 0, 0}"); } - currTransformationMatrix[0].push_back(eepromData.cameraData.at(srcCamera).extrinsics.specTranslation.x); - currTransformationMatrix[1].push_back(eepromData.cameraData.at(srcCamera).extrinsics.specTranslation.y); - currTransformationMatrix[2].push_back(eepromData.cameraData.at(srcCamera).extrinsics.specTranslation.z); + currTransformationMatrixSrc[0].push_back(eepromData.cameraData.at(srcCamera).extrinsics.specTranslation.x); + currTransformationMatrixSrc[1].push_back(eepromData.cameraData.at(srcCamera).extrinsics.specTranslation.y); + currTransformationMatrixSrc[2].push_back(eepromData.cameraData.at(srcCamera).extrinsics.specTranslation.z); } else { - currTransformationMatrix[0].push_back(eepromData.cameraData.at(srcCamera).extrinsics.translation.x); - currTransformationMatrix[1].push_back(eepromData.cameraData.at(srcCamera).extrinsics.translation.y); - currTransformationMatrix[2].push_back(eepromData.cameraData.at(srcCamera).extrinsics.translation.z); + currTransformationMatrixSrc[0].push_back(eepromData.cameraData.at(srcCamera).extrinsics.translation.x); + currTransformationMatrixSrc[1].push_back(eepromData.cameraData.at(srcCamera).extrinsics.translation.y); + currTransformationMatrixSrc[2].push_back(eepromData.cameraData.at(srcCamera).extrinsics.translation.z); } std::vector homogeneous_vector = {0, 0, 0, 1}; - currTransformationMatrix.push_back(homogeneous_vector); - return matMul(currTransformationMatrix, futureTransformationMatrix); + currTransformationMatrixSrc.push_back(homogeneous_vector); + return matMul(destTransformationMatrixCurr, currTransformationMatrixSrc); } } From 02b1be7b699f6f36060bd48b6adb970d212b3e22 Mon Sep 17 00:00:00 2001 From: saching13 Date: Thu, 28 Sep 2023 23:10:09 -0700 Subject: [PATCH 2/2] clang update --- src/device/CalibrationHandler.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/device/CalibrationHandler.cpp b/src/device/CalibrationHandler.cpp index 2291878c3..cf74c1361 100644 --- a/src/device/CalibrationHandler.cpp +++ b/src/device/CalibrationHandler.cpp @@ -435,7 +435,8 @@ std::vector> CalibrationHandler::getImuToCameraExtrinsics(Cam if(eepromData.imuExtrinsics.toCameraSocket == cameraId) { return currTransformationMatrixImu; } else { - std::vector> destTransformationMatrixCurr = getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation); + std::vector> destTransformationMatrixCurr = + getCameraExtrinsics(eepromData.imuExtrinsics.toCameraSocket, cameraId, useSpecTranslation); return matMul(destTransformationMatrixCurr, currTransformationMatrixImu); } }