<commit_before>/*
 * Copyright (C) 2017 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "src/base/test/vm_test_utils.h"

#include "perfetto/base/build_config.h"
#include "perfetto/ext/base/utils.h"

#include <memory>

#include <errno.h>
#include <string.h>

#if PERFETTO_BUILDFLAG(PERFETTO_OS_WIN)
#include <Windows.h>
#include <Psapi.h>
#else
#include <sys/mman.h>
#include <sys/stat.h>
#endif

#include "perfetto/base/build_config.h"
#include "perfetto/base/logging.h"

namespace perfetto {
namespace base {
namespace vm_test_utils {

bool IsMapped(void* start, size_t size) {
  PERFETTO_CHECK(size % kPageSize == 0);
#if PERFETTO_BUILDFLAG(PERFETTO_OS_WIN)
  int retries = 5;
  int number_of_entries = 4000;  // Just a guess.
  PSAPI_WORKING_SET_INFORMATION* ws_info = nullptr;

  std::vector<char> buffer;
  for (;;) {
    size_t buffer_size =
      sizeof(PSAPI_WORKING_SET_INFORMATION) +
      (number_of_entries * sizeof(PSAPI_WORKING_SET_BLOCK));

    buffer.resize(buffer_size);
    ws_info = reinterpret_cast<PSAPI_WORKING_SET_INFORMATION*>(&buffer[0]);

    // On success, |buffer_| is populated with info about the working set of
    // |process|. On ERROR_BAD_LENGTH failure, increase the size of the
    // buffer and try again.
    if (QueryWorkingSet(GetCurrentProcess(), &buffer[0], buffer_size))
      break;  // Success

    PERFETTO_CHECK(GetLastError() == ERROR_BAD_LENGTH);

    number_of_entries = ws_info->NumberOfEntries;

    // Maybe some entries are being added right now. Increase the buffer to
    // take that into account. Increasing by 10% should generally be enough.
    number_of_entries *= 1.1;

    PERFETTO_CHECK(--retries > 0);  // If we're looping, eventually fail.
  }

  void* end = reinterpret_cast<char*>(start) + size;
  // Now scan the working-set information looking for the addresses.
  unsigned pages_found = 0;
  for (unsigned i = 0; i < ws_info->NumberOfEntries; ++i) {
    void* address =
        reinterpret_cast<void*>(ws_info->WorkingSetInfo[i].VirtualPage *
        kPageSize);
    if (address >= start && address < end)
      ++pages_found;
  }

  if (pages_found * kPageSize == size)
    return true;
  return false;
#elif PERFETTO_BUILDFLAG(PERFETTO_OS_FUCHSIA)
  // Fuchsia doesn't yet support paging (b/119503290).
  return true;
#else
#if PERFETTO_BUILDFLAG(PERFETTO_OS_MACOSX)
  using PageState = char;
  static constexpr PageState kIncoreMask = MINCORE_INCORE;
#else
  using PageState = unsigned char;
  static constexpr PageState kIncoreMask = 1;
#endif
  const size_t num_pages = size / kPageSize;
  std::unique_ptr<PageState[]> page_states(new PageState[num_pages]);
  memset(page_states.get(), 0, num_pages * sizeof(PageState));
  int res = mincore(start, size, page_states.get());
  // Linux returns ENOMEM when an unmapped memory range is passed.
  // MacOS instead returns 0 but leaves the page_states empty.
  if (res == -1 && errno == ENOMEM)
    return false;
  PERFETTO_CHECK(res == 0);
  for (size_t i = 0; i < num_pages; i++) {
    if (!(page_states[i] & kIncoreMask))
      return false;
  }
  return true;
#endif
}

}  // namespace vm_test_utils
}  // namespace base
}  // namespace perfetto
<commit_msg>base: Fix windows build am: 65c898e6e2 am: 58ee913c1b am: 1ad8925c7b am: 4f441f1176<commit_after>/*
 * Copyright (C) 2017 The Android Open Source Project
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "src/base/test/vm_test_utils.h"

#include "perfetto/base/build_config.h"
#include "perfetto/ext/base/utils.h"

#include <memory>

#include <errno.h>
#include <string.h>

#if PERFETTO_BUILDFLAG(PERFETTO_OS_WIN)
#include <vector>

#include <Windows.h>
#include <Psapi.h>
#else  // PERFETTO_BUILDFLAG(PERFETTO_OS_WIN)
#include <sys/mman.h>
#include <sys/stat.h>
#endif  // PERFETTO_BUILDFLAG(PERFETTO_OS_WIN)

#include "perfetto/base/build_config.h"
#include "perfetto/base/logging.h"

namespace perfetto {
namespace base {
namespace vm_test_utils {

bool IsMapped(void* start, size_t size) {
  PERFETTO_CHECK(size % kPageSize == 0);
#if PERFETTO_BUILDFLAG(PERFETTO_OS_WIN)
  int retries = 5;
  int number_of_entries = 4000;  // Just a guess.
  PSAPI_WORKING_SET_INFORMATION* ws_info = nullptr;

  std::vector<char> buffer;
  for (;;) {
    size_t buffer_size =
      sizeof(PSAPI_WORKING_SET_INFORMATION) +
      (number_of_entries * sizeof(PSAPI_WORKING_SET_BLOCK));

    buffer.resize(buffer_size);
    ws_info = reinterpret_cast<PSAPI_WORKING_SET_INFORMATION*>(&buffer[0]);

    // On success, |buffer_| is populated with info about the working set of
    // |process|. On ERROR_BAD_LENGTH failure, increase the size of the
    // buffer and try again.
    if (QueryWorkingSet(GetCurrentProcess(), &buffer[0], buffer_size))
      break;  // Success

    PERFETTO_CHECK(GetLastError() == ERROR_BAD_LENGTH);

    number_of_entries = ws_info->NumberOfEntries;

    // Maybe some entries are being added right now. Increase the buffer to
    // take that into account. Increasing by 10% should generally be enough.
    number_of_entries *= 1.1;

    PERFETTO_CHECK(--retries > 0);  // If we're looping, eventually fail.
  }

  void* end = reinterpret_cast<char*>(start) + size;
  // Now scan the working-set information looking for the addresses.
  unsigned pages_found = 0;
  for (unsigned i = 0; i < ws_info->NumberOfEntries; ++i) {
    void* address =
        reinterpret_cast<void*>(ws_info->WorkingSetInfo[i].VirtualPage *
        kPageSize);
    if (address >= start && address < end)
      ++pages_found;
  }

  if (pages_found * kPageSize == size)
    return true;
  return false;
#elif PERFETTO_BUILDFLAG(PERFETTO_OS_FUCHSIA)
  // Fuchsia doesn't yet support paging (b/119503290).
  return true;
#else
#if PERFETTO_BUILDFLAG(PERFETTO_OS_MACOSX)
  using PageState = char;
  static constexpr PageState kIncoreMask = MINCORE_INCORE;
#else
  using PageState = unsigned char;
  static constexpr PageState kIncoreMask = 1;
#endif
  const size_t num_pages = size / kPageSize;
  std::unique_ptr<PageState[]> page_states(new PageState[num_pages]);
  memset(page_states.get(), 0, num_pages * sizeof(PageState));
  int res = mincore(start, size, page_states.get());
  // Linux returns ENOMEM when an unmapped memory range is passed.
  // MacOS instead returns 0 but leaves the page_states empty.
  if (res == -1 && errno == ENOMEM)
    return false;
  PERFETTO_CHECK(res == 0);
  for (size_t i = 0; i < num_pages; i++) {
    if (!(page_states[i] & kIncoreMask))
      return false;
  }
  return true;
#endif
}

}  // namespace vm_test_utils
}  // namespace base
}  // namespace perfetto
<|endoftext|>


<commit_before>/*
 * File:   ScanLineEdgelDetector.cpp
 * Author: claas
 * Author: Heinrich Mellmann
 *
 * Created on 14. march 2011, 14:22
 */

#include "ScanLineEdgelDetector.h"

// debug
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugImageDrawings.h"
#include "Tools/Debug/DebugDrawings.h"
#include "Tools/Debug/Stopwatch.h"

// tools
#include "Tools/ImageProcessing/BresenhamLineScan.h"
#include "Tools/CameraGeometry.h"

ScanLineEdgelDetector::ScanLineEdgelDetector()
:
  cameraID(CameraInfo::Top)
{
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:scanlines", "mark the scan lines", false);
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:mark_edgels", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:mark_double_edgels", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:mark_endpoints", "mark the endpints on the image", false);
}

ScanLineEdgelDetector::~ScanLineEdgelDetector()
{}

void ScanLineEdgelDetector::execute(CameraInfo::CameraID id)
{
  cameraID = id;
  CANVAS_PX(cameraID);
  getScanLineEdgelPercept().reset();

  // TODO: implement a general validation for timestamps
  if(getBodyContour().timestamp != getFrameInfo().getTime()) {
    return;
  }

  int horizon_height = (int)(std::min(getArtificialHorizon().begin().y, getArtificialHorizon().end().y)+0.5);
  // clamp it to the image
  horizon_height = Math::clamp(horizon_height,0,(int)getImage().height());

  // scan only inside the estimated field region
  //horizon_height = getFieldPerceptRaw().getValidField().points[0].y;

  // horizontal stepsize between the scanlines
  int step = (getImage().width() - 1) / (theParameters.scanline_count - 1);

  // don't scan the lower lines in the image
  int borderY = getImage().height() - theParameters.pixel_border_y - 1;

  // start and endpoints for the scanlines
  Vector2i start(step / 2, borderY);
  Vector2i end(step / 2, horizon_height );

  for (int i = 0; i < theParameters.scanline_count - 1; i++)
  {
    ASSERT(getImage().isInside(start.x, start.y));
    // don't scan the own body
    start = getBodyContour().getFirstFreeCell(start);

    // execute the scan
    ScanLineEdgelPercept::EndPoint endPoint = scanForEdgels(i, start, end);

    // check if endpoint is not same as the begin of the scanline
    if(endPoint.posInImage == start) {
      endPoint.color = ColorClasses::none;
      endPoint.posInImage.y = borderY;
    }

    // try to project it on the ground
    endPoint.valid = CameraGeometry::imagePixelToFieldCoord(
      getCameraMatrix(),
      getCameraInfo(),
      endPoint.posInImage.x,
      endPoint.posInImage.y,
      0.0,
      endPoint.posOnField);

    //
    getScanLineEdgelPercept().endPoints.push_back(endPoint);

    start.y = borderY;
    start.x += step;
    end.x = start.x;
  }//end for

  DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:mark_edgels",
    for(size_t i = 0; i < getScanLineEdgelPercept().edgels.size(); i++) {
      const Edgel& edgel = getScanLineEdgelPercept().edgels[i];
      LINE_PX(ColorClasses::black,edgel.point.x, edgel.point.y, edgel.point.x + (int)(edgel.direction.x*10), edgel.point.y + (int)(edgel.direction.y*10));
    }
  );

  // mark finished valid edgels
  DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:mark_double_edgels",
    for(size_t i = 0; i < getScanLineEdgelPercept().pairs.size(); i++)
    {
      const ScanLineEdgelPercept::EdgelPair& pair = getScanLineEdgelPercept().pairs[i];
      CIRCLE_PX(ColorClasses::black, (int)pair.point.x, (int)pair.point.y, 3);
      LINE_PX(ColorClasses::red   ,(int)pair.point.x, (int)pair.point.y, (int)(pair.point.x + pair.direction.x*10), (int)(pair.point.y + pair.direction.y*10));
    }
  );

  DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:mark_endpoints",
    for(size_t i = 0; i < getScanLineEdgelPercept().endPoints.size(); i++)
    {
      const ScanLineEdgelPercept::EndPoint& point_two = getScanLineEdgelPercept().endPoints[i];
      CIRCLE_PX(point_two.color, point_two.posInImage.x, point_two.posInImage.y, 5);

      if(i > 0) {
        const ScanLineEdgelPercept::EndPoint& point_one = getScanLineEdgelPercept().endPoints[i-1];
        LINE_PX(point_one.color,
                point_one.posInImage.x, point_one.posInImage.y,
                point_two.posInImage.x, point_two.posInImage.y);
      }
    }
  );
}//end execute

ScanLineEdgelPercept::EndPoint ScanLineEdgelDetector::scanForEdgels(int scan_id, const Vector2i& start, const Vector2i& end)
{
  ScanLineEdgelPercept::EndPoint endPoint;
  endPoint.posInImage = start;
  endPoint.color = ColorClasses::none;
  endPoint.ScanLineID = scan_id;

  const int step = 2;

  // no scan if the start is at the top of the image
  if(start.y <= step) {
    return endPoint;
  }

  Vector2i point(start);
  point.y -= step; // make one step

  Vector2i last_down_point(point); // needed for the endpoint
  bool begin_found = false;

  // calculate the threashold
  int t_edge = theParameters.brightness_threshold * 2;
  // HACK (TEST): make it dependend on the angle of the camera in the future
  if(cameraID == CameraInfo::Bottom) {
    t_edge *= 4;
  }

  // initialize the scanner
  Vector2i peak_point_max(point);
  Vector2i peak_point_min(point);
  MaximumScan<int,int> positiveScan(peak_point_max.y, t_edge);
  MaximumScan<int,int> negativeScan(peak_point_min.y, t_edge);

  int f_last = getImage().getY(point.x, point.y); // scan the first point
  // just go up
  for(;point.y >= end.y + step; point.y -= step)
  {
    // get the brightness chanel
    int f_y = getImage().getY(point.x, point.y);
    int g = f_y - f_last;
    f_last = f_y;

    // begin found
    if(positiveScan.addValue(point.y+1, g))
    {
      // refine the position of the peak
      int f_2 = getImage().getY(point.x, peak_point_max.y-2);
      int f0  = getImage().getY(point.x, peak_point_max.y);
      int f2  = getImage().getY(point.x, peak_point_max.y+2);

      if(f_2-f0 > positiveScan.maxValue()) peak_point_max.y -= 1;
      if(f0 -f2 > positiveScan.maxValue()) peak_point_max.y += 1;

      if(estimateColorOfSegment(last_down_point, peak_point_max) == ColorClasses::green) {
        endPoint.color = ColorClasses::green;
        endPoint.posInImage = peak_point_max;
      }

      add_edgel(peak_point_max);

      begin_found = true;
      last_down_point.y = peak_point_max.y;
    }//end if

    // end found
    if(negativeScan.addValue(point.y+1, -g))
    {
      // refine the position of the peak
      int f_2 = getImage().getY(point.x, peak_point_min.y-2);
      int f0  = getImage().getY(point.x, peak_point_min.y);
      int f2  = getImage().getY(point.x, peak_point_min.y+2);

      if(f_2-f0 < negativeScan.maxValue()) peak_point_min.y -= 1;
      if(f0 -f2 < negativeScan.maxValue()) peak_point_min.y += 1;

      if(estimateColorOfSegment(last_down_point, peak_point_min) == ColorClasses::green) {
        endPoint.color = ColorClasses::green;
        endPoint.posInImage = peak_point_min;
      }

      // new end edgel
      // found a new double edgel
      if(begin_found) {
        add_double_edgel(peak_point_min, scan_id);
        begin_found = false;
      } else {
        add_edgel(peak_point_min);
      }

      last_down_point.y = peak_point_min.y;
    }//end if

    DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:scanlines",
      Pixel pixel = getImage().get(point.x, point.y);
      ColorClasses::Color thisPixelColor = (getFieldColorPercept().isFieldColor(pixel.a, pixel.b, pixel.c))?ColorClasses::green:ColorClasses::none;
      POINT_PX(thisPixelColor, point.x, point.y);
    );
  }//end for

  if(point.y < end.y + step) {
    if(estimateColorOfSegment(last_down_point, end) == ColorClasses::green) {
        endPoint.color = ColorClasses::green;
        endPoint.posInImage = end;
      }
  }

  return endPoint;
}//end scanForEdgels

ColorClasses::Color ScanLineEdgelDetector::estimateColorOfSegment(const Vector2i& begin, const Vector2i& end) const
{
  ASSERT(begin.x == end.x && end.y <= begin.y);

  const int numberOfSamples = theParameters.green_sampling_points;
  int length = begin.y - end.y;
  int numberOfGreen = 0;
  Vector2i point(begin);
  Pixel pixel;

  if(numberOfSamples >= length)
  {
    for(; point.y > end.y; point.y--)
    {
      getImage().get(point.x, point.y, pixel);
      numberOfGreen += getFieldColorPercept().isFieldColor(pixel.a, pixel.b, pixel.c);
    }
  }
  else
  {
    int step = length / numberOfSamples;
    int offset = Math::random(length); // number in [0,length-1]

    for(int i = 0; i < numberOfSamples; i++)
    {
      int k = (offset + i*step) % length;
      point.y = end.y + k;
      getImage().get(point.x, point.y, pixel);
      numberOfGreen += getFieldColorPercept().isFieldColor(pixel.a, pixel.b, pixel.c);
    }
  }

  return (numberOfGreen > numberOfSamples/2) ? ColorClasses::green : ColorClasses::none;
}//end estimateColorOfSegment

Vector2d ScanLineEdgelDetector::calculateGradient(const Vector2i& point) const
{
  Vector2d gradient;

  // no angle at the border (shouldn't happen)
  if( point.x < 1 || point.x + 2 > (int)getImage().width() ||
      point.y < 1 || point.y + 2 > (int)getImage().height() ) {
    return gradient;
  }

  //apply Sobel Operator on (pointX, pointY)
  //and calculate gradient in x and y direction by that means

  gradient.x =
       getImage().getY(point.x-1, point.y+1)
    +2*getImage().getY(point.x  , point.y+1)
    +  getImage().getY(point.x+1, point.y+1)
    -  getImage().getY(point.x-1, point.y-1)
    -2*getImage().getY(point.x  , point.y-1)
    -  getImage().getY(point.x+1, point.y-1);

  gradient.y =
       getImage().getY(point.x-1, point.y-1)
    +2*getImage().getY(point.x-1, point.y  )
    +  getImage().getY(point.x-1, point.y+1)
    -  getImage().getY(point.x+1, point.y-1)
    -2*getImage().getY(point.x+1, point.y  )
    -  getImage().getY(point.x+1, point.y+1);

  //calculate the angle of the gradient
  return gradient.normalize();
}//end calculateGradient
<commit_msg>bugfix: check if start and end point are valid<commit_after>/*
 * File:   ScanLineEdgelDetector.cpp
 * Author: claas
 * Author: Heinrich Mellmann
 *
 * Created on 14. march 2011, 14:22
 */

#include "ScanLineEdgelDetector.h"

// debug
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugImageDrawings.h"
#include "Tools/Debug/DebugDrawings.h"
#include "Tools/Debug/Stopwatch.h"

// tools
#include "Tools/ImageProcessing/BresenhamLineScan.h"
#include "Tools/CameraGeometry.h"

ScanLineEdgelDetector::ScanLineEdgelDetector()
:
  cameraID(CameraInfo::Top)
{
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:scanlines", "mark the scan lines", false);
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:mark_edgels", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:mark_double_edgels", "mark the edgels on the image", false);
  DEBUG_REQUEST_REGISTER("Vision:Detectors:ScanLineEdgelDetector:mark_endpoints", "mark the endpints on the image", false);
}

ScanLineEdgelDetector::~ScanLineEdgelDetector()
{}

void ScanLineEdgelDetector::execute(CameraInfo::CameraID id)
{
  cameraID = id;
  CANVAS_PX(cameraID);
  getScanLineEdgelPercept().reset();

  // TODO: implement a general validation for timestamps
  if(getBodyContour().timestamp != getFrameInfo().getTime()) {
    return;
  }

  int horizon_height = (int)(std::min(getArtificialHorizon().begin().y, getArtificialHorizon().end().y)+0.5);
  // clamp it to the image
  horizon_height = Math::clamp(horizon_height,0,(int)getImage().height());

  // scan only inside the estimated field region
  //horizon_height = getFieldPerceptRaw().getValidField().points[0].y;

  // horizontal stepsize between the scanlines
  int step = (getImage().width() - 1) / (theParameters.scanline_count - 1);

  // don't scan the lower lines in the image
  int borderY = getImage().height() - theParameters.pixel_border_y - 1;

  // start and endpoints for the scanlines
  Vector2i start(step / 2, borderY);
  Vector2i end(step / 2, horizon_height );

  for (int i = 0; i < theParameters.scanline_count - 1; i++)
  {
    ASSERT(getImage().isInside(start.x, start.y));
    // don't scan the own body
    start = getBodyContour().getFirstFreeCell(start);

    // execute the scan
    ScanLineEdgelPercept::EndPoint endPoint = scanForEdgels(i, start, end);

    // check if endpoint is not same as the begin of the scanline
    if(endPoint.posInImage == start) {
      endPoint.color = ColorClasses::none;
      endPoint.posInImage.y = borderY;
    }

    // try to project it on the ground
    endPoint.valid = CameraGeometry::imagePixelToFieldCoord(
      getCameraMatrix(),
      getCameraInfo(),
      endPoint.posInImage.x,
      endPoint.posInImage.y,
      0.0,
      endPoint.posOnField);

    //
    getScanLineEdgelPercept().endPoints.push_back(endPoint);

    start.y = borderY;
    start.x += step;
    end.x = start.x;
  }//end for

  DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:mark_edgels",
    for(size_t i = 0; i < getScanLineEdgelPercept().edgels.size(); i++) {
      const Edgel& edgel = getScanLineEdgelPercept().edgels[i];
      LINE_PX(ColorClasses::black,edgel.point.x, edgel.point.y, edgel.point.x + (int)(edgel.direction.x*10), edgel.point.y + (int)(edgel.direction.y*10));
    }
  );

  // mark finished valid edgels
  DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:mark_double_edgels",
    for(size_t i = 0; i < getScanLineEdgelPercept().pairs.size(); i++)
    {
      const ScanLineEdgelPercept::EdgelPair& pair = getScanLineEdgelPercept().pairs[i];
      CIRCLE_PX(ColorClasses::black, (int)pair.point.x, (int)pair.point.y, 3);
      LINE_PX(ColorClasses::red   ,(int)pair.point.x, (int)pair.point.y, (int)(pair.point.x + pair.direction.x*10), (int)(pair.point.y + pair.direction.y*10));
    }
  );

  DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:mark_endpoints",
    for(size_t i = 0; i < getScanLineEdgelPercept().endPoints.size(); i++)
    {
      const ScanLineEdgelPercept::EndPoint& point_two = getScanLineEdgelPercept().endPoints[i];
      CIRCLE_PX(point_two.color, point_two.posInImage.x, point_two.posInImage.y, 5);

      if(i > 0) {
        const ScanLineEdgelPercept::EndPoint& point_one = getScanLineEdgelPercept().endPoints[i-1];
        LINE_PX(point_one.color,
                point_one.posInImage.x, point_one.posInImage.y,
                point_two.posInImage.x, point_two.posInImage.y);
      }
    }
  );
}//end execute

ScanLineEdgelPercept::EndPoint ScanLineEdgelDetector::scanForEdgels(int scan_id, const Vector2i& start, const Vector2i& end)
{
  ScanLineEdgelPercept::EndPoint endPoint;
  endPoint.posInImage = start;
  endPoint.color = ColorClasses::none;
  endPoint.ScanLineID = scan_id;

  const int step = 2;

  if(end.y + step > start.y || end.y + step >= (int)getImage().height()) {
    endPoint.posInImage.y = end.y;
    return endPoint;
  }

  // no scan if the start is at the top of the image
  if(start.y <= step) {
    return endPoint;
  }

  Vector2i point(start);
  point.y -= step; // make one step

  Vector2i last_down_point(point); // needed for the endpoint
  bool begin_found = false;

  // calculate the threashold
  int t_edge = theParameters.brightness_threshold * 2;
  // HACK (TEST): make it dependend on the angle of the camera in the future
  if(cameraID == CameraInfo::Bottom) {
    t_edge *= 4;
  }

  // initialize the scanner
  Vector2i peak_point_max(point);
  Vector2i peak_point_min(point);
  MaximumScan<int,int> positiveScan(peak_point_max.y, t_edge);
  MaximumScan<int,int> negativeScan(peak_point_min.y, t_edge);

  int f_last = getImage().getY(point.x, point.y); // scan the first point
  // just go up
  for(;point.y >= end.y + step; point.y -= step)
  {
    // get the brightness chanel
    int f_y = getImage().getY(point.x, point.y);
    int g = f_y - f_last;
    f_last = f_y;

    // begin found
    if(positiveScan.addValue(point.y+1, g))
    {
      // refine the position of the peak
      int f_2 = getImage().getY(point.x, peak_point_max.y-2);
      int f0  = getImage().getY(point.x, peak_point_max.y);
      int f2  = getImage().getY(point.x, peak_point_max.y+2);

      if(f_2-f0 > positiveScan.maxValue()) peak_point_max.y -= 1;
      if(f0 -f2 > positiveScan.maxValue()) peak_point_max.y += 1;

      if(estimateColorOfSegment(last_down_point, peak_point_max) == ColorClasses::green) {
        endPoint.color = ColorClasses::green;
        endPoint.posInImage = peak_point_max;
      }

      add_edgel(peak_point_max);

      begin_found = true;
      last_down_point.y = peak_point_max.y;
    }//end if

    // end found
    if(negativeScan.addValue(point.y+1, -g))
    {
      // refine the position of the peak
      int f_2 = getImage().getY(point.x, peak_point_min.y-2);
      int f0  = getImage().getY(point.x, peak_point_min.y);
      int f2  = getImage().getY(point.x, peak_point_min.y+2);

      if(f_2-f0 < negativeScan.maxValue()) peak_point_min.y -= 1;
      if(f0 -f2 < negativeScan.maxValue()) peak_point_min.y += 1;

      if(estimateColorOfSegment(last_down_point, peak_point_min) == ColorClasses::green) {
        endPoint.color = ColorClasses::green;
        endPoint.posInImage = peak_point_min;
      }

      // new end edgel
      // found a new double edgel
      if(begin_found) {
        add_double_edgel(peak_point_min, scan_id);
        begin_found = false;
      } else {
        add_edgel(peak_point_min);
      }

      last_down_point.y = peak_point_min.y;
    }//end if

    DEBUG_REQUEST("Vision:Detectors:ScanLineEdgelDetector:scanlines",
      Pixel pixel = getImage().get(point.x, point.y);
      ColorClasses::Color thisPixelColor = (getFieldColorPercept().isFieldColor(pixel.a, pixel.b, pixel.c))?ColorClasses::green:ColorClasses::none;
      POINT_PX(thisPixelColor, point.x, point.y);
    );
  }//end for

  if(point.y < end.y + step) {
    if(estimateColorOfSegment(last_down_point, end) == ColorClasses::green) {
        endPoint.color = ColorClasses::green;
        endPoint.posInImage = end;
      }
  }

  return endPoint;
}//end scanForEdgels

ColorClasses::Color ScanLineEdgelDetector::estimateColorOfSegment(const Vector2i& begin, const Vector2i& end) const
{
  ASSERT(begin.x == end.x && end.y <= begin.y);

  const int numberOfSamples = theParameters.green_sampling_points;
  int length = begin.y - end.y;
  int numberOfGreen = 0;
  Vector2i point(begin);
  Pixel pixel;

  if(numberOfSamples >= length)
  {
    for(; point.y > end.y; point.y--)
    {
      getImage().get(point.x, point.y, pixel);
      numberOfGreen += getFieldColorPercept().isFieldColor(pixel.a, pixel.b, pixel.c);
    }
  }
  else
  {
    int step = length / numberOfSamples;
    int offset = Math::random(length); // number in [0,length-1]

    for(int i = 0; i < numberOfSamples; i++)
    {
      int k = (offset + i*step) % length;
      point.y = end.y + k;
      getImage().get(point.x, point.y, pixel);
      numberOfGreen += getFieldColorPercept().isFieldColor(pixel.a, pixel.b, pixel.c);
    }
  }

  return (numberOfGreen > numberOfSamples/2) ? ColorClasses::green : ColorClasses::none;
}//end estimateColorOfSegment

Vector2d ScanLineEdgelDetector::calculateGradient(const Vector2i& point) const
{
  Vector2d gradient;

  // no angle at the border (shouldn't happen)
  if( point.x < 1 || point.x + 2 > (int)getImage().width() ||
      point.y < 1 || point.y + 2 > (int)getImage().height() ) {
    return gradient;
  }

  //apply Sobel Operator on (pointX, pointY)
  //and calculate gradient in x and y direction by that means

  gradient.x =
       getImage().getY(point.x-1, point.y+1)
    +2*getImage().getY(point.x  , point.y+1)
    +  getImage().getY(point.x+1, point.y+1)
    -  getImage().getY(point.x-1, point.y-1)
    -2*getImage().getY(point.x  , point.y-1)
    -  getImage().getY(point.x+1, point.y-1);

  gradient.y =
       getImage().getY(point.x-1, point.y-1)
    +2*getImage().getY(point.x-1, point.y  )
    +  getImage().getY(point.x-1, point.y+1)
    -  getImage().getY(point.x+1, point.y-1)
    -2*getImage().getY(point.x+1, point.y  )
    -  getImage().getY(point.x+1, point.y+1);

  //calculate the angle of the gradient
  return gradient.normalize();
}//end calculateGradient
<|endoftext|>


<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
//                          License Agreement
//               For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
//  * Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
//  * The name of the copyright holders may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//M*/

#include "test_precomp.hpp"

TEST(SoftCascade, readCascade)
{
    std::string xml = cvtest::TS::ptr()->get_data_path() + "cascadeandhog/icf-template.xml";
    cv::SoftCascade cascade;
    cv::FileStorage fs(xml, cv::FileStorage::READ);
    ASSERT_TRUE(cascade.read(fs));

}

TEST(SoftCascade, detect)
{
    typedef cv::SoftCascade::Detection detection_t;
    std::string xml =  cvtest::TS::ptr()->get_data_path() + "cascadeandhog/sc_cvpr_2012_to_opencv.xml";
    cv::SoftCascade cascade;
    cv::FileStorage fs(xml, cv::FileStorage::READ);
    ASSERT_TRUE(cascade.read(fs));

    cv::Mat colored = cv::imread(cvtest::TS::ptr()->get_data_path() + "cascadeandhog/bahnhof/image_00000000_0.png");
    ASSERT_FALSE(colored.empty());

    std::vector<detection_t> objects;
    std::vector<cv::Rect> rois;
    rois.push_back(cv::Rect(0, 0, 640, 480));

    cascade.detectMultiScale(colored, rois, objects);

    cv::Mat out = colored.clone();
    int level = 0, total = 0;
    int levelWidth = objects[0].rect.width;

    for(int i = 0 ; i < (int)objects.size(); ++i)
    {
        if (objects[i].rect.width != levelWidth)
        {
            std::cout << "Level: " << level << " total " << total << std::endl;
            cv::imshow("out", out);
            cv::waitKey(0);
            out = colored.clone();
            levelWidth = objects[i].rect.width;
            total = 0;
            level++;
        }
        cv::rectangle(out, objects[i].rect, cv::Scalar(255, 0, 0, 255), 1);
        std::cout << "detection: " << objects[i].rect.x
                                   << " " << objects[i].rect.y
                                   << " " << objects[i].rect.width
                                   << " " << objects[i].rect.height << std::endl;
        total++;
    }
    std::cout << "detected: " << (int)objects.size() << std::endl;
    ASSERT_EQ((int)objects.size(), 1501);
}<commit_msg>test update because changed Sobel Normalization<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
//
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
// By downloading, copying, installing or using the software you agree to this license.
// If you do not agree to this license, do not download, install,
// copy or use the software.
//
//
//                          License Agreement
//               For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
//  * Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
//  * The name of the copyright holders may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//M*/

#include "test_precomp.hpp"

TEST(SoftCascade, readCascade)
{
    std::string xml = cvtest::TS::ptr()->get_data_path() + "cascadeandhog/icf-template.xml";
    cv::SoftCascade cascade;
    cv::FileStorage fs(xml, cv::FileStorage::READ);
    ASSERT_TRUE(cascade.read(fs));

}

TEST(SoftCascade, detect)
{
    typedef cv::SoftCascade::Detection detection_t;
    std::string xml =  cvtest::TS::ptr()->get_data_path() + "cascadeandhog/sc_cvpr_2012_to_opencv.xml";
    cv::SoftCascade cascade;
    cv::FileStorage fs(xml, cv::FileStorage::READ);
    ASSERT_TRUE(cascade.read(fs));

    cv::Mat colored = cv::imread(cvtest::TS::ptr()->get_data_path() + "cascadeandhog/bahnhof/image_00000000_0.png");
    ASSERT_FALSE(colored.empty());

    std::vector<detection_t> objects;
    std::vector<cv::Rect> rois;
    rois.push_back(cv::Rect(0, 0, 640, 480));

    cascade.detectMultiScale(colored, rois, objects);

    cv::Mat out = colored.clone();
    int level = 0, total = 0;
    int levelWidth = objects[0].rect.width;

    for(int i = 0 ; i < (int)objects.size(); ++i)
    {
        if (objects[i].rect.width != levelWidth)
        {
            std::cout << "Level: " << level << " total " << total << std::endl;
            cv::imshow("out", out);
            cv::waitKey(0);
            out = colored.clone();
            levelWidth = objects[i].rect.width;
            total = 0;
            level++;
        }
        cv::rectangle(out, objects[i].rect, cv::Scalar(255, 0, 0, 255), 1);
        std::cout << "detection: " << objects[i].rect.x
                                   << " " << objects[i].rect.y
                                   << " " << objects[i].rect.width
                                   << " " << objects[i].rect.height << std::endl;
        total++;
    }
    std::cout << "detected: " << (int)objects.size() << std::endl;
    ASSERT_EQ((int)objects.size(), 1469);
}<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <cmath>
#include <memory>
#include <string>
#include <unordered_set>
#include <vector>
#include <iomanip>

#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/proto/map_id.pb.h"
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/common/prediction_map.h"

namespace apollo {
namespace prediction {

using apollo::hdmap::LaneInfo;
using apollo::hdmap::Id;
using apollo::hdmap::MapPathPoint;

PredictionMap::PredictionMap() : hdmap_(nullptr) { LoadMap(); }

PredictionMap::~PredictionMap() { Clear(); }

void PredictionMap::LoadMap() {
  hdmap_.reset(new apollo::hdmap::HDMap());
  CHECK(hdmap_ != nullptr);
  hdmap_->load_map_from_file(FLAGS_map_file);
  AINFO << "Succeeded in loading map file: " << FLAGS_map_file << ".";
}

void PredictionMap::Clear() { hdmap_.reset(); }

Id PredictionMap::id(const std::string& str_id) {
  Id id;
  id.set_id(str_id);
  return id;
}

Eigen::Vector2d PredictionMap::PositionOnLane(
    std::shared_ptr<const LaneInfo> lane_info,
    const double s) {
  apollo::common::PointENU point = lane_info->get_smooth_point(s);
  return {point.x(), point.y()};
}

double PredictionMap::HeadingOnLane(std::shared_ptr<const LaneInfo> lane_info,
                                    const double s) {
  const std::vector<double>& headings = lane_info->headings();
  const std::vector<double>& accumulated_s = lane_info->accumulate_s();
  CHECK(headings.size() == accumulated_s.size());
  size_t size = headings.size();
  if (size == 0) {
    return 0.0;
  }
  if (size == 1) {
    return headings[0];
  }
  const auto low_itr =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  CHECK(low_itr != accumulated_s.end());
  size_t index = low_itr - accumulated_s.begin();
  if (index == size - 1) {
    return headings.back();
  }
  return apollo::common::math::slerp(headings[index], accumulated_s[index],
                                     headings[index + 1],
                                     accumulated_s[index + 1], s);
}

double PredictionMap::LaneTotalWidth(
    std::shared_ptr<const apollo::hdmap::LaneInfo> lane_info_ptr,
    const double s) {
  double left = 0.0;
  double right = 0.0;
  lane_info_ptr->get_width(s, &left, &right);
  return left + right;
}

std::shared_ptr<const LaneInfo> PredictionMap::LaneById(const Id& id) {
  return hdmap_->get_lane_by_id(id);
}

std::shared_ptr<const LaneInfo> PredictionMap::LaneById(
    const std::string& str_id) {
  Id id;
  id.set_id(str_id);
  return LaneById(id);
}

void PredictionMap::GetProjection(const Eigen::Vector2d& position,
                                  std::shared_ptr<const LaneInfo> lane_info_ptr,
                                  double* s,
                                  double* l) {
  if (lane_info_ptr == nullptr) {
    return;
  }
  apollo::common::math::Vec2d pos(position[0], position[1]);
  lane_info_ptr->get_projection(pos, s, l);
}

bool PredictionMap::ProjectionFromLane(
    std::shared_ptr<const LaneInfo> lane_info_ptr, double s,
    MapPathPoint* path_point) {
  if (lane_info_ptr == nullptr) {
    return false;
  }
  apollo::common::PointENU point = lane_info_ptr->get_smooth_point(s);
  double heading = HeadingOnLane(lane_info_ptr, s);
  path_point->set_x(point.x());
  path_point->set_y(point.y());
  path_point->set_heading(heading);
  return true;
}

void PredictionMap::OnLane(
    const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
    const Eigen::Vector2d& point, const double heading,
    const double radius,
    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
  std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
  // TODO(kechxu) clean the messy code of this function
  apollo::common::PointENU hdmap_point;
  hdmap_point.set_x(point[0]);
  hdmap_point.set_y(point[1]);
  apollo::common::math::Vec2d vec_point;
  vec_point.set_x(point[0]);
  vec_point.set_y(point[1]);
  if (hdmap_->get_lanes_with_heading(hdmap_point, radius, heading, M_PI,
                                     &candidate_lanes) != 0) {
    return;
  }
  for (auto candidate_lane : candidate_lanes) {
    if (candidate_lane == nullptr) {
      continue;
    } else if (!candidate_lane->is_on_lane(vec_point)) {
      continue;
    } else if (!IsIdenticalLane(candidate_lane, prev_lanes) &&
               !IsSuccessorLane(candidate_lane, prev_lanes) &&
               !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
               !IsRightNeighborLane(candidate_lane, prev_lanes)) {
      continue;
    } else {
      double distance = 0.0;
      apollo::common::PointENU nearest_point =
          candidate_lane->get_nearest_point(vec_point, &distance);
      double nearest_point_heading =
          PathHeading(candidate_lane, nearest_point);
      AINFO << "heading = " << heading;
      AINFO << "nearest point heading = " << nearest_point_heading;
      double diff = std::fabs(
          apollo::common::math::AngleDiff(heading, nearest_point_heading));
      AINFO << "angle diff = " << diff;
      if (diff <= FLAGS_max_lane_angle_diff) {
        lanes->push_back(candidate_lane);
      }
    }
  }
}

double PredictionMap::PathHeading(std::shared_ptr<const LaneInfo> lane_info_ptr,
                                  const apollo::common::PointENU& point) {
  apollo::common::math::Vec2d vec_point;
  vec_point.set_x(point.x());
  vec_point.set_y(point.y());
  double s = -1.0;
  double l = 0.0;
  lane_info_ptr->get_projection(vec_point, &s, &l);
  return HeadingOnLane(lane_info_ptr, s);
}

int PredictionMap::SmoothPointFromLane(const apollo::hdmap::Id& id,
                                       const double s, const double l,
                                       Eigen::Vector2d* point,
                                       double* heading) {
  // TODO(kechxu) Double check this implement
  if (point == nullptr || heading == nullptr) {
    return -1;
  }
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  apollo::common::PointENU hdmap_point = lane->get_smooth_point(s);
  *heading = PathHeading(lane, hdmap_point);
  AINFO << "lane_s = ["
        << std::fixed << std::setprecision(6) << s << "], "
        << "hdmap pt = ["
        << std::fixed << std::setprecision(6)
        << hdmap_point.x() << ", " << hdmap_point.y() << "]";
  point->operator[](0) = hdmap_point.x() - std::sin(*heading) * l;
  point->operator[](1) = hdmap_point.y() + std::cos(*heading) * l;
  return 0;
}

void PredictionMap::NearbyLanesByCurrentLanes(
    const Eigen::Vector2d& point,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
    std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
  std::unordered_set<std::string> lane_ids;
  for (auto& lane_ptr : lanes) {
    if (lane_ptr == nullptr) {
      continue;
    }
    for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
      if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
        continue;
      }
      lane_ids.insert(lane_id.id());
      std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
      nearby_lanes->push_back(nearby_lane);
    }
    for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
      if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
        continue;
      }
      lane_ids.insert(lane_id.id());
      std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
      nearby_lanes->push_back(nearby_lane);
    }
  }
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (left_lane == nullptr) {
    return false;
  }
  for (auto& left_lane_id : curr_lane->lane().left_neighbor_forward_lane_id()) {
    if (id_string(left_lane) == left_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsLeftNeighborLane(left_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (right_lane == nullptr) {
    return false;
  }
  for (auto& right_lane_id :
       curr_lane->lane().right_neighbor_forward_lane_id()) {
    if (id_string(right_lane) == right_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsRightNeighborLane(right_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (succ_lane == nullptr) {
    return false;
  }
  for (auto& successor_lane_id : curr_lane->lane().successor_id()) {
    if (id_string(succ_lane) == successor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsSuccessorLane(succ_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (pred_lane == nullptr) {
    return false;
  }
  for (auto& predecessor_lane_id : curr_lane->lane().predecessor_id()) {
    if (id_string(pred_lane) == predecessor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsPredecessorLane(pred_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr || other_lane == nullptr) {
    return false;
  }
  return id_string(other_lane) == id_string(curr_lane);
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsIdenticalLane(other_lane, lane)) {
      return true;
    }
  }
  return false;
}

int PredictionMap::LaneTurnType(const Id& id) {
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  if (lane != nullptr) {
    return static_cast<int>(lane->lane().turn());
  }
  return 1;
}

int PredictionMap::LaneTurnType(const std::string& lane_id) {
  return LaneTurnType(id(lane_id));
}

}  // namespace prediction
}  // namespace apollo
<commit_msg>small change on log in prediction module<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <cmath>
#include <memory>
#include <string>
#include <unordered_set>
#include <vector>
#include <iomanip>

#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/proto/map_id.pb.h"
#include "modules/prediction/common/prediction_gflags.h"
#include "modules/prediction/common/prediction_map.h"

namespace apollo {
namespace prediction {

using apollo::hdmap::LaneInfo;
using apollo::hdmap::Id;
using apollo::hdmap::MapPathPoint;

PredictionMap::PredictionMap() : hdmap_(nullptr) { LoadMap(); }

PredictionMap::~PredictionMap() { Clear(); }

void PredictionMap::LoadMap() {
  hdmap_.reset(new apollo::hdmap::HDMap());
  CHECK(hdmap_ != nullptr);
  hdmap_->load_map_from_file(FLAGS_map_file);
  AINFO << "Succeeded in loading map file: " << FLAGS_map_file << ".";
}

void PredictionMap::Clear() { hdmap_.reset(); }

Id PredictionMap::id(const std::string& str_id) {
  Id id;
  id.set_id(str_id);
  return id;
}

Eigen::Vector2d PredictionMap::PositionOnLane(
    std::shared_ptr<const LaneInfo> lane_info,
    const double s) {
  apollo::common::PointENU point = lane_info->get_smooth_point(s);
  return {point.x(), point.y()};
}

double PredictionMap::HeadingOnLane(std::shared_ptr<const LaneInfo> lane_info,
                                    const double s) {
  const std::vector<double>& headings = lane_info->headings();
  const std::vector<double>& accumulated_s = lane_info->accumulate_s();
  CHECK(headings.size() == accumulated_s.size());
  size_t size = headings.size();
  if (size == 0) {
    return 0.0;
  }
  if (size == 1) {
    return headings[0];
  }
  const auto low_itr =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  CHECK(low_itr != accumulated_s.end());
  size_t index = low_itr - accumulated_s.begin();
  if (index == size - 1) {
    return headings.back();
  }
  return apollo::common::math::slerp(headings[index], accumulated_s[index],
                                     headings[index + 1],
                                     accumulated_s[index + 1], s);
}

double PredictionMap::LaneTotalWidth(
    std::shared_ptr<const apollo::hdmap::LaneInfo> lane_info_ptr,
    const double s) {
  double left = 0.0;
  double right = 0.0;
  lane_info_ptr->get_width(s, &left, &right);
  return left + right;
}

std::shared_ptr<const LaneInfo> PredictionMap::LaneById(const Id& id) {
  return hdmap_->get_lane_by_id(id);
}

std::shared_ptr<const LaneInfo> PredictionMap::LaneById(
    const std::string& str_id) {
  Id id;
  id.set_id(str_id);
  return LaneById(id);
}

void PredictionMap::GetProjection(const Eigen::Vector2d& position,
                                  std::shared_ptr<const LaneInfo> lane_info_ptr,
                                  double* s,
                                  double* l) {
  if (lane_info_ptr == nullptr) {
    return;
  }
  apollo::common::math::Vec2d pos(position[0], position[1]);
  lane_info_ptr->get_projection(pos, s, l);
}

bool PredictionMap::ProjectionFromLane(
    std::shared_ptr<const LaneInfo> lane_info_ptr, double s,
    MapPathPoint* path_point) {
  if (lane_info_ptr == nullptr) {
    return false;
  }
  apollo::common::PointENU point = lane_info_ptr->get_smooth_point(s);
  double heading = HeadingOnLane(lane_info_ptr, s);
  path_point->set_x(point.x());
  path_point->set_y(point.y());
  path_point->set_heading(heading);
  return true;
}

void PredictionMap::OnLane(
    const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
    const Eigen::Vector2d& point, const double heading,
    const double radius,
    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
  std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;
  // TODO(kechxu) clean the messy code of this function
  apollo::common::PointENU hdmap_point;
  hdmap_point.set_x(point[0]);
  hdmap_point.set_y(point[1]);
  apollo::common::math::Vec2d vec_point;
  vec_point.set_x(point[0]);
  vec_point.set_y(point[1]);
  if (hdmap_->get_lanes_with_heading(hdmap_point, radius, heading, M_PI,
                                     &candidate_lanes) != 0) {
    return;
  }
  for (auto candidate_lane : candidate_lanes) {
    if (candidate_lane == nullptr) {
      continue;
    } else if (!candidate_lane->is_on_lane(vec_point)) {
      continue;
    } else if (!IsIdenticalLane(candidate_lane, prev_lanes) &&
               !IsSuccessorLane(candidate_lane, prev_lanes) &&
               !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
               !IsRightNeighborLane(candidate_lane, prev_lanes)) {
      continue;
    } else {
      double distance = 0.0;
      apollo::common::PointENU nearest_point =
          candidate_lane->get_nearest_point(vec_point, &distance);
      double nearest_point_heading =
          PathHeading(candidate_lane, nearest_point);
      double diff = std::fabs(
          apollo::common::math::AngleDiff(heading, nearest_point_heading));
      if (diff <= FLAGS_max_lane_angle_diff) {
        lanes->push_back(candidate_lane);
      }
    }
  }
}

double PredictionMap::PathHeading(std::shared_ptr<const LaneInfo> lane_info_ptr,
                                  const apollo::common::PointENU& point) {
  apollo::common::math::Vec2d vec_point;
  vec_point.set_x(point.x());
  vec_point.set_y(point.y());
  double s = -1.0;
  double l = 0.0;
  lane_info_ptr->get_projection(vec_point, &s, &l);
  return HeadingOnLane(lane_info_ptr, s);
}

int PredictionMap::SmoothPointFromLane(const apollo::hdmap::Id& id,
                                       const double s, const double l,
                                       Eigen::Vector2d* point,
                                       double* heading) {
  // TODO(kechxu) Double check this implement
  if (point == nullptr || heading == nullptr) {
    return -1;
  }
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  apollo::common::PointENU hdmap_point = lane->get_smooth_point(s);
  *heading = PathHeading(lane, hdmap_point);
  AINFO << "lane_id = " << id.id() << ", "
        << "lane_s = ["
        << std::fixed << std::setprecision(6) << s << "], "
        << "hdmap pt = ["
        << std::fixed << std::setprecision(6)
        << hdmap_point.x() << ", " << hdmap_point.y() << "]";
  point->operator[](0) = hdmap_point.x() - std::sin(*heading) * l;
  point->operator[](1) = hdmap_point.y() + std::cos(*heading) * l;
  return 0;
}

void PredictionMap::NearbyLanesByCurrentLanes(
    const Eigen::Vector2d& point,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
    std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
  std::unordered_set<std::string> lane_ids;
  for (auto& lane_ptr : lanes) {
    if (lane_ptr == nullptr) {
      continue;
    }
    for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
      if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
        continue;
      }
      lane_ids.insert(lane_id.id());
      std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
      nearby_lanes->push_back(nearby_lane);
    }
    for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
      if (lane_ids.find(lane_id.id()) != lane_ids.end()) {
        continue;
      }
      lane_ids.insert(lane_id.id());
      std::shared_ptr<const LaneInfo> nearby_lane = LaneById(lane_id);
      nearby_lanes->push_back(nearby_lane);
    }
  }
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (left_lane == nullptr) {
    return false;
  }
  for (auto& left_lane_id : curr_lane->lane().left_neighbor_forward_lane_id()) {
    if (id_string(left_lane) == left_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsLeftNeighborLane(left_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (right_lane == nullptr) {
    return false;
  }
  for (auto& right_lane_id :
       curr_lane->lane().right_neighbor_forward_lane_id()) {
    if (id_string(right_lane) == right_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsRightNeighborLane(right_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (succ_lane == nullptr) {
    return false;
  }
  for (auto& successor_lane_id : curr_lane->lane().successor_id()) {
    if (id_string(succ_lane) == successor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsSuccessorLane(succ_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (pred_lane == nullptr) {
    return false;
  }
  for (auto& predecessor_lane_id : curr_lane->lane().predecessor_id()) {
    if (id_string(pred_lane) == predecessor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsPredecessorLane(pred_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr || other_lane == nullptr) {
    return false;
  }
  return id_string(other_lane) == id_string(curr_lane);
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsIdenticalLane(other_lane, lane)) {
      return true;
    }
  }
  return false;
}

int PredictionMap::LaneTurnType(const Id& id) {
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  if (lane != nullptr) {
    return static_cast<int>(lane->lane().turn());
  }
  return 1;
}

int PredictionMap::LaneTurnType(const std::string& lane_id) {
  return LaneTurnType(id(lane_id));
}

}  // namespace prediction
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/prediction/common/prediction_map.h"

#include <cmath>
#include <iomanip>
#include <memory>
#include <string>
#include <unordered_set>
#include <vector>

#include "modules/common/configs/config_gflags.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/proto/map_id.pb.h"
#include "modules/prediction/common/prediction_gflags.h"

namespace apollo {
namespace prediction {

using apollo::hdmap::LaneInfo;
using apollo::hdmap::Id;
using apollo::hdmap::MapPathPoint;
using apollo::hdmap::HDMapUtil;

PredictionMap::PredictionMap() {}

Eigen::Vector2d PredictionMap::PositionOnLane(
    std::shared_ptr<const LaneInfo> lane_info,
    const double s) {
  apollo::common::PointENU point = lane_info->get_smooth_point(s);
  return {point.x(), point.y()};
}

double PredictionMap::HeadingOnLane(std::shared_ptr<const LaneInfo> lane_info,
                                    const double s) {
  const std::vector<double>& headings = lane_info->headings();
  const std::vector<double>& accumulated_s = lane_info->accumulate_s();
  CHECK(headings.size() == accumulated_s.size());
  size_t size = headings.size();
  if (size == 0) {
    return 0.0;
  }
  if (size == 1) {
    return headings[0];
  }
  const auto low_itr =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  // CHECK(low_itr != accumulated_s.end());
  size_t index = low_itr - accumulated_s.begin();
  if (index >= size - 1) {
    return headings.back();
  }
  return apollo::common::math::slerp(headings[index], accumulated_s[index],
                                     headings[index + 1],
                                     accumulated_s[index + 1], s);
}

double PredictionMap::LaneTotalWidth(
    std::shared_ptr<const apollo::hdmap::LaneInfo> lane_info,
    const double s) {
  double left = 0.0;
  double right = 0.0;
  lane_info->get_width(s, &left, &right);
  return left + right;
}

std::shared_ptr<const LaneInfo> PredictionMap::LaneById(
    const std::string& str_id) {
  return HDMapUtil::instance()->BaseMapRef().get_lane_by_id(
      apollo::hdmap::MakeMapId(str_id));
}

void PredictionMap::GetProjection(const Eigen::Vector2d& position,
                                  std::shared_ptr<const LaneInfo> lane_info,
                                  double* s,
                                  double* l) {
  if (lane_info == nullptr) {
    return;
  }
  apollo::common::math::Vec2d pos(position[0], position[1]);
  lane_info->get_projection(pos, s, l);
}

bool PredictionMap::ProjectionFromLane(
    std::shared_ptr<const LaneInfo> lane_info, double s,
    MapPathPoint* path_point) {
  if (lane_info == nullptr) {
    return false;
  }
  apollo::common::PointENU point = lane_info->get_smooth_point(s);
  double heading = HeadingOnLane(lane_info, s);
  path_point->set_x(point.x());
  path_point->set_y(point.y());
  path_point->set_heading(heading);
  return true;
}

void PredictionMap::OnLane(
    const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
    const Eigen::Vector2d& point, const double heading,
    const double radius,
    const bool on_lane,
    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
  std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;

  apollo::common::PointENU hdmap_point;
  hdmap_point.set_x(point[0]);
  hdmap_point.set_y(point[1]);
  if (HDMapUtil::instance()->BaseMapRef().get_lanes_with_heading(
        hdmap_point, radius, heading, M_PI, &candidate_lanes) != 0) {
    return;
  }

  apollo::common::math::Vec2d vec_point(point[0], point[1]);
  for (const auto &candidate_lane : candidate_lanes) {
    if (candidate_lane == nullptr) {
      continue;
    }
    if (on_lane && !candidate_lane->is_on_lane(vec_point)) {
      continue;
    }
    if (!IsIdenticalLane(candidate_lane, prev_lanes) &&
        !IsSuccessorLane(candidate_lane, prev_lanes) &&
        !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
        !IsRightNeighborLane(candidate_lane, prev_lanes)) {
      continue;
    }
    double distance = 0.0;
    apollo::common::PointENU nearest_point =
        candidate_lane->get_nearest_point(vec_point, &distance);
    double nearest_point_heading =
        PathHeading(candidate_lane, nearest_point);
    double diff = std::fabs(
        apollo::common::math::AngleDiff(heading, nearest_point_heading));
    if (diff <= FLAGS_max_lane_angle_diff) {
      lanes->push_back(candidate_lane);
    }
  }
}

double PredictionMap::PathHeading(std::shared_ptr<const LaneInfo> lane_info,
                                  const apollo::common::PointENU& point) {
  apollo::common::math::Vec2d vec_point = {point.x(), point.y()};
  double s = -1.0;
  double l = 0.0;
  lane_info->get_projection(vec_point, &s, &l);
  return HeadingOnLane(lane_info, s);
}

int PredictionMap::SmoothPointFromLane(const std::string& id,
                                       const double s, const double l,
                                       Eigen::Vector2d* point,
                                       double* heading) {
  if (point == nullptr || heading == nullptr) {
    return -1;
  }
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  apollo::common::PointENU hdmap_point = lane->get_smooth_point(s);
  *heading = PathHeading(lane, hdmap_point);
  point->operator[](0) = hdmap_point.x() - std::sin(*heading) * l;
  point->operator[](1) = hdmap_point.y() + std::cos(*heading) * l;
  return 0;
}

void PredictionMap::NearbyLanesByCurrentLanes(
    const Eigen::Vector2d& point,
    double heading,
    double radius,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
    std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
  if (lanes.size() == 0) {
    std::vector<std::shared_ptr<const LaneInfo>> prev_lanes(0);
    OnLane(prev_lanes, point, heading, radius, false, nearby_lanes);
  } else {
    std::unordered_set<std::string> lane_ids;
    for (auto& lane_ptr : lanes) {
      if (lane_ptr == nullptr) {
        continue;
      }
      for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
        const std::string& id = lane_id.id();
        if (lane_ids.find(id) != lane_ids.end()) {
          continue;
        }
        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
        double s = -1.0;
        double l = 0.0;
        GetProjection(point, nearby_lane, &s, &l);
        if (::apollo::common::math::DoubleCompare(s, 0.0) >= 0 &&
            ::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
          continue;
        }
        lane_ids.insert(id);
        nearby_lanes->push_back(nearby_lane);
      }
      for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
        const std::string& id = lane_id.id();
        if (lane_ids.find(id) != lane_ids.end()) {
          continue;
        }
        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
        double s = -1.0;
        double l = 0.0;
        GetProjection(point, nearby_lane, &s, &l);
        if (::apollo::common::math::DoubleCompare(s, 0.0) >= 0 &&
            ::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
          continue;
        }
        lane_ids.insert(id);
        nearby_lanes->push_back(nearby_lane);
      }
    }
  }
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (left_lane == nullptr) {
    return false;
  }
  for (auto& left_lane_id : curr_lane->lane().left_neighbor_forward_lane_id()) {
    if (left_lane->id().id() == left_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsLeftNeighborLane(left_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (right_lane == nullptr) {
    return false;
  }
  for (auto& right_lane_id :
       curr_lane->lane().right_neighbor_forward_lane_id()) {
    if (right_lane->id().id() == right_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsRightNeighborLane(right_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (succ_lane == nullptr) {
    return false;
  }
  for (auto& successor_lane_id : curr_lane->lane().successor_id()) {
    if (succ_lane->id().id() == successor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsSuccessorLane(succ_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (pred_lane == nullptr) {
    return false;
  }
  for (auto& predecessor_lane_id : curr_lane->lane().predecessor_id()) {
    if (pred_lane->id().id() == predecessor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsPredecessorLane(pred_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr || other_lane == nullptr) {
    return true;
  }
  return other_lane->id().id() == curr_lane->id().id();
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsIdenticalLane(other_lane, lane)) {
      return true;
    }
  }
  return false;
}

int PredictionMap::LaneTurnType(const std::string& lane_id) {
  std::shared_ptr<const LaneInfo> lane = LaneById(lane_id);
  if (lane != nullptr) {
    return static_cast<int>(lane->lane().turn());
  }
  return 1;
}

}  // namespace prediction
}  // namespace apollo
<commit_msg>Prediction: reformatting<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/prediction/common/prediction_map.h"

#include <cmath>
#include <iomanip>
#include <memory>
#include <string>
#include <unordered_set>
#include <vector>

#include "modules/common/configs/config_gflags.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/map/hdmap/hdmap_util.h"
#include "modules/map/proto/map_id.pb.h"
#include "modules/prediction/common/prediction_gflags.h"

namespace apollo {
namespace prediction {

using apollo::hdmap::LaneInfo;
using apollo::hdmap::Id;
using apollo::hdmap::MapPathPoint;
using apollo::hdmap::HDMapUtil;

PredictionMap::PredictionMap() {}

Eigen::Vector2d PredictionMap::PositionOnLane(
    std::shared_ptr<const LaneInfo> lane_info,
    const double s) {
  apollo::common::PointENU point = lane_info->get_smooth_point(s);
  return {point.x(), point.y()};
}

double PredictionMap::HeadingOnLane(
    std::shared_ptr<const LaneInfo> lane_info,
    const double s) {
  const std::vector<double>& headings = lane_info->headings();
  const std::vector<double>& accumulated_s = lane_info->accumulate_s();
  CHECK(headings.size() == accumulated_s.size());
  size_t size = headings.size();

  if (size == 0) {
    return 0.0;
  }

  if (size == 1) {
    return headings[0];
  }

  const auto low_itr =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  size_t index = low_itr - accumulated_s.begin();
  if (index >= size - 1) {
    return headings.back();
  } else {
    return apollo::common::math::slerp(headings[index], accumulated_s[index],
                                       headings[index + 1],
                                       accumulated_s[index + 1], s);
  }
}

double PredictionMap::LaneTotalWidth(
    std::shared_ptr<const apollo::hdmap::LaneInfo> lane_info,
    const double s) {
  double left = 0.0;
  double right = 0.0;
  lane_info->get_width(s, &left, &right);
  return left + right;
}

std::shared_ptr<const LaneInfo> PredictionMap::LaneById(
    const std::string& str_id) {
  return HDMapUtil::instance()->BaseMapRef().get_lane_by_id(
      apollo::hdmap::MakeMapId(str_id));
}

void PredictionMap::GetProjection(
    const Eigen::Vector2d& position,
    std::shared_ptr<const LaneInfo> lane_info,
    double* s,
    double* l) {
  if (lane_info == nullptr) {
    return;
  }
  apollo::common::math::Vec2d pos(position[0], position[1]);
  lane_info->get_projection(pos, s, l);
}

bool PredictionMap::ProjectionFromLane(
    std::shared_ptr<const LaneInfo> lane_info, double s,
    MapPathPoint* path_point) {
  if (lane_info == nullptr) {
    return false;
  }
  apollo::common::PointENU point = lane_info->get_smooth_point(s);
  double heading = HeadingOnLane(lane_info, s);
  path_point->set_x(point.x());
  path_point->set_y(point.y());
  path_point->set_heading(heading);
  return true;
}

void PredictionMap::OnLane(
    const std::vector<std::shared_ptr<const LaneInfo>>& prev_lanes,
    const Eigen::Vector2d& point, const double heading,
    const double radius,
    const bool on_lane,
    std::vector<std::shared_ptr<const LaneInfo>>* lanes) {
  std::vector<std::shared_ptr<const LaneInfo>> candidate_lanes;

  apollo::common::PointENU hdmap_point;
  hdmap_point.set_x(point[0]);
  hdmap_point.set_y(point[1]);
  if (HDMapUtil::instance()->BaseMapRef().get_lanes_with_heading(
        hdmap_point, radius, heading, M_PI, &candidate_lanes) != 0) {
    return;
  }

  apollo::common::math::Vec2d vec_point(point[0], point[1]);
  for (const auto &candidate_lane : candidate_lanes) {
    if (candidate_lane == nullptr) {
      continue;
    }
    if (on_lane && !candidate_lane->is_on_lane(vec_point)) {
      continue;
    }
    if (!IsIdenticalLane(candidate_lane, prev_lanes) &&
        !IsSuccessorLane(candidate_lane, prev_lanes) &&
        !IsLeftNeighborLane(candidate_lane, prev_lanes) &&
        !IsRightNeighborLane(candidate_lane, prev_lanes)) {
      continue;
    }
    double distance = 0.0;
    apollo::common::PointENU nearest_point =
        candidate_lane->get_nearest_point(vec_point, &distance);
    double nearest_point_heading =
        PathHeading(candidate_lane, nearest_point);
    double diff = std::fabs(
        apollo::common::math::AngleDiff(heading, nearest_point_heading));
    if (diff <= FLAGS_max_lane_angle_diff) {
      lanes->push_back(candidate_lane);
    }
  }
}

double PredictionMap::PathHeading(
    std::shared_ptr<const LaneInfo> lane_info,
    const apollo::common::PointENU& point) {
  apollo::common::math::Vec2d vec_point = {point.x(), point.y()};
  double s = -1.0;
  double l = 0.0;
  lane_info->get_projection(vec_point, &s, &l);
  return HeadingOnLane(lane_info, s);
}

int PredictionMap::SmoothPointFromLane(
    const std::string& id,
    const double s, const double l,
    Eigen::Vector2d* point,
    double* heading) {
  if (point == nullptr || heading == nullptr) {
    return -1;
  }
  std::shared_ptr<const LaneInfo> lane = LaneById(id);
  apollo::common::PointENU hdmap_point = lane->get_smooth_point(s);
  *heading = PathHeading(lane, hdmap_point);
  point->operator[](0) = hdmap_point.x() - std::sin(*heading) * l;
  point->operator[](1) = hdmap_point.y() + std::cos(*heading) * l;
  return 0;
}

void PredictionMap::NearbyLanesByCurrentLanes(
    const Eigen::Vector2d& point,
    double heading,
    double radius,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes,
    std::vector<std::shared_ptr<const LaneInfo>>* nearby_lanes) {
  if (lanes.size() == 0) {
    std::vector<std::shared_ptr<const LaneInfo>> prev_lanes(0);
    OnLane(prev_lanes, point, heading, radius, false, nearby_lanes);
  } else {
    std::unordered_set<std::string> lane_ids;
    for (auto& lane_ptr : lanes) {
      if (lane_ptr == nullptr) {
        continue;
      }
      for (auto& lane_id : lane_ptr->lane().left_neighbor_forward_lane_id()) {
        const std::string& id = lane_id.id();
        if (lane_ids.find(id) != lane_ids.end()) {
          continue;
        }
        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
        double s = -1.0;
        double l = 0.0;
        GetProjection(point, nearby_lane, &s, &l);
        if (::apollo::common::math::DoubleCompare(s, 0.0) >= 0 &&
            ::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
          continue;
        }
        lane_ids.insert(id);
        nearby_lanes->push_back(nearby_lane);
      }
      for (auto& lane_id : lane_ptr->lane().right_neighbor_forward_lane_id()) {
        const std::string& id = lane_id.id();
        if (lane_ids.find(id) != lane_ids.end()) {
          continue;
        }
        std::shared_ptr<const LaneInfo> nearby_lane = LaneById(id);
        double s = -1.0;
        double l = 0.0;
        GetProjection(point, nearby_lane, &s, &l);
        if (::apollo::common::math::DoubleCompare(s, 0.0) >= 0 &&
            ::apollo::common::math::DoubleCompare(std::fabs(l), radius) > 0) {
          continue;
        }
        lane_ids.insert(id);
        nearby_lanes->push_back(nearby_lane);
      }
    }
  }
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (left_lane == nullptr) {
    return false;
  }
  for (auto& left_lane_id : curr_lane->lane().left_neighbor_forward_lane_id()) {
    if (left_lane->id().id() == left_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsLeftNeighborLane(
    std::shared_ptr<const LaneInfo> left_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsLeftNeighborLane(left_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (right_lane == nullptr) {
    return false;
  }
  for (auto& right_lane_id :
       curr_lane->lane().right_neighbor_forward_lane_id()) {
    if (right_lane->id().id() == right_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsRightNeighborLane(
    std::shared_ptr<const LaneInfo> right_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsRightNeighborLane(right_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (succ_lane == nullptr) {
    return false;
  }
  for (auto& successor_lane_id : curr_lane->lane().successor_id()) {
    if (succ_lane->id().id() == successor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsSuccessorLane(
    std::shared_ptr<const LaneInfo> succ_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsSuccessorLane(succ_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr) {
    return true;
  }
  if (pred_lane == nullptr) {
    return false;
  }
  for (auto& predecessor_lane_id : curr_lane->lane().predecessor_id()) {
    if (pred_lane->id().id() == predecessor_lane_id.id()) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsPredecessorLane(
    std::shared_ptr<const LaneInfo> pred_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsPredecessorLane(pred_lane, lane)) {
      return true;
    }
  }
  return false;
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    std::shared_ptr<const LaneInfo> curr_lane) {
  if (curr_lane == nullptr || other_lane == nullptr) {
    return true;
  }
  return other_lane->id().id() == curr_lane->id().id();
}

bool PredictionMap::IsIdenticalLane(
    std::shared_ptr<const LaneInfo> other_lane,
    const std::vector<std::shared_ptr<const LaneInfo>>& lanes) {
  if (lanes.size() == 0) {
    return true;
  }
  for (auto& lane : lanes) {
    if (IsIdenticalLane(other_lane, lane)) {
      return true;
    }
  }
  return false;
}

int PredictionMap::LaneTurnType(const std::string& lane_id) {
  std::shared_ptr<const LaneInfo> lane = LaneById(lane_id);
  if (lane != nullptr) {
    return static_cast<int>(lane->lane().turn());
  }
  return 1;
}

}  // namespace prediction
}  // namespace apollo
<|endoftext|>


<commit_before><commit_msg>created function to get only overloads<commit_after><|endoftext|>


<commit_before><commit_msg>Replace IMPL_STATIC_LINK[_TYPED] with more useful variants<commit_after><|endoftext|>