Skip to content

Commit

Permalink
fix some Bug( add matches times ), add run parameters.
Browse files Browse the repository at this point in the history
  • Loading branch information
@LiXin97
LiXin97 committed Jan 22, 2019
1 parent 9e47e2f commit 68b2951
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 47 deletions.
8 changes: 5 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,15 +16,17 @@ find_package(Ceres REQUIRED)
#mynteye
#find_package(mynteye REQUIRED)
#set(__link_libs mynteye)
#find_package(Pangolin REQUIRED))

# Pangolin
find_package(Pangolin REQUIRED)
#
## OpenCV

include_directories(
${PROJECT_SOURCE_DIR}/include
${OpenCV_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
# ${Pangolin_INCLUDE_DIRS}
${Pangolin_INCLUDE_DIRS}
)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
add_library(${PROJECT_NAME} SHARED
Expand All @@ -38,7 +40,7 @@ add_library(${PROJECT_NAME} SHARED
target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${CERES_LIBRARIES}
# ${Pangolin_LIBRARIES}
${Pangolin_LIBRARIES}
)

set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
Expand Down
4 changes: 2 additions & 2 deletions config/KITTI00-02.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,12 +66,12 @@ Viewer.ViewpointF: 2000

# number_of_features: 2000 keyframe_rotation: 0.1 keyframe_translation: 0.1 map_point_erase_ratio: 0.1
# VO Parameters
number_of_features: 800
number_of_features: 1000
scale_factor: 1.2
level_pyramid: 8
match_ratio: 2.0
max_num_lost: 10
min_inliers: 30
keyframe_rotation: 0.1
keyframe_translation: 0.5
map_point_erase_ratio: 0.1
map_point_erase_ratio: 0.5
14 changes: 10 additions & 4 deletions demo/KITTI.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@

//#include <pangolin/pangolin.h>

#include "tic_toc.hpp"
#include "config.hpp"
#include "camera.hpp"
#include "frame.hpp"
Expand All @@ -22,9 +23,9 @@
#include "map.hpp"
#include "mappoint.hpp"

const string PathToSequence = "/Users/lixin/Documents/KITTI/data_odometry/dataset/sequences/06";//07 14 04 06
string PathToSequence = "/Users/lixin/Documents/KITTI/data_odometry/dataset/sequences/";//07 14 04 06
const string ParameterFile = "../config/KITTI00-02.yaml";
const string GroundtruthFile = "/Users/lixin/Documents/KITTI/data_odometry/dataset/poses/06.txt";
string GroundtruthFile = "/Users/lixin/Documents/KITTI/data_odometry/dataset/poses/";

void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
vector<string> &vstrImageRight, vector<double> &vTimestamps);
Expand All @@ -34,6 +35,10 @@ void DrawTrajectory(vector<vector<double>> map_points);
// TODO add keyframe.cc and add a local BA
// TODO feature point should be
int main(int argc, char **argv) {
PathToSequence.append( string(argv[1]) );
GroundtruthFile.append( string(argv[1]) );
GroundtruthFile.append( string(".txt") );

bool view_t = true;
vector<vector<double>> truths, truths_R;
if (view_t)
Expand Down Expand Up @@ -78,6 +83,7 @@ int main(int argc, char **argv) {

for(int ni=begin ; ni<nImages; ni++)
{
TicToc time;
// Read left and right images from file
imLeft = cv::imread(vstrImageLeft[ni],CV_LOAD_IMAGE_UNCHANGED);
imRight = cv::imread(vstrImageRight[ni],CV_LOAD_IMAGE_UNCHANGED);
Expand Down Expand Up @@ -138,14 +144,14 @@ int main(int argc, char **argv) {
int x = (int)(cam_t.x) + 300,
y = (int)(-cam_t.z) + 700;
cv::circle(traj, cv::Point(x,y), 1, CV_RGB(255,0,0), 2 );
cv::rectangle(traj, cv::Point(10,30), cv::Point(550,70), CV_RGB(0,0,0), CV_FILLED);
cv::rectangle(traj, cv::Point(10,0), cv::Point(600,70), CV_RGB(0,0,0), CV_FILLED);
sprintf(text, "Coordinates: x = %6.2fm y = %6.2fm z = %6.2fm",cam_t.x, cam_t.y, cam_t.z);
if(view_t)
{
int x_t = (int)(/*truths[begin][0] - */truths[ni][0]) + 300,
y_t = (int)-(/*truths[begin][2] - */truths[ni][2]) + 700;
cv::circle(traj, cv::Point(x_t,y_t), 1, CV_RGB(255,255,255), 2 );
sprintf(text_t, "Coordinat_t: x = %6.2fm y = %6.2fm z = %6.2fm", truths[ni][0], truths[ni][1], truths[ni][2]);
sprintf(text_t, "Coordinat_t: x = %6.2fm y = %6.2fm z = %6.2fm cost: %.2fms", truths[ni][0], truths[ni][1], truths[ni][2], time.toc());
cv::putText(traj, text_t, cv::Point(10,70), 1, 1, cv::Scalar::all(255));
e_ATE += std::sqrt( (cam_t.x - truths[ni][0]) * (cam_t.x - truths[ni][0]) +
(cam_t.y - truths[ni][1]) * (cam_t.y - truths[ni][1]) +
Expand Down
34 changes: 34 additions & 0 deletions include/tic_toc.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,34 @@
//
// Created by 李鑫 on 2019/1/22.
//

#ifndef STEREOVO_TIC_TOC_HPP
#define STEREOVO_TIC_TOC_HPP


#include <ctime>
#include <cstdlib>
#include <chrono>

class TicToc {
public:
TicToc() {
tic();
}

void tic() {
start = std::chrono::system_clock::now();
}

double toc() {
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
return elapsed_seconds.count() * 1000;
}

private:
std::chrono::time_point<std::chrono::system_clock> start, end;
};


#endif //STEREOVO_TIC_TOC_HPP
73 changes: 35 additions & 38 deletions src/track.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,12 @@
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <tic_toc.hpp>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
//#include <boost/timer.hpp>

TicToc t;

namespace StereoVO
{
Expand Down Expand Up @@ -110,7 +112,7 @@ namespace StereoVO
scale_factor_ = Config::get<double> ( "scale_factor" );
level_pyramid_ = Config::get<int> ( "level_pyramid" );
match_ratio_ = Config::get<float> ( "match_ratio" );
max_num_lost_ = Config::get<float> ( "max_num_lost" );
max_num_lost_ = Config::get<int> ( "max_num_lost" );
min_inliers_ = Config::get<int> ( "min_inliers" );
key_frame_min_rot = Config::get<double> ( "keyframe_rotation" );
key_frame_min_trans = Config::get<double> ( "keyframe_translation" );
Expand All @@ -122,7 +124,7 @@ namespace StereoVO
// detector_ = cv::xfeatures2d::SURF::create();//cv::ORB::create(num_of_features_,scale_factor_, level_pyramid_);

descriptor_ = cv::ORB::create(num_of_features_ * 4 ,scale_factor_, level_pyramid_);
matcher_ = cv::DescriptorMatcher::create ( "BruteForce-Hamming" );
matcher_ = cv::DescriptorMatcher::create ("BruteForce-Hamming");//("FlannBased");//;( "BruteForce-Hamming" );
Mat R = Mat::eye(3,3,CV_64F),t = Mat::zeros(3,1,CV_64F);
cv::hconcat(R,t,T_c_w_estimated_);
}
Expand Down Expand Up @@ -165,11 +167,15 @@ namespace StereoVO
if ( checkEstimatedPose() == true ) // a good estimation
{
curr_->T_c_w_ = T_c_w_estimated_.clone();
t.tic();
optimizeMap();
std::cout << "optimizeMap cost time " << t.toc() << " ms\n";
num_lost_ = 0;
if ( checkKeyFrame() == true ) // is a key-frame
{
t.tic();
addKeyFrame();
std::cout << "addKeyFrame cost time " << t.toc() << " ms\n";
}
else
{
Expand Down Expand Up @@ -211,6 +217,7 @@ namespace StereoVO

void Track::extractKeyPoints()
{
t.tic();
keypoints_curr_right_.clear();
keypoints_curr_left_.clear();

Expand Down Expand Up @@ -288,13 +295,17 @@ namespace StereoVO
// keypoints_curr_right_.push_back(i);
// }

t.toc();
std::cout << "ORB num :" << keypoints_curr_left_.size() << std::endl;
std::cout << "extrack cost time " << t.toc() << " ms\n";
}

void Track::computeDescriptors()
{
t.tic();
descriptor_->compute ( curr_->left_, keypoints_curr_left_, descriptors_curr_left_ );
descriptor_->compute ( curr_->right_, keypoints_curr_right_, descriptors_curr_right_ );
std::cout << "descript cost time " << t.toc() << " ms\n";
}

// TODO not very good
Expand Down Expand Up @@ -440,6 +451,7 @@ namespace StereoVO

void Track::matchCurr()
{
t.tic();
int N = keypoints_curr_left_.size();
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);
Expand Down Expand Up @@ -501,8 +513,9 @@ namespace StereoVO
// mvDepth[vDistIdx[i].second]=-1;
// }
// }
std::cout << "strero matches: " << matches_curr_good_.size() << std::endl;
std::cout << "stereo matches: " << matches_curr_good_.size() << std::endl;

std::cout << "match stereo cost times: "<< t.toc() << " ms\n";


//绘制匹配结果
Expand All @@ -515,6 +528,7 @@ namespace StereoVO

void Track::featureMatching()
{
t.tic();
vector< vector<cv::DMatch>> matches;
// select the candidates in map
Mat desp_map;
Expand Down Expand Up @@ -587,34 +601,14 @@ namespace StereoVO
}


Mat match3d;
drawMatches ( ref_->left_, keypoints_ref_left_, curr_->left_ ,keypoints_curr_left_, good_matchs, match3d );
cv::resize(match3d, match3d, cv::Size(1000,400));
cv::imshow ( "match3d", match3d );



// matcher_flann_.match ( desp_map, descriptors_curr_left_, matches );
// // select the best matches
// float min_dis = std::min_element (
// matches.begin(), matches.end(),
// [] ( const cv::DMatch& m1, const cv::DMatch& m2 )
// {
// return m1.distance < m2.distance;
// } )->distance;

// match_3dpts_.clear();
// match_2dkp_index_.clear();
// for ( cv::DMatch& m : matches )
// {
// if ( m.distance < max<float> ( min_dis*match_ratio_, 50.0 ) )
// {
// match_3dpts_.push_back( candidate[m.queryIdx] );
// match_2dkp_index_.push_back( m.trainIdx );
// }
// }
cout<<"3D pointd: "<<desp_map.size()<<"\n2D points: "<<descriptors_curr_left_.size()<<endl;
cout<<"3D-2D matches: "<<match_3dpts_.size() <<endl;
// Mat match3d;
// drawMatches ( ref_->left_, keypoints_ref_left_, curr_->left_ ,keypoints_curr_left_, good_matchs, match3d );
// cv::resize(match3d, match3d, cv::Size(1000,400));
// cv::imshow ( "match3d", match3d );
//
// cout<<"3D pointd: "<<desp_map.size()<<"\n2D points: "<<descriptors_curr_left_.size()<<endl;
// cout<<"3D-2D matches: "<<match_3dpts_.size() <<endl;
std::cout << "3D-2D match cost time " << t.toc() << " ms\n";
}


Expand Down Expand Up @@ -671,6 +665,7 @@ namespace StereoVO

bool Track::poseEstimationPnP()
{
t.tic();
// construct the 3d 2d observations
vector<cv::Point3f> pts3d;
vector<cv::Point2f> pts2d, pts2d_ref;
Expand All @@ -686,7 +681,7 @@ namespace StereoVO
}

Mat K = ( cv::Mat_<double> ( 3,3 ) <<
ref_->camera_->fx_, 0, ref_->camera_->cx_,
ref_->camera_->fx_, 0, ref_->camera_->cx_,
0, ref_->camera_->fy_, ref_->camera_->cy_,
0,0,1
);
Expand All @@ -705,7 +700,8 @@ namespace StereoVO
for(int i=0; i<inliers.rows; i++ )
{
int index = inliers.at<int> ( i,0 );
double pt3d_in[3] = {pts3d[index].x, pts3d[index].y, pts3d[index].z};
// double pt3d_in[3] = {pts3d[index].x, pts3d[index].y, pts3d[index].z};
match_3dpts_[index]->matched_times_++;
ceres::CostFunction* cost_function = MinReprojectionError::Create( pts3d[index], pts2d[index], K );
ceres::LossFunction* loss_function = nullptr;//new ceres::CauchyLoss(1.5);
problem.AddResidualBlock(cost_function, loss_function, pose);
Expand Down Expand Up @@ -736,8 +732,9 @@ namespace StereoVO

cv::Rodrigues(rvec, R);
cv::hconcat(R,tvec,T_c_w_estimated_);
cout<<"T_c_w_estimated_: "<<endl<<T_c_w_estimated_<<endl;
// cout<<"T_c_w_estimated_: "<<endl<<T_c_w_estimated_<<endl;

std::cout << "PnP cost times: "<< t.toc() << " ms\n";
return true;
/*// using bundle adjustment to optimize the pose
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
Expand Down Expand Up @@ -802,7 +799,7 @@ namespace StereoVO

double d_n = my_nom(d);

if ( d_n > 5.0 )
if ( d_n > 10.0 )
{
cout<<"reject because motion is too large: "<<d_n <<endl;
return false;
Expand Down Expand Up @@ -878,15 +875,15 @@ namespace StereoVO
iter++;
}

if ( match_2dkp_index_.size() < 1000 )
if ( match_2dkp_index_.size() < 2000 )
addMapPoints();
if ( map_->map_points_.size() > 3000 )
{
// TODO map is too large, remove some one
map_point_erase_ratio_ += 0.05;
map_point_erase_ratio_ += 0.5;
}
else
map_point_erase_ratio_ = 0.1;
map_point_erase_ratio_ = 0.5;
cout<<"map points: "<<map_->map_points_.size()<<endl;
}

Expand Down

0 comments on commit 68b2951

Please sign in to comment.