rabbitdaxi
commented
4 years ago @guanming001 Thank you for the feedback and sharing the sample code.
I had a quick look of the green screen sample code and also the sample code that how you leverage the green screen sample to calibrate your two camera and transform the point cloud into the same camera space (in the sample code you shared here, they all transformed to the "main color" camera). I can compile your code without changing anything, it seems can align the two points clouds from the two camera in the same coordinate space pretty well. See the image below, apologize of the simplicity in the image, do not want to have much privacy data in the image. (you are rendering the blue and red to represent the point cloud from those two devices).
And yes, you are doing the right thing. To overlay point clouds from two or more cameras, which is quite a common case of using multiple Azure Kinect from the community here is an example. The sample code you shared looks reasonable. Essentially, the steps to do this task are:
- you need to calibrate the extrinsics (rotation/translation) between the two/more kinect devices. Depends on what is the hardware setup scenario (devices count and location/pointing direction, distance between object and cameras etc.), you may need to choose the best fit calibration method and target calibration pattern to ensure the subpixel reprojection accuracy. It looks like the green screen is using a simple stereo calibration from opencv as an example, there are many other ways such as multiboard, moving board etc. or many other targets other than chessboard can help you calibrate them based on your needs.
- you need to generate point clouds from the two depth camera which the SDK supports this
- you need to define the coordinate space that you want to render them, for example, you can simply define one of the cameras on the Azure Kinect as the coordinate space center (given you have the extrinsics can transform point cloud among all cameras by using the calibration comes from the device itself as well as the extrinsics you calibrated between device to device). then you can mimic how the green screen example to create the new calibration object that from the source camera to the target camera (even they are cross devices) and use the SDK transformation function to transform them.
The key of how you can get them overlay accurately will be the calibration stage.
Now, you mentioned you cannot get a good overlay of the two point clouds. It might be good you can share some more information:
- can you describe how much the alignment error you think is?
- what type of target object is for these point cloud (e.g. if you are having some material tends to bring depth accuracy issue, such as subsurface scattering on human skin, you may have slightly alignment error due to the depth accuracy on such material)
- is the alignment issue happening for moving object or stationery object as well? (if the point cloud aligns for stationary object, but more error on moving object, this may indicate some time synchronization issue)
guanming001
mbleyer
valvador92
forestsen
I use the parameters such as
"frame_sub_master.csv frame_sub_marker.csv 0.25 1.0 0.25 1.0"
in the 
Is your code running on Ubuntu 18.04?
This is the result which only based on the calibration result (extrinsic matrix between two kinects) from 
henryo12
jasjuang
amonnphillip
JulesThuillier
neilDGD
danilogr
ndaley7
Koring12
abdu307
lun73
Hi
I have tried to modify the green_screen example to allow the display of point clouds obtained from two sync cameras but couldn't get the point cloud to overlay correctly.
Is there be any sample code on overlaying the point cloud from two cameras?
Below is the modified main.cpp for reference and the util.h is adapted from AzureKinectSample
Modified green screen code
``` // Copyright (c) Microsoft Corporation. All rights reserved. // Licensed under the MIT License. #include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "util.h" // Adapted from AzureKinectSample
using std::cerr;
using std::cout;
using std::endl;
using std::vector;
#include "transformation.h"
#include "MultiDeviceCapturer.h"
// Allowing at least 160 microseconds between depth cameras should ensure they do not interfere with one another.
constexpr uint32_t MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC = 160;
// ideally, we could generalize this to many OpenCV types
static cv::Mat color_to_opencv(const k4a::image &im);
static cv::Mat depth_to_opencv(const k4a::image &im);
static cv::Matx33f calibration_to_color_camera_matrix(const k4a::calibration &cal);
static Transformation get_depth_to_color_transformation_from_calibration(const k4a::calibration &cal);
static k4a::calibration construct_device_to_device_calibration(const k4a::calibration &main_cal,
const k4a::calibration &secondary_cal,
const Transformation &secondary_to_main);
static vector calibration_to_color_camera_dist_coeffs(const k4a::calibration &cal);
static bool find_chessboard_corners_helper(const cv::Mat &main_color_image,
const cv::Mat &secondary_color_image,
const cv::Size &chessboard_pattern,
vector &main_chessboard_corners,
vector &secondary_chessboard_corners);
static Transformation stereo_calibration(const k4a::calibration &main_calib,
const k4a::calibration &secondary_calib,
const vector> &main_chessboard_corners_list,
const vector> &secondary_chessboard_corners_list,
const cv::Size &image_size,
const cv::Size &chessboard_pattern,
float chessboard_square_length);
static k4a_device_configuration_t get_master_config();
static k4a_device_configuration_t get_subordinate_config();
static Transformation calibrate_devices(MultiDeviceCapturer &capturer,
const k4a_device_configuration_t &main_config,
const k4a_device_configuration_t &secondary_config,
const cv::Size &chessboard_pattern,
float chessboard_square_length,
double calibration_timeout);
static k4a::image create_depth_image_like(const k4a::image &im);
// For plotting point cloud
k4a::image main_xyz_image;
k4a::image secondary_xyz_image;
cv::Mat main_xyz;
cv::Mat secondary_xyz;
// Viewer
cv::viz::Viz3d viewer;
int main(int argc, char **argv)
{
float chessboard_square_length = 0.; // must be included in the input params
int32_t color_exposure_usec = 8000; // somewhat reasonable default exposure time
int32_t powerline_freq = 2; // default to a 60 Hz powerline
cv::Size chessboard_pattern(0, 0); // height, width. Both need to be set.
uint16_t depth_threshold = 1000; // default to 1 meter
size_t num_devices = 0;
double calibration_timeout = 60.0; // default to timing out after 60s of trying to get calibrated
double greenscreen_duration = std::numeric_limits::max(); // run forever
vector device_indices{ 0 }; // Set up a MultiDeviceCapturer to handle getting many synchronous captures
// Note that the order of indices in device_indices is not necessarily
// preserved because MultiDeviceCapturer tries to find the master device based
// on which one has sync out plugged in. Start with just { 0 }, and add
// another if needed
if (argc < 5)
{
cout << "Usage: green_screen "
"[depth-threshold-mm (default 1000)] [color-exposure-time-usec (default 8000)] "
"[powerline-frequency-mode (default 2 for 60 Hz)] [calibration-timeout-sec (default 60)]"
"[greenscreen-duration-sec (default infinity- run forever)]"
<< endl;
cerr << "Not enough arguments!\n";
exit(1);
}
else
{
num_devices = static_cast(atoi(argv[1]));
if (num_devices > k4a::device::get_installed_count())
{
cerr << "Not enough cameras plugged in!\n";
exit(1);
}
chessboard_pattern.height = atoi(argv[2]);
chessboard_pattern.width = atoi(argv[3]);
chessboard_square_length = static_cast(atof(argv[4]));
if (argc > 5)
{
depth_threshold = static_cast(atoi(argv[5]));
if (argc > 6)
{
color_exposure_usec = atoi(argv[6]);
if (argc > 7)
{
powerline_freq = atoi(argv[7]);
if (argc > 8)
{
calibration_timeout = atof(argv[8]);
if (argc > 9)
{
greenscreen_duration = atof(argv[9]);
}
}
}
}
}
}
if (num_devices != 2 && num_devices != 1)
{
cerr << "Invalid choice for number of devices!\n";
exit(1);
}
else if (num_devices == 2)
{
device_indices.emplace_back(1); // now device indices are { 0, 1 }
}
if (chessboard_pattern.height == 0)
{
cerr << "Chessboard height is not properly set!\n";
exit(1);
}
if (chessboard_pattern.width == 0)
{
cerr << "Chessboard height is not properly set!\n";
exit(1);
}
if (chessboard_square_length == 0.)
{
cerr << "Chessboard square size is not properly set!\n";
exit(1);
}
cout << "Chessboard height: " << chessboard_pattern.height << ". Chessboard width: " << chessboard_pattern.width
<< ". Chessboard square length: " << chessboard_square_length << endl;
cout << "Depth threshold: : " << depth_threshold << ". Color exposure time: " << color_exposure_usec
<< ". Powerline frequency mode: " << powerline_freq << endl;
MultiDeviceCapturer capturer(device_indices, color_exposure_usec, powerline_freq);
// Create configurations for devices
k4a_device_configuration_t main_config = get_master_config();
if (num_devices == 1) // no need to have a master cable if it's standalone
{
main_config.wired_sync_mode = K4A_WIRED_SYNC_MODE_STANDALONE;
}
k4a_device_configuration_t secondary_config = get_subordinate_config();
// Construct all the things that we'll need whether or not we are running with 1 or 2 cameras
k4a::calibration main_calibration = capturer.get_master_device().get_calibration(main_config.depth_mode,
main_config.color_resolution);
// Set up a transformation. DO THIS OUTSIDE OF YOUR MAIN LOOP! Constructing transformations involves time-intensive
// hardware setup and should not change once you have a rigid setup, so only call it once or it will run very
// slowly.
k4a::transformation main_depth_to_main_color(main_calibration);
capturer.start_devices(main_config, secondary_config);
// get an image to be the background
vector background_captures = capturer.get_synchronized_captures(secondary_config);
cv::Mat background_image = color_to_opencv(background_captures[0].get_color_image());
cv::Mat output_image = background_image.clone(); // allocated outside the loop to avoid re-creating every time
if (num_devices == 1)
{
std::chrono::time_point start_time = std::chrono::system_clock::now();
while (std::chrono::duration(std::chrono::system_clock::now() - start_time).count() <
greenscreen_duration)
{
vector captures;
// secondary_config isn't actually used here because there's no secondary device but the function needs it
captures = capturer.get_synchronized_captures(secondary_config, true);
k4a::image main_color_image = captures[0].get_color_image();
k4a::image main_depth_image = captures[0].get_depth_image();
// let's green screen out things that are far away.
// first: let's get the main depth image into the color camera space
k4a::image main_depth_in_main_color = create_depth_image_like(main_color_image);
main_depth_to_main_color.depth_image_to_color_camera(main_depth_image, &main_depth_in_main_color);
cv::Mat cv_main_depth_in_main_color = depth_to_opencv(main_depth_in_main_color);
cv::Mat cv_main_color_image = color_to_opencv(main_color_image);
// single-camera case
cv::Mat within_threshold_range = (cv_main_depth_in_main_color != 0) &
(cv_main_depth_in_main_color < depth_threshold);
// show the close details
cv_main_color_image.copyTo(output_image, within_threshold_range);
// hide the rest with the background image
background_image.copyTo(output_image, ~within_threshold_range);
cv::imshow("Green Screen", output_image);
cv::waitKey(1);
}
}
else if (num_devices == 2)
{
// This wraps all the device-to-device details
Transformation tr_secondary_color_to_main_color = calibrate_devices(capturer,
main_config,
secondary_config,
chessboard_pattern,
chessboard_square_length,
calibration_timeout);
k4a::calibration secondary_calibration =
capturer.get_subordinate_device_by_index(0).get_calibration(secondary_config.depth_mode,
secondary_config.color_resolution);
// Get the transformation from secondary depth to secondary color using its calibration object
Transformation tr_secondary_depth_to_secondary_color = get_depth_to_color_transformation_from_calibration(
secondary_calibration);
// We now have the secondary depth to secondary color transform. We also have the transformation from the
// secondary color perspective to the main color perspective from the calibration earlier. Now let's compose the
// depth secondary -> color secondary, color secondary -> color main into depth secondary -> color main
Transformation tr_secondary_depth_to_main_color = tr_secondary_depth_to_secondary_color.compose_with(
tr_secondary_color_to_main_color);
// Construct a new calibration object to transform from the secondary depth camera to the main color camera
k4a::calibration secondary_depth_to_main_color_cal =
construct_device_to_device_calibration(main_calibration,
secondary_calibration,
tr_secondary_depth_to_main_color);
k4a::transformation secondary_depth_to_main_color(secondary_depth_to_main_color_cal);
// Pointcloud viewer
const cv::String window_name = "point cloud";
viewer = cv::viz::Viz3d(window_name);
// Show Coordinate System Origin
constexpr double scale = 100.0;
viewer.showWidget("origin", cv::viz::WCameraPosition(scale));
std::chrono::time_point start_time = std::chrono::system_clock::now();
while (std::chrono::duration(std::chrono::system_clock::now() - start_time).count() <
greenscreen_duration)
{
vector captures;
captures = capturer.get_synchronized_captures(secondary_config, true);
k4a::image main_color_image = captures[0].get_color_image();
k4a::image main_depth_image = captures[0].get_depth_image();
// let's green screen out things that are far away.
// first: let's get the main depth image into the color camera space
k4a::image main_depth_in_main_color = create_depth_image_like(main_color_image);
main_depth_to_main_color.depth_image_to_color_camera(main_depth_image, &main_depth_in_main_color);
cv::Mat cv_main_depth_in_main_color = depth_to_opencv(main_depth_in_main_color);
cv::Mat cv_main_color_image = color_to_opencv(main_color_image);
k4a::image secondary_depth_image = captures[1].get_depth_image();
k4a::image secondary_color_image = captures[1].get_color_image();
// Get the depth image in the main color perspective
k4a::image secondary_depth_in_main_color = create_depth_image_like(main_color_image);
secondary_depth_to_main_color.depth_image_to_color_camera(secondary_depth_image,
&secondary_depth_in_main_color);
cv::Mat cv_secondary_depth_in_main_color = depth_to_opencv(secondary_depth_in_main_color);
cv::Mat cv_secondary_color_image = color_to_opencv(secondary_color_image);
// Transform Depth Image to Point Cloud
main_xyz_image = main_depth_to_main_color.depth_image_to_point_cloud( main_depth_in_main_color, K4A_CALIBRATION_TYPE_COLOR );
secondary_xyz_image = secondary_depth_to_main_color.depth_image_to_point_cloud(secondary_depth_in_main_color, K4A_CALIBRATION_TYPE_COLOR);
// Get cv::Mat from k4a::image
main_xyz = k4a::get_mat(main_xyz_image);
secondary_xyz = k4a::get_mat(secondary_xyz_image);
// Create Point Cloud Widget
//cv::viz::WCloud cloud = cv::viz::WCloud(main_xyz, cv_main_color_image);
cv::viz::WCloud cloud = cv::viz::WCloud(main_xyz, cv::viz::Color::blue());
cv::viz::WCloud cloud2 = cv::viz::WCloud(secondary_xyz, cv::viz::Color::red());
// Show Widget
viewer.showWidget("cloud", cloud);
viewer.showWidget("cloud2", cloud2);
viewer.spinOnce();
//// Now it's time to actually construct the green screen. Where the depth is 0, the camera doesn't know how
//// far away the object is because it didn't get a response at that point. That's where we'll try to fill in
//// the gaps with the other camera.
//cv::Mat main_valid_mask = cv_main_depth_in_main_color != 0;
//cv::Mat secondary_valid_mask = cv_secondary_depth_in_main_color != 0;
//// build depth mask. If the main camera depth for a pixel is valid and the depth is within the threshold,
//// then set the mask to display that pixel. If the main camera depth for a pixel is invalid but the
//// secondary depth for a pixel is valid and within the threshold, then set the mask to display that pixel.
//cv::Mat within_threshold_range = (main_valid_mask & (cv_main_depth_in_main_color < depth_threshold)) |
// (~main_valid_mask & secondary_valid_mask &
// (cv_secondary_depth_in_main_color < depth_threshold));
//// copy main color image to output image only where the mask within_threshold_range is true
//cv_main_color_image.copyTo(output_image, within_threshold_range);
//// fill the rest with the background image
//background_image.copyTo(output_image, ~within_threshold_range);
//cv::imshow("Green Screen", output_image);
cv::waitKey(1);
}
// Close Viewer
viewer.close();
}
else
{
cerr << "Invalid number of devices!" << endl;
exit(1);
}
return 0;
}
static cv::Mat color_to_opencv(const k4a::image &im)
{
cv::Mat cv_image_with_alpha(im.get_height_pixels(), im.get_width_pixels(), CV_8UC4, (void *)im.get_buffer());
cv::Mat cv_image_no_alpha;
cv::cvtColor(cv_image_with_alpha, cv_image_no_alpha, cv::COLOR_BGRA2BGR);
return cv_image_no_alpha;
}
static cv::Mat depth_to_opencv(const k4a::image &im)
{
return cv::Mat(im.get_height_pixels(),
im.get_width_pixels(),
CV_16U,
(void *)im.get_buffer(),
static_cast(im.get_stride_bytes()));
}
static cv::Matx33f calibration_to_color_camera_matrix(const k4a::calibration &cal)
{
const k4a_calibration_intrinsic_parameters_t::_param &i = cal.color_camera_calibration.intrinsics.parameters.param;
cv::Matx33f camera_matrix = cv::Matx33f::eye();
camera_matrix(0, 0) = i.fx;
camera_matrix(1, 1) = i.fy;
camera_matrix(0, 2) = i.cx;
camera_matrix(1, 2) = i.cy;
return camera_matrix;
}
static Transformation get_depth_to_color_transformation_from_calibration(const k4a::calibration &cal)
{
const k4a_calibration_extrinsics_t &ex = cal.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_COLOR];
Transformation tr;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
tr.R(i, j) = ex.rotation[i * 3 + j];
}
}
tr.t = cv::Vec3d(ex.translation[0], ex.translation[1], ex.translation[2]);
return tr;
}
// This function constructs a calibration that operates as a transformation between the secondary device's depth camera
// and the main camera's color camera. IT WILL NOT GENERALIZE TO OTHER TRANSFORMS. Under the hood, the transformation
// depth_image_to_color_camera method can be thought of as converting each depth pixel to a 3d point using the
// intrinsics of the depth camera, then using the calibration's extrinsics to convert between depth and color, then
// using the color intrinsics to produce the point in the color camera perspective.
static k4a::calibration construct_device_to_device_calibration(const k4a::calibration &main_cal,
const k4a::calibration &secondary_cal,
const Transformation &secondary_to_main)
{
k4a::calibration cal = secondary_cal;
k4a_calibration_extrinsics_t &ex = cal.extrinsics[K4A_CALIBRATION_TYPE_DEPTH][K4A_CALIBRATION_TYPE_COLOR];
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ex.rotation[i * 3 + j] = static_cast(secondary_to_main.R(i, j));
}
}
for (int i = 0; i < 3; ++i)
{
ex.translation[i] = static_cast(secondary_to_main.t[i]);
}
cal.color_camera_calibration = main_cal.color_camera_calibration;
return cal;
}
static vector calibration_to_color_camera_dist_coeffs(const k4a::calibration &cal)
{
const k4a_calibration_intrinsic_parameters_t::_param &i = cal.color_camera_calibration.intrinsics.parameters.param;
return { i.k1, i.k2, i.p1, i.p2, i.k3, i.k4, i.k5, i.k6 };
}
bool find_chessboard_corners_helper(const cv::Mat &main_color_image,
const cv::Mat &secondary_color_image,
const cv::Size &chessboard_pattern,
vector &main_chessboard_corners,
vector &secondary_chessboard_corners)
{
bool found_chessboard_main = cv::findChessboardCorners(main_color_image,
chessboard_pattern,
main_chessboard_corners);
bool found_chessboard_secondary = cv::findChessboardCorners(secondary_color_image,
chessboard_pattern,
secondary_chessboard_corners);
// Cover the failure cases where chessboards were not found in one or both images.
if (!found_chessboard_main || !found_chessboard_secondary)
{
if (found_chessboard_main)
{
cout << "Could not find the chessboard corners in the secondary image. Trying again...\n";
}
// Likewise, if the chessboard was found in the secondary image, it was not found in the main image.
else if (found_chessboard_secondary)
{
cout << "Could not find the chessboard corners in the main image. Trying again...\n";
}
// The only remaining case is the corners were in neither image.
else
{
cout << "Could not find the chessboard corners in either image. Trying again...\n";
}
return false;
}
// Before we go on, there's a quick problem with calibration to address. Because the chessboard looks the same when
// rotated 180 degrees, it is possible that the chessboard corner finder may find the correct points, but in the
// wrong order.
// A visual:
// Image 1 Image 2
// ..................... .....................
// ..................... .....................
// .........xxxxx2...... .....xxxxx1..........
// .........xxxxxx...... .....xxxxxx..........
// .........xxxxxx...... .....xxxxxx..........
// .........1xxxxx...... .....2xxxxx..........
// ..................... .....................
// ..................... .....................
// The problem occurs when this case happens: the find_chessboard() function correctly identifies the points on the
// chessboard (shown as 'x's) but the order of those points differs between images taken by the two cameras.
// Specifically, the first point in the list of points found for the first image (1) is the *last* point in the list
// of points found for the second image (2), though they correspond to the same physical point on the chessboard.
// To avoid this problem, we can make the assumption that both of the cameras will be oriented in a similar manner
// (e.g. turning one of the cameras upside down will break this assumption) and enforce that the vector between the
// first and last points found in pixel space (which will be at opposite ends of the chessboard) are pointing the
// same direction- so, the dot product of the two vectors is positive.
cv::Vec2f main_image_corners_vec = main_chessboard_corners.back() - main_chessboard_corners.front();
cv::Vec2f secondary_image_corners_vec = secondary_chessboard_corners.back() - secondary_chessboard_corners.front();
if (main_image_corners_vec.dot(secondary_image_corners_vec) <= 0.0)
{
std::reverse(secondary_chessboard_corners.begin(), secondary_chessboard_corners.end());
}
return true;
}
Transformation stereo_calibration(const k4a::calibration &main_calib,
const k4a::calibration &secondary_calib,
const vector> &main_chessboard_corners_list,
const vector> &secondary_chessboard_corners_list,
const cv::Size &image_size,
const cv::Size &chessboard_pattern,
float chessboard_square_length)
{
// We have points in each image that correspond to the corners that the findChessboardCorners function found.
// However, we still need the points in 3 dimensions that these points correspond to. Because we are ultimately only
// interested in find a transformation between two cameras, these points don't have to correspond to an external
// "origin" point. The only important thing is that the relative distances between points are accurate. As a result,
// we can simply make the first corresponding point (0, 0) and construct the remaining points based on that one. The
// order of points inserted into the vector here matches the ordering of findChessboardCorners. The units of these
// points are in millimeters, mostly because the depth provided by the depth cameras is also provided in
// millimeters, which makes for easy comparison.
vector chessboard_corners_world;
for (int h = 0; h < chessboard_pattern.height; ++h)
{
for (int w = 0; w < chessboard_pattern.width; ++w)
{
chessboard_corners_world.emplace_back(
cv::Point3f{ w * chessboard_square_length, h * chessboard_square_length, 0.0 });
}
}
// Calibrating the cameras requires a lot of data. OpenCV's stereoCalibrate function requires:
// - a list of points in real 3d space that will be used to calibrate*
// - a corresponding list of pixel coordinates as seen by the first camera*
// - a corresponding list of pixel coordinates as seen by the second camera*
// - the camera matrix of the first camera
// - the distortion coefficients of the first camera
// - the camera matrix of the second camera
// - the distortion coefficients of the second camera
// - the size (in pixels) of the images
// - R: stereoCalibrate stores the rotation matrix from the first camera to the second here
// - t: stereoCalibrate stores the translation vector from the first camera to the second here
// - E: stereoCalibrate stores the essential matrix here (we don't use this)
// - F: stereoCalibrate stores the fundamental matrix here (we don't use this)
//
// * note: OpenCV's stereoCalibrate actually requires as input an array of arrays of points for these arguments,
// allowing a caller to provide multiple frames from the same camera with corresponding points. For example, if
// extremely high precision was required, many images could be taken with each camera, and findChessboardCorners
// applied to each of those images, and OpenCV can jointly solve for all of the pairs of corresponding images.
// However, to keep things simple, we use only one image from each device to calibrate. This is also why each of
// the vectors of corners is placed into another vector.
//
// A function in OpenCV's calibration function also requires that these points be F32 types, so we use those.
// However, OpenCV still provides doubles as output, strangely enough.
vector> chessboard_corners_world_nested_for_cv(main_chessboard_corners_list.size(),
chessboard_corners_world);
cv::Matx33f main_camera_matrix = calibration_to_color_camera_matrix(main_calib);
cv::Matx33f secondary_camera_matrix = calibration_to_color_camera_matrix(secondary_calib);
vector main_dist_coeff = calibration_to_color_camera_dist_coeffs(main_calib);
vector secondary_dist_coeff = calibration_to_color_camera_dist_coeffs(secondary_calib);
// Finally, we'll actually calibrate the cameras.
// Pass secondary first, then main, because we want a transform from secondary to main.
Transformation tr;
double error = cv::stereoCalibrate(chessboard_corners_world_nested_for_cv,
secondary_chessboard_corners_list,
main_chessboard_corners_list,
secondary_camera_matrix,
secondary_dist_coeff,
main_camera_matrix,
main_dist_coeff,
image_size,
tr.R, // output
tr.t, // output
cv::noArray(),
cv::noArray(),
cv::CALIB_FIX_INTRINSIC | cv::CALIB_RATIONAL_MODEL | cv::CALIB_CB_FAST_CHECK);
cout << "Finished calibrating!\n";
cout << "Got error of " << error << "\n";
return tr;
}
// The following functions provide the configurations that should be used for each camera.
// NOTE: For best results both cameras should have the same configuration (framerate, resolution, color and depth
// modes). Additionally the both master and subordinate should have the same exposure and power line settings. Exposure
// settings can be different but the subordinate must have a longer exposure from master. To synchronize a master and
// subordinate with different exposures the user should set `subordinate_delay_off_master_usec = ((subordinate exposure
// time) - (master exposure time))/2`.
//
static k4a_device_configuration_t get_default_config()
{
k4a_device_configuration_t camera_config = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
camera_config.color_format = K4A_IMAGE_FORMAT_COLOR_BGRA32;
camera_config.color_resolution = K4A_COLOR_RESOLUTION_720P;
camera_config.depth_mode = K4A_DEPTH_MODE_WFOV_UNBINNED; // No need for depth during calibration
camera_config.camera_fps = K4A_FRAMES_PER_SECOND_15; // Don't use all USB bandwidth
camera_config.subordinate_delay_off_master_usec = 0; // Must be zero for master
camera_config.synchronized_images_only = true;
return camera_config;
}
// Master customizable settings
static k4a_device_configuration_t get_master_config()
{
k4a_device_configuration_t camera_config = get_default_config();
camera_config.wired_sync_mode = K4A_WIRED_SYNC_MODE_MASTER;
// Two depth images should be seperated by MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC to ensure the depth imaging
// sensor doesn't interfere with the other. To accomplish this the master depth image captures
// (MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC / 2) before the color image, and the subordinate camera captures its
// depth image (MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC / 2) after the color image. This gives us two depth
// images centered around the color image as closely as possible.
camera_config.depth_delay_off_color_usec = -static_cast(MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC / 2);
camera_config.synchronized_images_only = true;
return camera_config;
}
// Subordinate customizable settings
static k4a_device_configuration_t get_subordinate_config()
{
k4a_device_configuration_t camera_config = get_default_config();
camera_config.wired_sync_mode = K4A_WIRED_SYNC_MODE_SUBORDINATE;
// Two depth images should be seperated by MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC to ensure the depth imaging
// sensor doesn't interfere with the other. To accomplish this the master depth image captures
// (MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC / 2) before the color image, and the subordinate camera captures its
// depth image (MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC / 2) after the color image. This gives us two depth
// images centered around the color image as closely as possible.
camera_config.depth_delay_off_color_usec = MIN_TIME_BETWEEN_DEPTH_CAMERA_PICTURES_USEC / 2;
return camera_config;
}
static Transformation calibrate_devices(MultiDeviceCapturer &capturer,
const k4a_device_configuration_t &main_config,
const k4a_device_configuration_t &secondary_config,
const cv::Size &chessboard_pattern,
float chessboard_square_length,
double calibration_timeout)
{
k4a::calibration main_calibration = capturer.get_master_device().get_calibration(main_config.depth_mode,
main_config.color_resolution);
k4a::calibration secondary_calibration =
capturer.get_subordinate_device_by_index(0).get_calibration(secondary_config.depth_mode,
secondary_config.color_resolution);
vector> main_chessboard_corners_list;
vector> secondary_chessboard_corners_list;
std::chrono::time_point start_time = std::chrono::system_clock::now();
while (std::chrono::duration(std::chrono::system_clock::now() - start_time).count() < calibration_timeout)
{
vector captures = capturer.get_synchronized_captures(secondary_config);
k4a::capture &main_capture = captures[0];
k4a::capture &secondary_capture = captures[1];
// get_color_image is guaranteed to be non-null because we use get_synchronized_captures for color
// (get_synchronized_captures also offers a flag to use depth for the secondary camera instead of color).
k4a::image main_color_image = main_capture.get_color_image();
k4a::image secondary_color_image = secondary_capture.get_color_image();
cv::Mat cv_main_color_image = color_to_opencv(main_color_image);
cv::Mat cv_secondary_color_image = color_to_opencv(secondary_color_image);
vector main_chessboard_corners;
vector secondary_chessboard_corners;
bool got_corners = find_chessboard_corners_helper(cv_main_color_image,
cv_secondary_color_image,
chessboard_pattern,
main_chessboard_corners,
secondary_chessboard_corners);
if (got_corners)
{
main_chessboard_corners_list.emplace_back(main_chessboard_corners);
secondary_chessboard_corners_list.emplace_back(secondary_chessboard_corners);
cv::drawChessboardCorners(cv_main_color_image, chessboard_pattern, main_chessboard_corners, true);
cv::drawChessboardCorners(cv_secondary_color_image, chessboard_pattern, secondary_chessboard_corners, true);
}
cv::imshow("Chessboard view from main camera", cv_main_color_image);
cv::waitKey(1);
cv::imshow("Chessboard view from secondary camera", cv_secondary_color_image);
cv::waitKey(1);
// Get 20 frames before doing calibration.
if (main_chessboard_corners_list.size() >= 20)
{
cout << "Calculating calibration..." << endl;
return stereo_calibration(main_calibration,
secondary_calibration,
main_chessboard_corners_list,
secondary_chessboard_corners_list,
cv_main_color_image.size(),
chessboard_pattern,
chessboard_square_length);
}
}
std::cerr << "Calibration timed out !\n ";
exit(1);
}
static k4a::image create_depth_image_like(const k4a::image &im)
{
return k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
im.get_width_pixels(),
im.get_height_pixels(),
im.get_width_pixels() * static_cast(sizeof(uint16_t)));
}
```