GrayScaleImage GrayScaleImage::ySpatialDiff() { Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic> yDiff(image.rows(),image.cols()); for(int i=0; i<image.rows(); i++){ for(int j=0; j<image.cols();j++){ yDiff(i,j) = 0; for(int ix=0; ix<yDiffOperator.cols();ix++){ for(int iy=0; iy<yDiffOperator.rows(); iy++){ yDiff(i,j) += yDiffOperator(iy,ix)*this->at_flatBC(i-1+iy, j-1+ix)/4; } } } } return GrayScaleImage(yDiff); }
double Point::distance(const Point& right) const{ double dx = xDiff(right); double dy = yDiff(right); return sqrt((dx * dx) + (dy * dy)); }
void quadSYSspline::trajectoryPlanning(double x0, double y0, double z0){ double xtf = 20; double ytf = 5; double ztf = 10; double tf = 20; //X-axis trajectory planning xa3to5(0,0) = 3; xa3to5(1,0) = 6; xa3to5(2,0) = 1; xa3to5(0,1) = 4*tf; xa3to5(1,1) = 12*tf; xa3to5(2,1) = tf; xa3to5(0,2) = 5*tf*tf; xa3to5(1,2) = 20*tf*tf; xa3to5(2,2) = tf*tf; Eigen::Vector3f xDiff(0,0,(xtf-x0)/(tf*tf*tf)); //y-axis trajectory planning ya3to5(0,0) = 3; ya3to5(1,0) = 6; ya3to5(2,0) = 1; ya3to5(0,1) = 4*tf; ya3to5(1,1) = 12*tf; ya3to5(2,1) = tf; ya3to5(0,2) = 5*tf*tf; ya3to5(1,2) = 20*tf*tf; ya3to5(2,2) = tf*tf; Eigen::Vector3f yDiff(0,0,(ytf-y0)/(tf*tf*tf)); //z-axis trajectory planning za3to5(0,0) = 3; za3to5(1,0) = 6; za3to5(2,0) = 1; za3to5(0,1) = 4*tf; za3to5(1,1) = 12*tf; za3to5(2,1) = tf; za3to5(0,2) = 5*tf*tf; za3to5(1,2) = 20*tf*tf; za3to5(2,2) = tf*tf; Eigen::Vector3f zDiff(0,0,(ztf-z0)/(tf*tf*tf)); Eigen::Vector3f xa3to5Vec = xa3to5.inverse()*xDiff; Eigen::Vector3f ya3to5Vec = ya3to5.inverse()*yDiff; Eigen::Vector3f za3to5Vec = za3to5.inverse()*zDiff; float xD = x0 + xa3to5Vec.transpose()*Eigen::Vector3f(tf*tf*tf,tf*tf*tf*tf,tf*tf*tf*tf*tf); float yD = y0 + ya3to5Vec.transpose()*Eigen::Vector3f(tf*tf*tf,tf*tf*tf*tf,tf*tf*tf*tf*tf); float zD = z0 + za3to5Vec.transpose()*Eigen::Vector3f(tf*tf*tf,tf*tf*tf*tf,tf*tf*tf*tf*tf); Eigen::Vector3f zetaD(xD,yD,zD); Eigen::Matrix3f zetaD1_term; zetaD1_term(0,0) = 3*xa3to5Vec(0); zetaD1_term(0,1) = 4*xa3to5Vec(1); zetaD1_term(0,2) = 5*xa3to5Vec(2); zetaD1_term(1,0) = 3*ya3to5Vec(0); zetaD1_term(1,1) = 3*ya3to5Vec(1); zetaD1_term(1,2) = 3*ya3to5Vec(2); zetaD1_term(2,0) = 3*za3to5Vec(0); zetaD1_term(2,1) = 3*za3to5Vec(1); zetaD1_term(2,2) = 3*za3to5Vec(2); Eigen::Vector3f zetaD1_term_2(tf*tf,tf*tf*tf,tf*tf*tf); zetaD1 = zetaD1_term * zetaD1_term_2; Eigen::Matrix3f zetaD2_term; zetaD2_term(0,0) = 3*xa3to5Vec(0); zetaD2_term(0,1) = 4*xa3to5Vec(1); zetaD2_term(0,2) = 5*xa3to5Vec(2); zetaD2_term(1,0) = 3*ya3to5Vec(0); zetaD2_term(1,1) = 3*ya3to5Vec(1); zetaD2_term(1,2) = 3*ya3to5Vec(2); zetaD2_term(2,0) = 3*za3to5Vec(0); zetaD2_term(2,1) = 3*za3to5Vec(1); zetaD2_term(2,2) = 3*za3to5Vec(2); Eigen::Vector3f zetaD2_term_2(tf*tf,tf*tf*tf,tf*tf*tf); zetaD2 = zetaD2_term * zetaD2_term_2; }