potok/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2026-07-31 00:03:03 +04:00
Code Packages Releases Wiki Activity

Merge pull request #21018 from savuor:levmarqfromscratch

Browse Source 
New LevMarq implementation

* Hash TSDF fix: apply volume pose when fetching pose

* DualQuat minor fix

* Pose Graph: getEdgePose(), getEdgeInfo()

* debugging code for pose graph

* add edge to submap

* pose averaging: DualQuats instead of matrix averaging

* overlapping ratio: rise it up; minor comment

* remove `Submap::addEdgeToSubmap`

* test_pose_graph: minor

* SparseBlockMatrix: support 1xN as well as Nx1 for residual vector

* small changes to old LMSolver

* new LevMarq impl

* Pose Graph rewritten to use new impl

* solvePnP(), findHomography() and findExtrinsicCameraParams2() use new impl

* estimateAffine...2D() use new impl

* calibration and stereo calibration use new impl

* BundleAdjusterBase::estimate() uses new impl

* new LevMarq interface

* PoseGraph: changing opt interface

* findExtrinsicCameraParams2(): opt interface updated

* HomographyRefine: opt interface updated

* solvePnPRefine opt interface fixed

* Affine2DRefine opt interface fixed

* BundleAdjuster::estimate() opt interface fixed

* calibration: opt interface fixed + code refactored a little

* minor warning fixes

* geodesic acceleration, Impl -> Backend rename

* calcFunc() always uses probe vars

* solveDecomposed, fixing negation

* fixing geodesic acceleration + minors

* PoseGraph exposes its optimizer now + its tests updated to check better convegence

* Rosenbrock test added for LevMarq

* LevMarq params upgraded

* Rosenbrock can do better

* fixing stereo calibration

* old implementation removed (as well as debug code)

* more debugging code removed

* fix warnings

* fixing warnings

* fixing Eigen dependency

* trying to fix Eigen deps

* debugging code for submat is now temporary

* trying to fix Eigen dependency

* relax sanity check for solvePnP

* relaxing sanity check even more

* trying to fix Eigen dependency

* warning fix

* Quat<T>: fixing warnings

* more warning fixes

* fixed warning

* fixing *KinFu OCL tests

* algo params -> struct Settings

* Backend moved to details

* BaseLevMarq -> LevMarqBase

* detail/pose_graph.hpp -> detail/optimizer.hpp

* fixing include stuff for details/optimizer.hpp

* doc fix

* LevMarqBase rework: Settings, pImpl, Backend

* Impl::settings and ::backend fix

* HashTSDFGPU fix

* fixing compilation

* warning fix for OdometryFrameImplTMat

* docs fix + compile warnings

* remake: new class LevMarq with pImpl and enums, LevMarqBase => detail, no Backend class, Settings() => .cpp, Settings==() removed, Settings.set...() inlines

* fixing warnings & whitespace
This commit is contained in:
Rostislav Vasilikhin
2021-12-28 00:51:32 +03:00
committed by GitHub
parent 4d9365990a
commit 9d6f388809
21 changed files with 2184 additions and 1067 deletions
Download Patch File Download Diff File Expand all files Collapse all files
modules/core/include/opencv2/core/dualquaternion.inl.hpp
+1 -1
View File
@@ -158,7 +158,7 @@ inline Quat<T> DualQuat<T>::getRotation(QuatAssumeType assumeUnit) const
template <typename T>
inline Vec<T, 3> DualQuat<T>::getTranslation(QuatAssumeType assumeUnit) const
{
Quat<T> trans = 2.0 * (getDualPart() * getRealPart().inv(assumeUnit));
Quat<T> trans = T(2.0) * (getDualPart() * getRealPart().inv(assumeUnit));
return Vec<T, 3>{trans[1], trans[2], trans[3]};
}
modules/core/include/opencv2/core/quaternion.inl.hpp
+10 -10
View File
@@ -54,7 +54,7 @@ Quat<T> Quat<T>::createFromAngleAxis(const T angle, const Vec<T, 3> &axis)
{
CV_Error(Error::StsBadArg, "this quaternion does not represent a rotation");
}
const T angle_half = angle * 0.5;
const T angle_half = angle * T(0.5);
w = std::cos(angle_half);
const T sin_v = std::sin(angle_half);
const T sin_norm = sin_v / vNorm;
@@ -79,35 +79,35 @@ Quat<T> Quat<T>::createFromRotMat(InputArray _R)
T trace = R(0, 0) + R(1, 1) + R(2, 2);
if (trace > 0)
{
S = std::sqrt(trace + 1) * 2;
S = std::sqrt(trace + 1) * T(2);
x = (R(1, 2) - R(2, 1)) / S;
y = (R(2, 0) - R(0, 2)) / S;
z = (R(0, 1) - R(1, 0)) / S;
w = -0.25 * S;
w = -T(0.25) * S;
}
else if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2))
{
S = std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)) * 2;
x = -0.25 * S;
S = std::sqrt(T(1.0) + R(0, 0) - R(1, 1) - R(2, 2)) * T(2);
x = -T(0.25) * S;
y = -(R(1, 0) + R(0, 1)) / S;
z = -(R(0, 2) + R(2, 0)) / S;
w = (R(1, 2) - R(2, 1)) / S;
}
else if (R(1, 1) > R(2, 2))
{
S = std::sqrt(1.0 - R(0, 0) + R(1, 1) - R(2, 2)) * 2;
S = std::sqrt(T(1.0) - R(0, 0) + R(1, 1) - R(2, 2)) * T(2);
x = (R(0, 1) + R(1, 0)) / S;
y = 0.25 * S;
y = T(0.25) * S;
z = (R(1, 2) + R(2, 1)) / S;
w = (R(0, 2) - R(2, 0)) / S;
}
else
{
S = std::sqrt(1.0 - R(0, 0) - R(1, 1) + R(2, 2)) * 2;
S = std::sqrt(T(1.0) - R(0, 0) - R(1, 1) + R(2, 2)) * T(2);
x = (R(0, 2) + R(2, 0)) / S;
y = (R(1, 2) + R(2, 1)) / S;
z = 0.25 * S;
z = T(0.25) * S;
w = -(R(0, 1) - R(1, 0)) / S;
}
return Quat<T> (w, x, y, z);
@@ -268,7 +268,7 @@ inline Quat<T>& Quat<T>::operator/=(const T a)
template <typename T>
inline Quat<T> Quat<T>::operator/(const T a) const
{
const T a_inv = 1.0 / a;
const T a_inv = T(1.0) / a;
return Quat<T>(w * a_inv, x * a_inv, y * a_inv, z * a_inv);
}