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

//==============================================================================
void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform)
{
  if(transform.matrix() == FixedFrame::mAspectProperties.mRelativeTf.matrix())
    return;

  const Eigen::Isometry3d oldTransform = getRelativeTransform();

  FixedFrame::setRelativeTransform(transform);
  dirtyJacobian();
  dirtyJacobianDeriv();

  mRelativeTransformUpdatedSignal.raise(
        this, oldTransform, getRelativeTransform());
}

int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
                  const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
                  std::vector<Contact>* result)
{
  dVector3 halfSize0;
  dVector3 halfSize1;

  convVector(0.5 * size0, halfSize0);
  convVector(0.5 * size1, halfSize1);

  dMatrix3 R0, R1;

  convMatrix(T0, R0);
  convMatrix(T1, R1);

  dVector3 p0;
  dVector3 p1;

  convVector(T0.translation(), p0);
  convVector(T1.translation(), p1);

  return dBoxBox(p1, R1, halfSize1, p0, R0, halfSize0, *result);
}

const double CMiniVisionToolbox::getTransformationDelta( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
    //ds compute pose change
    const Eigen::Isometry3d matTransformChange( p_matTransformationTo*p_matTransformationFrom.inverse( ) );

    //ds check point
    const Eigen::Vector4d vecSamplePoint( 40.2, -1.25, 2.5, 1.0 );
    double dNorm( vecSamplePoint.norm( ) );
    const Eigen::Vector4d vecDifference( vecSamplePoint-matTransformChange*vecSamplePoint );
    dNorm = ( dNorm + vecDifference.norm( ) )/2;

    //ds return norm
    return ( std::fabs( vecDifference(0) ) + std::fabs( vecDifference(1) ) + std::fabs( vecDifference(2) ) )/dNorm;
}

void VoEstimator::publishPose(int64_t utime, std::string channel, Eigen::Isometry3d pose,
  Eigen::Vector3d vel_lin, Eigen::Vector3d vel_ang){
  Eigen::Quaterniond r(pose.rotation());
  bot_core::pose_t pose_msg;
  pose_msg.utime =   utime;
  pose_msg.pos[0] = pose.translation().x();
  pose_msg.pos[1] = pose.translation().y();
  pose_msg.pos[2] = pose.translation().z();
  pose_msg.orientation[0] =  r.w();
  pose_msg.orientation[1] =  r.x();
  pose_msg.orientation[2] =  r.y();
  pose_msg.orientation[3] =  r.z();
  pose_msg.vel[0] = vel_lin(0);
  pose_msg.vel[1] = vel_lin(1);
  pose_msg.vel[2] = vel_lin(2);
  pose_msg.rotation_rate[0] = vel_ang(0);
  pose_msg.rotation_rate[1] = vel_ang(1);
  pose_msg.rotation_rate[2] = vel_ang(2);
  pose_msg.accel[0]=0; // not estimated or filled in
  pose_msg.accel[1]=0;
  pose_msg.accel[2]=0;
  lcm_->publish( channel, &pose_msg);
}

bool pronto_vis::interpolateScan(const std::vector<float>& iRanges,
                const double iTheta0, const double iThetaStep,
                const Eigen::Isometry3d& iPose0,
                const Eigen::Isometry3d& iPose1,
                std::vector<Eigen::Vector3f>& oPoints) {
  const int n = iRanges.size();
  if (n < 2) return false;
  const double tStep = 1.0/(n-1);
  Eigen::Quaterniond q0(iPose0.linear());
  Eigen::Quaterniond q1(iPose1.linear());
  Eigen::Vector3d pos0(iPose0.translation());
  Eigen::Vector3d pos1(iPose1.translation());
  oPoints.resize(n);
  double t = 0;
  double theta = iTheta0;
  for (int i = 0; i < n; ++i, t += tStep, theta += iThetaStep) {
    Eigen::Quaterniond q = q0.slerp(t,q1);
    Eigen::Vector3d pos = (1-t)*pos0 + t*pos1;
    Eigen::Vector3d pt = iRanges[i]*Eigen::Vector3d(cos(theta), sin(theta), 0);
    oPoints[i] = (q*pt + pos).cast<float>();
  }
  return true;
}

Mapper::PointCloud::Ptr Mapper::generatePointCloud(const RGBDFrame::Ptr &frame)
{
    PointCloud::Ptr tmp( new PointCloud() );
    if ( frame->pointcloud == nullptr )
    {
        // point cloud is null ptr
        frame->pointcloud = boost::make_shared<PointCloud>();
#pragma omp parallel for
        for ( int m=0; m<frame->depth.rows; m+=3 )
        {
            for ( int n=0; n<frame->depth.cols; n+=3 )
            {
                ushort d = frame->depth.ptr<ushort>(m)[n];
                if (d == 0)
                    continue;
                if (d > max_distance * frame->camera.scale)
                    continue;
                PointT p;
                cv::Point3f p_cv = frame->project2dTo3d(n, m);
                p.b = frame->rgb.ptr<uchar>(m)[n*3];
                p.g = frame->rgb.ptr<uchar>(m)[n*3+1];
                p.r = frame->rgb.ptr<uchar>(m)[n*3+2];

                p.x = p_cv.x;
                p.y = p_cv.y;
                p.z = p_cv.z;

                frame->pointcloud->points.push_back( p );
            }
        }
    }

    Eigen::Isometry3d T = frame->getTransform().inverse();
    pcl::transformPointCloud( *frame->pointcloud, *tmp, T.matrix());
    tmp->is_dense = false;
    return tmp;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量 顶点(姿态) 是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N  高斯牛顿
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M                 
    g2o::SparseOptimizer optimizer;  
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    // 添加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();//位姿
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );//旋转矩阵 和 平移向量
    pose->setId ( 0 );//id
    optimizer.addVertex ( pose );//添加顶点

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
            m.pos_world,//3D 位置
            K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray//相机内参数   灰度图
        );
        edge->setVertex ( 0, pose );//顶点
        edge->setMeasurement ( m.grayscale );//测量值为真是灰度值
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//误差 权重 信息矩阵
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"边的数量 edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();//优化初始化
    optimizer.optimize ( 30 );//最大优化次数
    Tcw = pose->estimate();// 变换矩阵
}

void compute_velocity(const Eigen::Vector3d& v_parent,
                      const Eigen::Vector3d& w_parent,
                      const Eigen::Vector3d& v_rel,
                      const Eigen::Vector3d& w_rel,
                      const Eigen::Vector3d& offset,
                      const Eigen::Isometry3d& tf_parent,
                      Eigen::Vector3d& v_child,
                      Eigen::Vector3d& w_child)
{
  const Eigen::Matrix3d& R = tf_parent.rotation();

  v_child = v_parent + R*v_rel + w_parent.cross(R*offset);

  w_child = w_parent + R*w_rel;
}

/**
 * @function transf2Params
 * @brief Fill par arguments for rot and trans from a _Tf
 */
void transf2Params( const Eigen::Isometry3d &_Tf,
		    SQ_params &_par ) {

    _par.px = _Tf.translation().x();
    _par.py = _Tf.translation().y();
    _par.pz = _Tf.translation().z();

    double r31, r11, r21, r32, r33;
    double r, p, y;

    r31 = _Tf.linear()(2,0);
    r11 = _Tf.linear()(0,0);
    r21 = _Tf.linear()(1,0);
    r32 = _Tf.linear()(2,1);
    r33 = _Tf.linear()(2,2);

    p = atan2( -r31, sqrt(r11*r11 + r21*r21) );
    y = atan2( r21 / cos(p), r11 / cos(p) );
    r = atan2( r32 / cos(p), r33 / cos(p) );

    _par.ra = r;
    _par.pa = p;
    _par.ya = y;
}

//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  const double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  const Eigen::Vector3d xAxis = pelvisXAxis.normalized();

  // Z-axis
  const Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}

int collideBoxBox(CollisionObject* o1, CollisionObject* o2,
                  const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
                  const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
                  CollisionResult& result)
{
  dVector3 halfSize0;
  dVector3 halfSize1;

  convVector(0.5 * size0, halfSize0);
  convVector(0.5 * size1, halfSize1);

  dMatrix3 R0, R1;

  convMatrix(T0, R0);
  convMatrix(T1, R1);

  dVector3 p0;
  dVector3 p1;

  convVector(T0.translation(), p0);
  convVector(T1.translation(), p1);

  return dBoxBox(o1, o2, p1, R1, halfSize1, p0, R0, halfSize0, result);
}

void FeatureTransformationEstimator::estimatePoseSVD(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d &T)
{
    pcl::TransformationFromCorrespondences tfc;
    for (int i = 0; i < P.cols(); i++) {
        Eigen::Vector3f p_i = P.col(i).cast<float>();
        Eigen::Vector3f q_i = Q.col(i).cast<float>();
        float inverse_weight = p_i(2)*p_i(2) + q_i(2)*q_i(2);
        float weight = 1;
        if (inverse_weight > 0) {
            weight = 1. / weight;
        }
        tfc.add(p_i, q_i, weight);
    }
    T.matrix() = tfc.getTransformation().matrix().cast<double>();
//    T.matrix() = Eigen::umeyama(P, Q, false);
}

PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){
	PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera);
	
	// 合并点云
	PointCloud::Ptr output(new PointCloud());
	pcl::transformPointCloud(*original,*output,T.matrix());
	*newCloud += *output;

	// Voxel grid 滤波降采样
	static pcl::VoxelGrid<PointT> voxel;
	static ParameterReader pd;
	double gridsize = atof(pd.getData("voxel_grid").c_str());
	voxel.setLeafSize(gridsize,gridsize,gridsize);
	voxel.setInputCloud(newCloud);
	PointCloud::Ptr tmp(new PointCloud());
	voxel.filter(*tmp);
	return tmp;
}

void compute_acceleration(const Eigen::Vector3d& a_parent,
                          const Eigen::Vector3d& alpha_parent,
                          const Eigen::Vector3d& w_parent,
                          const Eigen::Vector3d& a_rel,
                          const Eigen::Vector3d& alpha_rel,
                          const Eigen::Vector3d& v_rel,
                          const Eigen::Vector3d& w_rel,
                          const Eigen::Vector3d& offset,
                          const Eigen::Isometry3d& tf_parent,
                          Eigen::Vector3d& a_child,
                          Eigen::Vector3d& alpha_child)
{
  const Eigen::Matrix3d& R = tf_parent.rotation();

  a_child = a_parent + R*a_rel
          + alpha_parent.cross(R*offset)
          + 2*w_parent.cross(R*v_rel)
          + w_parent.cross(w_parent.cross(R*offset));

  alpha_child = alpha_parent + R*alpha_rel + w_parent.cross(R*w_rel);
}

Eigen::Isometry3d SmoothEstimatePropagator::SmoothPropagateAction::
  exponencialInterpolation(const Eigen::Isometry3d& from, const Eigen::Isometry3d& to, double step) const
{
  Eigen::Isometry3d res;

  double maxdist = maxDistance-2;

  // step goes from 1 to maxDistance, we need x from 0 to 1 in a linear way.
  double x = 1 - ((maxdist - (step-1))/maxdist);
  // alpha in [0, inf) describes the explonential ramp "steepness"
  double alpha = 50;
  // exponential ramp from 0 to 1
  double exp_ramp = 1 - (x/(1+alpha*(1-x)));

  // using quaternion representation and slerp for interpolate between from and to isometry transformations
  res.linear() = (Eigen::Quaterniond(from.rotation()).slerp(exp_ramp, Eigen::Quaterniond(to.rotation()))).toRotationMatrix();
  res.translation() = (1 - exp_ramp) * from.translation() + exp_ramp * to.translation();

  return res;
}