C++ Isometry3d::inverse方法代码示例

//==============================================================================
Eigen::Vector6d FreeJoint::getPositionDifferencesStatic(
    const Eigen::Vector6d& _q2,
    const Eigen::Vector6d& _q1) const
{
  const Eigen::Isometry3d T1 = convertToTransform(_q1);
  const Eigen::Isometry3d T2 = convertToTransform(_q2);

  return convertToPositions(T1.inverse() * T2);
}

void RegApp::featuresReferenceHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  reg::features_t* msg){
  
  if (registeration_mode_ != 1){ // ignore features if we havent just been asked to track to them
    return;
  }

  cout << "\n\ngot reference features @ "<< msg->utime<<"\n";

  // now find the corresponding image and pair it
  bool feat_image_matched_ = false;
  for(int i=0; i < image_queue_->size(); i++){
    //cout << image_queue_->at(i).utime << " " << i << "\n";
    if (msg->utime == image_queue_->at(i).utime){
      cur_features_ = getFeaturesFromLCM(msg,cur_pose_);
      cur_image_ = image_queue_->at(i);
      cout << "image and features matched: " << i << " " << image_queue_->at(i).utime << " ("<< cur_features_.size()<<"f)\n"; 
      feat_image_matched_ = true;

      break;
    }
  }
  
  if (!feat_image_matched_){
     cout <<"didn't find a matching reference image - make image deque bigger\n";
     exit(-1);
  } 

  ref_features_ = cur_features_;
  ref_image_ = cur_image_;
  ref_pose_ = cur_pose_;

  // set the reference image
  cout << "Will now register to " << ref_features_.size() << " features\n";
  
  // Send the pose we will try to register back to:
  Isometry3dTime ref_poseT = Isometry3dTime(ref_image_.utime, ref_pose_);
  pc_vis_->pose_to_lcm_from_list(70000, ref_poseT);

  deque_to_cloud(laser_queue_, botframes_, accum_cloud);

  Isometry3dTime null_poseT = Isometry3dTime(ref_image_.utime, Eigen::Isometry3d::Identity()  );
  pc_vis_->pose_to_lcm_from_list(70001, null_poseT);
  pc_vis_->ptcld_to_lcm_from_list(70002, *accum_cloud, ref_image_.utime, ref_image_.utime);

  std::stringstream ss2;
  print_Isometry3d(ref_pose_, ss2);
  std::cout << "Received refpose: " << ss2.str() << "\n";

  Eigen::Isometry3f pose_f = ref_pose_.inverse().cast<float>();
  Eigen::Quaternionf pose_quat(pose_f.rotation());
  pcl::transformPointCloud (*accum_cloud, *accum_cloud,
      pose_f.translation(), pose_quat); // !! modifies accum_cloud
  pc_vis_->ptcld_to_lcm_from_list(70003, *accum_cloud, ref_image_.utime, ref_image_.utime);

  registeration_mode_ = 2;
}

void foot_contact_classify::determineContactPoints(int64_t utime, Eigen::Isometry3d primary_foot, Eigen::Isometry3d secondary_foot){
  // Currently Deprecated to remove PCL dependency

  //////////////////////////////////////////////////////////
  bool success =false;

  // use primary and secondary foot to determine plane that CPs are one
  Isometry3dTime null_pose = Isometry3dTime(utime, Eigen::Isometry3d::Identity() );
  pc_vis_->pose_to_lcm_from_list(2001, null_pose);

  // Determine moving contact points in stationary foot's sole frame:
  // I define the sole frame as a frame directly below the foot frame on the sole
  pronto::PointCloud* cp_moving(new pronto::PointCloud ());
  Eigen::Isometry3d foot_to_foot =  primary_foot.inverse() * secondary_foot * foot_to_sole_;
  pc_vis_->transformPointCloud(*contact_points_, *cp_moving, Eigen::Affine3f ( foot_to_foot.cast<float>() ) );
  pc_vis_->ptcld_to_lcm_from_list(2004, *cp_moving, utime, utime); // this is visualised relative to 0,0,0
  
  if (cp_moving_prev_->points.size() != 4){
    std::cout << "Previous contact points not four - we have a problem\n";
    success = false;
  }else{

    int n_points_in_contact = 0;
    for (size_t i=0; i < 4 ; i++){
      pronto::Point cp = cp_moving->points[i];
      pronto::Point cp_prev = cp_moving_prev_->points[i];

      double raise = fabs(cp.z);

      if ( raise < 0.02){
        n_points_in_contact++;
        //std::cout <<utime << " "<< raise << " " << (int)i << " cp in contact\n";
      }else{
        //std::cout <<utime << " "<< raise << " " << (int)i << " cp NOT in contact\n";
      }
    }

    if (n_points_in_contact >0){
      std::cout << utime << " " << n_points_in_contact << "\n";
    }



    success = true;
  }

  cp_moving_prev_ = cp_moving;
  

  // determine the velocity of the SF CPs onto the PFCP plane
  // infer the time to contact by differencing

  // If the distance of the foot to the plane is less than a certain amount
  // and the time to contact is less than a certain amount
  // then contact is likely
}

  void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
    VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
    VertexSE3 *to   = static_cast<VertexSE3*>(_vertices[1]);

    Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
    
    if (from_.count(from) > 0) {
      to->setEstimate(from->estimate() * virtualMeasurement);
    } else
      from->setEstimate(to->estimate() * virtualMeasurement.inverse());
  }

const Eigen::Matrix3d CMiniVisionToolbox::getEssential( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
    //ds compute essential matrix: http://en.wikipedia.org/wiki/Essential_matrix TODO check math!
    const Eigen::Isometry3d matTransformation( p_matTransformationTo.inverse( )*p_matTransformationFrom );
    //const Eigen::Isometry3d matTransformation( p_matTransformationFrom.inverse( )*p_matTransformationTo );
    //const Eigen::Isometry3d matTransformation( p_matTransformationTo*p_matTransformationFrom.inverse( ) );
    const Eigen::Matrix3d matSkewTranslation( CMiniVisionToolbox::getSkew( matTransformation.translation( ) ) );

    //ds compute essential matrix
    return matTransformation.linear( )*matSkewTranslation;
}

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

double BodyNode::TransformObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
  assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());

  // Update forward kinematics information with _x
  // We are just insterested in transformation of mBodyNode
  mBodyNode->getParentJoint()->setConfigs(_x, true, false, false);

  // Compute and return the geometric distance between body node transformation
  // and target transformation
  Eigen::Isometry3d bodyT = mBodyNode->getWorldTransform();
  Eigen::Vector6d dist = math::logMap(bodyT.inverse() * mT);
  return dist.dot(dist);
}

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

TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS)
{
    int numTest = 100;

    // AdT(V) == T * V * InvT
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector6d V = Eigen::Vector6d::Random();

        Eigen::Vector6d AdTV = AdT(T, V);

        // Ad(T, V) = T * [V] * InvT
        Eigen::Matrix4d T_V_InvT = T.matrix() * toMatrixForm(V) * T.inverse().matrix();
        Eigen::Vector6d T_V_InvT_se3 = fromMatrixForm(T_V_InvT);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), T_V_InvT_se3(j), LIE_GROUP_OPT_TOL);

        // Ad(T, V) = [R 0; [p]R R] * V
        Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
        AdTMatrix.topLeftCorner<3,3>() = T.linear();
        AdTMatrix.bottomRightCorner<3,3>() = T.linear();
        AdTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(T.translation()) * T.linear();
        Eigen::Vector6d AdTMatrix_V = AdTMatrix * V;
        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTMatrix_V(j), LIE_GROUP_OPT_TOL);
    }

    // AdR == AdT([R 0; 0 1], V)
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
        R = T.linear();
        Eigen::Vector6d V = Eigen::Vector6d::Random();

        Eigen::Vector6d AdTV = AdT(R, V);
        Eigen::Vector6d AdRV = AdR(T, V);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdRV(j), LIE_GROUP_OPT_TOL);
    }

    // AdTAngular == AdT(T, se3(w, 0))
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d w = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.head<3>() = w;

        Eigen::Vector6d AdTV = AdT(T, V);
        Eigen::Vector6d AdTAng = AdTAngular(T, w);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTAng(j), LIE_GROUP_OPT_TOL);
    }

    // AdTLinear == AdT(T, se3(w, 0))
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d v = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.tail<3>() = v;

        Eigen::Vector6d AdTV = AdT(T, V);
        Eigen::Vector6d AdTLin = AdTLinear(T, v);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
    }

    // AdTJac
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Vector3d v = Eigen::Vector3d::Random();
        Eigen::Vector6d V = Eigen::Vector6d::Zero();
        V.tail<3>() = v;

        Eigen::Vector6d AdTV = AdT(T, V);
        Eigen::Vector6d AdTLin = AdTLinear(T, v);

        for (int j = 0; j < 6; ++j)
            EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
    }

    // AdInvT
    for (int i = 0; i < numTest; ++i)
    {
        Eigen::Vector6d t = Eigen::Vector6d::Random();
        Eigen::Isometry3d T = math::expMap(t);
        Eigen::Isometry3d InvT = T.inverse();
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0,
                         const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1,
                         std::vector<Contact>* result)
{
  Eigen::Vector3d normal = T1.linear() * plane_normal;
  Eigen::Vector3d Rx = T0.linear().rightCols(1);
  Eigen::Vector3d Ry = normal - normal.dot(Rx) * Rx;
  double mag = Ry.norm();
  Ry.normalize();
  if (mag < DART_COLLISION_EPS)
  {
    if (std::abs(Rx[2]) > 1.0 - DART_COLLISION_EPS)
      Ry = Eigen::Vector3d::UnitX();
    else
      Ry = (Eigen::Vector3d(Rx[1], -Rx[0], 0.0)).normalized();
  }

  Eigen::Vector3d Rz = Rx.cross(Ry);
  Eigen::Isometry3d T;
  T.linear().col(0) = Rx;
  T.linear().col(1) = Ry;
  T.linear().col(2) = Rz;
  T.translation() = T0.translation();

  Eigen::Vector3d nn = T.linear().transpose() * normal;
  Eigen::Vector3d pn = T.inverse() * T1.translation();

  // four corners c0 = ( -h/2, -r ), c1 = ( +h/2, -r ), c2 = ( +h/2, +r ), c3 = ( -h/2, +r )
  Eigen::Vector3d c[4] = {
    Eigen::Vector3d(-half_height, -cyl_rad, 0.0),
    Eigen::Vector3d(+half_height, -cyl_rad, 0.0),
    Eigen::Vector3d(+half_height, +cyl_rad, 0.0),
    Eigen::Vector3d(-half_height, +cyl_rad, 0.0) };

  double depth[4] = { (pn - c[0]).dot(nn), (pn - c[1]).dot(nn), (pn - c[2]).dot(nn), (pn - c[3]).dot(nn) };

  double penetration = -1.0;
  int found = -1;
  for (int i = 0; i < 4; i++)
  {
    if (depth[i] > penetration)
    {
      penetration = depth[i];
      found = i;
    }
  }

  Eigen::Vector3d point;

  if (std::abs(depth[found] - depth[(found+1)%4]) < DART_COLLISION_EPS)
    point = T * (0.5 * (c[found] + c[(found+1)%4]));
  else if (std::abs(depth[found] - depth[(found+3)%4]) < DART_COLLISION_EPS)
    point = T * (0.5 * (c[found] + c[(found+3)%4]));
  else
    point = T * c[found];

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

  return 0;
}

int collideCylinderSphere(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0,
                          const double& sphere_rad, const Eigen::Isometry3d& T1,
                          std::vector<Contact>* result)
{
  Eigen::Vector3d center = T0.inverse() * T1.translation();

  double dist = sqrt(center[0] * center[0] + center[1] * center[1]);

  if ( dist < cyl_rad && std::abs(center[2]) < half_height + sphere_rad )
  {
    Contact contact;
    contact.penetrationDepth = 0.5 * (half_height + sphere_rad - math::sgn(center[2]) * center[2]);
    contact.point = T0 * Eigen::Vector3d(center[0], center[1], half_height - contact.penetrationDepth);
    contact.normal = T0.linear() * Eigen::Vector3d(0.0, 0.0, math::sgn(center[2]));
    result->push_back(contact);
    return 1;
  }
  else
  {
    double penetration = 0.5 * (cyl_rad + sphere_rad - dist);
    if ( penetration > 0.0 )
    {
      if ( std::abs(center[2]) > half_height )
      {
        Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0).normalized());
        point *= cyl_rad;
        point[2] = math::sgn(center[2]) * half_height;
        Eigen::Vector3d normal = point - center;
        penetration = sphere_rad - normal.norm();
        normal = (T0.linear() * normal).normalized();
        point = T0 * point;

        if (penetration > 0.0)
        {
          Contact contact;
          contact.point = point;
          contact.normal = normal;
          contact.penetrationDepth = penetration;
          result->push_back(contact);
          return 1;
        }
      }
      else // if( center[2] >= -half_height && center[2] <= half_height )
      {
        Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0)).normalized();
        Eigen::Vector3d normal = -(T0.linear() * point);
        point *= (cyl_rad - penetration);
        point[2] = center[2];
        point = T0 * point;

        Contact contact;
        contact.point = point;
        contact.normal = normal;
        contact.penetrationDepth = penetration;
        result->push_back(contact);
        return 1;
      }
    }
  }
  return 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;
}

int	collideBoxSphere(CollisionObject* o1, CollisionObject* o2,
                     const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
                     const double& r1, const Eigen::Isometry3d& T1,
                     CollisionResult& result)
{
  Eigen::Vector3d halfSize = 0.5 * size0;
  bool inside_box = true;

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

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

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

  if (p[2] < -halfSize[2]) { p[2] = -halfSize[2]; inside_box = false; }
  if (p[2] >  halfSize[2]) { p[2] =  halfSize[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 = halfSize[0] - std::abs(p[0]);
    double tmin = halfSize[1] - std::abs(p[1]);
    int idx = 0;

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

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

    Contact contact;
    contact.collisionObject1 = o1;
    contact.collisionObject2 = o2;
    contact.point = c0;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result.addContact(contact);
    return 1;
  }

  Eigen::Vector3d contactpt = T0 * p;
  //normal = c0 - contactpt;
  normal = contactpt - c0;
  double mag = normal.norm();
  penetration = r1 - mag;

  if (penetration < 0.0)
  {
    return 0;
  }

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

    Contact contact;
    contact.collisionObject1 = o1;
    contact.collisionObject2 = o2;
    contact.point = contactpt;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result.addContact(contact);
  }
  else
  {
    double min = halfSize[0] - std::abs(p[0]);
    double tmin = halfSize[1] - std::abs(p[1]);
    int idx = 0;

    if ( tmin < min )
    {
      min = tmin;
      idx = 1;
    }
    tmin = halfSize[2] - std::abs(p[2]);
    if ( tmin < min )
    {
      min = tmin;
      idx = 2;
    }
//.........这里部分代码省略.........

void writeQueue() {
  SensorData* data;
  std::ofstream ofG2O(&filename[0]);
  geometry_msgs::TransformStamped msg;
  int num = 0;

  
  // this is the vertex where we are packing the data
  g2o::VertexSE3* activeVertex = 0;
  // this is the timestamp of the first measurement added to the vertex
  double activeVertexTime=0;

  // this is the previous vertex
  g2o::VertexSE3* previousVertex = 0;
  // this is the timestamp of the first measurement added to the previous vertex
  double previousVertexTime=0;

  // the last position of the robot (not of the vertex, needed to measure the distances)
  Eigen::Isometry3d lastRobotPose;

  // set of sensors we packed in the current data. 
  // We do not want to put 10 camera images of the same camera in the same vertex.
  std::set<Sensor*> addedSensors;

  Eigen::Vector2d distances(0.,0);
  while (true)
  {
    if(! _queue.empty())
    {
      data = (SensorData*)_queue.front();
      double timeNow = _queue.lastElementTime();
      conditionalPrint(annoyingLevel) <<  "size=" << _queue.size() << " lastTime=" << FIXED(timeNow) << endl;
      if (timeNow - data->timeStamp()> initialDelay)
      { // we have enough stuff in the queue
	_queue.pop_front();
	if (! nptr->ok())
	  continue;
	
	tf::StampedTransform transform;
	bool we_got_transf = false;
	try
	{
	  ros::Time timeStamp;
	  // Get transformation
	  (*tfListener).lookupTransform("/odom", "/base_link", timeStamp.fromSec(data->timeStamp()), transform);
	  we_got_transf = true;
	}
	catch (tf::TransformException & ex)
	{
	  ROS_ERROR("%s", ex.what());
	}

	if (! we_got_transf)
	  continue;
				
	Eigen::Isometry3d currentRobotPose = fromStampedTransform(transform);
	double currentDataTime = data->timeStamp();
	distances += isometry2distance(lastRobotPose.inverse()*currentRobotPose);
	double passedTime = currentDataTime-previousVertexTime;
	lastRobotPose = currentRobotPose;

	conditionalPrint(annoyingLevel) << "distances: " << distances[0] << " " << distances[1] <<  " " << passedTime << endl;
	if (distances[0] < minDistances[0] && 
	    distances[1] < minDistances[1] && 
	    passedTime   < minTime){
	  conditionalPrint(annoyingLevel) << "reject: (time/distance)" << endl;
	  // SKIP THE FRAME
	  delete data;
	  data = 0;
	  continue;
	} 

	if (!activeVertex) {
	  activeVertex = new g2o::VertexSE3();
	  activeVertex->setId(num);
	  activeVertex->setEstimate(fromStampedTransform(transform));
	  activeVertexTime = currentDataTime;
	}
						
	Sensor* sensor = data->sensor();
	assert (sensor && "!");
						
	// check if we already packed the data for this kind of sensor
	if (addedSensors.count(sensor)){
	  conditionalPrint(annoyingLevel) << "reject: (sensor) "<< endl;
	  delete data;
	} else {
	  addedSensors.insert(sensor);
	  Parameter* parameter = sensor->parameter();
	  assert (parameter && "[email protected]#");
	  //data->writeOut(filename);
	  if (! graph->parameters().getParameter(parameter->id())){
	    graph->parameters().addParameter(parameter);
	    graph->saveParameter(ofG2O, parameter);
	  }
					
	  activeVertex->addUserData(data);
	  data->setDataContainer(activeVertex);
	}
	// detach the data from the thing
//.........这里部分代码省略.........