C++ (Cpp) MapPoint::GetNormalの例

コード例 #1

ファイル: StereoFrame.cpp プロジェクト: snooble/sptam

bool StereoFrame::canView(const MapPoint& mapPoint) const
{
  boost::shared_lock<boost::shared_mutex> lock(stereo_frames_mutex_);

  const Eigen::Vector3d& point = mapPoint.GetPosition();

  // compute only once, since both cameras have the same orientation.
  bool similar_angle = false;
  {
    Eigen::Vector3d currentNormal = point - frameLeft_.GetPosition();
    currentNormal.normalize();

    // angle is in radians
    double angle = std::acos( ( mapPoint.GetNormal() ).dot( currentNormal ) );

    // Discard Points which were created from a greater 45 degrees pint of view.
    // TODO: pass this threshold as a parameter.
    similar_angle = angle < (M_PI / 4.0);
  }

  return similar_angle and ( frameLeft_.GetCamera().CanView( point ) or frameRight_.GetCamera().CanView( point ) );
}