hMatrix Inverse_Kinematics(hMatrix Initial_T,hMatrix Goal_T,double *Initial_t, double *DH_alpha, double *DH_a, double *DH_d, int joint){
for(int i=0; i<joint; i++){
Initial_theta[i] = *Initial_t;
Initial_t++;
}
hMatrix Initial_Theta(7,1);
hMatrix J(6,7), Pinv_J(7,6);
hMatrix n_a(3,1),s_a(3,1),a_a(3,1),n_t(3,1),s_t(3,1),a_t(3,1),p_del(3,1);
double x,y,z,rx,ry,rz;
double error_position[3]= {Goal_T.element(0,3)-Initial_T.element(0,3),Goal_T.element(1,3)-Initial_T.element(1,3),Goal_T.element(2,3)-Initial_T.element(2,3)};
hMatrix P(3,1),R(3,1),Rotation(3,3),dx_temp1(3,1),dx_temp2(3,1),dX(6,1),del_Theta(7,1),Temp(7,1);
Initial_Theta.SET(7,1,Initial_theta);
Initial_T = T_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0], joint);
J = Jacobian_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0]);
Pinv_J = Pseudo_Inverse(J);
for(int i = 0; i<3; i++){
n_a.SetElement(i,0,Initial_T.element(i,0));
s_a.SetElement(i,0,Initial_T.element(i,1));
a_a.SetElement(i,0,Initial_T.element(i,2));
n_t.SetElement(i,0,Goal_T.element(i,0));
s_t.SetElement(i,0,Goal_T.element(i,1));
a_t.SetElement(i,0,Goal_T.element(i,2));
p_del.SetElement(i,0,Goal_T.element(i,3)-Initial_T.element(i,3));
}
x = dot(n_a, p_del);
y = dot(s_a, p_del);
z = dot(a_a, p_del); ;
rx = (dot(a_a,s_t)-dot(a_t,s_a))/2;
ry = (dot(n_a,a_t)-dot(n_t,a_a))/2;
rz = (dot(s_a,n_t)-dot(s_t,n_a))/2;
double dx_P[3] = {x,y,z},dx_R[3] = {rx,ry,rz};
P.SET(3,1,&dx_P[0]);
R.SET(3,1,&dx_R[0]);
Rotation = T_Rotation(Initial_T);
dx_temp1 = Rotation*P;
dx_temp2 = Rotation*R;
for(int i =0; i<3; i++){
dX.SetElement(i,0,dx_temp1.element(i,0));
dX.SetElement(i+3,0,dx_temp2.element(i,0));
}
del_Theta = Pinv_J*dX;
for(int i=0; i<joint; i++)
Temp.SetElement(i,0,Initial_Theta.element(i,0) + del_Theta.element(i,0));
Initial_Theta = Temp;
return Initial_Theta;
}
bool yee_compare(CompareArgs &args)
{
if ((args.image_a_->get_width() != args.image_b_->get_width()) or
(args.image_a_->get_height() != args.image_b_->get_height()))
{
args.error_string_ = "Image dimensions do not match\n";
return false;
}
const auto w = args.image_a_->get_width();
const auto h = args.image_a_->get_height();
const auto dim = w * h;
auto identical = true;
for (auto i = 0u; i < dim; i++)
{
if (args.image_a_->get(i) != args.image_b_->get(i))
{
identical = false;
break;
}
}
if (identical)
{
args.error_string_ = "Images are binary identical\n";
return true;
}
// Assuming colorspaces are in Adobe RGB (1998) convert to XYZ.
std::vector<float> a_lum(dim);
std::vector<float> b_lum(dim);
std::vector<float> a_a(dim);
std::vector<float> b_a(dim);
std::vector<float> a_b(dim);
std::vector<float> b_b(dim);
if (args.verbose_)
{
std::cout << "Converting RGB to XYZ\n";
}
const auto gamma = args.gamma_;
const auto luminance = args.luminance_;
#pragma omp parallel for shared(args, a_lum, b_lum, a_a, a_b, b_a, b_b)
for (auto y = 0; y < static_cast<ptrdiff_t>(h); y++)
{
for (auto x = 0u; x < w; x++)
{
const auto i = x + y * w;
const auto a_color_r = powf(args.image_a_->get_red(i) / 255.0f,
gamma);
const auto a_color_g = powf(args.image_a_->get_green(i) / 255.0f,
gamma);
const auto a_color_b = powf(args.image_a_->get_blue(i) / 255.0f,
gamma);
float a_x;
float a_y;
float a_z;
adobe_rgb_to_xyz(a_color_r, a_color_g, a_color_b, a_x, a_y, a_z);
float l;
xyz_to_lab(a_x, a_y, a_z, l, a_a[i], a_b[i]);
const auto b_color_r = powf(args.image_b_->get_red(i) / 255.0f,
gamma);
const auto b_color_g = powf(args.image_b_->get_green(i) / 255.0f,
gamma);
const auto b_color_b = powf(args.image_b_->get_blue(i) / 255.0f,
gamma);
float b_x;
float b_y;
float b_z;
adobe_rgb_to_xyz(b_color_r, b_color_g, b_color_b, b_x, b_y, b_z);
xyz_to_lab(b_x, b_y, b_z, l, b_a[i], b_b[i]);
a_lum[i] = a_y * luminance;
b_lum[i] = b_y * luminance;
}
}
if (args.verbose_)
{
std::cout << "Constructing Laplacian Pyramids\n";
}
const LPyramid la(a_lum, w, h);
const LPyramid lb(b_lum, w, h);
const auto num_one_degree_pixels =
to_degrees(2 *
std::tan(args.field_of_view_ * to_radians(.5f)));
const auto pixels_per_degree = w / num_one_degree_pixels;
if (args.verbose_)
{
std::cout << "Performing test\n";
}
const auto adaptation_level = adaptation(num_one_degree_pixels);
float cpd[MAX_PYR_LEVELS];
cpd[0] = 0.5f * pixels_per_degree;
for (auto i = 1u; i < MAX_PYR_LEVELS; i++)
{
cpd[i] = 0.5f * cpd[i - 1];
}
const auto csf_max = csf(3.248f, 100.0f);
static_assert(MAX_PYR_LEVELS > 2,
"MAX_PYR_LEVELS must be greater than 2");
float f_freq[MAX_PYR_LEVELS - 2];
for (auto i = 0u; i < MAX_PYR_LEVELS - 2; i++)
{
f_freq[i] = csf_max / csf(cpd[i], 100.0f);
}
auto pixels_failed = 0u;
auto error_sum = 0.;
#pragma omp parallel for reduction(+ : pixels_failed, error_sum) \
shared(args, a_a, a_b, b_a, b_b, cpd, f_freq)
for (auto y = 0; y < static_cast<ptrdiff_t>(h); y++)
{
for (auto x = 0u; x < w; x++)
{
const auto index = y * w + x;
const auto adapt = std::max((la.get_value(x, y, adaptation_level) +
lb.get_value(x, y, adaptation_level)) * 0.5f,
1e-5f);
auto sum_contrast = 0.f;
auto factor = 0.f;
for (auto i = 0u; i < MAX_PYR_LEVELS - 2; i++)
{
const auto n1 =
fabsf(la.get_value(x, y, i) - la.get_value(x, y, i + 1));
const auto n2 =
fabsf(lb.get_value(x, y, i) - lb.get_value(x, y, i + 1));
const auto numerator = std::max(n1, n2);
const auto d1 = fabsf(la.get_value(x, y, i + 2));
const auto d2 = fabsf(lb.get_value(x, y, i + 2));
const auto denominator = std::max(std::max(d1, d2), 1e-5f);
const auto contrast = numerator / denominator;
const auto f_mask = mask(contrast * csf(cpd[i], adapt));
factor += contrast * f_freq[i] * f_mask;
sum_contrast += contrast;
}
sum_contrast = std::max(sum_contrast, 1e-5f);
factor /= sum_contrast;
factor = std::min(std::max(factor, 1.f), 10.f);
const auto delta =
fabsf(la.get_value(x, y, 0) - lb.get_value(x, y, 0));
error_sum += delta;
auto pass = true;
// pure luminance test
if (delta > factor * tvi(adapt))
{
pass = false;
}
if (not args.luminance_only_)
{
// CIE delta E test with modifications
auto color_scale = args.color_factor_;
// ramp down the color test in scotopic regions
if (adapt < 10.0f)
{
// Don't do color test at all.
color_scale = 0.0;
}
const auto da = a_a[index] - b_a[index];
const auto db = a_b[index] - b_b[index];
const auto delta_e = (da * da + db * db) * color_scale;
error_sum += delta_e;
if (delta_e > factor)
{
pass = false;
}
}
if (not pass)
{
pixels_failed++;
if (args.image_difference_)
{
args.image_difference_->set(255, 0, 0, 255, index);
}
}
else
{
if (args.image_difference_)
{
args.image_difference_->set(0, 0, 0, 255, index);
}
}
}
}
const auto error_sum_buff =
std::to_string(error_sum) + " error sum\n";
const auto different =
std::to_string(pixels_failed) + " pixels are different\n";
// Always output image difference if requested.
if (args.image_difference_)
{
args.image_difference_->write_to_file(args.image_difference_->get_name());
args.error_string_ += "Wrote difference image to ";
args.error_string_ += args.image_difference_->get_name();
args.error_string_ += "\n";
}
if (pixels_failed < args.threshold_pixels_)
{
args.error_string_ = "Images are perceptually indistinguishable\n";
args.error_string_ += different;
return true;
}
args.error_string_ = "Images are visibly different\n";
args.error_string_ += different;
if (args.sum_errors_)
{
args.error_string_ += error_sum_buff;
}
return false;
}
double PlanOperation::calcA(double totalTblSizeIn100k) { return a_a() * totalTblSizeIn100k + a_b(); }
