From 2a16fb2fc5091b5e00bd00b98713fc1a266621e8 Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Fri, 31 May 2019 11:38:49 +0200 Subject: [PATCH] added sobel kernel for image gradient calcualtion --- .vscode/c_cpp_properties.json | 16 ++++++++++ .../ipch/778a17e566a4909e/mmap_address.bin | Bin 0 -> 8 bytes .../ipch/ccf983af1f87ec2b/mmap_address.bin | Bin 0 -> 8 bytes .../ipch/e40aedd19a224f8d/mmap_address.bin | Bin 0 -> 8 bytes .vscode/settings.json | 6 ++++ include/msckf_vio/feature.hpp | 30 ++++++++++++++++++ launch/msckf_vio_tum.launch | 2 +- src/msckf_vio.cpp | 9 ++++-- 8 files changed, 60 insertions(+), 3 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/ipch/778a17e566a4909e/mmap_address.bin create mode 100644 .vscode/ipch/ccf983af1f87ec2b/mmap_address.bin create mode 100644 .vscode/ipch/e40aedd19a224f8d/mmap_address.bin create mode 100644 .vscode/settings.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..3d22904 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,16 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c11", + "cppStandard": "c++14", + "intelliSenseMode": "clang-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/ipch/778a17e566a4909e/mmap_address.bin b/.vscode/ipch/778a17e566a4909e/mmap_address.bin new file mode 100644 index 0000000000000000000000000000000000000000..42269a4348b4a6e1aa772ddb798bcf093c10412f GIT binary patch literal 8 PcmZQzV92>$&%gix2Mqzo literal 0 HcmV?d00001 diff --git a/.vscode/ipch/ccf983af1f87ec2b/mmap_address.bin b/.vscode/ipch/ccf983af1f87ec2b/mmap_address.bin new file mode 100644 index 0000000000000000000000000000000000000000..bea14df956f2662a8833d5673c14976bdf2072f3 GIT binary patch literal 8 PcmZQzU|4Xvo`C@X2hjoR literal 0 HcmV?d00001 diff --git a/.vscode/ipch/e40aedd19a224f8d/mmap_address.bin b/.vscode/ipch/e40aedd19a224f8d/mmap_address.bin new file mode 100644 index 0000000000000000000000000000000000000000..3e2f12af61ae10c427911503226fc90588481369 GIT binary patch literal 8 PcmZQzVDLz)XJ7yT1a$!_ literal 0 HcmV?d00001 diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..2086c47 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "core": "cpp", + "sparsecore": "cpp" + } +} \ No newline at end of file diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index fff17b8..2699104 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -168,6 +168,11 @@ struct Feature { const CameraCalibration& cam, Eigen::Vector3d& in_p) const; + double Kernel( + const cv::Point2f pose, + const cv::Mat frame, + std::string type) const; + /* * @brief IrradianceAnchorPatch_Camera returns irradiance values * of the Anchor Patch position in a camera frame @@ -387,6 +392,31 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const else return false; } +double Feature::Kernel( + const cv::Point2f pose, + const cv::Mat frame, + std::string type) const +{ +Eigen::Matrix kernel = Eigen::Matrix::Zero(); +if(type == "Sobel_x") + kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.; +else if(type == "Sobel_y") + kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.; + +double delta = 0; +int offs = (int)(kernel.rows()-1)/2; +for(int i = 0; i < kernel.rows(); i++){ + + for(int j = 0; j < kernel.cols(); j++) + { + std::cout << "i: " << i << ":" << "j: " << j << ":" << kernel(i,j) << std::endl; + std::cout <<"pose: " << pose.y+i-offs << " : " << pose.x+j-offs << std::endl; + delta += ((float)frame.at(pose.y+i-offs , pose.x+j-offs))/255 * (float)kernel(i,j); + } +} +std::cout << "delta " << delta << std::endl; +return delta; +} bool Feature::estimate_FrameIrradiance( const CAMState& cam_state, const StateIDType& cam_state_id, diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index eca1ebc..911f36b 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -24,7 +24,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 174be4a..203603b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1312,8 +1312,13 @@ void MsckfVio::PhotometricMeasurementJacobian( // calculate derivation for anchor frame, use position for derivation calculation // frame derivative calculated convoluting with kernel [-1, 0, 1] - dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); - dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); + + dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x"); + dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y"); + + // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame); + // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame); + dI_dhj(0, 0) = dx/(pixelDistance.x); dI_dhj(0, 1) = dy/(pixelDistance.y);