From 34f14241f13d99a5f45e3f8e13bd454542878a97 Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Thu, 16 Jul 2026 19:07:42 +0800 Subject: [PATCH 01/10] =?UTF-8?q?=E8=AE=BA=E6=96=87results=E9=83=A8?= =?UTF-8?q?=E5=88=86=E6=92=B0=E5=86=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../multi_obj_track/object_detection.sh | 2 +- waypoint_control/twin_paper/main.tex | 21 +- waypoint_control/twin_paper/sn-basic.bst | 1818 +++++++++++++++++ 3 files changed, 1839 insertions(+), 2 deletions(-) create mode 100644 waypoint_control/twin_paper/sn-basic.bst diff --git a/waypoint_control/multi_obj_track/object_detection.sh b/waypoint_control/multi_obj_track/object_detection.sh index fe642c2..eba721b 100755 --- a/waypoint_control/multi_obj_track/object_detection.sh +++ b/waypoint_control/multi_obj_track/object_detection.sh @@ -11,6 +11,6 @@ cd $WORK_DIR # 运行 Python 脚本 python -m pcdet.datasets.custom.custom_dataset create_custom_infos tools/cfgs/dataset_configs/custom_dataset.yaml -python tools/test.py --cfg_file output/cfgs/custom_models/pv_rcnn/default/pv_rcnn.yaml --ckpt latest_model.pth +python tools/test.py --cfg_file output/cfgs/custom_models/pv_rcnn/default/pv_rcnn/default/pv_rcnn.yaml --ckpt output/cfgs/custom_models/pv_rcnn/default/pv_rcnn/default/ckpt/latest_model.pth echo "========== 完成自动化目标检测 ==========" \ No newline at end of file diff --git a/waypoint_control/twin_paper/main.tex b/waypoint_control/twin_paper/main.tex index 3a74443..9625b7e 100644 --- a/waypoint_control/twin_paper/main.tex +++ b/waypoint_control/twin_paper/main.tex @@ -152,6 +152,25 @@ \section{Results}\label{sec2} \subsection{Twin system architecture} % 介绍我们这个孪生系统的组成结构,系统运行流程,侧重点在于系统的整体布局,突出整体性 % 介绍系统的运行流程 +Our system is an online digital twin system for urban transportation scenarios, primarily consisting of multimodal data collection endpoints and a centralized server system. + +On the multimodal data collection side, LiDAR and multi-angle camera arrays have been deployed at every major intersection to capture comprehensive data. +The raw point cloud and image data collected are massive in size and contain a large amount of redundant information; transmitting them directly to the central server would place a significant burden on the network. +To alleviate this pressure, we adopted an edge computing approach to perform preliminary data processing at the collection end. +We perform object detection and feature extraction on the raw data to obtain 3D point cloud bounding boxes, 2D image bounding boxes, and feature values, and then transmit this information to the central server via V2X. + +The central server is deployed at the location with the lowest network latency. +After receiving the data, the central server first performs multi-object tracking at a single intersection to obtain the trajectories of people and vehicles at that intersection. +Then, using a cross-intersection feature-based matching algorithm, the trajectories of the same object at adjacent intersections are correlated to obtain cross-intersection correlated vehicle and pedestrian trajectories. +Trajectories that span intersections are tracked within known areas, but there are still unknown areas between adjacent intersections across the entire city. +Therefore, we conducted trajectory prediction for its unknown area based on its driving pattern within the intersection, and obtained its predicted trajectory between intersections. +We then used the vehicle’s driving patterns within the intersection to predict its trajectory in the unknown areas, thereby obtaining its predicted trajectory between intersections. +Insert the predicted trajectory into the cross-intersection correlated trajectory to obtain the complete twin trajectory. +Finally, the twin trajectories are recreated in the corresponding virtual world to achieve a digital twin. +When reproducing a trajectory, we only need to ensure that the target appears at the correct position at the correct time, so we used the simplest control method. +The target starts at the current frame and ends at the next frame; the system calculates its speed and direction, and then moves the target to the position in the next frame using that speed and direction. + +% fig 系统整体构架图 \subsection{End-to-end data transmission} @@ -161,7 +180,7 @@ \subsection{End-to-end data transmission} \subsection{Generation of twin trajectories} % 介绍我们系统是如何生成孪生轨迹的,轨迹的生成方法 -% 分单路口轨迹与多路口轨迹 +% 分单路口轨迹与多路口轨迹,未知区域轨迹预测 % 说明用到的公式,模型,以及其在孪生系统中的作用,突出系统的创新性 diff --git a/waypoint_control/twin_paper/sn-basic.bst b/waypoint_control/twin_paper/sn-basic.bst new file mode 100644 index 0000000..1eea5a2 --- /dev/null +++ b/waypoint_control/twin_paper/sn-basic.bst @@ -0,0 +1,1818 @@ +%% +%% This is file `sn-basic.bst', +%% generated with the docstrip utility. +%% +%% The original source files were: merlin.mbs +%% ---------------------------------------- +%% +%%**************************************************************************************%% +%% %% +%% For Springer medical, life sciences, chemistry, geology, engineering and %% +%% computer science publications. %% +%% For use with the natbib package (see below). Default is author-year citations. %% +%% When citations are numbered, please use \documentclass[sn-basic, Numbered]{sn-jnl}. %% +%% %% +%% Per default the reference list entries are alphabetized. %% +%% If the entries in the list should be numbered consecutively comment out the %% +%% two lines including SORT (in uppercase). %% +%%**************************************************************************************%% + % This file can be redistributed and/or modified under the terms + % of the LaTeX Project Public License Distributed from CTAN + % archives in directory macros/latex/base/lppl.txt; either + % version 1 of the License, or any later version. + % + % This bibliography style file is intended for texts in ENGLISH + % This is an author-year citation style bibliography. As such, it is + % non-standard LaTeX, and requires a special package file to function properly. + % Such a package is natbib.sty by Patrick W. Daly + % The form of the \bibitem entries is + % \bibitem[Jones et al.(1990)]{key}... + % \bibitem[Jones et al.(1990)Jones, Baker, and Smith]{key}... + % The essential feature is that the label (the part in brackets) consists + % of the author names, as they should appear in the citation, with the year + % in parentheses following. There must be no space before the opening + % parenthesis! + % With natbib v5.3, a full list of authors may also follow the year. + % In natbib.sty, it is possible to define the type of enclosures that is + % really wanted (brackets or parentheses), but in either case, there must + % be parentheses in the label. + % The \cite command functions as follows: + % \citet{key} ==>> Jones et al. (1990) + % \citet*{key} ==>> Jones, Baker, and Smith (1990) + % \citep{key} ==>> (Jones et al. 1990) + % \citep*{key} ==>> (Jones, Baker, and Smith 1990) + % \citep[chap. 2]{key} ==>> (Jones et al. 1990, chap. 2) + % \citep[e.g.][]{key} ==>> (e.g. Jones et al. 1990) + % \citep[e.g.][p. 32]{key} ==>> (e.g. Jones et al., p. 32) + % \citeauthor{key} ==>> Jones et al. + % \citeauthor*{key} ==>> Jones, Baker, and Smith + % \citeyear{key} ==>> 1990 + %--------------------------------------------------------------------- + +FUNCTION {identify.basic.version} +{ % Put identifying string in the .blg file + "sn-basic.bst" + " [2024/07/19 v1.1 bibliography style]" + * top$ +} + +ENTRY + { address + archive + author + booktitle + chapter + doi + edition + editor + eid + eprint + howpublished + institution + journal + key + keywords + month + note + number + organization + pages + publisher + school + series + title + type + url + volume + year + archivePrefix + primaryClass + adsurl + adsnote + version + } + {} + { label extra.label sort.label short.list } +INTEGERS { output.state before.all mid.sentence after.sentence after.block } +FUNCTION {init.state.consts} +{ #0 'before.all := + #1 'mid.sentence := + #2 'after.sentence := + #3 'after.block := +} +STRINGS { s t} +FUNCTION {output.nonnull} +{ 's := + output.state mid.sentence = + { ", " * write$ } + { output.state after.block = + { add.period$ write$ + newline$ + "\newblock " write$ + } + { output.state before.all = + 'write$ + { add.period$ " " * write$ } + if$ + } + if$ + mid.sentence 'output.state := + } + if$ + s +} +FUNCTION {output} +{ duplicate$ empty$ + 'pop$ + 'output.nonnull + if$ +} +FUNCTION {output.check} +{ 't := + duplicate$ empty$ + { pop$ "empty " t * " in " * cite$ * warning$ } + 'output.nonnull + if$ +} +FUNCTION {fin.entry} +{ duplicate$ empty$ + 'pop$ + 'write$ + if$ + newline$ +} + +FUNCTION {new.block} +{ output.state before.all = + 'skip$ + { after.block 'output.state := } + if$ +} +FUNCTION {new.sentence} +{ output.state after.block = + 'skip$ + { output.state before.all = + 'skip$ + { after.sentence 'output.state := } + if$ + } + if$ +} +FUNCTION {add.blank} +{ " " * before.all 'output.state := +} + +FUNCTION {no.blank.or.punct} +{ "\hspace{0pt}" * before.all 'output.state := +} + +FUNCTION {date.block} +{ + add.blank +} + +FUNCTION {not} +{ { #0 } + { #1 } + if$ +} +FUNCTION {and} +{ 'skip$ + { pop$ #0 } + if$ +} +FUNCTION {or} +{ { pop$ #1 } + 'skip$ + if$ +} +STRINGS {z} +FUNCTION {remove.dots} +{ 'z := + "" + { z empty$ not } + { z #1 #1 substring$ + z #2 global.max$ substring$ 'z := + duplicate$ "." = 'pop$ + { * } + if$ + } + while$ +} +FUNCTION {new.block.checkb} +{ empty$ + swap$ empty$ + and + 'skip$ + 'new.block + if$ +} +FUNCTION {field.or.null} +{ duplicate$ empty$ + { pop$ "" } + 'skip$ + if$ +} +FUNCTION {emphasize} +{ skip$ } +FUNCTION {tie.or.space.prefix} +{ duplicate$ text.length$ #3 < + { "~" } + { " " } + if$ + swap$ +} + +FUNCTION {capitalize} +{ "u" change.case$ "t" change.case$ } + +FUNCTION {space.word} +{ " " swap$ * " " * } + % Here are the language-specific definitions for explicit words. + % Each function has a name bbl.xxx where xxx is the English word. + % The language selected here is ENGLISH +FUNCTION {bbl.and} +{ "and"} + +FUNCTION {bbl.etal} +{ "et~al." } + +FUNCTION {refbbl.etal} +{ "et~al" } + +FUNCTION {bbl.editors} +{ "eds" } + +FUNCTION {bbl.editor} +{ "ed" } + +FUNCTION {bbl.edby} +{ "edited by" } + +FUNCTION {bbl.edition} +{ "edn." } + +FUNCTION {bbl.volume} +{ "vol" } + +FUNCTION {bbl.of} +{ "of" } + +FUNCTION {bbl.number} +{ "no." } + +FUNCTION {bbl.nr} +{ "no." } + +FUNCTION {bbl.in} +{ "in" } + +FUNCTION {bbl.pages} +{ "pp" } + +FUNCTION {bbl.incoll.pages} +{ "p" } + +FUNCTION {bbl.page} +{ "p" } + +FUNCTION {bbl.chapter} +{ "chap" } + +FUNCTION {bbl.techrep} +{ "Tech. Rep." } + +FUNCTION {bbl.mthesis} +{ "Master's thesis" } + +FUNCTION {bbl.phdthesis} +{ "PhD thesis" } + +FUNCTION {bbl.first} +{ "1st" } + +FUNCTION {bbl.second} +{ "2nd" } + +FUNCTION {bbl.third} +{ "3rd" } + +FUNCTION {bbl.fourth} +{ "4th" } + +FUNCTION {bbl.fifth} +{ "5th" } + +FUNCTION {bbl.st} +{ "st" } + +FUNCTION {bbl.nd} +{ "nd" } + +FUNCTION {bbl.rd} +{ "rd" } + +FUNCTION {bbl.th} +{ "th" } + +MACRO {jan} {"Jan."} + +MACRO {feb} {"Feb."} + +MACRO {mar} {"Mar."} + +MACRO {apr} {"Apr."} + +MACRO {may} {"May"} + +MACRO {jun} {"Jun."} + +MACRO {jul} {"Jul."} + +MACRO {aug} {"Aug."} + +MACRO {sep} {"Sep."} + +MACRO {oct} {"Oct."} + +MACRO {nov} {"Nov."} + +MACRO {dec} {"Dec."} + +FUNCTION {eng.ord} +{ duplicate$ "1" swap$ * + #-2 #1 substring$ "1" = + { bbl.th * } + { duplicate$ #-1 #1 substring$ + duplicate$ "1" = + { pop$ bbl.st * } + { duplicate$ "2" = + { pop$ bbl.nd * } + { "3" = + { bbl.rd * } + { bbl.th * } + if$ + } + if$ + } + if$ + } + if$ +} + +MACRO {acmcs} {"ACM Comput Surv"} + +MACRO {acta} {"Acta Inf"} + +MACRO {cacm} {"Commun ACM"} + +MACRO {ibmjrd} {"IBM~J~Res Dev"} + +MACRO {ibmsj} {"IBM Syst~J"} + +MACRO {ieeese} {"IEEE Trans Softw Eng"} + +MACRO {ieeetc} {"IEEE Trans Comput"} + +MACRO {ieeetcad} + {"IEEE Trans Comput Aid Des"} + +MACRO {ipl} {"Inf Process Lett"} + +MACRO {jacm} {"J~ACM"} + +MACRO {jcss} {"J~Comput Syst Sci"} + +MACRO {scp} {"Sci Comput Program"} + +MACRO {sicomp} {"SIAM J~Comput"} + +MACRO {tocs} {"ACM Trans Comput Syst"} + +MACRO {tods} {"ACM Trans Database Syst"} + +MACRO {tog} {"ACM Trans Graphic"} + +MACRO {toms} {"ACM Trans Math Softw"} + +MACRO {toois} {"ACM Trans Office Inf Syst"} + +MACRO {toplas} {"ACM Trans Program Lang Syst"} + +MACRO {tcs} {"Theor Comput Sci"} + +FUNCTION {bibinfo.check} +{ swap$ + duplicate$ missing$ + { + pop$ pop$ + "" + } + { duplicate$ empty$ + { + swap$ pop$ + } + { swap$ + pop$ + } + if$ + } + if$ +} +FUNCTION {bibinfo.warn} +{ swap$ + duplicate$ missing$ + { + swap$ "missing " swap$ * " in " * cite$ * warning$ pop$ + "" + } + { duplicate$ empty$ + { + swap$ "empty " swap$ * " in " * cite$ * warning$ + } + { swap$ + pop$ + } + if$ + } + if$ +} + +%%Commented on 17th July 2024 +%FUNCTION {format.eprint} +%{ eprint duplicate$ empty$ +% 'skip$ +% { "\eprint" +% archive empty$ +% 'skip$ +% { "[" * archive * "]" * } +% if$ +% "{" * swap$ * "}" * +% } +% if$ +%} + +FUNCTION {format.archive} +{ + archivePrefix empty$ + { "" } + { archivePrefix ":" *} + if$ +} + +%%Updated on 17th July 2024 +FUNCTION {format.eprint} +{ eprint empty$ + { ""} + { archive empty$ + {"{\href{https://arxiv.org/abs/" eprint * "}" * + "{{" * format.archive * eprint * "}}}" *} + {"{\href{https://arxiv.org/abs/" eprint * "}" * + "{{" * format.archive * eprint * "}}}" *} + if$ + } + if$ +} + + + +FUNCTION {format.primaryClass} +{ + primaryClass empty$ + { "" } + { "{[" primaryClass * "]}" *} + if$ +} + +FUNCTION {format.soft.eprint} +{ eprint empty$ + { ""} + { archive empty$ + {"{\href{https://arxiv.org/abs/" eprint * "}" * + "{{" * format.archive * eprint * "}}}" *} + {"{\href{https://arxiv.org/abs/" eprint * "}" * + "{{" * format.archive * eprint * "}}}" *} + if$ + } + if$ +} + +FUNCTION {format.softmisc.archive} +{ + archivePrefix empty$ + { "" } + { "" } + if$ +} + +FUNCTION {format.softmisc.eprint} +{ eprint empty$ + { ""} + { archive empty$ + {"{\href{https://ascl.net/" format.softmisc.archive * eprint * "}" * + "{{" * format.archive * eprint * "}}}" *} + {"{\href{https://ascl.net/" format.softmisc.archive * eprint * "}" * + "{{" * format.archive * eprint * "}}}" *} + if$ + } + if$ +} + +FUNCTION {format.url} +{ url empty$ + { "" } + { "\urlprefix\url{" url * "}" * } + if$ +} + +STRINGS { bibinfo} +INTEGERS { nameptr namesleft numnames } + +FUNCTION {format.names} +{ 'bibinfo := + duplicate$ empty$ 'skip$ { + 's := + "" 't := + #1 'nameptr := + s num.names$ 'numnames := + numnames 'namesleft := + { namesleft #0 > } + { s nameptr + "{vv~}{ll}{ f{}}{jj}" + format.name$ 't := + nameptr #1 > + { + nameptr #3 + #1 + = + numnames #3 + > and + { "others" 't := + #1 'namesleft := } + 'skip$ + if$ + namesleft #1 > + { ", " * t * } + { + "," * + s nameptr "{ll}" format.name$ duplicate$ "others" = + { 't := } + { pop$ } + if$ + t "others" = + { + " " * refbbl.etal * + } + { " " * t * } + if$ + } + if$ + } + 't + if$ + nameptr #1 + 'nameptr := + namesleft #1 - 'namesleft := + } + while$ + } if$ +} + +FUNCTION {format.names.ed} +{ + format.names +} +FUNCTION {format.key} +{ empty$ + { key field.or.null } + { "" } + if$ +} + +FUNCTION {format.authors} +{ author "author" format.names +} +FUNCTION {get.bbl.editor} +{ editor num.names$ #1 > 'bbl.editors 'bbl.editor if$ } + +FUNCTION {format.editors} +{ editor "editor" format.names duplicate$ empty$ 'skip$ + { + " " * + get.bbl.editor + "(" swap$ * ")" * + * + } + if$ +} +FUNCTION {format.doi} +{ doi "doi" bibinfo.check + duplicate$ empty$ 'skip$ + { + "\doi{" swap$ * "}" * + } + if$ +} +FUNCTION {format.note} +{ + note empty$ + { "" } + { note #1 #1 substring$ + duplicate$ "{" = + 'skip$ + { output.state mid.sentence = + { "l" } + { "u" } + if$ + change.case$ + } + if$ + note #2 global.max$ substring$ * "note" bibinfo.check + } + if$ +} + +FUNCTION {format.title} +{ title + duplicate$ empty$ 'skip$ + { "t" change.case$ } + if$ + "title" bibinfo.check +} +FUNCTION {format.full.names} +{'s := + "" 't := + #1 'nameptr := + s num.names$ 'numnames := + numnames 'namesleft := + { namesleft #0 > } + { s nameptr + "{vv~}{ll}" format.name$ + 't := + nameptr #1 > + { + namesleft #1 > + { ", " * t * } + { + s nameptr "{ll}" format.name$ duplicate$ "others" = + { 't := } + { pop$ } + if$ + t "others" = + { + " " * bbl.etal * + } + { + numnames #2 > + { "," * } + 'skip$ + if$ + bbl.and + space.word * t * + } + if$ + } + if$ + } + 't + if$ + nameptr #1 + 'nameptr := + namesleft #1 - 'namesleft := + } + while$ +} + +FUNCTION {author.editor.key.full} +{ author empty$ + { editor empty$ + { key empty$ + { cite$ #1 #3 substring$ } + 'key + if$ + } + { editor format.full.names } + if$ + } + { author format.full.names } + if$ +} + +FUNCTION {author.key.full} +{ author empty$ + { key empty$ + { cite$ #1 #3 substring$ } + 'key + if$ + } + { author format.full.names } + if$ +} + +FUNCTION {editor.key.full} +{ editor empty$ + { key empty$ + { cite$ #1 #3 substring$ } + 'key + if$ + } + { editor format.full.names } + if$ +} + +FUNCTION {make.full.names} +{ type$ "book" = + type$ "inbook" = + or + 'author.editor.key.full + { type$ "proceedings" = + 'editor.key.full + 'author.key.full + if$ + } + if$ +} + +FUNCTION {output.bibitem} +{ newline$ + "\bibitem[{" write$ + label write$ + ")" make.full.names duplicate$ short.list = + { pop$ } + { * } + if$ + "}]{" * write$ + cite$ write$ + "}" write$ + newline$ + "" + before.all 'output.state := +} + +FUNCTION {add.period} +{ duplicate$ empty$ + 'skip$ + { "." * add.blank } + if$ +} + +FUNCTION {if.digit} +{ duplicate$ "0" = + swap$ duplicate$ "1" = + swap$ duplicate$ "2" = + swap$ duplicate$ "3" = + swap$ duplicate$ "4" = + swap$ duplicate$ "5" = + swap$ duplicate$ "6" = + swap$ duplicate$ "7" = + swap$ duplicate$ "8" = + swap$ "9" = or or or or or or or or or +} +FUNCTION {n.separate} +{ 't := + "" + #0 'numnames := + { t empty$ not } + { t #-1 #1 substring$ if.digit + { numnames #1 + 'numnames := } + { #0 'numnames := } + if$ + t #-1 #1 substring$ swap$ * + t #-2 global.max$ substring$ 't := + numnames #5 = + { duplicate$ #1 #2 substring$ swap$ + #3 global.max$ substring$ + "," swap$ * * + } + 'skip$ + if$ + } + while$ +} +FUNCTION {n.dashify} +{ +%% n.separate + 't := + "" + { t empty$ not } + { t #1 #1 substring$ "-" = + { t #1 #2 substring$ "--" = not + { "--" * + t #2 global.max$ substring$ 't := + } + { { t #1 #1 substring$ "-" = } + { "-" * + t #2 global.max$ substring$ 't := + } + while$ + } + if$ + } + { t #1 #1 substring$ * + t #2 global.max$ substring$ 't := + } + if$ + } + while$ +} + +FUNCTION {word.in} +{ bbl.in capitalize + ":" * + " " * } + +FUNCTION {format.date} +{ year "year" bibinfo.check duplicate$ empty$ + { + "empty year in " cite$ * "; set to ????" * warning$ + pop$ "????" + } + 'skip$ + if$ + extra.label * + before.all 'output.state := + " (" swap$ * ")" * +} +FUNCTION {format.btitle} +{ title "title" bibinfo.check + duplicate$ empty$ 'skip$ + { + } + if$ +} +FUNCTION {either.or.check} +{ empty$ + 'pop$ + { "can't use both " swap$ * " fields in " * cite$ * warning$ } + if$ +} +FUNCTION {format.bvolume} +{ volume empty$ + { "" } + { bbl.volume volume tie.or.space.prefix + "volume" bibinfo.check * * + series "series" bibinfo.check + duplicate$ empty$ 'pop$ + { emphasize ", " * swap$ * } + if$ + "volume and number" number either.or.check + } + if$ +} +FUNCTION {format.number.series} +{ volume empty$ + { number empty$ + { series field.or.null } + { series empty$ + { number "number" bibinfo.check } + { output.state mid.sentence = + { bbl.number } + { bbl.number capitalize } + if$ + number tie.or.space.prefix "number" bibinfo.check * * + bbl.in space.word * + series "series" bibinfo.check * + } + if$ + } + if$ + } + { "" } + if$ +} +FUNCTION {is.num} +{ chr.to.int$ + duplicate$ "0" chr.to.int$ < not + swap$ "9" chr.to.int$ > not and +} + +FUNCTION {extract.num} +{ duplicate$ 't := + "" 's := + { t empty$ not } + { t #1 #1 substring$ + t #2 global.max$ substring$ 't := + duplicate$ is.num + { s swap$ * 's := } + { pop$ "" 't := } + if$ + } + while$ + s empty$ + 'skip$ + { pop$ s } + if$ +} + +FUNCTION {convert.edition} +{ extract.num "l" change.case$ 's := + s "first" = s "1" = or + { bbl.first 't := } + { s "second" = s "2" = or + { bbl.second 't := } + { s "third" = s "3" = or + { bbl.third 't := } + { s "fourth" = s "4" = or + { bbl.fourth 't := } + { s "fifth" = s "5" = or + { bbl.fifth 't := } + { s #1 #1 substring$ is.num + { s eng.ord 't := } + { edition 't := } + if$ + } + if$ + } + if$ + } + if$ + } + if$ + } + if$ + t +} + +FUNCTION {format.edition} +{ edition duplicate$ empty$ 'skip$ + { + convert.edition + output.state mid.sentence = + { "l" } + { "t" } + if$ change.case$ + "edition" bibinfo.check + " " * bbl.edition * + } + if$ +} +INTEGERS { multiresult } +FUNCTION {multi.page.check} +{ 't := + #0 'multiresult := + { multiresult not + t empty$ not + and + } + { t #1 #1 substring$ + duplicate$ "-" = + swap$ duplicate$ "," = + swap$ "+" = + or or + { #1 'multiresult := } + { t #2 global.max$ substring$ 't := } + if$ + } + while$ + multiresult +} +FUNCTION {format.pages} +{ pages duplicate$ empty$ 'skip$ + { duplicate$ multi.page.check + { + bbl.pages swap$ + n.dashify + } + { + bbl.page swap$ + } + if$ + tie.or.space.prefix + "pages" bibinfo.check + * * + } + if$ +} +FUNCTION {format.journal.pages} +{ pages duplicate$ empty$ 'pop$ + { swap$ duplicate$ empty$ + { pop$ pop$ format.pages } + { + ":" * + swap$ + n.dashify + "pages" bibinfo.check + * + } + if$ + } + if$ +} +FUNCTION {format.journal.eid} +{ eid "eid" bibinfo.check + duplicate$ empty$ 'pop$ + { swap$ duplicate$ empty$ 'skip$ + { + ":" * + } + if$ + swap$ * + } + if$ +} + +FUNCTION {format.incoll.pages} +{ pages duplicate$ empty$ 'skip$ + { duplicate$ multi.page.check + { + bbl.incoll.pages swap$ + n.dashify + } + { + bbl.page swap$ + } + if$ + tie.or.space.prefix + "pages" bibinfo.check + * * + } + if$ +} + +FUNCTION {format.incoll.chapter.pages} +{ chapter empty$ + 'format.incoll.pages + { type empty$ + { bbl.chapter } + { type "l" change.case$ + "type" bibinfo.check + } + if$ + chapter tie.or.space.prefix + "chapter" bibinfo.check + * * + pages empty$ + 'skip$ + { ", " * format.incoll.pages * } + if$ + } + if$ +} + +FUNCTION {format.vol.num.pages} +{ volume field.or.null + duplicate$ empty$ 'skip$ + { + "volume" bibinfo.check + } + if$ + number "number" bibinfo.check duplicate$ empty$ 'skip$ + { + swap$ duplicate$ empty$ + { "there's a number but no volume in " cite$ * warning$ } + 'skip$ + if$ + swap$ + "(" swap$ * ")" * + } + if$ * + eid empty$ + { format.journal.pages } + { format.journal.eid } + if$ +} + +FUNCTION {format.chapter.pages} +{ chapter empty$ + 'format.pages + { type empty$ + { bbl.chapter } + { type "l" change.case$ + "type" bibinfo.check + } + if$ + chapter tie.or.space.prefix + "chapter" bibinfo.check + * * + pages empty$ + 'skip$ + { ", " * format.pages * } + if$ + } + if$ +} + +FUNCTION {format.booktitle} +{ + booktitle "booktitle" bibinfo.check +} +FUNCTION {format.in.ed.booktitle} +{ format.booktitle duplicate$ empty$ 'skip$ + { + editor "editor" format.names.ed duplicate$ empty$ 'pop$ + { + " " * + get.bbl.editor + "(" swap$ * ") " * + * swap$ + * } + if$ + word.in swap$ * + } + if$ +} +FUNCTION {format.thesis.type} +{ type duplicate$ empty$ + 'pop$ + { swap$ pop$ + "t" change.case$ "type" bibinfo.check + } + if$ +} +FUNCTION {format.tr.number} +{ number "number" bibinfo.check + type duplicate$ empty$ + { pop$ bbl.techrep } + 'skip$ + if$ + "type" bibinfo.check + swap$ duplicate$ empty$ + { pop$ "t" change.case$ } + { tie.or.space.prefix * * } + if$ +} +FUNCTION {format.article.crossref} +{ + word.in + " \cite{" * crossref * "}" * +} +FUNCTION {format.book.crossref} +{ volume duplicate$ empty$ + { "empty volume in " cite$ * "'s crossref of " * crossref * warning$ + pop$ word.in + } + { bbl.volume + capitalize + swap$ tie.or.space.prefix "volume" bibinfo.check * * bbl.of space.word * + } + if$ + " \cite{" * crossref * "}" * +} +FUNCTION {format.incoll.inproc.crossref} +{ + word.in + " \cite{" * crossref * "}" * +} +FUNCTION {format.org.or.pub} +{ 't := + "" + address empty$ t empty$ and + 'skip$ + { + t empty$ + { address "address" bibinfo.check * + } + { t * + address empty$ + 'skip$ + { ", " * address "address" bibinfo.check * } + if$ + } + if$ + } + if$ +} +FUNCTION {format.publisher.address} +{ publisher "publisher" bibinfo.warn format.org.or.pub +} + +FUNCTION {format.organization.address} +{ organization "organization" bibinfo.check format.org.or.pub +} + +FUNCTION {article} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title "title" output.check + new.sentence + crossref missing$ + { + journal + remove.dots + "journal" bibinfo.check + "journal" output.check + add.blank + format.vol.num.pages output + } + { format.article.crossref output.nonnull + format.pages output + } + if$ + new.sentence + format.doi output + format.url output + format.note output + format.soft.eprint output + add.blank + format.primaryClass output + fin.entry +} + +FUNCTION {book} +{ output.bibitem + author empty$ + { format.editors "author and editor" output.check + editor format.key output + add.blank + } + { format.authors output.nonnull + crossref missing$ + { "author and editor" editor either.or.check } + 'skip$ + if$ + } + if$ + format.date "year" output.check + date.block + format.btitle "title" output.check + crossref missing$ + { format.bvolume output + format.edition output + new.sentence + format.number.series output + format.publisher.address output + } + { + new.sentence + format.book.crossref output.nonnull + } + if$ + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {booklet} +{ output.bibitem + format.authors output + author format.key output + format.date "year" output.check + date.block + format.title "title" output.check + new.sentence + howpublished "howpublished" bibinfo.check output + address "address" bibinfo.check output + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {inbook} +{ output.bibitem + author empty$ + { format.editors "author and editor" output.check + editor format.key output + } + { format.authors output.nonnull + crossref missing$ + { "author and editor" editor either.or.check } + 'skip$ + if$ + } + if$ + format.date "year" output.check + date.block + format.btitle "title" output.check + crossref missing$ + { + format.bvolume output + format.edition output + format.publisher.address output + format.chapter.pages "chapter and pages" output.check + new.sentence + format.number.series output + } + { + format.chapter.pages "chapter and pages" output.check + new.sentence + format.book.crossref output.nonnull + } + if$ + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {incollection} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title "title" output.check + new.sentence + crossref missing$ + { format.in.ed.booktitle "booktitle" output.check + format.bvolume output + format.edition output + new.sentence + format.number.series output + format.publisher.address output + format.incoll.chapter.pages output + } + { format.incoll.inproc.crossref output.nonnull + format.incoll.chapter.pages output + } + if$ + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} +FUNCTION {inproceedings} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title "title" output.check + new.sentence + crossref missing$ + { format.in.ed.booktitle "booktitle" output.check + publisher empty$ + { format.organization.address output } + { organization "organization" bibinfo.check output + format.bvolume output + new.sentence + format.publisher.address output + } + if$ + format.number.series output + format.pages output + } + { format.incoll.inproc.crossref output.nonnull + format.pages output + } + if$ + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} +FUNCTION {conference} { inproceedings } +FUNCTION {manual} +{ output.bibitem + format.authors output + author format.key output + format.date "year" output.check + date.block + format.btitle "title" output.check + new.sentence + organization "organization" bibinfo.check output + address "address" bibinfo.check output + format.edition output + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {mastersthesis} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title + "title" output.check + new.sentence + bbl.mthesis format.thesis.type output.nonnull + school "school" bibinfo.warn output + address "address" bibinfo.check output + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {misc} +{ output.bibitem + format.authors output + author format.key output + format.date "year" output.check + date.block + format.title output + new.sentence + howpublished "howpublished" bibinfo.check output + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {insert.comma} +{ duplicate$ empty$ + 'skip$ + { ", " * add.blank } + if$ +} + +FUNCTION {softmisc} +{ output.bibitem + format.authors output + author format.key output + format.date "year" output.check + date.block + format.title output + insert.comma + version output + new.sentence + howpublished "howpublished" bibinfo.check output + format.doi output + format.url output + format.note output + format.softmisc.eprint output + fin.entry +} + +FUNCTION {phdthesis} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title + "title" output.check + new.sentence + bbl.phdthesis format.thesis.type output.nonnull + school "school" bibinfo.warn output + address "address" bibinfo.check output + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {proceedings} +{ output.bibitem + format.editors output + editor format.key output + format.date "year" output.check + date.block + format.btitle "title" output.check + format.bvolume output + format.number.series output + publisher empty$ + { format.organization.address output } + { organization "organization" bibinfo.check output + format.publisher.address output + } + if$ + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {techreport} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title + "title" output.check + new.sentence + format.tr.number output.nonnull + institution "institution" bibinfo.warn output + address "address" bibinfo.check output + format.doi output + format.url output + format.note output + format.eprint output + fin.entry +} + +FUNCTION {unpublished} +{ output.bibitem + format.authors "author" output.check + author format.key output + format.date "year" output.check + date.block + format.title "title" output.check + format.doi output + format.url output + format.note "note" output.check + format.eprint output + fin.entry +} + +FUNCTION {default.type} { misc } +READ + +EXECUTE {identify.basic.version} + +FUNCTION {sortify} +{ purify$ + "l" change.case$ +} +INTEGERS { len } +FUNCTION {chop.word} +{ 's := + 'len := + s #1 len substring$ = + { s len #1 + global.max$ substring$ } + 's + if$ +} +FUNCTION {format.lab.names} +{ 's := + "" 't := + s #1 "{vv~}{ll}" format.name$ + s num.names$ duplicate$ + #2 > + { pop$ + " " * bbl.etal * + } + { #2 < + 'skip$ + { s #2 "{ff }{vv }{ll}{ jj}" format.name$ "others" = + { + " " * bbl.etal * + } + { bbl.and space.word * s #2 "{vv~}{ll}" format.name$ + * } + if$ + } + if$ + } + if$ +} + +FUNCTION {author.key.label} +{ author empty$ + { key empty$ + { cite$ #1 #3 substring$ } + 'key + if$ + } + { author format.lab.names } + if$ +} + +FUNCTION {author.editor.key.label} +{ author empty$ + { editor empty$ + { key empty$ + { cite$ #1 #3 substring$ } + 'key + if$ + } + { editor format.lab.names } + if$ + } + { author format.lab.names } + if$ +} + +FUNCTION {editor.key.label} +{ editor empty$ + { key empty$ + { cite$ #1 #3 substring$ } + 'key + if$ + } + { editor format.lab.names } + if$ +} + +FUNCTION {calc.short.authors} +{ type$ "book" = + type$ "inbook" = + or + 'author.editor.key.label + { type$ "proceedings" = + 'editor.key.label + 'author.key.label + if$ + } + if$ + 'short.list := +} + +FUNCTION {calc.label} +{ calc.short.authors + short.list + "(" + * + year duplicate$ empty$ + { pop$ "????" } + 'skip$ + if$ + * + 'label := +} + +FUNCTION {sort.format.names} +{ 's := + #1 'nameptr := + "" + s num.names$ 'numnames := + numnames 'namesleft := + { namesleft #0 > } + { s nameptr + "{ll{ }}{ f{ }}{ jj{ }}" + format.name$ 't := + nameptr #1 > + { + " " * + namesleft #1 = t "others" = and + { "zzzzz" * } + { numnames #2 > nameptr #2 = and + { "zz" * year field.or.null * " " * } + 'skip$ + if$ + t sortify * + } + if$ + } + { t sortify * } + if$ + nameptr #1 + 'nameptr := + namesleft #1 - 'namesleft := + } + while$ +} + +FUNCTION {sort.format.title} +{ 't := + "A " #2 + "An " #3 + "The " #4 t chop.word + chop.word + chop.word + sortify + #1 global.max$ substring$ +} +FUNCTION {author.sort} +{ author empty$ + { key empty$ + { "to sort, need author or key in " cite$ * warning$ + "" + } + { key sortify } + if$ + } + { author sort.format.names } + if$ +} +FUNCTION {author.editor.sort} +{ author empty$ + { editor empty$ + { key empty$ + { "to sort, need author, editor, or key in " cite$ * warning$ + "" + } + { key sortify } + if$ + } + { editor sort.format.names } + if$ + } + { author sort.format.names } + if$ +} +FUNCTION {editor.sort} +{ editor empty$ + { key empty$ + { "to sort, need editor or key in " cite$ * warning$ + "" + } + { key sortify } + if$ + } + { editor sort.format.names } + if$ +} +FUNCTION {presort} +{ calc.label + label sortify + " " + * + type$ "book" = + type$ "inbook" = + or + 'author.editor.sort + { type$ "proceedings" = + 'editor.sort + 'author.sort + if$ + } + if$ + #1 entry.max$ substring$ + 'sort.label := + sort.label + * + " " + * + title field.or.null + sort.format.title + * + #1 entry.max$ substring$ + 'sort.key$ := +} + +ITERATE {presort} +SORT +STRINGS { last.label next.extra } +INTEGERS { last.extra.num number.label } +FUNCTION {initialize.extra.label.stuff} +{ #0 int.to.chr$ 'last.label := + "" 'next.extra := + #0 'last.extra.num := + #0 'number.label := +} +FUNCTION {forward.pass} +{ last.label label = + { last.extra.num #1 + 'last.extra.num := + last.extra.num int.to.chr$ 'extra.label := + } + { "a" chr.to.int$ 'last.extra.num := + "" 'extra.label := + label 'last.label := + } + if$ + number.label #1 + 'number.label := +} +FUNCTION {reverse.pass} +{ next.extra "b" = + { "a" 'extra.label := } + 'skip$ + if$ + extra.label 'next.extra := + extra.label + duplicate$ empty$ + 'skip$ + { "{\natexlab{" swap$ * "}}" * } + if$ + 'extra.label := + label extra.label * 'label := +} +EXECUTE {initialize.extra.label.stuff} +ITERATE {forward.pass} +REVERSE {reverse.pass} +FUNCTION {bib.sort.order} +{ sort.label + " " + * + year field.or.null sortify + * + " " + * + title field.or.null + sort.format.title + * + #1 entry.max$ substring$ + 'sort.key$ := +} +ITERATE {bib.sort.order} +SORT +FUNCTION {begin.bib} +{ preamble$ empty$ + 'skip$ + { preamble$ write$ newline$ } + if$ + "\begin{thebibliography}{" number.label int.to.str$ * "}" * + write$ newline$ + "\providecommand{\natexlab}[1]{#1}" + write$ newline$ + "\providecommand{\url}[1]{{#1}}" + write$ newline$ + "\providecommand{\urlprefix}{URL }" + write$ newline$ + %%"\expandafter\ifx\csname urlstyle\endcsname\relax" + %%write$ newline$ + %%" \providecommand{\doi}[1]{\discretionary{}{}{}https://doi.org/#1}\else" + %%write$ newline$ + %%" \providecommand{\doi}{\discretionary{}{}{}\begingroup \urlstyle{rm}\Url}\fi" + "\providecommand{\doi}[1]{\url{https://doi.org/#1}}" + write$ newline$ + "\providecommand{\eprint}[2][]{\url{#2}}" + write$ newline$ + " \bibcommenthead " + write$ newline$ +} +EXECUTE {begin.bib} +EXECUTE {init.state.consts} +ITERATE {call.type$} +FUNCTION {end.bib} +{ newline$ + "\end{thebibliography}" write$ newline$ +} +EXECUTE {end.bib} + +%% End of file `sn-basic.bst'. + From 911abee6a620f7477849d2a4e9504bac92c61a65 Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Fri, 17 Jul 2026 18:25:35 +0800 Subject: [PATCH 02/10] =?UTF-8?q?=E4=BC=98=E5=8C=96Town01=E8=B7=9F?= =?UTF-8?q?=E8=B8=AA=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Evaluation/demoMultiSensorEvaluation.m | 7 +- .../collect_intersection_camera_lidar.py | 28 ++++++-- .../multi_obj_track/getpersonTrackerConfig.m | 70 +++++++++---------- .../multi_obj_track/getvehicleTrackerConfig.m | 70 +++++++++---------- 4 files changed, 99 insertions(+), 76 deletions(-) diff --git a/waypoint_control/multi_obj_track/Evaluation/demoMultiSensorEvaluation.m b/waypoint_control/multi_obj_track/Evaluation/demoMultiSensorEvaluation.m index 97711f9..1799a5a 100644 --- a/waypoint_control/multi_obj_track/Evaluation/demoMultiSensorEvaluation.m +++ b/waypoint_control/multi_obj_track/Evaluation/demoMultiSensorEvaluation.m @@ -9,7 +9,12 @@ function demoMultiSensorEvaluation(townPath) categories = {'person', 'vehicle'}; % 指定的路口对 - junctionPairs = [1,2; 1,3; 1,4; 1,5; 2,4; 2,5; 3,4; 3,5]; + if strcmp(town, 'Town10HD_Opt') + junctionPairs = [1,2; 1,3; 1,4; 1,5; 2,4; 2,5; 3,4; 3,5]; + elseif strcmp(town, 'Town01') + junctionPairs = [1,4; 1,5; 2,3; 2,4; 2,5; 3,5]; + end + % junctionPairs = [1,2; 1,3; 1,4; 1,5; 2,4; 2,5; 3,4; 3,5]; for c = 1:length(categories) category = categories{c}; diff --git a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py index 4be22bd..e154c8a 100644 --- a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py +++ b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py @@ -574,7 +574,7 @@ def dist(actor): extra_time += duration sensor = sensors["v2x_point"] - pkl_file_path = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/output/cfgs/custom_models/pv_rcnn/default/pv_rcnn/default/eval/epoch_no_number/val/default/result.pkl" + pkl_file_path = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/output/cfgs/custom_models/pv_rcnn/default/pv_rcnn/default/pv_rcnn/default/eval/epoch_no_number/val/default/result.pkl" send_v2x_message_lidar(radar_data, sensor, pkl_file_path, junc, world) # 更新目标检测的文件夹 @@ -834,6 +834,24 @@ def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): num_colors = 12 available_colors = ["255,0,0", "0,255,0", "0,0,255", "255,255,0", "0,255,255", "255,0,255", "128,128,0", "128,0,128", "0,128,128", "255,165,0", "0,255,255", "255,192,203"] + # available_colors = [ + # "255,0,0", # 红 + # "0,255,0", # 绿 + # "0,0,255", # 蓝 + # "255,255,0", # 黄 + # "0,255,255", # 青 + # "255,0,255", # 品红 + # "128,128,0", # 橄榄 + # "128,0,128", # 紫 + # "0,128,128", # 墨绿 + # "255,165,0", # 橙 + # "255,192,203", # 粉红 + # "140,86,75", # 棕 + # "127,127,127", # 灰 + # "148,103,189", # 淡紫 + # "23,190,207", # 亮青 + # "255,215,0" # 金 + # ] # 生成车辆 vehicle_index = 0 @@ -861,7 +879,7 @@ def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): while attempts < num_blueprints: candidate_idx = vehicle_index % num_blueprints candidate_bp = filter_vehicle_blueprints[candidate_idx] - if candidate_bp.has_attribute('color'): + if candidate_bp.has_attribute('color'): # 关键修正点 vehicle_bp = candidate_bp color = available_colors[vehicle_index % num_colors] vehicle_bp.set_attribute('color', color) # 设置颜色 @@ -872,7 +890,7 @@ def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): vehicle_index += 1 attempts += 1 if not found: - # 实在找不到支持颜色的蓝图,使用当前蓝图且不设颜色 + # 实在找不到支持颜色的蓝图,使用当前蓝图且不设颜色(或报错) print("警告:无支持颜色的蓝图可用,使用默认外观") vehicle_bp = filter_vehicle_blueprints[vehicle_index % num_blueprints] vehicle_index += 1 @@ -1427,7 +1445,7 @@ def main(): argparser.add_argument( '-i', '--intersection', metavar='INTERSECTION', - default='road_intersection_1', # 默认路口 + default='road_intersection_5', # 默认路口 help='Name of the intersection within the town (default: road_intersection_1)' ) args = argparser.parse_args() @@ -1725,4 +1743,4 @@ def main(): try: main() except KeyboardInterrupt: - print(' - Exited by user.') + print(' - Exited by user.') \ No newline at end of file diff --git a/waypoint_control/multi_obj_track/getpersonTrackerConfig.m b/waypoint_control/multi_obj_track/getpersonTrackerConfig.m index 8be91cb..46d88f7 100644 --- a/waypoint_control/multi_obj_track/getpersonTrackerConfig.m +++ b/waypoint_control/multi_obj_track/getpersonTrackerConfig.m @@ -5,63 +5,63 @@ case 'Town01' switch juncName case 'test_data_junc1' - params.AssignmentThreshold = [2.97747335296038 14.5524897833759]; + params.AssignmentThreshold = [1.11739583095735 15.8814219908511]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.943321585537362; + params.DetectionProbability = 0.937595606689869; params.MaxNumEvents = 50; - params.ClutterDensity = 0.000866240568336769; - params.NewTargetDensity = 0.000419662491532024; - params.ConfirmationThreshold = 0.901631900495953; - params.DeletionThreshold = 0.291609699644504; - params.DeathRate = 0.670290537394368; + params.ClutterDensity = 8.04249655499483e-06; + params.NewTargetDensity = 1.09325783874401e-05; + params.ConfirmationThreshold = 0.903053308146516; + params.DeletionThreshold = 0.230258687740478; + params.DeathRate = 0.669498250122228; case 'test_data_junc2' % 为路口2设置不同的参数 - params.AssignmentThreshold = [8.23672238875688 15.7654598129533]; + params.AssignmentThreshold = [2.72230123533656 12.5460227030653]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.948756235152492; + params.DetectionProbability = 0.855289312694044; params.MaxNumEvents = 50; - params.ClutterDensity = 0.000936866119851987; - params.NewTargetDensity = 0.00675185082800010; - params.ConfirmationThreshold = 0.943711977295654; - params.DeletionThreshold = 0.589836499677124; - params.DeathRate = 0.553835116154699; + params.ClutterDensity = 0.000659240337500979; + params.NewTargetDensity = 0.000591231580808432; + params.ConfirmationThreshold = 0.940741177165994; + params.DeletionThreshold = 0.297490584524257; + params.DeathRate = 0.538205842603255; case 'test_data_junc3' % 为路口3设置不同的参数 - params.AssignmentThreshold = [8.84091737458980 13.1604487499737]; + params.AssignmentThreshold = [5.87125909919786 13.5728780546997]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.519428154683526; + params.DetectionProbability = 0.873117008982223; params.MaxNumEvents = 50; - params.ClutterDensity = 0.000949850659056131; - params.NewTargetDensity = 1.00824390435394e-05; - params.ConfirmationThreshold = 0.862022498749934; - params.DeletionThreshold = 0.490672384640826; - params.DeathRate = 0.332379789131892; + params.ClutterDensity = 3.07835010027278e-05; + params.NewTargetDensity = 0.000121908770381848; + params.ConfirmationThreshold = 0.930240067544257; + params.DeletionThreshold = 0.341832017924167; + params.DeathRate = 0.324585970621323; case 'test_data_junc4' % 为路口4设置不同的参数 - params.AssignmentThreshold = [3.92851813360968 16.8866840594349]; + params.AssignmentThreshold = [2.69528199852330 16.8774441871042]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.933675352705422; + params.DetectionProbability = 0.944717414070525; params.MaxNumEvents = 50; - params.ClutterDensity = 0.000480307134861586; - params.NewTargetDensity = 0.000843651073425903; - params.ConfirmationThreshold = 0.948494738478409; - params.DeletionThreshold = 0.574931226042544; - params.DeathRate = 0.657485915153470; + params.ClutterDensity = 0.000993755299533813; + params.NewTargetDensity = 0.00133720921785614; + params.ConfirmationThreshold = 0.630242583825310; + params.DeletionThreshold = 0.348875601893628; + params.DeathRate = 0.610313578638342; case 'test_data_junc5' % 为路口5设置不同的参数 - params.AssignmentThreshold = [9.45764148519469 12.0410452947432]; + params.AssignmentThreshold = [3.38369163501663 16.0807146788425]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.939206172741026; + params.DetectionProbability = 0.706273201335009; params.MaxNumEvents = 50; - params.ClutterDensity = 0.000896326081357801; - params.NewTargetDensity = 0.000478085270621507; - params.ConfirmationThreshold = 0.947584565574986; - params.DeletionThreshold = 0.220514737197920; - params.DeathRate = 0.647795707963701; + params.ClutterDensity = 0.000791779468725156; + params.NewTargetDensity = 0.00147091618224815; + params.ConfirmationThreshold = 0.945146473586896; + params.DeletionThreshold = 0.487301452073911; + params.DeathRate = 0.617210877411384; otherwise error('未知的路口名称: %s. 请使用 junc1 到 junc5', juncName); diff --git a/waypoint_control/multi_obj_track/getvehicleTrackerConfig.m b/waypoint_control/multi_obj_track/getvehicleTrackerConfig.m index 55f1100..76d3ad5 100644 --- a/waypoint_control/multi_obj_track/getvehicleTrackerConfig.m +++ b/waypoint_control/multi_obj_track/getvehicleTrackerConfig.m @@ -5,63 +5,63 @@ case 'Town01' switch juncName case 'test_data_junc1' - params.AssignmentThreshold = [2.45388204828801 15.2985175725122]; + params.AssignmentThreshold = [10.5931980769760 20.6109007224522]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.873481221682843; + params.DetectionProbability = 0.746037350223033; params.MaxNumEvents = 50; - params.ClutterDensity = 7.77423905834477e-07; - params.NewTargetDensity = 1.08921011114548e-06; - params.ConfirmationThreshold = 0.864432285931773; - params.DeletionThreshold = 0.392294713170270; - params.DeathRate = 0.366475549361350; + params.ClutterDensity = 1.98707373973405e-08; + params.NewTargetDensity = 1.38935371142489e-07; + params.ConfirmationThreshold = 0.988596149418680; + params.DeletionThreshold = 0.551207894776409; + params.DeathRate = 0.564283920861690; case 'test_data_junc2' % 为路口2设置不同的参数 - params.AssignmentThreshold = [13.5709965483189 17.2460423697133]; + params.AssignmentThreshold = [11.4964922403104 24.7043186375859]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.896613727728421; + params.DetectionProbability = 0.893460614566182; params.MaxNumEvents = 50; - params.ClutterDensity = 7.85736061175583e-08; - params.NewTargetDensity = 1.44429789941436e-07; - params.ConfirmationThreshold = 0.949470277923141; - params.DeletionThreshold = 0.365085821448857; - params.DeathRate = 0.548734743828175; + params.ClutterDensity = 9.95052015370506e-07; + params.NewTargetDensity = 6.31489337354542e-06; + params.ConfirmationThreshold = 0.983604870623160; + params.DeletionThreshold = 0.516303809270453; + params.DeathRate = 0.324570732835291; case 'test_data_junc3' % 为路口3设置不同的参数 - params.AssignmentThreshold = [2.11462850714591 21.1101660634957]; + params.AssignmentThreshold = [3.41774326286932 29.9718149294276]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.893897003235471; + params.DetectionProbability = 0.831434037698859; params.MaxNumEvents = 50; - params.ClutterDensity = 1.08187351723186e-07; - params.NewTargetDensity = 1.71737532836353e-07; - params.ConfirmationThreshold = 0.898604790469875; - params.DeletionThreshold = 0.586087844163779; - params.DeathRate = 0.362059107137417; + params.ClutterDensity = 9.98759128985741e-07; + params.NewTargetDensity = 1.92305080215396e-06; + params.ConfirmationThreshold = 0.866501524619089; + params.DeletionThreshold = 0.515993052659001; + params.DeathRate = 0.374056886065844; case 'test_data_junc4' % 为路口4设置不同的参数 - params.AssignmentThreshold = [2.58887872709735 54.7516789284473]; + params.AssignmentThreshold = [12.0330472887580 35.4772047804730]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.878952056917922; + params.DetectionProbability = 0.850992633252816; params.MaxNumEvents = 50; - params.ClutterDensity = 5.31965176074703e-07; - params.NewTargetDensity = 6.07855459589331e-07; - params.ConfirmationThreshold = 0.983401587201201; - params.DeletionThreshold = 0.428104981045802; - params.DeathRate = 0.302375614069101; + params.ClutterDensity = 5.12654967742099e-08; + params.NewTargetDensity = 1.16335974991717e-07; + params.ConfirmationThreshold = 0.877240524328496; + params.DeletionThreshold = 0.513808561871926; + params.DeathRate = 0.447659502700140; case 'test_data_junc5' % 为路口5设置不同的参数 - params.AssignmentThreshold = [13.9386778508241 22.4121305704114]; + params.AssignmentThreshold = [9.64160061922475 22.9439881814141]; params.MaxNumTracks = 500; - params.DetectionProbability = 0.536682102839997; + params.DetectionProbability = 0.821602914776620; params.MaxNumEvents = 50; - params.ClutterDensity = 3.40153471399495e-07; - params.NewTargetDensity = 1.00338737544349e-07; - params.ConfirmationThreshold = 0.830448044371134; - params.DeletionThreshold = 0.573481117328615; - params.DeathRate = 0.369064811153443; + params.ClutterDensity = 1.26150090915959e-07; + params.NewTargetDensity = 4.72626735460624e-07; + params.ConfirmationThreshold = 0.884787580569779; + params.DeletionThreshold = 0.500236998917005; + params.DeathRate = 0.633968708808360; otherwise error('未知的路口名称: %s. 请使用 junc1 到 junc5', juncName); From 4367b09ab42bd56af6f8113f69b4ba12e4dbe212 Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Fri, 17 Jul 2026 18:28:24 +0800 Subject: [PATCH 03/10] =?UTF-8?q?=E5=88=A0=E9=99=A4=E5=A4=9A=E4=BD=99?= =?UTF-8?q?=E5=AD=97=E7=AC=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../collect_intersection_camera_lidar.py | 23 ++----------------- 1 file changed, 2 insertions(+), 21 deletions(-) diff --git a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py index e154c8a..f804c21 100644 --- a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py +++ b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar.py @@ -834,25 +834,6 @@ def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): num_colors = 12 available_colors = ["255,0,0", "0,255,0", "0,0,255", "255,255,0", "0,255,255", "255,0,255", "128,128,0", "128,0,128", "0,128,128", "255,165,0", "0,255,255", "255,192,203"] - # available_colors = [ - # "255,0,0", # 红 - # "0,255,0", # 绿 - # "0,0,255", # 蓝 - # "255,255,0", # 黄 - # "0,255,255", # 青 - # "255,0,255", # 品红 - # "128,128,0", # 橄榄 - # "128,0,128", # 紫 - # "0,128,128", # 墨绿 - # "255,165,0", # 橙 - # "255,192,203", # 粉红 - # "140,86,75", # 棕 - # "127,127,127", # 灰 - # "148,103,189", # 淡紫 - # "23,190,207", # 亮青 - # "255,215,0" # 金 - # ] - # 生成车辆 vehicle_index = 0 for _ in range(num_vehicles): @@ -879,7 +860,7 @@ def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): while attempts < num_blueprints: candidate_idx = vehicle_index % num_blueprints candidate_bp = filter_vehicle_blueprints[candidate_idx] - if candidate_bp.has_attribute('color'): # 关键修正点 + if candidate_bp.has_attribute('color'): vehicle_bp = candidate_bp color = available_colors[vehicle_index % num_colors] vehicle_bp.set_attribute('color', color) # 设置颜色 @@ -890,7 +871,7 @@ def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): vehicle_index += 1 attempts += 1 if not found: - # 实在找不到支持颜色的蓝图,使用当前蓝图且不设颜色(或报错) + # 实在找不到支持颜色的蓝图,使用当前蓝图且不设颜色 print("警告:无支持颜色的蓝图可用,使用默认外观") vehicle_bp = filter_vehicle_blueprints[vehicle_index % num_blueprints] vehicle_index += 1 From 7cdfae1401b66d63e2cb2303e85882572258b876 Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Tue, 21 Jul 2026 18:16:55 +0800 Subject: [PATCH 04/10] =?UTF-8?q?=E8=AE=BA=E6=96=87results=E9=83=A8?= =?UTF-8?q?=E5=88=86=E6=92=B0=E5=86=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- waypoint_control/twin_paper/main.tex | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/waypoint_control/twin_paper/main.tex b/waypoint_control/twin_paper/main.tex index 9625b7e..8146972 100644 --- a/waypoint_control/twin_paper/main.tex +++ b/waypoint_control/twin_paper/main.tex @@ -160,23 +160,27 @@ \subsection{Twin system architecture} We perform object detection and feature extraction on the raw data to obtain 3D point cloud bounding boxes, 2D image bounding boxes, and feature values, and then transmit this information to the central server via V2X. The central server is deployed at the location with the lowest network latency. -After receiving the data, the central server first performs multi-object tracking at a single intersection to obtain the trajectories of people and vehicles at that intersection. -Then, using a cross-intersection feature-based matching algorithm, the trajectories of the same object at adjacent intersections are correlated to obtain cross-intersection correlated vehicle and pedestrian trajectories. -Trajectories that span intersections are tracked within known areas, but there are still unknown areas between adjacent intersections across the entire city. -Therefore, we conducted trajectory prediction for its unknown area based on its driving pattern within the intersection, and obtained its predicted trajectory between intersections. -We then used the vehicle’s driving patterns within the intersection to predict its trajectory in the unknown areas, thereby obtaining its predicted trajectory between intersections. -Insert the predicted trajectory into the cross-intersection correlated trajectory to obtain the complete twin trajectory. -Finally, the twin trajectories are recreated in the corresponding virtual world to achieve a digital twin. +After receiving the data, the central server performs multi-target tracking at the intersection. +When the target reaches the edge of an intersection, it enters an unknown area between two intersections. +Therefore, we use the current intersection as the starting point and the adjacent intersection as the endpoint for trajectory prediction. +Then, using the target eigenvalues, the predicted trajectory is matched with the trajectory at the destination intersection to obtain the cross-intersection trajectory. +Finally, reconstruct the trajectory across intersections in chronological order to create a digital twin. When reproducing a trajectory, we only need to ensure that the target appears at the correct position at the correct time, so we used the simplest control method. The target starts at the current frame and ends at the next frame; the system calculates its speed and direction, and then moves the target to the position in the next frame using that speed and direction. -% fig 系统整体构架图 \subsection{End-to-end data transmission} % 介绍我们系统的数据是怎样端到端传输的,收集端与服务端的布局是怎样的,如何计算延迟,详细说明”在线“属性 % 本论文的特色,创新点 +We selected five major intersections on each map as data collection points. +At each intersection, one lidar sensor, six cameras, and two V2X communication devices are deployed in the center of the road. +The six cameras—front, rear, front left, rear left, front right, and rear right—cover different areas of the intersection. +We have deployed microcomputers at every intersection to perform preliminary processing of the raw data collected. +We used the PV\_RCNN model to perform object detection on 3D point cloud data, obtaining 3D bounding boxes for vehicles and pedestrians. +Using the YOLOv8 model to perform object detection on image data yields 2D bounding boxes and feature values for vehicles and pedestrians. +Transmit the obtained 3D bounding boxes, 2D bounding boxes, and feature values to the central server via V2X. \subsection{Generation of twin trajectories} % 介绍我们系统是如何生成孪生轨迹的,轨迹的生成方法 From 25caeed224ee38ee90b62ce8859c4a85bae83679 Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Thu, 23 Jul 2026 19:59:29 +0800 Subject: [PATCH 05/10] =?UTF-8?q?=E8=AE=BA=E6=96=87results=E9=83=A8?= =?UTF-8?q?=E5=88=86=E6=92=B0=E5=86=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- waypoint_control/twin_paper/main.tex | 70 +++++++++++++++++----------- 1 file changed, 44 insertions(+), 26 deletions(-) diff --git a/waypoint_control/twin_paper/main.tex b/waypoint_control/twin_paper/main.tex index 8146972..55cba99 100644 --- a/waypoint_control/twin_paper/main.tex +++ b/waypoint_control/twin_paper/main.tex @@ -152,35 +152,53 @@ \section{Results}\label{sec2} \subsection{Twin system architecture} % 介绍我们这个孪生系统的组成结构,系统运行流程,侧重点在于系统的整体布局,突出整体性 % 介绍系统的运行流程 -Our system is an online digital twin system for urban transportation scenarios, primarily consisting of multimodal data collection endpoints and a centralized server system. - -On the multimodal data collection side, LiDAR and multi-angle camera arrays have been deployed at every major intersection to capture comprehensive data. -The raw point cloud and image data collected are massive in size and contain a large amount of redundant information; transmitting them directly to the central server would place a significant burden on the network. -To alleviate this pressure, we adopted an edge computing approach to perform preliminary data processing at the collection end. -We perform object detection and feature extraction on the raw data to obtain 3D point cloud bounding boxes, 2D image bounding boxes, and feature values, and then transmit this information to the central server via V2X. - -The central server is deployed at the location with the lowest network latency. -After receiving the data, the central server performs multi-target tracking at the intersection. -When the target reaches the edge of an intersection, it enters an unknown area between two intersections. -Therefore, we use the current intersection as the starting point and the adjacent intersection as the endpoint for trajectory prediction. -Then, using the target eigenvalues, the predicted trajectory is matched with the trajectory at the destination intersection to obtain the cross-intersection trajectory. -Finally, reconstruct the trajectory across intersections in chronological order to create a digital twin. -When reproducing a trajectory, we only need to ensure that the target appears at the correct position at the correct time, so we used the simplest control method. -The target starts at the current frame and ends at the next frame; the system calculates its speed and direction, and then moves the target to the position in the next frame using that speed and direction. - - - -\subsection{End-to-end data transmission} +This paper describes the design of an online digital twin system for an urban transportation scenario. +The system moves away from the traditional offline batch processing model and adopts a pipeline mechanism based on “real-time data collection and transmission, online inference, and streaming reconstruction.” Its workflow is primarily divided into three stages: data collection and processing, trajectory tracking and prediction, and twin trajectory reconstruction. + +During the data collection and processing phase, to obtain high-precision traffic information, we used LiDAR to capture 3D point cloud data and multi-view cameras to capture image data. +The collected raw point cloud and image data sets are massive and contain a large amount of redundant information, making direct transmission a significant challenge. +To alleviate this pressure, we perform preliminary processing on the data before it is transmitted. +We used the Point-Voxel Region-based Convolutional Neural Network (PV-RCNN) model to perform object detection on 3D point cloud data, obtaining 3D bounding boxes for vehicles and pedestrians. +PV-RCNN is a highly efficient two-stage 3D object detection network that maintains high detection accuracy for small-sized objects, such as pedestrians, while also delivering excellent inference speed. +Using the You Only Look Once version 8 (YOLOv8) model to perform object detection on image data, 2D bounding boxes for vehicles and pedestrians are obtained. +Image detection is located at the front end of the entire system workflow. YOLOv8’s extremely low inference latency ensures that no queuing bottlenecks occur, thereby improving system fluidity and reducing latency. +A 50-layer Residual Network (ResNet-50) model is used to extract features from image data, yielding feature values for vehicles and pedestrians. +ResNet-50 combines the ability to generate semantic representations applicable to both vehicles and pedestrians with fast inference speeds, enabling it to efficiently extract highly discriminative appearance features for heterogeneous targets such as vehicles and pedestrians while ensuring low latency. +To record the latency of data transmission, we used V2X communication devices to transmit bounding box coordinates and feature vectors, thereby ensuring the authenticity of the time attributes. + +During the trajectory tracking and prediction phase, we primarily focused on three tasks: single-intersection trajectory tracking, trajectory prediction in unknown areas, and cross-intersection trajectory correlation. +In single-intersection trajectory tracking, we use the Joint Probabilistic Data Association (JPDA) algorithm for multi-target tracking. +By using a probabilistic soft decision mechanism to weight and update target states, JPDA can effectively address issues such as frequent target occlusion, close-range overlap, and sensor clutter in high-density traffic flows, thereby significantly enhancing the robustness of the association process. +In trajectory prediction for unknown regions, we employ a fifth-degree polynomial trajectory inference method to infer a smooth trajectory for the unknown region based on the target’s current motion state and future boundary conditions. +The five-point polynomial is a classic analytical method that simultaneously satisfies constraints on the initial and final positions, velocity, and acceleration, balancing both the smoothness of the trajectory and computational efficiency. +In the process of correlating cross-intersection trajectories, we use the target’s feature values for matching, correlate trajectories from different intersections, and obtain cross-intersection trajectories. + +During the twin trajectory reproduction phase, we will reproduce the complete twin trajectory obtained in CARLA to create a digital twin. +Trajectories tracked across intersections inevitably contain minute high-frequency noise; if used directly to drive reconstruction, this can cause the target to exhibit non-physical visual oscillations in the digital twin scene. +To this end, this paper employs a trajectory smoothing algorithm to improve the geometric smoothness and kinematic continuity of the trajectory. +To ensure spatiotemporal consistency during trajectory reproduction while reducing the system’s computational overhead, this paper adopts a vector velocity-driven control strategy based on adjacent sampling frames. +The system sets the states of the current frame and the next frame as the starting point and ending point of the motion, respectively. Based on the inter-frame delay, it calculates the target’s vector velocity and orientation, thereby driving the target to move in segmented, uniform linear motion and accurately reproducing the target’s spatiotemporal trajectory. + + +\subsection{System physical layout and transmission latency} % 介绍我们系统的数据是怎样端到端传输的,收集端与服务端的布局是怎样的,如何计算延迟,详细说明”在线“属性 % 本论文的特色,创新点 - +Our system consists of a data collection end and a central server. We selected five major intersections on each map as data collection points. -At each intersection, one lidar sensor, six cameras, and two V2X communication devices are deployed in the center of the road. -The six cameras—front, rear, front left, rear left, front right, and rear right—cover different areas of the intersection. -We have deployed microcomputers at every intersection to perform preliminary processing of the raw data collected. -We used the PV\_RCNN model to perform object detection on 3D point cloud data, obtaining 3D bounding boxes for vehicles and pedestrians. -Using the YOLOv8 model to perform object detection on image data yields 2D bounding boxes and feature values for vehicles and pedestrians. -Transmit the obtained 3D bounding boxes, 2D bounding boxes, and feature values to the central server via V2X. +One lidar sensor, six cameras, and seven V2X communication devices have been deployed at each intersection. +We deployed the LiDAR at the center of the intersection, 1.8 meters above the ground. +The six cameras, centered around the LiDAR unit, are positioned as follows: front, rear, left front, left rear, right front, and right rear. They are all uniformly mounted at a height of 3.6 meters above the ground, spaced 14 meters apart front to back and 8 meters apart left to right. +The seven V2X communication devices are each mounted 0.8 meters above the lidar and the six cameras. +The central server is located at the point closest to all intersections to minimize data transmission latency. +At the same time, deploy one V2X communication device at the central server to receive data transmitted from the data collection endpoints. + +The latency in this system primarily stems from three sources: processing latency at the data collection end, data transmission latency, and processing latency at the central server. +Processing latency at the data collection end primarily stems from the preprocessing of raw data. We mark the moment just before the data enters preprocessing as the start time and the moment preprocessing ends as the end time, then calculate the difference between the two as the processing latency. +To calculate data transmission latency, we use the V2X transmission time at the data collection end as the start time and the V2X reception time at the central server as the end time, and calculate the difference between the two as the transmission latency. +Processing latency on the central server primarily stems from the execution time of the relevant programs, namely the generation and re-creation of trajectories. +We use the time when the data is input as the start time and the time when the target is successfully replicated in the virtual world as the end time, and calculate the difference between the two as the processing delay. +Total delay refers to the time elapsed from when the target appears in front of the sensor until it is successfully detected. + \subsection{Generation of twin trajectories} % 介绍我们系统是如何生成孪生轨迹的,轨迹的生成方法 From 4edf3b0c78bb7f80122746a516c54b6809ae023d Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Wed, 5 Aug 2026 16:23:10 +0800 Subject: [PATCH 06/10] =?UTF-8?q?=E8=AE=BA=E6=96=87results=E9=83=A8?= =?UTF-8?q?=E5=88=86=E6=92=B0=E5=86=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- waypoint_control/multi_obj_track/config.py | 81 +++++++++++++++---- .../multi_obj_track/reID/Utils/DEMO.m | 8 +- .../reID/Utils/linkIdentities.m | 35 +++++--- .../reID/Utils/processSingleJuncTraj.m | 48 ++++++----- waypoint_control/twin_paper/main.tex | 43 ++++++++++ 5 files changed, 166 insertions(+), 49 deletions(-) diff --git a/waypoint_control/multi_obj_track/config.py b/waypoint_control/multi_obj_track/config.py index 9102e75..4282788 100644 --- a/waypoint_control/multi_obj_track/config.py +++ b/waypoint_control/multi_obj_track/config.py @@ -182,38 +182,89 @@ def __init__(self, ego_vehicle_position, camera_positions): } ) }, - "ZhongDian": { + "HutbCarlaCity": { "road_intersection_1": IntersectionConfig( - carla.Transform(carla.Location(x=89, y=53, z=44), carla.Rotation(pitch=0, yaw=90, roll=0)), + carla.Transform(carla.Location(x=-233, y=-36, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), { - "back_camera": carla.Transform(carla.Location(x=89, y=46, z=46.62), + "back_camera": carla.Transform(carla.Location(x=-233, y=-43, z=3.6), carla.Rotation(pitch=0, yaw=-90, roll=0)), - "front_camera": carla.Transform(carla.Location(x=89, y=60, z=46.62), + "front_camera": carla.Transform(carla.Location(x=-233, y=-29, z=3.6), carla.Rotation(pitch=0, yaw=90, roll=0)), - "right_camera": carla.Transform(carla.Location(x=85, y=46, z=46.62), + "right_camera": carla.Transform(carla.Location(x=-237, y=-43, z=3.6), carla.Rotation(pitch=0, yaw=-178, roll=0)), - "front_right_camera": carla.Transform(carla.Location(x=85, y=60, z=46.62), + "front_right_camera": carla.Transform(carla.Location(x=-237, y=-29, z=3.6), carla.Rotation(pitch=0, yaw=-178, roll=0)), - "left_camera": carla.Transform(carla.Location(x=93, y=46, z=46.62), + "left_camera": carla.Transform(carla.Location(x=-229, y=-43, z=3.6), carla.Rotation(pitch=0, yaw=-0, roll=0)), - "front_left_camera": carla.Transform(carla.Location(x=93, y=60, z=46.62), + "front_left_camera": carla.Transform(carla.Location(x=-229, y=-29, z=3.6), carla.Rotation(pitch=0, yaw=-0, roll=0)) } ), "road_intersection_2": IntersectionConfig( - carla.Transform(carla.Location(x=234, y=-529, z=46),carla.Rotation(pitch=0, yaw=90, roll=0)), + carla.Transform(carla.Location(x=-47, y=-543, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), { - "back_camera": carla.Transform(carla.Location(x=234, y=-536, z=48.62), + "back_camera": carla.Transform(carla.Location(x=-47, y=-550, z=3.6), carla.Rotation(pitch=0, yaw=-90, roll=0)), - "front_camera": carla.Transform(carla.Location(x=234, y=-522, z=48.62), + "front_camera": carla.Transform(carla.Location(x=-47, y=-536, z=3.6), carla.Rotation(pitch=0, yaw=90, roll=0)), - "right_camera": carla.Transform(carla.Location(x=230, y=-536, z=48.62), + "right_camera": carla.Transform(carla.Location(x=-51, y=-550, z=3.6), carla.Rotation(pitch=0, yaw=-178, roll=0)), - "front_right_camera": carla.Transform(carla.Location(x=230, y=-522, z=48.62), + "front_right_camera": carla.Transform(carla.Location(x=-51, y=-536, z=3.6), carla.Rotation(pitch=0, yaw=-178, roll=0)), - "left_camera": carla.Transform(carla.Location(x=238, y=-536, z=48.62), + "left_camera": carla.Transform(carla.Location(x=-43, y=-550, z=3.6), carla.Rotation(pitch=0, yaw=-0, roll=0)), - "front_left_camera": carla.Transform(carla.Location(x=238, y=-522, z=48.62), + "front_left_camera": carla.Transform(carla.Location(x=-43, y=-536, z=3.6), + carla.Rotation(pitch=0, yaw=-0, roll=0)) + } + ), + "road_intersection_3": IntersectionConfig( + carla.Transform(carla.Location(x=434, y=-16, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), + { + "back_camera": carla.Transform(carla.Location(x=434, y=-23, z=3.6), + carla.Rotation(pitch=0, yaw=-90, roll=0)), + "front_camera": carla.Transform(carla.Location(x=434, y=-9, z=3.6), + carla.Rotation(pitch=0, yaw=90, roll=0)), + "right_camera": carla.Transform(carla.Location(x=430, y=-23, z=3.6), + carla.Rotation(pitch=0, yaw=-178, roll=0)), + "front_right_camera": carla.Transform(carla.Location(x=430, y=-9, z=3.6), + carla.Rotation(pitch=0, yaw=-178, roll=0)), + "left_camera": carla.Transform(carla.Location(x=438, y=-23, z=3.6), + carla.Rotation(pitch=0, yaw=-0, roll=0)), + "front_left_camera": carla.Transform(carla.Location(x=438, y=-9, z=3.6), + carla.Rotation(pitch=0, yaw=-0, roll=0)) + } + ), + "road_intersection_4": IntersectionConfig( + carla.Transform(carla.Location(x=417, y=-364, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), + { + "back_camera": carla.Transform(carla.Location(x=417, y=-371, z=3.6), + carla.Rotation(pitch=0, yaw=-90, roll=0)), + "front_camera": carla.Transform(carla.Location(x=417, y=-357, z=3.6), + carla.Rotation(pitch=0, yaw=90, roll=0)), + "right_camera": carla.Transform(carla.Location(x=413, y=-371, z=3.6), + carla.Rotation(pitch=0, yaw=-178, roll=0)), + "front_right_camera": carla.Transform(carla.Location(x=413, y=-357, z=3.6), + carla.Rotation(pitch=0, yaw=-178, roll=0)), + "left_camera": carla.Transform(carla.Location(x=421, y=-371, z=3.6), + carla.Rotation(pitch=0, yaw=-0, roll=0)), + "front_left_camera": carla.Transform(carla.Location(x=413, y=-357, z=3.6), + carla.Rotation(pitch=0, yaw=-0, roll=0)) + } + ), + "road_intersection_5": IntersectionConfig( + carla.Transform(carla.Location(x=375, y=-574, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), + { + "back_camera": carla.Transform(carla.Location(x=375, y=-581, z=3.6), + carla.Rotation(pitch=0, yaw=-90, roll=0)), + "front_camera": carla.Transform(carla.Location(x=375, y=-567, z=3.6), + carla.Rotation(pitch=0, yaw=90, roll=0)), + "right_camera": carla.Transform(carla.Location(x=371, y=-581, z=3.6), + carla.Rotation(pitch=0, yaw=-178, roll=0)), + "front_right_camera": carla.Transform(carla.Location(x=371, y=-567, z=3.6), + carla.Rotation(pitch=0, yaw=-178, roll=0)), + "left_camera": carla.Transform(carla.Location(x=379, y=-581, z=3.6), + carla.Rotation(pitch=0, yaw=-0, roll=0)), + "front_left_camera": carla.Transform(carla.Location(x=379, y=-567, z=3.6), carla.Rotation(pitch=0, yaw=-0, roll=0)) } ) diff --git a/waypoint_control/multi_obj_track/reID/Utils/DEMO.m b/waypoint_control/multi_obj_track/reID/Utils/DEMO.m index 13b2913..4a494b1 100644 --- a/waypoint_control/multi_obj_track/reID/Utils/DEMO.m +++ b/waypoint_control/multi_obj_track/reID/Utils/DEMO.m @@ -7,7 +7,7 @@ % 获取程序开始时间 sys_time_before = posixtime(datetime('now')); -townName = 'Town01'; % 可以修改城镇和路口 +townName = 'Town10'; % 可以修改城镇和路口 % dataSets = {'Town10HD_Opt/test_data_junc1', 'Town10HD_Opt/test_data_junc2', 'Town10HD_Opt/test_data_junc3', 'Town10HD_Opt/test_data_junc4', 'Town10HD_Opt/test_data_junc5'}; % 定义各城镇对应的数据集 if strcmp(townName, 'Town10') @@ -62,11 +62,11 @@ %% 轨迹匹配,链接全部路口的轨迹 % 车辆轨迹匹配 vehiclematchThreshold = 0.65; % 车辆匹配阈值 -vehicletraj = linkIdentities(vehiclejuncTrajCell, vehiclematchThreshold); +vehicletraj = linkIdentities(vehiclejuncTrajCell, vehiclematchThreshold, townName); % 行人轨迹匹配 -personmatchThreshold = 0.65; % 车辆匹配阈值 -persontraj = linkIdentities(personjuncTrajCell, personmatchThreshold); +personmatchThreshold = 0.45; % 行人匹配阈值 +persontraj = linkIdentities(personjuncTrajCell, personmatchThreshold, townName); % % 获取程序结束时间 % sys_time_after = posixtime(datetime('now')); diff --git a/waypoint_control/multi_obj_track/reID/Utils/linkIdentities.m b/waypoint_control/multi_obj_track/reID/Utils/linkIdentities.m index 3e14915..8e4873b 100644 --- a/waypoint_control/multi_obj_track/reID/Utils/linkIdentities.m +++ b/waypoint_control/multi_obj_track/reID/Utils/linkIdentities.m @@ -1,4 +1,4 @@ -function traj = linkIdentities(juncTrajCell, matchThreshold) % 1x5 cell +function traj = linkIdentities(juncTrajCell, matchThreshold, townName) % 1x5 cell % Town10场景中有5个路口,已经拿到每个路口的车辆轨迹,还包括其外观特征以及轨迹时间 % 现需将5个路口的轨迹根据时间串联起来,形成完整的车辆轨迹 threshold = matchThreshold; @@ -36,14 +36,31 @@ % 路口 1 可以去往:2, 3, 4, 5 % 路口 2 可以去往:4, 5 % 路口 3 可以去往:4, 5 - roadPairs = [1, 2; - 1, 3; - 1, 4; - 1, 5; - 2, 4; - 2, 5; - 3, 4; - 3, 5]; + if strcmp(townName, 'Town10') + roadPairs = [1, 2; + 1, 3; + 1, 4; + 1, 5; + 2, 4; + 2, 5; + 3, 4; + 3, 5]; + elseif strcmp(townName, 'Town01') + roadPairs = [1, 4; + 1, 5; + 2, 3; + 2, 4; + 2, 5; + 3, 5]; + end + % roadPairs = [1, 2; + % 1, 3; + % 1, 4; + % 1, 5; + % 2, 4; + % 2, 5; + % 3, 4; + % 3, 5]; % 按照拓扑转移顺序,顺次使用 matchpairs 进行路口间的全局最优匹配 for p = 1:size(roadPairs, 1) diff --git a/waypoint_control/multi_obj_track/reID/Utils/processSingleJuncTraj.m b/waypoint_control/multi_obj_track/reID/Utils/processSingleJuncTraj.m index 44ca90d..9163466 100644 --- a/waypoint_control/multi_obj_track/reID/Utils/processSingleJuncTraj.m +++ b/waypoint_control/multi_obj_track/reID/Utils/processSingleJuncTraj.m @@ -135,55 +135,61 @@ function union(u, v) end function finalTrajMerged = filterFinalTraj(finalTraj, traj) - % 现在的逻辑:将同一组的碎片轨迹按时间顺序首尾拼接 - - % 初始化结果,存放拼接后的新结构体 + % 将同一组碎片轨迹按时间排序后,只拼接时间严格递增的片段 + % 当前片段第一个时间戳 > 已拼接部分的最后一个时间戳 + finalTrajMerged = cell(1, length(finalTraj)); - - % 遍历每一组需要合并的轨迹 + for i = 1:length(finalTraj) trajIndices = finalTraj{i}; - - % 提取每段碎片的起始时间,用来排序(保证拼接顺序是从早到晚) + + % 按片段起始时间排序 startTimes = zeros(1, length(trajIndices)); for j = 1:length(trajIndices) startTimes(j) = traj{trajIndices(j)}.timestamp(1); end [~, sortOrder] = sort(startTimes); - sortedIndices = trajIndices(sortOrder); % 排序后的索引 - - % 准备拼接容器 + sortedIndices = trajIndices(sortOrder); + + % 初始化拼接容器 merged_wrl_pos = []; merged_timestamp = []; - - % 为了保留最具代表性的特征,我们找到点数最多的一段,继承它的 trackID 和 mean_hsv + maxPoints = -1; best_mean_hsv = []; best_trackID = []; - + + % 逐个片段拼接,增加时间连续性检查 for j = 1:length(sortedIndices) idx = sortedIndices(j); currentTraj = traj{idx}; - - % 首尾纵向拼接位置和时间数据 + + % 时间连续性检查 + if ~isempty(merged_timestamp) + if currentTraj.timestamp(1) <= merged_timestamp(end) + % 时间不满足“严格后续”条件,跳过该片段 + continue; + end + end + + % 时间连续,允许拼接 merged_wrl_pos = [merged_wrl_pos; currentTraj.wrl_pos]; merged_timestamp = [merged_timestamp; currentTraj.timestamp]; - - % 记录最长段的特征作为该车的代表特征 + + % 保留最长片段的代表特征 if size(currentTraj.wrl_pos, 1) > maxPoints maxPoints = size(currentTraj.wrl_pos, 1); best_mean_hsv = currentTraj.mean_hsv; best_trackID = currentTraj.trackID; end end - - % 打包成新的完整结构体 + mergedStruct.trackID = best_trackID; mergedStruct.wrl_pos = merged_wrl_pos; mergedStruct.mean_hsv = best_mean_hsv; mergedStruct.timestamp = merged_timestamp; - mergedStruct.category = traj{sortedIndices(1)}.category; % 类别取任意一个即可 - + mergedStruct.category = traj{sortedIndices(1)}.category; + finalTrajMerged{i} = mergedStruct; end end \ No newline at end of file diff --git a/waypoint_control/twin_paper/main.tex b/waypoint_control/twin_paper/main.tex index 55cba99..c40e1df 100644 --- a/waypoint_control/twin_paper/main.tex +++ b/waypoint_control/twin_paper/main.tex @@ -195,16 +195,59 @@ \subsection{System physical layout and transmission latency} The latency in this system primarily stems from three sources: processing latency at the data collection end, data transmission latency, and processing latency at the central server. Processing latency at the data collection end primarily stems from the preprocessing of raw data. We mark the moment just before the data enters preprocessing as the start time and the moment preprocessing ends as the end time, then calculate the difference between the two as the processing latency. To calculate data transmission latency, we use the V2X transmission time at the data collection end as the start time and the V2X reception time at the central server as the end time, and calculate the difference between the two as the transmission latency. +Since the central server must receive data from all intersections before it can perform trajectory tracking, the longest transmission delay is therefore the total transmission delay. Processing latency on the central server primarily stems from the execution time of the relevant programs, namely the generation and re-creation of trajectories. We use the time when the data is input as the start time and the time when the target is successfully replicated in the virtual world as the end time, and calculate the difference between the two as the processing delay. Total delay refers to the time elapsed from when the target appears in front of the sensor until it is successfully detected. +\begin{table}[htbp] + \centering + \caption{System latency.} + \label{tab:algorithm_performance} + + \begin{tabular*}{\textwidth}{@{\extracolsep{\fill}}ccccc} + \toprule + Scene & Data Preprocessing & Data Transmission & Trajectory Tracking and Replay & Total \\ + \midrule + Town01 & 2 & 1 & 1 & \\ + Town10 & 400 & 533 & 600 & 1533 \\ + \bottomrule + \end{tabular*} +\end{table} + + \subsection{Generation of twin trajectories} % 介绍我们系统是如何生成孪生轨迹的,轨迹的生成方法 % 分单路口轨迹与多路口轨迹,未知区域轨迹预测 % 说明用到的公式,模型,以及其在孪生系统中的作用,突出系统的创新性 +\begin{table}[htbp] + \centering + \footnotesize + \setlength{\tabcolsep}{3pt} + \caption{System latedncy.} + \label{tab:algorithmsd} + + \begin{tabular*}{\textwidth}{@{\extracolsep{\fill}}cccccccccccc} + \toprule + \textbf{Scene} & \textbf{Cat.} & \textbf{Rcll} & \textbf{Prcn} & \textbf{FTR} & \textbf{FP} & \textbf{FN} & \textbf{IDS} & \textbf{MT} & \textbf{ML} & \textbf{MOTA} & \textbf{MOTP} \\ + \midrule + \multirow{2}{*}{Town01} & person & 77.55 & 97.95 & 0.07 & 35.8 & 411.8 & 14.8 & 72.95 & 16.19 & 75.05 & 92.47 \\ + % & person & 77.5512 & 97.9486 & 0.0716 & 35.8 & 411.8 & 14.8 & 72.9524 & 16.1905 & 75.0471 & 92.4724 \\ + & vehicle & 83.48 & 84.89 & 0.54 & 267.6 & 332.2 & 10.8 & 81.31 & 13.69 & 68.30 & 87.75 \\ + \midrule + \multirow{2}{*}{Town10} & person & 80.50 & 76.27 & 2.71 & 1353.6 & 1011 & 70.6 & 61.31 & 10.58 & 53.14 & 91.12 \\ + & vehicle & 88.17 & 71.19 & 2.30 & 1153.6 & 320 & 22.2 & 83 & 11.43 & 45.47 & 85.64 \\ + \bottomrule + \end{tabular*} +\end{table} + +To demonstrate the effectiveness of this system, we conducted validation tests across four different scenarios. +Town01 and Town10 are virtual maps included with CARLA. +hutb and zhongdian were imported into CARLA as real-world maps via the road network. + + \subsection{Twins in different scenarios} % 对比分析虚拟场景与真实场景下的孪生效果,说明系统的鲁棒性,突出系统的适应性 From 9931fecce8b4b4777747d95977ceec8ef0a46b2b Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Mon, 10 Aug 2026 17:56:35 +0800 Subject: [PATCH 07/10] =?UTF-8?q?=E8=8E=B7=E5=8F=96hutb=E5=9C=B0=E5=9B=BE?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E7=BB=93=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../collect_intersection_camera_lidar_hutb.py | 1856 +++++++++++++++++ 1 file changed, 1856 insertions(+) create mode 100644 waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py diff --git a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py new file mode 100644 index 0000000..e6aad9e --- /dev/null +++ b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py @@ -0,0 +1,1856 @@ +""" + 模拟有ego_vehicle的情况: + 1.ego_vehicle没有实体,但是ego_vehicle_transform 与雷达的一致,仅z坐标不同 + 2.修改camera_loc 和 lidar_loc 设置在路口中间,先试验理想状态下的轨迹跟踪 + 3.保存数据格式为 panda_set 的格式 + camera_folder + .mat + .gpsData.mat 自车经纬等信息(可以省去,需要修改helperGenerateEgoTrajectory.m) + + ego_vehicle coordinate x:前,y:左侧 z:向上 + 4.多目标融合检测的实际范围是在45m以内 + +""" +import math +import time +import numpy as np +import carla +import os +import cv2 +import random +import scipy.io +import argparse +import json +import subprocess +import pickle +import struct +import itertools +import torch +import torchvision.models as models +import torchvision.transforms as transforms +import joblib +import multiprocessing +import traceback +import queue + +from PIL import Image +from fontTools.merge.util import current_time +from ultralytics import YOLO +from queue import Queue +from queue import Empty +from scipy.spatial.transform import Rotation as R +from config import IntersectionConfig, town_configurations +DATA_MUN = 500 +DROP_BUFFER_TIME = 50 # 车辆落地前的缓冲时间,防止车辆还没落地就开始保存图片 +FUSION_DETECTION_ACTUAL_DIS = 51.2 # 多目标跟踪的实际检测距离 +WAITE_NEXT_INTERSECTION_TIME = 300 # 等待一定时间后第二路口相机雷达开始记录数据 + +config_path = "sensor_config.json" +# 定义全局变量 +global_time = 0.0 +base_frame = None +# openpcdet进行目标识别所用时间 +extra_time = 0 + +relativePose_to_egoVehicle = { + "back_camera": [-7.00, 0.00, 2.62, -180.00, 0.00, 0.00], # 1 + "front_camera": [7.00, 0.00, 2.62, 0.00, 0.00, 0.00], # 2 + "right_camera": [0.00, -4.00, 2.62, -90.00, 0.00, 0.00], # 6 + "front_right_camera": [7.00, -4.00, 2.62, -90.00, 0.00, 0.00], # 4 + "left_camera": [0.00, 4.00, 2.62, 90.00, 0.00, 0.00], # 5 + "front_left_camera": [7.00, 4.00, 2.62, 90.00, 0.00, 0.00] # 3 +} +relativePose_lidar_to_egoVehicle = [0, 0, 0.82, 0, 0, 0, 0, 0, 0] + +# 相机名称列表 +camera_names = [ + 'back_camera', 'front_camera', 'front_left_camera', + 'front_right_camera', 'left_camera', 'right_camera' +] + +class SimpleBoxes: + def __init__(self, boxes_dict): + self.xyxy = boxes_dict['xyxy'] + self.xywh = boxes_dict['xywh'] + self.xywhn = boxes_dict['xywhn'] + self.conf = boxes_dict['conf'] + self.cls = boxes_dict['cls'] + self.id = boxes_dict['id'] + + def __len__(self): + return len(self.conf) if self.conf is not None else 0 + +class SimpleResults: + def __init__(self, data_dict): + self.orig_img = data_dict['orig_img'] + self.orig_shape = data_dict['orig_shape'] + self.names = data_dict['names'] + self._plot_img = data_dict.get('plot_img') + self.boxes = SimpleBoxes(data_dict['boxes']) if data_dict['boxes'] else None + + def plot(self): + if self._plot_img is not None: + return self._plot_img + raise RuntimeError("plot_img not available. Set plot=True in worker.") + + +def extract_results_data(results): + """将 ultralytics Results 转换为字典列表(不传输 orig_img 以提升跨进程性能)""" + data_list = [] + for r in results: + item = { + "orig_img": None, # 占位,留给主进程在本地挂载,防止 KeyError + "orig_shape": getattr(r, "orig_shape", None), # (h, w) + "path": getattr(r, "path", None), + "speed": getattr(r, "speed", {}), + "names": getattr(r, "names", {}), + } + + # 提取检测框 + if hasattr(r, "boxes") and r.boxes is not None and len(r.boxes) > 0: + boxes = r.boxes + item["boxes"] = { + "xyxy": ( + boxes.xyxy.cpu().numpy() if boxes.xyxy is not None else None + ), + "xywh": ( + boxes.xywh.cpu().numpy() if boxes.xywh is not None else None + ), + "xywhn": ( + boxes.xywhn.cpu().numpy() + if boxes.xywhn is not None + else None + ), + "conf": ( + boxes.conf.cpu().numpy() if boxes.conf is not None else None + ), + "cls": ( + boxes.cls.cpu().numpy().astype(int) + if boxes.cls is not None + else None + ), + "id": ( + boxes.id.cpu().numpy() if boxes.id is not None else None + ), + } + else: + item["boxes"] = None + + data_list.append(item) + + return data_list + + +def yolo_worker(input_q, output_q): + os.environ.pop('LD_PRELOAD', None) + try: + model = YOLO('yolov8n.pt') + except Exception as e: + # 加载失败时回传错误消息,防止主进程死等 + output_q.put((-1, f"INIT_ERROR: {e}")) + return + + while True: + task = input_q.get() + if task is None: + break + + img_id, img = task + try: + results = model.predict(img, conf=0.25, iou=0.7, verbose=False) + data = extract_results_data(results) + output_q.put((img_id, data)) # 成功传回数据 + except Exception as e: + # 捕获推理中的所有报错,主动往队列塞一个 Error 标识 + print(f"【子进程】处理帧 {img_id} 时报错: {e}") + output_q.put((img_id, f"WORKER_ERROR: {e}")) + +def create_town_folder(town): + folder_name = f"{town}" + # 检查文件夹是否已存在,若不存在则创建 + if not os.path.exists(folder_name): + os.makedirs(folder_name) + print(f"Created folder: {folder_name}") + return folder_name + + +# 创建保存雷达数据的文件夹 +def create_radar_folder(junc, town_folder): + folder_name = f"{town_folder}/{junc}" + # 检查文件夹是否已存在,若不存在则创建 + if not os.path.exists(folder_name): + os.makedirs(folder_name) + print(f"Created folder: {folder_name}") + return folder_name + +# 创建保存相机数据的文件夹 +def create_camera_folder(camera_id, junc, town_folder): + folder_name = f"{town_folder}/{junc}/camera/{camera_id}" + if not os.path.exists(folder_name): + os.makedirs(folder_name) + print(f"Created folder: {folder_name}") + return folder_name + + +def rename_intersection(input_string): + # 检查输入字符串是否以 'road_intersection_' 开头 + if input_string.startswith('road_intersection_'): + # 提取数字部分 + num_str = input_string.split('_')[-1] # 分割字符串并取最后一个部分(假设数字总是在最后) + # 构建新的字符串 + new_string = f'test_data_junc{num_str}' + return new_string + else: + # 如果输入字符串不符合预期格式,可以返回原字符串或抛出异常 + return input_string # 这里简单返回原字符串,但实际应用中可能需要更复杂的错误处理 + + +# 保存车辆标签 +def save_point_label(world, location, lidar_to_world_inv, time_stamp, all_vehicle_labels, all_pedestrian_labels): + # 获取雷达检测范围内的全部车辆 + # 获取附近的所有车辆 + vehicle_list = world.get_actors().filter("*vehicle*") + pedestrian_list = world.get_actors().filter("*walker*") + + # 筛选出距离雷达小于 45 米的车辆 + # def dist(v): + # return v.get_location().distance(location) + def dist(actor): + return actor.get_location().distance(location) + # 筛选出距离小于 LIDAR_RANGE 的车辆 + # vehicle_list = list(filter(lambda v: dist(v) < FUSION_DETECTION_ACTUAL_DIS, vehicle_list)) + vehicle_list = list(filter(lambda actor: dist(actor) < FUSION_DETECTION_ACTUAL_DIS, vehicle_list)) + pedestrian_list = list(filter(lambda actor: dist(actor) < FUSION_DETECTION_ACTUAL_DIS, pedestrian_list)) + vehicle_labels = [] # 车辆标签列表 + pedestrian_labels = [] # 行人标签列表 + car_labels = [] # Car 标签列表 + truck_labels = [] # Truck 标签列表 + pedestrian_labelspy = [] # Pedestrian 标签列表 + # 获取标签NX9 + for vehicle in vehicle_list: + bounding_box = vehicle.bounding_box + bbox_z = bounding_box.location.z + location = vehicle.get_transform().location + rotation = vehicle.get_transform().rotation + bounding_box_location = np.array([location.x, location.y, bbox_z, 1]) + # 使用逆变换矩阵将位置从世界坐标系转换到雷达坐标系 + bounding_box_location_lidar = lidar_to_world_inv @ bounding_box_location # 矩阵乘法 + bounding_box_location_lidar = bounding_box_location_lidar[:3] # 去掉齐次坐标部分,得到三维坐标 + + # 获取边界框的宽长高 + bounding_box_extent = bounding_box.extent + length = 2 * bounding_box_extent.x + width = 2 * bounding_box_extent.y + height = 2 * bounding_box_extent.z + + bounding_box_rotation = np.array([rotation.yaw, rotation.pitch, rotation.roll]) + # 将 Euler 角(pitch, yaw, roll)转换为旋转矩阵(3x3) + rotation_matrix_world = R.from_euler('zyx', bounding_box_rotation, degrees=True).as_matrix() + # 使用逆变换矩阵将位置从世界坐标系转换到雷达坐标系 + rotation_matrix_lidar = lidar_to_world_inv[:3, :3] @ rotation_matrix_world + rotation_lidar = R.from_matrix(rotation_matrix_lidar) + euler_angles_lidar = rotation_lidar.as_euler('zyx', degrees=True) + # 输出转换后的 pitch, yaw, roll + yaw_degrees, pitch_lidar, roll_lidar = euler_angles_lidar + + # 将角度转换为弧度 + # 此时范围会变成大约 [-3.14, 3.14],即 [-π, π] + yaw_radians = yaw_degrees * (np.pi / 180.0) + # 将 [-π, π] 映射到 [0, 2π] + yaw_lidar = yaw_radians % (2 * np.pi) + + # 构造标签数据(Nx9 格式) + label = [ + bounding_box_location_lidar[0], # x + bounding_box_location_lidar[1], # y + bounding_box_location_lidar[2] + 0.3, + length, + width, + height + # pitch_lidar, # pitch + # roll_lidar, # roll + # yaw_lidar # yaw + ] + + # 判断车辆的类别(Car, Truck) + category = recognize_vehicle_class(vehicle) + + labelpy = [ + bounding_box_location_lidar[0], # x + bounding_box_location_lidar[1], # y + bounding_box_location_lidar[2] + 0.3, # z ,需要把z替换成bounding_box.z + length, + width, + height, + yaw_lidar, # yaw + category + ] + # 根据类别保存标签 + if category == "Car": + car_labels.append(labelpy) + elif category == "Truck": + truck_labels.append(labelpy) + + vehicle_id = vehicle.id + vehicle_labels.append((time_stamp, vehicle_id, label)) + if not vehicle_list: + raise RuntimeError(f"时间戳 {time_stamp} 处未检测到任何车辆,数据可能不完整。") + all_vehicle_labels.append(vehicle_labels) + + + # 获取行人标签 + step = 2 # 每隔1个元素遍历(步长为2) + for pedestrian in pedestrian_list[1::step]: + # for pedestrian in pedestrian_list: + bounding_box = pedestrian.bounding_box + bbox_z = bounding_box.location.z + location = pedestrian.get_transform().location + rotation = pedestrian.get_transform().rotation + bounding_box_location = np.array([location.x, location.y, bbox_z, 1]) + # 使用逆变换矩阵将位置从世界坐标系转换到雷达坐标系 + bounding_box_location_lidar = lidar_to_world_inv @ bounding_box_location # 矩阵乘法 + bounding_box_location_lidar = bounding_box_location_lidar[:3] # 去掉齐次坐标部分,得到三维坐标 + + # 获取边界框的宽长高 + bounding_box_extent = bounding_box.extent + length = 2 * bounding_box_extent.x + width = 2 * bounding_box_extent.y + height = 2 * bounding_box_extent.z + + bounding_box_rotation = np.array([rotation.yaw, rotation.pitch, rotation.roll]) + # 将 Euler 角(pitch, yaw, roll)转换为旋转矩阵(3x3) + rotation_matrix_world = R.from_euler('zyx', bounding_box_rotation, degrees=True).as_matrix() + # 使用逆变换矩阵将位置从世界坐标系转换到雷达坐标系 + rotation_matrix_lidar = lidar_to_world_inv[:3, :3] @ rotation_matrix_world + rotation_lidar = R.from_matrix(rotation_matrix_lidar) + euler_angles_lidar = rotation_lidar.as_euler('zyx', degrees=True) + # 输出转换后的 pitch, yaw, roll + yaw_degrees, pitch_lidar, roll_lidar = euler_angles_lidar + # 将角度转换为弧度 + # 此时范围会变成大约 [-3.14, 3.14],即 [-π, π] + yaw_radians = yaw_degrees * (np.pi / 180.0) + # 将 [-π, π] 映射到 [0, 2π] + yaw_lidar = yaw_radians % (2 * np.pi) + + # 构造标签数据(Nx9 格式) + label = [ + bounding_box_location_lidar[0], # x + bounding_box_location_lidar[1], # y + bounding_box_location_lidar[2] + height / 2, # z ,需要把z替换成bounding_box.z + length, + width, + height, + # pitch_lidar, # pitch + # roll_lidar, # roll + # yaw_lidar # yaw + ] + + # 构造标签数据(Nx9 格式) + labelpy = [ + bounding_box_location_lidar[0], # x + bounding_box_location_lidar[1], # y + bounding_box_location_lidar[2] + height / 2, # z ,需要把z替换成bounding_box.z + length, + width, + height, + yaw_lidar, # yaw + 'Pedestrian' + ] + pedestrian_labelspy.append(labelpy) # 行人标签直接保存,无需分类 + + pedestrian_id = pedestrian.id + pedestrian_labels.append((time_stamp, pedestrian_id, label)) + if not pedestrian_list: + raise RuntimeError(f"时间戳 {time_stamp} 处未检测到任何行人,数据可能不完整。") + all_pedestrian_labels.append(pedestrian_labels) + + # 将所有类别的标签合并到一个列表中 + all_labels = [] + + # 处理Car标签 + if len(car_labels) > 0: + for label in car_labels: + if len(label) >= 7: + # 格式化数值为两位小数 + formatted_label = [] + for i, value in enumerate(label): + if i < 7: # 前7个是数值 + formatted_label.append(f"{float(value):.2f}") # 格式化为两位小数 + else: # 第8个及以后是类别名称 + formatted_label.append(str(value)) + + # 如果只有7个字段,添加类别名 + if len(formatted_label) == 7: + formatted_label.append("Vehicle") + + all_labels.append(formatted_label) + + # 处理Truck标签 + if len(truck_labels) > 0: + for label in truck_labels: + if len(label) >= 7: + formatted_label = [] + for i, value in enumerate(label): + if i < 7: + formatted_label.append(f"{float(value):.2f}") + else: + formatted_label.append(str(value)) + + if len(formatted_label) == 7: + formatted_label.append("Truck") + + all_labels.append(formatted_label) + + # 处理Pedestrian标签 + if len(pedestrian_labelspy) > 0: + for label in pedestrian_labelspy: + if len(label) >= 7: + formatted_label = [] + for i, value in enumerate(label): + if i < 7: + formatted_label.append(f"{float(value):.2f}") + else: + formatted_label.append(str(value)) + + if len(formatted_label) == 7: + formatted_label.append("Pedestrian") + + all_labels.append(formatted_label) + + return all_labels + + +def send_v2x_message_lidar(lidar_data, sensor, pkl_file_path, junc, world): + try: + # 1. 读取 pkl 文件获取帧 ID + with open(pkl_file_path, 'rb') as f: + data = pickle.load(f) + if isinstance(data, list): + data = data[0] + # 获取 frame_id 并转为字符串 (如果没有则默认为 "0") + frame_id = str(data.get('frame_id', '0')) + + # # 获取当前时间戳 (保留4位小数即可) + # current_time = f"{time.time() - extra_time:.4f}" + + send_lidar_message(data, sensor, world, junc) + # 拼接成最简单的纯文本字符串,用逗号隔开 + # text_payload = f"{frame_id},{current_time},{junc},点云数据" + # msg = carla.CustomV2XBytes() + # msg.set_bytes(bytearray(text_payload, 'utf-8')) + # sensor.send(msg) + + except Exception as e: + import traceback + traceback.print_exc() + print(f"[发包报错]: {e}") + +def send_lidar_message(data, sensor, world, junc): + # 将核心数据提取出来 + boxes_lidar = data['boxes_lidar'] + scores = data['score'] + frame_id = data['frame_id'] + junc = junc.split('_')[-1] + + for i in range(len(boxes_lidar)): + # 提取第 i 行的框和得分 + box = boxes_lidar[i] + obj_score = scores[i] + # 将框解包 + x, y, z, l, w, h, yaw = box + x = format_8_chars(x) + y = format_8_chars(y) + z = format_8_chars(z) + l = format_8_chars(l) + w = format_8_chars(w) + h = format_8_chars(h) + yaw = format_8_chars(yaw) + # 当得分大于0.6时,视为有效数据并发送 + if obj_score > 0.6: + # 获取当前时间戳 (保留4位小数即可) + current_time = f"{time.time() - extra_time:.4f}" + # 拼接成最简单的纯文本字符串,用逗号隔开 + text_payload = f"{frame_id},{junc},{x},{y},{z},{l},{w},{h},{yaw},{current_time},ptd" + msg = carla.CustomV2XBytes() + msg.set_bytes(bytearray(text_payload, 'utf-8')) + sensor.send(msg) + + +def format_8_chars(val): + """强制将数字格式化为刚好 8 位的字符串:超长截断,不够右边补0""" + s = str(val) + # 如果是纯整数(比如 15),给它加个小数点变成 "15.",方便后面补0 + if '.' not in s: + s += '.' + # 如果长度超过 8,直接强行截断 + if len(s) > 8: + return s[:8] + # 如果长度不够 8,使用 ljust 在右侧补 '0' + else: + return s.ljust(8, '0') + +# 定义函数来保存雷达点云数据 +def save_radar_data(radar_data, world, ego_vehicle_transform, actual_vehicle_num, actual_pedestrian_num,lidar_to_world_inv, all_vehicle_labels, all_pedestrian_labels, junc, town_folder, file_num, sensors, num): + global global_time + # 获取当前帧编号 + current_frame = radar_data.frame + # 时间戳 + # timestamp = world.get_snapshot().timestamp.elapsed_seconds + timestamp = global_time + global_time = timestamp + 0.05 + location = ego_vehicle_transform.location + all_labels = save_point_label(world, location, lidar_to_world_inv, timestamp, all_vehicle_labels, all_pedestrian_labels) + + # 获取雷达数据并将其转化为numpy数组 + points = np.frombuffer(radar_data.raw_data, dtype=np.dtype('f4')) + points = np.reshape(points, (len(points) // 4, 4)) + location = points[:, :3] + # 将 location 转换为 float64(即 double 类型) + location = location.astype(np.float64) + intensity = points[:, 3].reshape(-1, 1).astype(np.float64) # 获取强度数据(第四通道) + # intensity_scaled = np.round(intensity * 255).astype(np.uint8) + count = location.shape[0] + # 计算 x 的范围 + x_limits = [np.min(location[:, 0]), np.max(location[:, 0])] # x 轴的最小值和最大值 + y_limits = [np.min(location[:, 1]), np.max(location[:, 1])] # y 轴的最小值和最大值 + z_limits = [np.min(location[:, 2]), np.max(location[:, 2])] # z 轴的最小值和最大值 + + # 创建存储数据的文件夹 + radar_folder = create_radar_folder(junc, town_folder) + file_name = os.path.join(radar_folder, f"{current_frame}.mat") + LidarData = { + 'PointCloud': { + 'Location': location, + 'Count': count, + 'XLimits': x_limits, + 'YLimits': y_limits, + 'ZLimits': z_limits, + 'Color': [], + 'Normal': [], + 'Intensity': intensity + }, + 'Timestamp': timestamp, + 'Pose': { + 'Position': relativePose_lidar_to_egoVehicle[:3], + 'Velocity': [0, 0, 0], + 'Orientation': [0, 0, 0] + }, + 'Detections': [] + } + + # 创建CameraData结构体 + camera_data = [] + for i, name in enumerate(camera_names): + camera_data.append({ + 'ImagePath': f"camera/{name}/{current_frame}.jpg", # 字符串路径 + 'Pose': { + 'Position': relativePose_to_egoVehicle[name][:3], # 单独的struct + 'Velocity': [0, 0, 0], # 静止速度 + 'Orientation': relativePose_to_egoVehicle[name][3:] # 姿态 + }, + 'Timestamp': timestamp, # 时间戳 + 'Detections': [] # 假设是检测框数据 + }) + + # 构造 MATLAB 的结构体数组 + # 逐字段提取,确保 MATLAB 能正确识别为 struct array + CameraData = np.zeros(len(camera_data), dtype=[ + ('ImagePath', 'O'), + ('Pose', 'O'), + ('Timestamp', 'float64'), + ('Detections', 'O') + ]) + + for i, entry in enumerate(camera_data): + CameraData[i] = ( + entry['ImagePath'], # 字符串路径 + entry['Pose'], # Pose 字典会被转换为 MATLAB 的 struct + entry['Timestamp'], # 时间戳 + entry['Detections'] # 5x4 矩阵 + ) + datalog = { + 'LidarData': LidarData, + 'CameraData': CameraData # 使用结构体数组 + } + vehicle_list = [] + pedestrian_list = [] + # 保存每一帧融合检测实际范围内的车辆和行人数量 + vehicle_list = world.get_actors().filter("*vehicle*") + pedestrian_list = world.get_actors().filter("*walker*") + + # def dist(v): + # return v.get_location().distance(ego_vehicle_transform.location) + def dist(actor): + return actor.get_location().distance(ego_vehicle_transform.location) + + vehicle_list = [actor for actor in vehicle_list if dist(actor) < FUSION_DETECTION_ACTUAL_DIS] + pedestrian_list = [actor for actor in pedestrian_list if dist(actor) < FUSION_DETECTION_ACTUAL_DIS] + vehicle_count = len(vehicle_list) + pedestrian_count = len(pedestrian_list) + actual_vehicle_num.append((timestamp, vehicle_count)) + actual_pedestrian_num.append((timestamp, pedestrian_count)) + + + # 将点云数据保存为 .mat 文件 + # 使用 scipy.io.savemat 保存数据,MATLAB 可以读取的格式 + scipy.io.savemat(file_name, {'datalog': datalog}) + + + # 提取 points 的前四列 + locationpy = points[:, :4] + # 将 locationpy 转换为 float64(即 double 类型) + locationpy = locationpy.astype(np.float32) + # 如果 timestamp 是单个值,创建重复的数组 + # if np.isscalar(timestamp): + # timestamp_array = np.full((locationpy.shape[0], 1), timestamp, dtype=np.float32) + # else: + # # 如果 timestamp 已经是数组,确保形状正确 + # timestamp_array = timestamp.reshape(-1, 1) + + # 水平拼接 location 前四列和 timestamp + # datalogpy = np.column_stack([locationpy, timestamp_array]) + + datalogpy = locationpy + + # 1. 直接保存 datalogpy 为 .npy + radar_folder = create_radar_folder_py(town_folder, junc) + np.save(os.path.join(radar_folder, f"{file_num}.npy"), datalogpy) + # 保存备份用于目标检测 + target_dir = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/data/custom/points" + clear_folder_contents(target_dir) + np.save(os.path.join(target_dir, f"{file_num}.npy"), datalogpy) + # 2. 保存 label 为 .txt + label_folder = create_label_folder(town_folder, junc) + with open(os.path.join(label_folder, f"{file_num}.txt"), 'w') as f: + # 处理不同的数据结构 + if isinstance(all_labels, list): + # 检查是否是嵌套列表(多个标签) + if all_labels and isinstance(all_labels[0], list): + # 多个标签:每行一个标签 + for label_item in all_labels: + line = " ".join(str(item) for item in label_item) + f.write(line + "\n") + else: + # 单个标签:一行 + line = " ".join(str(item) for item in all_labels) + f.write(line + "\n") + else: + # 其他类型(字符串、数字等) + f.write(str(all_labels)) + + # 保存备份用于目标检测 + goal_dir = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/data/custom/labels" + clear_folder_contents(goal_dir) + with open(os.path.join(goal_dir, f"{file_num}.txt"), 'w') as f: + # 处理不同的数据结构 + if isinstance(all_labels, list): + # 检查是否是嵌套列表(多个标签) + if all_labels and isinstance(all_labels[0], list): + # 多个标签:每行一个标签 + for label_item in all_labels: + line = " ".join(str(item) for item in label_item) + f.write(line + "\n") + else: + # 单个标签:一行 + line = " ".join(str(item) for item in all_labels) + f.write(line + "\n") + else: + # 其他类型(字符串、数字等) + f.write(str(all_labels)) + # 3. 每次保存 file_num 到 num.txt,并换行 + with open("num.txt", 'a') as f: # 'a' 表示追加模式 + f.write(str(file_num) + "\n") # 添加换行符 + # 保存备份用于目标检测 + dir_train = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/data/custom/ImageSets/train.txt" + dir_val = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/data/custom/ImageSets/val.txt" + with open(dir_train, 'w') as f: + f.write(str(file_num) + "\n") # 添加换行符 + with open(dir_val, 'w') as f: + f.write(str(file_num) + "\n") # 添加换行符 + + # 运行自动化目标检测脚本 + duration = run_shell_script() + global extra_time + extra_time += duration + + sensor = sensors["v2x_point"] + pkl_file_path = "/home/yons/traffic_twin/waypoint_control/multi_obj_track/OpenPCDet/output/cfgs/custom_models/pv_rcnn/default/pv_rcnn/default/pv_rcnn/default/eval/epoch_no_number/val/default/result.pkl" + send_v2x_message_lidar(radar_data, sensor, pkl_file_path, junc, world) + +# 更新目标检测的文件夹 +def clear_folder_contents(folder_path): + # 如果文件夹本来就不存在,直接建一个就行了 + if not os.path.exists(folder_path): + os.makedirs(folder_path) + print(f"文件夹不存在,已新建: {folder_path}") + return + + # 如果存在,就遍历里面的所有内容 + for filename in os.listdir(folder_path): + file_path = os.path.join(folder_path, filename) + try: + # 如果是普通文件或软链接,直接删除 + if os.path.isfile(file_path) or os.path.islink(file_path): + os.unlink(file_path) # 等同于 os.remove + # 如果里面还有子文件夹,用 shutil.rmtree 删掉子文件夹 + elif os.path.isdir(file_path): + shutil.rmtree(file_path) + except Exception as e: + print(f'删除 {file_path} 失败。原因: {e}') + + print(f"文件夹内容已清空: {folder_path}") + +# 定义函数来保存相机图像数据 +def save_camera_data(image_data, camera_id, junc, town_folder, model, sensors, num, pca_model, quantize_scale, resnet_model, img_preprocess, compute_device): + global base_frame + current_frame = image_data.frame + # 如果是第一帧,就把它的 ID 存为基数 + if base_frame is None: + base_frame = current_frame + print(f"收到第一帧数据!将原始帧 ID {base_frame} 设置为基数 0。") + # 计算重置后的当前帧 + normalized_frame = current_frame - base_frame + frame_str = f"{normalized_frame:06d}" + + image = np.array(image_data.raw_data) + image = image.reshape((image_data.height, image_data.width, 4)) # 4th channel is alpha + image = image[:, :, :3] # 去掉 alpha 通道,只保留 RGB + # 使用yolov8检测图片 + # 设置为 spawn 启动模式 + multiprocessing.set_start_method("spawn", force=True) + + input_queue = multiprocessing.Queue() + output_queue = multiprocessing.Queue() + + p = multiprocessing.Process( + target=yolo_worker, args=(input_queue, output_queue) + ) + p.start() + + # 发送给 YOLO 子进程 + input_queue.put((1, image)) + + # 安全获取结果 + try: + # 设置 3 秒超时,防止死锁 + img_id, yolo_data = output_queue.get(timeout=3.0) + + # 检查返回的是否是子进程发送的报错字符串 + if isinstance(yolo_data, str) and "ERROR" in yolo_data: + print(f"主进程捕获到子进程报错,跳过当前帧: {yolo_data}") + else: + # 重新挂载图片 + for item in yolo_data: + item['orig_img'] = image + print("成功获得结果!") + + except queue.Empty: + # 如果超时了,检查子进程是否还活着 + if not p.is_alive(): + print("【严重错误】YOLO 子进程已经崩溃死亡!退出循环或重新启动子进程。") + p.terminate() # 强行释放资源 + else: + print("【警告】YOLO 处理超时,但子进程仍存活,跳过此帧。") + + # 结束进程 + input_queue.put(None) + p.join() + + # 将字典包装为 SimpleResults 对象 + results = [SimpleResults(d) for d in yolo_data] + # results = model.predict(source=image) + result = results[0] + + + if camera_id == "back_camera": + sensor = sensors["v2x_back"] + elif camera_id == "front_camera": + sensor = sensors["v2x_front"] + elif camera_id == "right_camera": + sensor = sensors["v2x_front_left"] + elif camera_id == "front_right_camera": + sensor = sensors["v2x_front_right"] + elif camera_id == "left_camera": + sensor = sensors["v2x_left"] + elif camera_id == "front_left_camera": + sensor = sensors["v2x_right"] + + send_v2x_message_camera(sensor, junc, frame_str, result, camera_id, pca_model, quantize_scale, image, resnet_model, img_preprocess, compute_device) + + camera_folder = create_camera_folder(camera_id, junc, town_folder) + file_name = os.path.join(camera_folder, f"{current_frame}.jpg") + try: + cv2.imwrite(file_name, image) # 使用 OpenCV 保存图像 + except Exception as e: + print(f"Error saving image for frame {current_frame}: {e}") + return None + return image + + +def send_v2x_message_camera(sensor, junc, frame_id, result, camera_id, pca_model, quantize_scale, image=None, + model=None, preprocess=None, device=None): + try: + # 简化路口编号 + junc = junc.split('_')[-1] + # 目标类别名称 + target_classes = ['person', 'car', 'truck', 'bus'] + boxes = result.boxes + + # 判空检查:如果 boxes 为 None 或长度为 0,直接 return(无目标不发包) + if boxes is None or len(boxes) == 0: + return + + orig_img = result.orig_img if hasattr(result, 'orig_img') else image + + number = 0 + + # 遍历图片中的每一个检测框 + for i in range(len(boxes)): + # 提取置信度 + conf = float(boxes.conf[i].item()) + + # 置信度大于 0.25 + if conf > 0.25: + # 提取类别 ID 并翻译成名称 + cls_id = int(boxes.cls[i].item()) + class_name = result.names[cls_id] + + # 只保留人和车 + if class_name in target_classes: + number = number + 1 + current_time = f"{time.time() - extra_time:.4f}" + # 提取中心点和宽高 + coords = boxes.xywh[i].tolist() + x, y, w, h = coords + if orig_img is not None and model is not None: + xyxy = boxes.xyxy[i].tolist() + x1, y1, x2, y2 = [int(val) for val in xyxy] + + # 边界保护 + img_h, img_w = orig_img.shape[:2] + x1, y1 = max(0, x1), max(0, y1) + x2, y2 = min(img_w, x2), min(img_h, y2) + + # 切片裁剪 + cropped_image = orig_img[y1:y2, x1:x2] + + # 检查裁剪后的图像是否有效 + if cropped_image.size > 0: + # 调用独立的函数提取特征 + feature_vector = extract_feature( + cropped_image, + model, + preprocess, + device + ) + # 压缩特征值 + feature_bytes = compress_feature_to_24bytes(feature_vector, pca_model, quantize_scale) + frameid = int(frame_id) + # junc_id = int(junc[4:]) + + text_payload = struct.pack(' 0: + print(f" 已找到轨迹 ") + final_script = [] + for item in raw_script: + spawn_loc = carla.Location(x=item["spawn_x"], y=item["spawn_y"], z=item["spawn_z"]) + dest_loc = carla.Location(x=item["dest_x"], y=item["dest_y"], z=item["dest_z"]) + final_script.append({ + "spawn_point": carla.Transform(spawn_loc), + "destination": dest_loc, + "speed": item["speed"] + }) + print(f" 成功加载 {len(final_script)} 名行人的运动轨迹!") + return final_script + + # ---- 针对自定义地图的无缝兼容逻辑 ---- + carla_map = world.get_map() + all_wps = carla_map.generate_waypoints(distance=2.0) + sidewalk_wps = [wp for wp in all_wps if wp.lane_type == carla.LaneType.Sidewalk] + target_wps = sidewalk_wps if len(sidewalk_wps) > 0 else all_wps + + def get_location(rng): + # 优先使用 Navmesh,如果自定义地图返回 None,则从人行道 Waypoint 随机抽取 + loc = world.get_random_location_from_navigation() + if loc is None and len(target_wps) > 0: + loc = rng.choice(target_wps).transform.location + carla.Location(z=0.2) + return loc + # ------------------------------------ + + MIN_DISTANCE = 30 + local_rng = random.Random(seed) + raw_script = [] + final_script = [] + generated_count = 0 + + max_attempts = num_pedestrians * 100 + + for attempt in range(max_attempts): + if generated_count >= num_pedestrians: + break + + spawn_point = get_location(local_rng) + destination = get_location(local_rng) + + if spawn_point is not None and destination is not None: + distance = spawn_point.distance(destination) + + if distance >= MIN_DISTANCE: + speed = round(local_rng.uniform(1.1, 1.5), 2) + + raw_script.append({ + "spawn_x": spawn_point.x, "spawn_y": spawn_point.y, "spawn_z": spawn_point.z, + "dest_x": destination.x, "dest_y": destination.y, "dest_z": destination.z, + "speed": speed + }) + + final_script.append({ + "spawn_point": carla.Transform(spawn_point), + "destination": destination, + "speed": speed + }) + generated_count += 1 + + if generated_count < num_pedestrians: + print(f"只找到了 {generated_count} 条大于 {MIN_DISTANCE} 米的路线。") + + with open(filepath, 'w') as f: + json.dump(raw_script, f, indent=4) + + print(f"轨迹生成完毕!已将 {generated_count} 名行人的轨迹保存在 {filepath}!") + return final_script + +# 生成随机运动行人 +def spawn_autonomous_pedestrians(world, num_pedestrians=150, random_seed=20): + random.seed(random_seed) + np.random.seed(random_seed) + pedestrian_list = [] + + auto_pedestrian_script = get_or_create_pedestrian_script(world, num_pedestrians) + pedestrian_gen = create_pedestrian_generator(world, seed=2024) + + # 记录成功生成的行人及其对应的脚本数据(起点、终点、速度) + spawned_info = [] + + for script_data in auto_pedestrian_script: + # 用 next() 获取下一个模型 + walker_bp = next(pedestrian_gen) + + walker_actor = world.try_spawn_actor(walker_bp, script_data["spawn_point"]) + if walker_actor: + pedestrian_list.append(walker_actor) + spawned_info.append((walker_actor, script_data)) + print(f"Spawned pedestrian: {walker_actor.id}") + + world.tick() + + # 绕过 NavMesh AI 模块,直接给每个行人施加速度和方向向量 + for walker_actor, script_data in spawned_info: + spawn_loc = script_data["spawn_point"].location + dest_loc = script_data["destination"] + + # 计算水平面上的朝向向量 + dx = dest_loc.x - spawn_loc.x + dy = dest_loc.y - spawn_loc.y + dist = math.sqrt(dx ** 2 + dy ** 2) + + if dist > 0: + # 单位化方向向量(锁定 z=0.0 避免向上或下倾斜) + direction = carla.Vector3D(x=dx / dist, y=dy / dist, z=0.0) + + # 设置直行移动控制 + control = carla.WalkerControl( + direction=direction, + speed=script_data["speed"] + ) + walker_actor.apply_control(control) + + return pedestrian_list + +def spawn_v2x_sensors(world, lidar_transform, z_height=2.57): + sensors = {} # 字典 + + # # 用字典直接给坐标命名,明确区分 + # coordinates = { + # "v2x_back": (-7.0, 0.0), + # "v2x_front": (7.0, 0.0), + # "v2x_front_left": (7.0, 4.0), + # "v2x_front_right": (7.0, -4.0), + # "v2x_left": (0.0, 4.0), + # "v2x_right": (0.0, -4.0), + # "v2x_point": (0.5, 0.5) + # } + with open(config_path, "r", encoding="utf-8") as f: + raw_coordinates = json.load(f) + + # 将 JSON 里的列表 [-7.0, 0.0] 转换回 Python 的元组 (-7.0, 0.0) + coordinates = {key: tuple(value) for key, value in raw_coordinates.items()} + + # 获取传感器蓝图 + bp = world.get_blueprint_library().find('sensor.other.v2x_custom') + # 定义通信频道 + bp.set_attribute("channel_id", "5") + + for name, (x, y)in coordinates.items(): + # 直接使用原始坐标 + location = carla.Location(x=x, y=y, z=z_height) + transform = carla.Transform(location) + # 生成V2X传感器 + sensor = world.spawn_actor(bp, transform) + # 激活传感器 + sensor.listen(lambda data: do_nothing(data)) + # 将生成的传感器以名字存入字典 + sensors[name] = sensor + + return sensors + +def do_nothing(data): + pass + +def spawn_v2x_receiver(world, quantize_scale, town_folder): + location = carla.Location(x=0, y=0, z=2.62) + transform = carla.Transform(location, carla.Rotation(yaw=0)) + + # 获取传感器蓝图 + bp = world.get_blueprint_library().find('sensor.other.v2x_custom') + # 定义通信频道 + bp.set_attribute("channel_id", "5") + # 生成传感器 + receiver = world.spawn_actor(bp, transform) + receiver.listen(lambda data: _on_v2x_received(data, quantize_scale, town_folder)) + + return receiver + +def _on_v2x_received(event, quantize_scale, town_folder): + """ + 接收端回调函数:将所有帧的数据保存在同一个文件夹下的独立 txt 中 + """ + if event.get_message_count() == 0: + return + + for i, custom_data in enumerate(event): + try: + # 获取底层数据 + parsed_data = custom_data.get() + raw_payload = None + text_payload = "" + is_binary_feature = False + + # 智能提取数据载体 + if isinstance(parsed_data, dict): + raw_payload = parsed_data.get("Message", {}).get("Message", {}).get("Bytes", "") + else: + raw_payload = parsed_data + + if isinstance(raw_payload, (bytes, bytearray)): + try: + text_payload = raw_payload.decode('utf-8') + except UnicodeDecodeError: + is_binary_feature = True + elif isinstance(raw_payload, str): + text_payload = raw_payload + else: + continue + + if is_binary_feature: + try: + frame_id, junc_id, camera_id, number, feature_bytes = struct.unpack(' Date: Tue, 18 Aug 2026 18:29:16 +0800 Subject: [PATCH 08/10] =?UTF-8?q?=E8=AE=BA=E6=96=87results=E6=92=B0?= =?UTF-8?q?=E5=86=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- waypoint_control/multi_obj_track/config.m | 52 ++++++++++++++++++----- waypoint_control/twin_paper/main.tex | 20 +++++++-- 2 files changed, 58 insertions(+), 14 deletions(-) diff --git a/waypoint_control/multi_obj_track/config.m b/waypoint_control/multi_obj_track/config.m index e07bb25..ca9457c 100644 --- a/waypoint_control/multi_obj_track/config.m +++ b/waypoint_control/multi_obj_track/config.m @@ -119,27 +119,59 @@ ) ... ); - -dataset.ZhongDian = struct(... +dataset.HutbCarlaCity = struct(... 'intersection_1', struct(... - 'name', 'ZhongDian/test_data_junc1', ... + 'name', 'HutbCarlaCity/test_data_junc1', ... 'initialTime', 0.0, ... 'juncNum', 1, ... 'TransformationMatrix', [ - -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, 8.90000000e+01; - 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, 5.30000000e+01; - 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 4.78199997e+01; + -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, -2.33000000e+02; + 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -3.60000000e+01; + 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 1.79999995e+00; 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 ] ... ), ... 'intersection_2', struct(... - 'name', 'ZhongDian/test_data_junc2', ... + 'name', 'HutbCarlaCity/test_data_junc2', ... 'initialTime', 0.0, ... 'juncNum', 2, ... 'TransformationMatrix', [ - -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, 2.34000000e+02; - 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -5.29000000e+02; - 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 4.98199997e+01; + -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, -4.70000000e+01; + 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -5.43000000e+02; + 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 1.79999995e+00; + 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 + ] ... + ), ... + 'intersection_3', struct(... + 'name', 'HutbCarlaCity/test_data_junc3', ... + 'initialTime', 0.0, ... + 'juncNum', 3, ... + 'TransformationMatrix', [ + -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, 4.34000000e+02; + 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -1.60000000e+01; + 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 1.79999995e+00; + 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 + ] ... + ), ... + 'intersection_4', struct(... + 'name', 'HutbCarlaCity/test_data_junc4', ... + 'initialTime', 0.0, ... + 'juncNum', 4, ... + 'TransformationMatrix', [ + -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, 4.17000000e+02; + 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -3.64000000e+02; + 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 1.79999995e+00; + 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 + ] ... + ), ... + 'intersection_5', struct(... + 'name', 'HutbCarlaCity/test_data_junc5', ... + 'initialTime', 0.0, ... + 'juncNum', 5, ... + 'TransformationMatrix', [ + -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, 3.75000000e+02; + 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -5.74000000e+02; + 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 1.79999995e+00; 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 ] ... ) ... diff --git a/waypoint_control/twin_paper/main.tex b/waypoint_control/twin_paper/main.tex index c40e1df..4e484f5 100644 --- a/waypoint_control/twin_paper/main.tex +++ b/waypoint_control/twin_paper/main.tex @@ -227,7 +227,7 @@ \subsection{Generation of twin trajectories} \footnotesize \setlength{\tabcolsep}{3pt} \caption{System latedncy.} - \label{tab:algorithmsd} + \label{tab:tracking-results} \begin{tabular*}{\textwidth}{@{\extracolsep{\fill}}cccccccccccc} \toprule @@ -239,14 +239,26 @@ \subsection{Generation of twin trajectories} \midrule \multirow{2}{*}{Town10} & person & 80.50 & 76.27 & 2.71 & 1353.6 & 1011 & 70.6 & 61.31 & 10.58 & 53.14 & 91.12 \\ & vehicle & 88.17 & 71.19 & 2.30 & 1153.6 & 320 & 22.2 & 83 & 11.43 & 45.47 & 85.64 \\ + \midrule + \multirow{2}{*}{HutbCarlaCity} & person & & & & & & & & & & \\ + & vehicle & & & & & & & & & & \\ + \midrule + \multirow{2}{*}{Zhongdian} & person & & & & & & & & & & \\ + & vehicle & & & & & & & & & & \\ \bottomrule \end{tabular*} \end{table} To demonstrate the effectiveness of this system, we conducted validation tests across four different scenarios. -Town01 and Town10 are virtual maps included with CARLA. -hutb and zhongdian were imported into CARLA as real-world maps via the road network. - +Town01 and Town10 are virtual scene maps. +HutbCarlaCity and Zhongdian are real-world maps. +We ran 500 frames for each scenario to simulate traffic conditions over a period of time, and used the overall average across the five intersections to represent the final results, yielding the experimental results shown in Table \ref{tab:tracking-results}. + +In a 500-frame simulation, this system successfully achieved a spatio-temporally consistent digital twin. +From Table \ref{tab:tracking-results}, we can see that: +In the virtual scenarios, the MOTA (75.05 and 68.30) and Prcn (97.95 and 84.89) for the Town01 scenario were generally higher than those for the Town10 scenario (MOTA of 53.14 and 45.47, Prcn of 76.27 and 71.19) . The highest MOTA and Prcn values both occurred in the pedestrian class in the Town01 scenario (75.05 and 97.95, respectively), while the lowest MOTA value occurred in the vehicle class in the Town10 scenario (45.47). +In both scenarios, the recall rates for vehicles (83.48 for Town01 and 88.17 for Town10) and MT (81.31 for Town01 and 83 for Town10) were both higher than those for pedestrians in the same scenarios. The highest recall rate was observed in the vehicle class for Town10 (88.17). +All error metrics (FP, FN, IDS, FTR) for Town10 are significantly higher than those for Town01. Specifically, the “human” class in Town10 has the highest values for FP (1,353.6), FN (1,011), IDS (70.6), and FTR (2.71); the “human” class in Town01 has the lowest FP value (35.8). \subsection{Twins in different scenarios} From 2e35d47c80a80999689dc6dd40b43eaf3afe388c Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Wed, 26 Aug 2026 17:02:32 +0800 Subject: [PATCH 09/10] =?UTF-8?q?=E5=9C=A8HutbCarlaCity=E5=9C=B0=E5=9B=BE?= =?UTF-8?q?=E4=B8=8A=E9=99=90=E5=88=B6=E4=BA=BA=E8=BD=A6=E7=94=9F=E6=88=90?= =?UTF-8?q?=E4=BD=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../multi_obj_track/collect_lidar_dataset.py | 215 +++++++++++++++++- 1 file changed, 210 insertions(+), 5 deletions(-) diff --git a/waypoint_control/multi_obj_track/collect_lidar_dataset.py b/waypoint_control/multi_obj_track/collect_lidar_dataset.py index 269ac06..a7d68da 100644 --- a/waypoint_control/multi_obj_track/collect_lidar_dataset.py +++ b/waypoint_control/multi_obj_track/collect_lidar_dataset.py @@ -444,13 +444,204 @@ def spawn_autonomous_pedestrians(world, num_pedestrians=120, random_seed=42): return pedestrian_list +# 生成自动驾驶车辆 +def spawn_autonomous_vehicles_hutb(world, tm, junction_weights, num_vehicles=50, random_seed=42): + random.seed(random_seed) + np.random.seed(random_seed) + vehicle_list = [] + + carla_map = world.get_map() + filter_bike_blueprinter = filter_vehicle_blueprinter(world.get_blueprint_library().filter('vehicle.*')) + all_waypoints = carla_map.generate_waypoints(distance=2.0) + + # 提取各个路口外的入口点 + junction_entries = {jid: [] for jid in junction_weights.keys()} + processed_jids = set() + + for wp in all_waypoints: + if wp.is_junction and wp.junction_id in junction_weights: + jid = wp.junction_id + if jid not in processed_jids: + processed_jids.add(jid) + junction = wp.get_junction() + waypoint_pairs = junction.get_waypoints(carla.LaneType.Driving) + for entry_wp, _ in waypoint_pairs: + outside_wps = entry_wp.previous(15.0) + if outside_wps: + junction_entries[jid].append(outside_wps[0]) + + valid_jids = {jid: w for jid, w in junction_weights.items() if len(junction_entries[jid]) > 0} + if not valid_jids: + print("警告: 未检测到有效的路口入口点!") + return [] + + # 按加权计算各路口车辆配额 + total_weight = sum(valid_jids.values()) + normalized_weights = {jid: w / total_weight for jid, w in valid_jids.items()} + + assigned_counts = {} + remaining_vehicles = num_vehicles + for jid, weight in normalized_weights.items(): + count = int(num_vehicles * weight) + assigned_counts[jid] = count + remaining_vehicles -= count + + if remaining_vehicles > 0: + assigned_counts[max(normalized_weights, key=normalized_weights.get)] += remaining_vehicles + + # 按配额生成车辆 + for start_jid, count in assigned_counts.items(): + entry_points = junction_entries[start_jid] + if not entry_points: + continue + + for _ in range(count): + start_wp = random.choice(entry_points) + transform = start_wp.transform + transform.location.z += 0.5 + + vehicle_bp = random.choice(filter_bike_blueprinter) + vehicle = world.try_spawn_actor(vehicle_bp, transform) + + if vehicle: + vehicle.set_autopilot(True) + tm.ignore_lights_percentage(vehicle, 100) + # 已移除 GlobalRoutePlanner 和 tm.set_path 跨路口导航逻辑 + vehicle_list.append(vehicle) + + print(f"成功按加权在各路口外生成了 {len(vehicle_list)} 辆车(本地行驶,无跨路口导航)。") + return vehicle_list + + +def create_pedestrian_generator(world, seed=42): + # 获取行人蓝图(排除小孩) + all_walkers = world.get_blueprint_library().filter('walker.pedestrian.*') + kid_ids = ['walker.pedestrian.0015', 'walker.pedestrian.0016', 'walker.pedestrian.0017'] + + adult_bps = [bp for bp in all_walkers if bp.id not in kid_ids] + + # 排序并打乱 + adult_bps.sort(key=lambda x: x.id) + local_rng = random.Random(seed) + local_rng.shuffle(adult_bps) + + # 使用 yield 循环输出模型 + for bp in itertools.cycle(adult_bps): + yield bp + + +def generate_pedestrian_trajectories(world, junction_weights, num_pedestrians=100, seed=2024): + """ + 按路口权重生成行人的初始位置和目标方向(避开路中央) + """ + carla_map = world.get_map() + all_wps = carla_map.generate_waypoints(distance=2.0) + + # 提取各个路口内的点 + junction_wps = {jid: [] for jid in junction_weights.keys()} + for wp in all_wps: + if wp.is_junction and wp.junction_id in junction_weights: + junction_wps[wp.junction_id].append(wp) + + # 寻找人行道点或向外侧偏移 + target_locations = {jid: [] for jid in junction_weights.keys()} + for jid, wps in junction_wps.items(): + for wp in wps: + # 尝试获取右侧车道,如果是人行道就使用它 + right_lane = wp.get_right_lane() + if right_lane and right_lane.lane_type == carla.LaneType.Sidewalk: + target_locations[jid].append(right_lane.transform.location + carla.Location(z=0.2)) + else: + # 否则直接向车道右侧垂直偏移 3.5 米,强行移出机动车道 + right_vec = wp.transform.get_right_vector() + target_locations[jid].append(wp.transform.location + right_vec * 3.5 + carla.Location(z=0.2)) + + valid_jids = {jid: w for jid, w in junction_weights.items() if len(target_locations[jid]) > 0} + if not valid_jids: + return [] + + # 按加权计算各路口行人配额 + total_weight = sum(valid_jids.values()) + assigned_counts = {} + remaining = num_pedestrians + + for jid, weight in valid_jids.items(): + count = int(num_pedestrians * (weight / total_weight)) + assigned_counts[jid] = count + remaining -= count + + if remaining > 0: + assigned_counts[max(valid_jids, key=lambda k: valid_jids[k] / total_weight)] += remaining + + # 4. 生成脚本数据(恢复为起点+终点的形式) + local_rng = random.Random(seed) + generated_script = [] + + for jid, count in assigned_counts.items(): + locs = target_locations[jid] + if len(locs) < 2: + continue + + for _ in range(count): + spawn_loc = local_rng.choice(locs) + dest_loc = local_rng.choice(locs) # 在同路口内随便选一个点作为运动方向 + + speed = round(local_rng.uniform(1.1, 1.5), 2) + generated_script.append({ + "spawn_point": carla.Transform(spawn_loc), + "destination": dest_loc, + "speed": speed + }) + + return generated_script + + +# 生成随机运动行人 +def spawn_autonomous_pedestrians_hutb(world, junction_weights, num_pedestrians=100, random_seed=20): + random.seed(random_seed) + np.random.seed(random_seed) + pedestrian_list = [] + + auto_pedestrian_script = generate_pedestrian_trajectories(world, junction_weights, num_pedestrians, seed=random_seed) + pedestrian_gen = create_pedestrian_generator(world, seed=random_seed) + + spawned_info = [] + for script_data in auto_pedestrian_script: + walker_bp = next(pedestrian_gen) + walker_actor = world.try_spawn_actor(walker_bp, script_data["spawn_point"]) + if walker_actor: + pedestrian_list.append(walker_actor) + spawned_info.append((walker_actor, script_data)) + + print(f"成功按权重 Spawn 了 {len(pedestrian_list)} 名行人(避开路中央)。") + world.tick() + + # 恢复你最开始的机械直线运动控制器逻辑 + for walker_actor, script_data in spawned_info: + spawn_loc = script_data["spawn_point"].location + dest_loc = script_data["destination"] + dx = dest_loc.x - spawn_loc.x + dy = dest_loc.y - spawn_loc.y + dist = math.sqrt(dx ** 2 + dy ** 2) + + if dist > 0: + direction = carla.Vector3D(x=dx / dist, y=dy / dist, z=0.0) + control = carla.WalkerControl( + direction=direction, + speed=script_data["speed"] + ) + walker_actor.apply_control(control) + + return pedestrian_list + # 主函数 def main(): # 连接到Carla服务器 client = carla.Client('localhost', 2000) client.set_timeout(10.0) - world = client.get_world() + map_name = 'HutbCarlaCity' + world = client.load_world(map_name) # 仿真设置 settings = world.get_settings() @@ -476,10 +667,24 @@ def main(): random_seed = 20 # 静止 ego_vehicle 的位置 ego_transform = carla.Transform(carla.Location(x=-46, y=21, z=1), carla.Rotation(pitch=0, yaw=90, roll=0)) - # 先生成自动驾驶车辆 - vehicles = spawn_autonomous_vehicles(world, tm, num_vehicles=50, random_seed=random_seed) - # 生成行人 - pedestrians = spawn_autonomous_pedestrians(world, num_pedestrians=100, random_seed=20) + # 定义 5 个路口 ID + target_junction_weights = { + 2121: 0.3, + 398: 0.1, + 576: 0.3, + 626: 0.1, + 510: 0.2 + } + if map_name in ["Town01", "Town10HD_Opt"]: + # 生成自动驾驶车辆 + vehicles = spawn_autonomous_vehicles(world, tm, num_vehicles=50, random_seed=random_seed) + # 生成行人 + pedestrians = spawn_autonomous_pedestrians(world, num_pedestrians=100, random_seed=20) + else: + # 生成自动驾驶车辆 + vehicles = spawn_autonomous_vehicles_hutb(world, tm, target_junction_weights, num_vehicles=50,random_seed=random_seed) + # 生成行人 + pedestrians = spawn_autonomous_pedestrians_hutb(world, target_junction_weights, num_pedestrians=100,random_seed=20) #启动行人碰撞 for pedestrian in pedestrians: if "walker.pedestrian." in pedestrian.type_id: From 94f9cfaf6e3c94dff78ad29979ff9816b6ce6dca Mon Sep 17 00:00:00 2001 From: liuao08 <2496556459@qq.com> Date: Wed, 16 Sep 2026 20:52:32 +0800 Subject: [PATCH 10/10] =?UTF-8?q?=E8=AE=BA=E6=96=87results=E5=86=85?= =?UTF-8?q?=E5=AE=B9=E4=BF=AE=E6=94=B9=EF=BC=8Chutb=E5=9C=BA=E6=99=AF?= =?UTF-8?q?=E4=BA=BA=E8=BD=A6=E7=94=9F=E6=88=90=E9=80=BB=E8=BE=91=E4=BF=AE?= =?UTF-8?q?=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../collect_intersection_camera_lidar_hutb.py | 579 ++++++++++++++---- waypoint_control/multi_obj_track/config.m | 4 +- waypoint_control/multi_obj_track/config.py | 14 +- waypoint_control/twin_paper/main.tex | 35 +- 4 files changed, 476 insertions(+), 156 deletions(-) diff --git a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py index e6aad9e..02b575c 100644 --- a/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py +++ b/waypoint_control/multi_obj_track/collect_intersection_camera_lidar_hutb.py @@ -32,6 +32,7 @@ import multiprocessing import traceback import queue +import shutil from PIL import Image from fontTools.merge.util import current_time @@ -51,6 +52,8 @@ base_frame = None # openpcdet进行目标识别所用时间 extra_time = 0 +# yolo子进程所用时间 +yolo_time = 0 relativePose_to_egoVehicle = { "back_camera": [-7.00, 0.00, 2.62, -180.00, 0.00, 0.00], # 1 @@ -294,7 +297,8 @@ def dist(actor): vehicle_id = vehicle.id vehicle_labels.append((time_stamp, vehicle_id, label)) if not vehicle_list: - raise RuntimeError(f"时间戳 {time_stamp} 处未检测到任何车辆,数据可能不完整。") + vehicle_labels.append((time_stamp, None, None)) + # raise RuntimeError(f"时间戳 {time_stamp} 处未检测到任何车辆,数据可能不完整。") all_vehicle_labels.append(vehicle_labels) @@ -361,7 +365,8 @@ def dist(actor): pedestrian_id = pedestrian.id pedestrian_labels.append((time_stamp, pedestrian_id, label)) if not pedestrian_list: - raise RuntimeError(f"时间戳 {time_stamp} 处未检测到任何行人,数据可能不完整。") + pedestrian_labels.append((time_stamp, None, None)) + # raise RuntimeError(f"时间戳 {time_stamp} 处未检测到任何行人,数据可能不完整。") all_pedestrian_labels.append(pedestrian_labels) # 将所有类别的标签合并到一个列表中 @@ -701,7 +706,7 @@ def clear_folder_contents(folder_path): print(f"文件夹内容已清空: {folder_path}") # 定义函数来保存相机图像数据 -def save_camera_data(image_data, camera_id, junc, town_folder, model, sensors, num, pca_model, quantize_scale, resnet_model, img_preprocess, compute_device): +def save_camera_data(image_data, camera_id, junc, town_folder, model, sensors, num, pca_model, quantize_scale, resnet_model, img_preprocess, compute_device, input_queue, output_queue, p): global base_frame current_frame = image_data.frame # 如果是第一帧,就把它的 ID 存为基数 @@ -715,17 +720,19 @@ def save_camera_data(image_data, camera_id, junc, town_folder, model, sensors, n image = np.array(image_data.raw_data) image = image.reshape((image_data.height, image_data.width, 4)) # 4th channel is alpha image = image[:, :, :3] # 去掉 alpha 通道,只保留 RGB + # 记录额外时间 + # 使用yolov8检测图片 # 设置为 spawn 启动模式 - multiprocessing.set_start_method("spawn", force=True) - - input_queue = multiprocessing.Queue() - output_queue = multiprocessing.Queue() - - p = multiprocessing.Process( - target=yolo_worker, args=(input_queue, output_queue) - ) - p.start() + # multiprocessing.set_start_method("spawn", force=True) + # + # input_queue = multiprocessing.Queue() + # output_queue = multiprocessing.Queue() + # + # p = multiprocessing.Process( + # target=yolo_worker, args=(input_queue, output_queue) + # ) + # p.start() # 发送给 YOLO 子进程 input_queue.put((1, image)) @@ -753,8 +760,8 @@ def save_camera_data(image_data, camera_id, junc, town_folder, model, sensors, n print("【警告】YOLO 处理超时,但子进程仍存活,跳过此帧。") # 结束进程 - input_queue.put(None) - p.join() + # input_queue.put(None) + # p.join() # 将字典包装为 SimpleResults 对象 results = [SimpleResults(d) for d in yolo_data] @@ -961,79 +968,145 @@ def filter_vehicle_blueprinter(vehicle_blueprints): # 生成自动驾驶车辆 -def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): - # 设置随机种子 +# def spawn_autonomous_vehicles(world, tm, num_vehicles=30, random_seed=42): +# # 设置随机种子 +# random.seed(random_seed) +# np.random.seed(random_seed) +# tm.set_random_device_seed(random_seed) +# vehicle_list = [] +# blueprint_library = world.get_blueprint_library() +# vehicle_blueprints = blueprint_library.filter('vehicle.*') +# filter_vehicle_blueprints = filter_vehicle_blueprinter(vehicle_blueprints) +# # 随机选择一个位置 +# spawn_points = world.get_map().get_spawn_points() +# if len(spawn_points) == 0: +# print("No spawn points available!") +# return [] +# +# # 如果蓝图不足,使用颜色来区分 +# num_blueprints = len(filter_vehicle_blueprints) +# num_colors = 12 +# available_colors = ["255,0,0", "0,255,0", "0,0,255", "255,255,0", "0,255,255", "255,0,255", "128,128,0", +# "128,0,128", "0,128,128", "255,165,0", "0,255,255", "255,192,203"] +# # 生成车辆 +# vehicle_index = 0 +# for _ in range(num_vehicles): +# # 选择一个随机位置生成车辆 +# transform = spawn_points[np.random.randint(len(spawn_points))] +# # vehicle_bp = random.choice(filter_vehicle_blueprints) +# # 选择蓝图,确保每个蓝图的车辆唯一 +# # if vehicle_index < num_blueprints: +# # vehicle_bp = filter_vehicle_blueprints[vehicle_index] +# # vehicle_index += 1 +# # else: +# # # 蓝图用完后,开始使用颜色来区分 +# # vehicle_bp = filter_vehicle_blueprints[vehicle_index % num_blueprints] +# # color = available_colors[vehicle_index % num_colors] +# # vehicle_bp.set_attribute('color', color) +# # vehicle_index += 1 +# +# if vehicle_index < num_blueprints: +# vehicle_bp = filter_vehicle_blueprints[vehicle_index] +# vehicle_index += 1 +# else: +# found = False +# attempts = 0 +# while attempts < num_blueprints: +# candidate_idx = vehicle_index % num_blueprints +# candidate_bp = filter_vehicle_blueprints[candidate_idx] +# if candidate_bp.has_attribute('color'): +# vehicle_bp = candidate_bp +# color = available_colors[vehicle_index % num_colors] +# vehicle_bp.set_attribute('color', color) # 设置颜色 +# vehicle_index += 1 +# found = True +# break +# else: +# vehicle_index += 1 +# attempts += 1 +# if not found: +# # 实在找不到支持颜色的蓝图,使用当前蓝图且不设颜色 +# print("警告:无支持颜色的蓝图可用,使用默认外观") +# vehicle_bp = filter_vehicle_blueprints[vehicle_index % num_blueprints] +# vehicle_index += 1 +# +# vehicle = world.try_spawn_actor(vehicle_bp, transform) +# if vehicle is None: +# continue +# # 配置自动驾驶 +# # vehicle.set_autopilot(True) # 启动自动驾驶模式 +# vehicle.set_autopilot(True, tm.get_port()) # 启动自动驾驶模式 +# # 不考虑交通灯 +# tm.ignore_lights_percentage(vehicle, 100) +# vehicle_list.append(vehicle) +# print(f"Spawned vehicle: {vehicle.id}") +# +# return vehicle_list +def spawn_autonomous_vehicles(world, tm, junction_weights, num_vehicles, random_seed=42): random.seed(random_seed) np.random.seed(random_seed) - tm.set_random_device_seed(random_seed) vehicle_list = [] - blueprint_library = world.get_blueprint_library() - vehicle_blueprints = blueprint_library.filter('vehicle.*') - filter_vehicle_blueprints = filter_vehicle_blueprinter(vehicle_blueprints) - # 随机选择一个位置 - spawn_points = world.get_map().get_spawn_points() - if len(spawn_points) == 0: - print("No spawn points available!") + + carla_map = world.get_map() + filter_bike_blueprinter = filter_vehicle_blueprinter(world.get_blueprint_library().filter('vehicle.*')) + all_waypoints = carla_map.generate_waypoints(distance=2.0) + + # 提取各个路口外的入口点 + junction_entries = {jid: [] for jid in junction_weights.keys()} + processed_jids = set() + + for wp in all_waypoints: + if wp.is_junction and wp.junction_id in junction_weights: + jid = wp.junction_id + if jid not in processed_jids: + processed_jids.add(jid) + junction = wp.get_junction() + waypoint_pairs = junction.get_waypoints(carla.LaneType.Driving) + for entry_wp, _ in waypoint_pairs: + outside_wps = entry_wp.previous(15.0) + if outside_wps: + junction_entries[jid].append(outside_wps[0]) + + valid_jids = {jid: w for jid, w in junction_weights.items() if len(junction_entries[jid]) > 0} + if not valid_jids: + print("警告: 未检测到有效的路口入口点!") return [] - # 如果蓝图不足,使用颜色来区分 - num_blueprints = len(filter_vehicle_blueprints) - num_colors = 12 - available_colors = ["255,0,0", "0,255,0", "0,0,255", "255,255,0", "0,255,255", "255,0,255", "128,128,0", - "128,0,128", "0,128,128", "255,165,0", "0,255,255", "255,192,203"] - # 生成车辆 - vehicle_index = 0 - for _ in range(num_vehicles): - # 选择一个随机位置生成车辆 - transform = spawn_points[np.random.randint(len(spawn_points))] - # vehicle_bp = random.choice(filter_vehicle_blueprints) - # 选择蓝图,确保每个蓝图的车辆唯一 - # if vehicle_index < num_blueprints: - # vehicle_bp = filter_vehicle_blueprints[vehicle_index] - # vehicle_index += 1 - # else: - # # 蓝图用完后,开始使用颜色来区分 - # vehicle_bp = filter_vehicle_blueprints[vehicle_index % num_blueprints] - # color = available_colors[vehicle_index % num_colors] - # vehicle_bp.set_attribute('color', color) - # vehicle_index += 1 - - if vehicle_index < num_blueprints: - vehicle_bp = filter_vehicle_blueprints[vehicle_index] - vehicle_index += 1 - else: - found = False - attempts = 0 - while attempts < num_blueprints: - candidate_idx = vehicle_index % num_blueprints - candidate_bp = filter_vehicle_blueprints[candidate_idx] - if candidate_bp.has_attribute('color'): - vehicle_bp = candidate_bp - color = available_colors[vehicle_index % num_colors] - vehicle_bp.set_attribute('color', color) # 设置颜色 - vehicle_index += 1 - found = True - break - else: - vehicle_index += 1 - attempts += 1 - if not found: - # 实在找不到支持颜色的蓝图,使用当前蓝图且不设颜色 - print("警告:无支持颜色的蓝图可用,使用默认外观") - vehicle_bp = filter_vehicle_blueprints[vehicle_index % num_blueprints] - vehicle_index += 1 - - vehicle = world.try_spawn_actor(vehicle_bp, transform) - if vehicle is None: + # 按加权计算各路口车辆配额 + total_weight = sum(valid_jids.values()) + normalized_weights = {jid: w / total_weight for jid, w in valid_jids.items()} + + assigned_counts = {} + remaining_vehicles = num_vehicles + for jid, weight in normalized_weights.items(): + count = int(num_vehicles * weight) + assigned_counts[jid] = count + remaining_vehicles -= count + + if remaining_vehicles > 0: + assigned_counts[max(normalized_weights, key=normalized_weights.get)] += remaining_vehicles + + # 按配额生成车辆 + for start_jid, count in assigned_counts.items(): + entry_points = junction_entries[start_jid] + if not entry_points: continue - # 配置自动驾驶 - # vehicle.set_autopilot(True) # 启动自动驾驶模式 - vehicle.set_autopilot(True, tm.get_port()) # 启动自动驾驶模式 - # 不考虑交通灯 - tm.ignore_lights_percentage(vehicle, 100) - vehicle_list.append(vehicle) - print(f"Spawned vehicle: {vehicle.id}") + for _ in range(count): + start_wp = random.choice(entry_points) + transform = start_wp.transform + transform.location.z += 0.5 + + vehicle_bp = random.choice(filter_bike_blueprinter) + vehicle = world.try_spawn_actor(vehicle_bp, transform) + + if vehicle: + vehicle.set_autopilot(True) + tm.ignore_lights_percentage(vehicle, 100) + # 已移除 GlobalRoutePlanner 和 tm.set_path 跨路口导航逻辑 + vehicle_list.append(vehicle) + + print(f"成功按加权在各路口外生成了 {len(vehicle_list)} 辆车(本地行驶,无跨路口导航)。") return vehicle_list def create_pedestrian_generator(world, seed=42): @@ -1053,12 +1126,146 @@ def create_pedestrian_generator(world, seed=42): yield bp -def get_or_create_pedestrian_script(world, num_pedestrians=150, filepath="HutbCarlaCity_pedestrians_generates_trajectory.json", seed=2024): +# def get_or_create_pedestrian_script( +# world, +# num_pedestrians=50, +# filepath="HutbCarlaCity_pedestrians_generates_trajectory.json", +# seed=2024, +# target_junction_ids=[2121, 398, 576, 626, 510], #目标路口ID列表 +# radius=35.0, # 新增:在路口中心多少米范围内生成 +# min_distance=15.0 # 修改:由于是局部生成,起终点距离要求应适当调小,原为30 +# ): +# if os.path.exists(filepath): +# with open(filepath, 'r') as f: +# raw_script = json.load(f) +# +# # 确保加载的非空 json 才会直接返回 +# if len(raw_script) > 0: +# print(f" 已找到轨迹 ") +# final_script = [] +# for item in raw_script: +# spawn_loc = carla.Location(x=item["spawn_x"], y=item["spawn_y"], z=item["spawn_z"]) +# dest_loc = carla.Location(x=item["dest_x"], y=item["dest_y"], z=item["dest_z"]) +# final_script.append({ +# "spawn_point": carla.Transform(spawn_loc), +# "destination": dest_loc, +# "speed": item["speed"] +# }) +# print(f" 成功加载 {len(final_script)} 名行人的运动轨迹!") +# return final_script +# +# # ---- 针对自定义地图的无缝兼容逻辑 ---- +# carla_map = world.get_map() +# all_wps = carla_map.generate_waypoints(distance=2.0) +# +# # 1. 解析指定路口 ID 的中心坐标 +# junction_centers = [] +# if target_junction_ids: +# seen_ids = set() +# for wp in all_wps: +# if wp.is_junction: +# j = wp.get_junction() +# if j.id in target_junction_ids and j.id not in seen_ids: +# junction_centers.append(j.bounding_box.location) +# seen_ids.add(j.id) +# if not junction_centers: +# print(f" 警告:未能在地图中找到指定的路口 ID {target_junction_ids},将退回全图生成。") +# +# # 2. 精确筛选位于目标路口范围内的人行道 Waypoint +# sidewalk_wps = [wp for wp in all_wps if wp.lane_type == carla.LaneType.Sidewalk] +# base_wps = sidewalk_wps if len(sidewalk_wps) > 0 else all_wps +# +# if junction_centers: +# # 只保留在任何一个目标路口半径范围内的人行道点 +# target_wps = [ +# wp for wp in base_wps +# if any(wp.transform.location.distance(center) <= radius for center in junction_centers) +# ] +# if len(target_wps) == 0: +# print(f" 警告:在指定路口 {radius} 米范围内没找到人行道,自动扩大搜索半径至 {radius * 1.5} 米...") +# target_wps = [ +# wp for wp in base_wps +# if any(wp.transform.location.distance(center) <= radius * 1.5 for center in junction_centers) +# ] +# else: +# target_wps = base_wps +# +# def get_location(rng): +# # 如果是全图生成,优先使用 Navmesh 以保证全图多样性 +# if not junction_centers: +# loc = world.get_random_location_from_navigation() +# if loc is not None: +# return loc +# +# # 如果限制在特定路口,全局 Navmesh 采样命中率太低 +# # 直接使用预先筛选的绝对合法的人行道 Waypoint (目标路口范围内) +# if len(target_wps) > 0: +# wp = rng.choice(target_wps) +# # 添加微小的 xy 随机偏移,避免行人完全排成一条直线 +# offset_x = rng.uniform(-0.5, 0.5) +# offset_y = rng.uniform(-0.5, 0.5) +# loc = wp.transform.location +# return carla.Location(x=loc.x + offset_x, y=loc.y + offset_y, z=loc.z + 0.2) +# +# return None +# +# # ------------------------------------ +# +# local_rng = random.Random(seed) +# raw_script = [] +# final_script = [] +# generated_count = 0 +# +# max_attempts = num_pedestrians * 100 +# +# for attempt in range(max_attempts): +# if generated_count >= num_pedestrians: +# break +# +# spawn_point = get_location(local_rng) +# destination = get_location(local_rng) +# +# if spawn_point is not None and destination is not None: +# distance = spawn_point.distance(destination) +# +# # 使用动态的 min_distance(局部路口生成时距离不能要求太大) +# if distance >= min_distance: +# speed = round(local_rng.uniform(1.1, 1.5), 2) +# +# raw_script.append({ +# "spawn_x": spawn_point.x, "spawn_y": spawn_point.y, "spawn_z": spawn_point.z, +# "dest_x": destination.x, "dest_y": destination.y, "dest_z": destination.z, +# "speed": speed +# }) +# +# final_script.append({ +# "spawn_point": carla.Transform(spawn_point), +# "destination": destination, +# "speed": speed +# }) +# generated_count += 1 +# +# if generated_count < num_pedestrians: +# print(f"只找到了 {generated_count} 条大于 {min_distance} 米的路线。") +# +# with open(filepath, 'w') as f: +# json.dump(raw_script, f, indent=4) +# +# print(f"轨迹生成完毕!已将 {generated_count} 名行人的轨迹保存在 {filepath}!") +# return final_script +def get_or_create_pedestrian_script( + world, + num_pedestrians=50, + filepath="HutbCarlaCity_pedestrians_generates_trajectory.json", + seed=2024, + target_junction_ids=[2121, 398, 576, 626, 510], # 目标路口ID列表 + radius=35.0, # 在路口中心多少米范围内生成(起点) + min_distance=30.0 # 终点全图随机,距离可以要求大一点 +): if os.path.exists(filepath): with open(filepath, 'r') as f: raw_script = json.load(f) - # 确保加载的非空 json 才会直接返回 if len(raw_script) > 0: print(f" 已找到轨迹 ") final_script = [] @@ -1073,39 +1280,96 @@ def get_or_create_pedestrian_script(world, num_pedestrians=150, filepath="HutbCa print(f" 成功加载 {len(final_script)} 名行人的运动轨迹!") return final_script - # ---- 针对自定义地图的无缝兼容逻辑 ---- carla_map = world.get_map() all_wps = carla_map.generate_waypoints(distance=2.0) + + # 获取目标路口中心并加入提前跳出优化 + junction_centers = {} + if target_junction_ids: + seen_ids = set() + target_count = len(set(target_junction_ids)) + for wp in all_wps: + if wp.is_junction: + j = wp.get_junction() + if j.id in target_junction_ids and j.id not in seen_ids: + junction_centers[j.id] = j.bounding_box.location + seen_ids.add(j.id) + if len(seen_ids) == target_count: + break + if not junction_centers: + print(f" 警告:未能在地图中找到指定的路口 ID,将退回全图生成。") + + # 将人行道点按路口ID分类建档字典 sidewalk_wps = [wp for wp in all_wps if wp.lane_type == carla.LaneType.Sidewalk] - target_wps = sidewalk_wps if len(sidewalk_wps) > 0 else all_wps + base_wps = sidewalk_wps if len(sidewalk_wps) > 0 else all_wps + + junction_wps_dict = {} # 字典:{ 路口ID : [可用Waypoint列表] } + valid_junction_ids = [] # 记录真正有合法生成点的路口ID + + if junction_centers: + for j_id, center in junction_centers.items(): + # 筛选只属于当前路口半径内的点 + wps_in_radius = [wp for wp in base_wps if wp.transform.location.distance(center) <= radius] - def get_location(rng): - # 优先使用 Navmesh,如果自定义地图返回 None,则从人行道 Waypoint 随机抽取 + # 防错:如果当前路口找不到点,扩大该路口的搜索范围 + if len(wps_in_radius) == 0: + print(f" 路口 {j_id} 在 {radius} 米内无点,扩大半径至 {radius * 1.5} 米...") + wps_in_radius = [wp for wp in base_wps if wp.transform.location.distance(center) <= radius * 1.5] + + if len(wps_in_radius) > 0: + junction_wps_dict[j_id] = wps_in_radius + valid_junction_ids.append(j_id) + else: + print(f" 路口 {j_id} 附近没有可用人行道,将被忽略分配。") + + # 独立起点与终点生成函数 + def get_spawn_location(rng, target_j_id): + """严格在指定的特定路口生成起点""" + if valid_junction_ids and target_j_id in junction_wps_dict: + wp = rng.choice(junction_wps_dict[target_j_id]) + offset_x = rng.uniform(-0.5, 0.5) + offset_y = rng.uniform(-0.5, 0.5) + loc = wp.transform.location + return carla.Location(x=loc.x + offset_x, y=loc.y + offset_y, z=loc.z + 0.2) + # 降级:全图随机 loc = world.get_random_location_from_navigation() - if loc is None and len(target_wps) > 0: - loc = rng.choice(target_wps).transform.location + carla.Location(z=0.2) - return loc + return loc if loc else (rng.choice(base_wps).transform.location + carla.Location(z=0.2)) + + def get_destination_location(rng): + """全图随机找终点,确保能散开且距离达标""" + loc = world.get_random_location_from_navigation() + if loc is not None: + return loc + return rng.choice(base_wps).transform.location + carla.Location(z=0.2) + # ------------------------------------ - MIN_DISTANCE = 30 local_rng = random.Random(seed) raw_script = [] final_script = [] generated_count = 0 - max_attempts = num_pedestrians * 100 + # 打印统计日志 + distribution_stats = {j_id: 0 for j_id in valid_junction_ids} + for attempt in range(max_attempts): if generated_count >= num_pedestrians: break - spawn_point = get_location(local_rng) - destination = get_location(local_rng) + # 轮询机制:决定当前这个人属于哪个路口 + if valid_junction_ids: + current_j_id = valid_junction_ids[generated_count % len(valid_junction_ids)] + else: + current_j_id = None # 降级全图 + + spawn_point = get_spawn_location(local_rng, current_j_id) + destination = get_destination_location(local_rng) if spawn_point is not None and destination is not None: distance = spawn_point.distance(destination) - if distance >= MIN_DISTANCE: + if distance >= min_distance: speed = round(local_rng.uniform(1.1, 1.5), 2) raw_script.append({ @@ -1119,19 +1383,27 @@ def get_location(rng): "destination": destination, "speed": speed }) + + # 记录成功生成的数量 generated_count += 1 + if current_j_id is not None: + distribution_stats[current_j_id] += 1 if generated_count < num_pedestrians: - print(f"只找到了 {generated_count} 条大于 {MIN_DISTANCE} 米的路线。") + print(f"只找到了 {generated_count} 条大于 {min_distance} 米的路线。") with open(filepath, 'w') as f: json.dump(raw_script, f, indent=4) - print(f"轨迹生成完毕!已将 {generated_count} 名行人的轨迹保存在 {filepath}!") + print(f"轨迹生成完毕!已成功保存 {generated_count} 名行人的轨迹。") + if distribution_stats: + print(f" 路口均分统计: {distribution_stats}") + return final_script + # 生成随机运动行人 -def spawn_autonomous_pedestrians(world, num_pedestrians=150, random_seed=20): +def spawn_autonomous_pedestrians(world, num_pedestrians=50, random_seed=20): random.seed(random_seed) np.random.seed(random_seed) pedestrian_list = [] @@ -1177,6 +1449,7 @@ def spawn_autonomous_pedestrians(world, num_pedestrians=150, random_seed=20): return pedestrian_list + def spawn_v2x_sensors(world, lidar_transform, z_height=2.57): sensors = {} # 字典 @@ -1218,7 +1491,7 @@ def do_nothing(data): pass def spawn_v2x_receiver(world, quantize_scale, town_folder): - location = carla.Location(x=0, y=0, z=2.62) + location = carla.Location(x=75.5, y=-283, z=2.62) transform = carla.Transform(location, carla.Rotation(yaw=0)) # 获取传感器蓝图 @@ -1535,7 +1808,7 @@ def main(): argparser.add_argument( '-n', '--number-of-vehicles', metavar='N0', - default=200, + default=120, type=int, help='Number of vehicles (default: 30)') argparser.add_argument( @@ -1553,7 +1826,7 @@ def main(): argparser.add_argument( '-i', '--intersection', metavar='INTERSECTION', - default='road_intersection_1', # 默认路口 + default='road_intersection_3', # 默认路口 help='Name of the intersection within the town (default: road_intersection_1)' ) args = argparser.parse_args() @@ -1606,16 +1879,36 @@ def main(): quantize_scale = pca_data['quantize_scale'] print("模型加载完成!") + # 开启子进程 + # 设置为 spawn 启动模式 + multiprocessing.set_start_method("spawn", force=True) + + input_queue = multiprocessing.Queue() + output_queue = multiprocessing.Queue() + + p = multiprocessing.Process( + target=yolo_worker, args=(input_queue, output_queue) + ) + p.start() + try: # 设置随机种子 random_seed = 20 intersection_config = town_configurations[args.town][args.intersection] ego_transform = intersection_config.ego_vehicle_position camera_loc = intersection_config.camera_positions + # 定义 5 个路口 ID + target_junction_weights = { + 2121: 0.3, + 398: 0.2, + 576: 0.2, + 626: 0.1, + 510: 0.2 + } # 先生成自动驾驶车辆 - vehicles = spawn_autonomous_vehicles(world, tm, num_vehicles=args.number_of_vehicles, random_seed=random_seed) + vehicles = spawn_autonomous_vehicles(world, tm, target_junction_weights, num_vehicles=args.number_of_vehicles, random_seed=random_seed) # 生成随机运动行人 - pedestrians = spawn_autonomous_pedestrians(world, num_pedestrians=150, random_seed=20) + pedestrians = spawn_autonomous_pedestrians(world, num_pedestrians=50, random_seed=20) # 启动行人碰撞 for pedestrian in pedestrians: if "walker.pedestrian." in pedestrian.type_id: @@ -1698,7 +1991,7 @@ def main(): if "lidar" in sensor_name: # lidar数据 save_radar_data(data, world, ego_transform, actual_vehicle_num, actual_pedestrian_num, lidar_to_world_inv, all_vehicle_labels, all_pedestrian_labels, junc, town_folder, file_num, sensors, num) else: - save_camera_data(data, sensor_name, junc, town_folder, model, sensors, num, pca_model, quantize_scale, resnet_model, img_preprocess, compute_device) + save_camera_data(data, sensor_name, junc, town_folder, model, sensors, num, pca_model, quantize_scale, resnet_model, img_preprocess, compute_device, input_queue, output_queue, p) # time.sleep(0.05) folder_index += 1 system_uptime_end = time.time() - extra_time @@ -1722,17 +2015,37 @@ def main(): flattened_data = [item for sublist in all_vehicle_labels for item in sublist] processed_data = [] + # for entry in flattened_data: + # timestamp, vehicle_id, position_with_dims = entry + # x, y, z, length, width, height = position_with_dims + # position = (x, y, z) + # box = (length, width, height) + # processed_data.append({ + # 'Time': timestamp, + # 'TruthID': vehicle_id, + # 'Position': position, + # 'Box': box + # }) for entry in flattened_data: timestamp, vehicle_id, position_with_dims = entry - x, y, z, length, width, height = position_with_dims - position = (x, y, z) - box = (length, width, height) - processed_data.append({ - 'Time': timestamp, - 'TruthID': vehicle_id, - 'Position': position, - 'Box': box - }) + + # 判断是否为空帧 + if vehicle_id is None or position_with_dims is None: + # 空帧:直接把字段设为空列表 [] + processed_data.append( + {"Time": timestamp, "TruthID": [], "Position": [], "Box": []} + ) + else: + # 有车帧:安全解包 + x, y, z, length, width, height = position_with_dims + processed_data.append( + { + "Time": timestamp, + "TruthID": vehicle_id, + "Position": (x, y, z), + "Box": (length, width, height), + } + ) truths = np.array(processed_data, dtype=object) file_path = os.path.join(folder_name, "truths.mat") @@ -1789,17 +2102,37 @@ def main(): flattened_data = [item for sublist in all_pedestrian_labels for item in sublist] processed_data = [] + # for entry in flattened_data: + # timestamp, pedestrian_id, position_with_dims = entry + # x, y, z, length, width, height = position_with_dims + # position = (x, y, z) + # box = (length, width, height) + # processed_data.append({ + # 'Time': timestamp, + # 'TruthID': pedestrian_id, + # 'Position': position, + # 'Box': box + # }) for entry in flattened_data: timestamp, pedestrian_id, position_with_dims = entry - x, y, z, length, width, height = position_with_dims - position = (x, y, z) - box = (length, width, height) - processed_data.append({ - 'Time': timestamp, - 'TruthID': pedestrian_id, - 'Position': position, - 'Box': box - }) + + # 判断是否为空帧 + if pedestrian_id is None or position_with_dims is None: + # 空帧:直接把字段设为空列表 [] + processed_data.append( + {"Time": timestamp, "TruthID": [], "Position": [], "Box": []} + ) + else: + # 有车帧:安全解包 + x, y, z, length, width, height = position_with_dims + processed_data.append( + { + "Time": timestamp, + "TruthID": pedestrian_id, + "Position": (x, y, z), + "Box": (length, width, height), + } + ) truths = np.array(processed_data, dtype=object) file_path = os.path.join(folder_name, "truths.mat") @@ -1842,6 +2175,10 @@ def main(): destroy_actor(lidar, camera_dict, vehicles, sensor_queue, pedestrians) + + # 结束子进程 + input_queue.put(None) + p.join() except Exception as e: print(f"Error occurred during execution: {e}") finally: diff --git a/waypoint_control/multi_obj_track/config.m b/waypoint_control/multi_obj_track/config.m index ca9457c..03a1519 100644 --- a/waypoint_control/multi_obj_track/config.m +++ b/waypoint_control/multi_obj_track/config.m @@ -125,8 +125,8 @@ 'initialTime', 0.0, ... 'juncNum', 1, ... 'TransformationMatrix', [ - -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, -2.33000000e+02; - 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, -3.60000000e+01; + -4.37113883e-08, -1.00000000e+00, 0.00000000e+00, -2.83000000e+02; + 1.00000000e+00, -4.37113883e-08, -0.00000000e+00, 7.65000000e+00; 0.00000000e+00, -0.00000000e+00, 1.00000000e+00, 1.79999995e+00; 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00 ] ... diff --git a/waypoint_control/multi_obj_track/config.py b/waypoint_control/multi_obj_track/config.py index 4282788..9a8ce03 100644 --- a/waypoint_control/multi_obj_track/config.py +++ b/waypoint_control/multi_obj_track/config.py @@ -184,19 +184,19 @@ def __init__(self, ego_vehicle_position, camera_positions): }, "HutbCarlaCity": { "road_intersection_1": IntersectionConfig( - carla.Transform(carla.Location(x=-233, y=-36, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), + carla.Transform(carla.Location(x=-283, y=7.65, z=0.98), carla.Rotation(pitch=0, yaw=90, roll=0)), { - "back_camera": carla.Transform(carla.Location(x=-233, y=-43, z=3.6), + "back_camera": carla.Transform(carla.Location(x=-283, y=0.65, z=3.6), carla.Rotation(pitch=0, yaw=-90, roll=0)), - "front_camera": carla.Transform(carla.Location(x=-233, y=-29, z=3.6), + "front_camera": carla.Transform(carla.Location(x=-283, y=14.65, z=3.6), carla.Rotation(pitch=0, yaw=90, roll=0)), - "right_camera": carla.Transform(carla.Location(x=-237, y=-43, z=3.6), + "right_camera": carla.Transform(carla.Location(x=-287, y=0.65, z=3.6), carla.Rotation(pitch=0, yaw=-178, roll=0)), - "front_right_camera": carla.Transform(carla.Location(x=-237, y=-29, z=3.6), + "front_right_camera": carla.Transform(carla.Location(x=-287, y=14.65, z=3.6), carla.Rotation(pitch=0, yaw=-178, roll=0)), - "left_camera": carla.Transform(carla.Location(x=-229, y=-43, z=3.6), + "left_camera": carla.Transform(carla.Location(x=-279, y=0.65, z=3.6), carla.Rotation(pitch=0, yaw=-0, roll=0)), - "front_left_camera": carla.Transform(carla.Location(x=-229, y=-29, z=3.6), + "front_left_camera": carla.Transform(carla.Location(x=-279, y=14.65, z=3.6), carla.Rotation(pitch=0, yaw=-0, roll=0)) } ), diff --git a/waypoint_control/twin_paper/main.tex b/waypoint_control/twin_paper/main.tex index 4e484f5..26d972f 100644 --- a/waypoint_control/twin_paper/main.tex +++ b/waypoint_control/twin_paper/main.tex @@ -152,32 +152,15 @@ \section{Results}\label{sec2} \subsection{Twin system architecture} % 介绍我们这个孪生系统的组成结构,系统运行流程,侧重点在于系统的整体布局,突出整体性 % 介绍系统的运行流程 -This paper describes the design of an online digital twin system for an urban transportation scenario. -The system moves away from the traditional offline batch processing model and adopts a pipeline mechanism based on “real-time data collection and transmission, online inference, and streaming reconstruction.” Its workflow is primarily divided into three stages: data collection and processing, trajectory tracking and prediction, and twin trajectory reconstruction. - -During the data collection and processing phase, to obtain high-precision traffic information, we used LiDAR to capture 3D point cloud data and multi-view cameras to capture image data. -The collected raw point cloud and image data sets are massive and contain a large amount of redundant information, making direct transmission a significant challenge. -To alleviate this pressure, we perform preliminary processing on the data before it is transmitted. -We used the Point-Voxel Region-based Convolutional Neural Network (PV-RCNN) model to perform object detection on 3D point cloud data, obtaining 3D bounding boxes for vehicles and pedestrians. -PV-RCNN is a highly efficient two-stage 3D object detection network that maintains high detection accuracy for small-sized objects, such as pedestrians, while also delivering excellent inference speed. -Using the You Only Look Once version 8 (YOLOv8) model to perform object detection on image data, 2D bounding boxes for vehicles and pedestrians are obtained. -Image detection is located at the front end of the entire system workflow. YOLOv8’s extremely low inference latency ensures that no queuing bottlenecks occur, thereby improving system fluidity and reducing latency. -A 50-layer Residual Network (ResNet-50) model is used to extract features from image data, yielding feature values for vehicles and pedestrians. -ResNet-50 combines the ability to generate semantic representations applicable to both vehicles and pedestrians with fast inference speeds, enabling it to efficiently extract highly discriminative appearance features for heterogeneous targets such as vehicles and pedestrians while ensuring low latency. -To record the latency of data transmission, we used V2X communication devices to transmit bounding box coordinates and feature vectors, thereby ensuring the authenticity of the time attributes. - -During the trajectory tracking and prediction phase, we primarily focused on three tasks: single-intersection trajectory tracking, trajectory prediction in unknown areas, and cross-intersection trajectory correlation. -In single-intersection trajectory tracking, we use the Joint Probabilistic Data Association (JPDA) algorithm for multi-target tracking. -By using a probabilistic soft decision mechanism to weight and update target states, JPDA can effectively address issues such as frequent target occlusion, close-range overlap, and sensor clutter in high-density traffic flows, thereby significantly enhancing the robustness of the association process. -In trajectory prediction for unknown regions, we employ a fifth-degree polynomial trajectory inference method to infer a smooth trajectory for the unknown region based on the target’s current motion state and future boundary conditions. -The five-point polynomial is a classic analytical method that simultaneously satisfies constraints on the initial and final positions, velocity, and acceleration, balancing both the smoothness of the trajectory and computational efficiency. -In the process of correlating cross-intersection trajectories, we use the target’s feature values for matching, correlate trajectories from different intersections, and obtain cross-intersection trajectories. - -During the twin trajectory reproduction phase, we will reproduce the complete twin trajectory obtained in CARLA to create a digital twin. -Trajectories tracked across intersections inevitably contain minute high-frequency noise; if used directly to drive reconstruction, this can cause the target to exhibit non-physical visual oscillations in the digital twin scene. -To this end, this paper employs a trajectory smoothing algorithm to improve the geometric smoothness and kinematic continuity of the trajectory. -To ensure spatiotemporal consistency during trajectory reproduction while reducing the system’s computational overhead, this paper adopts a vector velocity-driven control strategy based on adjacent sampling frames. -The system sets the states of the current frame and the next frame as the starting point and ending point of the motion, respectively. Based on the inter-frame delay, it calculates the target’s vector velocity and orientation, thereby driving the target to move in segmented, uniform linear motion and accurately reproducing the target’s spatiotemporal trajectory. +This system consists of data collection nodes deployed at five major intersections (each equipped with one LiDAR, six cameras, and seven V2X devices) and a centralized server (equipped with one V2X device and the CARLA simulation platform). It operates using a pipeline mechanism that involves “real-time data collection and transmission, online inference, and streaming simulation.” +During operation, each collection endpoint first captures point clouds and images. After preprocessing using PV-RCNN, YOLOv8, and ResNet-50 to extract the targets’ 3D and 2D bounding boxes and appearance feature vectors, the data is transmitted to the server via V2X. +After the server has collected all the data, it uses the JPDA algorithm to perform multi-object tracking at a single intersection and infers trajectories in unknown areas using a fifth-degree polynomial, and then completes cross-intersection trajectory association based on feature matching. +Finally, the system smooths and denoises the associated trajectories and uses a vector velocity control strategy based on adjacent sampled frames to drive the target in segmented, constant-speed linear motion, thereby reproducing the spatio-temporally consistent twin trajectories with high precision in the CARLA virtual environment. + +We conducted experiments on four different maps to test the effectiveness of this system. +Among these, Town01 and Town10 are maps included in the CARLA simulation software; both feature standard road topologies, spatial divisions for mixed-use traffic (pedestrians and vehicles), and complete 3D environments that include buildings and traffic infrastructure. +HutbCarlaCity and Zhongdian are maps we created based on real-world road networks and environments; the former depicts Hunan University of Commerce and its surrounding areas, while the latter depicts the Changsha China Electronics Software Park and its surrounding areas. +We ran 1,200 frames on each map to simulate traffic conditions over a two-minute period, and used the overall average across five intersections to represent the final result for that map. \subsection{System physical layout and transmission latency}