void CameraParameters::field2image(
GVector::vector3d<double> &p_f, GVector::vector2d<double> &p_i,
Eigen::VectorXd &p) {
Quaternion<double> q_field2cam = Quaternion<double>(
q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
q_field2cam.norm();
GVector::vector3d<double> translation = GVector::vector3d<double>(
tx->getDouble(),ty->getDouble(),tz->getDouble());
GVector::vector3d<double> aa_diff(p[Q_1], p[Q_2], p[Q_3]);
GVector::vector3d<double> t_diff(p[T_1], p[T_2], p[T_3]);
// Create a quaternion out of the 3D rotational representation
Quaternion<double> q_diff;
q_diff.setAxis(aa_diff.norm(), aa_diff.length());
// First transform the point from the field into the coordinate system of the
// camera
GVector::vector3d<double> p_c =
(q_diff * q_field2cam ).rotateVectorByQuaternion(p_f) + translation +
t_diff;
GVector::vector2d<double> p_un =
GVector::vector2d<double>(p_c.x/p_c.z, p_c.y/p_c.z);
// Apply distortion
GVector::vector2d<double> p_d;
radialDistortion(p_un,p_d,distortion->getDouble() + p[DIST]);
// Then project from the camera coordinate system onto the image plane using
// the instrinsic parameters
p_i = (focal_length->getDouble() + p[FOCAL_LENGTH]) * p_d +
GVector::vector2d<double>(principal_point_x->getDouble() + p[PP_X],
principal_point_y->getDouble() + p[PP_Y]);
}
void CameraParameters::radialDistortion(
const GVector::vector2d<double> pu,
GVector::vector2d<double> &pd, double dist) const {
double rd = radialDistortion(pu.length(), dist);
pd = pu;
pd = pd.norm(rd);
}
void CameraParameters::field2image(
const GVector::vector3d<double> &p_f,
GVector::vector2d<double> &p_i) const {
Quaternion<double> q_field2cam = Quaternion<double>(
q0->getDouble(),q1->getDouble(),q2->getDouble(),q3->getDouble());
q_field2cam.norm();
GVector::vector3d<double> translation = GVector::vector3d<double>(
tx->getDouble(),ty->getDouble(),tz->getDouble());
// First transform the point from the field into the coordinate system of the
// camera
GVector::vector3d<double> p_c =
q_field2cam.rotateVectorByQuaternion(p_f) + translation;
GVector::vector2d<double> p_un =
GVector::vector2d<double>(p_c.x/p_c.z, p_c.y/p_c.z);
// Apply distortion
GVector::vector2d<double> p_d;
radialDistortion(p_un,p_d);
// Then project from the camera coordinate system onto the image plane using
// the instrinsic parameters
p_i = focal_length->getDouble() * p_d +
GVector::vector2d<double>(principal_point_x->getDouble(),
principal_point_y->getDouble());
}
vec4 effect(vec4 color, Image texture, vec2 texture_coords, vec2 pixel_coords)
{
vec2 coords = radialDistortion(texture_coords, inputSize / textureSize);
vec4 texcolor = checkTexelBounds(texture, coords, vec2(inputSize.x / textureSize.x, 1.0 - inputSize.y / textureSize.y));
texcolor.a = 1.0;
return texcolor;
}
