C++ eigen::Matrix4d类代码示例

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