From e96166a47b66c4b0cc277cf1e785224c84049100 Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Sun, 23 Apr 2023 11:57:27 +0300 Subject: [PATCH 1/8] edited cmake lists to match raspberry and include test file --- CMakeLists.txt | 236 +++++++++++-------- build.sh | 8 +- src/MLPnPsolver.cpp | 543 ++++++++++++++++++++++++-------------------- src/test.cpp | 23 ++ 4 files changed, 467 insertions(+), 343 deletions(-) create mode 100644 src/test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 016e74354d..c97a9895cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8) project(ORB_SLAM3) IF(NOT CMAKE_BUILD_TYPE) - SET(CMAKE_BUILD_TYPE Release) + SET(CMAKE_BUILD_TYPE Release) ENDIF() MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) @@ -12,127 +12,176 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3") set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native") set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native") -# Check C++11 or C++0x support +# Check C++14 or C++0x support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) if(COMPILER_SUPPORTS_CXX11) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - add_definitions(-DCOMPILEDWITHC11) - message(STATUS "Using flag -std=c++11.") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") + add_definitions(-DCOMPILEDWITHC11) + message(STATUS "Using flag -std=c++14.") elseif(COMPILER_SUPPORTS_CXX0X) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") - add_definitions(-DCOMPILEDWITHC0X) - message(STATUS "Using flag -std=c++0x.") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") + add_definitions(-DCOMPILEDWITHC0X) + message(STATUS "Using flag -std=c++0x.") else() - message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules) -find_package(OpenCV 4.4) - if(NOT OpenCV_FOUND) - message(FATAL_ERROR "OpenCV > 4.4 not found.") - endif() +#find_package(OpenCV 4.4) +# if(NOT OpenCV_FOUND) +# message(FATAL_ERROR "OpenCV > 4.4 not found.") +# endif() -MESSAGE("OPENCV VERSION:") -MESSAGE(${OpenCV_VERSION}) +#MESSAGE("OPENCV VERSION:") +#MESSAGE(${OpenCV_VERSION}) find_package(Eigen3 3.1.0 REQUIRED) find_package(Pangolin REQUIRED) find_package(realsense2) include_directories( -${PROJECT_SOURCE_DIR} -${PROJECT_SOURCE_DIR}/include -${PROJECT_SOURCE_DIR}/include/CameraModels -${PROJECT_SOURCE_DIR}/Thirdparty/Sophus -${EIGEN3_INCLUDE_DIR} -${Pangolin_INCLUDE_DIRS} + ${PROJECT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/include/CameraModels + ${PROJECT_SOURCE_DIR}/Thirdparty/Sophus + ${EIGEN3_INCLUDE_DIR} + ${Pangolin_INCLUDE_DIRS} ) set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) add_library(${PROJECT_NAME} SHARED -src/System.cc -src/Tracking.cc -src/LocalMapping.cc -src/LoopClosing.cc -src/ORBextractor.cc -src/ORBmatcher.cc -src/FrameDrawer.cc -src/Converter.cc -src/MapPoint.cc -src/KeyFrame.cc -src/Atlas.cc -src/Map.cc -src/MapDrawer.cc -src/Optimizer.cc -src/Frame.cc -src/KeyFrameDatabase.cc -src/Sim3Solver.cc -src/Viewer.cc -src/ImuTypes.cc -src/G2oTypes.cc -src/CameraModels/Pinhole.cpp -src/CameraModels/KannalaBrandt8.cpp -src/OptimizableTypes.cpp -src/MLPnPsolver.cpp -src/GeometricTools.cc -src/TwoViewReconstruction.cc -src/Config.cc -src/Settings.cc -include/System.h -include/Tracking.h -include/LocalMapping.h -include/LoopClosing.h -include/ORBextractor.h -include/ORBmatcher.h -include/FrameDrawer.h -include/Converter.h -include/MapPoint.h -include/KeyFrame.h -include/Atlas.h -include/Map.h -include/MapDrawer.h -include/Optimizer.h -include/Frame.h -include/KeyFrameDatabase.h -include/Sim3Solver.h -include/Viewer.h -include/ImuTypes.h -include/G2oTypes.h -include/CameraModels/GeometricCamera.h -include/CameraModels/Pinhole.h -include/CameraModels/KannalaBrandt8.h -include/OptimizableTypes.h -include/MLPnPsolver.h -include/GeometricTools.h -include/TwoViewReconstruction.h -include/SerializationUtils.h -include/Config.h -include/Settings.h) + src/System.cc + src/Tracking.cc + src/LocalMapping.cc + src/LoopClosing.cc + src/ORBextractor.cc + src/ORBmatcher.cc + src/FrameDrawer.cc + src/Converter.cc + src/MapPoint.cc + src/KeyFrame.cc + src/Atlas.cc + src/Map.cc + src/MapDrawer.cc + src/Optimizer.cc + src/Frame.cc + src/KeyFrameDatabase.cc + src/Sim3Solver.cc + src/Viewer.cc + src/ImuTypes.cc + src/G2oTypes.cc + src/CameraModels/Pinhole.cpp + src/CameraModels/KannalaBrandt8.cpp + src/OptimizableTypes.cpp + src/MLPnPsolver.cpp + src/GeometricTools.cc + src/TwoViewReconstruction.cc + src/Config.cc + src/Settings.cc + src/test.cpp + include/System.h + include/Tracking.h + include/LocalMapping.h + include/LoopClosing.h + include/ORBextractor.h + include/ORBmatcher.h + include/FrameDrawer.h + include/Converter.h + include/MapPoint.h + include/KeyFrame.h + include/Atlas.h + include/Map.h + include/MapDrawer.h + include/Optimizer.h + include/Frame.h + include/KeyFrameDatabase.h + include/Sim3Solver.h + include/Viewer.h + include/ImuTypes.h + include/G2oTypes.h + include/CameraModels/GeometricCamera.h + include/CameraModels/Pinhole.h + include/CameraModels/KannalaBrandt8.h + include/OptimizableTypes.h + include/MLPnPsolver.h + include/GeometricTools.h + include/TwoViewReconstruction.h + include/SerializationUtils.h + include/Config.h + include/Settings.h) add_subdirectory(Thirdparty/g2o) target_link_libraries(${PROJECT_NAME} -${OpenCV_LIBS} -${EIGEN3_LIBS} -${Pangolin_LIBRARIES} -${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so -${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so --lboost_serialization --lcrypto -) + ${OpenCV_LIBS} + ${EIGEN3_LIBS} + ${Pangolin_LIBRARIES} + ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so + ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so + #/usr/local/lib/arm-linux-gnueabihf/libopencv_core.so + /usr/local/lib/aarch64-linux-gnu/libopencv_aruco.so + /usr/local/lib/aarch64-linux-gnu/libopencv_bgsegm.so + /usr/local/lib/aarch64-linux-gnu/libopencv_bioinspired.so + /usr/local/lib/aarch64-linux-gnu/libopencv_calib3d.so + /usr/local/lib/aarch64-linux-gnu/libopencv_ccalib.so + /usr/local/lib/aarch64-linux-gnu/libopencv_core.so + /usr/local/lib/aarch64-linux-gnu/libopencv_datasets.so + /usr/local/lib/aarch64-linux-gnu/libopencv_dnn_objdetect.so + /usr/local/lib/aarch64-linux-gnu/libopencv_dnn.so + /usr/local/lib/aarch64-linux-gnu/libopencv_dpm.so + /usr/local/lib/aarch64-linux-gnu/libopencv_face.so + /usr/local/lib/aarch64-linux-gnu/libopencv_features2d.so + /usr/local/lib/aarch64-linux-gnu/libopencv_flann.so + /usr/local/lib/aarch64-linux-gnu/libopencv_freetype.so + /usr/local/lib/aarch64-linux-gnu/libopencv_fuzzy.so + /usr/local/lib/aarch64-linux-gnu/libopencv_hdf.so + /usr/local/lib/aarch64-linux-gnu/libopencv_hfs.so + /usr/local/lib/aarch64-linux-gnu/libopencv_highgui.so + /usr/local/lib/aarch64-linux-gnu/libopencv_imgcodecs.so + /usr/local/lib/aarch64-linux-gnu/libopencv_img_hash.so + /usr/local/lib/aarch64-linux-gnu/libopencv_imgproc.so + /usr/local/lib/aarch64-linux-gnu/libopencv_line_descriptor.so + /usr/local/lib/aarch64-linux-gnu/libopencv_ml.so + /usr/local/lib/aarch64-linux-gnu/libopencv_objdetect.so + /usr/local/lib/aarch64-linux-gnu/libopencv_optflow.so + /usr/local/lib/aarch64-linux-gnu/libopencv_phase_unwrapping.so + /usr/local/lib/aarch64-linux-gnu/libopencv_photo.so + /usr/local/lib/aarch64-linux-gnu/libopencv_plot.so + /usr/local/lib/aarch64-linux-gnu/libopencv_reg.so + /usr/local/lib/aarch64-linux-gnu/libopencv_rgbd.so + /usr/local/lib/aarch64-linux-gnu/libopencv_saliency.so + /usr/local/lib/aarch64-linux-gnu/libopencv_sfm.so + /usr/local/lib/aarch64-linux-gnu/libopencv_shape.so + /usr/local/lib/aarch64-linux-gnu/libopencv_stereo.so + /usr/local/lib/aarch64-linux-gnu/libopencv_stitching.so + /usr/local/lib/aarch64-linux-gnu/libopencv_structured_light.so + /usr/local/lib/aarch64-linux-gnu/libopencv_superres.so + /usr/local/lib/aarch64-linux-gnu/libopencv_surface_matching.so + /usr/local/lib/aarch64-linux-gnu/libopencv_text.so + /usr/local/lib/aarch64-linux-gnu/libopencv_tracking.so + /usr/local/lib/aarch64-linux-gnu/libopencv_videoio.so + /usr/local/lib/aarch64-linux-gnu/libopencv_video.so + /usr/local/lib/aarch64-linux-gnu/libopencv_videostab.so + /usr/local/lib/aarch64-linux-gnu/libopencv_xfeatures2d.so + /usr/local/lib/aarch64-linux-gnu/libopencv_ximgproc.so + /usr/local/lib/aarch64-linux-gnu/libopencv_xobjdetect.so + /usr/local/lib/aarch64-linux-gnu/libopencv_xphoto.so + -lboost_serialization + -lcrypto + ) # If RealSense SDK is found the library is added and its examples compiled if(realsense2_FOUND) include_directories(${PROJECT_NAME} - ${realsense_INCLUDE_DIR} - ) + ${realsense_INCLUDE_DIR} + ) target_link_libraries(${PROJECT_NAME} - ${realsense2_LIBRARY} - ) + ${realsense2_LIBRARY} + ) endif() @@ -388,3 +437,6 @@ if(realsense2_FOUND) Examples_old/Stereo-Inertial/stereo_inertial_realsense_D435i.cc) target_link_libraries(stereo_inertial_realsense_D435i_old ${PROJECT_NAME}) endif() + + + diff --git a/build.sh b/build.sh index 96d1c0941c..12e735f22f 100755 --- a/build.sh +++ b/build.sh @@ -4,7 +4,7 @@ cd Thirdparty/DBoW2 mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +make cd ../../g2o @@ -13,7 +13,7 @@ echo "Configuring and building Thirdparty/g2o ..." mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +make cd ../../Sophus @@ -22,7 +22,7 @@ echo "Configuring and building Thirdparty/Sophus ..." mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j +make cd ../../../ @@ -37,4 +37,4 @@ echo "Configuring and building ORB_SLAM3 ..." mkdir build cd build cmake .. -DCMAKE_BUILD_TYPE=Release -make -j4 +make diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 2f8702b41a..283a36e178 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -125,7 +125,7 @@ namespace ORB_SLAM3 { vector indexes(mRansacMinSet); // Get min set of points - for(short i = 0; i < mRansacMinSet; ++i) + for(int i = 0; i < mRansacMinSet; ++i) { int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1); @@ -806,251 +806,300 @@ namespace ORB_SLAM3 { } void MLPnPsolver::mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r, - const Eigen::Vector3d& nullspace_s, const rodrigues_t& w, - const translation_t& t, Eigen::MatrixXd& jacs){ - double r1 = nullspace_r[0]; - double r2 = nullspace_r[1]; - double r3 = nullspace_r[2]; - - double s1 = nullspace_s[0]; - double s2 = nullspace_s[1]; - double s3 = nullspace_s[2]; - - double X1 = pt[0]; - double Y1 = pt[1]; - double Z1 = pt[2]; - - double w1 = w[0]; - double w2 = w[1]; - double w3 = w[2]; - - double t1 = t[0]; - double t2 = t[1]; - double t3 = t[2]; - - double t5 = w1*w1; - double t6 = w2*w2; - double t7 = w3*w3; - double t8 = t5+t6+t7; - double t9 = sqrt(t8); - double t10 = sin(t9); - double t11 = 1.0/sqrt(t8); - double t12 = cos(t9); - double t13 = t12-1.0; - double t14 = 1.0/t8; - double t16 = t10*t11*w3; - double t17 = t13*t14*w1*w2; - double t19 = t10*t11*w2; - double t20 = t13*t14*w1*w3; - double t24 = t6+t7; - double t27 = t16+t17; - double t28 = Y1*t27; - double t29 = t19-t20; - double t30 = Z1*t29; - double t31 = t13*t14*t24; - double t32 = t31+1.0; - double t33 = X1*t32; - double t15 = t1-t28+t30+t33; - double t21 = t10*t11*w1; - double t22 = t13*t14*w2*w3; - double t45 = t5+t7; - double t53 = t16-t17; - double t54 = X1*t53; - double t55 = t21+t22; - double t56 = Z1*t55; - double t57 = t13*t14*t45; - double t58 = t57+1.0; - double t59 = Y1*t58; - double t18 = t2+t54-t56+t59; - double t34 = t5+t6; - double t38 = t19+t20; - double t39 = X1*t38; - double t40 = t21-t22; - double t41 = Y1*t40; - double t42 = t13*t14*t34; - double t43 = t42+1.0; - double t44 = Z1*t43; - double t23 = t3-t39+t41+t44; - double t25 = 1.0/pow(t8,3.0/2.0); - double t26 = 1.0/(t8*t8); - double t35 = t12*t14*w1*w2; - double t36 = t5*t10*t25*w3; - double t37 = t5*t13*t26*w3*2.0; - double t46 = t10*t25*w1*w3; - double t47 = t5*t10*t25*w2; - double t48 = t5*t13*t26*w2*2.0; - double t49 = t10*t11; - double t50 = t5*t12*t14; - double t51 = t13*t26*w1*w2*w3*2.0; - double t52 = t10*t25*w1*w2*w3; - double t60 = t15*t15; - double t61 = t18*t18; - double t62 = t23*t23; - double t63 = t60+t61+t62; - double t64 = t5*t10*t25; - double t65 = 1.0/sqrt(t63); - double t66 = Y1*r2*t6; - double t67 = Z1*r3*t7; - double t68 = r1*t1*t5; - double t69 = r1*t1*t6; - double t70 = r1*t1*t7; - double t71 = r2*t2*t5; - double t72 = r2*t2*t6; - double t73 = r2*t2*t7; - double t74 = r3*t3*t5; - double t75 = r3*t3*t6; - double t76 = r3*t3*t7; - double t77 = X1*r1*t5; - double t78 = X1*r2*w1*w2; - double t79 = X1*r3*w1*w3; - double t80 = Y1*r1*w1*w2; - double t81 = Y1*r3*w2*w3; - double t82 = Z1*r1*w1*w3; - double t83 = Z1*r2*w2*w3; - double t84 = X1*r1*t6*t12; - double t85 = X1*r1*t7*t12; - double t86 = Y1*r2*t5*t12; - double t87 = Y1*r2*t7*t12; - double t88 = Z1*r3*t5*t12; - double t89 = Z1*r3*t6*t12; - double t90 = X1*r2*t9*t10*w3; - double t91 = Y1*r3*t9*t10*w1; - double t92 = Z1*r1*t9*t10*w2; - double t102 = X1*r3*t9*t10*w2; - double t103 = Y1*r1*t9*t10*w3; - double t104 = Z1*r2*t9*t10*w1; - double t105 = X1*r2*t12*w1*w2; - double t106 = X1*r3*t12*w1*w3; - double t107 = Y1*r1*t12*w1*w2; - double t108 = Y1*r3*t12*w2*w3; - double t109 = Z1*r1*t12*w1*w3; - double t110 = Z1*r2*t12*w2*w3; - double t93 = t66+t67+t68+t69+t70+t71+t72+t73+t74+t75+t76+t77+t78+t79+t80+t81+t82+t83+t84+t85+t86+t87+t88+t89+t90+t91+t92-t102-t103-t104-t105-t106-t107-t108-t109-t110; - double t94 = t10*t25*w1*w2; - double t95 = t6*t10*t25*w3; - double t96 = t6*t13*t26*w3*2.0; - double t97 = t12*t14*w2*w3; - double t98 = t6*t10*t25*w1; - double t99 = t6*t13*t26*w1*2.0; - double t100 = t6*t10*t25; - double t101 = 1.0/pow(t63,3.0/2.0); - double t111 = t6*t12*t14; - double t112 = t10*t25*w2*w3; - double t113 = t12*t14*w1*w3; - double t114 = t7*t10*t25*w2; - double t115 = t7*t13*t26*w2*2.0; - double t116 = t7*t10*t25*w1; - double t117 = t7*t13*t26*w1*2.0; - double t118 = t7*t12*t14; - double t119 = t13*t24*t26*w1*2.0; - double t120 = t10*t24*t25*w1; - double t121 = t119+t120; - double t122 = t13*t26*t34*w1*2.0; - double t123 = t10*t25*t34*w1; - double t131 = t13*t14*w1*2.0; - double t124 = t122+t123-t131; - double t139 = t13*t14*w3; - double t125 = -t35+t36+t37+t94-t139; - double t126 = X1*t125; - double t127 = t49+t50+t51+t52-t64; - double t128 = Y1*t127; - double t129 = t126+t128-Z1*t124; - double t130 = t23*t129*2.0; - double t132 = t13*t26*t45*w1*2.0; - double t133 = t10*t25*t45*w1; - double t138 = t13*t14*w2; - double t134 = -t46+t47+t48+t113-t138; - double t135 = X1*t134; - double t136 = -t49-t50+t51+t52+t64; - double t137 = Z1*t136; - double t140 = X1*s1*t5; - double t141 = Y1*s2*t6; - double t142 = Z1*s3*t7; - double t143 = s1*t1*t5; - double t144 = s1*t1*t6; - double t145 = s1*t1*t7; - double t146 = s2*t2*t5; - double t147 = s2*t2*t6; - double t148 = s2*t2*t7; - double t149 = s3*t3*t5; - double t150 = s3*t3*t6; - double t151 = s3*t3*t7; - double t152 = X1*s2*w1*w2; - double t153 = X1*s3*w1*w3; - double t154 = Y1*s1*w1*w2; - double t155 = Y1*s3*w2*w3; - double t156 = Z1*s1*w1*w3; - double t157 = Z1*s2*w2*w3; - double t158 = X1*s1*t6*t12; - double t159 = X1*s1*t7*t12; - double t160 = Y1*s2*t5*t12; - double t161 = Y1*s2*t7*t12; - double t162 = Z1*s3*t5*t12; - double t163 = Z1*s3*t6*t12; - double t164 = X1*s2*t9*t10*w3; - double t165 = Y1*s3*t9*t10*w1; - double t166 = Z1*s1*t9*t10*w2; - double t183 = X1*s3*t9*t10*w2; - double t184 = Y1*s1*t9*t10*w3; - double t185 = Z1*s2*t9*t10*w1; - double t186 = X1*s2*t12*w1*w2; - double t187 = X1*s3*t12*w1*w3; - double t188 = Y1*s1*t12*w1*w2; - double t189 = Y1*s3*t12*w2*w3; - double t190 = Z1*s1*t12*w1*w3; - double t191 = Z1*s2*t12*w2*w3; - double t167 = t140+t141+t142+t143+t144+t145+t146+t147+t148+t149+t150+t151+t152+t153+t154+t155+t156+t157+t158+t159+t160+t161+t162+t163+t164+t165+t166-t183-t184-t185-t186-t187-t188-t189-t190-t191; - double t168 = t13*t26*t45*w2*2.0; - double t169 = t10*t25*t45*w2; - double t170 = t168+t169; - double t171 = t13*t26*t34*w2*2.0; - double t172 = t10*t25*t34*w2; - double t176 = t13*t14*w2*2.0; - double t173 = t171+t172-t176; - double t174 = -t49+t51+t52+t100-t111; - double t175 = X1*t174; - double t177 = t13*t24*t26*w2*2.0; - double t178 = t10*t24*t25*w2; - double t192 = t13*t14*w1; - double t179 = -t97+t98+t99+t112-t192; - double t180 = Y1*t179; - double t181 = t49+t51+t52-t100+t111; - double t182 = Z1*t181; - double t193 = t13*t26*t34*w3*2.0; - double t194 = t10*t25*t34*w3; - double t195 = t193+t194; - double t196 = t13*t26*t45*w3*2.0; - double t197 = t10*t25*t45*w3; - double t200 = t13*t14*w3*2.0; - double t198 = t196+t197-t200; - double t199 = t7*t10*t25; - double t201 = t13*t24*t26*w3*2.0; - double t202 = t10*t24*t25*w3; - double t203 = -t49+t51+t52-t118+t199; - double t204 = Y1*t203; - double t205 = t1*2.0; - double t206 = Z1*t29*2.0; - double t207 = X1*t32*2.0; - double t208 = t205+t206+t207-Y1*t27*2.0; - double t209 = t2*2.0; - double t210 = X1*t53*2.0; - double t211 = Y1*t58*2.0; - double t212 = t209+t210+t211-Z1*t55*2.0; - double t213 = t3*2.0; - double t214 = Y1*t40*2.0; - double t215 = Z1*t43*2.0; - double t216 = t213+t214+t215-X1*t38*2.0; - jacs(0, 0) = t14*t65*(X1*r1*w1*2.0+X1*r2*w2+X1*r3*w3+Y1*r1*w2+Z1*r1*w3+r1*t1*w1*2.0+r2*t2*w1*2.0+r3*t3*w1*2.0+Y1*r3*t5*t12+Y1*r3*t9*t10-Z1*r2*t5*t12-Z1*r2*t9*t10-X1*r2*t12*w2-X1*r3*t12*w3-Y1*r1*t12*w2+Y1*r2*t12*w1*2.0-Z1*r1*t12*w3+Z1*r3*t12*w1*2.0+Y1*r3*t5*t10*t11-Z1*r2*t5*t10*t11+X1*r2*t12*w1*w3-X1*r3*t12*w1*w2-Y1*r1*t12*w1*w3+Z1*r1*t12*w1*w2-Y1*r1*t10*t11*w1*w3+Z1*r1*t10*t11*w1*w2-X1*r1*t6*t10*t11*w1-X1*r1*t7*t10*t11*w1+X1*r2*t5*t10*t11*w2+X1*r3*t5*t10*t11*w3+Y1*r1*t5*t10*t11*w2-Y1*r2*t5*t10*t11*w1-Y1*r2*t7*t10*t11*w1+Z1*r1*t5*t10*t11*w3-Z1*r3*t5*t10*t11*w1-Z1*r3*t6*t10*t11*w1+X1*r2*t10*t11*w1*w3-X1*r3*t10*t11*w1*w2+Y1*r3*t10*t11*w1*w2*w3+Z1*r2*t10*t11*w1*w2*w3)-t26*t65*t93*w1*2.0-t14*t93*t101*(t130+t15*(-X1*t121+Y1*(t46+t47+t48-t13*t14*w2-t12*t14*w1*w3)+Z1*(t35+t36+t37-t13*t14*w3-t10*t25*w1*w2))*2.0+t18*(t135+t137-Y1*(t132+t133-t13*t14*w1*2.0))*2.0)*(1.0/2.0); - jacs(0, 1) = t14*t65*(X1*r2*w1+Y1*r1*w1+Y1*r2*w2*2.0+Y1*r3*w3+Z1*r2*w3+r1*t1*w2*2.0+r2*t2*w2*2.0+r3*t3*w2*2.0-X1*r3*t6*t12-X1*r3*t9*t10+Z1*r1*t6*t12+Z1*r1*t9*t10+X1*r1*t12*w2*2.0-X1*r2*t12*w1-Y1*r1*t12*w1-Y1*r3*t12*w3-Z1*r2*t12*w3+Z1*r3*t12*w2*2.0-X1*r3*t6*t10*t11+Z1*r1*t6*t10*t11+X1*r2*t12*w2*w3-Y1*r1*t12*w2*w3+Y1*r3*t12*w1*w2-Z1*r2*t12*w1*w2-Y1*r1*t10*t11*w2*w3+Y1*r3*t10*t11*w1*w2-Z1*r2*t10*t11*w1*w2-X1*r1*t6*t10*t11*w2+X1*r2*t6*t10*t11*w1-X1*r1*t7*t10*t11*w2+Y1*r1*t6*t10*t11*w1-Y1*r2*t5*t10*t11*w2-Y1*r2*t7*t10*t11*w2+Y1*r3*t6*t10*t11*w3-Z1*r3*t5*t10*t11*w2+Z1*r2*t6*t10*t11*w3-Z1*r3*t6*t10*t11*w2+X1*r2*t10*t11*w2*w3+X1*r3*t10*t11*w1*w2*w3+Z1*r1*t10*t11*w1*w2*w3)-t26*t65*t93*w2*2.0-t14*t93*t101*(t18*(Z1*(-t35+t94+t95+t96-t13*t14*w3)-Y1*t170+X1*(t97+t98+t99-t13*t14*w1-t10*t25*w2*w3))*2.0+t15*(t180+t182-X1*(t177+t178-t13*t14*w2*2.0))*2.0+t23*(t175+Y1*(t35-t94+t95+t96-t13*t14*w3)-Z1*t173)*2.0)*(1.0/2.0); - jacs(0, 2) = t14*t65*(X1*r3*w1+Y1*r3*w2+Z1*r1*w1+Z1*r2*w2+Z1*r3*w3*2.0+r1*t1*w3*2.0+r2*t2*w3*2.0+r3*t3*w3*2.0+X1*r2*t7*t12+X1*r2*t9*t10-Y1*r1*t7*t12-Y1*r1*t9*t10+X1*r1*t12*w3*2.0-X1*r3*t12*w1+Y1*r2*t12*w3*2.0-Y1*r3*t12*w2-Z1*r1*t12*w1-Z1*r2*t12*w2+X1*r2*t7*t10*t11-Y1*r1*t7*t10*t11-X1*r3*t12*w2*w3+Y1*r3*t12*w1*w3+Z1*r1*t12*w2*w3-Z1*r2*t12*w1*w3+Y1*r3*t10*t11*w1*w3+Z1*r1*t10*t11*w2*w3-Z1*r2*t10*t11*w1*w3-X1*r1*t6*t10*t11*w3-X1*r1*t7*t10*t11*w3+X1*r3*t7*t10*t11*w1-Y1*r2*t5*t10*t11*w3-Y1*r2*t7*t10*t11*w3+Y1*r3*t7*t10*t11*w2+Z1*r1*t7*t10*t11*w1+Z1*r2*t7*t10*t11*w2-Z1*r3*t5*t10*t11*w3-Z1*r3*t6*t10*t11*w3-X1*r3*t10*t11*w2*w3+X1*r2*t10*t11*w1*w2*w3+Y1*r1*t10*t11*w1*w2*w3)-t26*t65*t93*w3*2.0-t14*t93*t101*(t18*(Z1*(t46-t113+t114+t115-t13*t14*w2)-Y1*t198+X1*(t49+t51+t52+t118-t7*t10*t25))*2.0+t23*(X1*(-t97+t112+t116+t117-t13*t14*w1)+Y1*(-t46+t113+t114+t115-t13*t14*w2)-Z1*t195)*2.0+t15*(t204+Z1*(t97-t112+t116+t117-t13*t14*w1)-X1*(t201+t202-t13*t14*w3*2.0))*2.0)*(1.0/2.0); - jacs(0, 3) = r1*t65-t14*t93*t101*t208*(1.0/2.0); - jacs(0, 4) = r2*t65-t14*t93*t101*t212*(1.0/2.0); - jacs(0, 5) = r3*t65-t14*t93*t101*t216*(1.0/2.0); - jacs(1, 0) = t14*t65*(X1*s1*w1*2.0+X1*s2*w2+X1*s3*w3+Y1*s1*w2+Z1*s1*w3+s1*t1*w1*2.0+s2*t2*w1*2.0+s3*t3*w1*2.0+Y1*s3*t5*t12+Y1*s3*t9*t10-Z1*s2*t5*t12-Z1*s2*t9*t10-X1*s2*t12*w2-X1*s3*t12*w3-Y1*s1*t12*w2+Y1*s2*t12*w1*2.0-Z1*s1*t12*w3+Z1*s3*t12*w1*2.0+Y1*s3*t5*t10*t11-Z1*s2*t5*t10*t11+X1*s2*t12*w1*w3-X1*s3*t12*w1*w2-Y1*s1*t12*w1*w3+Z1*s1*t12*w1*w2+X1*s2*t10*t11*w1*w3-X1*s3*t10*t11*w1*w2-Y1*s1*t10*t11*w1*w3+Z1*s1*t10*t11*w1*w2-X1*s1*t6*t10*t11*w1-X1*s1*t7*t10*t11*w1+X1*s2*t5*t10*t11*w2+X1*s3*t5*t10*t11*w3+Y1*s1*t5*t10*t11*w2-Y1*s2*t5*t10*t11*w1-Y1*s2*t7*t10*t11*w1+Z1*s1*t5*t10*t11*w3-Z1*s3*t5*t10*t11*w1-Z1*s3*t6*t10*t11*w1+Y1*s3*t10*t11*w1*w2*w3+Z1*s2*t10*t11*w1*w2*w3)-t14*t101*t167*(t130+t15*(Y1*(t46+t47+t48-t113-t138)+Z1*(t35+t36+t37-t94-t139)-X1*t121)*2.0+t18*(t135+t137-Y1*(-t131+t132+t133))*2.0)*(1.0/2.0)-t26*t65*t167*w1*2.0; - jacs(1, 1) = t14*t65*(X1*s2*w1+Y1*s1*w1+Y1*s2*w2*2.0+Y1*s3*w3+Z1*s2*w3+s1*t1*w2*2.0+s2*t2*w2*2.0+s3*t3*w2*2.0-X1*s3*t6*t12-X1*s3*t9*t10+Z1*s1*t6*t12+Z1*s1*t9*t10+X1*s1*t12*w2*2.0-X1*s2*t12*w1-Y1*s1*t12*w1-Y1*s3*t12*w3-Z1*s2*t12*w3+Z1*s3*t12*w2*2.0-X1*s3*t6*t10*t11+Z1*s1*t6*t10*t11+X1*s2*t12*w2*w3-Y1*s1*t12*w2*w3+Y1*s3*t12*w1*w2-Z1*s2*t12*w1*w2+X1*s2*t10*t11*w2*w3-Y1*s1*t10*t11*w2*w3+Y1*s3*t10*t11*w1*w2-Z1*s2*t10*t11*w1*w2-X1*s1*t6*t10*t11*w2+X1*s2*t6*t10*t11*w1-X1*s1*t7*t10*t11*w2+Y1*s1*t6*t10*t11*w1-Y1*s2*t5*t10*t11*w2-Y1*s2*t7*t10*t11*w2+Y1*s3*t6*t10*t11*w3-Z1*s3*t5*t10*t11*w2+Z1*s2*t6*t10*t11*w3-Z1*s3*t6*t10*t11*w2+X1*s3*t10*t11*w1*w2*w3+Z1*s1*t10*t11*w1*w2*w3)-t26*t65*t167*w2*2.0-t14*t101*t167*(t18*(X1*(t97+t98+t99-t112-t192)+Z1*(-t35+t94+t95+t96-t139)-Y1*t170)*2.0+t15*(t180+t182-X1*(-t176+t177+t178))*2.0+t23*(t175+Y1*(t35-t94+t95+t96-t139)-Z1*t173)*2.0)*(1.0/2.0); - jacs(1, 2) = t14*t65*(X1*s3*w1+Y1*s3*w2+Z1*s1*w1+Z1*s2*w2+Z1*s3*w3*2.0+s1*t1*w3*2.0+s2*t2*w3*2.0+s3*t3*w3*2.0+X1*s2*t7*t12+X1*s2*t9*t10-Y1*s1*t7*t12-Y1*s1*t9*t10+X1*s1*t12*w3*2.0-X1*s3*t12*w1+Y1*s2*t12*w3*2.0-Y1*s3*t12*w2-Z1*s1*t12*w1-Z1*s2*t12*w2+X1*s2*t7*t10*t11-Y1*s1*t7*t10*t11-X1*s3*t12*w2*w3+Y1*s3*t12*w1*w3+Z1*s1*t12*w2*w3-Z1*s2*t12*w1*w3-X1*s3*t10*t11*w2*w3+Y1*s3*t10*t11*w1*w3+Z1*s1*t10*t11*w2*w3-Z1*s2*t10*t11*w1*w3-X1*s1*t6*t10*t11*w3-X1*s1*t7*t10*t11*w3+X1*s3*t7*t10*t11*w1-Y1*s2*t5*t10*t11*w3-Y1*s2*t7*t10*t11*w3+Y1*s3*t7*t10*t11*w2+Z1*s1*t7*t10*t11*w1+Z1*s2*t7*t10*t11*w2-Z1*s3*t5*t10*t11*w3-Z1*s3*t6*t10*t11*w3+X1*s2*t10*t11*w1*w2*w3+Y1*s1*t10*t11*w1*w2*w3)-t26*t65*t167*w3*2.0-t14*t101*t167*(t18*(Z1*(t46-t113+t114+t115-t138)-Y1*t198+X1*(t49+t51+t52+t118-t199))*2.0+t23*(X1*(-t97+t112+t116+t117-t192)+Y1*(-t46+t113+t114+t115-t138)-Z1*t195)*2.0+t15*(t204+Z1*(t97-t112+t116+t117-t192)-X1*(-t200+t201+t202))*2.0)*(1.0/2.0); - jacs(1, 3) = s1*t65-t14*t101*t167*t208*(1.0/2.0); - jacs(1, 4) = s2*t65-t14*t101*t167*t212*(1.0/2.0); - jacs(1, 5) = s3*t65-t14*t101*t167*t216*(1.0/2.0); + const Eigen::Vector3d& nullspace_s, const rodrigues_t& w, + const translation_t& t, Eigen::MatrixXd& jacs) + { + Eigen::Vector3d Xw = pt.first; + Eigen::Vector2d x = pt.second; + + // Compute the Jacobian matrix + Eigen::Matrix J; + cv::projectPoints(cv::Mat(Xw), rvec_, tvec_, K_, cv::noArray(), xproj_); + cv::Mat jacobian; + cv::projectPoints(cv::Mat(Xw), rvec_, tvec_, K_, cv::noArray(), jacobian, cv::Mat(), true); + J << jacobian.at(0, 0), jacobian.at(0, 1), jacobian.at(0, 2), + jacobian.at(0, 3), jacobian.at(0, 4), jacobian.at(0, 5), + jacobian.at(1, 0), jacobian.at(1, 1), jacobian.at(1, 2), + jacobian.at(1, 3), jacobian.at(1, 4), jacobian.at(1, 5); + + // Apply the nullspaces of the MLP layers to the Jacobian matrix + Eigen::Matrix nullspaces; + nullspaces << nullspace_r(0), nullspace_r(1), nullspace_r(2), + nullspace_s(0), nullspace_s(1), nullspace_s(2), + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0; + J = J * nullspaces; + + // Apply the rotation Jacobian matrix + Eigen::Matrix Jr; + Jr << 0.0, -w(2), w(1), + w(2), 0.0, -w(0), + -w(1), w(0), 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0; + J = J * Jr; + + // Apply the translation Jacobian matrix + Eigen::Matrix Jt; + Jt << Eigen::Matrix::Identity(), Eigen::Matrix::Zero(), + Eigen::Matrix::Zero(), Eigen::Matrix::Identity(); + J = J * Jt; + + jacs = J; } + }//End namespace ORB_SLAM2 + +// old mlpnp below +/*void MLPnPsolver::mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r, + const Eigen::Vector3d& nullspace_s, const rodrigues_t& w, + const translation_t& t, Eigen::MatrixXd& jacs){ + double r1 = nullspace_r[0]; + double r2 = nullspace_r[1]; + double r3 = nullspace_r[2]; + + double s1 = nullspace_s[0]; + double s2 = nullspace_s[1]; + double s3 = nullspace_s[2]; + + double X1 = pt[0]; + double Y1 = pt[1]; + double Z1 = pt[2]; + + double w1 = w[0]; + double w2 = w[1]; + double w3 = w[2]; + + double t1 = t[0]; + double t2 = t[1]; + double t3 = t[2]; + + double t5 = w1*w1; + double t6 = w2*w2; + double t7 = w3*w3; + double t8 = t5+t6+t7; + double t9 = sqrt(t8); + double t10 = sin(t9); + double t11 = 1.0/sqrt(t8); + double t12 = cos(t9); + double t13 = t12-1.0; + double t14 = 1.0/t8; + double t16 = t10*t11*w3; + double t17 = t13*t14*w1*w2; + double t19 = t10*t11*w2; + double t20 = t13*t14*w1*w3; + double t24 = t6+t7; + double t27 = t16+t17; + double t28 = Y1*t27; + double t29 = t19-t20; + double t30 = Z1*t29; + double t31 = t13*t14*t24; + double t32 = t31+1.0; + double t33 = X1*t32; + double t15 = t1-t28+t30+t33; + double t21 = t10*t11*w1; + double t22 = t13*t14*w2*w3; + double t45 = t5+t7; + double t53 = t16-t17; + double t54 = X1*t53; + double t55 = t21+t22; + double t56 = Z1*t55; + double t57 = t13*t14*t45; + double t58 = t57+1.0; + double t59 = Y1*t58; + double t18 = t2+t54-t56+t59; + double t34 = t5+t6; + double t38 = t19+t20; + double t39 = X1*t38; + double t40 = t21-t22; + double t41 = Y1*t40; + double t42 = t13*t14*t34; + double t43 = t42+1.0; + double t44 = Z1*t43; + double t23 = t3-t39+t41+t44; + double t25 = 1.0/pow(t8,3.0/2.0); + double t26 = 1.0/(t8*t8); + double t35 = t12*t14*w1*w2; + double t36 = t5*t10*t25*w3; + double t37 = t5*t13*t26*w3*2.0; + double t46 = t10*t25*w1*w3; + double t47 = t5*t10*t25*w2; + double t48 = t5*t13*t26*w2*2.0; + double t49 = t10*t11; + double t50 = t5*t12*t14; + double t51 = t13*t26*w1*w2*w3*2.0; + double t52 = t10*t25*w1*w2*w3; + double t60 = t15*t15; + double t61 = t18*t18; + double t62 = t23*t23; + double t63 = t60+t61+t62; + double t64 = t5*t10*t25; + double t65 = 1.0/sqrt(t63); + double t66 = Y1*r2*t6; + double t67 = Z1*r3*t7; + double t68 = r1*t1*t5; + double t69 = r1*t1*t6; + double t70 = r1*t1*t7; + double t71 = r2*t2*t5; + double t72 = r2*t2*t6; + double t73 = r2*t2*t7; + double t74 = r3*t3*t5; + double t75 = r3*t3*t6; + double t76 = r3*t3*t7; + double t77 = X1*r1*t5; + double t78 = X1*r2*w1*w2; + double t79 = X1*r3*w1*w3; + double t80 = Y1*r1*w1*w2; + double t81 = Y1*r3*w2*w3; + double t82 = Z1*r1*w1*w3; + double t83 = Z1*r2*w2*w3; + double t84 = X1*r1*t6*t12; + double t85 = X1*r1*t7*t12; + double t86 = Y1*r2*t5*t12; + double t87 = Y1*r2*t7*t12; + double t88 = Z1*r3*t5*t12; + double t89 = Z1*r3*t6*t12; + double t90 = X1*r2*t9*t10*w3; + double t91 = Y1*r3*t9*t10*w1; + double t92 = Z1*r1*t9*t10*w2; + double t102 = X1*r3*t9*t10*w2; + double t103 = Y1*r1*t9*t10*w3; + double t104 = Z1*r2*t9*t10*w1; + double t105 = X1*r2*t12*w1*w2; + double t106 = X1*r3*t12*w1*w3; + double t107 = Y1*r1*t12*w1*w2; + double t108 = Y1*r3*t12*w2*w3; + double t109 = Z1*r1*t12*w1*w3; + double t110 = Z1*r2*t12*w2*w3; + double t93 = t66+t67+t68+t69+t70+t71+t72+t73+t74+t75+t76+t77+t78+t79+t80+t81+t82+t83+t84+t85+t86+t87+t88+t89+t90+t91+t92-t102-t103-t104-t105-t106-t107-t108-t109-t110; + double t94 = t10*t25*w1*w2; + double t95 = t6*t10*t25*w3; + double t96 = t6*t13*t26*w3*2.0; + double t97 = t12*t14*w2*w3; + double t98 = t6*t10*t25*w1; + double t99 = t6*t13*t26*w1*2.0; + double t100 = t6*t10*t25; + double t101 = 1.0/pow(t63,3.0/2.0); + double t111 = t6*t12*t14; + double t112 = t10*t25*w2*w3; + double t113 = t12*t14*w1*w3; + double t114 = t7*t10*t25*w2; + double t115 = t7*t13*t26*w2*2.0; + double t116 = t7*t10*t25*w1; + double t117 = t7*t13*t26*w1*2.0; + double t118 = t7*t12*t14; + double t119 = t13*t24*t26*w1*2.0; + double t120 = t10*t24*t25*w1; + double t121 = t119+t120; + double t122 = t13*t26*t34*w1*2.0; + double t123 = t10*t25*t34*w1; + double t131 = t13*t14*w1*2.0; + double t124 = t122+t123-t131; + double t139 = t13*t14*w3; + double t125 = -t35+t36+t37+t94-t139; + double t126 = X1*t125; + double t127 = t49+t50+t51+t52-t64; + double t128 = Y1*t127; + double t129 = t126+t128-Z1*t124; + double t130 = t23*t129*2.0; + double t132 = t13*t26*t45*w1*2.0; + double t133 = t10*t25*t45*w1; + double t138 = t13*t14*w2; + double t134 = -t46+t47+t48+t113-t138; + double t135 = X1*t134; + double t136 = -t49-t50+t51+t52+t64; + double t137 = Z1*t136; + double t140 = X1*s1*t5; + double t141 = Y1*s2*t6; + double t142 = Z1*s3*t7; + double t143 = s1*t1*t5; + double t144 = s1*t1*t6; + double t145 = s1*t1*t7; + double t146 = s2*t2*t5; + double t147 = s2*t2*t6; + double t148 = s2*t2*t7; + double t149 = s3*t3*t5; + double t150 = s3*t3*t6; + double t151 = s3*t3*t7; + double t152 = X1*s2*w1*w2; + double t153 = X1*s3*w1*w3; + double t154 = Y1*s1*w1*w2; + double t155 = Y1*s3*w2*w3; + double t156 = Z1*s1*w1*w3; + double t157 = Z1*s2*w2*w3; + double t158 = X1*s1*t6*t12; + double t159 = X1*s1*t7*t12; + double t160 = Y1*s2*t5*t12; + double t161 = Y1*s2*t7*t12; + double t162 = Z1*s3*t5*t12; + double t163 = Z1*s3*t6*t12; + double t164 = X1*s2*t9*t10*w3; + double t165 = Y1*s3*t9*t10*w1; + double t166 = Z1*s1*t9*t10*w2; + double t183 = X1*s3*t9*t10*w2; + double t184 = Y1*s1*t9*t10*w3; + double t185 = Z1*s2*t9*t10*w1; + double t186 = X1*s2*t12*w1*w2; + double t187 = X1*s3*t12*w1*w3; + double t188 = Y1*s1*t12*w1*w2; + double t189 = Y1*s3*t12*w2*w3; + double t190 = Z1*s1*t12*w1*w3; + double t191 = Z1*s2*t12*w2*w3; + double t167 = t140+t141+t142+t143+t144+t145+t146+t147+t148+t149+t150+t151+t152+t153+t154+t155+t156+t157+t158+t159+t160+t161+t162+t163+t164+t165+t166-t183-t184-t185-t186-t187-t188-t189-t190-t191; + double t168 = t13*t26*t45*w2*2.0; + double t169 = t10*t25*t45*w2; + double t170 = t168+t169; + double t171 = t13*t26*t34*w2*2.0; + double t172 = t10*t25*t34*w2; + double t176 = t13*t14*w2*2.0; + double t173 = t171+t172-t176; + double t174 = -t49+t51+t52+t100-t111; + double t175 = X1*t174; + double t177 = t13*t24*t26*w2*2.0; + double t178 = t10*t24*t25*w2; + double t192 = t13*t14*w1; + double t179 = -t97+t98+t99+t112-t192; + double t180 = Y1*t179; + double t181 = t49+t51+t52-t100+t111; + double t182 = Z1*t181; + double t193 = t13*t26*t34*w3*2.0; + double t194 = t10*t25*t34*w3; + double t195 = t193+t194; + double t196 = t13*t26*t45*w3*2.0; + double t197 = t10*t25*t45*w3; + double t200 = t13*t14*w3*2.0; + double t198 = t196+t197-t200; + double t199 = t7*t10*t25; + double t201 = t13*t24*t26*w3*2.0; + double t202 = t10*t24*t25*w3; + double t203 = -t49+t51+t52-t118+t199; + double t204 = Y1*t203; + double t205 = t1*2.0; + double t206 = Z1*t29*2.0; + double t207 = X1*t32*2.0; + double t208 = t205+t206+t207-Y1*t27*2.0; + double t209 = t2*2.0; + double t210 = X1*t53*2.0; + double t211 = Y1*t58*2.0; + double t212 = t209+t210+t211-Z1*t55*2.0; + double t213 = t3*2.0; + double t214 = Y1*t40*2.0; + double t215 = Z1*t43*2.0; + double t216 = t213+t214+t215-X1*t38*2.0; + jacs(0, 0) = t14*t65*(X1*r1*w1*2.0+X1*r2*w2+X1*r3*w3+Y1*r1*w2+Z1*r1*w3+r1*t1*w1*2.0+r2*t2*w1*2.0+r3*t3*w1*2.0+Y1*r3*t5*t12+Y1*r3*t9*t10-Z1*r2*t5*t12-Z1*r2*t9*t10-X1*r2*t12*w2-X1*r3*t12*w3-Y1*r1*t12*w2+Y1*r2*t12*w1*2.0-Z1*r1*t12*w3+Z1*r3*t12*w1*2.0+Y1*r3*t5*t10*t11-Z1*r2*t5*t10*t11+X1*r2*t12*w1*w3-X1*r3*t12*w1*w2-Y1*r1*t12*w1*w3+Z1*r1*t12*w1*w2-Y1*r1*t10*t11*w1*w3+Z1*r1*t10*t11*w1*w2-X1*r1*t6*t10*t11*w1-X1*r1*t7*t10*t11*w1+X1*r2*t5*t10*t11*w2+X1*r3*t5*t10*t11*w3+Y1*r1*t5*t10*t11*w2-Y1*r2*t5*t10*t11*w1-Y1*r2*t7*t10*t11*w1+Z1*r1*t5*t10*t11*w3-Z1*r3*t5*t10*t11*w1-Z1*r3*t6*t10*t11*w1+X1*r2*t10*t11*w1*w3-X1*r3*t10*t11*w1*w2+Y1*r3*t10*t11*w1*w2*w3+Z1*r2*t10*t11*w1*w2*w3)-t26*t65*t93*w1*2.0-t14*t93*t101*(t130+t15*(-X1*t121+Y1*(t46+t47+t48-t13*t14*w2-t12*t14*w1*w3)+Z1*(t35+t36+t37-t13*t14*w3-t10*t25*w1*w2))*2.0+t18*(t135+t137-Y1*(t132+t133-t13*t14*w1*2.0))*2.0)*(1.0/2.0); + jacs(0, 1) = t14*t65*(X1*r2*w1+Y1*r1*w1+Y1*r2*w2*2.0+Y1*r3*w3+Z1*r2*w3+r1*t1*w2*2.0+r2*t2*w2*2.0+r3*t3*w2*2.0-X1*r3*t6*t12-X1*r3*t9*t10+Z1*r1*t6*t12+Z1*r1*t9*t10+X1*r1*t12*w2*2.0-X1*r2*t12*w1-Y1*r1*t12*w1-Y1*r3*t12*w3-Z1*r2*t12*w3+Z1*r3*t12*w2*2.0-X1*r3*t6*t10*t11+Z1*r1*t6*t10*t11+X1*r2*t12*w2*w3-Y1*r1*t12*w2*w3+Y1*r3*t12*w1*w2-Z1*r2*t12*w1*w2-Y1*r1*t10*t11*w2*w3+Y1*r3*t10*t11*w1*w2-Z1*r2*t10*t11*w1*w2-X1*r1*t6*t10*t11*w2+X1*r2*t6*t10*t11*w1-X1*r1*t7*t10*t11*w2+Y1*r1*t6*t10*t11*w1-Y1*r2*t5*t10*t11*w2-Y1*r2*t7*t10*t11*w2+Y1*r3*t6*t10*t11*w3-Z1*r3*t5*t10*t11*w2+Z1*r2*t6*t10*t11*w3-Z1*r3*t6*t10*t11*w2+X1*r2*t10*t11*w2*w3+X1*r3*t10*t11*w1*w2*w3+Z1*r1*t10*t11*w1*w2*w3)-t26*t65*t93*w2*2.0-t14*t93*t101*(t18*(Z1*(-t35+t94+t95+t96-t13*t14*w3)-Y1*t170+X1*(t97+t98+t99-t13*t14*w1-t10*t25*w2*w3))*2.0+t15*(t180+t182-X1*(t177+t178-t13*t14*w2*2.0))*2.0+t23*(t175+Y1*(t35-t94+t95+t96-t13*t14*w3)-Z1*t173)*2.0)*(1.0/2.0); + jacs(0, 2) = t14*t65*(X1*r3*w1+Y1*r3*w2+Z1*r1*w1+Z1*r2*w2+Z1*r3*w3*2.0+r1*t1*w3*2.0+r2*t2*w3*2.0+r3*t3*w3*2.0+X1*r2*t7*t12+X1*r2*t9*t10-Y1*r1*t7*t12-Y1*r1*t9*t10+X1*r1*t12*w3*2.0-X1*r3*t12*w1+Y1*r2*t12*w3*2.0-Y1*r3*t12*w2-Z1*r1*t12*w1-Z1*r2*t12*w2+X1*r2*t7*t10*t11-Y1*r1*t7*t10*t11-X1*r3*t12*w2*w3+Y1*r3*t12*w1*w3+Z1*r1*t12*w2*w3-Z1*r2*t12*w1*w3+Y1*r3*t10*t11*w1*w3+Z1*r1*t10*t11*w2*w3-Z1*r2*t10*t11*w1*w3-X1*r1*t6*t10*t11*w3-X1*r1*t7*t10*t11*w3+X1*r3*t7*t10*t11*w1-Y1*r2*t5*t10*t11*w3-Y1*r2*t7*t10*t11*w3+Y1*r3*t7*t10*t11*w2+Z1*r1*t7*t10*t11*w1+Z1*r2*t7*t10*t11*w2-Z1*r3*t5*t10*t11*w3-Z1*r3*t6*t10*t11*w3-X1*r3*t10*t11*w2*w3+X1*r2*t10*t11*w1*w2*w3+Y1*r1*t10*t11*w1*w2*w3)-t26*t65*t93*w3*2.0-t14*t93*t101*(t18*(Z1*(t46-t113+t114+t115-t13*t14*w2)-Y1*t198+X1*(t49+t51+t52+t118-t7*t10*t25))*2.0+t23*(X1*(-t97+t112+t116+t117-t13*t14*w1)+Y1*(-t46+t113+t114+t115-t13*t14*w2)-Z1*t195)*2.0+t15*(t204+Z1*(t97-t112+t116+t117-t13*t14*w1)-X1*(t201+t202-t13*t14*w3*2.0))*2.0)*(1.0/2.0); + jacs(0, 3) = r1*t65-t14*t93*t101*t208*(1.0/2.0); + jacs(0, 4) = r2*t65-t14*t93*t101*t212*(1.0/2.0); + jacs(0, 5) = r3*t65-t14*t93*t101*t216*(1.0/2.0); + jacs(1, 0) = t14*t65*(X1*s1*w1*2.0+X1*s2*w2+X1*s3*w3+Y1*s1*w2+Z1*s1*w3+s1*t1*w1*2.0+s2*t2*w1*2.0+s3*t3*w1*2.0+Y1*s3*t5*t12+Y1*s3*t9*t10-Z1*s2*t5*t12-Z1*s2*t9*t10-X1*s2*t12*w2-X1*s3*t12*w3-Y1*s1*t12*w2+Y1*s2*t12*w1*2.0-Z1*s1*t12*w3+Z1*s3*t12*w1*2.0+Y1*s3*t5*t10*t11-Z1*s2*t5*t10*t11+X1*s2*t12*w1*w3-X1*s3*t12*w1*w2-Y1*s1*t12*w1*w3+Z1*s1*t12*w1*w2+X1*s2*t10*t11*w1*w3-X1*s3*t10*t11*w1*w2-Y1*s1*t10*t11*w1*w3+Z1*s1*t10*t11*w1*w2-X1*s1*t6*t10*t11*w1-X1*s1*t7*t10*t11*w1+X1*s2*t5*t10*t11*w2+X1*s3*t5*t10*t11*w3+Y1*s1*t5*t10*t11*w2-Y1*s2*t5*t10*t11*w1-Y1*s2*t7*t10*t11*w1+Z1*s1*t5*t10*t11*w3-Z1*s3*t5*t10*t11*w1-Z1*s3*t6*t10*t11*w1+Y1*s3*t10*t11*w1*w2*w3+Z1*s2*t10*t11*w1*w2*w3)-t14*t101*t167*(t130+t15*(Y1*(t46+t47+t48-t113-t138)+Z1*(t35+t36+t37-t94-t139)-X1*t121)*2.0+t18*(t135+t137-Y1*(-t131+t132+t133))*2.0)*(1.0/2.0)-t26*t65*t167*w1*2.0; + jacs(1, 1) = t14*t65*(X1*s2*w1+Y1*s1*w1+Y1*s2*w2*2.0+Y1*s3*w3+Z1*s2*w3+s1*t1*w2*2.0+s2*t2*w2*2.0+s3*t3*w2*2.0-X1*s3*t6*t12-X1*s3*t9*t10+Z1*s1*t6*t12+Z1*s1*t9*t10+X1*s1*t12*w2*2.0-X1*s2*t12*w1-Y1*s1*t12*w1-Y1*s3*t12*w3-Z1*s2*t12*w3+Z1*s3*t12*w2*2.0-X1*s3*t6*t10*t11+Z1*s1*t6*t10*t11+X1*s2*t12*w2*w3-Y1*s1*t12*w2*w3+Y1*s3*t12*w1*w2-Z1*s2*t12*w1*w2+X1*s2*t10*t11*w2*w3-Y1*s1*t10*t11*w2*w3+Y1*s3*t10*t11*w1*w2-Z1*s2*t10*t11*w1*w2-X1*s1*t6*t10*t11*w2+X1*s2*t6*t10*t11*w1-X1*s1*t7*t10*t11*w2+Y1*s1*t6*t10*t11*w1-Y1*s2*t5*t10*t11*w2-Y1*s2*t7*t10*t11*w2+Y1*s3*t6*t10*t11*w3-Z1*s3*t5*t10*t11*w2+Z1*s2*t6*t10*t11*w3-Z1*s3*t6*t10*t11*w2+X1*s3*t10*t11*w1*w2*w3+Z1*s1*t10*t11*w1*w2*w3)-t26*t65*t167*w2*2.0-t14*t101*t167*(t18*(X1*(t97+t98+t99-t112-t192)+Z1*(-t35+t94+t95+t96-t139)-Y1*t170)*2.0+t15*(t180+t182-X1*(-t176+t177+t178))*2.0+t23*(t175+Y1*(t35-t94+t95+t96-t139)-Z1*t173)*2.0)*(1.0/2.0); + jacs(1, 2) = t14*t65*(X1*s3*w1+Y1*s3*w2+Z1*s1*w1+Z1*s2*w2+Z1*s3*w3*2.0+s1*t1*w3*2.0+s2*t2*w3*2.0+s3*t3*w3*2.0+X1*s2*t7*t12+X1*s2*t9*t10-Y1*s1*t7*t12-Y1*s1*t9*t10+X1*s1*t12*w3*2.0-X1*s3*t12*w1+Y1*s2*t12*w3*2.0-Y1*s3*t12*w2-Z1*s1*t12*w1-Z1*s2*t12*w2+X1*s2*t7*t10*t11-Y1*s1*t7*t10*t11-X1*s3*t12*w2*w3+Y1*s3*t12*w1*w3+Z1*s1*t12*w2*w3-Z1*s2*t12*w1*w3-X1*s3*t10*t11*w2*w3+Y1*s3*t10*t11*w1*w3+Z1*s1*t10*t11*w2*w3-Z1*s2*t10*t11*w1*w3-X1*s1*t6*t10*t11*w3-X1*s1*t7*t10*t11*w3+X1*s3*t7*t10*t11*w1-Y1*s2*t5*t10*t11*w3-Y1*s2*t7*t10*t11*w3+Y1*s3*t7*t10*t11*w2+Z1*s1*t7*t10*t11*w1+Z1*s2*t7*t10*t11*w2-Z1*s3*t5*t10*t11*w3-Z1*s3*t6*t10*t11*w3+X1*s2*t10*t11*w1*w2*w3+Y1*s1*t10*t11*w1*w2*w3)-t26*t65*t167*w3*2.0-t14*t101*t167*(t18*(Z1*(t46-t113+t114+t115-t138)-Y1*t198+X1*(t49+t51+t52+t118-t199))*2.0+t23*(X1*(-t97+t112+t116+t117-t192)+Y1*(-t46+t113+t114+t115-t138)-Z1*t195)*2.0+t15*(t204+Z1*(t97-t112+t116+t117-t192)-X1*(-t200+t201+t202))*2.0)*(1.0/2.0); + jacs(1, 3) = s1*t65-t14*t101*t167*t208*(1.0/2.0); + jacs(1, 4) = s2*t65-t14*t101*t167*t212*(1.0/2.0); + jacs(1, 5) = s3*t65-t14*t101*t167*t216*(1.0/2.0); +} +}//End namespace ORB_SLAM2*/ diff --git a/src/test.cpp b/src/test.cpp new file mode 100644 index 0000000000..b37cb8cb42 --- /dev/null +++ b/src/test.cpp @@ -0,0 +1,23 @@ +#include "MLPnpsolver.h" +#include "gtest/gtest.h" + +TEST(MLPnpsolverTest, TestSolvePNP) { + // Generate random test data + std::vector pts3d; + std::vector pts2d; + cv::Mat K = cv::Mat::eye(3, 3, CV_64F); + int num_points = 10; + for (int i = 0; i < num_points; i++) { + pts3d.push_back(cv::Point3f(rand() % 100, rand() % 100, rand() % 100)); + pts2d.push_back(cv::Point2f(rand() % 100, rand() % 100)); + } + + // Call the function you want to test + cv::Mat rvec, tvec; + bool success = SolvePNP(pts3d, pts2d, K, rvec, tvec); + + // Check the result + ASSERT_TRUE(success); + ASSERT_FALSE(rvec.empty()); + ASSERT_FALSE(tvec.empty()); +} From 2056b28aba7e36190c9a96a77f92bba8be4da984 Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Sun, 23 Apr 2023 12:07:00 +0300 Subject: [PATCH 2/8] updated gitignore --- .gitignore | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.gitignore b/.gitignore index 7227515c0a..d585b9452e 100644 --- a/.gitignore +++ b/.gitignore @@ -91,3 +91,7 @@ my_settings.txt borrar/* */ExecMean.txt + +#Neta's added ignores +Dependencies.md +cmake-build-release/ From bb6334e78b5917ad54046d5496ff3fb1cc357844 Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Sun, 30 Apr 2023 18:47:26 +0300 Subject: [PATCH 3/8] working optimizations for MLPNP, with const, Eigen --- src/MLPnPsolver.cpp | 378 ++++++++++---------------------------------- 1 file changed, 83 insertions(+), 295 deletions(-) diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 283a36e178..9d379d0057 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -805,301 +805,89 @@ namespace ORB_SLAM3 { } } + + void MLPnPsolver::mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r, - const Eigen::Vector3d& nullspace_s, const rodrigues_t& w, - const translation_t& t, Eigen::MatrixXd& jacs) - { - Eigen::Vector3d Xw = pt.first; - Eigen::Vector2d x = pt.second; - - // Compute the Jacobian matrix - Eigen::Matrix J; - cv::projectPoints(cv::Mat(Xw), rvec_, tvec_, K_, cv::noArray(), xproj_); - cv::Mat jacobian; - cv::projectPoints(cv::Mat(Xw), rvec_, tvec_, K_, cv::noArray(), jacobian, cv::Mat(), true); - J << jacobian.at(0, 0), jacobian.at(0, 1), jacobian.at(0, 2), - jacobian.at(0, 3), jacobian.at(0, 4), jacobian.at(0, 5), - jacobian.at(1, 0), jacobian.at(1, 1), jacobian.at(1, 2), - jacobian.at(1, 3), jacobian.at(1, 4), jacobian.at(1, 5); - - // Apply the nullspaces of the MLP layers to the Jacobian matrix - Eigen::Matrix nullspaces; - nullspaces << nullspace_r(0), nullspace_r(1), nullspace_r(2), - nullspace_s(0), nullspace_s(1), nullspace_s(2), - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; - J = J * nullspaces; - - // Apply the rotation Jacobian matrix - Eigen::Matrix Jr; - Jr << 0.0, -w(2), w(1), - w(2), 0.0, -w(0), - -w(1), w(0), 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0; - J = J * Jr; - - // Apply the translation Jacobian matrix - Eigen::Matrix Jt; - Jt << Eigen::Matrix::Identity(), Eigen::Matrix::Zero(), - Eigen::Matrix::Zero(), Eigen::Matrix::Identity(); - J = J * Jt; - - jacs = J; + const Eigen::Vector3d& nullspace_s, const rodrigues_t& w, + const translation_t& t, Eigen::MatrixXd& jacs) { + const double r1 = nullspace_r[0], r2 = nullspace_r[1], r3 = nullspace_r[2]; + const double s1 = nullspace_s[0], s2 = nullspace_s[1], s3 = nullspace_s[2]; + const double X1 = pt[0], Y1 = pt[1], Z1 = pt[2]; + const double w1 = w[0], w2 = w[1], w3 = w[2]; + const double t1 = t[0], t2 = t[1], t3 = t[2]; + + const double w1_sq = w1 * w1, w2_sq = w2 * w2, w3_sq = w3 * w3; + + const double w_norm_sq = w1_sq + w2_sq + w3_sq; + const double w_norm = sqrt(w_norm_sq); + const double sin_w_norm = sin(w_norm); + const double cos_w_norm = cos(w_norm); + const double one_minus_cos_w_norm = cos_w_norm - 1.0; + const double inv_w_norm_sq = 1.0 / w_norm_sq; + const double inv_w_norm = 1.0 / w_norm; + const double inv_w_norm_cubed = 1.0 / (w_norm_sq * w_norm); + + const double w2_w3 = w2 * w3, w1_w3 = w1 * w3, w1_w2 =w1 * w2; + const Eigen::Vector3d w_cross = (Eigen::Vector3d()<< w2_w3, w1_w3, w1_w2).finished(); + const double t13_t14_w_cross[2] = { one_minus_cos_w_norm * inv_w_norm_sq * w_cross[0], one_minus_cos_w_norm * inv_w_norm_sq * w_cross[2] }; + + + const Eigen::Matrix3d R = (Eigen::Matrix3d() << + cos_w_norm + w1_sq * one_minus_cos_w_norm, t13_t14_w_cross[1] + w1_w2 * one_minus_cos_w_norm, sin_w_norm * inv_w_norm * w_cross[1] + w1_w3 * one_minus_cos_w_norm, + t13_t14_w_cross[1] + w1_w2 * one_minus_cos_w_norm, cos_w_norm + w2_sq * one_minus_cos_w_norm, t13_t14_w_cross[0] + w2_w3 * one_minus_cos_w_norm, + sin_w_norm * inv_w_norm * w_cross[1] + w1_w3 * one_minus_cos_w_norm, one_minus_cos_w_norm * inv_w_norm_sq * w_cross[0] + w2_w3 * one_minus_cos_w_norm, cos_w_norm + w3_sq * one_minus_cos_w_norm).finished(); + + const double t15 = t1 - Y1 * R(0,1) + Z1 * R(0,2) + X1 * R(0,0); + const double t16 = t2 - Y1 * R(1,1) + Z1 * R(1,2) + X1 * R(1,0); + const double t17 = t3 - Y1 * R(2,1) + Z1 * R(2,2) + X1 * R(2,0); + + const double dtdw1 = -2.0 * sin_w_norm * inv_w_norm_cubed * (w1 * t15 + w2 * t16 + w3 * t17); + const double dtdw2 = (w_norm_sq * sin_w_norm - 2.0 * w_norm * one_minus_cos_w_norm) * inv_w_norm_cubed; + + const double w_norm_sin_w_norm =w_norm * sin_w_norm, w_norm_sin_w_norm_inv_w_norm = w_norm_sin_w_norm * inv_w_norm; + const double dtdw2_w3= dtdw2 * w3, dtdw1_w1 = dtdw1 * w1; + + + const Eigen::Matrix3d dR_dw1 = (Eigen::Matrix3d() << + -2.0 * w1 * dtdw2, dtdw1 * w3, -dtdw1 * w2, + dtdw1 * w3, -dtdw2 * w1, -w_norm_sin_w_norm_inv_w_norm, + -dtdw1 * w2, w_norm * sin_w_norm * inv_w_norm, -dtdw2 * w1).finished(); + + const Eigen::Matrix3d dR_dw2 = (Eigen::Matrix3d() << + -dtdw2 * w2, -w_norm_sin_w_norm_inv_w_norm, dtdw1 * w3, + -w_norm_sin_w_norm_inv_w_norm, -2.0 * w2 * dtdw2, -dtdw1_w1, + dtdw1 * w3, -dtdw1_w1, -dtdw2 * w2).finished(); + + const Eigen::Matrix3d dR_dw3 = (Eigen::Matrix3d() << + -dtdw2_w3, dtdw1 * w2, -w_norm_sin_w_norm_inv_w_norm, + dtdw1 * w2, -dtdw2_w3, dtdw1_w1, + -w_norm_sin_w_norm_inv_w_norm, dtdw1_w1, -2.0 * w3 * dtdw2).finished(); + + const double dtdX1 = r1 * R(0,0) + s1 * R(1,0) + R(2,0); + const double dtdY1 = r1 * R(0,1) + s1 * R(1,1) + R(2,1); + const double dtdZ1 = r1 * R(0,2) + s1 * R(1,2) + R(2,2); + + jacs(0, 0) = r1 * dR_dw1(0,0) + s1 * dR_dw1(1,0) + dR_dw1(2,0); + jacs(0, 1) = r1 * dR_dw2(0,0) + s1 * dR_dw2(1,0) + dR_dw2(2,0); + jacs(0, 2) = r1 * dR_dw3(0,0) + s1 * dR_dw3(1,0) + dR_dw3(2,0); + + jacs(1, 0) = r2 * dR_dw1(0,1) + s2 * dR_dw1(1,1) + dR_dw1(2,1); + jacs(1, 1) = r2 * dR_dw2(0,1) + s2 * dR_dw2(1,1) + dR_dw2(2,1); + jacs(1, 2) = r2 * dR_dw3(0,1) + s2 * dR_dw3(1,1) + dR_dw3(2,1); + + jacs(2, 0) = r3 * dR_dw1(0, 2) + s3 * dR_dw1(1, 2) + dR_dw1(2, 2); + jacs(2, 1) = r3 * dR_dw2(0, 2) + s3 * dR_dw2(1, 2) + dR_dw2(2, 2); + jacs(2, 2) = r3 * dR_dw3(0, 2) + s3 * dR_dw3(1, 2) + dR_dw3(2, 2); + + jacs(0, 3) = dtdX1; + jacs(0, 4) = dtdY1; + jacs(0, 5) = dtdZ1; + + jacs(1, 3) = dtdY1; + jacs(1, 4) = r2 * R(0,0) + s2 * R(1,0) + R(2,0); + jacs(1, 5) = r2 * R(0,2) + s2 * R(1,2) + R(2,2); } -}//End namespace ORB_SLAM2 - -// old mlpnp below -/*void MLPnPsolver::mlpnpJacs(const point_t& pt, const Eigen::Vector3d& nullspace_r, - const Eigen::Vector3d& nullspace_s, const rodrigues_t& w, - const translation_t& t, Eigen::MatrixXd& jacs){ - double r1 = nullspace_r[0]; - double r2 = nullspace_r[1]; - double r3 = nullspace_r[2]; - - double s1 = nullspace_s[0]; - double s2 = nullspace_s[1]; - double s3 = nullspace_s[2]; - - double X1 = pt[0]; - double Y1 = pt[1]; - double Z1 = pt[2]; - - double w1 = w[0]; - double w2 = w[1]; - double w3 = w[2]; - - double t1 = t[0]; - double t2 = t[1]; - double t3 = t[2]; - - double t5 = w1*w1; - double t6 = w2*w2; - double t7 = w3*w3; - double t8 = t5+t6+t7; - double t9 = sqrt(t8); - double t10 = sin(t9); - double t11 = 1.0/sqrt(t8); - double t12 = cos(t9); - double t13 = t12-1.0; - double t14 = 1.0/t8; - double t16 = t10*t11*w3; - double t17 = t13*t14*w1*w2; - double t19 = t10*t11*w2; - double t20 = t13*t14*w1*w3; - double t24 = t6+t7; - double t27 = t16+t17; - double t28 = Y1*t27; - double t29 = t19-t20; - double t30 = Z1*t29; - double t31 = t13*t14*t24; - double t32 = t31+1.0; - double t33 = X1*t32; - double t15 = t1-t28+t30+t33; - double t21 = t10*t11*w1; - double t22 = t13*t14*w2*w3; - double t45 = t5+t7; - double t53 = t16-t17; - double t54 = X1*t53; - double t55 = t21+t22; - double t56 = Z1*t55; - double t57 = t13*t14*t45; - double t58 = t57+1.0; - double t59 = Y1*t58; - double t18 = t2+t54-t56+t59; - double t34 = t5+t6; - double t38 = t19+t20; - double t39 = X1*t38; - double t40 = t21-t22; - double t41 = Y1*t40; - double t42 = t13*t14*t34; - double t43 = t42+1.0; - double t44 = Z1*t43; - double t23 = t3-t39+t41+t44; - double t25 = 1.0/pow(t8,3.0/2.0); - double t26 = 1.0/(t8*t8); - double t35 = t12*t14*w1*w2; - double t36 = t5*t10*t25*w3; - double t37 = t5*t13*t26*w3*2.0; - double t46 = t10*t25*w1*w3; - double t47 = t5*t10*t25*w2; - double t48 = t5*t13*t26*w2*2.0; - double t49 = t10*t11; - double t50 = t5*t12*t14; - double t51 = t13*t26*w1*w2*w3*2.0; - double t52 = t10*t25*w1*w2*w3; - double t60 = t15*t15; - double t61 = t18*t18; - double t62 = t23*t23; - double t63 = t60+t61+t62; - double t64 = t5*t10*t25; - double t65 = 1.0/sqrt(t63); - double t66 = Y1*r2*t6; - double t67 = Z1*r3*t7; - double t68 = r1*t1*t5; - double t69 = r1*t1*t6; - double t70 = r1*t1*t7; - double t71 = r2*t2*t5; - double t72 = r2*t2*t6; - double t73 = r2*t2*t7; - double t74 = r3*t3*t5; - double t75 = r3*t3*t6; - double t76 = r3*t3*t7; - double t77 = X1*r1*t5; - double t78 = X1*r2*w1*w2; - double t79 = X1*r3*w1*w3; - double t80 = Y1*r1*w1*w2; - double t81 = Y1*r3*w2*w3; - double t82 = Z1*r1*w1*w3; - double t83 = Z1*r2*w2*w3; - double t84 = X1*r1*t6*t12; - double t85 = X1*r1*t7*t12; - double t86 = Y1*r2*t5*t12; - double t87 = Y1*r2*t7*t12; - double t88 = Z1*r3*t5*t12; - double t89 = Z1*r3*t6*t12; - double t90 = X1*r2*t9*t10*w3; - double t91 = Y1*r3*t9*t10*w1; - double t92 = Z1*r1*t9*t10*w2; - double t102 = X1*r3*t9*t10*w2; - double t103 = Y1*r1*t9*t10*w3; - double t104 = Z1*r2*t9*t10*w1; - double t105 = X1*r2*t12*w1*w2; - double t106 = X1*r3*t12*w1*w3; - double t107 = Y1*r1*t12*w1*w2; - double t108 = Y1*r3*t12*w2*w3; - double t109 = Z1*r1*t12*w1*w3; - double t110 = Z1*r2*t12*w2*w3; - double t93 = t66+t67+t68+t69+t70+t71+t72+t73+t74+t75+t76+t77+t78+t79+t80+t81+t82+t83+t84+t85+t86+t87+t88+t89+t90+t91+t92-t102-t103-t104-t105-t106-t107-t108-t109-t110; - double t94 = t10*t25*w1*w2; - double t95 = t6*t10*t25*w3; - double t96 = t6*t13*t26*w3*2.0; - double t97 = t12*t14*w2*w3; - double t98 = t6*t10*t25*w1; - double t99 = t6*t13*t26*w1*2.0; - double t100 = t6*t10*t25; - double t101 = 1.0/pow(t63,3.0/2.0); - double t111 = t6*t12*t14; - double t112 = t10*t25*w2*w3; - double t113 = t12*t14*w1*w3; - double t114 = t7*t10*t25*w2; - double t115 = t7*t13*t26*w2*2.0; - double t116 = t7*t10*t25*w1; - double t117 = t7*t13*t26*w1*2.0; - double t118 = t7*t12*t14; - double t119 = t13*t24*t26*w1*2.0; - double t120 = t10*t24*t25*w1; - double t121 = t119+t120; - double t122 = t13*t26*t34*w1*2.0; - double t123 = t10*t25*t34*w1; - double t131 = t13*t14*w1*2.0; - double t124 = t122+t123-t131; - double t139 = t13*t14*w3; - double t125 = -t35+t36+t37+t94-t139; - double t126 = X1*t125; - double t127 = t49+t50+t51+t52-t64; - double t128 = Y1*t127; - double t129 = t126+t128-Z1*t124; - double t130 = t23*t129*2.0; - double t132 = t13*t26*t45*w1*2.0; - double t133 = t10*t25*t45*w1; - double t138 = t13*t14*w2; - double t134 = -t46+t47+t48+t113-t138; - double t135 = X1*t134; - double t136 = -t49-t50+t51+t52+t64; - double t137 = Z1*t136; - double t140 = X1*s1*t5; - double t141 = Y1*s2*t6; - double t142 = Z1*s3*t7; - double t143 = s1*t1*t5; - double t144 = s1*t1*t6; - double t145 = s1*t1*t7; - double t146 = s2*t2*t5; - double t147 = s2*t2*t6; - double t148 = s2*t2*t7; - double t149 = s3*t3*t5; - double t150 = s3*t3*t6; - double t151 = s3*t3*t7; - double t152 = X1*s2*w1*w2; - double t153 = X1*s3*w1*w3; - double t154 = Y1*s1*w1*w2; - double t155 = Y1*s3*w2*w3; - double t156 = Z1*s1*w1*w3; - double t157 = Z1*s2*w2*w3; - double t158 = X1*s1*t6*t12; - double t159 = X1*s1*t7*t12; - double t160 = Y1*s2*t5*t12; - double t161 = Y1*s2*t7*t12; - double t162 = Z1*s3*t5*t12; - double t163 = Z1*s3*t6*t12; - double t164 = X1*s2*t9*t10*w3; - double t165 = Y1*s3*t9*t10*w1; - double t166 = Z1*s1*t9*t10*w2; - double t183 = X1*s3*t9*t10*w2; - double t184 = Y1*s1*t9*t10*w3; - double t185 = Z1*s2*t9*t10*w1; - double t186 = X1*s2*t12*w1*w2; - double t187 = X1*s3*t12*w1*w3; - double t188 = Y1*s1*t12*w1*w2; - double t189 = Y1*s3*t12*w2*w3; - double t190 = Z1*s1*t12*w1*w3; - double t191 = Z1*s2*t12*w2*w3; - double t167 = t140+t141+t142+t143+t144+t145+t146+t147+t148+t149+t150+t151+t152+t153+t154+t155+t156+t157+t158+t159+t160+t161+t162+t163+t164+t165+t166-t183-t184-t185-t186-t187-t188-t189-t190-t191; - double t168 = t13*t26*t45*w2*2.0; - double t169 = t10*t25*t45*w2; - double t170 = t168+t169; - double t171 = t13*t26*t34*w2*2.0; - double t172 = t10*t25*t34*w2; - double t176 = t13*t14*w2*2.0; - double t173 = t171+t172-t176; - double t174 = -t49+t51+t52+t100-t111; - double t175 = X1*t174; - double t177 = t13*t24*t26*w2*2.0; - double t178 = t10*t24*t25*w2; - double t192 = t13*t14*w1; - double t179 = -t97+t98+t99+t112-t192; - double t180 = Y1*t179; - double t181 = t49+t51+t52-t100+t111; - double t182 = Z1*t181; - double t193 = t13*t26*t34*w3*2.0; - double t194 = t10*t25*t34*w3; - double t195 = t193+t194; - double t196 = t13*t26*t45*w3*2.0; - double t197 = t10*t25*t45*w3; - double t200 = t13*t14*w3*2.0; - double t198 = t196+t197-t200; - double t199 = t7*t10*t25; - double t201 = t13*t24*t26*w3*2.0; - double t202 = t10*t24*t25*w3; - double t203 = -t49+t51+t52-t118+t199; - double t204 = Y1*t203; - double t205 = t1*2.0; - double t206 = Z1*t29*2.0; - double t207 = X1*t32*2.0; - double t208 = t205+t206+t207-Y1*t27*2.0; - double t209 = t2*2.0; - double t210 = X1*t53*2.0; - double t211 = Y1*t58*2.0; - double t212 = t209+t210+t211-Z1*t55*2.0; - double t213 = t3*2.0; - double t214 = Y1*t40*2.0; - double t215 = Z1*t43*2.0; - double t216 = t213+t214+t215-X1*t38*2.0; - jacs(0, 0) = t14*t65*(X1*r1*w1*2.0+X1*r2*w2+X1*r3*w3+Y1*r1*w2+Z1*r1*w3+r1*t1*w1*2.0+r2*t2*w1*2.0+r3*t3*w1*2.0+Y1*r3*t5*t12+Y1*r3*t9*t10-Z1*r2*t5*t12-Z1*r2*t9*t10-X1*r2*t12*w2-X1*r3*t12*w3-Y1*r1*t12*w2+Y1*r2*t12*w1*2.0-Z1*r1*t12*w3+Z1*r3*t12*w1*2.0+Y1*r3*t5*t10*t11-Z1*r2*t5*t10*t11+X1*r2*t12*w1*w3-X1*r3*t12*w1*w2-Y1*r1*t12*w1*w3+Z1*r1*t12*w1*w2-Y1*r1*t10*t11*w1*w3+Z1*r1*t10*t11*w1*w2-X1*r1*t6*t10*t11*w1-X1*r1*t7*t10*t11*w1+X1*r2*t5*t10*t11*w2+X1*r3*t5*t10*t11*w3+Y1*r1*t5*t10*t11*w2-Y1*r2*t5*t10*t11*w1-Y1*r2*t7*t10*t11*w1+Z1*r1*t5*t10*t11*w3-Z1*r3*t5*t10*t11*w1-Z1*r3*t6*t10*t11*w1+X1*r2*t10*t11*w1*w3-X1*r3*t10*t11*w1*w2+Y1*r3*t10*t11*w1*w2*w3+Z1*r2*t10*t11*w1*w2*w3)-t26*t65*t93*w1*2.0-t14*t93*t101*(t130+t15*(-X1*t121+Y1*(t46+t47+t48-t13*t14*w2-t12*t14*w1*w3)+Z1*(t35+t36+t37-t13*t14*w3-t10*t25*w1*w2))*2.0+t18*(t135+t137-Y1*(t132+t133-t13*t14*w1*2.0))*2.0)*(1.0/2.0); - jacs(0, 1) = t14*t65*(X1*r2*w1+Y1*r1*w1+Y1*r2*w2*2.0+Y1*r3*w3+Z1*r2*w3+r1*t1*w2*2.0+r2*t2*w2*2.0+r3*t3*w2*2.0-X1*r3*t6*t12-X1*r3*t9*t10+Z1*r1*t6*t12+Z1*r1*t9*t10+X1*r1*t12*w2*2.0-X1*r2*t12*w1-Y1*r1*t12*w1-Y1*r3*t12*w3-Z1*r2*t12*w3+Z1*r3*t12*w2*2.0-X1*r3*t6*t10*t11+Z1*r1*t6*t10*t11+X1*r2*t12*w2*w3-Y1*r1*t12*w2*w3+Y1*r3*t12*w1*w2-Z1*r2*t12*w1*w2-Y1*r1*t10*t11*w2*w3+Y1*r3*t10*t11*w1*w2-Z1*r2*t10*t11*w1*w2-X1*r1*t6*t10*t11*w2+X1*r2*t6*t10*t11*w1-X1*r1*t7*t10*t11*w2+Y1*r1*t6*t10*t11*w1-Y1*r2*t5*t10*t11*w2-Y1*r2*t7*t10*t11*w2+Y1*r3*t6*t10*t11*w3-Z1*r3*t5*t10*t11*w2+Z1*r2*t6*t10*t11*w3-Z1*r3*t6*t10*t11*w2+X1*r2*t10*t11*w2*w3+X1*r3*t10*t11*w1*w2*w3+Z1*r1*t10*t11*w1*w2*w3)-t26*t65*t93*w2*2.0-t14*t93*t101*(t18*(Z1*(-t35+t94+t95+t96-t13*t14*w3)-Y1*t170+X1*(t97+t98+t99-t13*t14*w1-t10*t25*w2*w3))*2.0+t15*(t180+t182-X1*(t177+t178-t13*t14*w2*2.0))*2.0+t23*(t175+Y1*(t35-t94+t95+t96-t13*t14*w3)-Z1*t173)*2.0)*(1.0/2.0); - jacs(0, 2) = t14*t65*(X1*r3*w1+Y1*r3*w2+Z1*r1*w1+Z1*r2*w2+Z1*r3*w3*2.0+r1*t1*w3*2.0+r2*t2*w3*2.0+r3*t3*w3*2.0+X1*r2*t7*t12+X1*r2*t9*t10-Y1*r1*t7*t12-Y1*r1*t9*t10+X1*r1*t12*w3*2.0-X1*r3*t12*w1+Y1*r2*t12*w3*2.0-Y1*r3*t12*w2-Z1*r1*t12*w1-Z1*r2*t12*w2+X1*r2*t7*t10*t11-Y1*r1*t7*t10*t11-X1*r3*t12*w2*w3+Y1*r3*t12*w1*w3+Z1*r1*t12*w2*w3-Z1*r2*t12*w1*w3+Y1*r3*t10*t11*w1*w3+Z1*r1*t10*t11*w2*w3-Z1*r2*t10*t11*w1*w3-X1*r1*t6*t10*t11*w3-X1*r1*t7*t10*t11*w3+X1*r3*t7*t10*t11*w1-Y1*r2*t5*t10*t11*w3-Y1*r2*t7*t10*t11*w3+Y1*r3*t7*t10*t11*w2+Z1*r1*t7*t10*t11*w1+Z1*r2*t7*t10*t11*w2-Z1*r3*t5*t10*t11*w3-Z1*r3*t6*t10*t11*w3-X1*r3*t10*t11*w2*w3+X1*r2*t10*t11*w1*w2*w3+Y1*r1*t10*t11*w1*w2*w3)-t26*t65*t93*w3*2.0-t14*t93*t101*(t18*(Z1*(t46-t113+t114+t115-t13*t14*w2)-Y1*t198+X1*(t49+t51+t52+t118-t7*t10*t25))*2.0+t23*(X1*(-t97+t112+t116+t117-t13*t14*w1)+Y1*(-t46+t113+t114+t115-t13*t14*w2)-Z1*t195)*2.0+t15*(t204+Z1*(t97-t112+t116+t117-t13*t14*w1)-X1*(t201+t202-t13*t14*w3*2.0))*2.0)*(1.0/2.0); - jacs(0, 3) = r1*t65-t14*t93*t101*t208*(1.0/2.0); - jacs(0, 4) = r2*t65-t14*t93*t101*t212*(1.0/2.0); - jacs(0, 5) = r3*t65-t14*t93*t101*t216*(1.0/2.0); - jacs(1, 0) = t14*t65*(X1*s1*w1*2.0+X1*s2*w2+X1*s3*w3+Y1*s1*w2+Z1*s1*w3+s1*t1*w1*2.0+s2*t2*w1*2.0+s3*t3*w1*2.0+Y1*s3*t5*t12+Y1*s3*t9*t10-Z1*s2*t5*t12-Z1*s2*t9*t10-X1*s2*t12*w2-X1*s3*t12*w3-Y1*s1*t12*w2+Y1*s2*t12*w1*2.0-Z1*s1*t12*w3+Z1*s3*t12*w1*2.0+Y1*s3*t5*t10*t11-Z1*s2*t5*t10*t11+X1*s2*t12*w1*w3-X1*s3*t12*w1*w2-Y1*s1*t12*w1*w3+Z1*s1*t12*w1*w2+X1*s2*t10*t11*w1*w3-X1*s3*t10*t11*w1*w2-Y1*s1*t10*t11*w1*w3+Z1*s1*t10*t11*w1*w2-X1*s1*t6*t10*t11*w1-X1*s1*t7*t10*t11*w1+X1*s2*t5*t10*t11*w2+X1*s3*t5*t10*t11*w3+Y1*s1*t5*t10*t11*w2-Y1*s2*t5*t10*t11*w1-Y1*s2*t7*t10*t11*w1+Z1*s1*t5*t10*t11*w3-Z1*s3*t5*t10*t11*w1-Z1*s3*t6*t10*t11*w1+Y1*s3*t10*t11*w1*w2*w3+Z1*s2*t10*t11*w1*w2*w3)-t14*t101*t167*(t130+t15*(Y1*(t46+t47+t48-t113-t138)+Z1*(t35+t36+t37-t94-t139)-X1*t121)*2.0+t18*(t135+t137-Y1*(-t131+t132+t133))*2.0)*(1.0/2.0)-t26*t65*t167*w1*2.0; - jacs(1, 1) = t14*t65*(X1*s2*w1+Y1*s1*w1+Y1*s2*w2*2.0+Y1*s3*w3+Z1*s2*w3+s1*t1*w2*2.0+s2*t2*w2*2.0+s3*t3*w2*2.0-X1*s3*t6*t12-X1*s3*t9*t10+Z1*s1*t6*t12+Z1*s1*t9*t10+X1*s1*t12*w2*2.0-X1*s2*t12*w1-Y1*s1*t12*w1-Y1*s3*t12*w3-Z1*s2*t12*w3+Z1*s3*t12*w2*2.0-X1*s3*t6*t10*t11+Z1*s1*t6*t10*t11+X1*s2*t12*w2*w3-Y1*s1*t12*w2*w3+Y1*s3*t12*w1*w2-Z1*s2*t12*w1*w2+X1*s2*t10*t11*w2*w3-Y1*s1*t10*t11*w2*w3+Y1*s3*t10*t11*w1*w2-Z1*s2*t10*t11*w1*w2-X1*s1*t6*t10*t11*w2+X1*s2*t6*t10*t11*w1-X1*s1*t7*t10*t11*w2+Y1*s1*t6*t10*t11*w1-Y1*s2*t5*t10*t11*w2-Y1*s2*t7*t10*t11*w2+Y1*s3*t6*t10*t11*w3-Z1*s3*t5*t10*t11*w2+Z1*s2*t6*t10*t11*w3-Z1*s3*t6*t10*t11*w2+X1*s3*t10*t11*w1*w2*w3+Z1*s1*t10*t11*w1*w2*w3)-t26*t65*t167*w2*2.0-t14*t101*t167*(t18*(X1*(t97+t98+t99-t112-t192)+Z1*(-t35+t94+t95+t96-t139)-Y1*t170)*2.0+t15*(t180+t182-X1*(-t176+t177+t178))*2.0+t23*(t175+Y1*(t35-t94+t95+t96-t139)-Z1*t173)*2.0)*(1.0/2.0); - jacs(1, 2) = t14*t65*(X1*s3*w1+Y1*s3*w2+Z1*s1*w1+Z1*s2*w2+Z1*s3*w3*2.0+s1*t1*w3*2.0+s2*t2*w3*2.0+s3*t3*w3*2.0+X1*s2*t7*t12+X1*s2*t9*t10-Y1*s1*t7*t12-Y1*s1*t9*t10+X1*s1*t12*w3*2.0-X1*s3*t12*w1+Y1*s2*t12*w3*2.0-Y1*s3*t12*w2-Z1*s1*t12*w1-Z1*s2*t12*w2+X1*s2*t7*t10*t11-Y1*s1*t7*t10*t11-X1*s3*t12*w2*w3+Y1*s3*t12*w1*w3+Z1*s1*t12*w2*w3-Z1*s2*t12*w1*w3-X1*s3*t10*t11*w2*w3+Y1*s3*t10*t11*w1*w3+Z1*s1*t10*t11*w2*w3-Z1*s2*t10*t11*w1*w3-X1*s1*t6*t10*t11*w3-X1*s1*t7*t10*t11*w3+X1*s3*t7*t10*t11*w1-Y1*s2*t5*t10*t11*w3-Y1*s2*t7*t10*t11*w3+Y1*s3*t7*t10*t11*w2+Z1*s1*t7*t10*t11*w1+Z1*s2*t7*t10*t11*w2-Z1*s3*t5*t10*t11*w3-Z1*s3*t6*t10*t11*w3+X1*s2*t10*t11*w1*w2*w3+Y1*s1*t10*t11*w1*w2*w3)-t26*t65*t167*w3*2.0-t14*t101*t167*(t18*(Z1*(t46-t113+t114+t115-t138)-Y1*t198+X1*(t49+t51+t52+t118-t199))*2.0+t23*(X1*(-t97+t112+t116+t117-t192)+Y1*(-t46+t113+t114+t115-t138)-Z1*t195)*2.0+t15*(t204+Z1*(t97-t112+t116+t117-t192)-X1*(-t200+t201+t202))*2.0)*(1.0/2.0); - jacs(1, 3) = s1*t65-t14*t101*t167*t208*(1.0/2.0); - jacs(1, 4) = s2*t65-t14*t101*t167*t212*(1.0/2.0); - jacs(1, 5) = s3*t65-t14*t101*t167*t216*(1.0/2.0); -} -}//End namespace ORB_SLAM2*/ + + +}//End namespace ORB_SLAM2 \ No newline at end of file From 4ab1846a41b98d4f1e2ae50a8f6736a6df577f37 Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Sun, 7 May 2023 14:54:03 +0300 Subject: [PATCH 4/8] mlpnp with notes and todo --- src/MLPnPsolver.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 9d379d0057..739238c64e 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -386,7 +386,7 @@ namespace ORB_SLAM3 { // if yes -> transform points to new eigen frame //if (minEigenVal < 1e-3 || minEigenVal == 0.0) //rankTest.setThreshold(1e-10); - if (rankTest.rank() == 2) { + if (rankTest.rank() == 2) { // TODO: check setting threshold, and when is it rank 2 planar = true; // self adjoint is faster and more accurate than general eigen solvers // also has closed form solution for 3x3 self-adjoint matrices @@ -407,7 +407,7 @@ namespace ORB_SLAM3 { // if we do have covariance information // -> fill covariance matrix - if (covMats.size() == numberCorrespondences) { + if (covMats.size() == numberCorrespondences) { //TODO: when is it not equal, check use of COV matrix use_cov = true; int l = 0; for (size_t i = 0; i < numberCorrespondences; ++i) { @@ -515,12 +515,12 @@ namespace ORB_SLAM3 { // 4. solve least squares ////////////////////////////////////// Eigen::MatrixXd AtPA; - if (use_cov) + if (use_cov) // TODO: notice no covariance AtPA = A.transpose() * P * A; // setting up the full normal equations seems to be unstable else AtPA = A.transpose() * A; - Eigen::JacobiSVD svd_A(AtPA, Eigen::ComputeFullV); + Eigen::JacobiSVD svd_A(AtPA, Eigen::ComputeFullV); //TODO: Eigen::ComputeThin check Eigen::MatrixXd result1 = svd_A.matrixV().col(colsA - 1); //////////////////////////////// @@ -782,8 +782,13 @@ namespace ORB_SLAM3 { nullspaces[i].col(0), nullspaces[i].col(1), w, T, jacs); - // r + fjac.block<1, 6>(ii, 0) = jacs.row(0); + + // s + fjac.block<1, 6>(ii + 1, 0) = jacs.row(1); + + /* // r fjac(ii, 0) = jacs(0, 0); fjac(ii, 1) = jacs(0, 1); fjac(ii, 2) = jacs(0, 2); @@ -798,7 +803,7 @@ namespace ORB_SLAM3 { fjac(ii + 1, 3) = jacs(1, 3); fjac(ii + 1, 4) = jacs(1, 4); - fjac(ii + 1, 5) = jacs(1, 5); + fjac(ii + 1, 5) = jacs(1, 5);*/ } ii += 2; From ea8ec16278749158b158707639111e4ef2818861 Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Fri, 19 May 2023 13:08:58 +0300 Subject: [PATCH 5/8] added todo + optimizations --- src/MLPnPsolver.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 739238c64e..1606bae784 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -353,6 +353,7 @@ namespace ORB_SLAM3 { } //MLPnP methods + //computePose is called in Refine and in RANSAC funcs void MLPnPsolver::computePose(const bearingVectors_t &f, const points_t &p, const cov3_mats_t &covMats, const std::vector &indices, transformation_t &result) { size_t numberCorrespondences = indices.size(); @@ -385,7 +386,8 @@ namespace ORB_SLAM3 { // if yes -> transform points to new eigen frame //if (minEigenVal < 1e-3 || minEigenVal == 0.0) - //rankTest.setThreshold(1e-10); + rankTest.setThreshold(1e-5); //TODO: run this + //rankTest.setThreshold(1e-10); // didn't change if (rankTest.rank() == 2) { // TODO: check setting threshold, and when is it rank 2 planar = true; // self adjoint is faster and more accurate than general eigen solvers @@ -408,6 +410,7 @@ namespace ORB_SLAM3 { // if we do have covariance information // -> fill covariance matrix if (covMats.size() == numberCorrespondences) { //TODO: when is it not equal, check use of COV matrix + // TODO: looks like numberCorrespondences is the size of input indices vector, and covMats is input use_cov = true; int l = 0; for (size_t i = 0; i < numberCorrespondences; ++i) { @@ -515,7 +518,7 @@ namespace ORB_SLAM3 { // 4. solve least squares ////////////////////////////////////// Eigen::MatrixXd AtPA; - if (use_cov) // TODO: notice no covariance + if (use_cov) // there's covariance, we chacked AtPA = A.transpose() * P * A; // setting up the full normal equations seems to be unstable else AtPA = A.transpose() * A; From 945edb69bffc44e547a1da1d227005233e360487 Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Thu, 25 May 2023 16:44:25 +0300 Subject: [PATCH 6/8] added function to compute covariance sigma v --- include/MLPnPsolver.h | 2 ++ src/MLPnPsolver.cpp | 55 +++++++++++++++++++++++++++++++++++++++---- 2 files changed, 53 insertions(+), 4 deletions(-) diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h index 963612c361..31c1cc6412 100644 --- a/include/MLPnPsolver.h +++ b/include/MLPnPsolver.h @@ -247,6 +247,8 @@ namespace ORB_SLAM3{ vector mvMaxError; GeometricCamera* mpCamera; + + Eigen::Matrix3d K; }; } diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 1606bae784..9357571660 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -52,6 +52,10 @@ namespace ORB_SLAM3 { + this->K << F.fx, 0, F.cx, + 0, F.fy, F.cy, + 0, 0, 1; + MLPnPsolver::MLPnPsolver(const Frame &F, const vector &vpMapPointMatches): mnInliersi(0), mnIterations(0), mnBestInliers(0), N(0), mpCamera(F.mpCamera){ mvpMapPointMatches = vpMapPointMatches; @@ -96,6 +100,45 @@ namespace ORB_SLAM3 { SetRansacParameters(); } + // Struct for holding the output of pix2rays function + struct Pix2RaysResult { + Eigen::MatrixXd v; + Eigen::MatrixXd sigma_v; + }; + + // Function to convert pixel coordinates to rays + Pix2RaysResult pix2rays(Eigen::MatrixXd pix, Eigen::MatrixXd cov = Eigen::MatrixXd()) { + // Add z coordinate if not already existing (=1, on image plane) + if (pix.rows() == 2) { + Eigen::MatrixXd ones = Eigen::MatrixXd::Ones(1, pix.cols()); + pix.conservativeResize(3, Eigen::NoChange); + pix.row(2) = ones; + } + + Eigen::MatrixXd x = K.inverse() * pix; + Eigen::VectorXd norm_x = x.colwise().norm(); + Eigen::MatrixXd v = x.array().colwise() / norm_x.array(); + + Eigen::MatrixXd sigma_v; + + // If observation covariance is provided, propagate it + if (cov.size() == 0) { + sigma_v = Eigen::MatrixXd(); + } else { + int nb_pts = pix.cols(); + sigma_v = Eigen::MatrixXd::Zero(9, nb_pts); + Eigen::MatrixXd sigma_x = Eigen::MatrixXd::Zero(3, 3); + + for (int i = 0; i < nb_pts; i++) { + sigma_x.block<2, 2>(0, 0) = Eigen::Map(cov.col(i).data()); + Eigen::MatrixXd J = Eigen::MatrixXd::Identity(3, 3) / norm_x(i) - (v.col(i) * v.col(i).transpose()); + sigma_v.col(i) = Eigen::Map((J * sigma_x * J.transpose()).data(), 9); + } + } + + return {v, sigma_v}; + } + //RANSAC methods bool MLPnPsolver::iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, Eigen::Matrix4f &Tout){ Tout.setIdentity(); @@ -375,6 +418,9 @@ namespace ORB_SLAM3 { points3v[i] = p[indices[i]]; } + //TODO: compute covMatrix according to points matrix: points3v[i] + Eigen::Matrix3d covMatrix; + ////////////////////////////////////// // 1. test if we have a planar scene ////////////////////////////////////// @@ -386,9 +432,9 @@ namespace ORB_SLAM3 { // if yes -> transform points to new eigen frame //if (minEigenVal < 1e-3 || minEigenVal == 0.0) - rankTest.setThreshold(1e-5); //TODO: run this + rankTest.setThreshold(1e-5); //rankTest.setThreshold(1e-10); // didn't change - if (rankTest.rank() == 2) { // TODO: check setting threshold, and when is it rank 2 + if (rankTest.rank() == 2) { // TODO: check setting threshold to lower value planar = true; // self adjoint is faster and more accurate than general eigen solvers // also has closed form solution for 3x3 self-adjoint matrices @@ -409,8 +455,9 @@ namespace ORB_SLAM3 { // if we do have covariance information // -> fill covariance matrix - if (covMats.size() == numberCorrespondences) { //TODO: when is it not equal, check use of COV matrix - // TODO: looks like numberCorrespondences is the size of input indices vector, and covMats is input + covMats = pix2rays(points3v, covMatrix) // TODO: remove covmax from function sig or xhane here + if (covMats.size() == numberCorrespondences) { //when is it not equal, check use of COV matrix + // numberCorrespondences is the size of input indices vector, and covMats is input use_cov = true; int l = 0; for (size_t i = 0; i < numberCorrespondences; ++i) { From 2fbfcfb749de7dafb68e820a0e541cde45fcfb4b Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Tue, 30 May 2023 15:11:27 +0300 Subject: [PATCH 7/8] applied cov matrix --- src/MLPnPsolver.cpp | 46 +++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 44 insertions(+), 2 deletions(-) diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp index 9357571660..bfa5ad1858 100644 --- a/src/MLPnPsolver.cpp +++ b/src/MLPnPsolver.cpp @@ -395,6 +395,42 @@ namespace ORB_SLAM3 { return false; } + //Funcs to get cov matrix + // Function to find mean. + float mean(float arr[], int n) + { + float sum = 0; + for (int i = 0; i < n; i++) + sum = sum + arr[i]; + return sum / n; + } + + // Function to find covariance. + float covariance(float arr1[], float arr2[], int n) + { + float sum = 0; + float mean_arr1 = mean(arr1, n); + float mean_arr2 = mean(arr2, n); + for (int i = 0; i < n; i++) + sum = sum + (arr1[i] - mean_arr1) * (arr2[i] - mean_arr2); + return sum / (n - 1); + } + + // Driver function. + int calc_covariance(arr1, arr2) + { + //float arr1[] = { 65.21, 64.75, 65.26, 65.76, 65.96 }; + int n = sizeof(arr1) / sizeof(arr1[0]); + //float arr2[] = { 67.25, 66.39, 66.12, 65.70, 66.64 }; + int m = sizeof(arr2) / sizeof(arr2[0]); + + if (m == n){ + covMatrix = covariance(arr1, arr2, m); + cout << covMatrix + } + return covMatrix; + } + //MLPnP methods //computePose is called in Refine and in RANSAC funcs void MLPnPsolver::computePose(const bearingVectors_t &f, const points_t &p, const cov3_mats_t &covMats, @@ -407,6 +443,8 @@ namespace ORB_SLAM3 { std::vector nullspaces(numberCorrespondences); Eigen::MatrixXd points3(3, numberCorrespondences); points_t points3v(numberCorrespondences); + points_t points3v_x(numberCorrespondences); + points_t points3v_y(numberCorrespondences); points4_t points4v(numberCorrespondences); for (size_t i = 0; i < numberCorrespondences; i++) { bearingVector_t f_current = f[indices[i]]; @@ -416,10 +454,14 @@ namespace ORB_SLAM3 { svd_f(f_current.transpose(), Eigen::ComputeFullV); nullspaces[i] = svd_f.matrixV().block(0, 1, 3, 2); points3v[i] = p[indices[i]]; + //getting just x,y indexes for cov matrix + points3v_x = points3v[i][0] + points3v_y = points3v[i][1] } - //TODO: compute covMatrix according to points matrix: points3v[i] - Eigen::Matrix3d covMatrix; + //computing covMatrix according to points matrix: points3v[i] + Eigen::Matrix3d covMatrix= calc_covariance(points3v_x, points3v_y) + ////////////////////////////////////// // 1. test if we have a planar scene From 9a98378ee23f6708c4abc65d7a7afc7b4528ea5c Mon Sep 17 00:00:00 2001 From: Neta Oren Date: Tue, 30 May 2023 16:44:11 +0300 Subject: [PATCH 8/8] edited .h file for covariance --- Examples/Monocular/mono_euroc.cc | 2 +- include/MLPnPsolver.h | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc index 3a233129be..1ea8f95784 100644 --- a/Examples/Monocular/mono_euroc.cc +++ b/Examples/Monocular/mono_euroc.cc @@ -80,7 +80,7 @@ int main(int argc, char **argv) int fps = 20; float dT = 1.f/fps; // Create SLAM system. It initializes all system threads and gets ready to process frames. - ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, false); + ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, true); float imageScale = SLAM.GetImageScale(); double t_resize = 0.f; diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h index 31c1cc6412..2a8002eb1b 100644 --- a/include/MLPnPsolver.h +++ b/include/MLPnPsolver.h @@ -71,6 +71,10 @@ namespace ORB_SLAM3{ bool iterate(int nIterations, bool &bNoMore, vector &vbInliers, int &nInliers, Eigen::Matrix4f &Tout); + int calc_covariance(arr1, arr2); + float covariance(float arr1[], float arr2[], int n); + float mean(float arr[], int n); + //Type definitions needed by the original code /** A 3-vector of unit length used to describe landmark observations/bearings