C++ (Cpp) setEstimateの例

コード例 #1

ファイル: types_seven_dof_expmap.cpp プロジェクト: doomzzju/test_calib

  bool VertexSim3Expmap::read(std::istream& is)
  {
    Vector7d cam2world;
    for (int i=0; i<6; i++){
      is >> cam2world[i];
    }
    is >> cam2world[6];
//    if (! is) {
//      // if the scale is not specified we set it to 1;
//      std::cerr << "!s";
//      cam2world[6]=0.;
//    }

    for (int i=0; i<2; i++)
    {
      is >> _focal_length1[i];
    }
    for (int i=0; i<2; i++)
    {
      is >> _principle_point1[i];
    }

    setEstimate(Sim3(cam2world).inverse());
    return true;
  }

コード例 #2

ファイル: leaf_blueprints.cpp プロジェクト: songhtdo/vespa

SimpleBlueprint::SimpleBlueprint(const SimpleResult &result)
    : SimpleLeafBlueprint(FieldSpecBaseList()),
      _tag(),
      _result(result)
{
    setEstimate(HitEstimate(result.getHitCount(), (result.getHitCount() == 0)));
}

コード例 #3

ファイル: vertex_se2.cpp プロジェクト: Aerobota/c2tam

 bool VertexSE2::read(std::istream& is)
 {
   Eigen::Vector3d p;
   is >> p[0] >> p[1] >> p[2];
   setEstimate(p);
   return true;
 }

コード例 #4

ファイル: HomogeneousPointParameterBlock.cpp プロジェクト: AhmadZakaria/okvis

// Constructor with estimate and time.
HomogeneousPointParameterBlock::HomogeneousPointParameterBlock(
    const Eigen::Vector3d& point, uint64_t id, bool initialized) {
  setEstimate(Eigen::Vector4d(point[0], point[1], point[2], 1.0));
  setId(id);
  setInitialized(initialized);
  setFixed(false);
}

コード例 #5

ファイル: vertex_se3.cpp プロジェクト: PianoCat/ORB_SLAM_iOS

 bool VertexSE3::read(std::istream& is)
 {
   Vector7d est;
   for (int i=0; i<7; i++)
     is  >> est[i];
   setEstimate(internal::fromVectorQT(est));
   return true;
 }

コード例 #6

ファイル: types_six_dof_expmap.cpp プロジェクト: Roboterbastler/ORB_SLAM

bool VertexSE3Expmap::read(std::istream& is) {
    Vector7d est;
    for (int i=0; i<7; i++)
        is  >> est[i];
    SE3Quat cam2world;
    cam2world.fromVector(est);
    setEstimate(cam2world.inverse());
    return true;
}

コード例 #7

ファイル: leaf_blueprints.cpp プロジェクト: songhtdo/vespa

FakeBlueprint::FakeBlueprint(const FieldSpec &field, const FakeResult &result)
    : SimpleLeafBlueprint(field),
      _tag("<tag>"),
      _term("<term>"),
      _field(field),
      _result(result),
      _is_attr(false)
{
    setEstimate(HitEstimate(result.inspect().size(), result.inspect().empty()));
}

コード例 #8

ファイル: vertex_se3_quat.cpp プロジェクト: rmihalyi/g2o

  bool VertexSE3::read(std::istream& is)
  {

    Vector7d est;
    for (int i=0; i<7; i++)
      is  >> est[i];
    setEstimate(SE3Quat(est));
    updateCache();
    return true;
  }

コード例 #9

ファイル: anchored_points.cpp プロジェクト: asimay/ScaViSLAM_ros

bool G2oVertexPointXYZ
::read(std::istream& is)
{
  Vector3d lv;
  for (int i=0; i<3; i++)
  {
    is >> lv[i];
  }
  setEstimate(lv);
  return true;
}

コード例 #10

ファイル: anchored_points.cpp プロジェクト: asimay/ScaViSLAM_ros

//TODO: implement, but first remove camera parameters from vertex state
bool G2oVertexSE3
::read(std::istream& is)
{
  std::cout << "read G2oVertexSE3 called" << "\n";
  Vector3d vec;
  for(int i=0; i<3; i++) is >> vec[i];

  double q_in [4];
  for(int i=0; i<4; i++) is >> q_in[i];
  Quaterniond q(q_in[0], q_in[1], q_in[2], q_in[3]);

  Sophus::SE3 se3(q, vec);
  setEstimate(se3);
  return true;
}

コード例 #11

ファイル: AnchoredRectangleHandlerBootstrap.cpp プロジェクト: AIRLab-POLIMI/C-SLAM

void AnchoredRectangleHandlerBootstrap::fixImmutableFeaturePoses(
    const Eigen::VectorXd &pose, double percentageThreshold,
    double minZDistance) {
  map<double, unsigned int> candidates;

  for (auto& feature : _objects) {
    auto& map = feature.second.zHistory;
    voteFixedPoseCandidates(candidates, map, minZDistance);
  }

  for (auto& candidate : candidates) {
    double percentage = static_cast<double>(candidate.second)
        / static_cast<double>(_objects.size());

    if (percentage >= percentageThreshold) {
      auto pose_ptr = _filter->getNearestPoseByTimestamp(candidate.first);
      pose_ptr->setEstimate(pose);
      pose_ptr->setFixed(true);
    }
  }

}

コード例 #12

ファイル: anchored_points.cpp プロジェクト: asimay/ScaViSLAM_ros

void G2oVertexSE3
::oplusImpl(const double * update_p)
{
  Map<const Vector6d> update(update_p);
  setEstimate(SE3::exp(update)*estimate());
}