antonio-ardu
4 months ago
committed by
Lee
18 changed files with 506 additions and 574 deletions
@ -1,7 +1,14 @@
|
||||
build/ |
||||
.vscode/ |
||||
install/ |
||||
log/ |
||||
.xmake/ |
||||
|
||||
py_arducam/ |
||||
CMakeFiles/ |
||||
*.patch |
||||
|
||||
build/ |
||||
|
||||
/ros2_publisher/install/ |
||||
/ros2_publisher/log/ |
||||
/ros2_publisher/*.json |
||||
/example/python/*.json |
||||
|
||||
*.patch |
||||
|
||||
@ -0,0 +1,29 @@
|
||||
cmake_minimum_required(VERSION 3.4) |
||||
|
||||
project(PreviewPointcloud) |
||||
|
||||
add_compile_options(-Wno-psabi) |
||||
|
||||
find_package(Open3D QUIET) |
||||
find_package(OpenCV REQUIRED) |
||||
|
||||
set(CORE_LIBS ${CXXLIBS} Open3D::Open3D) |
||||
|
||||
set(WITH_OPENCV_WORLD OFF CACHE BOOL "with opencv_world") |
||||
if(WITH_OPENCV_WORLD) |
||||
set(CORE_LIBS ${CORE_LIBS} opencv_world) |
||||
else() |
||||
set(CORE_LIBS ${CORE_LIBS} ${OpenCV_LIBS}) |
||||
endif() |
||||
|
||||
if(Open3D_FOUND) |
||||
add_executable(preview_pointcloud preview_pointcloud.cpp) |
||||
target_include_directories(preview_pointcloud PRIVATE |
||||
${OpenCV_INCLUDE_DIRS} |
||||
) |
||||
target_link_libraries(preview_pointcloud PRIVATE |
||||
${CORE_LIBS} |
||||
) |
||||
else() |
||||
message(WARNING "Open3D point cloud preview need libopen3d!") |
||||
endif() |
||||
@ -0,0 +1,240 @@
|
||||
#include "ArducamTOFCamera.hpp" |
||||
// #include <atomic>
|
||||
#include <chrono> |
||||
#include <fstream> |
||||
#include <iostream> |
||||
#include <opencv2/core.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/imgproc.hpp> |
||||
|
||||
#include <Eigen/Dense> |
||||
#include <open3d/Open3D.h> |
||||
|
||||
using namespace Arducam; |
||||
|
||||
// MAX_DISTANCE value modifiable is 2 or 4
|
||||
#define MAX_DISTANCE 4000 |
||||
|
||||
cv::Rect seletRect(0, 0, 0, 0); |
||||
cv::Rect followRect(0, 0, 0, 0); |
||||
int max_width = 640; |
||||
int max_height = 480; |
||||
int max_range = 0; |
||||
int confidence_value = 0; |
||||
// std::atomic<int> confidence_value{0};
|
||||
|
||||
void on_confidence_changed(int pos, void* userdata) |
||||
{ |
||||
// confidence_value = pos;
|
||||
} |
||||
|
||||
void display_fps(void) |
||||
{ |
||||
using std::chrono::high_resolution_clock; |
||||
using namespace std::literals; |
||||
static int count = 0; |
||||
static auto time_beg = high_resolution_clock::now(); |
||||
auto time_end = high_resolution_clock::now(); |
||||
++count; |
||||
auto duration_ms = (time_end - time_beg) / 1ms; |
||||
if (duration_ms >= 1000) { |
||||
std::cout << "fps:" << count << std::endl; |
||||
count = 0; |
||||
time_beg = time_end; |
||||
} |
||||
} |
||||
|
||||
void save_image(float* image, int width, int height) |
||||
{ |
||||
using namespace std::literals; |
||||
// filename = "depth_$width$_$height$_f32_$time.raw"
|
||||
auto now = std::chrono::system_clock::now().time_since_epoch() / 1ms; |
||||
std::string filename = |
||||
"depth_" + std::to_string(width) + "_" + std::to_string(height) + "_f32_" + std::to_string(now) + ".raw"; |
||||
std::ofstream file(filename, std::ios::binary); |
||||
file.write(reinterpret_cast<char*>(image), width * height * sizeof(float)); |
||||
file.close(); |
||||
} |
||||
|
||||
cv::Mat matRotateClockWise180(cv::Mat src) |
||||
{ |
||||
if (src.empty()) { |
||||
std::cerr << "RorateMat src is empty!"; |
||||
} |
||||
|
||||
flip(src, src, 0); |
||||
flip(src, src, 1); |
||||
return src; |
||||
} |
||||
|
||||
void getPreview(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) |
||||
{ |
||||
auto len = preview_ptr.rows * preview_ptr.cols; |
||||
for (int line = 0; line < preview_ptr.rows; line++) { |
||||
for (int col = 0; col < preview_ptr.cols; col++) { |
||||
if (amplitude_image_ptr.at<float>(line, col) < confidence_value) |
||||
preview_ptr.at<uint8_t>(line, col) = 255; |
||||
} |
||||
} |
||||
} |
||||
|
||||
void getPreviewRGB(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) |
||||
{ |
||||
preview_ptr.setTo(cv::Scalar(0, 0, 0), amplitude_image_ptr < confidence_value); |
||||
// cv::GaussianBlur(preview_ptr, preview_ptr, cv::Size(7, 7), 0);
|
||||
} |
||||
|
||||
void onMouse(int event, int x, int y, int flags, void* param) |
||||
{ |
||||
if (x < 4 || x > (max_width - 4) || y < 4 || y > (max_height - 4)) |
||||
return; |
||||
switch (event) { |
||||
case cv::EVENT_LBUTTONDOWN: |
||||
|
||||
break; |
||||
|
||||
case cv::EVENT_LBUTTONUP: |
||||
seletRect.x = x - 4 ? x - 4 : 0; |
||||
seletRect.y = y - 4 ? y - 4 : 0; |
||||
seletRect.width = 8; |
||||
seletRect.height = 8; |
||||
break; |
||||
default: |
||||
followRect.x = x - 4 ? x - 4 : 0; |
||||
followRect.y = y - 4 ? y - 4 : 0; |
||||
followRect.width = 8; |
||||
followRect.height = 8; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
int main() |
||||
{ |
||||
ArducamTOFCamera tof; |
||||
ArducamFrameBuffer* frame; |
||||
if (tof.open(Connection::CSI, 0)) { |
||||
std::cerr << "Failed to open camera" << std::endl; |
||||
return -1; |
||||
} |
||||
|
||||
if (tof.start(FrameType::DEPTH_FRAME)) { |
||||
std::cerr << "Failed to start camera" << std::endl; |
||||
return -1; |
||||
} |
||||
// Modify the range also to modify the MAX_DISTANCE
|
||||
tof.setControl(CameraCtrl::RANGE, MAX_DISTANCE); |
||||
tof.getControl(CameraCtrl::RANGE, &max_range); |
||||
auto info = tof.getCameraInfo(); |
||||
std::cout << "open camera with (" << info.width << "x" << info.height << ")" << std::endl; |
||||
|
||||
uint8_t* preview_ptr = new uint8_t[info.width * info.height * 2]; |
||||
cv::namedWindow("preview", cv::WINDOW_AUTOSIZE); |
||||
cv::setMouseCallback("preview", onMouse); |
||||
// if (info.device_type == Arducam::DeviceType::DEVICE_VGA) {
|
||||
// // only vga support confidence
|
||||
// cv::createTrackbar("confidence", "preview", NULL, 255, on_confidence_changed);
|
||||
// }
|
||||
|
||||
float fx = max_width / (2 * tan(0.5 * M_PI * 64.3 / 180)); // 640 / 2 / tan(0.5*64.3)
|
||||
float fy = max_height / (2 * tan(0.5 * M_PI * 50.4 / 180)); // 480 / 2 / tan(0.5*50.4)
|
||||
float cx = max_width / 2; |
||||
float cy = max_height / 2; |
||||
|
||||
Eigen::Matrix4d m = Eigen::Matrix4d::Identity(); |
||||
m << 1, 0, 0, 0,
|
||||
0, -1, 0, 0, |
||||
0, 0, -1, 0, |
||||
0, 0, 0, 1; |
||||
|
||||
open3d::camera::PinholeCameraIntrinsic camera_intrinsic(max_width, max_height, fx, fy, cx, cy); |
||||
auto pcd = std::make_shared<open3d::geometry::PointCloud>(); |
||||
open3d::visualization::Visualizer vis; |
||||
vis.CreateVisualizerWindow("Point Cloud", max_width, max_height); |
||||
bool first = true; |
||||
|
||||
for (;;) { |
||||
Arducam::FrameDataFormat format; |
||||
frame = tof.requestFrame(200); |
||||
if (frame == nullptr) { |
||||
continue; |
||||
} |
||||
frame->getFrameDataFormat(FrameType::DEPTH_FRAME, format); |
||||
std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl; |
||||
max_height = format.height; |
||||
max_width = format.width; |
||||
|
||||
float* depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); |
||||
float* confidence_ptr = (float*)frame->getData(FrameType::CONFIDENCE_FRAME); |
||||
// getPreview(preview_ptr, depth_ptr, confidence_ptr);
|
||||
|
||||
cv::Mat result_frame(format.height, format.width, CV_8U, preview_ptr); |
||||
cv::Mat depth_frame(format.height, format.width, CV_32F, depth_ptr); |
||||
cv::Mat confidence_frame(format.height, format.width, CV_32F, confidence_ptr); |
||||
|
||||
// depth_frame = matRotateClockWise180(depth_frame);
|
||||
// result_frame = matRotateClockWise180(result_frame);
|
||||
// confidence_frame = matRotateClockWise180(confidence_frame);
|
||||
depth_frame.convertTo(result_frame, CV_8U, 255.0 / 7000, 0); |
||||
|
||||
cv::applyColorMap(result_frame, result_frame, cv::COLORMAP_RAINBOW); |
||||
getPreviewRGB(result_frame, confidence_frame); |
||||
|
||||
// confidence_frame.convertTo(confidence_frame, CV_8U, 255.0 / 1024, 0);
|
||||
|
||||
// cv::imshow("confidence", confidence_frame);
|
||||
|
||||
cv::rectangle(result_frame, seletRect, cv::Scalar(0, 0, 0), 2); |
||||
cv::rectangle(result_frame, followRect, cv::Scalar(255, 255, 255), 1); |
||||
|
||||
std::cout << "select Rect distance: " << cv::mean(depth_frame(seletRect)).val[0] << std::endl; |
||||
|
||||
cv::imshow("preview", result_frame); |
||||
depth_frame.setTo(0, confidence_frame < confidence_value); |
||||
auto depth_image = open3d::geometry::Image(); |
||||
depth_image.Prepare(max_width, max_height, 1, 4); |
||||
|
||||
memcpy(depth_image.data_.data(), depth_frame.data, depth_image.data_.size()); |
||||
|
||||
auto curr_pcd = open3d::geometry::PointCloud::CreateFromDepthImage( |
||||
depth_image, camera_intrinsic, Eigen::Matrix4d::Identity(), 5000.0, 5000.0); |
||||
|
||||
// memcpy(pcd->points_.data(), curr_pcd->points_.data(), pcd->points_.size());
|
||||
pcd->points_ = curr_pcd->points_; |
||||
|
||||
pcd->Transform(m); |
||||
|
||||
if (first) { |
||||
vis.AddGeometry(pcd); |
||||
first = false; |
||||
} |
||||
|
||||
// auto voxel_down_pcd = pcd->VoxelDownSample(20);
|
||||
// auto tuple = voxel_down_pcd->RemoveStatisticalOutliers(20, 2.0);
|
||||
// auto inlier_cloud = voxel_down_pcd->SelectByIndex(std::get<std::vector<size_t>>(tuple));
|
||||
// pcd->points_ = inlier_cloud->points_;
|
||||
vis.UpdateGeometry(pcd); |
||||
vis.PollEvents(); |
||||
vis.UpdateRender(); |
||||
|
||||
auto key = cv::waitKey(1); |
||||
if (key == 27 || key == 'q') { |
||||
break; |
||||
} else if (key == 's') { |
||||
save_image(depth_ptr, format.width, format.height); |
||||
} |
||||
display_fps(); |
||||
tof.releaseFrame(frame); |
||||
} |
||||
|
||||
vis.DestroyVisualizerWindow(); |
||||
|
||||
if (tof.stop()) { |
||||
return -1; |
||||
} |
||||
|
||||
if (tof.close()) { |
||||
return -1; |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
@ -1,36 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.4) |
||||
|
||||
project(PreviewPointcloud) |
||||
|
||||
add_compile_options(-Wno-psabi) |
||||
find_package(PCL QUIET COMPONENTS common io visualization) |
||||
find_package(OpenCV REQUIRED) |
||||
|
||||
set(CORE_LIBS ${CXXLIBS} ${PCL_LIBRARIES}) |
||||
|
||||
set(WITH_OPENCV_WORLD OFF CACHE BOOL "with opencv_world") |
||||
if(WITH_OPENCV_WORLD) |
||||
set(CORE_LIBS ${CORE_LIBS} opencv_world) |
||||
else() |
||||
set(CORE_LIBS ${CORE_LIBS} ${OpenCV_LIBS}) |
||||
endif() |
||||
link_directories(${PCL_LIBRARY_DIRS}) |
||||
|
||||
if(PCL_FOUND) |
||||
message(STATUS "LIBPCL_LINK_LIBRARIES=${PCL_INCLUDE_DIRS}") |
||||
message(STATUS "EPOXY_LINK_LIBRARIES=${PCL_LIBRARY_DIRS}") |
||||
add_executable(preview_pointcloud preview_pointcloud.cpp) |
||||
target_include_directories(preview_pointcloud PRIVATE |
||||
${OpenCV_INCLUDE_DIRS} |
||||
${PCL_INCLUDE_DIRS} |
||||
) |
||||
target_compile_definitions(preview_pointcloud PRIVATE |
||||
${PCL_DEFINITIONS} |
||||
) |
||||
#-lboost_thread -lpthread |
||||
target_link_libraries(preview_pointcloud |
||||
${CORE_LIBS} |
||||
) |
||||
else() |
||||
message(WARNING "PCL point cloud preview need libpcl!") |
||||
endif() |
||||
@ -1,302 +0,0 @@
|
||||
#include "ArducamTOFCamera.hpp" |
||||
#include <ctime> |
||||
#include <iostream> |
||||
#include <opencv2/core.hpp> |
||||
#include <opencv2/highgui.hpp> |
||||
#include <opencv2/imgproc.hpp> |
||||
#include <pcl/io/pcd_io.h> |
||||
#include <pcl/point_types.h> |
||||
#include <pcl/visualization/cloud_viewer.h> |
||||
#if 0 |
||||
#include <boost/signals2/mutex.hpp> |
||||
#include <boost/thread/thread.hpp> |
||||
#endif |
||||
|
||||
using namespace Arducam; |
||||
|
||||
int max_width = 240; |
||||
int max_height = 180; |
||||
int max_range = 0; |
||||
|
||||
int confidence_value = 30; |
||||
void on_confidence_changed(int pos, void* userdata) |
||||
{ |
||||
//
|
||||
} |
||||
|
||||
#if 0 |
||||
boost::mutex updateModelMutex; |
||||
boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud) |
||||
{ |
||||
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); |
||||
viewer->addCoordinateSystem(1); |
||||
viewer->setCameraPosition(0, 0, 0, 0, 0, 0, 0, 0, -1); |
||||
viewer->setBackgroundColor(0, 0, 0); |
||||
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud"); |
||||
viewer->initCameraParameters(); |
||||
return (viewer); |
||||
} |
||||
|
||||
void viewerRunner(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer) |
||||
{ |
||||
while (!viewer->wasStopped()) |
||||
{ |
||||
viewer->spinOnce(100); |
||||
boost::this_thread::sleep(boost::posix_time::microseconds(100)); |
||||
} |
||||
} |
||||
#else |
||||
pcl::visualization::PCLVisualizer::Ptr simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud) |
||||
{ |
||||
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); |
||||
viewer->addCoordinateSystem(100); |
||||
viewer->setCameraPosition(0, 0, 0, 0, 0, 0, 0, 0, -1); |
||||
viewer->setBackgroundColor(0, 0, 0); |
||||
viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud"); |
||||
viewer->initCameraParameters(); |
||||
return (viewer); |
||||
} |
||||
#endif |
||||
|
||||
void getPreview(uint8_t* preview_ptr, float* depth_image_ptr, float* amplitude_image_ptr) |
||||
{ |
||||
const auto len = max_width * max_height; |
||||
for (int i = 0; i < len; i++) { |
||||
uint8_t mask = *(amplitude_image_ptr + i) > 30 ? 254 : 0; |
||||
float depth = ((1 - (*(depth_image_ptr + i) * 1000.f / max_range)) * 255); |
||||
uint8_t pixel = depth > 255 ? 255 : depth; |
||||
*(preview_ptr + i) = pixel & mask; |
||||
} |
||||
} |
||||
|
||||
void getPreview(cv::Mat preview_ptr, cv::Mat amplitude_image_ptr) |
||||
{ |
||||
auto len = preview_ptr.rows * preview_ptr.cols; |
||||
for (int line = 0; line < preview_ptr.rows; line++) { |
||||
for (int col = 0; col < preview_ptr.cols; col++) { |
||||
if (amplitude_image_ptr.at<float>(line, col) < confidence_value) |
||||
preview_ptr.at<uint8_t>(line, col) = 255; |
||||
} |
||||
} |
||||
} |
||||
|
||||
int main(int argc, char* argv[]) |
||||
{ |
||||
ArducamTOFCamera tof; |
||||
if (argc > 1) { |
||||
const char* cfg_path = argv[1]; |
||||
if (tof.openWithFile(cfg_path, 375e5)) { |
||||
std::cerr << "Failed to open camera with cfg: " << cfg_path << std::endl; |
||||
return -1; |
||||
} |
||||
} else { |
||||
if (tof.open(Connection::CSI)) { |
||||
std::cerr << "initialization failed" << std::endl; |
||||
return -1; |
||||
} |
||||
} |
||||
|
||||
auto info = tof.getCameraInfo(); |
||||
|
||||
if (info.device_type == DeviceType::DEVICE_VGA) { |
||||
if (1) { |
||||
tof.setControl(CameraCtrl::FMT_WIDTH, 640); |
||||
tof.setControl(CameraCtrl::FMT_HEIGHT, 480); |
||||
tof.setControl(CameraCtrl::MODE, to_int(TofWorkMode::DOUBLE_FREQ)); |
||||
} else { |
||||
tof.setControl(CameraCtrl::FMT_WIDTH, 640); |
||||
tof.setControl(CameraCtrl::FMT_HEIGHT, 480); |
||||
tof.setControl(CameraCtrl::MODE, to_int(TofWorkMode::SINGLE_FREQ)); |
||||
} |
||||
} |
||||
|
||||
if (tof.start(FrameType::DEPTH_FRAME)) { |
||||
std::cerr << "Failed to start camera" << std::endl; |
||||
return -1; |
||||
} |
||||
|
||||
tof.getControl(CameraCtrl::RANGE, &max_range); |
||||
info = tof.getCameraInfo(); |
||||
std::cout << "open camera with (" << info.width << "x" << info.height << ") with range " << max_range << std::endl; |
||||
// double x = 0, y = 0, z = 0, dx = 0, dy = 0, dz = 0;
|
||||
|
||||
cv::namedWindow("preview", cv::WINDOW_AUTOSIZE); |
||||
cv::createTrackbar("confidence", "preview", &confidence_value, 255, on_confidence_changed); |
||||
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); |
||||
|
||||
// cloud_ptr->width = num_points;
|
||||
// cloud_ptr->height = 1;
|
||||
// cloud_ptr->is_dense = true;
|
||||
// cloud_ptr->resize(num_points);
|
||||
vtkObject::GlobalWarningDisplayOff(); |
||||
#if 0 |
||||
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = simpleVis(cloud_ptr); |
||||
boost::thread vthread(&viewerRunner, viewer); |
||||
#else |
||||
pcl::visualization::PCLVisualizer::Ptr viewer = simpleVis(cloud_ptr); |
||||
#endif |
||||
for (;;) { |
||||
ArducamFrameBuffer* frame = tof.requestFrame(200); |
||||
if (frame != nullptr) { |
||||
float* depth_ptr = (float*)frame->getData(FrameType::DEPTH_FRAME); |
||||
float* amplitude_ptr = (float*)frame->getData(FrameType::CONFIDENCE_FRAME); |
||||
|
||||
Arducam::FrameDataFormat format; |
||||
frame->getFrameDataFormat(FrameType::DEPTH_FRAME, format); |
||||
std::cout << "frame: (" << format.width << "x" << format.height << ")" << std::endl; |
||||
max_height = format.height; |
||||
max_width = format.width; |
||||
|
||||
float fx = max_width / (2 * tan(0.5 * M_PI * 64.3 / 180)); // 640 / 2 / tan(0.5*64.3)
|
||||
float fy = max_height / (2 * tan(0.5 * M_PI * 50.4 / 180)); // 480 / 2 / tan(0.5*50.4)
|
||||
// const float fx = 509.128;
|
||||
// const float fy = 509.334;
|
||||
// getPreview(preview_ptr, depth_ptr, amplitude_ptr);
|
||||
|
||||
cv::Mat result_frame(format.height, format.width, CV_8U); |
||||
cv::Mat depth_frame(format.height, format.width, CV_32F, depth_ptr); |
||||
cv::Mat amplitude_frame(max_height, max_width, CV_32F, amplitude_ptr); |
||||
|
||||
depth_frame.convertTo(result_frame, CV_8U, 255.0 / max_range, 0); |
||||
getPreview(result_frame, amplitude_frame); |
||||
cv::applyColorMap(result_frame, result_frame, cv::COLORMAP_RAINBOW); |
||||
|
||||
// cv::resize(result_frame, result_frame, cv::Size(720, 540));
|
||||
cv::imshow("preview", result_frame); |
||||
cloud_ptr->clear(); |
||||
unsigned long int pos = 0; |
||||
for (int row_idx = 0; row_idx < max_height; row_idx++) |
||||
for (int col_idx = 0; col_idx < max_width; col_idx++, pos++) { |
||||
if (amplitude_ptr[pos] >= confidence_value) { |
||||
float zz = depth_ptr[pos]; |
||||
float xx = ((((max_width / 2) - col_idx)) / fx) * zz; |
||||
float yy = (((max_height / 2) - row_idx) / fy) * zz; |
||||
pcl::PointXYZ ptemp(xx, yy, zz); |
||||
cloud_ptr->points.push_back(ptemp); |
||||
} |
||||
// else
|
||||
// {
|
||||
// pcl::PointXYZ ptemp(0, 0, 0);
|
||||
// cloud_ptr->points.push_back(ptemp);
|
||||
// }
|
||||
} |
||||
cloud_ptr->width = cloud_ptr->points.size(); |
||||
cloud_ptr->height = 1; |
||||
cloud_ptr->is_dense = true; |
||||
tof.releaseFrame(frame); |
||||
#if 0 |
||||
boost::mutex::scoped_lock updateLock(updateModelMutex); |
||||
viewer->updatePointCloud<pcl::PointXYZ>(cloud_ptr, "sample cloud"); |
||||
updateLock.unlock(); |
||||
#else |
||||
// viewer->setCameraPosition(x, y, z, dx, dy, dz, 0, 0, 0);
|
||||
viewer->updatePointCloud<pcl::PointXYZ>(cloud_ptr, "sample cloud"); |
||||
try { |
||||
viewer->spinOnce(50); |
||||
} catch (...) { |
||||
std::cerr << __FILE__ << '[' << __LINE__ << "]: " << __FUNCTION__ |
||||
<< "viewer->spinOnce(100); Segmentation fault,You can refer to " |
||||
"https://github.com/PointCloudLibrary/pcl/issues/5189 to solve the problem" |
||||
<< std::endl; |
||||
viewer->spin(); |
||||
} |
||||
#endif |
||||
int key = cv::waitKey(1); |
||||
// std::cout << "key: " << key << std::endl;
|
||||
switch (key) { |
||||
// case 'w': {
|
||||
// z += 0.1;
|
||||
// break;
|
||||
// }
|
||||
// case 'a': {
|
||||
// x += 0.1;
|
||||
// break;
|
||||
// }
|
||||
// case 's': {
|
||||
// z -= 0.1;
|
||||
// break;
|
||||
// }
|
||||
// case 'd': {
|
||||
// x -= 0.1;
|
||||
// break;
|
||||
// }
|
||||
// case 'q': {
|
||||
// y -= 0.1;
|
||||
// break;
|
||||
// }
|
||||
// case 'e': {
|
||||
// y += 0.1;
|
||||
// break;
|
||||
// }
|
||||
// case 'z': {
|
||||
// x = 0;
|
||||
// y = 0;
|
||||
// z = 0;
|
||||
// break;
|
||||
// }
|
||||
// case 82: {
|
||||
// dy += 0.1 * M_PI / 180;
|
||||
// break;
|
||||
// }
|
||||
// case 81: {
|
||||
// dx += 0.1 * M_PI / 180;
|
||||
// break;
|
||||
// }
|
||||
// case 84: {
|
||||
// dy -= 0.1 * M_PI / 180;
|
||||
// break;
|
||||
// }
|
||||
// case 83: {
|
||||
// dx -= 0.1 * M_PI / 180;
|
||||
// break;
|
||||
// }
|
||||
|
||||
case 'p': { |
||||
char buff[60]; |
||||
std::time_t t = std::time(0); |
||||
tm* nowtime = localtime(&t); |
||||
sprintf(buff, "image_%04d%02d%02d-%02d%02d%02d.png", 1900 + nowtime->tm_year, nowtime->tm_mon + 1, |
||||
nowtime->tm_mday, nowtime->tm_hour + 1, nowtime->tm_min + 1, nowtime->tm_sec + 1); |
||||
cv::imwrite(buff, result_frame); |
||||
std::cout << "save image! file: '" << buff << "'" << std::endl; |
||||
break; |
||||
} |
||||
case 's': { |
||||
char buff[60]; |
||||
std::time_t t = std::time(0); |
||||
tm* nowtime = localtime(&t); |
||||
sprintf(buff, "sensor_%04d%02d%02d-%02d%02d%02d.pcd", 1900 + nowtime->tm_year, nowtime->tm_mon + 1, |
||||
nowtime->tm_mday, nowtime->tm_hour + 1, nowtime->tm_min + 1, nowtime->tm_sec + 1); |
||||
pcl::io::savePCDFileASCII(buff, *cloud_ptr); |
||||
std::cout << "save pcd! file: '" << buff << "'" << std::endl; |
||||
break; |
||||
} |
||||
case 'q': |
||||
case 27: { |
||||
goto exit_main; |
||||
break; |
||||
} |
||||
default: { |
||||
// std::cout << "key: " << key << std::endl;
|
||||
break; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
exit_main: |
||||
cv::destroyAllWindows(); |
||||
#if 0 |
||||
viewer->close(); |
||||
vthread.join(); |
||||
#else |
||||
viewer->close(); |
||||
// runner.join();
|
||||
#endif |
||||
|
||||
tof.stop(); |
||||
|
||||
tof.close(); |
||||
|
||||
return 0; |
||||
} |
||||
@ -1,50 +0,0 @@
|
||||
cmake_minimum_required(VERSION 3.8) |
||||
project(arducam) |
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") |
||||
add_compile_options(-Wall -Wextra -Wpedantic) |
||||
endif() |
||||
|
||||
|
||||
# set(ENV{ARDUCAM_LIB_PATH} /usr/lib) |
||||
set(ENV{ARDUCAM_HEARD_PATH} /usr/include/cpp) |
||||
|
||||
# find dependencies |
||||
find_package(ament_cmake REQUIRED) |
||||
# uncomment the following section in order to fill in |
||||
# further dependencies manually. |
||||
# find_package(<dependency> REQUIRED) |
||||
find_package(rclcpp REQUIRED) |
||||
find_package(std_msgs REQUIRED) |
||||
find_package(sensor_msgs REQUIRED) |
||||
|
||||
include_directories( |
||||
# /usr/include/cpp |
||||
$ENV{ARDUCAM_HEARD_PATH} |
||||
${PCL_INCLUDE_DIRS} |
||||
${ament_INCLUDE_DIRS} |
||||
) |
||||
link_directories(${PCL_LIBRARY_DIRS}) |
||||
add_definitions(${PCL_DEFINITIONS}) |
||||
|
||||
add_executable(tof_pointcloud src/tof_pointcloud.cpp) |
||||
ament_target_dependencies(tof_pointcloud rclcpp std_msgs sensor_msgs) |
||||
target_link_libraries(tof_pointcloud ArducamDepthCamera) |
||||
|
||||
install(TARGETS tof_pointcloud |
||||
DESTINATION lib/${PROJECT_NAME} |
||||
) |
||||
|
||||
if(BUILD_TESTING) |
||||
find_package(ament_lint_auto REQUIRED) |
||||
# the following line skips the linter which checks for copyrights |
||||
# comment the line when a copyright and license is added to all source files |
||||
set(ament_cmake_copyright_FOUND TRUE) |
||||
# the following line skips cpplint (only works in a git repo) |
||||
# comment the line when this package is in a git repo and when |
||||
# a copyright and license is added to all source files |
||||
set(ament_cmake_cpplint_FOUND TRUE) |
||||
ament_lint_auto_find_test_dependencies() |
||||
endif() |
||||
|
||||
ament_package() |
||||
@ -0,0 +1,4 @@
|
||||
from tof_pointcloud import main |
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -0,0 +1,19 @@
|
||||
<?xml version="1.0"?> |
||||
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> |
||||
<package format="2"> |
||||
<name>arducam_rclpy_tof_pointcloud</name> |
||||
<version>0.0.1</version> |
||||
<description>Arducam Tof Camera Examples.</description> |
||||
<maintainer email="dennis@arducam.com">pi</maintainer> |
||||
<license>Apache License, Version 2.0</license> |
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend> |
||||
|
||||
<exec_depend>rclpy</exec_depend> |
||||
<exec_depend>std_msgs</exec_depend> |
||||
<exec_depend>sensor_msgs</exec_depend> |
||||
|
||||
<export> |
||||
<build_type>ament_python</build_type> |
||||
</export> |
||||
</package> |
||||
@ -0,0 +1,4 @@
|
||||
[develop] |
||||
script_dir=$base/lib/arducam_rclpy_tof_pointcloud |
||||
[install] |
||||
install_scripts=$base/lib/arducam_rclpy_tof_pointcloud |
||||
@ -0,0 +1,35 @@
|
||||
from setuptools import setup |
||||
|
||||
package_name = 'arducam_rclpy_tof_pointcloud' |
||||
|
||||
setup( |
||||
name=package_name, |
||||
version='0.0.1', |
||||
packages=[package_name], |
||||
data_files=[ |
||||
('share/ament_index/resource_index/packages', |
||||
['resource/' + package_name]), |
||||
('share/' + package_name, ['package.xml']), |
||||
], |
||||
install_requires=['setuptools'], |
||||
zip_safe=True, |
||||
author='arducam', |
||||
author_email='dennis@arducam.com', |
||||
maintainer='arducam', |
||||
maintainer_email='dennis@arducam.com', |
||||
keywords=['ROS'], |
||||
classifiers=[ |
||||
'Intended Audience :: Developers', |
||||
'License :: OSI Approved :: Apache Software License', |
||||
'Programming Language :: Python', |
||||
'Topic :: Software Development', |
||||
], |
||||
description='Arducam Tof Camera Examples.', |
||||
license='Apache License, Version 2.0', |
||||
tests_require=['pytest'], |
||||
entry_points={ |
||||
'console_scripts': [ |
||||
'tof_pointcloud = ' + package_name + '.tof_pointcloud:main', |
||||
], |
||||
}, |
||||
) |
||||
@ -1,21 +0,0 @@
|
||||
<?xml version="1.0"?> |
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> |
||||
<package format="3"> |
||||
<name>arducam</name> |
||||
<version>0.0.1</version> |
||||
<description>TODO: Package description</description> |
||||
<maintainer email="dennis@arducam.com">pi</maintainer> |
||||
<license>TODO: License declaration</license> |
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend> |
||||
|
||||
<depend>rclcpp</depend> |
||||
<depend>std_msgs</depend> |
||||
<depend>sensor_msgs</depend> |
||||
<test_depend>ament_lint_auto</test_depend> |
||||
<test_depend>ament_lint_common</test_depend> |
||||
|
||||
<export> |
||||
<build_type>ament_cmake</build_type> |
||||
</export> |
||||
</package> |
||||
@ -1,125 +0,0 @@
|
||||
#include <chrono> |
||||
#include <memory> |
||||
#include <string> |
||||
#include <sensor_msgs/msg/point_cloud.hpp> |
||||
#include <rclcpp/rclcpp.hpp> |
||||
|
||||
#include <std_msgs/msg/float32_multi_array.hpp> |
||||
#include <vector> |
||||
|
||||
#include "ArducamTOFCamera.hpp" |
||||
|
||||
using namespace std::chrono_literals; |
||||
using namespace Arducam; |
||||
|
||||
ArducamTOFCamera tof; |
||||
|
||||
class TOFPublisher : public rclcpp::Node |
||||
{ |
||||
public: |
||||
TOFPublisher() : Node("arducam"), pointsize_(43200) |
||||
{ |
||||
pc2_msg_ = std::make_shared<sensor_msgs::msg::PointCloud>(); |
||||
pc2_msg_->points.resize(pointsize_); |
||||
pc2_msg_->channels.resize(1); |
||||
pc2_msg_->channels[0].name = "intensities"; |
||||
pc2_msg_->channels[0].values.resize(pointsize_); |
||||
depth_msg_ = std::make_shared<std_msgs::msg::Float32MultiArray>(); |
||||
depth_msg_->layout.dim.resize(2); |
||||
depth_msg_->layout.dim[0].label = "height"; |
||||
depth_msg_->layout.dim[0].size = 180; |
||||
depth_msg_->layout.dim[0].stride = 43200; |
||||
depth_msg_->layout.dim[1].label = "width"; |
||||
depth_msg_->layout.dim[1].size = 240; |
||||
depth_msg_->layout.dim[1].stride = 240; |
||||
publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud>("point_cloud", 10); |
||||
publisher_depth_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("depth_frame", 10); |
||||
|
||||
timer_ = this->create_wall_timer( |
||||
50ms, std::bind(&TOFPublisher::update, this)); |
||||
} |
||||
|
||||
private: |
||||
void generateSensorPointCloud() |
||||
{ |
||||
ArducamFrameBuffer *frame; |
||||
do |
||||
{ |
||||
frame = tof.requestFrame(200); |
||||
} while (frame == nullptr); |
||||
depth_frame.clear(); |
||||
float *depth_ptr = (float *)frame->getData(FrameType::DEPTH_FRAME); |
||||
float *amplitude_ptr = (float *)frame->getData(FrameType::CONFIDENCE_FRAME); |
||||
unsigned long int pos = 0; |
||||
for (int row_idx = 0; row_idx < 180; row_idx++) |
||||
for (int col_idx = 0; col_idx < 240; col_idx++, pos++) |
||||
{ |
||||
if (amplitude_ptr[pos] > 30) |
||||
{ |
||||
float zz = depth_ptr[pos];
|
||||
pc2_msg_->points[pos].x = (((120 - col_idx)) / fx) * zz; |
||||
pc2_msg_->points[pos].y = ((90 - row_idx) / fy) * zz; |
||||
pc2_msg_->points[pos].z = zz; |
||||
pc2_msg_->channels[0].values[pos] = depth_ptr[pos]; |
||||
depth_frame.push_back(depth_ptr[pos]); |
||||
} |
||||
else |
||||
{ |
||||
pc2_msg_->points[pos].x = 0; |
||||
pc2_msg_->points[pos].y = 0; |
||||
pc2_msg_->points[pos].z = 0; |
||||
pc2_msg_->channels[0].values[pos] = 0; |
||||
depth_frame.push_back(0); |
||||
} |
||||
} |
||||
tof.releaseFrame(frame); |
||||
pc2_msg_->header.frame_id = frame_id_; |
||||
|
||||
depth_msg_->data = depth_frame; |
||||
} |
||||
void update() |
||||
{ |
||||
generateSensorPointCloud(); |
||||
pc2_msg_->header.stamp = now(); |
||||
publisher_->publish(*pc2_msg_); |
||||
publisher_depth_->publish(*depth_msg_); |
||||
} |
||||
std::string frame_id_ = "sensor_frame"; |
||||
std::vector<float> depth_frame; |
||||
sensor_msgs::msg::PointCloud::SharedPtr pc2_msg_; |
||||
std_msgs::msg::Float32MultiArray::SharedPtr depth_msg_; |
||||
|
||||
size_t pointsize_; |
||||
rclcpp::TimerBase::SharedPtr timer_; |
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud>::SharedPtr publisher_; |
||||
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr publisher_depth_; |
||||
|
||||
float fx = 240 / (2 * tan(0.5 * M_PI * 64.3 / 180)); |
||||
float fy = 180 / (2 * tan(0.5 * M_PI * 50.4 / 180)); |
||||
}; |
||||
|
||||
int main(int argc, char *argv[]) |
||||
{ |
||||
rclcpp::init(argc, argv); |
||||
if (tof.open(Connection::CSI)) |
||||
{ |
||||
printf("initialize fail\n"); |
||||
exit(-1); |
||||
} |
||||
|
||||
if (tof.start()) |
||||
{ |
||||
printf("start fail\n"); |
||||
exit(-1); |
||||
} |
||||
tof.setControl(CameraCtrl::RANGE,4); |
||||
|
||||
printf("pointcloud publisher start\n"); |
||||
|
||||
setvbuf(stdout, NULL, _IONBF, BUFSIZ); |
||||
rclcpp::spin(std::make_shared<TOFPublisher>()); |
||||
rclcpp::shutdown(); |
||||
tof.stop(); |
||||
tof.close(); |
||||
return 0; |
||||
} |
||||
Loading…
Reference in new issue