Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Created KeypointsParser node. #1130

Open
wants to merge 6 commits into
base: v3_develop
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -312,6 +312,7 @@ set(TARGET_CORE_SOURCES
src/pipeline/node/AprilTag.cpp
src/pipeline/node/ObjectTracker.cpp
src/pipeline/node/IMU.cpp
src/pipeline/node/KeypointsParser.cpp
src/pipeline/node/EdgeDetector.cpp
src/pipeline/node/SPIIn.cpp
src/pipeline/node/FeatureTracker.cpp
Expand All @@ -328,6 +329,7 @@ set(TARGET_CORE_SOURCES
src/pipeline/datatype/Buffer.cpp
src/pipeline/datatype/ImgFrame.cpp
src/pipeline/datatype/ImgTransformations.cpp
src/pipeline/datatype/Keypoints.cpp
src/pipeline/datatype/EncodedFrame.cpp
src/pipeline/datatype/ImageManipConfig.cpp
src/pipeline/datatype/ImageManipConfigV2.cpp
Expand Down
2 changes: 2 additions & 0 deletions bindings/python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ set(SOURCE_LIST
src/pipeline/node/SpatialDetectionNetworkBindings.cpp
src/pipeline/node/ObjectTrackerBindings.cpp
src/pipeline/node/IMUBindings.cpp
src/pipeline/node/KeypointsParserBindings.cpp
src/pipeline/node/EdgeDetectorBindings.cpp
src/pipeline/node/FeatureTrackerBindings.cpp
src/pipeline/node/ToFBindings.cpp
Expand Down Expand Up @@ -126,6 +127,7 @@ set(SOURCE_LIST
src/pipeline/datatype/ImgFrameBindings.cpp
src/pipeline/datatype/EncodedFrameBindings.cpp
src/pipeline/datatype/IMUDataBindings.cpp
src/pipeline/datatype/KeypointsBindings.cpp
src/pipeline/datatype/MessageGroupBindings.cpp
src/pipeline/datatype/NNDataBindings.cpp
src/pipeline/datatype/SpatialImgDetectionsBindings.cpp
Expand Down
3 changes: 3 additions & 0 deletions bindings/python/src/DatatypeBindings.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@ void bind_imgdetections(pybind11::module& m, void* pCallstack);
void bind_imgframe(pybind11::module& m, void* pCallstack);
void bind_encodedframe(pybind11::module& m, void* pCallstack);
void bind_imudata(pybind11::module& m, void* pCallstack);
void bind_keypoints(pybind11::module& m, void* pCallstack);
void bind_message_group(pybind11::module& m, void* pCallstack);
void bind_nndata(pybind11::module& m, void* pCallstack);
void bind_spatialimgdetections(pybind11::module& m, void* pCallstack);
Expand Down Expand Up @@ -51,6 +52,7 @@ void DatatypeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
callstack.push_front(bind_imgframe);
callstack.push_front(bind_encodedframe);
callstack.push_front(bind_imudata);
callstack.push_front(bind_keypoints);
callstack.push_front(bind_message_group);
callstack.push_front(bind_nndata);
callstack.push_front(bind_spatialimgdetections);
Expand Down Expand Up @@ -96,6 +98,7 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack){
.value("ImageManipConfigV2", DatatypeEnum::ImageManipConfigV2)
.value("CameraControl", DatatypeEnum::CameraControl)
.value("ImgDetections", DatatypeEnum::ImgDetections)
.value("Keypoints", DatatypeEnum::Keypoints)
.value("SpatialImgDetections", DatatypeEnum::SpatialImgDetections)
.value("SystemInformation", DatatypeEnum::SystemInformation)
.value("SpatialLocationCalculatorConfig", DatatypeEnum::SpatialLocationCalculatorConfig)
Expand Down
58 changes: 58 additions & 0 deletions bindings/python/src/pipeline/datatype/KeypointsBindings.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
#include "DatatypeBindings.hpp"
#include "pipeline/CommonBindings.hpp"

// depthai
#include "depthai/pipeline/datatype/Keypoints.hpp"
//pybind
#include <pybind11/chrono.h>
#include <pybind11/stl.h>


void bind_keypoints(pybind11::module& m, void* pCallstack){

using namespace dai;

py::class_<Keypoints, Py<Keypoints>, Buffer, std::shared_ptr<Keypoints>> keypoints(m, "Keypoints", DOC(dai, Keypoints));
py::class_<Keypoint> keypoint(m, "Keypoint", DOC(dai, Keypoint));

///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
// Call the rest of the type defines, then perform the actual bindings
Callstack* callstack = (Callstack*) pCallstack;
auto cb = callstack->top();
callstack->pop();
cb(m, pCallstack);
// Actual bindings
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////

// Single point struct
keypoint
.def(py::init<>())
.def_readwrite("x", &Keypoint::x)
.def_readwrite("y", &Keypoint::y)
.def_readwrite("z", &Keypoint::z)
.def_readwrite("confidence", &Keypoint::confidence)
;

// Message
keypoints
.def(py::init<>())

// getters
.def("getTimestamp", &Keypoints::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp))
.def("getTimestampDevice", &Keypoints::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice))
.def("getSequenceNum", &Keypoints::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum))
.def("getKeypoints", &Keypoints::getKeypoints, DOC(dai, Keypoints, getKeypoints))

// setters
.def("setTimestamp", &Keypoints::Buffer::setTimestamp, py::arg("timestamp"), DOC(dai, Buffer, setTimestamp))
.def("setTimestampDevice", &Keypoints::Buffer::setTimestampDevice, DOC(dai, Buffer, setTimestampDevice))
.def("setSequenceNum", &Keypoints::Buffer::setSequenceNum, DOC(dai, Buffer, setSequenceNum))
// Binds only the overload that takes Keypoint objects
.def("setKeypoints", py::overload_cast<const std::vector<Keypoint>&>(&Keypoints::setKeypoints), DOC(dai, Keypoints, setKeypoints))
;

}
56 changes: 56 additions & 0 deletions bindings/python/src/pipeline/node/KeypointsParserBindings.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
#include "Common.hpp"
#include "NodeBindings.hpp"

#include "depthai/pipeline/Node.hpp"
#include "depthai/pipeline/Pipeline.hpp"
#include "depthai/properties/KeypointsParserProperties.hpp"
#include "depthai/pipeline/node/KeypointsParser.hpp"

void bind_keypointsparser(pybind11::module& m, void* pCallstack){

using namespace dai;
using namespace dai::node;

// Node and Properties declare upfront
py::class_<KeypointsParserProperties> keypointsParserProperties(m, "keypointsParserProperties", DOC(dai, KeypointsParserProperties));
auto keypointsParser = ADD_NODE(KeypointsParser);

///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
// Call the rest of the type defines, then perform the actual bindings
Callstack* callstack = (Callstack*) pCallstack;
auto cb = callstack->top();
callstack->pop();
cb(m, pCallstack);
// Actual bindings
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////

// Properties
keypointsParserProperties
.def_readwrite("scaleFactor", &KeypointsParserProperties::scaleFactor)
.def_readwrite("numKeypoints", &KeypointsParserProperties::numKeypoints)
;

// Node
keypointsParser
.def_readonly("input", &KeypointsParser::input, DOC(dai, node, KeypointsParser, input))
.def_readonly("out", &KeypointsParser::out, DOC(dai, node, KeypointsParser, out))

.def("build", &KeypointsParser::build, DOC(dai, node, KeypointsParser, build))

// getters
.def("runOnHost", &KeypointsParser::runOnHost, DOC(dai, node, KeypointsParser, runOnHost))
.def("getScaleFactor", &KeypointsParser::getScaleFactor, DOC(dai, node, KeypointsParser, getScaleFactor))
.def("getNumKeypoints", &KeypointsParser::getNumKeypoints, DOC(dai, node, KeypointsParser, getNumKeypoints))

// setters
.def("setRunOnHost", &KeypointsParser::setRunOnHost, DOC(dai, node, KeypointsParser, setRunOnHost))
.def("setScaleFactor", &KeypointsParser::setScaleFactor, DOC(dai, node, KeypointsParser, setScaleFactor))
.def("setNumKeypoints", &KeypointsParser::setNumKeypoints, DOC(dai, node, KeypointsParser, setNumKeypoints))
;
daiNodeModule.attr("KeypointsParser").attr("Properties") = keypointsParserProperties;

}
2 changes: 2 additions & 0 deletions bindings/python/src/pipeline/node/NodeBindings.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ void bind_spatiallocationcalculator(pybind11::module& m, void* pCallstack);
void bind_spatialdetectionnetwork(pybind11::module& m, void* pCallstack);
void bind_objecttracker(pybind11::module& m, void* pCallstack);
void bind_imu(pybind11::module& m, void* pCallstack);
void bind_keypointsparser(pybind11::module& m, void* pCallstack);
void bind_edgedetector(pybind11::module& m, void* pCallstack);
void bind_featuretracker(pybind11::module& m, void* pCallstack);
void bind_apriltag(pybind11::module& m, void* pCallstack);
Expand Down Expand Up @@ -192,6 +193,7 @@ void NodeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
callstack.push_front(bind_spatialdetectionnetwork);
callstack.push_front(bind_objecttracker);
callstack.push_front(bind_imu);
callstack.push_front(bind_keypointsparser);
callstack.push_front(bind_edgedetector);
callstack.push_front(bind_featuretracker);
callstack.push_front(bind_apriltag);
Expand Down
4 changes: 4 additions & 0 deletions examples/cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -483,6 +483,10 @@ dai_add_example(threaded_host_node HostNodes/threaded_host_node.cpp ON OFF)
# Model zoo
dai_add_example(model_zoo RVC2/ModelZoo/model_zoo.cpp ON OFF)

# Keypoints parser
dai_add_example(keypoints_device RVC2/parsers/keypoints_device.cpp ON OFF)
dai_add_example(keypoints_host RVC2/parsers/keypoints_host.cpp ON OFF)

if(DEPTHAI_RTABMAP_SUPPORT)
include(FetchContent)
FetchContent_Declare(rerun_sdk URL https://github.com/rerun-io/rerun/releases/download/0.16.1/rerun_cpp_sdk.zip)
Expand Down
49 changes: 49 additions & 0 deletions examples/cpp/RVC2/parsers/keypoints_device.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
#include <depthai/depthai.hpp>

int main() {
dai::NNModelDescription modelDescription;
modelDescription.modelSlug = "mediapipe-face-landmarker";
modelDescription.modelVersionSlug = "192x192";
modelDescription.platform = "RVC2";
std::string archivePath = dai::getModelFromZoo(modelDescription, true);
dai::NNArchive nnArchive(archivePath);

dai::Pipeline pipeline;

auto cam = pipeline.create<dai::node::Camera>()->build();
auto largeOutput = cam->requestOutput(std::pair(720, 720), dai::ImgFrame::Type::BGR888p);

auto manip = pipeline.create<dai::node::ImageManip>();
manip->initialConfig.setResize(192, 192);
largeOutput->link(manip->inputImage);

auto nn = pipeline.create<dai::node::NeuralNetwork>()->build(manip->out, nnArchive);

auto parser = pipeline.create<dai::node::KeypointsParser>()->build(nnArchive);
nn->out.link(parser->input);

auto videoQ = largeOutput->createOutputQueue();
auto keypointsQ = parser->out.createOutputQueue();

pipeline.start();

while(pipeline.isRunning()) {
auto frame = videoQ->get<dai::ImgFrame>();
auto cvFrame = frame->getCvFrame();
auto keypoints = keypointsQ->get<dai::Keypoints>()->keypoints;

for (auto keypoint : keypoints) {
int x = keypoint.x * frame->getWidth();
int y = keypoint.y * frame->getHeight();
cv::circle(cvFrame, cv::Point(x, y), 2, cv::Scalar(0, 255, 0), -1);
}

cv::imshow("Display", cvFrame);
auto key = cv::waitKey(1);
if(key == 'q' || key == 'Q') {
break;
}
}

return 0;
}
50 changes: 50 additions & 0 deletions examples/cpp/RVC2/parsers/keypoints_host.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,50 @@
#include <depthai/depthai.hpp>

int main() {
dai::NNModelDescription modelDescription;
modelDescription.modelSlug = "mediapipe-face-landmarker";
modelDescription.modelVersionSlug = "192x192";
modelDescription.platform = "RVC2";
std::string archivePath = dai::getModelFromZoo(modelDescription, true);
dai::NNArchive nnArchive(archivePath);

dai::Pipeline pipeline;

auto cam = pipeline.create<dai::node::Camera>()->build();
auto largeOutput = cam->requestOutput(std::pair(720, 720), dai::ImgFrame::Type::BGR888p);

auto manip = pipeline.create<dai::node::ImageManip>();
manip->initialConfig.setResize(192, 192);
largeOutput->link(manip->inputImage);

auto nn = pipeline.create<dai::node::NeuralNetwork>()->build(manip->out, nnArchive);

auto parser = pipeline.create<dai::node::KeypointsParser>()->build(nnArchive);
parser->setRunOnHost(true);
nn->out.link(parser->input);

auto videoQ = largeOutput->createOutputQueue();
auto keypointsQ = parser->out.createOutputQueue();

pipeline.start();

while(pipeline.isRunning()) {
auto frame = videoQ->get<dai::ImgFrame>();
auto cvFrame = frame->getCvFrame();
auto keypoints = keypointsQ->get<dai::Keypoints>()->keypoints;

for (auto keypoint : keypoints) {
int x = keypoint.x * frame->getWidth();
int y = keypoint.y * frame->getHeight();
cv::circle(cvFrame, cv::Point(x, y), 2, cv::Scalar(0, 255, 0), -1);
}

cv::imshow("Display", cvFrame);
auto key = cv::waitKey(1);
if(key == 'q' || key == 'Q') {
break;
}
}

return 0;
}
46 changes: 46 additions & 0 deletions examples/python/RVC2/parsers/keypoints.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
import depthai as dai

import cv2

RUN_ON_HOST = True

modelDescription = dai.NNModelDescription(modelSlug="mediapipe-face-landmarker", modelVersionSlug="192x192", platform="RVC2")
archivePath = dai.getModelFromZoo(modelDescription, useCached=True)
nnArchive = dai.NNArchive(archivePath)

with dai.Pipeline() as pipeline:

print("Creating pipeline...")
cam = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A)
full = cam.requestOutput((720, 720), dai.ImgFrame.Type.BGR888p)

manip = pipeline.create(dai.node.ImageManip)
manip.initialConfig.setResize(192, 192)
full.link(manip.inputImage)

nn = pipeline.create(dai.node.NeuralNetwork).build(
input=manip.out,
nnArchive=nnArchive
)

parser = pipeline.create(dai.node.KeypointsParser).build(nnArchive)
parser.setRunOnHost(RUN_ON_HOST)
nn.out.link(parser.input)

video_q = full.createOutputQueue()
keypoints_q = parser.out.createOutputQueue()

pipeline.start()

while pipeline.isRunning():
frame = video_q.get().getCvFrame()
keypoints: dai.Keypoints = keypoints_q.get()

for keypoint in keypoints.getKeypoints():
x, y = keypoint.x, keypoint.y
x, y = int(x * frame.shape[1]), int(y * frame.shape[0])
frame = cv2.circle(frame, (int(x), int(y)), 2, (0, 255, 0), -1)

cv2.imshow("Frame", frame)
if cv2.waitKey(1) == ord("q"):
break
1 change: 1 addition & 0 deletions include/depthai/pipeline/datatype/DatatypeEnum.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ enum class DatatypeEnum : std::int32_t {
AprilTags,
Tracklets,
IMUData,
Keypoints,
StereoDepthConfig,
FeatureTrackerConfig,
ToFConfig,
Expand Down
Loading