ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'pinhole_init' into 'master' 

Pinhole init

See merge request basalt/basalt!37
This commit is contained in:
Vladyslav Usenko 2019-11-18 07:33:04 +00:00
parent fd7ccb2ad0 619f6be6bd
commit 3f4342c9d3
6 changed files with 184 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/basalt/calibration/aprilgrid.h
View File

@ -44,6 +44,9 @@ struct AprilGrid {
Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
inline int getTagCols() const { return tagCols; }
inline int getTagRows() const { return tagRows; }
private:
int tagCols; // number of apriltags
int tagRows; // number of apriltags

11
include/basalt/calibration/calibration_helper.h
View File

@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <basalt/calibration/aprilgrid.h>
#include <basalt/io/dataset_io.h>
#include <basalt/utils/common_types.h>
#include <basalt/calibration/calibration.hpp>
@ -85,9 +86,13 @@ class CalibHelper {
static bool initializeIntrinsics(
const Eigen::aligned_vector<Eigen::Vector2d>& corners,
const std::vector<int>& corner_ids,
const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
int cols, int rows, Eigen::Vector4d& init_intr);
const std::vector<int>& corner_ids, const AprilGrid& aprilgrid, int cols,
int rows, Eigen::Vector4d& init_intr);
static bool initializeIntrinsicsPinhole(
const std::vector<CalibCornerData*> pinhole_corners,
const AprilGrid& aprilgrid, int cols, int rows,
Eigen::Vector4d& init_intr);
private:
inline static double square(double x) { return x * x; }

18
include/basalt/optimization/poses_optimize.h
View File

@ -34,11 +34,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <basalt/calibration/aprilgrid.h>
#include <basalt/calibration/calibration_helper.h>
#include <basalt/optimization/poses_linearize.h>
#include <basalt/camera/unified_camera.hpp>
namespace basalt {
class PosesOptimization {
@ -61,21 +60,6 @@ class PosesOptimization {
PosesOptimization()
: lambda(1e-6), min_lambda(1e-12), max_lambda(100), lambda_vee(2) {}
bool initializeIntrinsics(
size_t cam_id, const Eigen::aligned_vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids,
const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
int cols, int rows) {
Eigen::Vector4d init_intr;
bool val = CalibHelper::initializeIntrinsics(
corners, corner_ids, aprilgrid_corner_pos_3d, cols, rows, init_intr);
calib->intrinsics[cam_id].setFromInit(init_intr);
return val;
}
Vector2 getOpticalCenter(size_t i) {
return calib->intrinsics[i].getParam().template segment<2>(2);
}

122
src/calibration/calibraiton_helper.cpp
View File

@ -47,6 +47,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
#include <opencv2/calib3d/calib3d.hpp>
namespace basalt {
template <class CamT>
@ -171,9 +173,8 @@ void CalibHelper::initCamPoses(
bool CalibHelper::initializeIntrinsics(
const Eigen::aligned_vector<Eigen::Vector2d> &corners,
const std::vector<int> &corner_ids,
const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,
int cols, int rows, Eigen::Vector4d &init_intr) {
const std::vector<int> &corner_ids, const AprilGrid &aprilgrid, int cols,
int rows, Eigen::Vector4d &init_intr) {
// First, initialize the image center at the center of the image.
Eigen::aligned_map<int, Eigen::Vector2d> id_to_corner;
@ -181,17 +182,17 @@ bool CalibHelper::initializeIntrinsics(
id_to_corner[corner_ids[i]] = corners[i];
}
double _xi = 1.0;
double _cu = cols / 2.0 - 0.5;
double _cv = rows / 2.0 - 0.5;
const double _xi = 1.0;
const double _cu = cols / 2.0 - 0.5;
const double _cv = rows / 2.0 - 0.5;
/// Initialize some temporaries needed.
double gamma0 = 0.0;
double minReprojErr = std::numeric_limits<double>::max();
// Now we try to find a non-radial line to initialize the focal length
const size_t target_cols = 6;
const size_t target_rows = 6;
const size_t target_cols = aprilgrid.getTagCols();
const size_t target_rows = aprilgrid.getTagRows();
bool success = false;
for (int tag_corner_offset = 0; tag_corner_offset < 2; tag_corner_offset++)
@ -270,14 +271,14 @@ bool CalibHelper::initializeIntrinsics(
size_t num_inliers;
Sophus::SE3d T_target_camera;
if (!estimateTransformation(cam_calib, corners, corner_ids,
aprilgrid_corner_pos_3d, T_target_camera,
num_inliers)) {
aprilgrid.aprilgrid_corner_pos_3d,
T_target_camera, num_inliers)) {
continue;
}
double reprojErr = 0.0;
size_t numReprojected = computeReprojectionError(
cam_calib, corners, corner_ids, aprilgrid_corner_pos_3d,
cam_calib, corners, corner_ids, aprilgrid.aprilgrid_corner_pos_3d,
T_target_camera, reprojErr);
// std::cerr << "numReprojected " << numReprojected << " reprojErr "
@ -301,6 +302,105 @@ bool CalibHelper::initializeIntrinsics(
return success;
}
bool CalibHelper::initializeIntrinsicsPinhole(
const std::vector<CalibCornerData *> pinhole_corners,
const AprilGrid &aprilgrid, int cols, int rows,
Eigen::Vector4d &init_intr) {
// First, initialize the image center at the center of the image.
const double _cu = cols / 2.0 - 0.5;
const double _cv = rows / 2.0 - 0.5;
// Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000
size_t nImages = pinhole_corners.size();
// Eigen::MatrixXd A(2 * nImages, 2);
// Eigen::VectorXd b(2 * nImages);
Eigen::MatrixXd A(nImages * 2, 2);
Eigen::VectorXd b(nImages * 2, 1);
int i = 0;
for (const CalibCornerData *ccd : pinhole_corners) {
const auto &corners = ccd->corners;
const auto &corner_ids = ccd->corner_ids;
std::vector<cv::Point2f> M(corners.size()), imagePoints(corners.size());
for (size_t j = 0; j < corners.size(); ++j) {
M.at(j) =
cv::Point2f(aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]][0],
aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]][1]);
// std::cout << "corner "
// <<
// aprilgrid.aprilgrid_corner_pos_3d[corner_ids[j]].transpose()
// << std::endl;
imagePoints.at(j) = cv::Point2f(corners[j][0], corners[j][1]);
}
cv::Mat H = cv::findHomography(M, imagePoints);
if (H.empty()) return false;
// std::cout << H << std::endl;
H.at<double>(0, 0) -= H.at<double>(2, 0) * _cu;
H.at<double>(0, 1) -= H.at<double>(2, 1) * _cu;
H.at<double>(0, 2) -= H.at<double>(2, 2) * _cu;
H.at<double>(1, 0) -= H.at<double>(2, 0) * _cv;
H.at<double>(1, 1) -= H.at<double>(2, 1) * _cv;
H.at<double>(1, 2) -= H.at<double>(2, 2) * _cv;
double h[3], v[3], d1[3], d2[3];
double n[4] = {0, 0, 0, 0};
for (int j = 0; j < 3; ++j) {
double t0 = H.at<double>(j, 0);
double t1 = H.at<double>(j, 1);
h[j] = t0;
v[j] = t1;
d1[j] = (t0 + t1) * 0.5;
d2[j] = (t0 - t1) * 0.5;
n[0] += t0 * t0;
n[1] += t1 * t1;
n[2] += d1[j] * d1[j];
n[3] += d2[j] * d2[j];
}
for (int j = 0; j < 4; ++j) {
n[j] = 1.0 / sqrt(n[j]);
}
for (int j = 0; j < 3; ++j) {
h[j] *= n[0];
v[j] *= n[1];
d1[j] *= n[2];
d2[j] *= n[3];
}
A(i * 2, 0) = h[0] * v[0];
A(i * 2, 1) = h[1] * v[1];
A(i * 2 + 1, 0) = d1[0] * d2[0];
A(i * 2 + 1, 1) = d1[1] * d2[1];
b(i * 2, 0) = -h[2] * v[2];
b(i * 2 + 1, 0) = -d1[2] * d2[2];
i++;
}
Eigen::Vector2d f = (A.transpose() * A).ldlt().solve(A.transpose() * b);
double fx = sqrt(fabs(1.0 / f(0)));
double fy = sqrt(fabs(1.0 / f(1)));
init_intr << fx, fy, _cu, _cv;
return true;
}
void CalibHelper::computeInitialPose(
const Calibration<double>::Ptr &calib, size_t cam_id,
const Eigen::aligned_vector<Eigen::Vector4d> &aprilgrid_corner_pos_3d,

65
src/calibration/cam_calib.cpp
View File

@ -450,8 +450,14 @@ void CamCalib::initCamIntrinsics() {
calib_opt->resetCalib(vio_dataset->get_num_cams(), cam_types);
std::vector<bool> cam_initialized(vio_dataset->get_num_cams(), false);
int inc = 1;
if (vio_dataset->get_image_timestamps().size() > 100) inc = 3;
for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) {
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size();
i += inc) {
const int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
const std::vector<basalt::ImageData> &img_vec =
vio_dataset->get_image_data(timestamp_ns);
@ -461,10 +467,59 @@ void CamCalib::initCamIntrinsics() {
if (calib_corners.find(tcid) != calib_corners.end()) {
CalibCornerData cid = calib_corners.at(tcid);
bool success = calib_opt->initializeIntrinsics(
j, cid.corners, cid.corner_ids, april_grid.aprilgrid_corner_pos_3d,
img_vec[j].img->w, img_vec[j].img->h);
if (success) break;
Eigen::Vector4d init_intr;
bool success = CalibHelper::initializeIntrinsics(
cid.corners, cid.corner_ids, april_grid, img_vec[j].img->w,
img_vec[j].img->h, init_intr);
if (success) {
cam_initialized[j] = true;
calib_opt->calib->intrinsics[j].setFromInit(init_intr);
break;
}
}
}
}
// Try perfect pinhole initialization for cameras that are not initalized.
for (size_t j = 0; j < vio_dataset->get_num_cams(); j++) {
if (!cam_initialized[j]) {
std::vector<CalibCornerData *> pinhole_corners;
int w, h;
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size();
i += inc) {
const int64_t timestamp_ns = vio_dataset->get_image_timestamps()[i];
const std::vector<basalt::ImageData> &img_vec =
vio_dataset->get_image_data(timestamp_ns);
TimeCamId tcid(timestamp_ns, j);
auto it = calib_corners.find(tcid);
if (it != calib_corners.end()) {
if (it->second.corners.size() > 8) {
pinhole_corners.emplace_back(&it->second);
}
}
w = img_vec[j].img->w;
h = img_vec[j].img->h;
}
Eigen::Vector4d init_intr;
bool success = CalibHelper::initializeIntrinsicsPinhole(
pinhole_corners, april_grid, w, h, init_intr);
if (success) {
cam_initialized[j] = true;
std::cout << "Initialized camera " << j
<< " with pinhole model. You should set pinhole model for "
"this camera!"
<< std::endl;
calib_opt->calib->intrinsics[j].setFromInit(init_intr);
}
}
}

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit 34668f662181af61b504400fa80c7f4416c7f6e2
Subproject commit d4c366688fa4c6daa74822dffdca3cefd0e04223