From 37be50f908bd2fe75aaff313fafa8339897b1b80 Mon Sep 17 00:00:00 2001 From: Mariia Gladkova Date: Mon, 23 Jul 2018 10:25:27 +0200 Subject: [PATCH] Performed some testing of ORB-based feature extraction --- include/VisualSLAM.h | 2 +- src/BundleAdjuster.cpp | 2 +- src/VisualOdometry.cpp | 2 +- src/VisualSLAM.cpp | 69 +++++++++++++++++++++++++++++++++++++----- testVisualSLAM.cpp | 7 +++-- 5 files changed, 70 insertions(+), 12 deletions(-) diff --git a/include/VisualSLAM.h b/include/VisualSLAM.h index 35a70eb..880e470 100644 --- a/include/VisualSLAM.h +++ b/include/VisualSLAM.h @@ -52,7 +52,7 @@ class VisualSLAM { void readCameraIntrisics(std::string camera_intrinsics_file); void readGroundTruthData(std::string fileName, int numberFrames, std::vector& groundTruthData); - Sophus::SE3d performFrontEndStep(cv::Mat image_left, cv::Mat disparity_map, std::vector& keyPointsPrevFrame, cv::Mat& descriptorsPrevFrame); // feature detection / tracking and matching + Sophus::SE3d performFrontEndStep(cv::Mat image_left, cv::Mat disparity_map, std::vector& keyPointsPrevFrame, cv::Mat& descriptorsPrevFrame, bool isLeftImage); // feature detection / tracking and matching Sophus::SE3d performFrontEndStepWithTracking(cv::Mat image_left, cv::Mat disparity_map, std::vector& pointsCurrentFrame, std::vector& pointsPrevFrame, cv::Mat& prevImageLeft, bool isLeftImage); //bool performPoseGraphOptimization(int keyFrameStep, int numKeyFrames); diff --git a/src/BundleAdjuster.cpp b/src/BundleAdjuster.cpp index 9426352..dd1c08e 100644 --- a/src/BundleAdjuster.cpp +++ b/src/BundleAdjuster.cpp @@ -93,7 +93,7 @@ bool BundleAdjuster::performBAWithKeyFrames(Map& map_left, Map& map_right, int k ceres::Problem problem; ceres::LossFunction* loss_function = new ceres::HuberLoss(2.0); double baseline = 0.53716; - double confid = 1e2; + double confid = 1e3; Eigen::Matrix information_matrix = Eigen::MatrixXd::Identity(7, 7)*confid; for (int i = startFrame; i < currentCameraIndex; i += keyFrameStep){ diff --git a/src/VisualOdometry.cpp b/src/VisualOdometry.cpp index ab6364e..513abfd 100644 --- a/src/VisualOdometry.cpp +++ b/src/VisualOdometry.cpp @@ -31,7 +31,7 @@ void VisualOdometry::setKeyFrameKeypoints(std::vector updatedKeypo } void VisualOdometry::extractORBFeatures(cv::Mat frame_new, std::vector& keypoints_new, cv::Mat& descriptors_new){ - int max_features = 2000; + int max_features = 200; // Detect ORB features and compute descriptors. cv::Ptr orb = cv::ORB::create(max_features); orb->detect(frame_new, keypoints_new); diff --git a/src/VisualSLAM.cpp b/src/VisualSLAM.cpp index b58bb79..33828b4 100644 --- a/src/VisualSLAM.cpp +++ b/src/VisualSLAM.cpp @@ -219,18 +219,34 @@ void VisualSLAM::readGroundTruthData(std::string fileName, int numberFrames, std this->groundTruthData = groundTruthData; } -Sophus::SE3d VisualSLAM::performFrontEndStep(cv::Mat image_left, cv::Mat disparityCurrentFrame, std::vector& keyPointsPrevFrame, cv::Mat& descriptorsPrevFrame){ +Sophus::SE3d VisualSLAM::performFrontEndStep(cv::Mat image, cv::Mat disparityCurrentFrame, std::vector& keyPointsPrevFrame, cv::Mat& descriptorsPrevFrame, bool isLeftImage){ cv::Mat descriptorsCurrentFrame; std::vector keyPointsCurrentFrame; - VO.extractORBFeatures(image_left, keyPointsCurrentFrame, descriptorsCurrentFrame); + VO.extractORBFeatures(image, keyPointsCurrentFrame, descriptorsCurrentFrame); Sophus::SE3d pose; if (keyPointsPrevFrame.empty()){ - keyPointsPrevFrame = keyPointsCurrentFrame; - descriptorsCurrentFrame.copyTo(descriptorsPrevFrame); + std::vector p2d_currFrame; + for (auto kpt: keyPointsCurrentFrame){ + p2d_currFrame.push_back(kpt.pt); + } + std::vector validPoints3D; + std::vector p3d_currFrame = VO.get3DCoordinates(p2d_currFrame, disparityCurrentFrame, K, validPoints3D); + + std::vector indices(p2d_currFrame.size()); + std::iota(indices.begin(), indices.end(), 0); + + if (isLeftImage){ + map_left.updateDataCurrentFrame(pose, p2d_currFrame, indices, p3d_currFrame, true, false); + } else { + map_right.updateDataCurrentFrame(pose, p2d_currFrame, indices, p3d_currFrame, true, true); + } + + keyPointsPrevFrame = keyPointsCurrentFrame; + descriptorsCurrentFrame.copyTo(descriptorsPrevFrame); - return pose; + return pose; } std::vector matches = VO.findGoodORBFeatureMatches(keyPointsPrevFrame, keyPointsCurrentFrame, descriptorsPrevFrame, descriptorsCurrentFrame); @@ -242,9 +258,48 @@ Sophus::SE3d VisualSLAM::performFrontEndStep(cv::Mat image_left, cv::Mat dispari std::vector validPoints3D; std::vector p3d_currFrame = VO.get3DCoordinates(p2d_currFrame, disparityCurrentFrame, K, validPoints3D); - std::vector indices; + std::vector trackedPrevFramePoints, trackedCurrFramePoints; + std::vector trackedPoints3DCurrentFrame; - VO.estimatePose3D2D(p3d_currFrame, p2d_prevFrame, p2d_currFrame, indices, K, pose); + for (int i = 0; i < validPoints3D.size(); i++){ + if (validPoints3D[i] && checkPoint2DCoordinates(p2d_currFrame[i], image)){ + trackedPrevFramePoints.push_back(p2d_prevFrame[i]); + trackedCurrFramePoints.push_back(p2d_currFrame[i]); + trackedPoints3DCurrentFrame.push_back(p3d_currFrame[i]); + } + } + + std::vector trackedPointIndices_dummy(trackedCurrFramePoints.size()); + std::iota(trackedPointIndices_dummy.begin(), trackedPointIndices_dummy.end(), 0); + + VO.estimatePose3D2D(trackedPoints3DCurrentFrame, trackedPrevFramePoints, trackedCurrFramePoints, trackedPointIndices_dummy, K, pose); + + std::vector indices(trackedCurrFramePoints.size()); + std::iota(indices.begin(), indices.end(), 0); + + + int keyFrameStep = 3; + int numKeyFrames = 20; + + if (isLeftImage){ + map_left.updateDataCurrentFrame(pose, trackedCurrFramePoints, indices, trackedPoints3DCurrentFrame, true, false); + + //if ((map_left.getCurrentCameraIndex() - numKeyFrames*keyFrameStep) >= 0){ + /*if ((map_left.getCurrentCameraIndex() % (numKeyFrames*keyFrameStep)) == 0 && map_left.getCurrentCameraIndex() > 0){ + std::string fileName = "BAFile" + std::to_string(map_left.getCurrentCameraIndex() / (numKeyFrames*keyFrameStep)) + "L.txt"; + map_left.writeBAFile(fileName, keyFrameStep, numKeyFrames); + BA.performBAWithKeyFrames(map_left, keyFrameStep, numKeyFrames); + }*/ + } else { + map_right.updateDataCurrentFrame(pose, trackedCurrFramePoints, indices, trackedPoints3DCurrentFrame, true, true); + + //if ((map_right.getCurrentCameraIndex() - numKeyFrames*keyFrameStep) >= 0){ + if ((map_right.getCurrentCameraIndex() % (numKeyFrames*keyFrameStep)) == 0 && map_right.getCurrentCameraIndex() > 0){ + std::string fileName = "BAFile" + std::to_string(map_right.getCurrentCameraIndex() / (numKeyFrames*keyFrameStep)) + "R.txt"; + //map_right.writeBAFile(fileName, keyFrameStep, numKeyFrames); + BA.performBAWithKeyFrames(map_left, map_right, keyFrameStep, numKeyFrames); + } + } descriptorsCurrentFrame.copyTo(descriptorsPrevFrame); diff --git a/testVisualSLAM.cpp b/testVisualSLAM.cpp index 694eb43..af5e362 100644 --- a/testVisualSLAM.cpp +++ b/testVisualSLAM.cpp @@ -37,8 +37,8 @@ int main(int argc, char** argv){ cv::Mat prevImageLeft, prevImageRight; std::vector pointsCurrentFrame_left, pointsPrevFrame_left; std::vector pointsCurrentFrame_right, pointsPrevFrame_right; - std::vector keypoints; - cv::Mat descriptors; + std::vector keypoints_left, keypoints_right; + cv::Mat descriptors_left,descriptors_right; Eigen::Matrix3d cumR = Eigen::Matrix3d::Identity(); int k = 1; @@ -84,6 +84,9 @@ int main(int argc, char** argv){ Sophus::SE3d pose_left = slam.performFrontEndStepWithTracking(images_left[i], disparity_map, pointsCurrentFrame_left, pointsPrevFrame_left, prevImageLeft, true); Sophus::SE3d pose_right = slam.performFrontEndStepWithTracking(images_right[i], disparity_map, pointsCurrentFrame_right, pointsPrevFrame_right, prevImageRight, false); + //Sophus::SE3d pose_left = slam.performFrontEndStep(images_left[i], disparity_map, keypoints_left, descriptors_left, true); + //Sophus::SE3d pose_right = slam.performFrontEndStep(images_right[i], disparity_map, keypoints_right, descriptors_right, false); + visToolkit->plot2DPoints(images_right[i], pointsCurrentFrame_right); visToolkit->setDataForPointCloudVisualization(images_left[i], disparity_map);