Compare commits
2 Commits
36a0da7c29
...
fbde9bbb9f
| Author | SHA1 | Date |
|---|---|---|
 詹力
|
fbde9bbb9f | |
 ray
|
a2bb0d838f |
|
|
@ -0,0 +1 @@
|
||||||
|
# This file currently only serves to mark the location of a catkin workspace for tool integration
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
catkin_make
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
/home/firefly/sweeping_robot/src
|
||||||
|
|
@ -0,0 +1,2 @@
|
||||||
|
- setup-file:
|
||||||
|
local-name: /home/firefly/sweeping_robot/devel/setup.sh
|
||||||
|
|
@ -0,0 +1,304 @@
|
||||||
|
#!/usr/bin/python2
|
||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
|
# Software License Agreement (BSD License)
|
||||||
|
#
|
||||||
|
# Copyright (c) 2012, Willow Garage, Inc.
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# 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.
|
||||||
|
# * Neither the name of Willow Garage, Inc. nor the names of its
|
||||||
|
# contributors may 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
|
||||||
|
# COPYRIGHT OWNER 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.
|
||||||
|
|
||||||
|
"""This file generates shell code for the setup.SHELL scripts to set environment variables."""
|
||||||
|
|
||||||
|
from __future__ import print_function
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import copy
|
||||||
|
import errno
|
||||||
|
import os
|
||||||
|
import platform
|
||||||
|
import sys
|
||||||
|
|
||||||
|
CATKIN_MARKER_FILE = '.catkin'
|
||||||
|
|
||||||
|
system = platform.system()
|
||||||
|
IS_DARWIN = (system == 'Darwin')
|
||||||
|
IS_WINDOWS = (system == 'Windows')
|
||||||
|
|
||||||
|
PATH_TO_ADD_SUFFIX = ['bin']
|
||||||
|
if IS_WINDOWS:
|
||||||
|
# while catkin recommends putting dll's into bin, 3rd party packages often put dll's into lib
|
||||||
|
# since Windows finds dll's via the PATH variable, prepend it with path to lib
|
||||||
|
PATH_TO_ADD_SUFFIX.extend([['lib', os.path.join('lib', 'aarch64-linux-gnu')]])
|
||||||
|
|
||||||
|
# subfolder of workspace prepended to CMAKE_PREFIX_PATH
|
||||||
|
ENV_VAR_SUBFOLDERS = {
|
||||||
|
'CMAKE_PREFIX_PATH': '',
|
||||||
|
'LD_LIBRARY_PATH' if not IS_DARWIN else 'DYLD_LIBRARY_PATH': ['lib', os.path.join('lib', 'aarch64-linux-gnu')],
|
||||||
|
'PATH': PATH_TO_ADD_SUFFIX,
|
||||||
|
'PKG_CONFIG_PATH': [os.path.join('lib', 'pkgconfig'), os.path.join('lib', 'aarch64-linux-gnu', 'pkgconfig')],
|
||||||
|
'PYTHONPATH': 'lib/python2.7/dist-packages',
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
def rollback_env_variables(environ, env_var_subfolders):
|
||||||
|
"""
|
||||||
|
Generate shell code to reset environment variables.
|
||||||
|
|
||||||
|
by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH.
|
||||||
|
This does not cover modifications performed by environment hooks.
|
||||||
|
"""
|
||||||
|
lines = []
|
||||||
|
unmodified_environ = copy.copy(environ)
|
||||||
|
for key in sorted(env_var_subfolders.keys()):
|
||||||
|
subfolders = env_var_subfolders[key]
|
||||||
|
if not isinstance(subfolders, list):
|
||||||
|
subfolders = [subfolders]
|
||||||
|
value = _rollback_env_variable(unmodified_environ, key, subfolders)
|
||||||
|
if value is not None:
|
||||||
|
environ[key] = value
|
||||||
|
lines.append(assignment(key, value))
|
||||||
|
if lines:
|
||||||
|
lines.insert(0, comment('reset environment variables by unrolling modifications based on all workspaces in CMAKE_PREFIX_PATH'))
|
||||||
|
return lines
|
||||||
|
|
||||||
|
|
||||||
|
def _rollback_env_variable(environ, name, subfolders):
|
||||||
|
"""
|
||||||
|
For each catkin workspace in CMAKE_PREFIX_PATH remove the first entry from env[NAME] matching workspace + subfolder.
|
||||||
|
|
||||||
|
:param subfolders: list of str '' or subfoldername that may start with '/'
|
||||||
|
:returns: the updated value of the environment variable.
|
||||||
|
"""
|
||||||
|
value = environ[name] if name in environ else ''
|
||||||
|
env_paths = [path for path in value.split(os.pathsep) if path]
|
||||||
|
value_modified = False
|
||||||
|
for subfolder in subfolders:
|
||||||
|
if subfolder:
|
||||||
|
if subfolder.startswith(os.path.sep) or (os.path.altsep and subfolder.startswith(os.path.altsep)):
|
||||||
|
subfolder = subfolder[1:]
|
||||||
|
if subfolder.endswith(os.path.sep) or (os.path.altsep and subfolder.endswith(os.path.altsep)):
|
||||||
|
subfolder = subfolder[:-1]
|
||||||
|
for ws_path in _get_workspaces(environ, include_fuerte=True, include_non_existing=True):
|
||||||
|
path_to_find = os.path.join(ws_path, subfolder) if subfolder else ws_path
|
||||||
|
path_to_remove = None
|
||||||
|
for env_path in env_paths:
|
||||||
|
env_path_clean = env_path[:-1] if env_path and env_path[-1] in [os.path.sep, os.path.altsep] else env_path
|
||||||
|
if env_path_clean == path_to_find:
|
||||||
|
path_to_remove = env_path
|
||||||
|
break
|
||||||
|
if path_to_remove:
|
||||||
|
env_paths.remove(path_to_remove)
|
||||||
|
value_modified = True
|
||||||
|
new_value = os.pathsep.join(env_paths)
|
||||||
|
return new_value if value_modified else None
|
||||||
|
|
||||||
|
|
||||||
|
def _get_workspaces(environ, include_fuerte=False, include_non_existing=False):
|
||||||
|
"""
|
||||||
|
Based on CMAKE_PREFIX_PATH return all catkin workspaces.
|
||||||
|
|
||||||
|
:param include_fuerte: The flag if paths starting with '/opt/ros/fuerte' should be considered workspaces, ``bool``
|
||||||
|
"""
|
||||||
|
# get all cmake prefix paths
|
||||||
|
env_name = 'CMAKE_PREFIX_PATH'
|
||||||
|
value = environ[env_name] if env_name in environ else ''
|
||||||
|
paths = [path for path in value.split(os.pathsep) if path]
|
||||||
|
# remove non-workspace paths
|
||||||
|
workspaces = [path for path in paths if os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE)) or (include_fuerte and path.startswith('/opt/ros/fuerte')) or (include_non_existing and not os.path.exists(path))]
|
||||||
|
return workspaces
|
||||||
|
|
||||||
|
|
||||||
|
def prepend_env_variables(environ, env_var_subfolders, workspaces):
|
||||||
|
"""Generate shell code to prepend environment variables for the all workspaces."""
|
||||||
|
lines = []
|
||||||
|
lines.append(comment('prepend folders of workspaces to environment variables'))
|
||||||
|
|
||||||
|
paths = [path for path in workspaces.split(os.pathsep) if path]
|
||||||
|
|
||||||
|
prefix = _prefix_env_variable(environ, 'CMAKE_PREFIX_PATH', paths, '')
|
||||||
|
lines.append(prepend(environ, 'CMAKE_PREFIX_PATH', prefix))
|
||||||
|
|
||||||
|
for key in sorted(key for key in env_var_subfolders.keys() if key != 'CMAKE_PREFIX_PATH'):
|
||||||
|
subfolder = env_var_subfolders[key]
|
||||||
|
prefix = _prefix_env_variable(environ, key, paths, subfolder)
|
||||||
|
lines.append(prepend(environ, key, prefix))
|
||||||
|
return lines
|
||||||
|
|
||||||
|
|
||||||
|
def _prefix_env_variable(environ, name, paths, subfolders):
|
||||||
|
"""
|
||||||
|
Return the prefix to prepend to the environment variable NAME.
|
||||||
|
|
||||||
|
Adding any path in NEW_PATHS_STR without creating duplicate or empty items.
|
||||||
|
"""
|
||||||
|
value = environ[name] if name in environ else ''
|
||||||
|
environ_paths = [path for path in value.split(os.pathsep) if path]
|
||||||
|
checked_paths = []
|
||||||
|
for path in paths:
|
||||||
|
if not isinstance(subfolders, list):
|
||||||
|
subfolders = [subfolders]
|
||||||
|
for subfolder in subfolders:
|
||||||
|
path_tmp = path
|
||||||
|
if subfolder:
|
||||||
|
path_tmp = os.path.join(path_tmp, subfolder)
|
||||||
|
# skip nonexistent paths
|
||||||
|
if not os.path.exists(path_tmp):
|
||||||
|
continue
|
||||||
|
# exclude any path already in env and any path we already added
|
||||||
|
if path_tmp not in environ_paths and path_tmp not in checked_paths:
|
||||||
|
checked_paths.append(path_tmp)
|
||||||
|
prefix_str = os.pathsep.join(checked_paths)
|
||||||
|
if prefix_str != '' and environ_paths:
|
||||||
|
prefix_str += os.pathsep
|
||||||
|
return prefix_str
|
||||||
|
|
||||||
|
|
||||||
|
def assignment(key, value):
|
||||||
|
if not IS_WINDOWS:
|
||||||
|
return 'export %s="%s"' % (key, value)
|
||||||
|
else:
|
||||||
|
return 'set %s=%s' % (key, value)
|
||||||
|
|
||||||
|
|
||||||
|
def comment(msg):
|
||||||
|
if not IS_WINDOWS:
|
||||||
|
return '# %s' % msg
|
||||||
|
else:
|
||||||
|
return 'REM %s' % msg
|
||||||
|
|
||||||
|
|
||||||
|
def prepend(environ, key, prefix):
|
||||||
|
if key not in environ or not environ[key]:
|
||||||
|
return assignment(key, prefix)
|
||||||
|
if not IS_WINDOWS:
|
||||||
|
return 'export %s="%s$%s"' % (key, prefix, key)
|
||||||
|
else:
|
||||||
|
return 'set %s=%s%%%s%%' % (key, prefix, key)
|
||||||
|
|
||||||
|
|
||||||
|
def find_env_hooks(environ, cmake_prefix_path):
|
||||||
|
"""Generate shell code with found environment hooks for the all workspaces."""
|
||||||
|
lines = []
|
||||||
|
lines.append(comment('found environment hooks in workspaces'))
|
||||||
|
|
||||||
|
generic_env_hooks = []
|
||||||
|
generic_env_hooks_workspace = []
|
||||||
|
specific_env_hooks = []
|
||||||
|
specific_env_hooks_workspace = []
|
||||||
|
generic_env_hooks_by_filename = {}
|
||||||
|
specific_env_hooks_by_filename = {}
|
||||||
|
generic_env_hook_ext = 'bat' if IS_WINDOWS else 'sh'
|
||||||
|
specific_env_hook_ext = environ['CATKIN_SHELL'] if not IS_WINDOWS and 'CATKIN_SHELL' in environ and environ['CATKIN_SHELL'] else None
|
||||||
|
# remove non-workspace paths
|
||||||
|
workspaces = [path for path in cmake_prefix_path.split(os.pathsep) if path and os.path.isfile(os.path.join(path, CATKIN_MARKER_FILE))]
|
||||||
|
for workspace in reversed(workspaces):
|
||||||
|
env_hook_dir = os.path.join(workspace, 'etc', 'catkin', 'profile.d')
|
||||||
|
if os.path.isdir(env_hook_dir):
|
||||||
|
for filename in sorted(os.listdir(env_hook_dir)):
|
||||||
|
if filename.endswith('.%s' % generic_env_hook_ext):
|
||||||
|
# remove previous env hook with same name if present
|
||||||
|
if filename in generic_env_hooks_by_filename:
|
||||||
|
i = generic_env_hooks.index(generic_env_hooks_by_filename[filename])
|
||||||
|
generic_env_hooks.pop(i)
|
||||||
|
generic_env_hooks_workspace.pop(i)
|
||||||
|
# append env hook
|
||||||
|
generic_env_hooks.append(os.path.join(env_hook_dir, filename))
|
||||||
|
generic_env_hooks_workspace.append(workspace)
|
||||||
|
generic_env_hooks_by_filename[filename] = generic_env_hooks[-1]
|
||||||
|
elif specific_env_hook_ext is not None and filename.endswith('.%s' % specific_env_hook_ext):
|
||||||
|
# remove previous env hook with same name if present
|
||||||
|
if filename in specific_env_hooks_by_filename:
|
||||||
|
i = specific_env_hooks.index(specific_env_hooks_by_filename[filename])
|
||||||
|
specific_env_hooks.pop(i)
|
||||||
|
specific_env_hooks_workspace.pop(i)
|
||||||
|
# append env hook
|
||||||
|
specific_env_hooks.append(os.path.join(env_hook_dir, filename))
|
||||||
|
specific_env_hooks_workspace.append(workspace)
|
||||||
|
specific_env_hooks_by_filename[filename] = specific_env_hooks[-1]
|
||||||
|
env_hooks = generic_env_hooks + specific_env_hooks
|
||||||
|
env_hooks_workspace = generic_env_hooks_workspace + specific_env_hooks_workspace
|
||||||
|
count = len(env_hooks)
|
||||||
|
lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_COUNT', count))
|
||||||
|
for i in range(count):
|
||||||
|
lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d' % i, env_hooks[i]))
|
||||||
|
lines.append(assignment('_CATKIN_ENVIRONMENT_HOOKS_%d_WORKSPACE' % i, env_hooks_workspace[i]))
|
||||||
|
return lines
|
||||||
|
|
||||||
|
|
||||||
|
def _parse_arguments(args=None):
|
||||||
|
parser = argparse.ArgumentParser(description='Generates code blocks for the setup.SHELL script.')
|
||||||
|
parser.add_argument('--extend', action='store_true', help='Skip unsetting previous environment variables to extend context')
|
||||||
|
parser.add_argument('--local', action='store_true', help='Only consider this prefix path and ignore other prefix path in the environment')
|
||||||
|
return parser.parse_known_args(args=args)[0]
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
try:
|
||||||
|
try:
|
||||||
|
args = _parse_arguments()
|
||||||
|
except Exception as e:
|
||||||
|
print(e, file=sys.stderr)
|
||||||
|
sys.exit(1)
|
||||||
|
|
||||||
|
if not args.local:
|
||||||
|
# environment at generation time
|
||||||
|
CMAKE_PREFIX_PATH = r'/home/firefly/pibot_ros/ros_ws/devel;/opt/ros/melodic'.split(';')
|
||||||
|
else:
|
||||||
|
# don't consider any other prefix path than this one
|
||||||
|
CMAKE_PREFIX_PATH = []
|
||||||
|
# prepend current workspace if not already part of CPP
|
||||||
|
base_path = os.path.dirname(__file__)
|
||||||
|
# CMAKE_PREFIX_PATH uses forward slash on all platforms, but __file__ is platform dependent
|
||||||
|
# base_path on Windows contains backward slashes, need to be converted to forward slashes before comparison
|
||||||
|
if os.path.sep != '/':
|
||||||
|
base_path = base_path.replace(os.path.sep, '/')
|
||||||
|
|
||||||
|
if base_path not in CMAKE_PREFIX_PATH:
|
||||||
|
CMAKE_PREFIX_PATH.insert(0, base_path)
|
||||||
|
CMAKE_PREFIX_PATH = os.pathsep.join(CMAKE_PREFIX_PATH)
|
||||||
|
|
||||||
|
environ = dict(os.environ)
|
||||||
|
lines = []
|
||||||
|
if not args.extend:
|
||||||
|
lines += rollback_env_variables(environ, ENV_VAR_SUBFOLDERS)
|
||||||
|
lines += prepend_env_variables(environ, ENV_VAR_SUBFOLDERS, CMAKE_PREFIX_PATH)
|
||||||
|
lines += find_env_hooks(environ, CMAKE_PREFIX_PATH)
|
||||||
|
print('\n'.join(lines))
|
||||||
|
|
||||||
|
# need to explicitly flush the output
|
||||||
|
sys.stdout.flush()
|
||||||
|
except IOError as e:
|
||||||
|
# and catch potential "broken pipe" if stdout is not writable
|
||||||
|
# which can happen when piping the output to a file but the disk is full
|
||||||
|
if e.errno == errno.EPIPE:
|
||||||
|
print(e, file=sys.stderr)
|
||||||
|
sys.exit(2)
|
||||||
|
raise
|
||||||
|
|
||||||
|
sys.exit(0)
|
||||||
|
|
@ -0,0 +1,16 @@
|
||||||
|
#!/usr/bin/env sh
|
||||||
|
# generated from catkin/cmake/templates/env.sh.in
|
||||||
|
|
||||||
|
if [ $# -eq 0 ] ; then
|
||||||
|
/bin/echo "Usage: env.sh COMMANDS"
|
||||||
|
/bin/echo "Calling env.sh without arguments is not supported anymore. Instead spawn a subshell and source a setup file manually."
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
# ensure to not use different shell type which was set before
|
||||||
|
CATKIN_SHELL=sh
|
||||||
|
|
||||||
|
# source setup.sh from same directory as this file
|
||||||
|
_CATKIN_SETUP_DIR=$(cd "`dirname "$0"`" > /dev/null && pwd)
|
||||||
|
. "$_CATKIN_SETUP_DIR/setup.sh"
|
||||||
|
exec "$@"
|
||||||
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,8 @@
|
||||||
|
prefix=/home/firefly/sweeping_robot/devel
|
||||||
|
|
||||||
|
Name: fusion_car
|
||||||
|
Description: Description of fusion_car
|
||||||
|
Version: 0.0.0
|
||||||
|
Cflags:
|
||||||
|
Libs: -L${prefix}/lib
|
||||||
|
Requires:
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
#!/usr/bin/env bash
|
||||||
|
# generated from catkin/cmake/templates/local_setup.bash.in
|
||||||
|
|
||||||
|
CATKIN_SHELL=bash
|
||||||
|
|
||||||
|
# source setup.sh from same directory as this file
|
||||||
|
_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
|
||||||
|
. "$_CATKIN_SETUP_DIR/setup.sh" --extend --local
|
||||||
|
|
@ -0,0 +1,9 @@
|
||||||
|
#!/usr/bin/env sh
|
||||||
|
# generated from catkin/cmake/template/local_setup.sh.in
|
||||||
|
|
||||||
|
# since this file is sourced either use the provided _CATKIN_SETUP_DIR
|
||||||
|
# or fall back to the destination set at configure time
|
||||||
|
: ${_CATKIN_SETUP_DIR:=/home/firefly/sweeping_robot/devel}
|
||||||
|
CATKIN_SETUP_UTIL_ARGS="--extend --local"
|
||||||
|
. "$_CATKIN_SETUP_DIR/setup.sh"
|
||||||
|
unset CATKIN_SETUP_UTIL_ARGS
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
#!/usr/bin/env zsh
|
||||||
|
# generated from catkin/cmake/templates/local_setup.zsh.in
|
||||||
|
|
||||||
|
CATKIN_SHELL=zsh
|
||||||
|
|
||||||
|
# source setup.sh from same directory as this file
|
||||||
|
_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
|
||||||
|
emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh" --extend --local'
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
#!/usr/bin/env bash
|
||||||
|
# generated from catkin/cmake/templates/setup.bash.in
|
||||||
|
|
||||||
|
CATKIN_SHELL=bash
|
||||||
|
|
||||||
|
# source setup.sh from same directory as this file
|
||||||
|
_CATKIN_SETUP_DIR=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)
|
||||||
|
. "$_CATKIN_SETUP_DIR/setup.sh"
|
||||||
|
|
@ -0,0 +1,96 @@
|
||||||
|
#!/usr/bin/env sh
|
||||||
|
# generated from catkin/cmake/template/setup.sh.in
|
||||||
|
|
||||||
|
# Sets various environment variables and sources additional environment hooks.
|
||||||
|
# It tries it's best to undo changes from a previously sourced setup file before.
|
||||||
|
# Supported command line options:
|
||||||
|
# --extend: skips the undoing of changes from a previously sourced setup file
|
||||||
|
# --local: only considers this workspace but not the chained ones
|
||||||
|
# In plain sh shell which doesn't support arguments for sourced scripts you can
|
||||||
|
# set the environment variable `CATKIN_SETUP_UTIL_ARGS=--extend/--local` instead.
|
||||||
|
|
||||||
|
# since this file is sourced either use the provided _CATKIN_SETUP_DIR
|
||||||
|
# or fall back to the destination set at configure time
|
||||||
|
: ${_CATKIN_SETUP_DIR:=/home/firefly/sweeping_robot/devel}
|
||||||
|
_SETUP_UTIL="$_CATKIN_SETUP_DIR/_setup_util.py"
|
||||||
|
unset _CATKIN_SETUP_DIR
|
||||||
|
|
||||||
|
if [ ! -f "$_SETUP_UTIL" ]; then
|
||||||
|
echo "Missing Python script: $_SETUP_UTIL"
|
||||||
|
return 22
|
||||||
|
fi
|
||||||
|
|
||||||
|
# detect if running on Darwin platform
|
||||||
|
_UNAME=`uname -s`
|
||||||
|
_IS_DARWIN=0
|
||||||
|
if [ "$_UNAME" = "Darwin" ]; then
|
||||||
|
_IS_DARWIN=1
|
||||||
|
fi
|
||||||
|
unset _UNAME
|
||||||
|
|
||||||
|
# make sure to export all environment variables
|
||||||
|
export CMAKE_PREFIX_PATH
|
||||||
|
if [ $_IS_DARWIN -eq 0 ]; then
|
||||||
|
export LD_LIBRARY_PATH
|
||||||
|
else
|
||||||
|
export DYLD_LIBRARY_PATH
|
||||||
|
fi
|
||||||
|
unset _IS_DARWIN
|
||||||
|
export PATH
|
||||||
|
export PKG_CONFIG_PATH
|
||||||
|
export PYTHONPATH
|
||||||
|
|
||||||
|
# remember type of shell if not already set
|
||||||
|
if [ -z "$CATKIN_SHELL" ]; then
|
||||||
|
CATKIN_SHELL=sh
|
||||||
|
fi
|
||||||
|
|
||||||
|
# invoke Python script to generate necessary exports of environment variables
|
||||||
|
# use TMPDIR if it exists, otherwise fall back to /tmp
|
||||||
|
if [ -d "${TMPDIR:-}" ]; then
|
||||||
|
_TMPDIR="${TMPDIR}"
|
||||||
|
else
|
||||||
|
_TMPDIR=/tmp
|
||||||
|
fi
|
||||||
|
_SETUP_TMP=`mktemp "${_TMPDIR}/setup.sh.XXXXXXXXXX"`
|
||||||
|
unset _TMPDIR
|
||||||
|
if [ $? -ne 0 -o ! -f "$_SETUP_TMP" ]; then
|
||||||
|
echo "Could not create temporary file: $_SETUP_TMP"
|
||||||
|
return 1
|
||||||
|
fi
|
||||||
|
CATKIN_SHELL=$CATKIN_SHELL "$_SETUP_UTIL" $@ ${CATKIN_SETUP_UTIL_ARGS:-} >> "$_SETUP_TMP"
|
||||||
|
_RC=$?
|
||||||
|
if [ $_RC -ne 0 ]; then
|
||||||
|
if [ $_RC -eq 2 ]; then
|
||||||
|
echo "Could not write the output of '$_SETUP_UTIL' to temporary file '$_SETUP_TMP': may be the disk if full?"
|
||||||
|
else
|
||||||
|
echo "Failed to run '\"$_SETUP_UTIL\" $@': return code $_RC"
|
||||||
|
fi
|
||||||
|
unset _RC
|
||||||
|
unset _SETUP_UTIL
|
||||||
|
rm -f "$_SETUP_TMP"
|
||||||
|
unset _SETUP_TMP
|
||||||
|
return 1
|
||||||
|
fi
|
||||||
|
unset _RC
|
||||||
|
unset _SETUP_UTIL
|
||||||
|
. "$_SETUP_TMP"
|
||||||
|
rm -f "$_SETUP_TMP"
|
||||||
|
unset _SETUP_TMP
|
||||||
|
|
||||||
|
# source all environment hooks
|
||||||
|
_i=0
|
||||||
|
while [ $_i -lt $_CATKIN_ENVIRONMENT_HOOKS_COUNT ]; do
|
||||||
|
eval _envfile=\$_CATKIN_ENVIRONMENT_HOOKS_$_i
|
||||||
|
unset _CATKIN_ENVIRONMENT_HOOKS_$_i
|
||||||
|
eval _envfile_workspace=\$_CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
|
||||||
|
unset _CATKIN_ENVIRONMENT_HOOKS_${_i}_WORKSPACE
|
||||||
|
# set workspace for environment hook
|
||||||
|
CATKIN_ENV_HOOK_WORKSPACE=$_envfile_workspace
|
||||||
|
. "$_envfile"
|
||||||
|
unset CATKIN_ENV_HOOK_WORKSPACE
|
||||||
|
_i=$((_i + 1))
|
||||||
|
done
|
||||||
|
unset _i
|
||||||
|
|
||||||
|
unset _CATKIN_ENVIRONMENT_HOOKS_COUNT
|
||||||
|
|
@ -0,0 +1,8 @@
|
||||||
|
#!/usr/bin/env zsh
|
||||||
|
# generated from catkin/cmake/templates/setup.zsh.in
|
||||||
|
|
||||||
|
CATKIN_SHELL=zsh
|
||||||
|
|
||||||
|
# source setup.sh from same directory as this file
|
||||||
|
_CATKIN_SETUP_DIR=$(builtin cd -q "`dirname "$0"`" > /dev/null && pwd)
|
||||||
|
emulate -R zsh -c 'source "$_CATKIN_SETUP_DIR/setup.sh"'
|
||||||
|
|
@ -0,0 +1,14 @@
|
||||||
|
# generated from catkin/cmake/template/pkgConfig-version.cmake.in
|
||||||
|
set(PACKAGE_VERSION "0.0.0")
|
||||||
|
|
||||||
|
set(PACKAGE_VERSION_EXACT False)
|
||||||
|
set(PACKAGE_VERSION_COMPATIBLE False)
|
||||||
|
|
||||||
|
if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}")
|
||||||
|
set(PACKAGE_VERSION_EXACT True)
|
||||||
|
set(PACKAGE_VERSION_COMPATIBLE True)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}")
|
||||||
|
set(PACKAGE_VERSION_COMPATIBLE True)
|
||||||
|
endif()
|
||||||
|
|
@ -0,0 +1,223 @@
|
||||||
|
# generated from catkin/cmake/template/pkgConfig.cmake.in
|
||||||
|
|
||||||
|
# append elements to a list and remove existing duplicates from the list
|
||||||
|
# copied from catkin/cmake/list_append_deduplicate.cmake to keep pkgConfig
|
||||||
|
# self contained
|
||||||
|
macro(_list_append_deduplicate listname)
|
||||||
|
if(NOT "${ARGN}" STREQUAL "")
|
||||||
|
if(${listname})
|
||||||
|
list(REMOVE_ITEM ${listname} ${ARGN})
|
||||||
|
endif()
|
||||||
|
list(APPEND ${listname} ${ARGN})
|
||||||
|
endif()
|
||||||
|
endmacro()
|
||||||
|
|
||||||
|
# append elements to a list if they are not already in the list
|
||||||
|
# copied from catkin/cmake/list_append_unique.cmake to keep pkgConfig
|
||||||
|
# self contained
|
||||||
|
macro(_list_append_unique listname)
|
||||||
|
foreach(_item ${ARGN})
|
||||||
|
list(FIND ${listname} ${_item} _index)
|
||||||
|
if(_index EQUAL -1)
|
||||||
|
list(APPEND ${listname} ${_item})
|
||||||
|
endif()
|
||||||
|
endforeach()
|
||||||
|
endmacro()
|
||||||
|
|
||||||
|
# pack a list of libraries with optional build configuration keywords
|
||||||
|
# copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig
|
||||||
|
# self contained
|
||||||
|
macro(_pack_libraries_with_build_configuration VAR)
|
||||||
|
set(${VAR} "")
|
||||||
|
set(_argn ${ARGN})
|
||||||
|
list(LENGTH _argn _count)
|
||||||
|
set(_index 0)
|
||||||
|
while(${_index} LESS ${_count})
|
||||||
|
list(GET _argn ${_index} lib)
|
||||||
|
if("${lib}" MATCHES "^(debug|optimized|general)$")
|
||||||
|
math(EXPR _index "${_index} + 1")
|
||||||
|
if(${_index} EQUAL ${_count})
|
||||||
|
message(FATAL_ERROR "_pack_libraries_with_build_configuration() the list of libraries '${ARGN}' ends with '${lib}' which is a build configuration keyword and must be followed by a library")
|
||||||
|
endif()
|
||||||
|
list(GET _argn ${_index} library)
|
||||||
|
list(APPEND ${VAR} "${lib}${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}${library}")
|
||||||
|
else()
|
||||||
|
list(APPEND ${VAR} "${lib}")
|
||||||
|
endif()
|
||||||
|
math(EXPR _index "${_index} + 1")
|
||||||
|
endwhile()
|
||||||
|
endmacro()
|
||||||
|
|
||||||
|
# unpack a list of libraries with optional build configuration keyword prefixes
|
||||||
|
# copied from catkin/cmake/catkin_libraries.cmake to keep pkgConfig
|
||||||
|
# self contained
|
||||||
|
macro(_unpack_libraries_with_build_configuration VAR)
|
||||||
|
set(${VAR} "")
|
||||||
|
foreach(lib ${ARGN})
|
||||||
|
string(REGEX REPLACE "^(debug|optimized|general)${CATKIN_BUILD_CONFIGURATION_KEYWORD_SEPARATOR}(.+)$" "\\1;\\2" lib "${lib}")
|
||||||
|
list(APPEND ${VAR} "${lib}")
|
||||||
|
endforeach()
|
||||||
|
endmacro()
|
||||||
|
|
||||||
|
|
||||||
|
if(fusion_car_CONFIG_INCLUDED)
|
||||||
|
return()
|
||||||
|
endif()
|
||||||
|
set(fusion_car_CONFIG_INCLUDED TRUE)
|
||||||
|
|
||||||
|
# set variables for source/devel/install prefixes
|
||||||
|
if("TRUE" STREQUAL "TRUE")
|
||||||
|
set(fusion_car_SOURCE_PREFIX /home/firefly/sweeping_robot/src/fusion_car)
|
||||||
|
set(fusion_car_DEVEL_PREFIX /home/firefly/sweeping_robot/devel)
|
||||||
|
set(fusion_car_INSTALL_PREFIX "")
|
||||||
|
set(fusion_car_PREFIX ${fusion_car_DEVEL_PREFIX})
|
||||||
|
else()
|
||||||
|
set(fusion_car_SOURCE_PREFIX "")
|
||||||
|
set(fusion_car_DEVEL_PREFIX "")
|
||||||
|
set(fusion_car_INSTALL_PREFIX /home/firefly/sweeping_robot/install)
|
||||||
|
set(fusion_car_PREFIX ${fusion_car_INSTALL_PREFIX})
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# warn when using a deprecated package
|
||||||
|
if(NOT "" STREQUAL "")
|
||||||
|
set(_msg "WARNING: package 'fusion_car' is deprecated")
|
||||||
|
# append custom deprecation text if available
|
||||||
|
if(NOT "" STREQUAL "TRUE")
|
||||||
|
set(_msg "${_msg} ()")
|
||||||
|
endif()
|
||||||
|
message("${_msg}")
|
||||||
|
endif()
|
||||||
|
|
||||||
|
# flag project as catkin-based to distinguish if a find_package()-ed project is a catkin project
|
||||||
|
set(fusion_car_FOUND_CATKIN_PROJECT TRUE)
|
||||||
|
|
||||||
|
if(NOT " " STREQUAL " ")
|
||||||
|
set(fusion_car_INCLUDE_DIRS "")
|
||||||
|
set(_include_dirs "")
|
||||||
|
if(NOT " " STREQUAL " ")
|
||||||
|
set(_report "Check the issue tracker '' and consider creating a ticket if the problem has not been reported yet.")
|
||||||
|
elseif(NOT " " STREQUAL " ")
|
||||||
|
set(_report "Check the website '' for information and consider reporting the problem.")
|
||||||
|
else()
|
||||||
|
set(_report "Report the problem to the maintainer 'firefly <firefly@todo.todo>' and request to fix the problem.")
|
||||||
|
endif()
|
||||||
|
foreach(idir ${_include_dirs})
|
||||||
|
if(IS_ABSOLUTE ${idir} AND IS_DIRECTORY ${idir})
|
||||||
|
set(include ${idir})
|
||||||
|
elseif("${idir} " STREQUAL "include ")
|
||||||
|
get_filename_component(include "${fusion_car_DIR}/../../../include" ABSOLUTE)
|
||||||
|
if(NOT IS_DIRECTORY ${include})
|
||||||
|
message(FATAL_ERROR "Project 'fusion_car' specifies '${idir}' as an include dir, which is not found. It does not exist in '${include}'. ${_report}")
|
||||||
|
endif()
|
||||||
|
else()
|
||||||
|
message(FATAL_ERROR "Project 'fusion_car' specifies '${idir}' as an include dir, which is not found. It does neither exist as an absolute directory nor in '/home/firefly/sweeping_robot/src/fusion_car/${idir}'. ${_report}")
|
||||||
|
endif()
|
||||||
|
_list_append_unique(fusion_car_INCLUDE_DIRS ${include})
|
||||||
|
endforeach()
|
||||||
|
endif()
|
||||||
|
|
||||||
|
set(libraries "")
|
||||||
|
foreach(library ${libraries})
|
||||||
|
# keep build configuration keywords, target names and absolute libraries as-is
|
||||||
|
if("${library}" MATCHES "^(debug|optimized|general)$")
|
||||||
|
list(APPEND fusion_car_LIBRARIES ${library})
|
||||||
|
elseif(${library} MATCHES "^-l")
|
||||||
|
list(APPEND fusion_car_LIBRARIES ${library})
|
||||||
|
elseif(${library} MATCHES "^-")
|
||||||
|
# This is a linker flag/option (like -pthread)
|
||||||
|
# There's no standard variable for these, so create an interface library to hold it
|
||||||
|
if(NOT fusion_car_NUM_DUMMY_TARGETS)
|
||||||
|
set(fusion_car_NUM_DUMMY_TARGETS 0)
|
||||||
|
endif()
|
||||||
|
# Make sure the target name is unique
|
||||||
|
set(interface_target_name "catkin::fusion_car::wrapped-linker-option${fusion_car_NUM_DUMMY_TARGETS}")
|
||||||
|
while(TARGET "${interface_target_name}")
|
||||||
|
math(EXPR fusion_car_NUM_DUMMY_TARGETS "${fusion_car_NUM_DUMMY_TARGETS}+1")
|
||||||
|
set(interface_target_name "catkin::fusion_car::wrapped-linker-option${fusion_car_NUM_DUMMY_TARGETS}")
|
||||||
|
endwhile()
|
||||||
|
add_library("${interface_target_name}" INTERFACE IMPORTED)
|
||||||
|
if("${CMAKE_VERSION}" VERSION_LESS "3.13.0")
|
||||||
|
set_property(
|
||||||
|
TARGET
|
||||||
|
"${interface_target_name}"
|
||||||
|
APPEND PROPERTY
|
||||||
|
INTERFACE_LINK_LIBRARIES "${library}")
|
||||||
|
else()
|
||||||
|
target_link_options("${interface_target_name}" INTERFACE "${library}")
|
||||||
|
endif()
|
||||||
|
list(APPEND fusion_car_LIBRARIES "${interface_target_name}")
|
||||||
|
elseif(TARGET ${library})
|
||||||
|
list(APPEND fusion_car_LIBRARIES ${library})
|
||||||
|
elseif(IS_ABSOLUTE ${library})
|
||||||
|
list(APPEND fusion_car_LIBRARIES ${library})
|
||||||
|
else()
|
||||||
|
set(lib_path "")
|
||||||
|
set(lib "${library}-NOTFOUND")
|
||||||
|
# since the path where the library is found is returned we have to iterate over the paths manually
|
||||||
|
foreach(path /home/firefly/sweeping_robot/devel/lib;/home/firefly/pibot_ros/ros_ws/devel/lib;/opt/ros/melodic/lib)
|
||||||
|
find_library(lib ${library}
|
||||||
|
PATHS ${path}
|
||||||
|
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
|
||||||
|
if(lib)
|
||||||
|
set(lib_path ${path})
|
||||||
|
break()
|
||||||
|
endif()
|
||||||
|
endforeach()
|
||||||
|
if(lib)
|
||||||
|
_list_append_unique(fusion_car_LIBRARY_DIRS ${lib_path})
|
||||||
|
list(APPEND fusion_car_LIBRARIES ${lib})
|
||||||
|
else()
|
||||||
|
# as a fall back for non-catkin libraries try to search globally
|
||||||
|
find_library(lib ${library})
|
||||||
|
if(NOT lib)
|
||||||
|
message(FATAL_ERROR "Project '${PROJECT_NAME}' tried to find library '${library}'. The library is neither a target nor built/installed properly. Did you compile project 'fusion_car'? Did you find_package() it before the subdirectory containing its code is included?")
|
||||||
|
endif()
|
||||||
|
list(APPEND fusion_car_LIBRARIES ${lib})
|
||||||
|
endif()
|
||||||
|
endif()
|
||||||
|
endforeach()
|
||||||
|
|
||||||
|
set(fusion_car_EXPORTED_TARGETS "")
|
||||||
|
# create dummy targets for exported code generation targets to make life of users easier
|
||||||
|
foreach(t ${fusion_car_EXPORTED_TARGETS})
|
||||||
|
if(NOT TARGET ${t})
|
||||||
|
add_custom_target(${t})
|
||||||
|
endif()
|
||||||
|
endforeach()
|
||||||
|
|
||||||
|
set(depends "")
|
||||||
|
foreach(depend ${depends})
|
||||||
|
string(REPLACE " " ";" depend_list ${depend})
|
||||||
|
# the package name of the dependency must be kept in a unique variable so that it is not overwritten in recursive calls
|
||||||
|
list(GET depend_list 0 fusion_car_dep)
|
||||||
|
list(LENGTH depend_list count)
|
||||||
|
if(${count} EQUAL 1)
|
||||||
|
# simple dependencies must only be find_package()-ed once
|
||||||
|
if(NOT ${fusion_car_dep}_FOUND)
|
||||||
|
find_package(${fusion_car_dep} REQUIRED NO_MODULE)
|
||||||
|
endif()
|
||||||
|
else()
|
||||||
|
# dependencies with components must be find_package()-ed again
|
||||||
|
list(REMOVE_AT depend_list 0)
|
||||||
|
find_package(${fusion_car_dep} REQUIRED NO_MODULE ${depend_list})
|
||||||
|
endif()
|
||||||
|
_list_append_unique(fusion_car_INCLUDE_DIRS ${${fusion_car_dep}_INCLUDE_DIRS})
|
||||||
|
|
||||||
|
# merge build configuration keywords with library names to correctly deduplicate
|
||||||
|
_pack_libraries_with_build_configuration(fusion_car_LIBRARIES ${fusion_car_LIBRARIES})
|
||||||
|
_pack_libraries_with_build_configuration(_libraries ${${fusion_car_dep}_LIBRARIES})
|
||||||
|
_list_append_deduplicate(fusion_car_LIBRARIES ${_libraries})
|
||||||
|
# undo build configuration keyword merging after deduplication
|
||||||
|
_unpack_libraries_with_build_configuration(fusion_car_LIBRARIES ${fusion_car_LIBRARIES})
|
||||||
|
|
||||||
|
_list_append_unique(fusion_car_LIBRARY_DIRS ${${fusion_car_dep}_LIBRARY_DIRS})
|
||||||
|
list(APPEND fusion_car_EXPORTED_TARGETS ${${fusion_car_dep}_EXPORTED_TARGETS})
|
||||||
|
endforeach()
|
||||||
|
|
||||||
|
set(pkg_cfg_extras "")
|
||||||
|
foreach(extra ${pkg_cfg_extras})
|
||||||
|
if(NOT IS_ABSOLUTE ${extra})
|
||||||
|
set(extra ${fusion_car_DIR}/${extra})
|
||||||
|
endif()
|
||||||
|
include(${extra})
|
||||||
|
endforeach()
|
||||||
|
|
@ -0,0 +1,62 @@
|
||||||
|
cmake_minimum_required(VERSION 3.0.2)
|
||||||
|
project(fusion_car)
|
||||||
|
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED COMPONENTS
|
||||||
|
rosconsole
|
||||||
|
roscpp
|
||||||
|
roscpp_serialization
|
||||||
|
roslib
|
||||||
|
rospy
|
||||||
|
rostime
|
||||||
|
)
|
||||||
|
find_package(OpenCV REQUIRED)
|
||||||
|
catkin_package(
|
||||||
|
# INCLUDE_DIRS include
|
||||||
|
# LIBRARIES dynamic_connect
|
||||||
|
# CATKIN_DEPENDS roscpp rosmsg rospy
|
||||||
|
# DEPENDS system_lib
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
include_directories(
|
||||||
|
include
|
||||||
|
/usr/local/include
|
||||||
|
/usr/local/include/opencv4
|
||||||
|
/usr/include
|
||||||
|
/opt/ros/melodic/include
|
||||||
|
/home/firefly/Desktop/object_detect_sdk_1804/install/include
|
||||||
|
${catkin_INCLUDE_DIRS}
|
||||||
|
${OpenCV_INCLUDE_DIRS}
|
||||||
|
)
|
||||||
|
|
||||||
|
add_library(head
|
||||||
|
include/fusion_car/fusion.h
|
||||||
|
include/fusion_car/lidar.h
|
||||||
|
include/fusion_car/ranging.h
|
||||||
|
include/fusion_car/rknn_sdk.h
|
||||||
|
src/fusion.cpp
|
||||||
|
src/lidar.cpp
|
||||||
|
src/ranging.cpp
|
||||||
|
)
|
||||||
|
add_dependencies(head ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||||
|
target_link_libraries(head
|
||||||
|
${catkin_LIBRARIES}
|
||||||
|
)
|
||||||
|
add_executable(main src/main.cpp)
|
||||||
|
|
||||||
|
add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||||
|
|
||||||
|
target_link_libraries(main
|
||||||
|
head
|
||||||
|
${catkin_LIBRARIES}
|
||||||
|
${OpenCV_LIBS}
|
||||||
|
boost_system
|
||||||
|
pthread
|
||||||
|
udev
|
||||||
|
/home/firefly/sweeping_robot/src/librknn_sdk.so
|
||||||
|
/home/firefly/sweeping_robot/src/librknnrt.so
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,100 @@
|
||||||
|
#pragma once
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/LaserScan.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
#include "lidar.h"
|
||||||
|
#include "ranging.h"
|
||||||
|
|
||||||
|
using namespace cv;
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
struct ImgScan
|
||||||
|
{
|
||||||
|
float angleMin; //弧度
|
||||||
|
float angleMax;
|
||||||
|
float angleInc;
|
||||||
|
std::vector<float> distance; // 米
|
||||||
|
std::vector<uint8_t> intensities;
|
||||||
|
Mat pointCloud;
|
||||||
|
std::vector<std::vector<Mat>> clustedCloud;
|
||||||
|
};
|
||||||
|
class Ranging; //友元类
|
||||||
|
class LCFusion
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
LCFusion(Ranging* ranging)
|
||||||
|
{
|
||||||
|
r = ranging;
|
||||||
|
rotate = cameraExtrinsicMat.rowRange(0, 3).colRange(0, 3).t(); // 雷达 -> 相机坐标系旋转矩阵
|
||||||
|
translation = cameraExtrinsicMat.col(3).rowRange(0, 3);
|
||||||
|
translation = -rotate * translation; // 雷达 -> 相机坐标系平移矩阵
|
||||||
|
p2r = r_y * r_c; // 像素坐标系矫正矩阵（旋转）
|
||||||
|
};
|
||||||
|
char fusion(std::vector<Mat> &camInfo); // fusion函数
|
||||||
|
uchar lidar2box(Mat uv, Mat boxes, uchar *boxFlag); // 无用
|
||||||
|
void filter_laser(const FrameData &lidarInfo, float angle_min, float angle_inc); // 雷达点和相机视野同步
|
||||||
|
void scan_to_pointcloud(); // 雷达极坐标到二维坐标转换
|
||||||
|
std::vector<int> clusters_bboxs(Mat &bboxs); // 所有的聚类的雷达簇和所有的检测框的匹配
|
||||||
|
int cluster_bboxs(std::vector<Mat> &one_cluster, Mat &bboxs); // 寻找一簇雷达簇匹配的检测框
|
||||||
|
void optimize_clusterlable(vector<int> & clusterlable, Mat & bboxs); // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇
|
||||||
|
void choose_forceground(vector<int> & cluster_num, Mat & bbox, vector<int> & clusterlable); // 针对镂空物体选择对应的雷达簇，筛选过程
|
||||||
|
void big_fusion(vector<int> & clusterlable, Mat & bboxs, Mat &dwha); // 大物体赋语义
|
||||||
|
void small_fusion(Mat & bboxs, Mat & dwha); // 小物体赋语义
|
||||||
|
Mat pointcloud_pixel(Mat &pointcloud); // 二维雷达点到像素坐标系转换
|
||||||
|
void cluster(); // 聚类
|
||||||
|
float ratio_in_1box(Mat &begin, Mat &end, Mat &box); // 计算一簇雷达落入检测框的比例
|
||||||
|
void draw_point(std::vector<Mat> &camInfo); // 画出雷达在图像中的映射点
|
||||||
|
void draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha); // 扩散物体生成圆弧状伪激光雷达
|
||||||
|
vector<float> find_circle_center(int cluster_num); // 寻找扩散物体圆弧的圆心
|
||||||
|
float circle(float d, float alfa ,float theta, float data); // 圆弧角度和距离的数学关系
|
||||||
|
vector<float> width_ladar(const int* coordinates);
|
||||||
|
void small_box_without_dis(Mat & bboxs, Mat & dwha); // 检测框到雷达坐标系的映射关系
|
||||||
|
void set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData); // 设置雷达发布数据（原始雷达）
|
||||||
|
void set_laser_data(sensor_msgs::LaserScan& scan); // 设置雷达发布数据（语义雷达）
|
||||||
|
|
||||||
|
ImgScan upScan;
|
||||||
|
// 调试使用，将雷达点映射到图像中显示
|
||||||
|
vector<Point2f> projectedPoints; // 雷达 -> 像素坐标系映射的点
|
||||||
|
std::vector<uchar> clusterIdx;
|
||||||
|
|
||||||
|
private:
|
||||||
|
Ranging* r;
|
||||||
|
//float cameraAnglemin = 1.117, cameraAnglemax = 2.0071;
|
||||||
|
float cameraAnglemin = 1.345, cameraAnglemax = 2.0595; // 相机视野角度
|
||||||
|
Mat rotate, translation, p2r; // 雷达点映射矫正
|
||||||
|
Mat cameraExtrinsicMat = (Mat_<float>(4, 4) << 9.9710325100937192e-01, 5.9677185357037893e-02,
|
||||||
|
-4.7156551765403301e-02, 2.6591864512557874e-02,
|
||||||
|
4.7136795335845721e-02, 1.7395474156941537e-03,
|
||||||
|
9.9888692878636431e-01, 7.4034983851991365e-02,
|
||||||
|
5.9692791457662736e-02, -9.9821621281296091e-01,
|
||||||
|
-1.0784828889205400e-03, -9.6176066499866750e-02, 0., 0., 0., 1.);
|
||||||
|
|
||||||
|
Mat cameraMat = (Mat_<float>(3, 3) << 5.0280958247840368e+02, 0., 3.1925413622163182e+02, 0.,
|
||||||
|
5.0206199606708202e+02, 2.1999125419411467e+02, 0., 0., 1.);
|
||||||
|
|
||||||
|
Mat distCoeff = (Mat_<float>(1, 5) << 1.2903666948900971e-01, -9.2734018203156590e-02,
|
||||||
|
-7.0528508350750380e-03, 8.3183285124261049e-04,
|
||||||
|
7.2727934388070986e-02);
|
||||||
|
|
||||||
|
// Mat c2lR = (Mat_<float>(2, 2) << 0.99664806, 0.0802792, -0.0802792, 0.99664806);
|
||||||
|
Mat c2lR = (Mat_<float>(2, 2) << 0.98163, 0.19081, -0.19081, 0.98163); //11
|
||||||
|
//Mat c2lR = (Mat_<float>(2, 2) << 0.9703, 0.24192, -0.24192, 0.9703); //13
|
||||||
|
Mat c2lT = (Mat_<float>(2, 1) << 0.15, 0.01);
|
||||||
|
//Mat c2lT = (Mat_<float>(2, 1) << 0.13693697, 0.05094143);
|
||||||
|
Mat r_c = (Mat_<float>(3, 3) << 9.977e-1, 6.7744e-02, 0., -6.7744e-02, 9.977e-1, 0., 0., 0., 1.);
|
||||||
|
Mat t = (Mat_<float>(3, 1) << 50., 0., 0.);
|
||||||
|
Mat r_y = (Mat_<float>(3, 3) << 1., 0., 0., 0., 1.19444e+00, 0., 0., 0., 1.);
|
||||||
|
/*int class_intensity[24] = {
|
||||||
|
0, 120, 130, 130, 130, 130, 130, 130, 146, 146,
|
||||||
|
146, 162, 162, 178, 178, 196, 196, 212, 212, 228,
|
||||||
|
228, 0, 0, 0}; */
|
||||||
|
int class_intensity[25] = {
|
||||||
|
0, 115, 116, 117, 118, 119, 120, 121, 150, 151,
|
||||||
|
152, 153, 154, 155, 156, 200, 201, 202, 203, 204,
|
||||||
|
205, 206, 207, 208, 209}; // 物体类别和强度映射关系
|
||||||
|
|
||||||
|
map<int, int> class_index = {{-1, 23}, {0, 0}, {1, 13}, {2, 14}, {3, 7}, {4, 8}, {5, 9}, {6, 1},
|
||||||
|
{7, 2}, {8, 19}, {9, 18}, {10, 6}, {11, 3}, {12, 15}, {13, 10}, {14, 4}, {15, 5}, {16, 11}, {17, 16},
|
||||||
|
{18, 12}, {19, 17}, {20, 20}, {21, 21}, {22, 22}};
|
||||||
|
}; // 类别映射，方便大小物体判断
|
||||||
|
|
@ -0,0 +1,174 @@
|
||||||
|
/**
|
||||||
|
* @file lidar.h
|
||||||
|
* @author LD Robot
|
||||||
|
* @version V01
|
||||||
|
|
||||||
|
* @brief
|
||||||
|
* @note
|
||||||
|
* @attention COPYRIGHT LDROBOT
|
||||||
|
**/
|
||||||
|
|
||||||
|
#ifndef __LIPKG_H
|
||||||
|
#define __LIPKG_H
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <vector>
|
||||||
|
#include <array>
|
||||||
|
#include <iostream>
|
||||||
|
#include <functional>
|
||||||
|
#include <thread>
|
||||||
|
#include <atomic>
|
||||||
|
#include <inttypes.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include <string>
|
||||||
|
#include <condition_variable>
|
||||||
|
#include <sys/file.h>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <fcntl.h>
|
||||||
|
#include <errno.h>
|
||||||
|
#include <termios.h>
|
||||||
|
#include <memory.h>
|
||||||
|
#include <libudev.h>
|
||||||
|
#include<opencv2/opencv.hpp>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
struct PointData
|
||||||
|
{
|
||||||
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ
|
||||||
|
float angle;
|
||||||
|
uint16_t distance;
|
||||||
|
uint8_t confidence;
|
||||||
|
//ֱ<><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE>ʽ
|
||||||
|
double x;
|
||||||
|
double y;
|
||||||
|
PointData(float angle, uint16_t distance, uint8_t confidence, double x = 0, double y = 0)
|
||||||
|
{
|
||||||
|
this->angle = angle;
|
||||||
|
this->distance = distance;
|
||||||
|
this->confidence = confidence;
|
||||||
|
this->x = x;
|
||||||
|
this->y = y;
|
||||||
|
}
|
||||||
|
PointData() {}
|
||||||
|
friend std::ostream& operator<<(std::ostream& os, const PointData& data)
|
||||||
|
{
|
||||||
|
os << data.angle << " " << data.distance << " " << (int)data.confidence << " " << data.x << " " << data.y;
|
||||||
|
return os;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
typedef std::vector<PointData> Points2D;
|
||||||
|
|
||||||
|
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
PKG_HEADER = 0x54,
|
||||||
|
PKG_VER_LEN = 0x2C,
|
||||||
|
POINT_PER_PACK = 12,
|
||||||
|
};
|
||||||
|
typedef struct __attribute__((packed))
|
||||||
|
{
|
||||||
|
uint16_t distance;
|
||||||
|
uint8_t confidence;
|
||||||
|
}LidarPointStructDef;
|
||||||
|
typedef struct __attribute__((packed))
|
||||||
|
{
|
||||||
|
uint8_t header;
|
||||||
|
uint8_t ver_len;
|
||||||
|
uint16_t speed;
|
||||||
|
uint16_t start_angle;
|
||||||
|
LidarPointStructDef point[POINT_PER_PACK];
|
||||||
|
uint16_t end_angle;
|
||||||
|
uint16_t timestamp;
|
||||||
|
uint8_t crc8;
|
||||||
|
}LiDARFrameTypeDef;
|
||||||
|
struct FrameData
|
||||||
|
{
|
||||||
|
ros::Time timestamp;
|
||||||
|
float angle_min;
|
||||||
|
float angle_max;
|
||||||
|
uint32_t len;
|
||||||
|
std::vector<uint16_t> distance;
|
||||||
|
std::vector<uint8_t> intensities;
|
||||||
|
};
|
||||||
|
class LiPkg
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
LiPkg();
|
||||||
|
double GetSpeed(void); /*Lidar spin speed (Hz)*/
|
||||||
|
uint16_t GetTimestamp(void) { return mTimestamp; } /*time stamp of the packet */
|
||||||
|
bool IsPkgReady(void) { return mIsPkgReady; }/*a packet is ready */
|
||||||
|
bool IsFrameReady(void) { return mIsFrameReady; }/*Lidar data frame is ready*/
|
||||||
|
long GetErrorTimes(void) { return mErrorTimes; }/*the number of errors in parser process of lidar data frame*/
|
||||||
|
const std::array<PointData, POINT_PER_PACK>& GetPkgData(void);/*original data package*/
|
||||||
|
bool Parse(const uint8_t* data, long len);/*parse single packet*/
|
||||||
|
virtual void Transform(std::vector<PointData>& tmp) = 0;/*transform raw data to stantard data */
|
||||||
|
bool AssemblePacket();/*combine stantard data into data frames and calibrate*/
|
||||||
|
const FrameData& GetFrameData(void) { mIsFrameReady = false; return mFrameData; }
|
||||||
|
std::vector<cv::Mat> scan_to_pointcloud(FrameData mFrameData);
|
||||||
|
private:
|
||||||
|
uint16_t mTimestamp;
|
||||||
|
double mSpeed;
|
||||||
|
std::vector<uint8_t> mDataTmp;
|
||||||
|
long mErrorTimes;
|
||||||
|
std::array<PointData, POINT_PER_PACK>mOnePkg;
|
||||||
|
std::vector<PointData> mFrameTemp;
|
||||||
|
FrameData mFrameData;
|
||||||
|
bool mIsPkgReady;
|
||||||
|
bool mIsFrameReady;
|
||||||
|
};
|
||||||
|
class LD14_LiPkg : public LiPkg
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual void Transform(std::vector<PointData>& tmp);
|
||||||
|
};
|
||||||
|
|
||||||
|
enum class LDVersion
|
||||||
|
{
|
||||||
|
LD_ZERO, /*Zero generation lidar*/
|
||||||
|
LD_THREE, /*Third generation lidar*/
|
||||||
|
LD_EIGHT, /*Eight generation radar*/
|
||||||
|
LD_NINE, /*Nine generation radar*/
|
||||||
|
LD_FOURTEENTH /*Fourteenth generation radar*/
|
||||||
|
};
|
||||||
|
|
||||||
|
class SlTransform
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
bool to_right_hand = true;
|
||||||
|
double offset_x;
|
||||||
|
double offset_y;
|
||||||
|
|
||||||
|
public:
|
||||||
|
SlTransform(LDVersion version, bool to_right_hand = false);
|
||||||
|
Points2D Transform(const Points2D& data);
|
||||||
|
~SlTransform();
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class CmdInterfaceLinux
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
CmdInterfaceLinux(int32_t ver);
|
||||||
|
~CmdInterfaceLinux();
|
||||||
|
|
||||||
|
bool Open(std::string& port_name);
|
||||||
|
bool Close();
|
||||||
|
bool ReadFromIO(uint8_t* rx_buf, uint32_t rx_buf_len, uint32_t* rx_len);
|
||||||
|
bool WriteToIo(const uint8_t* tx_buf, uint32_t tx_buf_len, uint32_t* tx_len);
|
||||||
|
bool GetCmdDevices(std::vector<std::pair<std::string, std::string> >& device_list);
|
||||||
|
void SetReadCallback(std::function<void(const char*, size_t length)> callback) { mReadCallback = callback; }
|
||||||
|
bool IsOpened() { return mIsCmdOpened.load(); };
|
||||||
|
bool IsExited() { return mRxThreadExitFlag.load(); };
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::thread* mRxThread;
|
||||||
|
static void mRxThreadProc(void* param);
|
||||||
|
long long mRxCount;
|
||||||
|
int32_t version;
|
||||||
|
int32_t mComHandle;
|
||||||
|
std::atomic<bool> mIsCmdOpened, mRxThreadExitFlag;
|
||||||
|
std::function<void(const char*, size_t length)> mReadCallback;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/
|
||||||
|
|
@ -0,0 +1,112 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/calib3d.hpp>
|
||||||
|
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <vector>
|
||||||
|
#include "rknn_sdk.h"
|
||||||
|
#include "fusion.h"
|
||||||
|
|
||||||
|
using namespace cv;
|
||||||
|
class LCFusion;
|
||||||
|
class Ranging
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
friend class LCFusion;
|
||||||
|
VideoCapture vcapture;
|
||||||
|
rknn_handle hdx;
|
||||||
|
const char *modelPath = "/home/firefly/sweeping_robot/src/fusion_car/m28.rknn";
|
||||||
|
std::vector<std::string> className = {
|
||||||
|
"Unknown", //æªç¥
|
||||||
|
"MaoGouWan", //ç«ç¢/çç¢
|
||||||
|
"BTYDiZuo", //å§å°æ€
åºåº§
|
||||||
|
"FSDiZuo", //é£æåºåº§
|
||||||
|
"XiYiJi", //æŽè¡£æº
|
||||||
|
"BingXiang", //å°ç®±
|
||||||
|
"MaTong", //马桶
|
||||||
|
"TiZhongCheng", //äœé秀
|
||||||
|
"DianXian", //çµçº¿
|
||||||
|
"DianShiJi", //çµè§æº
|
||||||
|
"CanZhuo", //é€æ¡
|
||||||
|
"DiTan", //å°æ¯¯
|
||||||
|
"MaBu", //æ¹åž
|
||||||
|
"ChaJi", //è¶å
|
||||||
|
"DianShiGu", //çµè§æ
|
||||||
|
"XieZi", //æé/éå
|
||||||
|
"WaZi", //è¢å
|
||||||
|
"YiGui", //è¡£æ
|
||||||
|
"Chuang", //åº
|
||||||
|
"ShaFa", //æ²å
|
||||||
|
"YiZi", //æ€
å
|
||||||
|
"Men", //éš
|
||||||
|
"DLS", //倧çç³
|
||||||
|
"DiBan", //å°æ¿
|
||||||
|
};
|
||||||
|
|
||||||
|
// åç®åæ°
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||||
|
4.700950170847520e+02, 0, 0,
|
||||||
|
0, 4.709244004725060e+02, 0,
|
||||||
|
3.220628601465352e+02, 2.265265667516499e+02, 1);
|
||||||
|
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||||
|
4.696016526986375e+02, 0, 0,
|
||||||
|
0, 4.702371464881610e+02, 0,
|
||||||
|
3.216994731643572e+02, 2.267449827655198e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.088023418049400, -0.047968406803285, 0,
|
||||||
|
0, 0);
|
||||||
|
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.097018738959281, -0.081707209740704, 0,
|
||||||
|
0, 0);
|
||||||
|
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||||||
|
0.999895538560624, 1.514179660491009e-04, 0.014452994124399,
|
||||||
|
-1.935031776135467e-04, 0.999995745723263, 0.002910514026079,
|
||||||
|
-0.014452491933249, -0.002913006689884, 0.999891314028152);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) <<
|
||||||
|
-59.978995009996730, 0.050886852679934, -0.088565378305175);
|
||||||
|
/*
|
||||||
|
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||||
|
3.627346593660044e+02, 0, 0,
|
||||||
|
0, 3.629168542317134e+02, 0,
|
||||||
|
3.093231127433517e+02, 2.409353074183058e+02, 1);
|
||||||
|
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||||
|
3.615909129180773e+02, 0, 0,
|
||||||
|
0, 3.617772267770084e+02, 0,
|
||||||
|
3.062363746645480e+02, 2.298948842159400e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.117219440226075, -0.148594691012994, 0,
|
||||||
|
0, 0);
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.116412295415583, -0.146817143572705, 0,
|
||||||
|
0, 0);
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||||||
|
0.999962353120719, 4.648106081008926e-04, 0.008664657660520,
|
||||||
|
-4.493098874759400e-04, 0.999998295540464, -0.001790820145135,
|
||||||
|
-0.008665475284162, 0.001786859609987, 0.999960857569352);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) <<
|
||||||
|
-60.097178758944190, -0.033859553542635, -0.423965574285253);
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) << 3.629995979572968e+02, 0, 0, 0, 3.605811984929196e+02, 0, 3.056618479641253e+02, 2.425978482096805e+02, 1);
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) << 3.642383320232306e+02, 0, 0, 0, 3.619176322068948e+02, 0, 3.048699917391443e+02, 2.355042537698953e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) << 0.114519742751324, -0.147768779338520, 0.001196745401736, -0.00274469994995, 0.008589264227682);
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) << 0.107862414325132, -0.112272107836791, 0.002046119288761, -8.703752362970271e-04, -0.050706452190122);
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) << 0.999984364022271, 6.994668898371339e-04, 0.005548194034452, -6.988446369697146e-04, 0.999999749299562, -1.140920137281147e-04, -0.005548272447104, 1.102129041420978e-04, 0.999984602144437);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) << -60.134851044411256, 0.007279986569326, -0.065092184396760);
|
||||||
|
*/
|
||||||
|
|
||||||
|
Mat mapX1, mapX2, mapY1, mapY2, q, Z;
|
||||||
|
|
||||||
|
int imgw, imgh;
|
||||||
|
|
||||||
|
public:
|
||||||
|
Ranging(int index, int imgw, int imgh);
|
||||||
|
void rectifyImage(Mat &oriImgL, Mat &oriImgR);
|
||||||
|
void getInfo(Mat &imgl, Mat &imgr, Mat &detBoxes, Mat &info);
|
||||||
|
std::vector<float> pic2cam(int u, int v);
|
||||||
|
std::vector<int> muban(Mat &left_image, Mat &right_image, const int *coordinates);
|
||||||
|
std::vector<Mat> get_range();
|
||||||
|
void horizon_estimate(Mat& img, Mat& bboxs,int k);
|
||||||
|
};
|
||||||
|
|
@ -0,0 +1,110 @@
|
||||||
|
#ifndef __RKNN_SDK_H__
|
||||||
|
#define __RKNN_SDK_H__
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
// 返回错误码
|
||||||
|
enum XErrorCode {
|
||||||
|
RKNN_SUCCEED = 0,
|
||||||
|
RKNN_NOT_INIT = -1, //SDK未初始化
|
||||||
|
RKNN_INVAILD_IMGSIZE = -2, //输入图像尺寸不匹配模型输入要求
|
||||||
|
RKNN_INVAILD_PARAM = -3, //参数无效
|
||||||
|
};
|
||||||
|
|
||||||
|
enum ObjCls {
|
||||||
|
OBJ_Unknown = -1, //未知
|
||||||
|
OBJ_MaoGouWan = 0, //猫碗/狗碗
|
||||||
|
OBJ_BTYDiZuo = 1, //吧台椅底座
|
||||||
|
OBJ_FSDiZuo = 2, //风扇底座
|
||||||
|
OBJ_XiYiJi = 3, //洗衣机
|
||||||
|
OBJ_BingXiang = 4, //冰箱
|
||||||
|
OBJ_MaTong = 5, //马桶
|
||||||
|
OBJ_TiZhongCheng = 6, //体重秤
|
||||||
|
OBJ_DianXian = 7, //电线
|
||||||
|
OBJ_DianShiJi = 8, //电视机
|
||||||
|
OBJ_CanZhuo = 9, //餐桌
|
||||||
|
OBJ_DiTan = 10, //地毯
|
||||||
|
OBJ_MaBu = 11, //抹布
|
||||||
|
OBJ_ChaJi = 12, //茶几
|
||||||
|
OBJ_DianShiGui = 13, //电视柜
|
||||||
|
OBJ_XieZi = 14, //拖鞋/鞋子
|
||||||
|
OBJ_WaZi = 15, //袜子
|
||||||
|
OBJ_YiGui = 16, //衣柜
|
||||||
|
OBJ_Chuang = 17, //床
|
||||||
|
OBJ_ShaFa = 18, //沙发
|
||||||
|
OBJ_YiZi = 19, //椅子
|
||||||
|
OBJ_Men = 20, //门
|
||||||
|
OBJ_DLS = 21, //da li shi
|
||||||
|
OBJ_DiBan = 22 //di ban
|
||||||
|
//OBJ_Xiang = 21 //宠物粪便
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// 图像信息与buffer
|
||||||
|
typedef struct _InputData {
|
||||||
|
int shape[4]; // Num, Channel, Height, Width
|
||||||
|
void* data; // img.data
|
||||||
|
} InputData;
|
||||||
|
|
||||||
|
// 检测结果
|
||||||
|
typedef struct _DetRet
|
||||||
|
{
|
||||||
|
int nID; //图像ID
|
||||||
|
int nLabel; //物体类别 ObjCls nLabel; //物体类别
|
||||||
|
float fConf; //置信度
|
||||||
|
float fLeft; //检测框左上角比例坐标x_min -> int(x_min*width) 获取像素坐标
|
||||||
|
float fTop; //检测框左上角比例坐标y_min -> int(y_min*height) 获取像素坐标
|
||||||
|
float fRight; //检测框右下角比例坐标x_max -> int(x_max*width) 获取像素坐标
|
||||||
|
float fBottom; //检测框右下角比例坐标y_max -> int(y_max*height) 获取像素坐标
|
||||||
|
} DetRet;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#define RKNNSDK_API(retType) retType
|
||||||
|
|
||||||
|
///
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
typedef uint64_t rknn_handle;
|
||||||
|
|
||||||
|
|
||||||
|
/* RKNN_Create
|
||||||
|
SDK加载模型，仅初始化一次;
|
||||||
|
input:
|
||||||
|
rknn_handle hd 句柄
|
||||||
|
const char* model_path 模型路径
|
||||||
|
return:
|
||||||
|
int 错误码
|
||||||
|
*/
|
||||||
|
RKNNSDK_API(int) RKNN_Create(rknn_handle* hd, const char* model_path);
|
||||||
|
|
||||||
|
/* RKNN_ObjDet
|
||||||
|
输入图像，获取检测结果和检测目标数量;
|
||||||
|
input:
|
||||||
|
rknn_handle hd 句柄
|
||||||
|
const InputData* input_data 输入图像数据
|
||||||
|
DetRet** det_ret 获取检测结果
|
||||||
|
int* det_num 获取检测目标数量
|
||||||
|
return:
|
||||||
|
int 错误码
|
||||||
|
*/
|
||||||
|
RKNNSDK_API(int) RKNN_ObjDet(rknn_handle hd, const InputData* input_data, DetRet** det_ret, int* det_num);
|
||||||
|
|
||||||
|
/* RKNN_Release
|
||||||
|
SDK资源释放，仅在程序退出前调用;
|
||||||
|
input:
|
||||||
|
rknn_handle hd 句柄
|
||||||
|
return:
|
||||||
|
int 错误码
|
||||||
|
*/
|
||||||
|
RKNNSDK_API(int) RKNN_Release(rknn_handle hd);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#endif //__RKNN_SDK_H__
|
||||||
Binary file not shown.
|
|
@ -0,0 +1,74 @@
|
||||||
|
<?xml version="1.0"?>
|
||||||
|
<package format="2">
|
||||||
|
<name>fusion_car</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The fusion_car package</description>
|
||||||
|
|
||||||
|
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||||
|
<maintainer email="firefly@todo.todo">firefly</maintainer>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||||
|
<!-- Commonly used license strings: -->
|
||||||
|
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||||
|
<license>TODO</license>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||||
|
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <url type="website">http://wiki.ros.org/fusion_car</url> -->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||||
|
<!-- Authors do not have to be maintainers, but could be -->
|
||||||
|
<!-- Example: -->
|
||||||
|
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- The *depend tags are used to specify dependencies -->
|
||||||
|
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||||
|
<!-- Examples: -->
|
||||||
|
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||||
|
<!-- <depend>roscpp</depend> -->
|
||||||
|
<!-- Note that this is equivalent to the following: -->
|
||||||
|
<!-- <build_depend>roscpp</build_depend> -->
|
||||||
|
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||||
|
<!-- Use build_depend for packages you need at compile time: -->
|
||||||
|
<!-- <build_depend>message_generation</build_depend> -->
|
||||||
|
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||||
|
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||||
|
<!-- Use buildtool_depend for build tool packages: -->
|
||||||
|
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||||
|
<!-- Use exec_depend for packages you need at runtime: -->
|
||||||
|
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||||
|
<!-- Use test_depend for packages you need only for testing: -->
|
||||||
|
<!-- <test_depend>gtest</test_depend> -->
|
||||||
|
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||||
|
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<build_depend>rosconsole</build_depend>
|
||||||
|
<build_depend>roscpp</build_depend>
|
||||||
|
<build_depend>roscpp_serialization</build_depend>
|
||||||
|
<build_depend>roslib</build_depend>
|
||||||
|
<build_depend>rospy</build_depend>
|
||||||
|
<build_export_depend>rosconsole</build_export_depend>
|
||||||
|
<build_export_depend>roscpp</build_export_depend>
|
||||||
|
<build_export_depend>roscpp_serialization</build_export_depend>
|
||||||
|
<build_export_depend>roslib</build_export_depend>
|
||||||
|
<build_export_depend>rospy</build_export_depend>
|
||||||
|
<exec_depend>rosconsole</exec_depend>
|
||||||
|
<exec_depend>roscpp</exec_depend>
|
||||||
|
<exec_depend>roscpp_serialization</exec_depend>
|
||||||
|
<exec_depend>roslib</exec_depend>
|
||||||
|
<exec_depend>rospy</exec_depend>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- The export tag contains other, unspecified, tags -->
|
||||||
|
<export>
|
||||||
|
<!-- Other tools can request additional information be placed here -->
|
||||||
|
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
|
|
@ -0,0 +1,685 @@
|
||||||
|
#include <set>
|
||||||
|
#include "fusion_car/fusion.h"
|
||||||
|
// 角度 -> 弧度转换
|
||||||
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||||
|
// fusion函数
|
||||||
|
char LCFusion::fusion(std::vector<Mat> &camInfo)
|
||||||
|
{
|
||||||
|
projectedPoints.clear();
|
||||||
|
if (camInfo.size() < 3) // 判断返回的双目测距信息是否为空，正常为3
|
||||||
|
{
|
||||||
|
std::cout << "There is no detection box!" << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
Mat boxes = camInfo[1], dwha = camInfo[2];
|
||||||
|
projectedPoints.clear();
|
||||||
|
|
||||||
|
if (this->upScan.pointCloud.empty())
|
||||||
|
{
|
||||||
|
std::cout << "There is no lidar data!" << std::endl;
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (boxes.empty())
|
||||||
|
{
|
||||||
|
std::cout << "There is no detection box!" << std::endl;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
draw_point(camInfo); // 画出雷达映射到图像中的点
|
||||||
|
std::cout << "Fusion start!" << std::endl;
|
||||||
|
// 大物体
|
||||||
|
vector<int> clusterlable = clusters_bboxs(boxes); // 得到每簇雷达映射的检测框标签（检测框索引值）
|
||||||
|
optimize_clusterlable(clusterlable, boxes); // 优化检测框（针对镂空物体）
|
||||||
|
big_fusion(clusterlable, boxes, dwha); // 大物体赋语义
|
||||||
|
|
||||||
|
//小物体
|
||||||
|
small_box_without_dis(boxes, dwha); // 无距离信息的检测框映射
|
||||||
|
small_fusion(boxes, dwha); // 小物体赋语义
|
||||||
|
}
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::big_fusion(vector<int> &clusterlable, Mat &bboxs, Mat &dwha) // 大物体赋语义
|
||||||
|
{
|
||||||
|
std::cout << "big_fusion" << std::endl;
|
||||||
|
for (int i = 0; i < clusterlable.size(); i++) // 遍历每簇雷达
|
||||||
|
{
|
||||||
|
int bbox_num = clusterlable[i];
|
||||||
|
|
||||||
|
if (bbox_num != -1) // -1代表无检测框与之对应
|
||||||
|
{
|
||||||
|
int bbox_cls = bboxs.at<float>(bbox_num, 5);
|
||||||
|
if (class_index[bbox_cls] != 13 && class_index[bbox_cls] != 14) // 排除扩散物体（风扇底座、吧台椅）
|
||||||
|
{
|
||||||
|
//std::cout << "bboxcls:" << bbox_cls << std::endl;
|
||||||
|
for (int j = 0; j < this->upScan.clustedCloud[i].size(); j++) // 遍历一簇雷达的所有点赋语义
|
||||||
|
{
|
||||||
|
int laser_index = this->upScan.clustedCloud[i][j].at<float>(0, 3);
|
||||||
|
this->upScan.intensities[laser_index] = class_intensity[bbox_cls + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (upScan.clustedCloud[i].size() > 0 && upScan.clustedCloud[i].size() < 10) // 扩散物体雷达点应该很少
|
||||||
|
{
|
||||||
|
//cout << "******start draw circle******" << endl;
|
||||||
|
double d = dwha.at<float>(bbox_num, 0);
|
||||||
|
if (d > 0)
|
||||||
|
{
|
||||||
|
draw_circle(i, bbox_num, bboxs, dwha);
|
||||||
|
} // 扩散物体赋语义
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void LCFusion::small_fusion(Mat &bboxs, Mat &dwha) // 小物体u赋语义
|
||||||
|
{
|
||||||
|
std::cout << "small_fusion" << std::endl;
|
||||||
|
for (int i = 0; i < bboxs.rows; i++)
|
||||||
|
{
|
||||||
|
int c = bboxs.at<float>(i, 5);
|
||||||
|
float x_mim = bboxs.at<float>(i, 0);
|
||||||
|
float y_min = bboxs.at<float>(i, 1);
|
||||||
|
float x_max = bboxs.at<float>(i, 2);
|
||||||
|
float y_max = bboxs.at<float>(i, 3);
|
||||||
|
std::cout << "cls:" << c << " xmin:" << x_mim << " ymin:" << y_min << " xmax:" << x_max << " ymax:" << y_max << std::endl;
|
||||||
|
if (class_index[c] == 0 || class_index[c] == 2 || (class_index[c] == 4 || class_index[c] == 5)) // 判断小物体u
|
||||||
|
{
|
||||||
|
|
||||||
|
Mat temp(2, 1, CV_32F);
|
||||||
|
double d = dwha.at<float>(i, 0);
|
||||||
|
double a = dwha.at<float>(i, 3); // 取距离和角度
|
||||||
|
if (d == -1)
|
||||||
|
continue;
|
||||||
|
//std::cout << "d:" << d << " a: " << a << std::endl;
|
||||||
|
temp.at<float>(0, 0) = float((dwha.at<float>(i, 0) / 100.0) * cos(dwha.at<float>(i, 3)));
|
||||||
|
temp.at<float>(1, 0) = float((dwha.at<float>(i, 0) / 100.0) * sin(dwha.at<float>(i, 3)));
|
||||||
|
temp = c2lR * temp + c2lT; // 相机坐标系-> 雷达坐标系转换
|
||||||
|
double y = temp.at<float>(0, 0);
|
||||||
|
double x = temp.at<float>(1, 0);
|
||||||
|
float dis = sqrt(x * x + y * y); // 转极坐标
|
||||||
|
|
||||||
|
float angle = 0.0; // 得到雷达点索引
|
||||||
|
if (y >= 0)
|
||||||
|
angle = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
|
||||||
|
double data_angle = atan((dwha.at<float>(i, 1)) / (200 * dis));
|
||||||
|
data_angle /= this->upScan.angleInc; // 由测距宽度信息得到需更改的雷达点数的一半
|
||||||
|
angle = (int)angle - (int)data_angle; // 起始雷达索引
|
||||||
|
int num_point = 2 * (int)data_angle + 1; // 更改的雷达点数
|
||||||
|
|
||||||
|
int len = this->upScan.distance.size();
|
||||||
|
for (int i = 0; i < num_point; i++)
|
||||||
|
{
|
||||||
|
if ((angle + i) < 0 || (angle + i) > (len - 1)) // 防止越界
|
||||||
|
continue;
|
||||||
|
|
||||||
|
this->upScan.distance[angle + i] = dis;
|
||||||
|
this->upScan.intensities[angle + i] = class_intensity[c + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (class_index[c] == 1 || class_index[c] == 3 ||class_index[c] == 6)
|
||||||
|
{
|
||||||
|
int len = this->upScan.distance.size();
|
||||||
|
Mat z = r->Z.clone();
|
||||||
|
if (countNonZero(z) < 1)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
if (z.cols >= 2)
|
||||||
|
{
|
||||||
|
int begin_index = 0;
|
||||||
|
for(int j = z.cols-1; j >=0; j-- )
|
||||||
|
{
|
||||||
|
int d = z.at<float>(0, j);
|
||||||
|
if (d != 0)
|
||||||
|
{
|
||||||
|
begin_index = j;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cout << "tizhongcheng " << z.cols << endl;
|
||||||
|
float d_begin = z.at<float>(0, begin_index)/1000.;
|
||||||
|
float a_begin = z.at<float>(1, begin_index);
|
||||||
|
cout << "d_begin: " << d_begin << " a_begin: " << a_begin << endl;
|
||||||
|
Mat temp(2, 1, CV_32F);
|
||||||
|
temp.at<float>(0, 0) = float(d_begin * cos(a_begin));
|
||||||
|
temp.at<float>(1, 0) = float(d_begin * sin(a_begin));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
double y = temp.at<float>(0, 0);
|
||||||
|
double x = temp.at<float>(1, 0);
|
||||||
|
float dis = sqrt(x * x + y * y);
|
||||||
|
float angle_index = 0.0; // 得到雷达点索引
|
||||||
|
if (y >= 0)
|
||||||
|
angle_index = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle_index = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
angle_index = (int)angle_index;
|
||||||
|
if (angle_index >= 0 && angle_index <len) // 防止越界
|
||||||
|
{
|
||||||
|
this->upScan.distance[angle_index] = dis;
|
||||||
|
this->upScan.intensities[angle_index] = class_intensity[c + 1];
|
||||||
|
}
|
||||||
|
cout << "dis: " << dis << " angle_index: " << angle_index << endl;
|
||||||
|
angle_index += 1;
|
||||||
|
|
||||||
|
for (int i = begin_index-1; i >= 0; i--)
|
||||||
|
{
|
||||||
|
float temp_d = z.at<float>(0, i)/1000.;
|
||||||
|
float temp_a = z.at<float>(1, i);
|
||||||
|
temp.at<float>(0, 0) = float(temp_d * cos(temp_a));
|
||||||
|
temp.at<float>(1, 0) = float(temp_d * sin(temp_a));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
y = temp.at<float>(0, 0);
|
||||||
|
x = temp.at<float>(1, 0);
|
||||||
|
dis = sqrt(x * x + y * y);
|
||||||
|
float temp_angle = 0.0; // 得到雷达点索引
|
||||||
|
if (y >= 0)
|
||||||
|
temp_angle = acos(x / dis);
|
||||||
|
else
|
||||||
|
temp_angle =2 * M_PI - acos(x / dis);
|
||||||
|
float angle_now = this->upScan.angleMin + this->upScan.angleInc*angle_index;
|
||||||
|
if (temp_angle >= angle_now)
|
||||||
|
{
|
||||||
|
if (angle_index >= 0 && angle_index < len) // 防止越界
|
||||||
|
{
|
||||||
|
this->upScan.distance[angle_index] = dis;
|
||||||
|
// cout << "dis: " << dis << " angle_index: " << angle_index << endl;
|
||||||
|
this->upScan.intensities[angle_index] = class_intensity[c + 1];
|
||||||
|
}
|
||||||
|
angle_index += 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
r->Z = Mat::zeros(2, 1, CV_32FC1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::optimize_clusterlable(vector<int> &clusterlable, Mat &bboxs) // 针对镂空物体选择对应的雷达簇，寻找镂空物体检测框对应的所有雷达簇
|
||||||
|
{
|
||||||
|
for (int i = 0; i < bboxs.rows; i++) // 寻找每个框对应的所有雷达簇
|
||||||
|
{
|
||||||
|
vector<int> temp_cluster_num;
|
||||||
|
Mat temp_bbox = bboxs.row(i).clone();
|
||||||
|
for (int j = 0; j < clusterlable.size(); j++)
|
||||||
|
{
|
||||||
|
if (clusterlable[j] == i)
|
||||||
|
{
|
||||||
|
temp_cluster_num.push_back(j);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (temp_cluster_num.size() == 0)
|
||||||
|
continue;
|
||||||
|
choose_forceground(temp_cluster_num, temp_bbox, clusterlable); // 筛选镂空物体的前景
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::choose_forceground(vector<int> &cluster_num, Mat &bbox, vector<int> &clusterlable) // 筛选镂空物体的前景
|
||||||
|
{
|
||||||
|
int box_class = bbox.at<float>(0, 5);
|
||||||
|
if ((class_index[box_class] >= 15 && class_index[box_class] <= 18 || class_index[box_class] == 12 || class_index[box_class] == 10) && cluster_num.size() > 1)
|
||||||
|
{
|
||||||
|
cout << "class " << box_class << " have " << cluster_num.size() << " clusters" << endl;
|
||||||
|
int total_point = 0;
|
||||||
|
for (int i = 0; i < cluster_num.size(); i++) // 计算框内雷达点总数
|
||||||
|
{
|
||||||
|
total_point += (this->upScan.clustedCloud[cluster_num[i]].size());
|
||||||
|
}
|
||||||
|
for (int j = 0; j < cluster_num.size(); j++) // 计算每个雷达簇占比，大于0.2认为是背景
|
||||||
|
{
|
||||||
|
float num_cluster = this->upScan.clustedCloud[cluster_num[j]].size();
|
||||||
|
if ((num_cluster / total_point > 0.2) || num_cluster > 8)
|
||||||
|
{
|
||||||
|
clusterlable[cluster_num[j]] = -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::draw_point(std::vector<Mat> &camInfo) // 画出映射到图像的雷达点
|
||||||
|
{
|
||||||
|
projectedPoints.clear();
|
||||||
|
for (int i = 0; i < this->upScan.pointCloud.rows; i++)
|
||||||
|
{
|
||||||
|
Mat temp_pointcloud = this->upScan.pointCloud.row(i).clone();
|
||||||
|
Mat uv = pointcloud_pixel(temp_pointcloud);
|
||||||
|
int x = uv.at<float>(0, 0);
|
||||||
|
int y = uv.at<float>(1, 0);
|
||||||
|
if (x > 0 && x < 640 && y > 0 && y < 480)
|
||||||
|
{
|
||||||
|
Point2d pt_uv(x, y);
|
||||||
|
projectedPoints.push_back(pt_uv);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
// void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc)
|
||||||
|
// {
|
||||||
|
// upScan.distance.clear();
|
||||||
|
// upScan.intensities.clear();
|
||||||
|
// float pointAngle;
|
||||||
|
// upScan.angleInc = angleInc;
|
||||||
|
|
||||||
|
// for (int i = 0; i < laserData.len; i++)
|
||||||
|
// {
|
||||||
|
// pointAngle = angle_min + i * upScan.angleInc;
|
||||||
|
// if (pointAngle > cameraAnglemin)
|
||||||
|
// {
|
||||||
|
// if (pointAngle > cameraAnglemax)
|
||||||
|
// {
|
||||||
|
// upScan.angleMax = pointAngle - upScan.angleInc;
|
||||||
|
// break;
|
||||||
|
// }
|
||||||
|
// upScan.distance.push_back(laserData.distance[i] / 1000.f);
|
||||||
|
// upScan.intensities.push_back(0);
|
||||||
|
// if (i == (laserData.len - 1))
|
||||||
|
// upScan.angleMax = pointAngle;
|
||||||
|
// // upScan.intensities.push_back(laserData.intensities[i]);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// upScan.angleMin = upScan.angleMax - (upScan.distance.size() - 1) * upScan.angleInc;
|
||||||
|
// this->scan_to_pointcloud();
|
||||||
|
// this->cluster();
|
||||||
|
// }
|
||||||
|
void LCFusion::filter_laser(const FrameData &laserData, float angle_min, float angleInc) // 雷达点和相机视野同步
|
||||||
|
{
|
||||||
|
upScan.distance.clear();
|
||||||
|
upScan.intensities.clear();
|
||||||
|
upScan.angleMin = angle_min;
|
||||||
|
upScan.angleInc = angleInc;
|
||||||
|
upScan.angleMax = ANGLE_TO_RADIAN(laserData.angle_max);
|
||||||
|
float pointAngle;
|
||||||
|
for (int i = 0; i < laserData.len; i++)
|
||||||
|
{
|
||||||
|
upScan.distance.push_back(laserData.distance[i] / 1000.f);
|
||||||
|
pointAngle = angle_min + i * upScan.angleInc;
|
||||||
|
if (pointAngle > cameraAnglemin && pointAngle < cameraAnglemax)
|
||||||
|
upScan.intensities.push_back(1); // 视野范围内强度赋1
|
||||||
|
else
|
||||||
|
upScan.intensities.push_back(0); // 视野范围外强度赋0
|
||||||
|
}
|
||||||
|
|
||||||
|
this->scan_to_pointcloud(); // 雷达极坐标到二维坐标转换
|
||||||
|
this->cluster(); // 雷达聚类
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::scan_to_pointcloud() // 雷达极坐标到二维坐标转换
|
||||||
|
{
|
||||||
|
u_int32_t len = upScan.distance.size();
|
||||||
|
Mat pointcloud(len, 4, CV_32F);
|
||||||
|
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
float dis = this->upScan.distance[i];
|
||||||
|
float x_temp = cos(upScan.angleMin + i * upScan.angleInc) * dis;
|
||||||
|
float y_temp = sin(upScan.angleMin + i * upScan.angleInc) * dis;
|
||||||
|
pointcloud.at<float>(i, 0) = x_temp;
|
||||||
|
pointcloud.at<float>(i, 1) = y_temp;
|
||||||
|
pointcloud.at<float>(i, 2) = 0;
|
||||||
|
pointcloud.at<float>(i, 3) = i;
|
||||||
|
}
|
||||||
|
this->upScan.pointCloud = pointcloud.clone();
|
||||||
|
}
|
||||||
|
|
||||||
|
uchar LCFusion::lidar2box(Mat uv, Mat boxes, uchar *boxflag) // 无用
|
||||||
|
{
|
||||||
|
int x = uv.at<float>(0, 0);
|
||||||
|
int y = uv.at<float>(1, 0);
|
||||||
|
// 实验测试，过滤类别
|
||||||
|
std::set<uchar> filterCls = {0, 7, 11, 14, 15, 21};
|
||||||
|
for (uchar i = 0; i < boxes.rows; i++)
|
||||||
|
{
|
||||||
|
int clsIdx = boxes.at<float>(i, 5);
|
||||||
|
if (filterCls.find(clsIdx) != filterCls.end())
|
||||||
|
continue;
|
||||||
|
|
||||||
|
float xmin = boxes.at<float>(i, 0);
|
||||||
|
float xmax = boxes.at<float>(i, 2);
|
||||||
|
float ymin = boxes.at<float>(i, 1);
|
||||||
|
float ymax = boxes.at<float>(i, 3);
|
||||||
|
|
||||||
|
// 实验测试，过滤过大的误检框
|
||||||
|
float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
|
||||||
|
if (ratio > 0.7)
|
||||||
|
{
|
||||||
|
boxflag[i] += 1;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 若雷达点处于某个目标框内就返回其对应强度值，未考虑目标框重叠情况
|
||||||
|
if (x >= xmin && x <= xmax && y >= ymin && y <= ymax)
|
||||||
|
{
|
||||||
|
boxflag[i] += 1;
|
||||||
|
return class_intensity[clsIdx + 1];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int> LCFusion::clusters_bboxs(Mat &bboxs) // 所有的聚类的雷达簇和所有的检测框的匹配
|
||||||
|
{
|
||||||
|
std::vector<int> clusterlable;
|
||||||
|
for (int i = 0; i < this->upScan.clustedCloud.size(); i++) // 遍历每簇雷达
|
||||||
|
{
|
||||||
|
std::vector<Mat> temp_cluster = this->upScan.clustedCloud[i];
|
||||||
|
int temp_clusters_bbox = cluster_bboxs(temp_cluster, bboxs); // 为一簇雷达寻找匹配的检测框
|
||||||
|
clusterlable.push_back(temp_clusters_bbox);
|
||||||
|
}
|
||||||
|
return clusterlable;
|
||||||
|
}
|
||||||
|
|
||||||
|
int LCFusion::cluster_bboxs(vector<Mat> &one_cluster, Mat &bboxs) // 为一簇雷达寻找匹配的检测框
|
||||||
|
{
|
||||||
|
vector<int> temp_result;
|
||||||
|
Mat begin = pointcloud_pixel(one_cluster[one_cluster.size() - 1]);
|
||||||
|
Mat end = pointcloud_pixel(one_cluster[0]); // 一簇雷达起始点和终止点在像素坐标系的坐标
|
||||||
|
for (int i = 0; i < bboxs.rows; i++)
|
||||||
|
{
|
||||||
|
Mat bbox = bboxs.row(i).clone();
|
||||||
|
int cls = bbox.at<float>(0, 5);
|
||||||
|
float ratio = ratio_in_1box(begin, end, bbox); // 在框中占比
|
||||||
|
if (ratio > 0.8 && this->class_index[cls] >= 7 && class_index[cls] <= 18) // 适用于大物体
|
||||||
|
temp_result.push_back(i);
|
||||||
|
}
|
||||||
|
if (temp_result.size() < 1) // 若一簇雷达落入多个框里，选择宽度较小的框
|
||||||
|
return -1;
|
||||||
|
else if (temp_result.size() < 2)
|
||||||
|
return temp_result[0];
|
||||||
|
else
|
||||||
|
{
|
||||||
|
int min = temp_result[0];
|
||||||
|
int width_min = bboxs.at<float>(min, 2) - bboxs.at<float>(min, 0);
|
||||||
|
for (int i = 0; i < temp_result.size(); i++)
|
||||||
|
{
|
||||||
|
int width_new = bboxs.at<float>(temp_result[i], 2) - bboxs.at<float>(temp_result[i], 0);
|
||||||
|
if (width_new < width_min)
|
||||||
|
{
|
||||||
|
min = temp_result[i];
|
||||||
|
width_min = width_new;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return min;
|
||||||
|
}
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
Mat LCFusion::pointcloud_pixel(Mat &pointcloud) // 二维雷达点到像素坐标系转换
|
||||||
|
{
|
||||||
|
Mat uv(3, 1, CV_32F), cutPointCloud(pointcloud.colRange(0, 3));
|
||||||
|
Mat camPoint = rotate * cutPointCloud.t() + translation;
|
||||||
|
// float ppp = camPoint.at<float>(2, 0);
|
||||||
|
if (camPoint.at<float>(2, 0) <= 0)
|
||||||
|
{
|
||||||
|
uv = (Mat_<float>(3, 1) << -1, -1, -1);
|
||||||
|
return uv;
|
||||||
|
}
|
||||||
|
|
||||||
|
float scaleX = camPoint.at<float>(0, 0) / camPoint.at<float>(2, 0);
|
||||||
|
float scaleY = camPoint.at<float>(1, 0) / camPoint.at<float>(2, 0);
|
||||||
|
float scaleD = scaleX * scaleX + scaleY * scaleY;
|
||||||
|
float tempD = 1 + distCoeff.at<float>(0, 0) * scaleD + distCoeff.at<float>(0, 1) * scaleD * scaleD + distCoeff.at<float>(0, 4) * scaleD * scaleD * scaleD;
|
||||||
|
|
||||||
|
camPoint.at<float>(0, 0) = scaleX * tempD + 2 * distCoeff.at<float>(0, 2) * scaleX * scaleY +
|
||||||
|
distCoeff.at<float>(0, 3) * (scaleD + 2 * scaleX * scaleX);
|
||||||
|
|
||||||
|
camPoint.at<float>(1, 0) = scaleY * tempD + distCoeff.at<float>(0, 2) * (scaleD + 2 * scaleY * scaleY) +
|
||||||
|
2 * distCoeff.at<float>(0, 3) * scaleX * scaleY;
|
||||||
|
|
||||||
|
camPoint.at<float>(2, 0) = 1.0;
|
||||||
|
uv = cameraMat * camPoint; // uv为像素坐标
|
||||||
|
uv = p2r * uv + t; // 矫正
|
||||||
|
return uv;
|
||||||
|
}
|
||||||
|
|
||||||
|
float LCFusion::ratio_in_1box(Mat &begin, Mat &end, Mat &box) // 计算一簇雷达落入检测框的比例
|
||||||
|
{
|
||||||
|
int x_begin = begin.at<float>(0, 0), x_end = end.at<float>(0, 0), y_begin = begin.at<float>(1, 0), y_end = end.at<float>(1, 0);
|
||||||
|
int xmin = box.at<float>(0, 0), ymin = box.at<float>(0, 1), xmax = box.at<float>(0, 2), ymax = box.at<float>(0, 3);
|
||||||
|
if (y_begin < ymin || y_begin > ymax)
|
||||||
|
return 0;
|
||||||
|
if (x_begin < xmin)
|
||||||
|
{
|
||||||
|
if (x_end < xmin)
|
||||||
|
return 0;
|
||||||
|
else if (x_end < xmax)
|
||||||
|
return (float)(x_end - xmin) / (x_end - x_begin);
|
||||||
|
else
|
||||||
|
return (float)(xmax - xmin) / (x_end - x_begin);
|
||||||
|
}
|
||||||
|
else if (x_begin < xmax)
|
||||||
|
{
|
||||||
|
if (x_end < xmax)
|
||||||
|
return 1;
|
||||||
|
else
|
||||||
|
return (float)(xmax - x_begin) / (x_end - x_begin);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void LCFusion::cluster() // 聚类
|
||||||
|
{
|
||||||
|
this->upScan.clustedCloud.clear();
|
||||||
|
int i = 0;
|
||||||
|
|
||||||
|
for (; i < this->upScan.distance.size();)
|
||||||
|
{
|
||||||
|
std::vector<Mat> temp;
|
||||||
|
int j = i;
|
||||||
|
temp.push_back(this->upScan.pointCloud.row(i));
|
||||||
|
for (; j < this->upScan.distance.size() - 1; j++)
|
||||||
|
{
|
||||||
|
|
||||||
|
// 距离的6%作为分类的阈值
|
||||||
|
if (abs(this->upScan.distance[j] - this->upScan.distance[j + 1]) < this->upScan.distance[j] * 0.07 )
|
||||||
|
{
|
||||||
|
temp.push_back(this->upScan.pointCloud.row(j + 1));
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
i = j + 1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
this->upScan.clustedCloud.push_back(temp);
|
||||||
|
if (j == this->upScan.distance.size() - 1)
|
||||||
|
break;
|
||||||
|
if (i == upScan.distance.size() - 1)
|
||||||
|
{
|
||||||
|
temp.push_back(upScan.pointCloud.row(i));
|
||||||
|
this->upScan.clustedCloud.push_back(temp);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
this->clusterIdx.clear();
|
||||||
|
int ii = 0, totoalNum = 0;
|
||||||
|
for (auto c : this->upScan.clustedCloud)
|
||||||
|
{
|
||||||
|
for (int jj = 0; jj < c.size(); jj++)
|
||||||
|
{
|
||||||
|
this->clusterIdx.push_back(ii);
|
||||||
|
}
|
||||||
|
totoalNum += c.size();
|
||||||
|
ii++;
|
||||||
|
}
|
||||||
|
//std::cout << "cluster num: " << this->upScan.clustedCloud.size() << " ii:" << ii << " total:" << totoalNum << " oriNum: " << this->upScan.distance.size() << std::endl;
|
||||||
|
}
|
||||||
|
void LCFusion::draw_circle(int cluster_num, int bbox_num, Mat &bboxs, Mat &dwha) // 扩散物体生成圆弧状伪激光雷达
|
||||||
|
{
|
||||||
|
vector<float> center_point = find_circle_center(cluster_num);
|
||||||
|
float alfa = center_point[1] * upScan.angleInc + upScan.angleMin, r = dwha.at<float>(bbox_num, 1)/200.f,
|
||||||
|
d = center_point[0], half_data_num = atan(r/d)/(upScan.angleInc);
|
||||||
|
int num_laser_circle = half_data_num * 2 + 1, cls = bboxs.at<float>(bbox_num, 5);
|
||||||
|
float laser_index = center_point[1] - half_data_num;
|
||||||
|
//cout << "alfa: " << alfa << " r: " << r << " d: " << d << " num_laser_circle: " << num_laser_circle << " start_index: " << laser_index << endl;
|
||||||
|
for (int i = 0; i < num_laser_circle; i++)
|
||||||
|
{
|
||||||
|
float theta = upScan.angleMin + laser_index * upScan.angleInc;
|
||||||
|
if (laser_index < 0 || laser_index > upScan.distance.size())
|
||||||
|
{
|
||||||
|
laser_index++;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
float data = pow(r, 2) - pow(d, 2) * pow(sin(theta - alfa), 2);
|
||||||
|
if (data >= 0)
|
||||||
|
{
|
||||||
|
//cout << "draw_one_point" << endl;
|
||||||
|
upScan.distance[laser_index] = circle(d, alfa ,theta, data);
|
||||||
|
upScan.intensities[laser_index] = class_intensity[cls + 1];
|
||||||
|
}
|
||||||
|
laser_index++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
vector<float> LCFusion::find_circle_center(int cluster_num) // 寻找扩散物体圆弧的圆心
|
||||||
|
{
|
||||||
|
vector<float> center_point(2);
|
||||||
|
int index = 0;
|
||||||
|
float total_dis = 0;
|
||||||
|
int num = upScan.clustedCloud[cluster_num].size();
|
||||||
|
int no_zero_dis = 0; // 非零距离和
|
||||||
|
for (int i = 0 ; i < num; i++)
|
||||||
|
{
|
||||||
|
index = upScan.clustedCloud[cluster_num][i].at<float>(0, 3);
|
||||||
|
if (upScan.distance[index] != 0)
|
||||||
|
no_zero_dis++;
|
||||||
|
total_dis += upScan.distance[index];
|
||||||
|
}
|
||||||
|
center_point[0] = total_dis/ no_zero_dis;
|
||||||
|
index -= (num/2);
|
||||||
|
float center_laser_index = index;
|
||||||
|
center_point[1] = center_laser_index;
|
||||||
|
return center_point;
|
||||||
|
}
|
||||||
|
float LCFusion::circle(float d, float alfa ,float theta, float data) // d 中心点距离 r 圆半径 a 中心点角度 theta 雷达角度
|
||||||
|
{
|
||||||
|
float ro = d * cos(theta - alfa) - sqrt(data);
|
||||||
|
return ro > 0 ? ro:0;
|
||||||
|
}
|
||||||
|
vector<float> LCFusion::width_ladar(const int* coordinates)
|
||||||
|
{
|
||||||
|
int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
|
||||||
|
|
||||||
|
std::vector<float> mid = r-> pic2cam(640 / 2, 480); //<2F>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||||
|
std::vector<float> loc_tar_start = r-> pic2cam(x1, 480); // <20>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ʼλ<CABC><CEBB>
|
||||||
|
std::vector<float> loc_tar_end = r-> pic2cam(x2, 480); //<2F>ҵ<EFBFBD>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD>ֹλ<D6B9><CEBB>
|
||||||
|
//ת<><D7AA><EFBFBD>ɻ<EFBFBD><C9BB><EFBFBD>ֵ
|
||||||
|
float alfa_start = atan((loc_tar_start[0] - mid[0]) / r-> q.at<double>(2, 3));
|
||||||
|
float alfa_end = atan((loc_tar_end[0] - mid[0]) / r-> q.at<double>(2, 3));
|
||||||
|
std::vector<float> ladar_range;
|
||||||
|
ladar_range.push_back(alfa_start);
|
||||||
|
ladar_range.push_back(alfa_end);
|
||||||
|
return ladar_range;
|
||||||
|
}
|
||||||
|
void LCFusion::small_box_without_dis(Mat & bboxs, Mat & dwha) // 无距离检测框到像素坐标系映射，过程和small_fusion类似
|
||||||
|
{
|
||||||
|
std::cout << "small_box_without_dis" << std::endl;
|
||||||
|
double set_dis = 2.0; // 设置距离为2米
|
||||||
|
for (int i = 0; i < bboxs.rows; i++)
|
||||||
|
{
|
||||||
|
int c = bboxs.at<float>(i, 5);
|
||||||
|
int x_mim = bboxs.at<float>(i, 0);
|
||||||
|
int y_min = bboxs.at<float>(i, 1);
|
||||||
|
int x_max = bboxs.at<float>(i, 2);
|
||||||
|
int y_max = bboxs.at<float>(i, 3);
|
||||||
|
double d = dwha.at<float>(i, 0);
|
||||||
|
int coordinates[4] = {x_mim, y_min, x_max, y_max};
|
||||||
|
if ((class_index[c] >= 0 && class_index[c] <= 1) || (class_index[c] >= 3 && class_index[c] <= 6))
|
||||||
|
{
|
||||||
|
|
||||||
|
Mat temp(2, 1, CV_32F);
|
||||||
|
|
||||||
|
|
||||||
|
if (d == -1)
|
||||||
|
{
|
||||||
|
vector<float> box_angle = width_ladar(coordinates);
|
||||||
|
//std::cout << "d:" << d << " a: " << a << std::endl;
|
||||||
|
temp.at<float>(0, 0) = float(set_dis * cos(box_angle[0]));
|
||||||
|
temp.at<float>(1, 0) = float(set_dis * sin(box_angle[0]));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
double y = temp.at<float>(0, 0);
|
||||||
|
double x = temp.at<float>(1, 0);
|
||||||
|
float dis = sqrt(x * x + y * y);
|
||||||
|
float angle_end = 0.0;
|
||||||
|
if (y >= 0)
|
||||||
|
angle_end = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle_end = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
|
||||||
|
angle_end = int(angle_end);
|
||||||
|
// std:: cout << "angle end: " << angle_end << std::endl;
|
||||||
|
temp.at<float>(0, 0) = float(set_dis * cos(box_angle[1]));
|
||||||
|
temp.at<float>(1, 0) = float(set_dis * sin(box_angle[1]));
|
||||||
|
temp = c2lR * temp + c2lT;
|
||||||
|
y = temp.at<float>(0, 0);
|
||||||
|
x = temp.at<float>(1, 0);
|
||||||
|
dis = sqrt(x * x + y * y);
|
||||||
|
float angle_start = 0.0;
|
||||||
|
if (y >= 0)
|
||||||
|
angle_start = (acos(x / dis) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
else
|
||||||
|
angle_start = ((2 * M_PI - acos(x / dis)) - this->upScan.angleMin) / this->upScan.angleInc;
|
||||||
|
|
||||||
|
angle_start = int(angle_start);
|
||||||
|
// std:: cout << "angle start: " << angle_start << std::endl;
|
||||||
|
if (angle_end-angle_start <= 0)
|
||||||
|
continue;
|
||||||
|
int num_point = angle_end-angle_start+1;
|
||||||
|
for (int i = 0; i < num_point; i++)
|
||||||
|
{
|
||||||
|
if ((angle_start + i) < 0 || (angle_start + i) > (upScan.distance.size() - 1))
|
||||||
|
continue;
|
||||||
|
this->upScan.intensities[angle_start + i] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan, FrameData &laserData) // 设置雷达发布数据（原始雷达）
|
||||||
|
{
|
||||||
|
float angle_min = ANGLE_TO_RADIAN(laserData.angle_min);
|
||||||
|
float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
|
||||||
|
uint32_t len = laserData.len;
|
||||||
|
float angle_inc = (angle_max - angle_min) / (len - 1.);
|
||||||
|
scan.header.frame_id = "down_laser_link";
|
||||||
|
scan.range_min = 0.15;
|
||||||
|
scan.range_max = 8.0;
|
||||||
|
scan.angle_min = angle_min;
|
||||||
|
scan.angle_max = angle_max;
|
||||||
|
scan.angle_increment = angle_inc;
|
||||||
|
scan.ranges.resize(len);
|
||||||
|
scan.intensities.resize(len);
|
||||||
|
scan.time_increment = 1. / 2400.;
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
scan.ranges[i] = laserData.distance[i] / 1000.f;
|
||||||
|
scan.intensities[i] = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void LCFusion::set_laser_data(sensor_msgs::LaserScan &scan) // 设置雷达发布数据（语义雷达）
|
||||||
|
{
|
||||||
|
scan.header.frame_id = "up_laser_link";
|
||||||
|
scan.range_min = 0.15;
|
||||||
|
scan.range_max = 8.0;
|
||||||
|
scan.angle_increment = upScan.angleInc;
|
||||||
|
scan.ranges.resize(upScan.distance.size());
|
||||||
|
scan.intensities.resize(upScan.distance.size());
|
||||||
|
scan.angle_min = upScan.angleMin;
|
||||||
|
scan.angle_max = upScan.angleMax;
|
||||||
|
for (int i = 0; i < upScan.distance.size(); i++)
|
||||||
|
{
|
||||||
|
scan.ranges[i] = upScan.distance[i];
|
||||||
|
scan.intensities[i] = upScan.intensities[i];
|
||||||
|
}
|
||||||
|
scan.time_increment = 1. / 2400.;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,583 @@
|
||||||
|
/**
|
||||||
|
* @file lipkg.cpp
|
||||||
|
* @author LD Robot
|
||||||
|
* @version V01
|
||||||
|
* @brief
|
||||||
|
* @note
|
||||||
|
* @attention COPYRIGHT LDROBOT
|
||||||
|
**/
|
||||||
|
|
||||||
|
#include "fusion_car/lidar.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
// #ifdef USE_SLBI
|
||||||
|
// #include "slbi.h"
|
||||||
|
// #endif
|
||||||
|
|
||||||
|
// #ifdef USE_SLBF
|
||||||
|
// #include "slbf.h"
|
||||||
|
// #endif
|
||||||
|
//#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59/180000)
|
||||||
|
#define MAX_ACK_BUF_LEN 2304000
|
||||||
|
|
||||||
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||||
|
|
||||||
|
static const uint8_t CrcTable[256] =
|
||||||
|
{
|
||||||
|
0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
|
||||||
|
0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
|
||||||
|
0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
|
||||||
|
0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
|
||||||
|
0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
|
||||||
|
0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
|
||||||
|
0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
|
||||||
|
0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
|
||||||
|
0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
|
||||||
|
0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
|
||||||
|
0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
|
||||||
|
0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
|
||||||
|
0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
|
||||||
|
0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
|
||||||
|
0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
|
||||||
|
0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
|
||||||
|
0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
|
||||||
|
0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
|
||||||
|
0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
|
||||||
|
0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
|
||||||
|
0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
|
||||||
|
0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8};
|
||||||
|
|
||||||
|
CmdInterfaceLinux::CmdInterfaceLinux(int32_t ver) : mRxThread(nullptr),
|
||||||
|
mRxCount(0),
|
||||||
|
mReadCallback(nullptr),
|
||||||
|
version(ver)
|
||||||
|
{
|
||||||
|
mComHandle = -1;
|
||||||
|
}
|
||||||
|
CmdInterfaceLinux::~CmdInterfaceLinux()
|
||||||
|
{
|
||||||
|
Close();
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::Open(std::string &port_name)
|
||||||
|
{
|
||||||
|
int flags = (O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||||
|
|
||||||
|
mComHandle = open(port_name.c_str(), flags);
|
||||||
|
if (-1 == mComHandle)
|
||||||
|
{
|
||||||
|
std::cout << "CmdInterfaceLinux::Open " << port_name << " error !" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get port options
|
||||||
|
struct termios options;
|
||||||
|
if (-1 == tcgetattr(mComHandle, &options))
|
||||||
|
{
|
||||||
|
Close();
|
||||||
|
std::cout << "CmdInterfaceLinux::Open tcgetattr error!" << std::endl;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS);
|
||||||
|
options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD);
|
||||||
|
options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
|
||||||
|
ISIG | IEXTEN); //|ECHOPRT
|
||||||
|
options.c_oflag &= (tcflag_t) ~(OPOST);
|
||||||
|
options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | INLCR | IGNCR | ICRNL | IGNBRK);
|
||||||
|
|
||||||
|
options.c_cc[VMIN] = 0;
|
||||||
|
options.c_cc[VTIME] = 0;
|
||||||
|
|
||||||
|
switch (version)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
case 6:
|
||||||
|
cfsetispeed(&options, B230400);
|
||||||
|
break;
|
||||||
|
case 8:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
case 14:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
cfsetispeed(&options, B115200);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (tcsetattr(mComHandle, TCSANOW, &options) < 0)
|
||||||
|
{
|
||||||
|
std::cout << "CmdInterfaceLinux::Open tcsetattr error!" << std::endl;
|
||||||
|
Close();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
tcflush(mComHandle, TCIFLUSH);
|
||||||
|
|
||||||
|
mRxThreadExitFlag = false;
|
||||||
|
mRxThread = new std::thread(mRxThreadProc, this);
|
||||||
|
mIsCmdOpened = true;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::Close()
|
||||||
|
{
|
||||||
|
if (mIsCmdOpened == false)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
mRxThreadExitFlag = true;
|
||||||
|
|
||||||
|
if (mComHandle != -1)
|
||||||
|
{
|
||||||
|
close(mComHandle);
|
||||||
|
mComHandle = -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((mRxThread != nullptr) && mRxThread->joinable())
|
||||||
|
{
|
||||||
|
mRxThread->join();
|
||||||
|
delete mRxThread;
|
||||||
|
mRxThread = NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
mIsCmdOpened = false;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::GetCmdDevices(std::vector<std::pair<std::string, std::string>> &device_list)
|
||||||
|
{
|
||||||
|
struct udev *udev;
|
||||||
|
struct udev_enumerate *enumerate;
|
||||||
|
struct udev_list_entry *devices, *dev_list_entry;
|
||||||
|
struct udev_device *dev;
|
||||||
|
|
||||||
|
udev = udev_new();
|
||||||
|
if (!udev)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
enumerate = udev_enumerate_new(udev);
|
||||||
|
udev_enumerate_add_match_subsystem(enumerate, "tty");
|
||||||
|
udev_enumerate_scan_devices(enumerate);
|
||||||
|
devices = udev_enumerate_get_list_entry(enumerate);
|
||||||
|
udev_list_entry_foreach(dev_list_entry, devices)
|
||||||
|
{
|
||||||
|
const char *path;
|
||||||
|
|
||||||
|
path = udev_list_entry_get_name(dev_list_entry);
|
||||||
|
dev = udev_device_new_from_syspath(udev, path);
|
||||||
|
std::string dev_path = std::string(udev_device_get_devnode(dev));
|
||||||
|
dev = udev_device_get_parent_with_subsystem_devtype(dev, "usb", "usb_device");
|
||||||
|
if (dev)
|
||||||
|
{
|
||||||
|
std::pair<std::string, std::string> p;
|
||||||
|
p.first = dev_path;
|
||||||
|
p.second = udev_device_get_sysattr_value(dev, "product");
|
||||||
|
device_list.push_back(p);
|
||||||
|
udev_device_unref(dev);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
udev_enumerate_unref(enumerate);
|
||||||
|
udev_unref(udev);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::ReadFromIO(uint8_t *rx_buf, uint32_t rx_buf_len, uint32_t *rx_len)
|
||||||
|
{
|
||||||
|
static timespec timeout = {0, (long)(100 * 1e6)};
|
||||||
|
int32_t len = -1;
|
||||||
|
|
||||||
|
if (IsOpened())
|
||||||
|
{
|
||||||
|
fd_set read_fds;
|
||||||
|
FD_ZERO(&read_fds);
|
||||||
|
FD_SET(mComHandle, &read_fds);
|
||||||
|
int r = pselect(mComHandle + 1, &read_fds, NULL, NULL, &timeout, NULL);
|
||||||
|
if (r < 0)
|
||||||
|
{
|
||||||
|
// Select was interrupted
|
||||||
|
if (errno == EINTR)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (r == 0) // timeout
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (FD_ISSET(mComHandle, &read_fds))
|
||||||
|
{
|
||||||
|
len = (int32_t)read(mComHandle, rx_buf, rx_buf_len);
|
||||||
|
if ((len != -1) && rx_len)
|
||||||
|
{
|
||||||
|
*rx_len = len;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return len == -1 ? false : true;
|
||||||
|
}
|
||||||
|
bool CmdInterfaceLinux::WriteToIo(const uint8_t *tx_buf, uint32_t tx_buf_len, uint32_t *tx_len)
|
||||||
|
{
|
||||||
|
int32_t len = -1;
|
||||||
|
|
||||||
|
if (IsOpened())
|
||||||
|
{
|
||||||
|
len = (int32_t)write(mComHandle, tx_buf, tx_buf_len);
|
||||||
|
if ((len != -1) && tx_len)
|
||||||
|
{
|
||||||
|
*tx_len = len;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return len == -1 ? false : true;
|
||||||
|
}
|
||||||
|
void CmdInterfaceLinux::mRxThreadProc(void *param)
|
||||||
|
{
|
||||||
|
CmdInterfaceLinux *cmd_if = (CmdInterfaceLinux *)param;
|
||||||
|
char *rx_buf = new char[MAX_ACK_BUF_LEN + 1];
|
||||||
|
uchar failedNum = 0;
|
||||||
|
while (!cmd_if->mRxThreadExitFlag.load())
|
||||||
|
{
|
||||||
|
uint32_t readed = 0;
|
||||||
|
bool res = cmd_if->ReadFromIO((uint8_t *)rx_buf, MAX_ACK_BUF_LEN, &readed);
|
||||||
|
if (res && readed)
|
||||||
|
{
|
||||||
|
cmd_if->mRxCount += readed;
|
||||||
|
if (cmd_if->mReadCallback != nullptr)
|
||||||
|
{
|
||||||
|
cmd_if->mReadCallback(rx_buf, readed);
|
||||||
|
}
|
||||||
|
failedNum = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if(++failedNum == 255)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
cmd_if->mRxThreadExitFlag = true;
|
||||||
|
delete[] rx_buf;
|
||||||
|
}
|
||||||
|
|
||||||
|
LiPkg::LiPkg() : mTimestamp(0),
|
||||||
|
mSpeed(0),
|
||||||
|
mErrorTimes(0),
|
||||||
|
mIsFrameReady(false),
|
||||||
|
mIsPkgReady(false)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
double LiPkg::GetSpeed(void)
|
||||||
|
{
|
||||||
|
return mSpeed / 360.0;
|
||||||
|
}
|
||||||
|
bool LiPkg::Parse(const uint8_t *data, long len)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
mDataTmp.push_back(*(data + i));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (mDataTmp.size() > sizeof(LiDARFrameTypeDef) * 100)
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
mDataTmp.clear();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t start = 0;
|
||||||
|
|
||||||
|
while (start < mDataTmp.size() - 2)
|
||||||
|
{
|
||||||
|
start = 0;
|
||||||
|
while (start < mDataTmp.size() - 2)
|
||||||
|
{
|
||||||
|
if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == PKG_VER_LEN))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((mDataTmp[start] == PKG_HEADER) && (mDataTmp[start + 1] == (PKG_VER_LEN | (0x07 << 5))))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
start++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (start != 0)
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
for (int i = 0; i < start; i++)
|
||||||
|
{
|
||||||
|
mDataTmp.erase(mDataTmp.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
|
||||||
|
return false;
|
||||||
|
|
||||||
|
LiDARFrameTypeDef *pkg = (LiDARFrameTypeDef *)mDataTmp.data();
|
||||||
|
uint8_t crc = 0;
|
||||||
|
|
||||||
|
for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef) - 1; i++)
|
||||||
|
{
|
||||||
|
crc = CrcTable[(crc ^ mDataTmp[i]) & 0xff];
|
||||||
|
}
|
||||||
|
|
||||||
|
if (crc == pkg->crc8)
|
||||||
|
{
|
||||||
|
double diff = (pkg->end_angle / 100 - pkg->start_angle / 100 + 360) % 360;
|
||||||
|
if (diff > (double)pkg->speed * POINT_PER_PACK / 2300 * 3 / 2)
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mSpeed = pkg->speed;
|
||||||
|
mTimestamp = pkg->timestamp;
|
||||||
|
uint32_t diff = ((uint32_t)pkg->end_angle + 36000 - (uint32_t)pkg->start_angle) % 36000;
|
||||||
|
float step = diff / (POINT_PER_PACK - 1) / 100.0;
|
||||||
|
float start = (double)pkg->start_angle / 100.0;
|
||||||
|
float end = (double)(pkg->end_angle % 36000) / 100.0;
|
||||||
|
PointData data;
|
||||||
|
for (int i = 0; i < POINT_PER_PACK; i++)
|
||||||
|
{
|
||||||
|
data.distance = pkg->point[i].distance;
|
||||||
|
data.angle = start + i * step;
|
||||||
|
if (data.angle >= 360.0)
|
||||||
|
{
|
||||||
|
data.angle -= 360.0;
|
||||||
|
}
|
||||||
|
data.confidence = pkg->point[i].confidence;
|
||||||
|
mOnePkg[i] = data;
|
||||||
|
mFrameTemp.push_back(PointData(data.angle, data.distance, data.confidence));
|
||||||
|
}
|
||||||
|
// prevent angle invert
|
||||||
|
mOnePkg.back().angle = end;
|
||||||
|
|
||||||
|
mIsPkgReady = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (uint32_t i = 0; i < sizeof(LiDARFrameTypeDef); i++)
|
||||||
|
{
|
||||||
|
mDataTmp.erase(mDataTmp.begin());
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mDataTmp.size() < sizeof(LiDARFrameTypeDef))
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mErrorTimes++;
|
||||||
|
/*only remove header,not all frame,because lidar data may contain head*/
|
||||||
|
for (int i = 0; i < 2; i++)
|
||||||
|
{
|
||||||
|
mDataTmp.erase(mDataTmp.begin());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
bool LiPkg::AssemblePacket()
|
||||||
|
{
|
||||||
|
float last_angle = 0;
|
||||||
|
std::vector<PointData> tmp;
|
||||||
|
int count = 0;
|
||||||
|
for (auto n : mFrameTemp)
|
||||||
|
{
|
||||||
|
/*wait for enough data, need enough data to show a circle*/
|
||||||
|
if (n.angle - last_angle < (-350.f)) /* enough data has been obtained */
|
||||||
|
{
|
||||||
|
mFrameData.len = tmp.size();
|
||||||
|
Transform(tmp); /*transform raw data to stantard data */
|
||||||
|
|
||||||
|
if (tmp.size() == 0)
|
||||||
|
{
|
||||||
|
mFrameTemp.clear();
|
||||||
|
mIsFrameReady = false;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef USE_SLBI
|
||||||
|
Slbi sb(mSpeed);
|
||||||
|
sb.FindBarcode(tmp);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_SLBF
|
||||||
|
Slbf sb(mSpeed);
|
||||||
|
tmp = sb.NearFilter(tmp);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b)
|
||||||
|
{ return a.angle < b.angle; });
|
||||||
|
mFrameData.angle_min = tmp[0].angle;
|
||||||
|
mFrameData.angle_max = tmp.back().angle;
|
||||||
|
mFrameData.distance.clear();
|
||||||
|
mFrameData.intensities.clear();
|
||||||
|
mFrameData.distance.resize(mFrameData.len);
|
||||||
|
mFrameData.intensities.resize(mFrameData.len);
|
||||||
|
float step = (mFrameData.angle_max - mFrameData.angle_min) / mFrameData.len;
|
||||||
|
float angle_acc = mFrameData.angle_min;
|
||||||
|
int tmp_count = 0;
|
||||||
|
/* interpolation method */
|
||||||
|
for (uint32_t i = 0; i < mFrameData.len; i++)
|
||||||
|
{
|
||||||
|
if (angle_acc >= tmp[tmp_count].angle)
|
||||||
|
{
|
||||||
|
mFrameData.distance[i] = tmp[tmp_count].distance;
|
||||||
|
mFrameData.intensities[i] = tmp[tmp_count].confidence;
|
||||||
|
tmp_count++;
|
||||||
|
if (tmp_count == tmp.size())
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
mFrameData.distance[i] = 0;
|
||||||
|
mFrameData.intensities[i] = 0;
|
||||||
|
}
|
||||||
|
angle_acc += step;
|
||||||
|
}
|
||||||
|
std::vector<PointData> tmp2;
|
||||||
|
for (uint32_t i = count; i < mFrameTemp.size(); i++)
|
||||||
|
{
|
||||||
|
tmp2.push_back(mFrameTemp[i]);
|
||||||
|
}
|
||||||
|
mFrameTemp.clear();
|
||||||
|
mFrameTemp = tmp2;
|
||||||
|
mIsFrameReady = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
tmp.push_back(n); /* getting data */
|
||||||
|
}
|
||||||
|
|
||||||
|
count++;
|
||||||
|
last_angle = n.angle;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
const std::array<PointData, POINT_PER_PACK> &LiPkg::GetPkgData(void)
|
||||||
|
{
|
||||||
|
mIsPkgReady = false;
|
||||||
|
return mOnePkg;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<cv::Mat> LiPkg::scan_to_pointcloud(FrameData mFrameData)
|
||||||
|
{
|
||||||
|
u_int32_t len = mFrameData.len;
|
||||||
|
float angle_min = ANGLE_TO_RADIAN(mFrameData.angle_min);
|
||||||
|
float angle_max = ANGLE_TO_RADIAN(mFrameData.angle_max);
|
||||||
|
cv::Mat pointcloud2(len, 3, CV_32F);
|
||||||
|
cv::Mat intensities(1, len, CV_32F);
|
||||||
|
cv::Mat pointdis(1, len, CV_32F);
|
||||||
|
float a_increment = (angle_max - angle_min) / (float)(len - 1);
|
||||||
|
for (int i = 0; i < len; i++)
|
||||||
|
{
|
||||||
|
float dis = mFrameData.distance[i] / 1000.f;
|
||||||
|
intensities.at<float>(0, i) = mFrameData.intensities[i];
|
||||||
|
pointdis.at<float>(0, i) = dis;
|
||||||
|
|
||||||
|
float x_temp = cos(angle_min + i * a_increment) * dis;
|
||||||
|
float y_temp = sin(angle_min + i * a_increment) * dis;
|
||||||
|
pointcloud2.at<float>(i, 0) = x_temp;
|
||||||
|
pointcloud2.at<float>(i, 1) = y_temp;
|
||||||
|
pointcloud2.at<float>(i, 2) = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
return std::vector<cv::Mat>{pointcloud2, intensities, pointdis};
|
||||||
|
}
|
||||||
|
|
||||||
|
/********************* (C) COPYRIGHT LD Robot *******END OF FILE ********/
|
||||||
|
|
||||||
|
SlTransform::SlTransform(LDVersion version, bool to_right_hand)
|
||||||
|
{
|
||||||
|
switch (version)
|
||||||
|
{
|
||||||
|
case LDVersion::LD_ZERO:
|
||||||
|
case LDVersion::LD_NINE:
|
||||||
|
offset_x = 8.1;
|
||||||
|
offset_y = -22.5156;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case LDVersion::LD_THREE:
|
||||||
|
case LDVersion::LD_EIGHT:
|
||||||
|
offset_x = 5.9;
|
||||||
|
offset_y = -20.14;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case LDVersion::LD_FOURTEENTH:
|
||||||
|
offset_x = 5.9;
|
||||||
|
offset_y = -20.14;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
Points2D SlTransform::Transform(const Points2D &data)
|
||||||
|
{
|
||||||
|
Points2D tmp2;
|
||||||
|
for (auto n : data)
|
||||||
|
{
|
||||||
|
/*Filter out invalid data*/
|
||||||
|
if (n.distance == 0)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
/*transfer the origin to the center of lidar circle*/
|
||||||
|
/*The default direction of radar rotation is clockwise*/
|
||||||
|
/*transfer to the right-hand coordinate system*/
|
||||||
|
float right_hand = (360.f - n.angle);
|
||||||
|
double x = n.distance + offset_x;
|
||||||
|
double y = n.distance * 0.11923 + offset_y;
|
||||||
|
double d = sqrt(x * x + y * y);
|
||||||
|
double shift = atan(y / x) * 180.f / 3.14159;
|
||||||
|
/*Choose whether to use the right-hand system according to the flag*/
|
||||||
|
double angle;
|
||||||
|
if (to_right_hand)
|
||||||
|
angle = right_hand + shift;
|
||||||
|
else
|
||||||
|
angle = n.angle - shift;
|
||||||
|
|
||||||
|
if (angle > 360)
|
||||||
|
{
|
||||||
|
angle -= 360;
|
||||||
|
}
|
||||||
|
if (angle < 0)
|
||||||
|
{
|
||||||
|
angle += 360;
|
||||||
|
}
|
||||||
|
|
||||||
|
tmp2.push_back(PointData(angle, n.distance, n.confidence, x, y));
|
||||||
|
}
|
||||||
|
|
||||||
|
return tmp2;
|
||||||
|
}
|
||||||
|
SlTransform::~SlTransform()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
void LD14_LiPkg::Transform(std::vector<PointData> &tmp)
|
||||||
|
{
|
||||||
|
SlTransform trans(LDVersion::LD_FOURTEENTH);
|
||||||
|
tmp = trans.Transform(tmp);
|
||||||
|
// std::cout << "Transform LD_FOURTEENTH !!" << std::endl;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,158 @@
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "fusion_car/lidar.h"
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#include <sensor_msgs/LaserScan.h>
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include "fusion_car/ranging.h"
|
||||||
|
#include "fusion_car/fusion.h"
|
||||||
|
#include <string>
|
||||||
|
#include <pthread.h>
|
||||||
|
#include <mutex>
|
||||||
|
#include <sys/time.h>
|
||||||
|
/*#include <cv_bridge/cv_bridge.h>
|
||||||
|
#include <image_transport/image_transport.h>*/
|
||||||
|
#include "opencv2/imgcodecs/imgcodecs.hpp"
|
||||||
|
#include <ctime>
|
||||||
|
|
||||||
|
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||||
|
|
||||||
|
Ranging r(9, 640, 480);
|
||||||
|
std::queue<std::vector<Mat>> frameInfo;
|
||||||
|
std::queue<FrameData> laser_queue;
|
||||||
|
std::mutex g_mutex;
|
||||||
|
|
||||||
|
void *ranging(void *args) // ranging线程
|
||||||
|
{
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
std::cout<<" ************ enter ranging *********** "<<std::endl;
|
||||||
|
std::vector<Mat> result = r.get_range();
|
||||||
|
g_mutex.lock();
|
||||||
|
for (uchar i = 0; i < frameInfo.size(); i++) // 只保存当前最新的图片帧信息
|
||||||
|
frameInfo.pop();
|
||||||
|
frameInfo.push(result);
|
||||||
|
g_mutex.unlock();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int main(int argc, char **argv)
|
||||||
|
{
|
||||||
|
uint8_t laser_queue_size = 2; // 雷达帧队列大小参数
|
||||||
|
LD14_LiPkg *pkg = new LD14_LiPkg;
|
||||||
|
CmdInterfaceLinux cmd_port(9);
|
||||||
|
std::string port_name = "/dev/ttyUSB0";
|
||||||
|
if (argc > 1)
|
||||||
|
port_name = argv[1];
|
||||||
|
// 雷达读取回调函数，一直在执行雷达读取和处理数据
|
||||||
|
cmd_port.SetReadCallback([&pkg](const char *byte, size_t len) {
|
||||||
|
if (pkg->Parse((uint8_t*)byte, len))
|
||||||
|
{
|
||||||
|
pkg->AssemblePacket();
|
||||||
|
} });
|
||||||
|
if (!cmd_port.Open(port_name))
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
ros::init(argc, argv, "publish");
|
||||||
|
ros::NodeHandle nh; /* create a ROS Node */
|
||||||
|
ros::Publisher uplidar_pub = nh.advertise<sensor_msgs::LaserScan>("up_scan", 1);
|
||||||
|
//ros::Publisher downlidar_pub = nh.advertise<sensor_msgs::LaserScan>("down_scan", 1);
|
||||||
|
sensor_msgs::LaserScan upscan;
|
||||||
|
// sensor_msgs::LaserScan downscan;
|
||||||
|
|
||||||
|
|
||||||
|
/*image_transport::ImageTransport it(nh);
|
||||||
|
image_transport::Publisher img_pub = it.advertise("camera/image", 1);*/
|
||||||
|
pthread_t tids[1]; // 执行ranging线程
|
||||||
|
int ret = pthread_create(&tids[0], NULL, ranging, NULL);
|
||||||
|
if (ret != 0)
|
||||||
|
{
|
||||||
|
std::cout << "Multithreading error !" << std::endl;
|
||||||
|
return -2;
|
||||||
|
}
|
||||||
|
|
||||||
|
LCFusion lcFusion(&r);
|
||||||
|
namedWindow("camera");
|
||||||
|
|
||||||
|
while (ros::ok() && !cmd_port.IsExited())
|
||||||
|
{
|
||||||
|
int64 t = getTickCount();
|
||||||
|
if (pkg->IsFrameReady()) // 若一帧雷达处理好，执行后续操作
|
||||||
|
{
|
||||||
|
FrameData laserData_queue = pkg->GetFrameData(); // 得到雷达数据
|
||||||
|
laserData_queue.timestamp = ros::Time::now();
|
||||||
|
laser_queue.push(laserData_queue);
|
||||||
|
while(laser_queue.size() > laser_queue_size) // 只保存两帧雷达数据
|
||||||
|
{
|
||||||
|
laser_queue.pop();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
continue;
|
||||||
|
if (laser_queue.size() == laser_queue_size) // 若队列有两帧数据，执行后续操作
|
||||||
|
{
|
||||||
|
FrameData laserData = laser_queue.front();
|
||||||
|
laser_queue.pop();
|
||||||
|
upscan.header.stamp = laserData.timestamp;
|
||||||
|
// upscan.header.stamp = ros::Time::now();
|
||||||
|
// downscan.header.stamp = upscan.header.stamp;
|
||||||
|
|
||||||
|
// uint16_t times_stamp = pkg->GetTimestamp();
|
||||||
|
// FrameData laserData = pkg->GetFrameData();
|
||||||
|
char state = -1;
|
||||||
|
|
||||||
|
float angle_min = ANGLE_TO_RADIAN(laserData.angle_min); // 角度弧度转换
|
||||||
|
float angle_max = ANGLE_TO_RADIAN(laserData.angle_max);
|
||||||
|
uint32_t len = laserData.len;
|
||||||
|
float angle_inc = (angle_max - angle_min) / (len - 1.); // 角度增量
|
||||||
|
lcFusion.filter_laser(laserData, angle_min, angle_inc); // 视角同步，处理视角内雷达数据
|
||||||
|
if (!frameInfo.empty())
|
||||||
|
{
|
||||||
|
g_mutex.lock();
|
||||||
|
std::vector<Mat> camInfo = frameInfo.front(); // 读取图像帧数据
|
||||||
|
frameInfo.pop();
|
||||||
|
g_mutex.unlock();
|
||||||
|
double fusion_old,fusion_now;
|
||||||
|
fusion_old = ros::Time::now().toSec();
|
||||||
|
state = lcFusion.fusion(camInfo); // 融合
|
||||||
|
fusion_now = ros::Time::now().toSec();
|
||||||
|
cout << "data_fusion: " << fusion_now - fusion_old << endl;
|
||||||
|
if (state != -2)
|
||||||
|
{
|
||||||
|
lcFusion.set_laser_data(upscan); // 设置雷达发布数据
|
||||||
|
uplidar_pub.publish(upscan); // 发布
|
||||||
|
}
|
||||||
|
// 调试使用，将雷达点映射到图像中显示
|
||||||
|
if (!lcFusion.projectedPoints.empty())
|
||||||
|
{
|
||||||
|
for (int i = 0; i < lcFusion.projectedPoints.size(); i++)
|
||||||
|
{
|
||||||
|
Point2f p = lcFusion.projectedPoints[i];
|
||||||
|
circle(camInfo[0], p, 3, Scalar(0, 255, 0), 1, 8);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
/* sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", camInfo[0]).toImageMsg();
|
||||||
|
img_pub.publish(msg);*/
|
||||||
|
if (!camInfo[0].empty())
|
||||||
|
{
|
||||||
|
cv::Mat pic;
|
||||||
|
cv::Size dsize= Size(int(640*0.75),int(480*0.75));
|
||||||
|
cv::resize(camInfo[0],pic,dsize,0,0,INTER_AREA);
|
||||||
|
// cv::pyrDown(camInfo[0],pic);
|
||||||
|
imshow("camera", pic);
|
||||||
|
}
|
||||||
|
t = getTickCount() - t;
|
||||||
|
char fps[50];
|
||||||
|
std::cout << "fps: " << int(1 / (t / getTickFrequency())) << std::endl;
|
||||||
|
if (waitKey(1) == 81)
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
//lcFusion.set_laser_data(downscan, laserData);
|
||||||
|
//downlidar_pub.publish(downscan);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
std::cout << "ROS or lidar error !" << std::endl;
|
||||||
|
delete pkg;
|
||||||
|
cmd_port.Close();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,444 @@
|
||||||
|
#include <opencv2/opencv.hpp>
|
||||||
|
#include <opencv2/highgui.hpp>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdio.h>
|
||||||
|
#include "fusion_car/ranging.h"
|
||||||
|
#include <math.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "fusion_car/rknn_sdk.h"
|
||||||
|
#include <string.h>
|
||||||
|
#include <sys/time.h>
|
||||||
|
#include <dlfcn.h>
|
||||||
|
#include "opencv2/core.hpp"
|
||||||
|
#include "opencv2/imgcodecs.hpp"
|
||||||
|
#include "opencv2/imgproc.hpp"
|
||||||
|
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <sys/types.h>
|
||||||
|
#include <dirent.h>
|
||||||
|
#include <vector>
|
||||||
|
#include <tuple>
|
||||||
|
#include <string>
|
||||||
|
#include <string.h>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
using namespace cv;
|
||||||
|
|
||||||
|
|
||||||
|
void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数
|
||||||
|
{
|
||||||
|
remap(oriImgL, oriImgL, mapX1, mapX2, INTER_LINEAR);
|
||||||
|
remap(oriImgR, oriImgR, mapY1, mapY2, INTER_LINEAR);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
|
||||||
|
{
|
||||||
|
//(u,v)->(x,y)"(loc[0],loc[1])"
|
||||||
|
std::vector<float> loc;
|
||||||
|
loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
|
||||||
|
loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
|
||||||
|
return loc;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配
|
||||||
|
{
|
||||||
|
int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
|
||||||
|
// cv::Mat right_image_,left_image_;
|
||||||
|
// cv::pyrDown(right_image, right_image_);
|
||||||
|
// cv::pyrDown(left_image, left_image_);
|
||||||
|
Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框
|
||||||
|
Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域
|
||||||
|
// Mat tpl = right_image_.rowRange(max(int(0.5*y1 - 2), 0), min(int(0.5*y2 + 2), 479)).colRange(int(0.5*x1), int(0.5*x2));
|
||||||
|
// Mat target = left_image_.rowRange(max(int(0.5*y1 - 10), 0), min(int(0.5*y2 + 10), 239)).colRange(0, 319);
|
||||||
|
int th = tpl.rows, tw = tpl.cols;
|
||||||
|
Mat result;
|
||||||
|
// methods = TM_CCOEFF_NORMED, TM_SQDIFF_NORMED, TM_CCORR_NORMED]
|
||||||
|
matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关
|
||||||
|
double minVal, maxVal;
|
||||||
|
Point minLoc, maxLoc;
|
||||||
|
minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标
|
||||||
|
Point tl = maxLoc, br;
|
||||||
|
|
||||||
|
br.x = min(maxLoc.x + tw, 639); //转为像素坐标系
|
||||||
|
br.y = min(maxLoc.y + th, 479); //转为像素坐标系
|
||||||
|
////展示匹配结果
|
||||||
|
// br.x = min(maxLoc.x + tw, 319);
|
||||||
|
// br.y = min(maxLoc.y + th, 239);
|
||||||
|
//rectangle(target, tl, br, (0, 255, 0), 3);
|
||||||
|
//imshow("match-", target);
|
||||||
|
//waitKey(2);
|
||||||
|
std::vector<int> maxloc;
|
||||||
|
maxloc.push_back(maxLoc.x);
|
||||||
|
maxloc.push_back(maxLoc.y);
|
||||||
|
return maxloc;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k)
|
||||||
|
{
|
||||||
|
//保证摄像头与地面平行
|
||||||
|
int x1 = bboxs.at<float>(k, 0);
|
||||||
|
int x2 = bboxs.at<float>(k, 2);
|
||||||
|
int y1 = bboxs.at<float>(k, 1);
|
||||||
|
int y2 = bboxs.at<float>(k, 3);
|
||||||
|
float Conf = bboxs.at<float>(k, 4);
|
||||||
|
int cls = bboxs.at<float>(k, 5);
|
||||||
|
float Y_B, Y_H;
|
||||||
|
Mat edge, grayImage;
|
||||||
|
vector<cv::Point> idx;
|
||||||
|
Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围
|
||||||
|
//Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639);
|
||||||
|
Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1);
|
||||||
|
cvtColor(tpl, grayImage, COLOR_BGR2GRAY);
|
||||||
|
GaussianBlur(grayImage,grayImage,Size(5,5),0);
|
||||||
|
Canny(grayImage, edge, 120, 180, 3); //提取边缘，获取与地面接触点
|
||||||
|
//cv::imshow("1",edge);
|
||||||
|
//cv::waitKey(1);
|
||||||
|
float cluster[650];
|
||||||
|
for (int i = 0;i<650;i++)
|
||||||
|
{
|
||||||
|
cluster[i] = 0;
|
||||||
|
}
|
||||||
|
int y_b, y_h;
|
||||||
|
int j = 0;
|
||||||
|
for (int i = 0; i < x2-x1; i++)
|
||||||
|
{
|
||||||
|
//Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2);
|
||||||
|
Mat temp = edge.col(i); //取第i列
|
||||||
|
// std::cout << "temp: " <<temp << std::endl;
|
||||||
|
cv::findNonZero(temp, idx);
|
||||||
|
std::vector<float> point_b = pic2cam(x1 + i, 240); //转为相机坐标系
|
||||||
|
std::vector<float> point_H = pic2cam(320, 240);
|
||||||
|
float alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
|
||||||
|
if (idx.size() < 1)
|
||||||
|
{
|
||||||
|
Z.at<float>(0, i) = 0;
|
||||||
|
Z.at<float>(1, i) = alfa;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
int y_b = idx[idx.size() - 1].y + y1; //
|
||||||
|
y_b = int(y_b + y_b*0.03);
|
||||||
|
int y_h = 240;
|
||||||
|
point_b = pic2cam(x1 + i, y_b); //转为相机坐标系
|
||||||
|
point_H = pic2cam(320, y_h);
|
||||||
|
Y_B = point_b[1];
|
||||||
|
Y_H = point_H[1];
|
||||||
|
// std::cout << "y_b: " << y_b << std::endl;
|
||||||
|
float H_c = 60; //摄像头离地高度，单位mm
|
||||||
|
float theta = 0; //摄像头与地面夹角，弧度
|
||||||
|
float d = (1/cos(theta)* cos(theta)) * q.at<double>(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta);
|
||||||
|
alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
|
||||||
|
//cout << "d: " << d << endl;
|
||||||
|
if (d > 700)
|
||||||
|
{d = 0;}
|
||||||
|
Z.at<float>(0, i) = d/cos(alfa);
|
||||||
|
Z.at<float>(1, i) = alfa;
|
||||||
|
}
|
||||||
|
/*if (i > 0)
|
||||||
|
{
|
||||||
|
if (abs(Z.at<float>(0, i) - Z.at<float>(0, i - 1)) < 40) //聚类
|
||||||
|
{
|
||||||
|
cluster[j] = cluster[j] + 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
j = j + 1;
|
||||||
|
cluster[j] = 1;
|
||||||
|
//std::cout<<"j : "<< j<< std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
//int max_loc = distance(cluster,max_element(cluster,cluster + sizeof(cluster)/sizeof(cluster[0]))); //只保留数量最多的一类，其余置0
|
||||||
|
//std::cout<<" max_loc : "<< max_loc<<std::endl;
|
||||||
|
int temp = 0;
|
||||||
|
for (int i = 0; i < j; i++)
|
||||||
|
{
|
||||||
|
if (cluster[i] < 3)
|
||||||
|
{
|
||||||
|
for (int t = temp; t < temp + cluster[i]; t++)
|
||||||
|
{
|
||||||
|
Z.at<float>(0, t) = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
temp = temp + cluster[i];
|
||||||
|
}*/
|
||||||
|
// std::cout << "z : " <<Z << std::endl;
|
||||||
|
this->Z = Z.clone();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
|
||||||
|
{
|
||||||
|
// ֱ<><D6B1>ͼ<EFBFBD><CDBC><EFBFBD>⻯
|
||||||
|
Mat imgGrayL, imgGrayR;
|
||||||
|
cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY);
|
||||||
|
cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY);
|
||||||
|
Mat imgR_weight = imgR.clone();
|
||||||
|
Mat infoRow;
|
||||||
|
for (uchar i = 0; i < detBoxes.rows; i++)
|
||||||
|
{
|
||||||
|
int x1 = detBoxes.at<float>(i, 0);
|
||||||
|
int y1 = detBoxes.at<float>(i, 1);
|
||||||
|
int x2 = detBoxes.at<float>(i, 2);
|
||||||
|
int y2 = detBoxes.at<float>(i, 3);
|
||||||
|
//std::cout<<"x1: "<<x1<<"x2: "<<x2<<"y1: "<<y1<<"y2: "<<y2<<std::endl;
|
||||||
|
float Conf = detBoxes.at<float>(i, 4);
|
||||||
|
int cls = detBoxes.at<float>(i, 5);
|
||||||
|
|
||||||
|
if (cls == 6 || cls == 10 || cls == 11) //体重秤，地毯和毛巾采用单目测距方法
|
||||||
|
{
|
||||||
|
horizon_estimate(imgR_weight, detBoxes, i);
|
||||||
|
}
|
||||||
|
if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体
|
||||||
|
{
|
||||||
|
|
||||||
|
//char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
rectangle(imgR, Point(int(x1), int(y1)),
|
||||||
|
Point(int(x2), int(y2)), Scalar(0, 0, 255));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (cls!=0 && cls!=1 && cls!=2 && cls!= 6 && cls!= 7 && cls!= 10 && cls!=11 && cls!=14 && cls!=15) //大物体不进行测距
|
||||||
|
{
|
||||||
|
if (x1 < 30 || x2 < 80 || x1>580 || x2 > 610) //删除边缘的目标框
|
||||||
|
{
|
||||||
|
detBoxes.at<float>(i, 5) = -1;
|
||||||
|
cls = detBoxes.at<float>(i, 5);
|
||||||
|
}
|
||||||
|
//char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
rectangle(imgR, Point(int(x1), int(y1)),
|
||||||
|
Point(int(x2), int(y2)), Scalar(0, 0, 255));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
rectangle(imgR, Point(int(x1), int(y1)),
|
||||||
|
Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框
|
||||||
|
|
||||||
|
int coordinates[4] = {x1, y1, x2, y2};
|
||||||
|
std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
|
||||||
|
float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差
|
||||||
|
float disp_pixel_y = disp_pixel[1] - y1; //
|
||||||
|
disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到
|
||||||
|
|
||||||
|
//Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz;
|
||||||
|
Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息
|
||||||
|
Mat threed_pixel_xyz, threedImage;
|
||||||
|
reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
|
||||||
|
threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方，
|
||||||
|
std::vector<Mat> channels;
|
||||||
|
split(threed_pixel_xyz.clone(), channels);
|
||||||
|
threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离
|
||||||
|
threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d
|
||||||
|
|
||||||
|
int mid_pixel = int((x1 + x2) / 2);
|
||||||
|
std::vector<float> mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标
|
||||||
|
std::vector<float> loc_tar = pic2cam(mid_pixel, imgGrayR.rows);
|
||||||
|
float alfa = atan((loc_tar[0] - mid[0]) / q.at<double>(2, 3));
|
||||||
|
|
||||||
|
|
||||||
|
if (disp_pixel_x > 240) // 距离太近，视差过大
|
||||||
|
{
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
float median = threed_pixel_xyz.at<float>((int)(y1 + y2) / 2, (int)(x1 + x2) / 2);
|
||||||
|
|
||||||
|
std::vector<float> ltPoint = pic2cam(x1, y1);
|
||||||
|
std::vector<float> rbPoint = pic2cam(x2, y2);
|
||||||
|
float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高
|
||||||
|
float f = q.at<double>(2, 3);
|
||||||
|
float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出
|
||||||
|
//float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1;
|
||||||
|
float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1;
|
||||||
|
float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f);
|
||||||
|
//float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2;
|
||||||
|
float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2;
|
||||||
|
float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
|
||||||
|
pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
|
||||||
|
pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
|
||||||
|
|
||||||
|
w1 = w1 / 10;
|
||||||
|
h1 = (h1 + h2) / 10;
|
||||||
|
median /= 10;
|
||||||
|
if (median > 120) //过远测距误差较大
|
||||||
|
{
|
||||||
|
//char tf[9];
|
||||||
|
//sprintf(tf, "Too far!");
|
||||||
|
char cm[15];
|
||||||
|
//sprintf(cm, "cannot match !");
|
||||||
|
sprintf(cm, "%d , %.2f", cls,Conf);
|
||||||
|
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||||
|
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
// <20><>ͼ<EFBFBD><CDBC><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
||||||
|
char dc[50], wh[50];
|
||||||
|
std::string cname = className[cls + 1];
|
||||||
|
sprintf(dc, "dis:%.2fcm %d", median, cls);
|
||||||
|
sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa);
|
||||||
|
putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2);
|
||||||
|
putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5);
|
||||||
|
|
||||||
|
|
||||||
|
//返回数据
|
||||||
|
infoRow = (Mat_<float>(1, 4) << median, w1, h1, alfa);
|
||||||
|
infoRow.copyTo(info.row(i));
|
||||||
|
};
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
Ranging::Ranging(int index, int imgw, int imgh) : //初始化
|
||||||
|
mapX1(imgh, imgw, CV_64F), //初始化矩阵 ，用于计算无畸变和修正转换映射。
|
||||||
|
mapX2(imgh, imgw, CV_64F),
|
||||||
|
mapY1(imgh, imgw, CV_64F),
|
||||||
|
mapY2(imgh, imgw, CV_64F),
|
||||||
|
q(4, 4, CV_64F),
|
||||||
|
imgw(imgw),
|
||||||
|
imgh(imgh)
|
||||||
|
{
|
||||||
|
// Z = Mat::zeros(2, 1, CV_32FC1);
|
||||||
|
if (!vcapture.open(index))
|
||||||
|
{
|
||||||
|
std::cout << "Open camera failed !" << std::endl;
|
||||||
|
exit(EXIT_FAILURE);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
//vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG);
|
||||||
|
vcapture.set(CAP_PROP_FPS, 30);
|
||||||
|
vcapture.set(CAP_PROP_FRAME_WIDTH, imgw * 2);
|
||||||
|
vcapture.set(CAP_PROP_FRAME_HEIGHT, imgh);
|
||||||
|
vcapture.set(CAP_PROP_BUFFERSIZE, 1);
|
||||||
|
|
||||||
|
auto imgSize = Size(imgw, imgh);
|
||||||
|
Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F);
|
||||||
|
|
||||||
|
stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,
|
||||||
|
imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正
|
||||||
|
|
||||||
|
initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射
|
||||||
|
initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射
|
||||||
|
|
||||||
|
RKNN_Create(&hdx, modelPath); // 初始化检测模型
|
||||||
|
std::cout<< " ******************* CAMERA initialization ********************" << std::endl;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
|
||||||
|
// {
|
||||||
|
// std::vector<float> loc;
|
||||||
|
// loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
|
||||||
|
// loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
|
||||||
|
// return loc;
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
std::vector<Mat> Ranging::get_range()
|
||||||
|
{
|
||||||
|
double rang_old, rang_now;
|
||||||
|
rang_old = ros::Time::now().toSec(); //测试运行时间
|
||||||
|
Mat frame, lframe, rframe;
|
||||||
|
vcapture.read(frame); //获取视频帧
|
||||||
|
|
||||||
|
if (!frame.empty())
|
||||||
|
{
|
||||||
|
int64 t = getTickCount();
|
||||||
|
Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图
|
||||||
|
Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图
|
||||||
|
//imshow("lframe",lframe);
|
||||||
|
//waitKey(1);
|
||||||
|
rectifyImage(lframe, rframe); //
|
||||||
|
|
||||||
|
DetRet *det_ret = NULL; //
|
||||||
|
InputData input_data; //定义输入数据
|
||||||
|
int shape[4] = {1, 3, rframe.rows, rframe.cols}, det_num = 0;
|
||||||
|
memcpy(input_data.shape, shape, sizeof(shape));
|
||||||
|
Mat Rframe = rframe.clone();
|
||||||
|
|
||||||
|
double detect_old, detect_now;
|
||||||
|
detect_old = ros::Time::now().toSec();
|
||||||
|
input_data.data = Rframe.data;
|
||||||
|
//std::cout<<"Rframe.data.shape"<<Rframe.rows<<std::endl;
|
||||||
|
RKNN_ObjDet(hdx, &input_data, &det_ret, &det_num);
|
||||||
|
detect_now = ros::Time::now().toSec();
|
||||||
|
|
||||||
|
//cout << "data_detect_time:" << detect_now-detect_old << endl;
|
||||||
|
//std::cout<<"det_num "<<det_num<<std::endl;
|
||||||
|
if (!det_num)
|
||||||
|
{
|
||||||
|
//std::cout<<"return NULL"<<std::endl;
|
||||||
|
return std::vector<Mat>{rframe}; //没有检测框时，退出
|
||||||
|
}
|
||||||
|
std::cout << "det_num: " << det_num << std::endl;
|
||||||
|
// detction box transfor to our format
|
||||||
|
Mat detBoxes(det_num, 6, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls)
|
||||||
|
for (int i = 0; i < det_num; i++) //存储目标检测内容 (x,y,x,y,conf,cls)
|
||||||
|
{
|
||||||
|
DetRet det_result = det_ret[i];
|
||||||
|
// xyxy conf cls
|
||||||
|
float xmin = rframe.cols * det_result.fLeft;
|
||||||
|
float ymin = rframe.rows * det_result.fTop;
|
||||||
|
float xmax = rframe.cols * det_result.fRight;
|
||||||
|
float ymax = rframe.rows * det_result.fBottom;
|
||||||
|
|
||||||
|
detBoxes.at<float>(i, 0) = xmin;
|
||||||
|
detBoxes.at<float>(i, 1) = ymin;
|
||||||
|
detBoxes.at<float>(i, 2) = xmax;
|
||||||
|
detBoxes.at<float>(i, 3) = ymax;
|
||||||
|
detBoxes.at<float>(i, 4) = det_result.fConf;
|
||||||
|
// 实验测试，过滤过大的误检框
|
||||||
|
float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
|
||||||
|
if (ratio > 0.7)
|
||||||
|
{
|
||||||
|
detBoxes.at<float>(i, 5) = -1;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
detBoxes.at<float>(i, 5) = det_result.nLabel;
|
||||||
|
if (det_result.nLabel == 1 || det_result.nLabel == 2) //检测目标为体重秤和风扇底座时，将目标框拉高，使杆子的雷达点能与目标框对应
|
||||||
|
{
|
||||||
|
detBoxes.at<float>(i, 1) = 100;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
Mat info(det_num, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
|
||||||
|
if (det_num)
|
||||||
|
{
|
||||||
|
// double rang_old, rang_now;
|
||||||
|
// rang_old = ros::Time::now().toSec();
|
||||||
|
getInfo(lframe, rframe, detBoxes, info);
|
||||||
|
// rang_now = ros::Time::now().toSec();
|
||||||
|
// cout << "data_dis_time: " << rang_now-rang_old << endl;
|
||||||
|
}
|
||||||
|
t = getTickCount() - t;
|
||||||
|
char fps[50];
|
||||||
|
sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency())));
|
||||||
|
putText(rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5);
|
||||||
|
// rang_now = ros::Time::now().toSec();
|
||||||
|
// cout << "data_rang_time" << rang_now - rang_old << endl;
|
||||||
|
return std::vector<Mat>{rframe, detBoxes, info};
|
||||||
|
}
|
||||||
|
return std::vector<Mat>{rframe};
|
||||||
|
}
|
||||||
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,19 @@
|
||||||
|
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||||
|
7.678687801628776e+02, 0, 0,
|
||||||
|
0,7.679707836247087e+02, 0,
|
||||||
|
6.566692968103456e+02, 3.390198663125851e+02, 1);
|
||||||
|
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||||
|
7.824799867061021e+02, 0, 0,
|
||||||
|
0,7.823093118883010e+02, 0,
|
||||||
|
6.354310487450947e+02, 3.471699497317592e+02, 1);
|
||||||
|
Mat distortion_l = (Mat_<double>(1, 5) <<0.124875722106329,-0.135334967786191, 0,
|
||||||
|
0, 0);
|
||||||
|
|
||||||
|
Mat distortion_r = (Mat_<double>(1, 5) <.115700316133502, -0.115533076004258, 0,
|
||||||
|
0, 0);
|
||||||
|
Mat rotate = (Mat_<double>(3, 3) <<
|
||||||
|
0.999990145255059, 4.053889756923358e-04, 0.004420978686347,
|
||||||
|
-3.935924151610147e-04, 0.999996361134497, -0.002668857953253,
|
||||||
|
-0.004422044524592, 0.002667091588660,0.999986665983442);
|
||||||
|
Mat trans = (Mat_<double>(3, 1) <<
|
||||||
|
-60.619847880565940, -0.180746754134188, -0.343456473917128);
|
||||||
Loading…
Reference in New Issue