예제 #1
0
파일: ShapeNode.cpp 프로젝트: a-price/dart
//==============================================================================
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());
}
예제 #2
0
파일: DARTCollide.cpp 프로젝트: dtbinh/dart
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);
}
예제 #3
0
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;
}
예제 #4
0
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);
}
예제 #5
0
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;
}
예제 #6
0
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;
}
예제 #7
0
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();// 变换矩阵
}
예제 #8
0
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;
}
예제 #9
0
/**
 * @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;
}
예제 #10
0
파일: State.cpp 프로젝트: Shushman/dart
//==============================================================================
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;
}
예제 #11
0
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);
}
예제 #13
0
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;
}
예제 #14
0
  void computeProjectiveTransform()
  {
    // y points downward in the image, upward in real world.
    // same for z.
    Eigen::Isometry3d to_opencv = Eigen::Isometry3d::Identity();
    to_opencv(1,1) = to_opencv(2,2) = -1;

    Eigen::Projective3d projection = Eigen::Projective3d::Identity();
    if (iface->isOrthographic())
    {
      projection(2,2) = 0;
      projection(2,3) = 1;
    }

    Eigen::Isometry3d intrinsics = intrinsicsTransform();
    inv_camera_transform = camera_transform.inverse();

    project_transform = intrinsics * projection * to_opencv * camera_transform;
    inv_project_transform = project_transform.inverse();
  }
예제 #15
0
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);
}
예제 #16
0
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;
}
예제 #17
0
int
main (int argc, char** argv)
{
  // 1. Parse arguments:
  print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }  
  int mode=atoi(argv[1]);
  
  // 2 Construct the simulation method:
  int width = 640;
  int height = 480;
  simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));
  
  // 3 Generate a series of simulation poses:
  // -0 100 fixed poses 
  // -1 a 'halo' camera 
  // -2 slerp between two different poses
  std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
  if (mode==0){
    // Create a pose:
    Eigen::Isometry3d pose;
    pose.setIdentity();
    Matrix3d m;
    //ypr:
    m = AngleAxisd(-9.14989, Vector3d::UnitZ())     * AngleAxisd(0.20944, Vector3d::UnitY())    * AngleAxisd(0, Vector3d::UnitX());  
    pose *= m;
    Vector3d v;
    v << 1.31762, 0.382931, 1.89533;
    pose.translation() = v;  
    for (int i=0;i< 100;i++){ // duplicate the pose 100 times
      poses.push_back(pose); 
    }
  }else if(mode==1){
    Eigen::Vector3d focus_center(0,0,1.3);
    double halo_r = 4;
    double halo_dz = 2;
    int n_poses=16;
    generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
  }else if (mode==2){
    Eigen::Isometry3d pose1;
    pose1.setIdentity();
    pose1.translation() << 1,0.75,2;
    Eigen::Matrix3d rot1;
    rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
	* AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
	* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
    pose1.rotate(rot1);  
  
    Eigen::Isometry3d pose2;
    pose2.setIdentity();
    pose2.translation() << 1,-1,3;
    Eigen::Matrix3d rot2;
    rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
	* AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
	* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr    
    pose2.rotate(rot2);  
  
    int n_poses = 20;
    for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
      Eigen::Quaterniond rot3; 
      Eigen::Quaterniond r1(pose1.rotation());
      Eigen::Quaterniond r2(pose2.rotation());
      rot3 = r1.slerp(i,r2);
      Eigen::Isometry3d pose;
      pose.setIdentity();
      Eigen::Vector3d trans3 = (1-i)*pose1.translation() +  i*pose2.translation();
      pose.translation() <<  trans3[0] ,trans3[1] ,trans3[2];
      pose.rotate(rot3);
      poses.push_back(pose);
    }
  }

  // 4 Do the simulation and write the output:
  double tic_main = getTime();
  for (size_t i=0;i < poses.size();i++){
    std::stringstream ss;
    ss.precision(20);
    ss << "simcloud_" << i;// << ".pcd";
    double tic = getTime();
    simexample->doSim(poses[i]);
    
    write_sim_output(ss.str());
    cout << (getTime() -tic) << " sec\n";
  }
  cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
  cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";
  
  return 0;
}
예제 #18
0
int main( int argc, char** argv )
{
    // 调用格式:命令 [第一个图] [第二个图]
    if (argc != 3)
    {
        cout<<"Usage: ba_example img1, img2"<<endl;
        exit(1);
    }
    
    // 读取图像
    cv::Mat img1 = cv::imread( argv[1] ); 
    cv::Mat img2 = cv::imread( argv[2] ); 
    
    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    {
        cout<<"匹配点不够!"<<endl;
        return 0;
    }
    cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降 
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );
    
    optimizer.setAlgorithm( algorithm );
    optimizer.setVerbose( false );
    
    // 添加节点
    // 两个位姿节点
    for ( int i=0; i<2; i++ )
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true ); // 第一个点固定为零
        // 预设值为单位Pose,因为我们不知道任何信息
        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );
        // 由于深度不知道,只能把深度设置为1了
        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx; 
        double y = ( pts1[i].y - cy ) * z / fy; 
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }
    
    // 准备相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );
    
    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    // 第二帧
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);
        // 核函数
        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }
    
    cout<<"开始优化"<<endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout<<"优化完毕"<<endl;
    
    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;
    
    // 以及所有特征点的位置
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }
    
    // 估计inlier的个数
    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大,说明此边的值与其他边很不相符
        if ( e->chi2() > 1 )
        {
            cout<<"error = "<<e->chi2()<<endl;
        }
        else 
        {
            inliers++;
        }
    }
    
    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");
    return 0;
}
예제 #19
0
int main(int /*argc*/, char** /*argv*/) {
  Line3D l0;
  std::cout << "l0: " << std::endl;
  printPlueckerLine(std::cout, l0);
  std::cout << std::endl;

  Vector6d v;
  v << 1.0, 1.0, -0.3, 0.5, 0.2, 0.3;
  Line3D l1(v);
  l1.normalize();
  std::cout << "v: ";
  printVector(std::cout, v);
  std::cout << std::endl;
  std::cout << "l1: " << std::endl;
  printPlueckerLine(std::cout, l1);
  std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
  std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
  std::cout << std::endl;

  Line3D l2(l1);
  std::cout << "l2: " << std::endl;
  printPlueckerLine(std::cout, l2);
  std::cout << std::endl;

  Eigen::Vector4d mv = l2.ominus(l1);
  Eigen::Quaterniond q(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
  Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
  double yaw = euler_angles[0];
  double pitch = euler_angles[1];
  double roll = euler_angles[2];  
  std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
  std::cout << std::endl;

  v << 0.0, 0.0, 1.0, 0.5, 0.5, 0.0;
  l1 = Line3D(v);
  l1.normalize();
  std::cout << "l1: " << std::endl;
  printPlueckerLine(std::cout, l1);
  std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
  std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
  std::cout << std::endl;
  
  v << 0.0, 0.0, 1.0, 0.5, -0.5, 0.0;
  l2 = Line3D(v);
  l2.normalize();
  std::cout << "l2: " << std::endl;
  printPlueckerLine(std::cout, l2);
  std::cout << "azimuth: " << g2o::internal::getAzimuth(l2.d()) << std::endl;
  std::cout << "elevation: " << g2o::internal::getElevation(l2.d()) << std::endl;
  std::cout << std::endl;

  mv = l2.ominus(l1);
  q = Eigen::Quaterniond(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
  euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler_angles[0];
  pitch = euler_angles[1];
  roll = euler_angles[2];  
  std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
  std::cout << std::endl;
  
  Line3D l3 = Line3D(l1);
  std::cout << "l3: " << std::endl;
  printPlueckerLine(std::cout, l3);
  l3.oplus(mv);
  std::cout << "l3 oplus mv: " << std::endl;
  printPlueckerLine(std::cout, l3);
  std::cout << std::endl;

  std::vector<Line3D> l;
  v << 0.0, 0.0, 1.0, 1.0, 0.0, 0.0;
  Line3D ll = Line3D(v);
  ll.normalize();
  l.push_back(ll);
  v << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0;
  ll = Line3D(v);
  ll.normalize();
  l.push_back(ll);
  v << 0.0, 0.0, 1.0, 0.0, 0.0, 1.0;
  ll = Line3D(v);
  ll.normalize();
  l.push_back(ll);

  for(size_t i = 0; i < l.size(); ++i) {
    Line3D& line = l[i];
    std::cout << "line: "
	      << line.d()[0] << " " << line.d()[1] << " "  << line.d()[2] << " "
	      << line.w()[0] << " " << line.w()[1] << " "  << line.w()[2] << std::endl;
  }
  std::cout << std::endl;

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  T.translation().x() = 0.9;
  std::cout << "R: " << std::endl << T.linear() << std::endl;
  std::cout << "t: " << std::endl << T.translation() << std::endl;
  std::cout << std::endl;
  
  for(size_t i = 0; i < l.size(); ++i) {
    Line3D& line = l[i];
    line = T * line;
    std::cout << "line: "
	      << line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " "
	      << line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl;
  }
  std::cout << std::endl;
  
  return 0;
}
예제 #20
0
void GraphicEnd::lostRecovery()
{
    //以present作为新的关键帧
    cout<<BOLDYELLOW<<"Lost Recovery..."<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );

    //由于当前位置不知道,所以不添加与上一帧相关的边
    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices()[0] = opt.vertex( _currKF.id - 1 );
        edge->vertices()[1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = information(1, 1) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry);
        edge->setInformation( information );
        edge->setMeasurement( To );
        opt.addEdge( edge );
        _odo_last = _odo_this;
        _lost = 0;
        return;
    }
    //check loop closure
    for (int i=0; i<_keyframes.size() - 1; i++)
    {
        vector<PLANE>& p1 = _keyframes[ i ].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上,则在两个帧之间加一条边
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[i].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(1,1) = information(2,2) = 100; 
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }
    
}
예제 #21
0
파일: gicp_demo.cpp 프로젝트: 2maz/g2o
int main()
{
  double euc_noise = 0.01;       // noise in position, m
  //  double outlier_ratio = 0.1;


  SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  // variable-size block solver
  BlockSolverX::LinearSolverType * linearSolver = new LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
  BlockSolverX * solver_ptr = new BlockSolverX(linearSolver);
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

  optimizer.setAlgorithm(solver);

  vector<Vector3d> true_points;
  for (size_t i=0;i<1000; ++i)
  {
    true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
                                   Sample::uniform()-0.5,
                                   Sample::uniform()+10));
  }


  // set up two poses
  int vertex_id = 0;
  for (size_t i=0; i<2; ++i)
  {
    // set up rotation and translation for this node
    Vector3d t(0,0,i);
    Quaterniond q;
    q.setIdentity();

    Eigen::Isometry3d cam; // camera pose
    cam = q;
    cam.translation() = t;

    // set up node
    VertexSE3 *vc = new VertexSE3();
    vc->setEstimate(cam);

    vc->setId(vertex_id);      // vertex id

    cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;

    // set first cam pose fixed
    if (i==0)
      vc->setFixed(true);

    // add to optimizer
    optimizer.addVertex(vc);

    vertex_id++;                
  }

  // set up point matches
  for (size_t i=0; i<true_points.size(); ++i)
  {
    // get two poses
    VertexSE3* vp0 = 
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
    VertexSE3* vp1 = 
      dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);

    // calculate the relative 3D position of the point
    Vector3d pt0,pt1;
    pt0 = vp0->estimate().inverse() * true_points[i];
    pt1 = vp1->estimate().inverse() * true_points[i];

    // add in noise
    pt0 += Vector3d(Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ));

    pt1 += Vector3d(Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ),
                    Sample::gaussian(euc_noise ));

    // form edge, with normals in varioius positions
    Vector3d nm0, nm1;
    nm0 << 0, i, 1;
    nm1 << 0, i, 1;
    nm0.normalize();
    nm1.normalize();

    Edge_V_V_GICP * e           // new edge with correct cohort for caching
        = new Edge_V_V_GICP(); 

    e->setVertex(0, vp0);      // first viewpoint

    e->setVertex(1, vp1);      // second viewpoint

    EdgeGICP meas;
    meas.pos0 = pt0;
    meas.pos1 = pt1;
    meas.normal0 = nm0;
    meas.normal1 = nm1;

    e->setMeasurement(meas);
    //        e->inverseMeasurement().pos() = -kp;
    
    meas = e->measurement();
    // use this for point-plane
    e->information() = meas.prec0(0.01);

    // use this for point-point 
    //    e->information().setIdentity();

    //    e->setRobustKernel(true);
    //e->setHuberWidth(0.01);

    optimizer.addEdge(e);
  }

  // move second cam off of its true position
  VertexSE3* vc = 
    dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
  Eigen::Isometry3d cam = vc->estimate();
  cam.translation() = Vector3d(0,0,0.2);
  vc->setEstimate(cam);

  optimizer.initializeOptimization();
  optimizer.computeActiveErrors();
  cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;

  optimizer.setVerbose(true);

  optimizer.optimize(5);

  cout << endl << "Second vertex should be near 0,0,1" << endl;
  cout <<  dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
    ->estimate().translation().transpose() << endl;
  cout <<  dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
    ->estimate().translation().transpose() << endl;
}
예제 #22
0
int GraphicEnd::run()
{
    //清空present并读取新的数据
    cout<<"********************"<<endl;
    _present.planes.clear();
    
    readimage();
    
    //处理present
    _present.planes = extractPlanesAndGenerateImage( _currCloud, _currRGB, _currDep );
    //_present.planes = extractPlanes( _currCloud );
    //generateImageOnPlane( _currRGB, _present.planes, _currDep );

    for ( size_t i=0; i<_present.planes.size(); i++ )
    {
        _present.planes[i].kp = extractKeypoints( _present.planes[i].image );
        _present.planes[i].desp = extractDescriptor( _currRGB, _present.planes[i].kp );
        compute3dPosition( _present.planes[i], _currDep);
    }

    // 求解present到current的变换矩阵
    RESULT_OF_MULTIPNP result = multiPnP( _currKF.planes, _present.planes );
    Eigen::Isometry3d T = result.T;
    T = T.inverse();  //好像是反着的
    
    // 如果平移和旋转超过一个阈值,则定义新的关键帧
    if ( T.matrix() == Eigen::Isometry3d::Identity().matrix() )
    {
        //未匹配上
        cout<<BOLDRED"This frame lost"<<RESET<<endl;
        _lost++;
    }
    else if (result.norm > _max_pos_change)
    {
        //生成一个新的关键帧
        _robot = T * _kf_pos;
        generateKeyFrame(T);
        if (_loop_closure_detection == true)
            loopClosure();
        _lost = 0;
    }
    else
    {
        //小变化,更新robot位置
        _robot = T * _kf_pos;
        _lost = 0;
    }
    if (_lost > _lost_frames)
    {
        cerr<<"the robot lost. Perform lost recovery."<<endl;
        lostRecovery();
    }


    cout<<RED<<"key frame size = "<<_keyframes.size()<<RESET<<endl;
    _index ++;
    if (_use_odometry)
    {
        _odo_this = _odometry[ _index-1 ];
    }

    return 1;
}
예제 #23
0
//回环检测:在过去的帧中随机取_loopclosure_frames那么多帧进行两两比较
void GraphicEnd::loopClosure()
{
    if (_keyframes.size() <= 3 )  //小于3时,回环没有意义
        return;
    cout<<"Checking loop closure."<<endl;
    waitKey(10);
    vector<int> checked;
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;

    //相邻帧
    for (int i=-3; i>-5; i--)
    {
        int n = _keyframes.size() + i;
        if (n>=0)
        {
            vector<PLANE>& p1 = _keyframes[n].planes;
            Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
            if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
                continue;
            T = T.inverse();
            //若匹配上,则在两个帧之间加一条边
            EdgeSE3* edge = new EdgeSE3();
            edge->vertices() [0] = opt.vertex( _keyframes[n].id );
            edge->vertices() [1] = opt.vertex( _currKF.id );
            Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
            information(0, 0) = information(2,2) = 100; 
            information(1,1) = 100;
            information(3,3) = information(4,4) = information(5,5) = 100; 
            edge->setInformation( information );
            edge->setMeasurement( T );
            edge->setRobustKernel( _pSLAMEnd->_robustKernel );
            opt.addEdge( edge );
        }
        else
            break;
    }
    //搜索种子帧
    cout<<"checking seeds, seed.size()"<<_seed.size()<<endl;
    vector<int> newseed;
    for (size_t i=0; i<_seed.size(); i++)
    {
        vector<PLANE>& p1 = _keyframes[_seed[i]].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上,则在两个帧之间加一条边
        checked.push_back( _seed[i] );
        newseed.push_back( _seed[i] );
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[_seed[i]].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }

    //随机搜索
    cout<<"checking random frames"<<endl;
    for (int i=0; i<_loopclosure_frames; i++)
    {
        int frame = rand() % (_keyframes.size() -3 ); //随机在过去的帧中取一帧
        if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //之前已检查过
            continue;
        checked.push_back( frame );
        vector<PLANE>& p1 = _keyframes[frame].planes;
        RESULT_OF_MULTIPNP result = multiPnP( p1, _currKF.planes, true, _keyframes[frame].frame_index, 20 );
        Eigen::Isometry3d T = result.T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        displayLC( _keyframes[frame].frame_index, _currKF.frame_index, result.norm);
        newseed.push_back( frame );
        //若匹配上,则在两个帧之间加一条边
        cout<<BOLDBLUE<<"find a loop closure between kf "<<_currKF.id<<" and kf "<<frame<<RESET<<endl;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[frame].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }

    waitKey(10);
    _seed = newseed;
}
예제 #24
0
void test_relative_values(bool spatial_targets, bool spatial_followers)
{
  const double tolerance = 1e-8;

  // These frames will form a chain
  SimpleFrame A(Frame::World(), "A");
  SimpleFrame B(&A, "B");
  SimpleFrame C(&B, "C");
  SimpleFrame D(&C, "D");

  std::vector<SimpleFrame*> targets;
  targets.push_back(&A);
  targets.push_back(&B);
  targets.push_back(&C);
  targets.push_back(&D);

  // R will be an arbitrary reference frame
  SimpleFrame R(Frame::World(), "R");
  targets.push_back(&R);

  // Each of these frames will attempt to track one of the frames in the chain
  // with respect to the frame R.
  SimpleFrame AR(&R, "AR");
  SimpleFrame BR(&R, "BR");
  SimpleFrame CR(&R, "CR");
  SimpleFrame DR(&R, "DR");
  SimpleFrame RR(&R, "RR");

  std::vector<SimpleFrame*> followers;
  followers.push_back(&AR);
  followers.push_back(&BR);
  followers.push_back(&CR);
  followers.push_back(&DR);
  followers.push_back(&RR);

  assert( targets.size() == followers.size() );

  randomize_target_values(targets, spatial_targets);
  set_relative_values(targets, followers, spatial_followers);
  check_world_values(targets, followers, tolerance);

  // Check every combination of relative values
  for(std::size_t i=0; i<targets.size(); ++i)
  {
    Frame* T = targets[i];
    for(std::size_t j=0; j<followers.size(); ++j)
    {
      Frame* F = followers[j];

      check_values(targets, followers, T, F, tolerance);
      check_values(targets, followers, F, T, tolerance);
      check_offset_computations(targets, followers, T, F, tolerance);
    }
  }

  // Test SimpleFrame::setTransform()
  for(std::size_t i=0; i<followers.size(); ++i)
  {
    for(std::size_t j=0; j<followers.size(); ++j)
    {
      SimpleFrame* F = followers[i];
      SimpleFrame* T = followers[j];
      Eigen::Isometry3d tf;
      randomize_transform(tf, 1, 2*M_PI);
      T->setTransform(tf, F);
      if(i != j)
        EXPECT_TRUE( equals(T->getTransform(F).matrix(), tf.matrix(), 1e-10));
    }
  }
}
예제 #25
0
파일: DARTCollide.cpp 프로젝트: dtbinh/dart
int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0,
                     const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
                     std::vector<Contact>* result)
{
  Eigen::Vector3d size = 0.5 * size1;
  bool inside_box = true;

  // clipping a center of the sphere to a boundary of the box
  Eigen::Vector3d c0 = T0.translation();
  Eigen::Vector3d p = T1.inverse() * c0;

  if (p[0] < -size[0]) { p[0] = -size[0]; inside_box = false; }
  if (p[0] >  size[0]) { p[0] =  size[0]; inside_box = false; }

  if (p[1] < -size[1]) { p[1] = -size[1]; inside_box = false; }
  if (p[1] >  size[1]) { p[1] =  size[1]; inside_box = false; }

  if (p[2] < -size[2]) { p[2] = -size[2]; inside_box = false; }
  if (p[2] >  size[2]) { p[2] =  size[2]; inside_box = false; }


  Eigen::Vector3d normal(0.0, 0.0, 0.0);
  double penetration;

  if ( inside_box )
  {
    // find nearest side from the sphere center
    double min = size[0] - std::abs(p[0]);
    double tmin = size[1] - std::abs(p[1]);
    int idx = 0;

    if ( tmin < min )
    {
      min = tmin;
      idx = 1;
    }
    tmin = size[2] - std::abs(p[2]);
    if ( tmin < min )
    {
      min = tmin;
      idx = 2;
    }

    normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
    normal = T1.linear() * normal;
    penetration = min + r0;

    Contact contact;
    contact.point = c0;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result->push_back(contact);
    return 1;
  }


  Eigen::Vector3d contactpt = T1 * p;
  normal = c0 - contactpt;
  double mag = normal.norm();
  penetration = r0 - mag;

  if (penetration < 0.0)
  {
    return 0;
  }

  if (mag > DART_COLLISION_EPS)
  {
    normal *= (1.0/mag);

    Contact contact;
    contact.point = contactpt;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result->push_back(contact);}
  else
  {
    double min = size[0] - std::abs(p[0]);
    double tmin = size[1] - std::abs(p[1]);
    int idx = 0;

    if ( tmin < min )
    {
      min = tmin;
      idx = 1;
    }
    tmin = size[2] - std::abs(p[2]);
    if ( tmin < min )
    {
      min = tmin;
      idx = 2;
    }
    normal.setZero();
    normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
    normal = T1.linear() * normal;

    Contact contact;
    contact.point = contactpt;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result->push_back(contact);
  }
  return 1;
}
예제 #26
0
void check_offset_computations(const std::vector<SimpleFrame*>& targets,
                               const std::vector<SimpleFrame*>& followers,
                               const Frame* relativeTo,
                               const Frame* inCoordinatesOf,
                               double tolerance)
{
  for(std::size_t i=0; i<targets.size(); ++i)
  {
    Frame* T = targets[i];
    Frame* F = followers[i];

    Eigen::Isometry3d coordTf = relativeTo->getTransform(inCoordinatesOf);

    Vector3d offset_T = random_vec<3>();
    Vector3d offset_F = T->getTransform(F) * offset_T;

    Vector3d v_TO, w_TO, v_FO, w_FO, a_TO, alpha_TO, a_FO, alpha_FO;

    // Compute velocity of the offfset in the relative frame
    compute_velocity(T->getLinearVelocity(relativeTo, relativeTo),
                     T->getAngularVelocity(relativeTo, relativeTo),
                     Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                     offset_T, T->getTransform(relativeTo), v_TO, w_TO);
    // Convert to the desired coordinate system
    v_TO = coordTf.linear() * v_TO;
    w_TO = coordTf.linear() * w_TO;

    // Compute acceleration of the offset in the relative frame
    compute_acceleration(T->getLinearAcceleration(relativeTo, relativeTo),
                         T->getAngularAcceleration(relativeTo, relativeTo),
                         T->getAngularVelocity(relativeTo, relativeTo),
                         Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         offset_T, T->getTransform(relativeTo), a_TO, alpha_TO);
    // Convert to the desired coordinate system
    a_TO = coordTf.linear() * a_TO;
    alpha_TO = coordTf.linear() * alpha_TO;

    compute_velocity(F->getLinearVelocity(relativeTo, relativeTo),
                     F->getAngularVelocity(relativeTo, relativeTo),
                     Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                     offset_F, F->getTransform(relativeTo), v_FO, w_FO);
    v_FO = coordTf.linear() * v_FO;
    w_FO = coordTf.linear() * w_FO;

    compute_acceleration(F->getLinearAcceleration(relativeTo, relativeTo),
                         F->getAngularAcceleration(relativeTo, relativeTo),
                         F->getAngularVelocity(relativeTo, relativeTo),
                         Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         offset_F, F->getTransform(relativeTo), a_FO, alpha_FO);
    a_FO = coordTf.linear() * a_FO;
    alpha_FO = coordTf.linear() * alpha_FO;

    Eigen::Vector3d v_TO_actual = T->getLinearVelocity(offset_T, relativeTo,
                                                       inCoordinatesOf);
    Eigen::Vector3d v_FO_actual = F->getLinearVelocity(offset_F, relativeTo,
                                                       inCoordinatesOf);

    EXPECT_TRUE( equals(v_TO, v_FO, tolerance) );
    EXPECT_TRUE( equals(v_TO, v_TO_actual, tolerance) );
    EXPECT_TRUE( equals(v_FO, v_FO_actual, tolerance) );

    Eigen::Vector3d a_TO_actual = T->getLinearAcceleration(offset_T, relativeTo,
                                                           inCoordinatesOf);
    Eigen::Vector3d a_FO_actual = F->getLinearAcceleration(offset_F, relativeTo,
                                                           inCoordinatesOf);

    EXPECT_TRUE( equals(a_TO, a_FO, tolerance) );
    EXPECT_TRUE( equals(a_TO, a_TO_actual, tolerance) );
    EXPECT_TRUE( equals(a_FO, a_FO_actual, tolerance) );
  }
}
예제 #27
0
TEST(FRAMES, FORWARD_KINEMATICS_CHAIN)
{
  std::vector<SimpleFrame*> frames;

  double tolerance = 1e-6;

  SimpleFrame A(Frame::World(), "A");
  frames.push_back(&A);
  SimpleFrame B(&A, "B");
  frames.push_back(&B);
  SimpleFrame C(&B, "C");
  frames.push_back(&C);
  SimpleFrame D(&C, "D");
  frames.push_back(&D);

  // -- Test Position --------------------------------------------------------
  EXPECT_TRUE( equals(D.getTransform().matrix(),
                      Eigen::Isometry3d::Identity().matrix(),
                      tolerance));

  Eigen::aligned_vector<Eigen::Isometry3d> tfs;
  tfs.resize(frames.size(), Eigen::Isometry3d::Identity());

  randomize_transforms(tfs);

  for(std::size_t i=0; i<frames.size(); ++i)
  {
    SimpleFrame* F = frames[i];
    F->setRelativeTransform(tfs[i]);
  }

  for(std::size_t i=0; i<frames.size(); ++i)
  {
    Frame* F = frames[i];
    Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
    for(std::size_t j=0; j<=i; ++j)
    {
      expectation = expectation * tfs[j];
    }

    Eigen::Isometry3d actual = F->getTransform();
    EXPECT_TRUE( equals(actual.matrix(), expectation.matrix(), tolerance));
  }

  randomize_transforms(tfs);
  for(std::size_t i=0; i<frames.size(); ++i)
  {
    SimpleFrame* F = frames[i];
    F->setRelativeTransform(tfs[i]);
  }

  Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
  for(std::size_t j=0; j<frames.size(); ++j)
    expectation = expectation * tfs[j];

  EXPECT_TRUE( equals(frames.back()->getTransform().matrix(),
                      expectation.matrix(),
                      tolerance) );


  // -- Test Velocity --------------------------------------------------------

  // Basic forward spatial velocity propagation
  { // The brackets are to allow reusing variable names
    Eigen::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
    Eigen::aligned_vector<Eigen::Vector6d> v_total(frames.size());

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<6>();

      SimpleFrame* F = frames[i];
      F->setRelativeSpatialVelocity(v_rels[i]);

      if(i>0)
        compute_spatial_velocity(v_total[i-1], v_rels[i],
            F->getRelativeTransform(), v_total[i]);
      else
        compute_spatial_velocity(Eigen::Vector6d::Zero(), v_rels[i],
                                 F->getRelativeTransform(), v_total[i]);
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];

      Eigen::Vector6d v_actual = F->getSpatialVelocity();

      EXPECT_TRUE( equals(v_total[i], v_actual) );
    }
  }

  // Testing conversion beteween spatial and classical velocities
  {
    std::vector<Eigen::Vector3d> v_rels(frames.size());
    std::vector<Eigen::Vector3d> w_rels(frames.size());

    std::vector<Eigen::Vector3d> v_total(frames.size());
    std::vector<Eigen::Vector3d> w_total(frames.size());

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<3>();
      w_rels[i] = random_vec<3>();

      SimpleFrame* F = frames[i];
      F->setClassicDerivatives(v_rels[i], w_rels[i]);

      Eigen::Vector3d offset = F->getRelativeTransform().translation();

      Eigen::Isometry3d tf = i>0? frames[i-1]->getTransform() :
          Eigen::Isometry3d::Identity();

      if(i>0)
        compute_velocity(v_total[i-1], w_total[i-1], v_rels[i], w_rels[i],
            offset, tf, v_total[i], w_total[i]);
      else
        compute_velocity(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         v_rels[i], w_rels[i],
                         offset, tf, v_total[i], w_total[i]);
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];
      Eigen::Vector3d v_actual = F->getLinearVelocity();
      Eigen::Vector3d w_actual = F->getAngularVelocity();

      EXPECT_TRUE( equals(v_total[i], v_actual, tolerance) );
      EXPECT_TRUE( equals(w_total[i], w_actual, tolerance) );
    }
  }

  // -- Acceleration ---------------------------------------------------------

  // Basic forward spatial acceleration propagation
  {
    Eigen::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
    Eigen::aligned_vector<Eigen::Vector6d> a_rels(frames.size());

    Eigen::aligned_vector<Eigen::Vector6d> v_total(frames.size());
    Eigen::aligned_vector<Eigen::Vector6d> a_total(frames.size());


    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<6>();
      a_rels[i] = random_vec<6>();

      SimpleFrame* F = frames[i];
      F->setRelativeSpatialVelocity(v_rels[i]);
      F->setRelativeSpatialAcceleration(a_rels[i]);
      Eigen::Isometry3d tf = F->getRelativeTransform();

      if(i>0)
      {
        compute_spatial_velocity(v_total[i-1], v_rels[i], tf, v_total[i]);
        compute_spatial_acceleration(a_total[i-1], a_rels[i], v_total[i],
            v_rels[i], tf, a_total[i]);
      }
      else
      {
        compute_spatial_velocity(Eigen::Vector6d::Zero(), v_rels[i],
                                 tf, v_total[i]);
        compute_spatial_acceleration(Eigen::Vector6d::Zero(), a_rels[i],
                                     v_total[i], v_rels[i], tf, a_total[i]);
      }
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];

      Eigen::Vector6d a_actual = F->getSpatialAcceleration();

      EXPECT_TRUE( equals(a_total[i], a_actual) );
    }

    // Test relative computations
    for(std::size_t i=0; i<frames.size(); ++i)
    {
      Frame* F = frames[i];
      Frame* P = F->getParentFrame();

      Eigen::Vector6d v_rel = F->getSpatialVelocity(P, F);
      Eigen::Vector6d a_rel = F->getSpatialAcceleration(P, F);

      EXPECT_TRUE( equals(v_rels[i], v_rel, tolerance) );
      EXPECT_TRUE( equals(a_rels[i], a_rel, tolerance) );
    }

    // Test offset computations
    for(std::size_t i=0; i<frames.size(); ++i)
    {
      Eigen::Vector3d offset = random_vec<3>();
      Frame* F = frames[i];

      Eigen::Vector3d v_actual = F->getLinearVelocity(offset);
      Eigen::Vector3d w_actual = F->getAngularVelocity();
      Eigen::Vector3d v_expect, w_expect;
      compute_velocity(F->getLinearVelocity(), F->getAngularVelocity(),
                       Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                       offset, F->getWorldTransform(), v_expect, w_expect);

      EXPECT_TRUE( equals( v_expect, v_actual) );
      EXPECT_TRUE( equals( w_expect, w_actual) );

      Eigen::Vector3d a_actual = F->getLinearAcceleration(offset);
      Eigen::Vector3d alpha_actual = F->getAngularAcceleration();
      Eigen::Vector3d a_expect, alpha_expect;
      compute_acceleration(F->getLinearAcceleration(),
                           F->getAngularAcceleration(), F->getAngularVelocity(),
                           Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                           Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                           offset, F->getWorldTransform(),
                           a_expect, alpha_expect);

      EXPECT_TRUE( equals( a_expect, a_actual) );
      EXPECT_TRUE( equals( alpha_expect, alpha_actual) );
    }
  }

  // Testing conversion between spatial and classical accelerations
  {
    std::vector<Eigen::Vector3d> v_rels(frames.size());
    std::vector<Eigen::Vector3d> w_rels(frames.size());
    std::vector<Eigen::Vector3d> a_rels(frames.size());
    std::vector<Eigen::Vector3d> alpha_rels(frames.size());

    std::vector<Eigen::Vector3d> v_total(frames.size());
    std::vector<Eigen::Vector3d> w_total(frames.size());
    std::vector<Eigen::Vector3d> a_total(frames.size());
    std::vector<Eigen::Vector3d> alpha_total(frames.size());

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      v_rels[i] = random_vec<3>();
      w_rels[i] = random_vec<3>();
      a_rels[i] = random_vec<3>();
      alpha_rels[i] = random_vec<3>();

      SimpleFrame* F = frames[i];
      Eigen::Isometry3d rel_tf;
      randomize_transform(rel_tf);
      F->setRelativeTransform(rel_tf);
      F->setClassicDerivatives(v_rels[i], w_rels[i], a_rels[i], alpha_rels[i]);

      Eigen::Vector3d offset = F->getRelativeTransform().translation();

      Eigen::Isometry3d tf = i>0? frames[i-1]->getTransform() :
          Eigen::Isometry3d::Identity();

      if(i>0)
      {
        compute_velocity(v_total[i-1], w_total[i-1], v_rels[i], w_rels[i],
            offset, tf, v_total[i], w_total[i]);
        compute_acceleration(a_total[i-1], alpha_total[i-1], w_total[i-1],
            a_rels[i], alpha_rels[i], v_rels[i], w_rels[i], offset, tf,
            a_total[i], alpha_total[i]);
      }
      else
      {
        compute_velocity(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                         v_rels[i], w_rels[i], offset, tf,
                         v_total[i], w_total[i]);
        compute_acceleration(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
                             Eigen::Vector3d::Zero(), a_rels[i], alpha_rels[i],
                             v_rels[i], w_rels[i], offset, tf,
                             a_total[i], alpha_total[i]);
      }
    }

    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];
      Eigen::Vector3d v_actual = F->getLinearVelocity();
      Eigen::Vector3d w_actual = F->getAngularVelocity();
      Eigen::Vector3d a_actual = F->getLinearAcceleration();
      Eigen::Vector3d alpha_actual = F->getAngularAcceleration();

      EXPECT_TRUE( equals(v_total[i], v_actual, tolerance) );
      EXPECT_TRUE( equals(w_total[i], w_actual, tolerance) );
      EXPECT_TRUE( equals(a_total[i], a_actual, tolerance) );
      EXPECT_TRUE( equals(alpha_total[i], alpha_actual, tolerance) );
    }

    // Test relative computations
    for(std::size_t i=0; i<frames.size(); ++i)
    {
      SimpleFrame* F = frames[i];
      Frame* P = F->getParentFrame();
      Eigen::Vector3d v_rel = F->getLinearVelocity(P, P);
      Eigen::Vector3d w_rel = F->getAngularVelocity(P, P);
      Eigen::Vector3d a_rel = F->getLinearAcceleration(P, P);
      Eigen::Vector3d alpha_rel = F->getAngularAcceleration(P, P);

      EXPECT_TRUE( equals(v_rels[i], v_rel, tolerance) );
      EXPECT_TRUE( equals(w_rels[i], w_rel, tolerance) );
      EXPECT_TRUE( equals(a_rels[i], a_rel, tolerance) );
      EXPECT_TRUE( equals(alpha_rels[i], alpha_rel, tolerance) );
    }
  }
}
예제 #28
0
  void ViewerState::optimizeSelected(){
      DrawableFrame* current = drawableFrameVector.back();
      DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2];
      cerr << "optimizing" << endl;
      cerr << "current=" << current->frame() << endl;
      cerr << "reference= " << reference->frame() << endl;


      

      // cerr computing initial guess based on the frame positions, just for convenience
      Eigen::Isometry3d delta = reference->_vertex->estimate().inverse()*current->_vertex->estimate();
      for(int c=0; c<4; c++)
	for(int r=0; r<3; r++)
	  initialGuess.matrix()(r,c) = delta.matrix()(r,c);


      Eigen::Isometry3f odometryMean;
      Matrix6f odometryInfo;
      bool hasOdometry = extractRelativePrior(odometryMean, odometryInfo, reference, current);
      if (hasOdometry)
	initialGuess=odometryMean;

      Eigen::Isometry3f imuMean;
      Matrix6f imuInfo;
      bool hasImu = extractAbsolutePrior(imuMean, imuInfo, current);

      initialGuess.matrix().row(3) << 0,0,0,1;
      
      if(!wasInitialGuess) {
	aligner->clearPriors();
	aligner->setOuterIterations(al_outerIterations);
	aligner->setReferenceFrame(reference->frame());
	aligner->setCurrentFrame(current->frame());
	aligner->setInitialGuess(initialGuess);
	aligner->setSensorOffset(sensorOffset);
	if (hasOdometry)
	  aligner->addRelativePrior(odometryMean, odometryInfo);
	if (hasImu)
	  aligner->addAbsolutePrior(reference->transformation(), imuMean, imuInfo);
	aligner->align();
      }

      Eigen::Isometry3f localTransformation =aligner->T();
      if (aligner->inliers()<1000 || aligner->error()/aligner->inliers()>10){
	cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
	cerr << "aligner: monster failure: inliers = " << aligner->inliers() << endl;
	cerr << "aligner: monster failure: error/inliers = " << aligner->error()/aligner->inliers() << endl;
	cerr  << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
      	localTransformation = initialGuess;
	sleep(1);
      }
      cout << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
      
      globalT = reference->transformation()*aligner->T();
      
      // Update cloud drawing position.
      current->setTransformation(globalT);
      current->setLocalTransformation(aligner->T());

      // Show zBuffers.
      refScn->clear();
      currScn->clear();
      QImage refQImage;
      QImage currQImage;
      DepthImageView div;
      div.computeColorMap(300, 2000, 128);
      div.convertToQImage(refQImage, aligner->correspondenceFinder()->referenceDepthImage());
      div.convertToQImage(currQImage, aligner->correspondenceFinder()->currentDepthImage());
      refScn->addPixmap((QPixmap::fromImage(refQImage)).scaled(QSize((int)refQImage.width()/(ng_scale*3), (int)(refQImage.height()/(ng_scale*3)))));
      currScn->addPixmap((QPixmap::fromImage(currQImage)).scaled(QSize((int)currQImage.width()/(ng_scale*3), (int)(currQImage.height()/(ng_scale*3)))));
      pwnGMW->graphicsView1_2d->show();
      pwnGMW->graphicsView2_2d->show();
      
      wasInitialGuess = false;
      newCloudAdded = false;
      *initialGuessViewer = 0;
      *optimizeViewer = 0;
  }
예제 #29
0
bool GraphGridMapper::convertLaserScans2Map(SlamGraph &graph, nav_msgs::OccupancyGrid &map, ros::Publisher &mapPub, Eigen::Isometry3d diff_transform)
{
    std::string map_frame = "/map";

    double diff_translation = diff_transform.translation().norm();
    Eigen::AngleAxisd diff_angleaxis(diff_transform.linear());
    double diff_rotation = fabs(diff_angleaxis.angle()) * 180 / M_PI;
    ROS_DEBUG("map -> odom diff: %f m, %f deg", diff_translation, diff_rotation);

    std::vector<std::string> nodes_to_add;
    if (diff_translation <= 0.5 && diff_rotation < 5 && last_node_id_ != "") {
        nodes_to_add = graph.getNodesAfter(last_node_id_);
    } else {
        // Clear occupancy grid.
        clouds_.clear();

        //setup map dimensions
        nav_msgs::MapMetaData info;
        cv::Point2d gridSize;
        info.origin = getMapOrigin(graph, gridSize);
        info.resolution = config_.resolution;
        info.width = gridSize.x / info.resolution;
        info.height = gridSize.y / info.resolution;

        fake_grid_.info = info;
        overlay_ = occupancy_grid_utils::createCloudOverlay(fake_grid_, map_frame, 0.1, 10, 1);

        for (auto node_it = graph.nodeIterator(); node_it.first != node_it.second; node_it.first++) {
            nodes_to_add.push_back(node_it.first->first);
        }
    }
    if (nodes_to_add.size() > 0) {
        last_node_id_ = nodes_to_add.back();
    }

    for (auto node_id : nodes_to_add) {
        if (graph.existsNode(node_id)) {
            const SlamNode &node = graph.node(node_id);
            auto node_pose = affineTransformToPoseMsg(node.pose_);
            occupancy_grid_utils::addKnownFreePoint(&overlay_, node_pose.position, 0.5);

            // If there is a node off the grid, force clearing in the next iteration.
            if (node.pose_.translation()(0) < fake_grid_.info.origin.position.x + config_.range_max ||
                    node.pose_.translation()(1) < fake_grid_.info.origin.position.y + config_.range_max ||
                    node.pose_.translation()(0) > fake_grid_.info.origin.position.x + fake_grid_.info.width * fake_grid_.info.resolution - config_.range_max ||
                    node.pose_.translation()(1) > fake_grid_.info.origin.position.y + fake_grid_.info.height * fake_grid_.info.resolution - config_.range_max) {
                last_node_id_ = "";
            }
        }
    }

    for (auto node_id : nodes_to_add) {
        if (graph.existsNode(node_id)) {
            const SlamNode &node = graph.node(node_id);

            // Search for laser scan.
            int total_points = 0;
            for (auto data : node.sensor_data_) {
                if (data->type_ == graph_slam_msgs::SensorData::SENSOR_TYPE_LASERSCAN) {
                    boost::shared_ptr<LaserscanData> depth_data = boost::dynamic_pointer_cast<LaserscanData>(data);
                    sensor_msgs::LaserScan laser = depth_data->scan_;
                    laser.range_max = config_.range_max;
                    try
                    {
                        total_points += laser.ranges.size();

                        sensor_msgs::PointCloud fixed_frame_cloud;
                        laser_geometry::LaserProjection projector;
                        projector.projectLaser(laser,fixed_frame_cloud);

                        // Construct and save LocalizedCloud
                        CloudPtr loc_cloud(new occupancy_grid_utils::LocalizedCloud());
                        loc_cloud->cloud.points = fixed_frame_cloud.points;
                        loc_cloud->sensor_pose = affineTransformToPoseMsg(node.pose_ * depth_data->displacement_);
                        loc_cloud->header.frame_id = map_frame;
                        loc_cloud->header.stamp = laser.header.stamp;
                        clouds_.push_back(loc_cloud);

                        if (clouds_.size() > 1 && total_points > 0) {
                            occupancy_grid_utils::addCloud(&overlay_, clouds_.back());
                        }
                    }
                    catch (tf::LookupException& e)
                    {
                        ROS_WARN("lookup error");
                        return false;
                    }
                    catch (tf::ExtrapolationException)
                    {
                        ROS_ERROR("extrapolation exception");
                        return false;
                    }
                    catch (tf::ConnectivityException)
                    {
                        ROS_ERROR("connectivity exception");
                        return false;
                    }
                }
            }
        }
    }

    //retrieve grid
    nav_msgs::OccupancyGridConstPtr grid = occupancy_grid_utils::getGrid(overlay_);
    map = *grid;
    return true;
}
예제 #30
0
파일: line3d.cpp 프로젝트: asimay/g2o
 Vector6d transformCartesianLine(const Eigen::Isometry3d& t, const Vector6d& line){
   Vector6d l;
   l.head<3>() = t*line.head<3>();
   l.tail<3>() = t.linear()*line.tail<3>();
   return normalizeCartesianLine(l);
 }