void doArcball( double * _viewMatrix, double const * _rotationCenter, double const * _projectionMatrix, double const * _initialViewMatrix, //double const * _currentViewMatrix, double const * _initialMouse, double const * _currentMouse, bool screenSpaceRadius, double radius) { Eigen::Map<Eigen::Vector3d const> rotationCenter(_rotationCenter); Eigen::Map<Eigen::Matrix4d const> initialViewMatrix(_initialViewMatrix); //Eigen::Map<Eigen::Matrix4d const> currentViewMatrix(_currentViewMatrix); Eigen::Map<Eigen::Matrix4d const> projectionMatrix(_projectionMatrix); Eigen::Map<Eigen::Matrix4d> viewMatrix(_viewMatrix); Eigen::Vector2d initialMouse(_initialMouse); Eigen::Vector2d currentMouse(_currentMouse); Eigen::Quaterniond q; Eigen::Vector3d Pa, Pc; if (screenSpaceRadius) { double aspectRatio = projectionMatrix(1, 1) / projectionMatrix(0, 0); initialMouse(0) *= aspectRatio; currentMouse(0) *= aspectRatio; Pa = mapToSphere(initialMouse, radius); Pc = mapToSphere(currentMouse, radius); q.setFromTwoVectors(Pa - rotationCenter, Pc - rotationCenter); } else { Pa = mapToSphere(projectionMatrix.inverse(), initialMouse, rotationCenter, radius); Pc = mapToSphere(projectionMatrix.inverse(), currentMouse, rotationCenter, radius); Eigen::Vector3d a = Pa - rotationCenter, b = Pc - rotationCenter; #if 0 std::cout << "Mouse Initial Radius = " << sqrt(a.dot(a)) << " Current Radius = " << sqrt(b.dot(b)) << std::endl; #endif q.setFromTwoVectors(a, b); } Eigen::Matrix4d qMat4; qMat4.setIdentity(); qMat4.topLeftCorner<3, 3>() = q.toRotationMatrix(); Eigen::Matrix4d translate; translate.setIdentity(); translate.topRightCorner<3, 1>() = -rotationCenter; Eigen::Matrix4d invTranslate; invTranslate.setIdentity(); invTranslate.topRightCorner<3, 1>() = rotationCenter; viewMatrix = invTranslate * qMat4 * translate * initialViewMatrix; }
Eigen::Matrix4d compute_A_of_DH(int i, double q_abb) { Eigen::Matrix4d A; Eigen::Matrix3d R; Eigen::Vector3d p; double a = DH_a_params[i]; double d = DH_d_params[i]; double alpha = DH_alpha_params[i]; double q = q_abb + DH_q_offsets[i]; A = Eigen::Matrix4d::Identity(); R = Eigen::Matrix3d::Identity(); //ROS_INFO("compute_A_of_DH: a,d,alpha,q = %f, %f %f %f",a,d,alpha,q); double cq = cos(q); double sq = sin(q); double sa = sin(alpha); double ca = cos(alpha); R(0, 0) = cq; R(0, 1) = -sq*ca; //% - sin(q(i))*cos(alpha); R(0, 2) = sq*sa; //%sin(q(i))*sin(alpha); R(1, 0) = sq; R(1, 1) = cq*ca; //%cos(q(i))*cos(alpha); R(1, 2) = -cq*sa; //% //%R(3,1)= 0; %already done by default R(2, 1) = sa; R(2, 2) = ca; p(0) = a * cq; p(1) = a * sq; p(2) = d; A.block<3, 3>(0, 0) = R; A.col(3).head(3) = p; return A; }
void Camera::get_frame(std::vector<unsigned char>& buffer_rgb, std::vector<float>& point_cloud) { buffer_rgb.resize(3 * width * height); point_cloud.resize(3 * width * height); std::vector<float> buffer_depth(width * height); glBindFramebuffer(GL_FRAMEBUFFER, fbo[0]); glReadPixels(0, 0, width, height, GL_RGB, GL_UNSIGNED_BYTE, &buffer_rgb[0]); glReadPixels(0, 0, width, height, GL_DEPTH_COMPONENT, GL_FLOAT, &buffer_depth[0]); glBindFramebuffer(GL_FRAMEBUFFER, 0); for (int i = 0; i < width * height / 2; ++i) { const div_t coord = std::div(i, width); const int x = coord.rem, y = coord.quot, y_inv = height - 1 - y; std::swap(buffer_depth[i], buffer_depth[y_inv * width + x]); for (int j = 0; j < 3; ++j) std::swap(buffer_rgb[3 * i + j], buffer_rgb[3 * (y_inv * width + x) + j]); } int viewport[4] = {0, 0, width, height}; Eigen::Matrix4d id = Eigen::Matrix4d::Identity(); for (unsigned i = 0; i < buffer_depth.size(); ++i) { const std::div_t coords = std::div(i, width); double out[3]; gluUnProject(coords.rem, coords.quot, buffer_depth[i], id.data(), projection_matrix.data(), viewport, out + 0, out + 1, out + 2); for (int j = 0; j < 3; ++j) point_cloud[i * 3 + j] = out[j]; point_cloud[i * 3 + 2] *= -1.0f; } glBindVertexArray(vao[0]); glBindBuffer(GL_ARRAY_BUFFER, vbo[0]); glBufferData(GL_ARRAY_BUFFER, point_cloud.size() * sizeof(float), &point_cloud[0], GL_STATIC_DRAW); glVertexPointer(3, GL_FLOAT, 0, 0); glEnableClientState(GL_VERTEX_ARRAY); glBindVertexArray(0); n_points = point_cloud.size() / 3; }
Eigen::Vector3d GetCameraCenterWorld() { GLdouble projmat[16]; GLdouble modelmat[16]; glGetDoublev( GL_PROJECTION_MATRIX, projmat); glGetDoublev( GL_MODELVIEW_MATRIX, modelmat); Eigen::Matrix4d InvModelMat; Eigen::Matrix4d InvProjMat; InvModelMat.setZero(); InvProjMat.setZero(); for(int i=0; i<4; i++){ for(int j=0; j<4; j++){ InvModelMat(i,j)=modelmat[4*i+j]; InvProjMat(i,j)=projmat[4*i+j]; //printf("%lf ", InvModelMat[i][j]); } //printf("\n"); } InvModelMat=(InvModelMat*InvProjMat).transpose(); InvModelMat = (InvModelMat.inverse()); Eigen::Vector4d cc(0, 0, 0, 1); cc=InvModelMat*cc; if(cc.norm()!=0) cc=cc/cc[3]; return Eigen::Vector3d(cc[0], cc[1], cc[2]); }
double RotationAverage::chordal(Sophus::SO3d *avg) { double max_angle=0; Eigen::Matrix4d Q; Q.setZero(); auto rest = rotations.begin(); rest++; for(auto && t: zip_range(weights, rotations)) { double w=t.get<0>(); Sophus::SO3d& q = t.get<1>(); Q += w * q.unit_quaternion().coeffs() * q.unit_quaternion().coeffs().transpose(); max_angle = std::accumulate(rest, rotations.end(), max_angle, [&q](double max, const Sophus::SO3d& other) { return std::max(max, q.unit_quaternion().angularDistance(other.unit_quaternion())); }); } Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> solver; solver.compute(Q); Eigen::Vector4d::Map(avg->data()) = solver.eigenvectors().col(3); return max_angle; }
void BezierBoundarySolver::_IterateCurvatureReduction(BezierBoundaryProblem* pProblem,Eigen::Vector4d& params) { double epsilon = 0.0001; //create a jacobian for the parameters by perturbing them Eigen::Vector4d Jt; //transpose of the jacobian BezierBoundaryProblem origProblem = *pProblem; double maxK = _GetMaximumCurvature(pProblem); for(int ii = 0; ii < 4 ; ii++){ Eigen::Vector4d epsilonParams = params; epsilonParams[ii] += epsilon; _Get5thOrderBezier(pProblem,epsilonParams); _Sample5thOrderBezier(pProblem); double kPlus = _GetMaximumCurvature(pProblem); epsilonParams[ii] -= 2*epsilon; _Get5thOrderBezier(pProblem,epsilonParams); _Sample5thOrderBezier(pProblem); double kMinus = _GetMaximumCurvature(pProblem); Jt[ii] = (kPlus-kMinus)/(2*epsilon); } //now that we have Jt, we can calculate JtJ Eigen::Matrix4d JtJ = Jt*Jt.transpose(); //thikonov regularization JtJ += Eigen::Matrix4d::Identity(); Eigen::Vector4d deltaParams = JtJ.inverse() * Jt*maxK; params -= deltaParams; _Get5thOrderBezier(pProblem,params); _Sample5thOrderBezier(pProblem); //double finalMaxK = _GetMaximumCurvature(pProblem); //dout("2D Iteration took k from " << maxK << " to " << finalMaxK); }
Eigen::Matrix4d View::calcProjectionMatrix(Point2f frameBufferSize,float margin) { float near=0,far=0; Point2d frustLL,frustUR; frustLL.x() = -imagePlaneSize * (1.0 + margin); frustUR.x() = imagePlaneSize * (1.0 + margin); double ratio = ((double)frameBufferSize.y() / (double)frameBufferSize.x()); frustLL.y() = -imagePlaneSize * ratio * (1.0 + margin); frustUR.y() = imagePlaneSize * ratio * (1.0 + margin); near = nearPlane; far = farPlane; // Borrowed from the "OpenGL ES 2.0 Programming" book Eigen::Matrix4d projMat; Point3d delta(frustUR.x()-frustLL.x(),frustUR.y()-frustLL.y(),far-near); projMat.setIdentity(); projMat(0,0) = 2.0f * near / delta.x(); projMat(1,0) = projMat(2,0) = projMat(3,0) = 0.0f; projMat(1,1) = 2.0f * near / delta.y(); projMat(0,1) = projMat(2,1) = projMat(3,1) = 0.0f; projMat(0,2) = (frustUR.x()+frustLL.x()) / delta.x(); projMat(1,2) = (frustUR.y()+frustLL.y()) / delta.y(); projMat(2,2) = -(near + far ) / delta.z(); projMat(3,2) = -1.0f; projMat(2,3) = -2.0f * near * far / delta.z(); projMat(0,3) = projMat(1,3) = projMat(3,3) = 0.0f; return projMat; }
Eigen::Matrix4d compute_A_of_DH(double q, double a, double d, double alpha) { Eigen::Matrix4d A; Eigen::Matrix3d R; Eigen::Vector3d p; A = Eigen::Matrix4d::Identity(); R = Eigen::Matrix3d::Identity(); //ROS_INFO("compute_A_of_DH: a,d,alpha,q = %f, %f %f %f",a,d,alpha,q); //could correct q w/ q_offset now... double cq = cos(q); double sq = sin(q); double sa = sin(alpha); double ca = cos(alpha); R(0, 0) = cq; R(0, 1) = -sq*ca; //% - sin(q(i))*cos(alpha); R(0, 2) = sq*sa; //%sin(q(i))*sin(alpha); R(1, 0) = sq; R(1, 1) = cq*ca; //%cos(q(i))*cos(alpha); R(1, 2) = -cq*sa; //% //%R(3,1)= 0; %already done by default R(2, 1) = sa; R(2, 2) = ca; p(0) = a * cq; p(1) = a * sq; p(2) = d; A.block<3, 3>(0, 0) = R; A.col(3).head(3) = p; return A; }
//debug void Dataset_analyzer::testetransformation(std::string file_pose_s, std::string file_pose_d, std::string ply_filename_s,std::string ply_filename_d){ PCloud cloud_s; PCloud cloud_d; LoadCloudFromTXT(file_pose_s.data(), cloud_s); LoadCloudFromTXT(file_pose_d.data(), cloud_d); Eigen::Matrix4d Ts; Ts.setIdentity(); Ts.block<3,3>(0,0) = (qs*qp).toRotationMatrix (); Ts.block<3,1>(0,3) = ts; Eigen::Matrix4d T = Computetransformation(); //T.setIdentity(); //T.block<3,3>(0,0) = (qs*qp).toRotationMatrix()*(qd*qp).toRotationMatrix().transpose(); //T.block<3,1>(0,3) = td-ts; Eigen::Matrix4d Taux; Taux.setIdentity(); Taux.block<3,3>(0,0) = T.block<3,3>(0,0)*Ts.block<3,3>(0,0); Taux.block<3,1>(0,3) = Ts.block<3,1>(0,3) + T.block<3,1>(0,3); PrintCloudToPLY(ply_filename_s.data(), cloud_s, Ts); PrintCloudToPLY(ply_filename_d.data(), cloud_d, Taux); //std::cout << T<< "\n"; //std::cout << atan2(T(2,1), T(2,2)) << " " << asin(-T(2,0)) << " " << atan2(T(1,0), T(0,0)) << "\n"; }
Eigen::Matrix4d Dataset_analyzer::Computetransformation(){ Eigen::Quaterniond qs_n = qs*qp; //change axis Eigen::Quaterniond qd_n = qd*qp; //change axis /* Eigen::Matrix4d Ts; Ts.setIdentity(); Eigen::Matrix4d Td; Td.setIdentity(); Ts.block<3,3>(0,0) = qs_n.toRotationMatrix (); Ts.block<3,1>(0,3) = ts; Td.block<3,3>(0,0) = qd_n.toRotationMatrix (); Td.block<3,1>(0,3) = td; Eigen::Matrix4d T = Ts*Td.inverse(); */ Eigen::Quaterniond q12 = qd_n*qs_n.inverse(); //compute rotation s --> d Eigen::Matrix4d T; T.setIdentity(); T.block<3,3>(0,0) = q12.toRotationMatrix (); T.block<3,1>(0,3) = td-ts; return T; }
void calculateInverseTransformationMatrix(const Eigen::Matrix4d& src, Eigen::Matrix4d& dst) { dst = Eigen::Matrix4d::Identity(); Eigen::Matrix3d R_trans = src.block(0, 0, 3, 3).transpose(); dst.block(0, 0, 3, 3) = R_trans; dst.block(0, 3, 3, 1) = -R_trans * src.block(0, 3, 3, 1); }
void Manipulator::computeCabs() { if(C_abs_.size() != T_abs_.size()) { std::stringstream msg; msg << "C_abs_.size() != T_abs_.size()" << std::endl << " C_abs_.size : " << C_abs_.size() << std::endl << " T_abs_.size : " << T_abs_.size(); throw ahl_utils::Exception("Manipulator::computeCabs", msg.str()); } else if(C_abs_.size() != link_.size()) { std::stringstream msg; msg << "C_abs_.size() != link_.size()" << std::endl << " C_abs_.size : " << C_abs_.size() << std::endl << " link_.size : " << link_.size(); throw ahl_utils::Exception("Manipulator::computeCabs", msg.str()); } else if(C_abs_.size() == 0) { std::stringstream msg; msg << "C_abs_.size() == 0" << std::endl << " C_abs_.size : " << C_abs_.size(); throw ahl_utils::Exception("Manipulator::computeCabs", msg.str()); } for(uint32_t i = 0; i < C_abs_.size(); ++i) { Eigen::Matrix4d Tlc = Eigen::Matrix4d::Identity(); Tlc.block(0, 3, 3, 1) = link_[i]->C; C_abs_[i] = T_abs_[i] * Tlc; } }
void setTransform(Eigen::Matrix3d &rot, Eigen::Vector3d &t, Eigen::Matrix4d &Tf) { Tf.block(0,0,3,3) << rot; Tf.block(0,3,3,1) << t; Tf.block(3,0,1,4) << 0.0,0.0,0.0,1.0; }
void Camera::draw(const std::function<void()>& f) const { const Eigen::Matrix4d m = Eigen::Map<const Eigen::Matrix4d>(modelview_matrix.data()).inverse(); glPushMatrix(); glMultMatrixd(m.data()); glColor3fv(color.data()); f(); glPopMatrix(); }
void Dist_grad(const Eigen::Matrix<double,Eigen::Dynamic,1> &t, const std::vector<skeleton::BranchContProjSkel::Ptr> &skel, double &value, Eigen::Matrix<double,Eigen::Dynamic,1> &gradient) { Eigen::Matrix4d A = Eigen::Matrix4d::Zero(); Eigen::Vector4d b = Eigen::Vector4d::Zero(); value = 0.0; gradient = Eigen::Matrix<double,Eigen::Dynamic,1>::Zero(t.rows(),1); std::vector<Eigen::Vector4d> ori(skel.size()); std::vector<Eigen::Vector4d> vec(skel.size()); std::vector<Eigen::Vector4d> orijac(skel.size()); std::vector<Eigen::Vector4d> vecjac(skel.size()); std::vector<Eigen::Matrix4d> A_part_jac(skel.size()); std::vector<Eigen::Vector4d> b_part_jac(skel.size()); for(unsigned int i=0;i<skel.size();i++) { Compositor<Application<Eigen::Matrix<double,8,1>,Eigen::Vector3d>,Application<Eigen::Vector3d,double> > fun(skel[i]->getModel()->getR8Fun(),skel[i]->getCompFun()); Eigen::Matrix<double,8,1> veclin = fun(t(i)); Eigen::Matrix<double,8,1> jaclin = fun.jac(t(i)); ori[i] = veclin.block<4,1>(0,0); vec[i] = veclin.block<4,1>(4,0).normalized(); orijac[i] = jaclin.block<4,1>(0,0); vecjac[i] = jaclin.block<4,1>(4,0)*(1.0/veclin.block<4,1>(4,0).norm()); Eigen::Matrix4d A_part = Eigen::Matrix4d::Identity() - vec[i]*vec[i].transpose(); Eigen::Vector4d b_part = A_part*ori[i]; A_part_jac[i] = - vecjac[i]*vec[i].transpose() - vec[i]*vecjac[i].transpose(); b_part_jac[i] = A_part_jac[i] * ori[i] + A_part * orijac[i]; A+=A_part; b+=b_part; } Eigen::Matrix4d A_inv = A.inverse(); Eigen::Vector4d P = A_inv*b; for(unsigned int i=0;i<skel.size();i++) { Eigen::Vector4d P_jac = A_inv * A_part_jac[i] * P + A_inv * b_part_jac[i]; double Dist = (P - ori[i]).squaredNorm() - pow((P - ori[i]).dot(vec[i]),2); gradient(i) = 2*(P_jac - orijac[i]).dot(P - ori[i]) -2*(P_jac - orijac[i]).dot(vec[i]) * (P - ori[i]).dot(vec[i]) -2*(P - ori[i]).dot(vecjac[i]) * (P - ori[i]).dot(vec[i]); value += Dist; } }
TEST(TransformationTestSuite, testConstructor) { using namespace sm::kinematics; Transformation T_a_b; Eigen::Matrix4d T; T.setIdentity(); sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity"); }
Plane Plane::transform(const Eigen::Affine3d& transform) { Eigen::Vector4d n; n[0] = normal_[0]; n[1] = normal_[1]; n[2] = normal_[2]; n[3] = d_; Eigen::Matrix4d m = transform.matrix(); Eigen::Vector4d n_d = m.transpose() * n; Eigen::Vector4d n_dd = n_d.normalized(); return Plane(Eigen::Vector3d(n_dd[0], n_dd[1], n_dd[2]), n_dd[3]); }
Eigen::Matrix4d recenter_transform3d(const Eigen::Matrix4d &transfo, const Eigen::Vector3d& center) { //% remove former translation part Eigen::Matrix4d res = Eigen::Matrix4d::Identity(); res.block(0, 0, 3, 3) = transfo.block(0, 0, 3, 3); //% create translations Eigen::Matrix4d t1 = create_translation3d(-center); Eigen::Matrix4d t2 = create_translation3d(center); //% compute translated transform res = t2*res*t1; return res; }
Eigen::Matrix4d VertexPointXYZCov::covTransform(){ EigenSolver<Matrix3d> solv(_cov); Matrix3d eigenVectors = solv.eigenvectors().real(); Eigen::Matrix4d covGlTransform; covGlTransform.setIdentity(); for(int i=0; i < 3; ++i) for(int j=0; j < 3; ++j) covGlTransform(i,j) = eigenVectors(i,j); for(int i=0; i < 3; ++i) covGlTransform(i,3) = estimate()(i); return covGlTransform; }
Eigen::Matrix4d createHomogeneousTransformMatrix(tf::StampedTransform transform) { tf::Point p = transform.getOrigin(); tf::Quaternion q = transform.getRotation(); tf::Matrix3x3 R1(q); Eigen::Matrix3d R2; tf::matrixTFToEigen(R1, R2); ROS_INFO_STREAM("R2:\n"<<R2); Eigen::Matrix4d T; T.block(0, 0, 3, 3) << R2; T.block(0, 3, 3, 1) << p.x(), p.y(), p.z(); T.row(3) << 0, 0, 0, 1; return T; }
TEST(UncertainTransformationTestSuite, testConstructor) { using namespace sm::kinematics; UncertainTransformation T_a_b; Eigen::Matrix4d T; T.setIdentity(); UncertainTransformation::covariance_t U; U.setZero(); sm::eigen::assertNear(T, T_a_b.T(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating identity"); sm::eigen::assertNear(U, T_a_b.U(), 1e-10, SM_SOURCE_FILE_POS, "Checking for the default constructor creating zero uncertainty"); }
bool ViewControl::ConvertToPinholeCameraParameters( PinholeCameraIntrinsic &intrinsic, Eigen::Matrix4d &extrinsic) { if (window_height_ <= 0 || window_width_ <= 0) { PrintWarning("[ViewControl] ConvertToPinholeCameraParameters() failed because window height and width are not set.\n"); return false; } if (GetProjectionType() == ProjectionType::Orthogonal) { PrintWarning("[ViewControl] ConvertToPinholeCameraParameters() failed because orthogonal view cannot be translated to a pinhole camera.\n"); return false; } SetProjectionParameters(); intrinsic.width_ = window_width_; intrinsic.height_ = window_height_; intrinsic.intrinsic_matrix_.setIdentity(); double fov_rad = field_of_view_ / 180.0 * M_PI; double tan_half_fov = std::tan(fov_rad / 2.0); intrinsic.intrinsic_matrix_(0, 0) = intrinsic.intrinsic_matrix_(1, 1) = (double)window_height_ / tan_half_fov / 2.0; intrinsic.intrinsic_matrix_(0, 2) = (double)window_width_ / 2.0 - 0.5; intrinsic.intrinsic_matrix_(1, 2) = (double)window_height_ / 2.0 - 0.5; extrinsic.setZero(); Eigen::Vector3d front_dir = front_.normalized(); Eigen::Vector3d up_dir = up_.normalized(); Eigen::Vector3d right_dir = right_.normalized(); extrinsic.block<1, 3>(0, 0) = right_dir.transpose(); extrinsic.block<1, 3>(1, 0) = -up_dir.transpose(); extrinsic.block<1, 3>(2, 0) = -front_dir.transpose(); extrinsic(0, 3) = -right_dir.dot(eye_); extrinsic(1, 3) = up_dir.dot(eye_); extrinsic(2, 3) = front_dir.dot(eye_); extrinsic(3, 3) = 1.0; return true; }
void pcl::visualization::getViewFrustum (const Eigen::Matrix4d &view_projection_matrix, double planes[24]) { // Set up the normals Eigen::Vector4d normals[6]; for (int i=0; i < 6; i++) { normals[i] = Eigen::Vector4d (0.0, 0.0, 0.0, 1.0); // if i is even set to -1, if odd set to +1 normals[i] (i/2) = 1 - (i%2)*2; } // Transpose the matrix for use with normals Eigen::Matrix4d view_matrix = view_projection_matrix.transpose (); // Transform the normals to world coordinates for (int i=0; i < 6; i++) { normals[i] = view_matrix * normals[i]; double f = 1.0/sqrt (normals[i].x () * normals[i].x () + normals[i].y () * normals[i].y () + normals[i].z () * normals[i].z ()); planes[4*i + 0] = normals[i].x ()*f; planes[4*i + 1] = normals[i].y ()*f; planes[4*i + 2] = normals[i].z ()*f; planes[4*i + 3] = normals[i].w ()*f; } }
int GraphOptimizer_G2O::addVertex(Eigen::Matrix4d& vertexPose, int id, bool isFixed) { g2o::Vector3d t(vertexPose(0,3),vertexPose(1,3),vertexPose(2,3)); Eigen::Matrix3d rot = vertexPose.block(0,0,3,3); g2o::Quaterniond q(rot); q.normalize(); // set up node g2o::VertexSE3 *vc = new g2o::VertexSE3(); Eigen::Isometry3d cam; // camera pose cam = q; cam.translation() = t; vc->setEstimate(cam); vc->setId(id); // vertex id //set pose fixed if (isFixed) { vc->setFixed(true); } // add to optimizer optimizer.addVertex(vc); vertexIdVec.push_back(id); return id; }
geometry_msgs::Pose Sensors::robot2sensorTransformation(geometry_msgs::Pose pose) { Eigen::Matrix4d robotPoseMat, robot2sensorMat, sensorPoseMat; //Robot matrix pose Eigen::Matrix3d R; Eigen::Vector3d T1(pose.position.x,pose.position.y,pose.position.z); tf::Quaternion qt(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w); tf::Matrix3x3 R1(qt); tf::matrixTFToEigen(R1,R); robotPoseMat.setZero (); robotPoseMat.block (0, 0, 3, 3) = R; robotPoseMat.block (0, 3, 3, 1) = T1; robotPoseMat (3, 3) = 1; //transformation matrix qt = tf::createQuaternionFromRPY(sensorRPY[0],sensorRPY[1],sensorRPY[2]); tf::Matrix3x3 R2(qt);Eigen::Vector3d T2(sensorPose[0], sensorPose[1], sensorPose[2]); tf::matrixTFToEigen(R2,R); robot2sensorMat.setZero (); robot2sensorMat.block (0, 0, 3, 3) = R; robot2sensorMat.block (0, 3, 3, 1) = T2; robot2sensorMat (3, 3) = 1; //preform the transformation sensorPoseMat = robotPoseMat * robot2sensorMat; Eigen::Matrix4d sensor2sensorMat; //the frustum culling sensor needs this //the transofrmation is rotation by +90 around x axis of the sensor sensor2sensorMat << 1, 0, 0, 0, 0, 0,-1, 0, 0, 1, 0, 0, 0, 0, 0, 1; Eigen::Matrix4d newSensorPoseMat = sensorPoseMat * sensor2sensorMat; geometry_msgs::Pose p; Eigen::Vector3d T3;Eigen::Matrix3d Rd; tf::Matrix3x3 R3; Rd = newSensorPoseMat.block (0, 0, 3, 3); tf::matrixEigenToTF(Rd,R3); T3 = newSensorPoseMat.block (0, 3, 3, 1); p.position.x=T3[0];p.position.y=T3[1];p.position.z=T3[2]; R3.getRotation(qt); p.orientation.x = qt.getX(); p.orientation.y = qt.getY();p.orientation.z = qt.getZ();p.orientation.w = qt.getW(); return p; }
void xyzrpyToTransformationMatrix(const Eigen::Vector3d& xyz, const Eigen::Vector3d& rpy, Eigen::Matrix4d& T) { T = Eigen::Matrix4d::Identity(); Eigen::Matrix3d R; rpyToRotationMatrix(rpy, R); T.block(0, 0, 3, 3) = R; T.block(0, 3, 3, 1) = xyz; }
void GLWidgetShader::paintGL() { if (nVertices!=0) { Eigen::Matrix4d MVP; MVP << -0.0709951 , 0.0997458 , -0.152342 , -0.15187, -0.0744123 ,-0.00746067, 0.0552333 , -2.31223, 0.263665 , 0.276252 , 0.924317 , -17.4836, -0.263639, -0.276224 , -0.924224 , 17.6819; glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glLoadIdentity(); glLoadMatrixd(MVP.data()); QVector3D norm(parameters->mirrorPlane.normal().x(), parameters->mirrorPlane.normal().y(),parameters->mirrorPlane.normal().z()); QVector3D projCenter(parameters->projCenter.x(),parameters->projCenter.y(),parameters->projCenter.z()); QVector3D reflectedProjector(parameters->reflectedProjector.x(),parameters->reflectedProjector.y(),parameters->reflectedProjector.z()); QMatrix4x4 qm; for (int i=0; i<4;i++) { for (int j=0; j<4;j++) { qm(i,j) = this->m(i,j); } } this->beginShader(); glPointSize(0.1); glEnableClientState(GL_VERTEX_ARRAY); shaderProgram->setUniformValue("viewerDistance",(GLfloat)parameters->viewerDistance); shaderProgram->setUniformValue("viewerHeight",(GLfloat)parameters->viewerHeight); shaderProgram->setUniformValue( "mirrorNormal",norm ); shaderProgram->setUniformValue("projectorModelViewProjectionMatrix",qm); //shaderProgram->setUniformValue("ProjectorCenter", projCenter); shaderProgram->setUniformValue("ReflectedProjector",reflectedProjector); shaderProgram->setUniformValue("mirrorCenter",QVector3D(parameters->mirrorCenter.x(),parameters->mirrorCenter.y(),parameters->mirrorCenter.z())); glVertexPointer(3,GL_FLOAT, 0, this->verticesPtr ); glDrawArrays(GL_POINTS, 0, this->nVertices); if (!this->shaderProgram->log().isEmpty()) qDebug() << "Shader log " << this->shaderProgram->log(); this->endShader(); glDisableClientState(GL_VERTEX_ARRAY); } }
Eigen::Vector3d GetPointEyeToWorld(const Eigen::Ref<const Eigen::Vector3d>& _pt) { GLdouble modelmat[16]; glGetDoublev( GL_MODELVIEW_MATRIX, modelmat); Eigen::Matrix4d InvModelMat; InvModelMat.setZero(); for(int i=0; i<4; i++){ for(int j=0; j<4; j++){ InvModelMat(i,j)=modelmat[4*i+j]; } } InvModelMat=(InvModelMat.transpose()); InvModelMat=(InvModelMat.inverse()); Eigen::Vector4d cc( _pt(0),_pt(1), _pt(2),1); cc=InvModelMat*cc; if(cc.norm()!=0) cc=cc/cc[3]; return Eigen::Vector3d(cc[0], cc[1], cc[2]); }
TEST(ForwardKinematicsTest, computeHandTransform) { const double epsilon = 1e-5; const std::string packagePath = ros::package::getPath("forward_kinematics"); if (packagePath.empty()) { FAIL() << "Error: Could not find package forward_kinematics. Make sure that you call source/devel/setup.bash first."; } ForwardKinematics fk; FileIO fio(packagePath + "/data/joints.txt"); fio.results.reserve(fio.jointAngles.size()); for (size_t i = 0; i < fio.jointAngles.size(); ++i) { Eigen::Matrix4d result = fk.computeHandTransform(&fio.jointAngles[i][0]); Eigen::Vector3d translation = result.topRightCorner(3, 1); for (size_t j = 0; j < 3; ++j) { ASSERT_NEAR((double) fio.groundTruth[i][j], (double) translation(j), epsilon); } } }
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT ) { //ds A matrix for system: A*X=0 Eigen::Matrix4d matAHomogeneous; matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0); matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1); matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0); matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1); //ds solve system subject to ||A*x||=0 ||x||=1 const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV ); //ds solution x is the last column of V const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) ); return vecX.head( 3 )/vecX(3); }