173 lines
3.8 KiB
C++
173 lines
3.8 KiB
C++
/*
|
|
* math_utils.h
|
|
*
|
|
* Created on: Jul 20, 2012
|
|
* Author: cforster
|
|
*/
|
|
|
|
#ifndef MATH_UTILS_H_
|
|
#define MATH_UTILS_H_
|
|
|
|
|
|
#include <Eigen/Core>
|
|
#include <Eigen/StdVector>
|
|
#include <sophus/se3.h>
|
|
#include <iostream>
|
|
|
|
namespace vk
|
|
{
|
|
|
|
using namespace Eigen;
|
|
using namespace std;
|
|
using namespace Sophus;
|
|
|
|
Vector3d triangulateFeatureNonLin(
|
|
const Matrix3d& R,
|
|
const Vector3d& t,
|
|
const Vector3d& feature1,
|
|
const Vector3d& feature2);
|
|
|
|
/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e.
|
|
/// perfect triangulation without noise!
|
|
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);
|
|
|
|
double reprojError(
|
|
const Vector3d& f1,
|
|
const Vector3d& f2,
|
|
double error_multiplier2);
|
|
|
|
double computeInliers(
|
|
const vector<Vector3d>& features1,
|
|
const vector<Vector3d>& features2,
|
|
const Matrix3d& R,
|
|
const Vector3d& t,
|
|
const double reproj_thresh,
|
|
double error_multiplier2,
|
|
vector<Vector3d>& xyz_vec,
|
|
vector<int>& inliers,
|
|
vector<int>& outliers);
|
|
|
|
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);
|
|
|
|
//! Direct Cosine Matrix to Roll Pitch Yaw
|
|
Vector3d dcm2rpy(const Matrix3d &R);
|
|
|
|
//! Roll Pitch Yaw to Direct Cosine Matrix
|
|
Matrix3d rpy2dcm(const Vector3d &rpy);
|
|
|
|
//! Angle Axis parametrization to Quaternion
|
|
Quaterniond angax2quat(const Vector3d& n, const double& angle);
|
|
|
|
//! Angle Axis parametrization to Matrix representation
|
|
Matrix3d angax2dcm(const Vector3d& n, const double& angle);
|
|
|
|
double sampsonusError(
|
|
const Vector2d &v2Dash,
|
|
const Matrix3d& m3Essential,
|
|
const Vector2d& v2);
|
|
|
|
inline Matrix3d sqew(const Vector3d& v)
|
|
{
|
|
Matrix3d v_sqew;
|
|
v_sqew << 0, -v[2], v[1],
|
|
v[2], 0, -v[0],
|
|
-v[1], v[0], 0;
|
|
return v_sqew;
|
|
}
|
|
|
|
inline double norm_max(const Eigen::VectorXd & v)
|
|
{
|
|
double max = -1;
|
|
for (int i=0; i<v.size(); i++)
|
|
{
|
|
double abs = fabs(v[i]);
|
|
if(abs>max){
|
|
max = abs;
|
|
}
|
|
}
|
|
return max;
|
|
}
|
|
|
|
inline Vector2d project2d(const Vector3d& v)
|
|
{
|
|
return v.head<2>()/v[2];
|
|
}
|
|
|
|
inline Vector3d unproject2d(const Vector2d& v)
|
|
{
|
|
return Vector3d(v[0], v[1], 1.0);
|
|
}
|
|
|
|
inline Vector3d project3d(const Vector4d& v)
|
|
{
|
|
return v.head<3>()/v[3];
|
|
}
|
|
|
|
inline Vector4d unproject3d(const Vector3d& v)
|
|
{
|
|
return Vector4d(v[0], v[1], v[2], 1.0);
|
|
}
|
|
|
|
template<class T>
|
|
T getMedian(vector<T>& data_vec)
|
|
{
|
|
assert(!data_vec.empty());
|
|
typename vector<T>::iterator it = data_vec.begin()+floor(data_vec.size()/2);
|
|
nth_element(data_vec.begin(), it, data_vec.end());
|
|
return *it;
|
|
}
|
|
|
|
inline double pyrFromZero_d(double x_0, int level)
|
|
{
|
|
return x_0/(1<<level); // = 1 / 2^level
|
|
}
|
|
|
|
inline Vector2d pyrFromZero_2d(const Vector2d& uv_0, int level)
|
|
{
|
|
return Vector2d(pyrFromZero_d(uv_0[0], level),
|
|
pyrFromZero_d(uv_0[1], level));
|
|
}
|
|
|
|
inline void
|
|
frameJac_xyz2uv(const Vector3d & xyz,
|
|
const double & focal_length,
|
|
Matrix<double,2,6> & frame_jac)
|
|
{
|
|
const double x = xyz[0];
|
|
const double y = xyz[1];
|
|
const double z = xyz[2];
|
|
const double z_2 = z*z;
|
|
|
|
frame_jac(0,0) = -1./z *focal_length;
|
|
frame_jac(0,1) = 0;
|
|
frame_jac(0,2) = x/z_2 *focal_length;
|
|
frame_jac(0,3) = x*y/z_2 * focal_length;
|
|
frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length;
|
|
frame_jac(0,5) = y/z *focal_length;
|
|
|
|
frame_jac(1,0) = 0;
|
|
frame_jac(1,1) = -1./z *focal_length;
|
|
frame_jac(1,2) = y/z_2 *focal_length;
|
|
frame_jac(1,3) = (1+y*y/z_2) *focal_length;
|
|
frame_jac(1,4) = -x*y/z_2 *focal_length;
|
|
frame_jac(1,5) = -x/z *focal_length;
|
|
}
|
|
|
|
} // end namespace vk
|
|
|
|
#endif /* MATH_UTILS_H_ */
|