C++ (Cpp) VertexSet::size Examples

Example #1

 void EdgePointXYCovPointXYCov::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to)
 {
   assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXYCov");
   VertexPointXYCov* vi     = static_cast<VertexPointXYCov*>(_vertices[0]);
   VertexPointXYCov* vj = static_cast<VertexPointXYCov*>(_vertices[1]);
   if (from.count(vi) > 0 && to == vj) {
     vj->estimate() = vi->estimate(); // * _measurement;
   }
 }

Example #2

File: edge_se2_pointxy_calib.cpp Project: 2maz/g2o

  void EdgeSE2PointXYCalib::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
  {
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXY");

    if (from.count(_vertices[0]) != 1)
      return;
    VertexSE2* vi     = static_cast<VertexSE2*>(_vertices[0]);
    VertexPointXY* vj = static_cast<VertexPointXY*>(_vertices[1]);
    vj->setEstimate(vi->estimate() * _measurement);
  }

Example #3

File: edge_se3_pointxyz_depth.cpp Project: 2maz/g2o

  void EdgeSE3PointXYZDepth::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");

    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
    const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
    Eigen::Vector3d p;
    p(2) = _measurement(2);
    p.head<2>() = _measurement.head<2>()*p(2);
    p=invKcam*p;
    point->setEstimate(cam->estimate() * (params->offsetMatrix() * p));
  }

Example #4

File: edge_se2_pointxy_bearing.cpp Project: arunsg/g2o

  void EdgeSE2PointXYBearing::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
  {
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXY");

    if (from.count(_vertices[0]) != 1)
      return;
    double r=2.;
    const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]);
    VertexPointXY* l2 = static_cast<VertexPointXY*>(_vertices[1]);
    SE2 t=v1->estimate();
    t.setRotation(t.rotation().angle()+_measurement);
    Vector2d vr;
    vr[0]=r; vr[1]=0;
    l2->setEstimate(t*vr);
  }

Example #5

File: edge_se3_pointxyz.cpp Project: AlexandrGraschenkov/ORB_SLAM

  void EdgeSE3PointXYZ::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to_*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");

    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);
    // SE3OffsetCache* vcache = (SE3OffsetCache* ) cam->getCache(_cacheIds[0]);
    // if (! vcache){
    //   cerr << "fatal error in retrieving cache" << endl;
    // }
    // SE3OffsetParameters* params=vcache->params;
    Eigen::Vector3d p=_measurement;
    point->setEstimate(cam->estimate() * (offsetParam->offset() * p));
  }

Example #6

File: edge_se3_pointxyz_disparity.cpp Project: mikejmills/g2o

  void EdgeSE3PointXYZDisparity::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
  {
    (void) from;
    assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexDepthCam position by VertexTrackXYZ");
    VertexSE3 *cam = dynamic_cast<VertexSE3*>(_vertices[0]);
    VertexPointXYZ *point = dynamic_cast<VertexPointXYZ*>(_vertices[1]);

    // VertexCameraCache* vcache = (VertexCameraCache* ) cam->getCache(_cacheIds[0]);
    // if (! vcache){
    //   cerr << "fatal error in retrieving cache" << endl;
    // }
    //ParameterCamera* params=vcache->params;
    const Eigen::Matrix<double, 3, 3>& invKcam = params->invKcam();
    Eigen::Vector3d p;
    double w=1./_measurement(2);
    p.head<2>() = _measurement.head<2>()*w;
    p(2) = w;
    p = invKcam * p;
    p = cam->estimate() * (params->offset() * p);
    point->setEstimate(p);
  }