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);
}