Skip to content

Commit

Permalink
Merge pull request #6 from aangerma/AC1xx
Browse files Browse the repository at this point in the history 
Optimization by P
  • Loading branch information
@maloel
maloel authored May 21, 2020
2 parents e007cd9 + 3d831b6 commit 952ba82
Show file tree
Hide file tree
Showing 16 changed files with 857 additions and 625 deletions.
92 changes: 61 additions & 31 deletions src/algo/depth-to-rgb-calibration/calibration-types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#pragma once

#include <cstdint> // all the basic types (uint8_t)
#include <vector> // all the basic types (uint8_t)


namespace librealsense {
Expand All @@ -27,6 +28,66 @@ namespace depth_to_rgb_calibration {
bool operator != (const double2 d) { return !(*this == d); }
};

struct double3x3
{
double mat[3][3];
double3x3 transpose()
{
double3x3 res = { 0 };

for (auto i = 0; i < 3; i++)
{
for (auto j = 0; j < 3; j++)
{
res.mat[i][j] = mat[j][i];
}
}
return res;
}

double3x3 operator*(const double3x3& other)
{
double3x3 res = { 0 };

for (auto i = 0; i < 3; i++)
{
for (auto j = 0; j < 3; j++)
{
double sum = 0;
for (auto l = 0; l < 3; l++)
{
sum += mat[i][l] * other.mat[l][j];
}
res.mat[i][j] = sum;
}
}
return res;
}

double3 operator*(const double3& other)
{
double3 res = { 0 };

res.x = mat[0][0] * other.x + mat[0][1] * other.y + mat[0][2] * other.z;
res.y = mat[1][0] * other.x + mat[1][1] * other.y + mat[1][2] * other.z;
res.z = mat[2][0] * other.x + mat[2][1] * other.y + mat[2][2] * other.z;
return res;
}

std::vector<double> to_vector()
{
std::vector<double> res;
for (auto i = 0; i < 3; i++)
{
for (auto j = 0; j < 3; j++)
{
res.push_back(mat[i][j]);
}
}
return res;
}
};

enum direction :uint8_t
{
//deg_111, //to be aligned with matlab (maybe should be removed later)
Expand Down Expand Up @@ -79,37 +140,6 @@ namespace depth_to_rgb_calibration {
rotation extract_rotation_from_angles( const rotation_in_angles & rot_angles );
rotation_in_angles extract_angles_from_rotation( const double r[9] );

struct p_matrix
{
double vals[12];

bool operator==(const p_matrix& other)
{
for (auto i = 0; i < 12; i++)
{
if (vals[i] != other.vals[i])
return false;
}
return true;
}
bool operator!=(const p_matrix& other)
{
return !(*this == other);
}

bool operator<(const p_matrix& other)
{
for (auto i = 0; i < 12; i++)
{
if (vals[i] < other.vals[i])
return false;
if (vals[i] > other.vals[i])
return true;
}
return true;
}
};

struct k_matrix
{
double fx;
Expand Down
144 changes: 87 additions & 57 deletions src/algo/depth-to-rgb-calibration/calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,22 +63,18 @@ rs2_extrinsics_double calib::get_extrinsics() const
};
}

p_matrix const & calib::get_p_matrix() const
{
return p_mat;
}

p_matrix const & calib::calc_p_mat()
p_matrix const calib::calc_p_mat() const
{
auto r = rot.rot;
auto t = trans;
auto fx = k_mat.fx;
auto fy = k_mat.fy;
auto ppx = k_mat.ppx;
auto ppy = k_mat.ppy;
p_mat = { fx* r[0] + ppx * r[2], fx* r[3] + ppx * r[5], fx* r[6] + ppx * r[8], fx* t.t1 + ppx * t.t3,
fy* r[1] + ppy * r[2], fy* r[4] + ppy * r[5], fy* r[7] + ppy * r[8], fy* t.t2 + ppy * t.t3,
r[2] , r[5] , r[8] , t.t3 };
p_matrix p_mat = { fx* r[0] + ppx * r[6], fx* r[1] + ppx * r[7], fx* r[2] + ppx * r[8], fx* t.t1 + ppx * t.t3,
fy* r[3] + ppy * r[6], fy* r[4] + ppy * r[7], fy* r[5] + ppy * r[8], fy* t.t2 + ppy * t.t3,
r[6] , r[7] , r[8] , t.t3 };

return p_mat;
}

Expand All @@ -104,9 +100,8 @@ calib calib::operator*( double step_size ) const
res.k_mat.ppx = this->k_mat.ppx * step_size;
res.k_mat.ppy = this->k_mat.ppy *step_size;

res.rot_angles.alpha = this->rot_angles.alpha *step_size;
res.rot_angles.beta = this->rot_angles.beta *step_size;
res.rot_angles.gamma = this->rot_angles.gamma *step_size;
for (auto i = 0; i < 9; i++)
res.rot.rot[i] = this->rot.rot[i] * step_size;

res.trans.t1 = this->trans.t1 *step_size;
res.trans.t2 = this->trans.t2 * step_size;
Expand All @@ -130,9 +125,8 @@ calib calib::operator+( const calib & c ) const
res.k_mat.ppx = this->k_mat.ppx + c.k_mat.ppx;
res.k_mat.ppy = this->k_mat.ppy + c.k_mat.ppy;

res.rot_angles.alpha = this->rot_angles.alpha + c.rot_angles.alpha;
res.rot_angles.beta = this->rot_angles.beta + c.rot_angles.beta;
res.rot_angles.gamma = this->rot_angles.gamma + c.rot_angles.gamma;
for (auto i = 0; i < 9; i++)
res.rot.rot[i] = this->rot.rot[i] + c.rot.rot[i];

res.trans.t1 = this->trans.t1 + c.trans.t1;
res.trans.t2 = this->trans.t2 + c.trans.t2;
Expand All @@ -151,9 +145,8 @@ calib calib::operator-( const calib & c ) const
res.k_mat.ppx = this->k_mat.ppx - c.k_mat.ppx;
res.k_mat.ppy = this->k_mat.ppy - c.k_mat.ppy;

res.rot_angles.alpha = this->rot_angles.alpha - c.rot_angles.alpha;
res.rot_angles.beta = this->rot_angles.beta - c.rot_angles.beta;
res.rot_angles.gamma = this->rot_angles.gamma - c.rot_angles.gamma;
for (auto i = 0; i < 9; i++)
res.rot.rot[i] = this->rot.rot[i] - c.rot.rot[i];

res.trans.t1 = this->trans.t1 - c.trans.t1;
res.trans.t2 = this->trans.t2 - c.trans.t2;
Expand All @@ -172,9 +165,9 @@ calib calib::operator/( const calib & c ) const
res.k_mat.ppx = this->k_mat.ppx / c.k_mat.ppx;
res.k_mat.ppy = this->k_mat.ppy / c.k_mat.ppy;

res.rot_angles.alpha = this->rot_angles.alpha / c.rot_angles.alpha;
res.rot_angles.beta = this->rot_angles.beta / c.rot_angles.beta;
res.rot_angles.gamma = this->rot_angles.gamma / c.rot_angles.gamma;
for (auto i = 0; i < 9; i++)
res.rot.rot[i] = this->rot.rot[i] / c.rot.rot[i];


res.trans.t1 = this->trans.t1 / c.trans.t1;
res.trans.t2 = this->trans.t2 / c.trans.t2;
Expand All @@ -185,59 +178,96 @@ calib calib::operator/( const calib & c ) const
return res;
}

double calib::get_norma()

p_matrix p_matrix::operator*(double step_size) const
{
std::vector<double> grads = { rot_angles.alpha,rot_angles.beta, rot_angles.gamma,
trans.t1, trans.t2, trans.t3,
k_mat.fx, k_mat.fy, k_mat.ppx, k_mat.ppy };
p_matrix res;

double grads_norm = 0; // TODO meant to have as float??
for (auto i = 0; i < 12; i++)
res.vals[i] = vals[i] * step_size;

for( auto i = 0; i < grads.size(); i++ )
{
grads_norm += grads[i] * grads[i];
}
grads_norm = sqrt( grads_norm );
return res;
}

return grads_norm;
p_matrix p_matrix::operator/(double factor) const
{
return (*this)*(1.f / factor);
}

double calib::sum()
p_matrix p_matrix::operator+(const p_matrix & c) const
{
double res = 0; // TODO meant to have float??
std::vector<double> grads = { rot_angles.alpha,rot_angles.beta, rot_angles.gamma,
trans.t1, trans.t2, trans.t3,
k_mat.fx, k_mat.fy, k_mat.ppx, k_mat.ppy };
p_matrix res;
for (auto i = 0; i < 12; i++)
res.vals[i] = vals[i] + c.vals[i];

return res;
}

for( auto i = 0; i < grads.size(); i++ )
{
res += grads[i];
}
p_matrix p_matrix::operator-(const p_matrix & c) const
{
p_matrix res;
for (auto i = 0; i < 12; i++)
res.vals[i] = vals[i] - c.vals[i];

return res;
return res;
}

p_matrix p_matrix::operator*(const p_matrix & c) const
{
p_matrix res;
for (auto i = 0; i < 12; i++)
res.vals[i] = vals[i] * c.vals[i];

return res;
}

calib calib::normalize()
p_matrix p_matrix::operator/(const p_matrix & c) const
{
std::vector<double> grads = { rot_angles.alpha,rot_angles.beta, rot_angles.gamma,
trans.t1, trans.t2, trans.t3,
k_mat.fx, k_mat.fy, k_mat.ppx, k_mat.ppy };
p_matrix res;
for (auto i = 0; i < 12; i++)
res.vals[i] = c.vals[i] == 0 ? 0 : vals[i] / c.vals[i];

auto norma = get_norma();
return res;
}

std::vector<double> res_grads( grads.size() );
double p_matrix::get_norma()
{
double grads_norm = 0; // TODO meant to have as float??

for( auto i = 0; i < grads.size(); i++ )
{
res_grads[i] = grads[i] / norma;
}
for (auto i = 0; i < 12; i++)
grads_norm += vals[i] * vals[i];

calib res;
res.rot_angles = { res_grads[0], res_grads[1], res_grads[2] };
res.trans = { res_grads[3], res_grads[4], res_grads[5] };
res.k_mat = { res_grads[6], res_grads[7], res_grads[8], res_grads[9] };
grads_norm = sqrt(grads_norm);

return res;
return grads_norm;
}

double p_matrix::sum()
{
double res = 0; // TODO meant to have float??

for (auto i = 0; i <12; i++)
{
res += vals[i];
}

return res;
}

p_matrix p_matrix::normalize()
{
p_matrix res;

auto norma = get_norma();

for (auto i = 0; i < 12; i++)
{
res.vals[i] = vals[i] / norma;
}
return res;
}

calib librealsense::algo::depth_to_rgb_calibration::decompose(p_matrix mat)
{
return calib();
}
Loading

0 comments on commit 952ba82

Please sign in to comment.