add banchmark image alignment 2

2021-08-23 15:35:30 +02:00
parent 8df5e42a5b
commit 90e452f0c9
34 changed files with 3294 additions and 0 deletions
@@ -0,0 +1,202 @@
+/*
+ * math_utils.cpp
+ *
+ *  Created on: Jul 20, 2012
+ *      Author: cforster
+ */
+
+#include <math_utils.h>
+
+namespace vk {
+
+using namespace Eigen;
+
+Vector3d
+triangulateFeatureNonLin(const Matrix3d& R,  const Vector3d& t,
+                         const Vector3d& feature1, const Vector3d& feature2 )
+{
+  Vector3d f2 = R * feature2;
+  Vector2d b;
+  b[0] = t.dot(feature1);
+  b[1] = t.dot(f2);
+  Matrix2d A;
+  A(0,0) = feature1.dot(feature1);
+  A(1,0) = feature1.dot(f2);
+  A(0,1) = -A(1,0);
+  A(1,1) = -f2.dot(f2);
+  Vector2d lambda = A.inverse() * b;
+  Vector3d xm = lambda[0] * feature1;
+  Vector3d xn = t + lambda[1] * f2;
+  return ( xm + xn )/2;
+}
+
+bool
+depthFromTriangulationExact(
+    const Matrix3d& R_r_c,
+    const Vector3d& t_r_c,
+    const Vector3d& f_r,
+    const Vector3d& f_c,
+    double& depth_in_r,
+    double& depth_in_c)
+{
+  // bearing vectors (f_r, f_c) do not need to be unit length
+  const Vector3d f_c_in_r(R_r_c*f_c);
+  const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r);
+  const double b = f_c_in_r.dot(t_r_c);
+  const double denom = (a*b - f_c_in_r.dot(f_c_in_r));
+
+  if(abs(denom) < 0.000001)
+    return false;
+
+  depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom;
+  depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm();
+  return true;
+}
+
+double
+reprojError(const Vector3d& f1,
+            const Vector3d& f2,
+            double error_multiplier2)
+{
+  Vector2d e = project2d(f1) - project2d(f2);
+  return error_multiplier2 * e.norm();
+}
+
+double
+computeInliers(const vector<Vector3d>& features1, // c1
+               const vector<Vector3d>& features2, // c2
+               const Matrix3d& R,                 // R_c1_c2
+               const Vector3d& t,                 // c1_t
+               const double reproj_thresh,
+               double error_multiplier2,
+               vector<Vector3d>& xyz_vec,         // in frame c1
+               vector<int>& inliers,
+               vector<int>& outliers)
+{
+  inliers.clear(); inliers.reserve(features1.size());
+  outliers.clear(); outliers.reserve(features1.size());
+  xyz_vec.clear(); xyz_vec.reserve(features1.size());
+  double tot_error = 0;
+  //triangulate all features and compute reprojection errors and inliers
+  for(size_t j=0; j<features1.size(); ++j)
+  {
+    xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
+    double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
+    double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
+    if(e1 > reproj_thresh || e2 > reproj_thresh)
+      outliers.push_back(j);
+    else
+    {
+      inliers.push_back(j);
+      tot_error += e1+e2;
+    }
+  }
+  return tot_error;
+}
+
+void
+computeInliersOneView(const vector<Vector3d> & feature_sphere_vec,
+                      const vector<Vector3d> & xyz_vec,
+                      const Matrix3d &R,
+                      const Vector3d &t,
+                      const double reproj_thresh,
+                      const double error_multiplier2,
+                      vector<int>& inliers,
+                      vector<int>& outliers)
+{
+  inliers.clear(); inliers.reserve(xyz_vec.size());
+  outliers.clear(); outliers.reserve(xyz_vec.size());
+  for(size_t j = 0; j < xyz_vec.size(); j++ )
+  {
+    double e = reprojError(feature_sphere_vec[j],
+                           R.transpose() * ( xyz_vec[j] - t ),
+                           error_multiplier2);
+    if(e < reproj_thresh)
+      inliers.push_back(j);
+    else
+      outliers.push_back(j);
+  }
+}
+
+Vector3d
+dcm2rpy(const Matrix3d &R)
+{
+  Vector3d rpy;
+  rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) );
+  if( fabs( rpy[1] - M_PI/2 ) < 0.00001 )
+  {
+    rpy[2] = 0;
+    rpy[0] = -atan2( R(0,1), R(1,1) );
+  }
+  else
+  {
+    if( fabs( rpy[1] + M_PI/2 ) < 0.00001 )
+    {
+      rpy[2] = 0;
+      rpy[0] = -atan2( R(0,1), R(1,1) );
+    }
+    else
+    {
+      rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) );
+      rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) );
+    }
+  }
+  return rpy;
+}
+
+Matrix3d
+rpy2dcm(const Vector3d &rpy)
+{
+  Matrix3d R1;
+  R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0;
+  R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]);
+  R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1);
+
+  Matrix3d R2;
+  R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]);
+  R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0;
+  R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0);
+
+  Matrix3d R3;
+  R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0;
+  R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0;
+  R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0;
+
+  return R3 * R2 * R1;
+}
+
+Quaterniond
+angax2quat(const Vector3d& n, const double& angle)
+{
+  // n must be normalized!
+  double s(sin(angle/2));
+  return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s );
+}
+
+
+Matrix3d
+angax2dcm(const Vector3d& n, const double& angle)
+{
+  // n must be normalized
+  Matrix3d sqewn(sqew(n));
+  return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle)));
+}
+
+double
+sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2)
+{
+  Vector3d v3Dash = unproject2d(v2Dash);
+  Vector3d v3 = unproject2d(v2);
+
+  double dError = v3Dash.transpose() * Essential * v3;
+
+  Vector3d fv3 = Essential * v3;
+  Vector3d fTv3Dash = Essential.transpose() * v3Dash;
+
+  Vector2d fv3Slice = fv3.head<2>();
+  Vector2d fTv3DashSlice = fTv3Dash.head<2>();
+
+  return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice)));
+}
+
+} // end namespace vk