ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v3

Browse Source
This commit is contained in:
Ivan
2022-04-14 13:51:42 +03:00
parent 9d73ebfc72
commit 34d7abd905
1 changed files with 30 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
src/vio.cpp
View File

@@ -412,13 +412,13 @@ void DrawCube(const float &size,const float x, const float y, const float z)
void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_, const std::vector<double>& distCoeffs_){ void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_, const std::vector<double>& distCoeffs_){
cv::Point3d point1 = {xSkew, ySkew, zSkew}; cv::Point3d point1 = {xSkew, ySkew, zSkew};
cv::Point3d point2 = {xSkew, ySkew+1, zSkew}; cv::Point3d point2 = {xSkew, ySkew+4, zSkew};
cv::Point3d point3 = {xSkew+1, ySkew, zSkew}; cv::Point3d point3 = {xSkew+4, ySkew, zSkew};
cv::Point3d point4 = {xSkew+1, ySkew+1, zSkew}; cv::Point3d point4 = {xSkew+4, ySkew+4, zSkew};
cv::Point3d point5 = {xSkew, ySkew, zSkew+1}; cv::Point3d point5 = {xSkew, ySkew, zSkew+4};
cv::Point3d point6 = {xSkew, ySkew+1, zSkew+1}; cv::Point3d point6 = {xSkew, ySkew+4, zSkew+4};
cv::Point3d point7 = {xSkew+1, ySkew, zSkew+1}; cv::Point3d point7 = {xSkew+4, ySkew, zSkew+4};
cv::Point3d point8 = {xSkew+1, ySkew+1, zSkew+1}; cv::Point3d point8 = {xSkew+4, ySkew+4, zSkew+4};
std::vector<cv::Point3d> objectPoints = {point1, point2, point3, point4, point5, point6, point7, point8}; std::vector<cv::Point3d> objectPoints = {point1, point2, point3, point4, point5, point6, point7, point8};
std::vector<cv::Point2d> imagePoints; std::vector<cv::Point2d> imagePoints;
@@ -431,7 +431,7 @@ void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_
// Projecting points // Projecting points
cv::projectPoints(objectPoints, rVec, tVec, K_, distCoeffs_, imagePoints); cv::projectPoints(objectPoints, rVec, tVec, K_, distCoeffs_, imagePoints);
cv::Vec3b color1 = {133, 247, 208}; cv::Vec3b color1 = {10, 255, 10};
cv::line(image, imagePoints[0], imagePoints[1], color1, 2); cv::line(image, imagePoints[0], imagePoints[1], color1, 2);
cv::line(image, imagePoints[0], imagePoints[2], color1, 2); cv::line(image, imagePoints[0], imagePoints[2], color1, 2);
@@ -724,21 +724,21 @@ int main(int argc, char** argv) {
auto cx = intrinsics_vector[2]; auto cx = intrinsics_vector[2];
auto cy = intrinsics_vector[3]; auto cy = intrinsics_vector[3];
// // std::cout << "Hessian calib" << std::endl; // std::cout << "Hessian calib" << std::endl;
// K.at<double>(0,0) = 718.856; K.at<double>(0,0) = 707.092;
// // std::cout << "fx: " << K.at<double>(0, 0); // std::cout << "fx: " << K.at<double>(0, 0);
// K.at<double>(1,1) = 718.856; K.at<double>(1,1) = 707.0912;
// // std::cout << "fy: " << K.at<double>(1, 1); // std::cout << "fy: " << K.at<double>(1, 1);
// K.at<double>(0,2) = 607.1928; K.at<double>(0,2) = 601.8873;
// // std::cout << "cx: " << K.at<double>(0, 2); // std::cout << "cx: " << K.at<double>(0, 2);
// K.at<double>(1,2) = 185.2157; K.at<double>(1,2) = 183.1104;
// // std::cout << "cy: " << K.at<double> (1, 2); // std::cout << "cy: " << K.at<double> (1, 2);
//
// distCoeffs[0] = 0.003482389402; distCoeffs[0] = 0.003482389402;
// distCoeffs[1] = 0.000715034845; distCoeffs[1] = 0.000715034845;
// distCoeffs[2] = -0.002053236141; distCoeffs[2] = -0.002053236141;
// distCoeffs[3] = 0.000202936736; distCoeffs[3] = 0.000202936736;
// distCoeffs[4] = 0; distCoeffs[4] = 0;
ar_3d_camera = pangolin::OpenGlRenderState( ar_3d_camera = pangolin::OpenGlRenderState(
pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000), pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000),
@@ -839,7 +839,11 @@ int main(int argc, char** argv) {
} else if (!it_ar->second->frames.empty()) { } else if (!it_ar->second->frames.empty()) {
T_w_i = it_ar->second->frames.back(); T_w_i = it_ar->second->frames.back();
} }
auto T_i_c = calib.T_i_c[0];
// Draw OpenCV cube
if (IfDrawOpenCVCube){
DrawOpenCVCube(img_cv_data[2], (T_w_i * T_i_c), K, distCoeffs);
}
// For some reason, there are 4 elements, and two first of them are the empty images. // For some reason, there are 4 elements, and two first of them are the empty images.
if (drawTexture) { if (drawTexture) {
DrawImageTexture(imageTexture, img_cv_data[2]); DrawImageTexture(imageTexture, img_cv_data[2]);
@@ -892,7 +896,7 @@ int main(int argc, char** argv) {
// LoadCameraPose(Transform_matrix); // LoadCameraPose(Transform_matrix);
// } // }
// End appended. // End appended.
std::cout << "Number of cameras" << calib.intrinsics.size() << std::endl;
if (show_frame.GuiChanged()) { if (show_frame.GuiChanged()) {
for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) { for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) {
size_t frame_id = static_cast<size_t>(show_frame); size_t frame_id = static_cast<size_t>(show_frame);
@@ -1274,7 +1278,7 @@ void draw_scene_no_camera(pangolin::View& view) {
if (drawCubeBool){ if (drawCubeBool){
DrawCube(cubeSize, xSkew, ySkew, zSkew); DrawCube(cubeSize, xSkew, ySkew, zSkew);
} }
else if(drawLineCubeBool){ if (drawLineCubeBool){
drawLinesCube(xSkew, ySkew, zSkew, cubeSize); drawLinesCube(xSkew, ySkew, zSkew, cubeSize);
} }
// For the 6th sequence mono-kitti // For the 6th sequence mono-kitti